Merge pull request #114 from hytech-racing/feature/drivebrain-emergen…

…cy-control

Feature/drivebrain emergency control

lib/interfaces/src/DrivebrainETHInterface.cpp

@@ -6,30 +6,29 @@ 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.brakePercent;
-    
+    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,
                           shared_state.raw_loadcell_data.raw_load_cell_data.FR,
                           shared_state.raw_loadcell_data.raw_load_cell_data.RL,
                           shared_state.raw_loadcell_data.raw_load_cell_data.RR};
     out.has_load_cell_data = true;
+
+    out.timing_failure_active = shared_state.drivebrain_timing_failure;
     return out;
 }
 
 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/shared_data/include/SharedDataTypes.h

@@ -157,6 +157,7 @@ struct SharedCarState_s
     VectornavData_s vn_data;
     DrivebrainData_s db_data;
     TorqueControllerMuxStatus tc_mux_status;
+    bool drivebrain_timing_failure = false;
     SharedCarState_s() = delete;
     SharedCarState_s(const SysTick_s &_systick,
               const SteeringSystemData_s &_steering_data,
@@ -186,7 +187,8 @@ struct SharedCarState_s
               const PedalsSystemData_s &_pedals_data,
               const VectornavData_s &_vn_data,
               const DrivebrainData_s &_db_data,
-              const TorqueControllerMuxStatus &_tc_mux_status)
+              const TorqueControllerMuxStatus &_tc_mux_status,
+              bool _drivebrain_timing_failure)
         : systick(_systick),
           steering_data(_steering_data),
           drivetrain_data(_drivetrain_data),
@@ -195,7 +197,8 @@ struct SharedCarState_s
           pedals_data(_pedals_data),
           vn_data(_vn_data),
           db_data(_db_data),
-          tc_mux_status(_tc_mux_status)
+          tc_mux_status(_tc_mux_status),
+          drivebrain_timing_failure(_drivebrain_timing_failure)
     {
         // constructor body (if needed)
     }

lib/systems/include/DrivebrainController.h

@@ -1,16 +1,16 @@
 #ifndef __DRIVEBRAINCONTROLLER_H__
 #define __DRIVEBRAINCONTROLLER_H__
 
-
-#include "TorqueControllers.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 - [ ] 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,33 +19,73 @@
 //              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_timestamp = 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;
 
     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__

lib/systems/include/DrivetrainSystem.h

@@ -15,8 +15,8 @@ class DrivetrainSystem
 public:
     /// @brief order of array: 0: FL, 1: FR, 2: RL, 3: RR
     /// @param inverters inverter pointers
-    DrivetrainSystem(const std::array<InverterType *, 4> &inverters, MCUInterface *mcu_interface, int init_time_limit_ms, uint16_t min_hv_voltage = 60, int min_cmd_period_ms = 1)
-        : inverters_(inverters), init_time_limit_ms_(init_time_limit_ms), min_hv_voltage_(min_hv_voltage), min_cmd_period_(min_cmd_period_ms)
+    DrivetrainSystem(const std::array<InverterType *, 4> &inverters, MCUInterface *mcu_interface, int init_time_limit_ms, uint16_t min_hv_voltage = 60, int min_cmd_period_ms = 1, float max_torque_setpoint_nm = 21.42)
+        : inverters_(inverters), init_time_limit_ms_(init_time_limit_ms), min_hv_voltage_(min_hv_voltage), min_cmd_period_(min_cmd_period_ms), max_torque_setpoint_nm_(max_torque_setpoint_nm)
     {
         // values from: https://www.amk-motion.com/amk-dokucd/dokucd/en/content/resources/pdf-dateien/fse/motor_data_sheet_a2370dd_dd5.pdf
         motor_pole_pairs_ = 5;
@@ -94,6 +94,7 @@ class DrivetrainSystem
     unsigned long drivetrain_initialization_phase_start_time_;
     DrivetrainCommand_s current_drivetrain_command_;
     DrivetrainDynamicReport_s dynamic_data_;
+    float max_torque_setpoint_nm_;
 };
 
 #include "DrivetrainSystem.tpp"

lib/systems/include/DrivetrainSystem.tpp

@@ -148,7 +148,15 @@ void DrivetrainSystem<InverterType>::command_drivetrain(const DrivetrainCommand_
         int index = 0;
         for (auto inv_pointer : inverters_)
         {
-            inv_pointer->handle_command({data.inverter_torque_limit[index], data.speeds_rpm[index]});
+            float setpoint = 0;
+            if(data.inverter_torque_limit[index] > max_torque_setpoint_nm_)
+            {
+                setpoint = max_torque_setpoint_nm_;
+            } else {
+                setpoint = data.inverter_torque_limit[index];
+            }
+
+            inv_pointer->handle_command({setpoint, data.speeds_rpm[index]});
             index++;
         }
         // last_general_cmd_time_ = curr_system_millis_;

lib/systems/src/DrivebrainController.cpp

@@ -25,9 +25,7 @@ TorqueControllerOutput_s DrivebrainController::evaluate(const SharedCarState_s &
 
     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);   
     }
 
 
