Feature/drivebrain interface load cell and timing (#113)

* tuning pedals and updated to latest can definition * drivebrain controller creation * added receiving of drivebrain messages * fixing pedals tests * adding drivebrain interface empty mock header * feat(drivebrain_controller): added tests for drivebrain controller latency and jitter * adding in latching logic for the db controller * added in drivebrain interface into main * ensuring that the regen and torque limits dont get applied to the drivebrain controller * fix(drivebrain_controller): fixed drivebrain_controller failure and reset logic * adjusting logic; removing interaction with tc and giving drivebrain more responsbility * fixing oopsie in drivebrain control mode and adding it to main * ready to test on car * fixing header * seemingly working ethernet integration * switching ethernet driver * removing CAN based drivebrain interface, addressing naming for inverter interface updates, updating ethernet interface since only one message is received, and fixing tests for new interface * making the veh_vec constuctor actually a constructor * addressing comments * a little cleanup * added compiler macro flag for debug prints * added in type alias * adding in better handling of timing for drivebrain message latency * updated to latest HT-prot * updating pedal params and to latest HT_proto * added in loadcell data to the output message and added in more metrics for drivebrain comms * updating name and changing loadcell data to int * merge changes * fixing removal of eth interface comms --------- Co-authored-by: sreekara <[email protected]>
hytech-racing · Oct 20, 2024 · 10775c6 · 10775c6
1 parent c631465
commit 10775c6
Show file tree

Hide file tree

Showing 8 changed files with 93 additions and 35 deletions.
diff --git a/lib/interfaces/include/LoadCellInterface.h b/lib/interfaces/include/LoadCellInterface.h
@@ -26,6 +26,8 @@ class LoadCellInterface
         loadCellForcesFiltered_ = veh_vec<float>();
     }
     void tick(const LoadCellInterfaceTick_s &intake);
