adding in latching logic for the db controller

lib/systems/include/DrivebrainController.h

@@ -6,12 +6,22 @@
 #include "DrivebrainData.h"
 
 // TODO - [ ] 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 - [ ] set 
+//            and ensure that they are within tolerence to current sys-tick
+
+// TODO - [ ] 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"
+
+
 class DrivebrainController : public TorqueController<TC_DRIVEBRAIN>
 {
 public:
-    void tick(const SysTick_s &sys_tick, DrivebrainData db_input);
+    void tick(const SysTick_s &sys_tick, DrivebrainData db_input, bool is_active_controller);
 
     DrivebrainController(TorqueControllerOutput_s &writeout, unsigned long allowed_latency, unsigned long allowed_jitter) : _writeout(writeout)
     {
@@ -25,6 +35,7 @@ class DrivebrainController : public TorqueController<TC_DRIVEBRAIN>
         unsigned long allowed_latency;
     } _params;
 
+    bool _timing_failure = false;
     unsigned long _last_sent_torque_lim_millis = -1;
     unsigned long _last_sent_speed_setpoint_millis = -1;
 

lib/systems/include/TorqueControllerMux.h

@@ -12,6 +12,7 @@
 #include "VectornavInterface.h"
 #include "LoadCellInterface.h"
 #include "TelemetryInterface.h"
+#include "DrivebrainController.h"
 
 const float MAX_SPEED_FOR_MODE_CHANGE = 5.0;        // m/s
 const float MAX_TORQUE_DELTA_FOR_MODE_CHANGE = 0.5; // Nm
@@ -24,16 +25,17 @@ class TorqueControllerMux
     TorqueControllerSimple torqueControllerSimple_;
     TorqueControllerLoadCellVectoring torqueControllerLoadCellVectoring_;
     TorqueControllerSimpleLaunch torqueControllerSimpleLaunch_;
-    TorqueControllerSlipLaunch torqueControllerSlipLaunch_;
+    DrivebrainController _dbController;
     TorqueControllerCASEWrapper tcCASEWrapper_;
 
+
     // Use this to map the dial to TCMUX modes
     std::unordered_map<DialMode_e, TorqueController_e> dialModeMap_ = {
         {DialMode_e::MODE_0, TorqueController_e::TC_SAFE_MODE},
         {DialMode_e::MODE_1, TorqueController_e::TC_LOAD_CELL_VECTORING},
         {DialMode_e::MODE_2, TorqueController_e::TC_CASE_SYSTEM},
         {DialMode_e::MODE_3, TorqueController_e::TC_SIMPLE_LAUNCH},
-        {DialMode_e::MODE_4, TorqueController_e::TC_SLIP_LAUNCH},
+        {DialMode_e::MODE_4, TorqueController_e::TC_DRIVEBRAIN},
         {DialMode_e::MODE_5, TorqueController_e::TC_NO_CONTROLLER},
     };
     std::unordered_map<TorqueLimit_e, float> torqueLimitMap_ = {
@@ -53,7 +55,7 @@ class TorqueControllerMux
         static_cast<TorqueControllerBase*>(&torqueControllerSimple_),
         static_cast<TorqueControllerBase*>(&torqueControllerLoadCellVectoring_),
         static_cast<TorqueControllerBase*>(&torqueControllerSimpleLaunch_),
-        static_cast<TorqueControllerBase*>(&torqueControllerSlipLaunch_),
+        static_cast<TorqueControllerBase*>(&_dbController),
         static_cast<TorqueControllerBase*>(&tcCASEWrapper_)
     };
 
@@ -73,7 +75,7 @@ class TorqueControllerMux
     , torqueControllerSimple_(controllerOutputs_[static_cast<int>(TorqueController_e::TC_SAFE_MODE)])
     , torqueControllerLoadCellVectoring_(controllerOutputs_[static_cast<int>(TorqueController_e::TC_LOAD_CELL_VECTORING)])
     , torqueControllerSimpleLaunch_(controllerOutputs_[static_cast<int>(TorqueController_e::TC_SIMPLE_LAUNCH)])
-    , torqueControllerSlipLaunch_(controllerOutputs_[static_cast<int>(TorqueController_e::TC_SLIP_LAUNCH)])
+    , _dbController(controllerOutputs_[static_cast<int>(TorqueController_e::TC_DRIVEBRAIN)], 30, 10)
     , tcCASEWrapper_(controllerOutputs_[static_cast<int>(TorqueController_e::TC_CASE_SYSTEM)])
     , telemHandle_(telemInterface) {}
 
@@ -86,7 +88,7 @@ class TorqueControllerMux
     , torqueControllerSimple_(controllerOutputs_[static_cast<int>(TorqueController_e::TC_SAFE_MODE)], simpleTCRearTorqueScale, simpleTCRegenTorqueScale)
     , torqueControllerLoadCellVectoring_(controllerOutputs_[static_cast<int>(TorqueController_e::TC_LOAD_CELL_VECTORING)], 1.0, simpleTCRegenTorqueScale)
     , torqueControllerSimpleLaunch_(controllerOutputs_[static_cast<int>(TorqueController_e::TC_SIMPLE_LAUNCH)])
-    , torqueControllerSlipLaunch_(controllerOutputs_[static_cast<int>(TorqueController_e::TC_SLIP_LAUNCH)])
+    , _dbController(controllerOutputs_[static_cast<int>(TorqueController_e::TC_DRIVEBRAIN)], 30, 10)
     , tcCASEWrapper_(controllerOutputs_[static_cast<int>(TorqueController_e::TC_CASE_SYSTEM)])
     , telemHandle_(telemInterface) {}
 
@@ -102,7 +104,8 @@ class TorqueControllerMux
         float accDerateFactor,
         bool dashboardTorqueModeButtonPressed,
         const vector_nav &vn_data, 
-        const DrivetrainCommand_s &CASECommand
+        const DrivetrainCommand_s &CASECommand,
+        DrivebrainData db_input
     );
 
     /// @brief apply corresponding limits on drivetrain command calculated by torque controller