@@ -54,7 +52,8 @@ TorqueControllerOutput_s DrivebrainController::evaluate(const SharedCarState_s &
     else
     {
         _timing_failure = true;
-        output.command = {{0.0f}, {0.0f}};
+        // output.command = {{0.0f}, {0.0f}};
+        output = _emergency_control.evaluate(state);
     }
     return output;
 }

src/main.cpp

@@ -1,6 +1,6 @@
 #ifdef ENABLE_DEBUG_PRINTS
 #define DEBUG_PRINTS true
-#else 
+#else
 #define DEBUG_PRINTS false
 #endif
 
@@ -11,8 +11,7 @@
 #include "FlexCAN_T4.h"
 #include "HyTech_CAN.h"
 #include "MCU_rev15_defs.h"
-// #include "NativeEthernet.h"
-
+#include "pb.h"
 // /* Interfaces */
 #include "DrivebrainETHInterface.h"
 #include "ProtobufMsgInterface.h"
@@ -53,6 +52,7 @@
 /*
     PARAMETER STRUCTS
 */
+using namespace qindesign::network;
 
 const TelemetryInterfaceReadChannels telem_read_channels = {
     .accel1_channel = MCU15_ACCEL1_CHANNEL,
@@ -312,7 +312,7 @@ TorqueControllerCASEWrapper<CircularBufferType> case_wrapper(&case_system);
 // mode 3
 TorqueControllerSimpleLaunch simple_launch;
 // mode 4
-DrivebrainController db_controller(210, 210);
+DrivebrainController db_controller(210);
 
 TCMuxType torque_controller_mux({static_cast<Controller *>(&tc_simple),
                                  static_cast<Controller *>(&tc_vec),
@@ -431,21 +431,23 @@ void loop()
     // single source of truth for the state of the car.
     // no systems or interfaces should write directly to this.
     SharedCarState_s car_state_inst(curr_tick,
-                             steering_system.getSteeringSystemData(),
-                             drivetrain.get_dynamic_data(),
-                             load_cell_interface.getLoadCellForces(),
-                             pedals_system.getPedalsSystemData(),
-                             vn_interface.get_vn_struct(),
-                             db_eth_interface.get_latest_data(),
-                             torque_controller_mux.get_tc_mux_status());
-
+                                    steering_system.getSteeringSystemData(),
+                                    drivetrain.get_dynamic_data(),
+                                    load_cell_interface.getLoadCellForces(),
+                                    load_cell_interface.get_latest_raw_data(),
+                                    pedals_system.getPedalsSystemData(),
+                                    vn_interface.get_vn_struct(),
+                                    db_eth_interface.get_latest_data(),
+                                    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);
 
     tick_all_systems(curr_tick);
-
-    // logger.log_out(static_cast<int>(torque_controller_mux.get_tc_mux_status().current_controller_mode_), curr_tick.millis, 100);
+
     // inverter procedure before entering state machine
     // reset inverters
     if (dashboard.inverterResetButtonPressed() && drivetrain.drivetrain_error_occured())
@@ -505,6 +507,9 @@ void loop()
         Serial.print("Current TC error: ");
         Serial.println(static_cast<int>(torque_controller_mux.get_tc_mux_status().active_error));
         Serial.println();
+        Serial.print("dial state: ");
+        Serial.println(static_cast<int>(dashboard.getDialMode()));
+        Serial.println();
         Serial.println();
     }
 }
@@ -587,6 +592,9 @@ void tick_all_interfaces(const SysTick_s &current_system_tick)
             a1.get().conversions[MCU15_BRAKE2_CHANNEL],
             pedals_system.getMechBrakeActiveThreshold(),
             torque_controller_mux.get_tc_mux_status().active_error);
+
+        ams_interface.tick(current_system_tick);
+
     }
 
     if (t.trigger50) // 50Hz
@@ -660,9 +668,6 @@ void handle_ethernet_interface_comms(const SysTick_s &systick, const hytech_msgs
 {
     // function that will handle receiving and distributing of all messages to all ethernet interfaces
     // via the union message. this is a little bit cursed ngl.
-    // TODO un fuck this and make it more sane
-    // Serial.println("bruh");
-    // handle_ethernet_socket_receive(&protobuf_recv_socket, &recv_pb_stream_union_msg, ethernet_interfaces);
 
     std::function<void(unsigned long, const uint8_t *, size_t, DrivebrainETHInterface &, const pb_msgdesc_t *)> recv_boi = &recv_pb_stream_msg<hytech_msgs_MCUCommandData, DrivebrainETHInterface>;
     handle_ethernet_socket_receive<1024, hytech_msgs_MCUCommandData>(systick, &protobuf_recv_socket, recv_boi, db_eth_interface, hytech_msgs_MCUCommandData_fields);