+    void update_raw_data(const LoadCellInterfaceTick_s &intake);
+    LoadCellInterfaceRawOutput_s get_latest_raw_data() { return _raw_data; }
     LoadCellInterfaceOutput_s getLoadCellForces();
 private:
     /*
@@ -57,6 +59,7 @@ class LoadCellInterface
     veh_vec<AnalogConversion_s> loadCellConversions_;
     veh_vec<float[numFIRTaps_]> loadCellForcesUnfiltered_;
     veh_vec<float> loadCellForcesFiltered_;
+    LoadCellInterfaceRawOutput_s _raw_data;
     bool FIRSaturated_ = false;
 
 };

diff --git a/lib/interfaces/src/DrivebrainETHInterface.cpp b/lib/interfaces/src/DrivebrainETHInterface.cpp
@@ -7,13 +7,20 @@ 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.rpm_data = {shared_state.drivetrain_data.measuredSpeeds[0],
-                    shared_state.drivetrain_data.measuredSpeeds[1],
-                    shared_state.drivetrain_data.measuredSpeeds[2],
-                    shared_state.drivetrain_data.measuredSpeeds[3]};
-
+
+    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.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;
     return out;
 }
 
@@ -27,5 +34,4 @@ void DrivebrainETHInterface::receive_pb_msg(const hytech_msgs_MCUCommandData &ms
     _latest_data.speed_setpoints_rpm = speed_set;
     _latest_data.DB_prev_MCU_recv_millis = msg_in.prev_MCU_recv_millis;
     _latest_data.last_receive_time_millis = curr_millis; // current tick millis
-
 }
diff --git a/lib/interfaces/src/LoadCellInterface.cpp b/lib/interfaces/src/LoadCellInterface.cpp
@@ -34,9 +34,14 @@ void LoadCellInterface::tick(const LoadCellInterfaceTick_s &intake)
 
 LoadCellInterfaceOutput_s LoadCellInterface::getLoadCellForces()
 {
-    return (LoadCellInterfaceOutput_s) {
+    return (LoadCellInterfaceOutput_s){
         .loadCellForcesFiltered = loadCellForcesFiltered_,
         .loadCellConversions = loadCellConversions_,
-        .FIRSaturated = FIRSaturated_
-    };
+        .FIRSaturated = FIRSaturated_};
+}
+
+// this is a hack, i know, i just want all the data.
+void LoadCellInterface::update_raw_data(const LoadCellInterfaceTick_s &intake)
+{
+    _raw_data.raw_load_cell_data = {intake.FLConversion.raw, intake.FRConversion.raw, intake.RLConversion.raw, intake.RRConversion.raw};
 }
diff --git a/lib/shared_data/include/SharedDataTypes.h b/lib/shared_data/include/SharedDataTypes.h
@@ -120,6 +120,11 @@ struct LoadCellInterfaceOutput_s
     bool FIRSaturated;
 };
 
+struct LoadCellInterfaceRawOutput_s
+{
+    veh_vec<int> raw_load_cell_data;
+};
+
 // Enums
 enum class SteeringSystemStatus_e
 {
@@ -147,6 +152,7 @@ struct SharedCarState_s
     SteeringSystemData_s steering_data;
     DrivetrainDynamicReport_s drivetrain_data;
     LoadCellInterfaceOutput_s loadcell_data;
+    LoadCellInterfaceRawOutput_s raw_loadcell_data;
     PedalsSystemData_s pedals_data;
     VectornavData_s vn_data;
     DrivebrainData_s db_data;
@@ -164,6 +170,28 @@ struct SharedCarState_s
           steering_data(_steering_data),
           drivetrain_data(_drivetrain_data),
           loadcell_data(_loadcell_data),
+          raw_loadcell_data({}),
+          pedals_data(_pedals_data),
+          vn_data(_vn_data),
+          db_data(_db_data),
+          tc_mux_status(_tc_mux_status)
+    {
+        // constructor body (if needed)
+    }
+    SharedCarState_s(const SysTick_s &_systick,
+              const SteeringSystemData_s &_steering_data,
+              const DrivetrainDynamicReport_s &_drivetrain_data,
+              const LoadCellInterfaceOutput_s &_loadcell_data,
+              const LoadCellInterfaceRawOutput_s &_raw_loadcell_data,
+              const PedalsSystemData_s &_pedals_data,
+              const VectornavData_s &_vn_data,
+              const DrivebrainData_s &_db_data,
+              const TorqueControllerMuxStatus &_tc_mux_status)
+        : systick(_systick),
+          steering_data(_steering_data),
+          drivetrain_data(_drivetrain_data),
+          loadcell_data(_loadcell_data),
+          raw_loadcell_data(_raw_loadcell_data),
           pedals_data(_pedals_data),
           vn_data(_vn_data),
           db_data(_db_data),

diff --git a/lib/systems/include/DrivebrainController.h b/lib/systems/include/DrivebrainController.h
@@ -27,6 +27,8 @@ class DrivebrainController : public Controller
                          float max_fault_clear_speed_m_s = 1.0,
                          ControllerMode_e assigned_controller_mode = ControllerMode_e::MODE_4)
     {
+        _last_worst_latency_timestamp = 0;
+        _worst_latency_so_far = -1;
         _params = {allowed_latency, max_fault_clear_speed_m_s, assigned_controller_mode};
     }
 
@@ -40,6 +42,8 @@ class DrivebrainController : public Controller
         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;
 };

diff --git a/lib/systems/src/DrivebrainController.cpp b/lib/systems/src/DrivebrainController.cpp
@@ -1,6 +1,5 @@
 #include "DrivebrainController.h"
 
-
 TorqueControllerOutput_s DrivebrainController::evaluate(const SharedCarState_s &state)
 {
 
@@ -15,10 +14,22 @@ TorqueControllerOutput_s DrivebrainController::evaluate(const SharedCarState_s &
     // (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)
     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_timestamp) > 5000)
+    {    
+        _last_worst_latency_timestamp = 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);
+
+    }
+
 
     bool timing_failure = (message_too_latent || no_messages_received || drivebrain_has_not_received_time);
 

diff --git a/platformio.ini b/platformio.ini
@@ -4,7 +4,7 @@ lib_deps_shared =
   git+ssh://[email protected]/hytech-racing/CASE_lib.git#v49
   https://github.com/hytech-racing/shared_firmware_types.git#feb3b83
   https://github.com/hytech-racing/shared_firmware_systems.git#af96a63
-  https://github.com/hytech-racing/HT_proto/releases/download/2024-10-04T04_27_55/hytech_msgs_pb_lib.tar.gz
+  https://github.com/hytech-racing/HT_proto/releases/download/2024-10-20T18_54_37/hytech_msgs_pb_lib.tar.gz
 
 [env:test_env]
 platform = native

diff --git a/src/main.cpp b/src/main.cpp
@@ -11,8 +11,7 @@
 #include "FlexCAN_T4.h"
 #include "HyTech_CAN.h"
 #include "MCU_rev15_defs.h"
-#include "pb.h"
-#include "PrintLogger.h"
+// #include "NativeEthernet.h"
 
 // /* Interfaces */
 #include "DrivebrainETHInterface.h"
