DrivebrainController.h

Go to the documentation of this file.

#ifndef __DRIVEBRAINCONTROLLER_H__
#define __DRIVEBRAINCONTROLLER_H__
 
#include <Controllers/SimpleController.h>
#include "DrivebrainData.h"
#include "BaseController.h"
#include "SharedDataTypes.h"
#include <cmath>
 
// TODO - [x] need to validate that the times that are apparent in the drivebrain data
//            and ensure that they are within tolerence to current sys-tick
 
// TODO - [x] if the drivebrain controller is currently the active controller,
//            the latency fault should be latching
 
//              meaning that if we fail any of the latency checks while active we cannot clear the timing failure fault
//              unless we switch to another controller and back again. in reality, the CAN line or ethernet conditions
//              probably wont improve randomly during runtime so it will keep faulting. primarily this is just to make sure
//              we dont keep going from latent to not latent while driving and thus the driver gets jittered and the
//              drivetrain will keep "powering up" and "powering down"
 
// TODO - [x] correctly measure latency between drivebrain and MCU
//
//         measure latency by subtracting the latest MCU millis stamped message from the drivebrain from the 
//         current system millis to 
 
 
class DrivebrainController : public Controller
{
public:
 
    DrivebrainController(unsigned long allowed_latency,
                         ControllerMode_e assigned_controller_mode = ControllerMode_e::MODE_4)
        : _emergency_control(1.0f, 1.0f)
    {
        _last_worst_latency_timestamp = 0;
        _worst_latency_so_far = -1;
        _params = {allowed_latency, assigned_controller_mode};
    }
 
    TorqueControllerOutput_s evaluate(const SharedCarState_s &state);
 
    bool get_timing_failure_status() { return _timing_failure; }
 
private:
    struct
    {
        unsigned long allowed_latency;
        ControllerMode_e assigned_controller_mode;
    } _params;
 
    unsigned long _last_worst_latency_timestamp;
    int64_t _worst_latency_so_far;
    bool _timing_failure = false;
    unsigned long _last_setpoint_millis = -1;
    TorqueControllerSimple _emergency_control;
};
 
#endif // __DRIVEBRAINCONTROLLER_H__