Feature/drivebrain interface load cell and timing

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;
 
 };

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
-
 }

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};
 }

lib/shared_data/include/SharedDataTypes.h

@@ -120,6 +120,11 @@ struct LoadCellInterfaceOutput_s
     bool FIRSaturated;
 };
 
+struct LoadCellInterfaceRawOutput_s
+{
+    veh_vec<float> 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),

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;
 };

lib/systems/src/DrivebrainController.cpp

@@ -1,6 +1,5 @@
 #include "DrivebrainController.h"
 
-
 TorqueControllerOutput_s DrivebrainController::evaluate(const SharedCarState_s &state)
 {
 
@@ -15,10 +14,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);
 

platformio.ini

@@ -4,7 +4,7 @@ lib_deps_shared =
   git+ssh://git@github.com/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

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();
 
 /*
     SETUP
@@ -434,19 +431,17 @@ 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());
-
-    hytech_msgs_MCUOutputData out_eth_msg = db_eth_interface.make_db_msg(car_state_inst);
-
-    handle_ethernet_interface_comms(curr_tick, out_eth_msg);
+                             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());
 
     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 +501,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 +583,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 +604,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 +650,14 @@ 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()
 {
     // 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)
-    {
-        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);
-    }
-}
+}