added docs for the drivebrain controller

lib/interfaces/src/DrivebrainETHInterface.cpp

@@ -7,13 +7,13 @@ hytech_msgs_MCUOutputData DrivebrainETHInterface::make_db_msg(const SharedCarSta
     hytech_msgs_MCUOutputData out;
     out.accel_percent = shared_state.pedals_data.accelPercent;
     out.brake_percent = shared_state.pedals_data.regenPercent;
-    
+
     out.has_rpm_data = true;
     out.rpm_data.FL = shared_state.drivetrain_data.measuredSpeeds[0];
     out.rpm_data.FR = shared_state.drivetrain_data.measuredSpeeds[1];
     out.rpm_data.RL = shared_state.drivetrain_data.measuredSpeeds[2];
     out.rpm_data.RR = shared_state.drivetrain_data.measuredSpeeds[3];
-    
+
     out.steering_angle_deg = shared_state.steering_data.angle;
     out.MCU_recv_millis = _latest_data.last_receive_time_millis;
     out.load_cell_data = {shared_state.raw_loadcell_data.raw_load_cell_data.FL,
@@ -29,9 +29,7 @@ hytech_msgs_MCUOutputData DrivebrainETHInterface::make_db_msg(const SharedCarSta
 void DrivebrainETHInterface::receive_pb_msg(const hytech_msgs_MCUCommandData &msg_in, unsigned long curr_millis)
 {
     veh_vec<float> nm_lim(msg_in.torque_limit_nm.FL, msg_in.torque_limit_nm.FR, msg_in.torque_limit_nm.RL, msg_in.torque_limit_nm.RR);
-
     veh_vec<float> speed_set(msg_in.desired_rpms.FL, msg_in.desired_rpms.FR, msg_in.desired_rpms.RL, msg_in.desired_rpms.RR);
-
     _latest_data.torque_limits_nm = nm_lim;
     _latest_data.speed_setpoints_rpm = speed_set;
     _latest_data.DB_prev_MCU_recv_millis = msg_in.prev_MCU_recv_millis;

lib/systems/include/DrivebrainController.h

@@ -2,15 +2,15 @@
 #define __DRIVEBRAINCONTROLLER_H__
 
 #include <SimpleController.h>
-// #include "TorqueControllers.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 - [ ] if the drivebrain controller is currently the active controller,
+// 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
@@ -19,28 +19,65 @@
 //              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 
+
+
+/*! \brief Drivebrain Controller for fail-safe pass-through control of the car
+           RESPONSIBILITIES AND DOCUMENTATION
+  
+    this class is the "controller" that allows for pass-through control as commanded by the drivebrain. It also calculates the 
+    latency of the most recent input and checks to see if the most recent input is still valid and has not expired. If the 
+    input has expired then it switches over to the fail-safe control mode (MODE0) to allow for safe failing even while the car
+    is driving so that we dont lose hard-braking capabilities.
+
+    This class can clear it's own fault by switching off of this operating mode and then swapping back to this operating mode. 
+    The fault clears the first time this controller gets evaluated while switch from the swapped-to mode back to this pass 
+    through mode. 
+
+    latency measurement:
+        - the latency is measured by comparing the mcu systick's millis stamp within the most recent packet received from the 
+          drivebrain to the current systick millis. if the difference is too great, we swap to MODE0 control. The drivebrain's 
+          MCU timestamp comes from the most recent packet it received from the MCU as it contains the MCU systick's millis 
+          stamp at that packet's creation.
+
+    config: 
+        - maximum allowed latency
+        - assigned controller mode (currently defaults to MODE4)
+    inputs:
+        - the current car state from which it gets the latest drivebrain input, current controller mode and systick
+ */
 class DrivebrainController : public Controller
 {
 public:
 
+    /// @brief constructor for the drivebrain controller class
+    /// @param allowed_latency the allowed latency in milliseconds for which if the most recent packet has a timestamp older than this measure of time we fail safe
+    /// @param assigned_controller_mode the controller mode that the drivebrain controller is assigned to. is required for evaluating whether or not we are active or not
     DrivebrainController(unsigned long allowed_latency,
-                         float max_fault_clear_speed_m_s = 1.0,
                          ControllerMode_e assigned_controller_mode = ControllerMode_e::MODE_4)
         : _emergency_control(1.0f, 1.0f)
     {
-        _last_worst_latency_rec_time =0;
+        _last_worst_latency_rec_time = 0;
         _worst_latency_so_far = -1;
-        _params = {allowed_latency, max_fault_clear_speed_m_s, assigned_controller_mode};
+        _params = {allowed_latency, assigned_controller_mode};
     }
 
+    /// @brief evaluate function for running the business logic
+    /// @param state the current state of the car
+    /// @return torque controller output that gets passed through the TC MUX
     TorqueControllerOutput_s evaluate(const SharedCarState_s &state);
+
+    /// @brief getter for the current status of whether or not the controller has had a timing failure during operation
+    /// @return bool of status
     bool get_timing_failure_status() { return _timing_failure; }
 
 private:
     struct
     {
         unsigned long allowed_latency;
-        float max_fault_clear_speed_m_s;
         ControllerMode_e assigned_controller_mode;
     } _params;
 

lib/systems/src/DrivebrainController.cpp

@@ -1,5 +1,5 @@
 #include "DrivebrainController.h"
-// #include <Arduino.h>
+
 TorqueControllerOutput_s DrivebrainController::evaluate(const SharedCarState_s &state)
 {
 
@@ -12,32 +12,26 @@ TorqueControllerOutput_s DrivebrainController::evaluate(const SharedCarState_s &
 
     // 2 if the DB_prev_MCU_recv_millis < 0, then the drivebrain has not received a time from the MCU 
     // (meaning that the MCU is not sending properly or the drivebrain is not receiving properly or it has 
-    // yet to receive from the MCU yet)
+    // yet to receive from the MCU yet) 
     bool drivebrain_has_not_received_time = (db_input.DB_prev_MCU_recv_millis < 0);
-    // Serial.println("uh");
+
     // 3 if the time between the current MCU sys_tick.millis time and the last millis time that the drivebrain received is too high
     bool message_too_latent = (::abs((int)(sys_tick.millis - db_input.DB_prev_MCU_recv_millis)) > (int)_params.allowed_latency);
     if((sys_tick.millis - _last_worst_latency_rec_time) > 5000)
-    {
-        // Serial.print("_worst_latency_so_far ");
-        // Serial.println(_worst_latency_so_far);
-        // Serial.print("errord:");
-        // Serial.println(_timing_failure);
+    {    
         _last_worst_latency_rec_time = sys_tick.millis;
         _worst_latency_so_far = -1;
     }
 
     if( (sys_tick.millis - db_input.DB_prev_MCU_recv_millis) > _worst_latency_so_far)
     {
-
-        _worst_latency_so_far = (sys_tick.millis - db_input.DB_prev_MCU_recv_millis);
-
+        _worst_latency_so_far = (sys_tick.millis - db_input.DB_prev_MCU_recv_millis);   
     }
 
 
     bool timing_failure = (message_too_latent || no_messages_received || drivebrain_has_not_received_time);
 
-    // only in the case that our speed is low enough (<1 m/s) do we want to clear the fault
+    // only in the case that we are not the active controller yet do we want to clear the fault
 
     bool is_active_controller = state.tc_mux_status.current_controller_mode_ == _params.assigned_controller_mode;
 

src/main.cpp

@@ -442,6 +442,7 @@ void loop()
                                     torque_controller_mux.get_tc_mux_status(),
                                     db_controller.get_timing_failure_status());
 
+    car_state_inst.drivebrain_timing_failure = db_controller.get_timing_failure_status();
     hytech_msgs_MCUOutputData out_eth_msg = db_eth_interface.make_db_msg(car_state_inst);
 
     handle_ethernet_interface_comms(curr_tick, out_eth_msg);