@@ -54,8 +53,6 @@
 /*
     PARAMETER STRUCTS
 */
-using namespace qindesign::network;
-
 
 const TelemetryInterfaceReadChannels telem_read_channels = {
     .accel1_channel = MCU15_ACCEL1_CHANNEL,
@@ -347,7 +344,7 @@ void tick_all_systems(const SysTick_s &current_system_tick);
 /* Reset inverters */
 void drivetrain_reset();
 
-void handle_ethernet_interface_comms(const SysTick_s& systick, const hytech_msgs_MCUOutputData& out_msg);
+void handle_ethernet_interface_comms(const SysTick_s &systick, const hytech_msgs_MCUOutputData &out_msg);
 
 /*
     SETUP
@@ -434,19 +431,21 @@ 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(),
+                             pedals_system.getPedalsSystemData(),
+                             vn_interface.get_vn_struct(),
+                             db_eth_interface.get_latest_data(),
+                             torque_controller_mux.get_tc_mux_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())
@@ -506,9 +505,6 @@ 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();
     }
 }
@@ -591,8 +587,6 @@ 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
@@ -614,6 +608,11 @@ void tick_all_interfaces(const SysTick_s &current_system_tick)
                 .RLConversion = sab_interface.rlLoadCell.convert(),
                 .RRConversion = sab_interface.rrLoadCell.convert()});
     }
+    load_cell_interface.update_raw_data((LoadCellInterfaceTick_s){
+        .FLConversion = a2.get().conversions[MCU15_FL_LOADCELL_CHANNEL],
+        .FRConversion = a3.get().conversions[MCU15_FR_LOADCELL_CHANNEL],
+        .RLConversion = sab_interface.rlLoadCell.convert(),
+        .RRConversion = sab_interface.rrLoadCell.convert()});
     // // Untriggered
     main_ecu.read_mcu_status(); // should be executed at the same rate as state machine
                                 // DO NOT call in main_ecu.tick()
@@ -655,19 +654,21 @@ void tick_all_systems(const SysTick_s &current_system_tick)
         fsm.get_state(),
         dashboard.startButtonPressed(),
         vn_interface.get_vn_struct().vn_status);
-
 }
 
-void handle_ethernet_interface_comms(const SysTick_s& systick, const hytech_msgs_MCUOutputData& out_msg)
+void handle_ethernet_interface_comms(const SysTick_s &systick, const hytech_msgs_MCUOutputData &out_msg)
 {
     // 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);
 
-    if(systick.triggers.trigger500)
+    if (systick.triggers.trigger1000)
     {
         handle_ethernet_socket_send_pb<hytech_msgs_MCUOutputData, 1024>(EthParams::default_TCU_ip, EthParams::default_protobuf_send_port, &protobuf_send_socket, out_msg, hytech_msgs_MCUOutputData_fields);
     }
-}
+}