lib/systems/include/TorqueControllers.h

@@ -73,11 +73,9 @@ enum TorqueController_e
     TC_SAFE_MODE = 1,
     TC_LOAD_CELL_VECTORING = 2,
     TC_SIMPLE_LAUNCH = 3,
-    TC_SLIP_LAUNCH = 4,
-    TC_LOOKUP_LAUNCH = 5,
-    TC_CASE_SYSTEM = 6,
-    TC_DRIVEBRAIN= 7,
-    TC_NUM_CONTROLLERS = 8,
+    TC_CASE_SYSTEM = 4,
+    TC_DRIVEBRAIN= 5,
+    TC_NUM_CONTROLLERS = 6,
 };
 
 enum class LaunchStates_e
@@ -267,54 +265,54 @@ class TorqueControllerSimpleLaunch : public TorqueController<TC_SIMPLE_LAUNCH>,
     void calc_launch_algo(const vector_nav *vn_data) override;
 };
 
-class TorqueControllerSlipLaunch : public TorqueController<TC_SLIP_LAUNCH>, public BaseLaunchController
-{
-private:
-    float slip_ratio_;
-
-public:
-    /*!
-        SLIP LAUNCH CONTROLLER
-        This launch controller is based off of a specified slip constant. It will at all times attempt
-        to keep the wheelspeed at this certain higher percent of the body velocity of the car to keep it
-        in constant slip
-        @param slip_ratio specified launch rate in m/s^2
-        @param initial_speed_target the initial speed commanded to the wheels
-    */
-    TorqueControllerSlipLaunch(TorqueControllerOutput_s &writeout, float slip_ratio, int16_t initial_speed_target)
-        : BaseLaunchController(writeout, initial_speed_target),
-          slip_ratio_(slip_ratio) {}
-
-    TorqueControllerSlipLaunch(TorqueControllerOutput_s &writeout) : TorqueControllerSlipLaunch(writeout, DEFAULT_SLIP_RATIO, DEFAULT_LAUNCH_SPEED_TARGET) {}
-
-    LaunchStates_e get_launch_state() override { return launch_state_; }
-
-    void calc_launch_algo(const vector_nav *vn_data) override;
-};
-
-class TorqueControllerLookupLaunch : public TorqueController<TC_LOOKUP_LAUNCH>, BaseLaunchController
-{
-private:
-    bool init_position = false;
-
-public:
-    /*!
-        Lookup Launch Controller
-        This launch controller is based off of a matlab and symlink generated lookup table.
-        This has been converted to a C array with some basic python code using the array index
-        as the input for the controller
-        @param slip_ratio specified launch rate in m/s^2
-        @param initial_speed_target the initial speed commanded to the wheels
-    */
-    TorqueControllerLookupLaunch(TorqueControllerOutput_s &writeout, int16_t initial_speed_target)
-        : BaseLaunchController(writeout, initial_speed_target) {}
-
-    TorqueControllerLookupLaunch(TorqueControllerOutput_s &writeout) : TorqueControllerLookupLaunch(writeout, DEFAULT_LAUNCH_SPEED_TARGET) {}
-
-    LaunchStates_e get_launch_state() override { return launch_state_; }
-
-    void calc_launch_algo(const vector_nav *vn_data) override;
-};
+// class TorqueControllerSlipLaunch : public TorqueController<TC_SLIP_LAUNCH>, public BaseLaunchController
+// {
+// private:
+//     float slip_ratio_;
+
+// public:
+//     /*!
+//         SLIP LAUNCH CONTROLLER
+//         This launch controller is based off of a specified slip constant. It will at all times attempt
+//         to keep the wheelspeed at this certain higher percent of the body velocity of the car to keep it
+//         in constant slip
+//         @param slip_ratio specified launch rate in m/s^2
+//         @param initial_speed_target the initial speed commanded to the wheels
+//     */
+//     TorqueControllerSlipLaunch(TorqueControllerOutput_s &writeout, float slip_ratio, int16_t initial_speed_target)
+//         : BaseLaunchController(writeout, initial_speed_target),
+//           slip_ratio_(slip_ratio) {}
+
+//     TorqueControllerSlipLaunch(TorqueControllerOutput_s &writeout) : TorqueControllerSlipLaunch(writeout, DEFAULT_SLIP_RATIO, DEFAULT_LAUNCH_SPEED_TARGET) {}
+
+//     LaunchStates_e get_launch_state() override { return launch_state_; }
+
+//     void calc_launch_algo(const vector_nav *vn_data) override;
+// };
+
+// class TorqueControllerLookupLaunch : public TorqueController<TC_LOOKUP_LAUNCH>, BaseLaunchController
+// {
+// private:
+//     bool init_position = false;
+
+// public:
+//     /*!
+//         Lookup Launch Controller
+//         This launch controller is based off of a matlab and symlink generated lookup table.
+//         This has been converted to a C array with some basic python code using the array index
+//         as the input for the controller
+//         @param slip_ratio specified launch rate in m/s^2
+//         @param initial_speed_target the initial speed commanded to the wheels
+//     */
+//     TorqueControllerLookupLaunch(TorqueControllerOutput_s &writeout, int16_t initial_speed_target)
+//         : BaseLaunchController(writeout, initial_speed_target) {}
+
+//     TorqueControllerLookupLaunch(TorqueControllerOutput_s &writeout) : TorqueControllerLookupLaunch(writeout, DEFAULT_LAUNCH_SPEED_TARGET) {}
+
+//     LaunchStates_e get_launch_state() override { return launch_state_; }
+
+//     void calc_launch_algo(const vector_nav *vn_data) override;
+// };
 
 class TorqueControllerCASEWrapper : public TorqueController<TC_CASE_SYSTEM>
 {

lib/systems/src/DrivebrainController.cpp

@@ -1,17 +1,25 @@
 #include "DrivebrainController.h"
 
-void DrivebrainController::tick(const SysTick_s &sys_tick, DrivebrainData db_input)
+void DrivebrainController::tick(const SysTick_s &sys_tick, DrivebrainData db_input, bool is_active_controller)
 {
     _writeout.ready = false;
 
     bool speed_setpoint_too_latent = (::abs((int)(sys_tick.millis - db_input.last_speed_setpoint_receive_time_millis)) > (int)_params.allowed_latency);
     bool torque_setpoint_too_latent = (::abs((int)(sys_tick.millis - db_input.last_torque_lim_receive_time_millis)) > (int)_params.allowed_latency);
     bool msg_jitter_too_high = (::abs((int)(db_input.last_speed_setpoint_receive_time_millis - db_input.last_torque_lim_receive_time_millis)) > (int)_params.allowed_jitter);
+
+    bool timing_failure = (speed_setpoint_too_latent || torque_setpoint_too_latent || msg_jitter_too_high); 
+
+    // only in the case that this is the active controller do we want to clear our timing failure
+    if((!is_active_controller) && (!timing_failure) )
+    {
+        // timing failure should be false here
+        _timing_failure = timing_failure;
+    }
 
-    if (!(speed_setpoint_too_latent || torque_setpoint_too_latent || msg_jitter_too_high))
+    if (!timing_failure)
     {
         _writeout.ready = true;
-
         _last_sent_speed_setpoint_millis = db_input.last_speed_setpoint_receive_time_millis;
         _last_sent_torque_lim_millis = db_input.last_torque_lim_receive_time_millis;
 
@@ -20,6 +28,7 @@ void DrivebrainController::tick(const SysTick_s &sys_tick, DrivebrainData db_inp
     }
     else
     {
+        _timing_failure = true;
         _writeout.ready = false;
         _writeout.command = {}; // set command to all zeros if bad latency is apparent
     }

lib/systems/src/TorqueControllerMux.cpp

@@ -1,6 +1,7 @@
 #include "TorqueControllerMux.h"
 #include "Utility.h"
 #include "PhysicalParameters.h"
+#include "DrivebrainController.h"
 
 void TorqueControllerMux::tick(
     const SysTick_s &tick,
@@ -12,13 +13,16 @@ void TorqueControllerMux::tick(
     float accDerateFactor,
     bool dashboardTorqueModeButtonPressed,
     const vector_nav &vn_data,
-    const DrivetrainCommand_s &CASECommand)
+    const DrivetrainCommand_s &CASECommand,
+    DrivebrainData db_input)
 {
     // Tick all torque controllers
     torqueControllerSimple_.tick(tick, pedalsData, torqueLimitMap_[torqueLimit_]);
     torqueControllerLoadCellVectoring_.tick(tick, pedalsData, torqueLimitMap_[torqueLimit_], loadCellData);
     torqueControllerSimpleLaunch_.tick(tick, pedalsData, drivetrainData.measuredSpeeds, &vn_data);
-    torqueControllerSlipLaunch_.tick(tick, pedalsData, drivetrainData.measuredSpeeds, &vn_data);
+
+    bool drivebrain_in_control = (muxMode_ == TorqueController_e::TC_DRIVEBRAIN);
+    _dbController.tick(tick, db_input, drivebrain_in_control);
     tcCASEWrapper_.tick(
         (TCCaseWrapperTick_s){
             .command = CASECommand,
@@ -106,13 +110,12 @@ void TorqueControllerMux::tick(
         // Apply setpoints value limits
         // Derating for endurance
 
-
-        if (muxMode_ != TC_CASE_SYSTEM)
+        if (muxMode_ != TorqueController_e::TC_CASE_SYSTEM)
         {
             // Safety checks for CASE: CASE handles regen, torque, and power limit internally
             applyRegenLimit(&drivetrainCommand_, &drivetrainData);
             // Apply torque limit before power limit to not power limit
-            if ((muxMode_ != TC_SIMPLE_LAUNCH) && (muxMode_ != TC_SLIP_LAUNCH) && (muxMode_ != TC_LOOKUP_LAUNCH))
+            if ((muxMode_ != TC_SIMPLE_LAUNCH))
             {
                 applyTorqueLimit(&drivetrainCommand_);
             }            

lib/systems/src/TorqueControllers.cpp

@@ -291,53 +291,53 @@ void TorqueControllerSimpleLaunch::calc_launch_algo(const vector_nav* vn_data) {
             launch_speed_target_ = std::min((int)AMK_MAX_RPM, std::max(0, (int)launch_speed_target_));
 }
 
-void TorqueControllerSlipLaunch::calc_launch_algo(const vector_nav* vn_data) {
-    // accelerate at constant speed for a period of time to get body velocity up
-    // may want to make this the ht07 launch algo
+// void TorqueControllerSlipLaunch::calc_launch_algo(const vector_nav* vn_data) {
+//     // accelerate at constant speed for a period of time to get body velocity up
+//     // may want to make this the ht07 launch algo
 
-    // makes sure that the car launches at the target launch speed
-    launch_speed_target_ = std::max(launch_speed_target_, (float)DEFAULT_LAUNCH_SPEED_TARGET);
-
-    /*
-    New slip-ratio based launch algorithm by Luke Chen. The basic idea
-    is to always be pushing the car a certain 'slip_ratio_' faster than
-    the car is currently going, theoretically always keeping the car in slip
-    */
-    // m/s
-    float new_speed_target = (1 + slip_ratio_) * (vn_data->velocity_x);
-    // rpm
-    new_speed_target *= METERS_PER_SECOND_TO_RPM;
-    // makes sure the car target speed never goes lower than prev. target
-    // allows for the vn to 'spool' up and us to get reliable vx data
-    launch_speed_target_ = std::max(launch_speed_target_, new_speed_target);
-}
-
-void TorqueControllerLookupLaunch::calc_launch_algo(const vector_nav* vn_data) {
-
-    launch_speed_target_ = std::max((float)DEFAULT_LAUNCH_SPEED_TARGET, launch_speed_target_);
-
-    double dx = vn_data->ecef_coords[0] - initial_ecef_x_;
-    double dy = vn_data->ecef_coords[1] - initial_ecef_y_;
-    double dz = vn_data->ecef_coords[2] - initial_ecef_z_;
-
-    double distance = sqrt((dx*dx) + (dy*dy) + (dz*dz));
-
-    /*
-    Distance-lookup launch algorithm. Takes in the vel_dist_lookup
-    generated from Luke's matlab/symlink to set speed targets based
-    on distance travelled from the start point.
-    This can also and may be better to replace with an integration
-    of body velocity.
-    */
-
-    uint32_t idx = (uint32_t)(distance * 10); // multiply by 10 to be used as index for meters in steps of 1/10
-    idx = std::min(idx, (uint32_t)(sizeof(vel_dist_lookup) / sizeof(float)));
-    float mps_target = vel_dist_lookup[idx];
-
-    float new_speed_target = mps_target * METERS_PER_SECOND_TO_RPM;
-    launch_speed_target_ = std::max(launch_speed_target_, new_speed_target);
-
-}
+//     // makes sure that the car launches at the target launch speed
+//     launch_speed_target_ = std::max(launch_speed_target_, (float)DEFAULT_LAUNCH_SPEED_TARGET);
+
+//     /*
+//     New slip-ratio based launch algorithm by Luke Chen. The basic idea
+//     is to always be pushing the car a certain 'slip_ratio_' faster than
+//     the car is currently going, theoretically always keeping the car in slip
+//     */
+//     // m/s
+//     float new_speed_target = (1 + slip_ratio_) * (vn_data->velocity_x);
+//     // rpm
+//     new_speed_target *= METERS_PER_SECOND_TO_RPM;
+//     // makes sure the car target speed never goes lower than prev. target
+//     // allows for the vn to 'spool' up and us to get reliable vx data
+//     launch_speed_target_ = std::max(launch_speed_target_, new_speed_target);
+// }
+
+// void TorqueControllerLookupLaunch::calc_launch_algo(const vector_nav* vn_data) {
+
+//     launch_speed_target_ = std::max((float)DEFAULT_LAUNCH_SPEED_TARGET, launch_speed_target_);
+
+//     double dx = vn_data->ecef_coords[0] - initial_ecef_x_;
+//     double dy = vn_data->ecef_coords[1] - initial_ecef_y_;
+//     double dz = vn_data->ecef_coords[2] - initial_ecef_z_;
+
+//     double distance = sqrt((dx*dx) + (dy*dy) + (dz*dz));
+
+//     /*
+//     Distance-lookup launch algorithm. Takes in the vel_dist_lookup
+//     generated from Luke's matlab/symlink to set speed targets based
+//     on distance travelled from the start point.
+//     This can also and may be better to replace with an integration
+//     of body velocity.
+//     */
+
+//     uint32_t idx = (uint32_t)(distance * 10); // multiply by 10 to be used as index for meters in steps of 1/10
+//     idx = std::min(idx, (uint32_t)(sizeof(vel_dist_lookup) / sizeof(float)));
+//     float mps_target = vel_dist_lookup[idx];
+
+//     float new_speed_target = mps_target * METERS_PER_SECOND_TO_RPM;
+//     launch_speed_target_ = std::max(launch_speed_target_, new_speed_target);
+
+// }
 
 void TorqueControllerCASEWrapper::tick(const TCCaseWrapperTick_s &intake)
 {