added in loadcell data to the output message and added in more metric…

…s for drivebrain comms
hytech-racing · Oct 10, 2024 · a785107 · a785107
1 parent 2c78972
commit a785107
Show file tree

Hide file tree

Showing 8 changed files with 86 additions and 21 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
@@ -109,6 +109,11 @@ struct LoadCellInterfaceOutput_s
     bool FIRSaturated;
 };
 
+struct LoadCellInterfaceRawOutput_s
+{
+    veh_vec<float> raw_load_cell_data;
+};
+
 // Enums
 enum class SteeringSystemStatus_e
 {
@@ -136,6 +141,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;
@@ -153,6 +159,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_rec_time =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_rec_time;
+    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,5 +1,5 @@
 #include "DrivebrainController.h"
-
+// #include <Arduino.h>
 
 TorqueControllerOutput_s DrivebrainController::evaluate(const SharedCarState_s &state)
 {
@@ -15,10 +15,26 @@ 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_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);
+
+    }
+
 
     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-07T23_13_48/hytech_msgs_pb_lib.tar.gz
 
 [env:test_env]
 platform = native

diff --git a/src/main.cpp b/src/main.cpp
@@ -1,6 +1,6 @@
 #ifdef ENABLE_DEBUG_PRINTS
 #define DEBUG_PRINTS true
-#else 
+#else
 #define DEBUG_PRINTS false
 #endif
 
@@ -55,7 +55,6 @@
 */
 using namespace qindesign::network;
 
-
 const TelemetryInterfaceReadChannels telem_read_channels = {
     .accel1_channel = MCU15_ACCEL1_CHANNEL,
     .accel2_channel = MCU15_ACCEL2_CHANNEL,
@@ -346,7 +345,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
@@ -421,7 +420,6 @@ void loop()
     // get latest tick from sys clock
     SysTick_s curr_tick = sys_clock.tick(micros());
 
-
     // process received CAN messages
     process_ring_buffer(CAN2_rxBuffer, CAN_receive_interfaces, curr_tick.millis);
     process_ring_buffer(CAN3_rxBuffer, CAN_receive_interfaces, curr_tick.millis);
@@ -437,6 +435,7 @@ void loop()
                                     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(),
@@ -617,6 +616,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()
@@ -658,18 +662,17 @@ 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.
 
     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.trigger100)
     {
         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);
     }