diff --git a/lib/interfaces/include/LoadCellInterface.h b/lib/interfaces/include/LoadCellInterface.h index f0e260f3..5cfd3794 100644 --- a/lib/interfaces/include/LoadCellInterface.h +++ b/lib/interfaces/include/LoadCellInterface.h @@ -26,6 +26,8 @@ class LoadCellInterface loadCellForcesFiltered_ = veh_vec(); } 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 loadCellConversions_; veh_vec loadCellForcesUnfiltered_; veh_vec loadCellForcesFiltered_; + LoadCellInterfaceRawOutput_s _raw_data; bool FIRSaturated_ = false; }; diff --git a/lib/interfaces/src/DrivebrainETHInterface.cpp b/lib/interfaces/src/DrivebrainETHInterface.cpp index 29aadf49..3626541c 100644 --- 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 - } \ No newline at end of file diff --git a/lib/interfaces/src/LoadCellInterface.cpp b/lib/interfaces/src/LoadCellInterface.cpp index b6aa570f..b3a032ee 100644 --- 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}; } \ No newline at end of file diff --git a/lib/shared_data/include/SharedDataTypes.h b/lib/shared_data/include/SharedDataTypes.h index 8de8f1a4..fc2b1492 100644 --- 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 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 index 7eb7b6e4..ce3e0da5 100644 --- 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 index 675c3517..82278ba8 100644 --- 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 index 4d816a99..5ef95794 100644 --- a/platformio.ini +++ b/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-20T18_54_37/hytech_msgs_pb_lib.tar.gz [env:test_env] platform = native diff --git a/src/main.cpp b/src/main.cpp index 53552b3f..4e7090c6 100644 --- 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 ¤t_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(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(torque_controller_mux.get_tc_mux_status().active_error)); Serial.println(); - Serial.print("dial state: "); - Serial.println(static_cast(dashboard.getDialMode())); - Serial.println(); Serial.println(); } } @@ -591,8 +587,6 @@ void tick_all_interfaces(const SysTick_s ¤t_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 ¤t_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 ¤t_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 recv_boi = &recv_pb_stream_msg; 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(EthParams::default_TCU_ip, EthParams::default_protobuf_send_port, &protobuf_send_socket, out_msg, hytech_msgs_MCUOutputData_fields); } -} +} \ No newline at end of file