From bc54fd2259968d9fb83e1e80d834e8c2f0853a7c Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Wed, 4 Sep 2024 22:37:04 -0400 Subject: [PATCH 01/33] tuning pedals and updated to latest can definition --- include/MCU_rev15_defs.h | 12 +-- lib/interfaces/include/DrivebrainInterface.h | 21 +++++ lib/interfaces/include/HytechCANInterface.h | 6 +- lib/interfaces/include/InverterInterface.h | 4 +- lib/interfaces/include/InverterInterface.tpp | 3 +- lib/interfaces/include/TelemetryInterface.h | 6 ++ lib/interfaces/include/VectornavInterface.h | 4 +- lib/interfaces/include/VectornavInterface.tpp | 24 +++--- lib/interfaces/src/DrivebrainInterface.cpp | 0 lib/interfaces/src/TelemetryInterface.cpp | 26 ++++++- .../include/Utility.h | 0 platformio.ini | 2 +- src/main.cpp | 78 +++++++++---------- 13 files changed, 116 insertions(+), 70 deletions(-) create mode 100644 lib/interfaces/include/DrivebrainInterface.h create mode 100644 lib/interfaces/src/DrivebrainInterface.cpp rename lib/{systems => shared_data}/include/Utility.h (100%) diff --git a/include/MCU_rev15_defs.h b/include/MCU_rev15_defs.h index 16ed0f851..f6edeb0fa 100644 --- a/include/MCU_rev15_defs.h +++ b/include/MCU_rev15_defs.h @@ -54,17 +54,17 @@ const unsigned long TELEM_CAN_BAUDRATE = 500000; // All of these values are the PEDAL min/max // the sensor min/max that trip implaus are calculated // in the PedalsSystem constructor -const int ACCEL1_PEDAL_MAX = 3409; // 3330 -const int ACCEL2_PEDAL_MAX = 259; // 388 +const int ACCEL1_PEDAL_MAX = 3491; // 3330 +const int ACCEL2_PEDAL_MAX = 189; // 388 -const int ACCEL1_PEDAL_MIN = 2129; -const int ACCEL2_PEDAL_MIN = 1517; +const int ACCEL1_PEDAL_MIN = 2244; +const int ACCEL2_PEDAL_MIN = 1405; const int BRAKE1_PEDAL_MAX = 1945; // 2200; const int BRAKE2_PEDAL_MAX = 1742; // 2200; -const int BRAKE1_PEDAL_MIN = 1230; // 785; // 1230 to 1750 -const int BRAKE2_PEDAL_MIN = 2437; // 785; // 2450 to 1930 +const int BRAKE1_PEDAL_MIN = 1192; // 785; // 1230 to 1750 +const int BRAKE2_PEDAL_MIN = 2476; // 785; // 2450 to 1930 const int ACCEL1_PEDAL_OOR_MIN = 90; const int ACCEL2_PEDAL_OOR_MIN = 90; diff --git a/lib/interfaces/include/DrivebrainInterface.h b/lib/interfaces/include/DrivebrainInterface.h new file mode 100644 index 000000000..27c55f8ff --- /dev/null +++ b/lib/interfaces/include/DrivebrainInterface.h @@ -0,0 +1,21 @@ +// #ifndef __DRIVEBRAININTERFACE_H__ +// #define __DRIVEBRAININTERFACE_H__ +// #include "Utility.h" +// #include "MessageQueueDefine.h" + +// class DrivebrainInterface +// { + +// public: +// DrivebrainInterface(CANBufferType * msg_output_queue) +// { +// _msg_queue = msg_output_queue; +// } + +// void receive_db_message(CAN_message_t &msg); +// private: +// CANBufferType * _msg_queue; + +// } + +// #endif // __DRIVEBRAININTERFACE_H__ \ No newline at end of file diff --git a/lib/interfaces/include/HytechCANInterface.h b/lib/interfaces/include/HytechCANInterface.h index 7e1c47235..6ae6e911c 100644 --- a/lib/interfaces/include/HytechCANInterface.h +++ b/lib/interfaces/include/HytechCANInterface.h @@ -170,9 +170,9 @@ void process_ring_buffer(BufferType &rx_buffer, const InterfaceType &interfaces, case VN_LAT_LON_CANID: interfaces.vn_interface->retrieve_lat_lon_CAN(recvd_msg); break; - case VN_GPS_TIME_CANID: - interfaces.vn_interface->retrieve_gps_time_CAN(recvd_msg); - break; + // case VN_GPS_TIME_MSG_CANID: + // interfaces.vn_interface->retrieve_gps_time_CAN(recvd_msg); + // break; case VN_STATUS_CANID: interfaces.vn_interface->retrieve_vn_status_CAN(recvd_msg); // double check this break; diff --git a/lib/interfaces/include/InverterInterface.h b/lib/interfaces/include/InverterInterface.h index be9652dae..8853becef 100644 --- a/lib/interfaces/include/InverterInterface.h +++ b/lib/interfaces/include/InverterInterface.h @@ -120,7 +120,7 @@ class InverterInterface int16_t get_speed() { return speed_; } float get_torque_current() { return torque_current_; } - float get_mag_current() { return magnetizing_current_; } + int16_t get_mag_current() { return magnetizing_current_; } float get_actual_torque() { return actual_torque_nm_; } uint16_t get_error_status(); MC_temps get_temps_msg() { return mc_temps_; } @@ -128,7 +128,7 @@ class InverterInterface private: float id110_val_; // for scaling to proper iq and id vals - float torque_current_, magnetizing_current_; // iq and id in A respectively + int16_t torque_current_, magnetizing_current_; // iq and id in A respectively float actual_torque_nm_; /* write setpoints message to the CAN buffer */ void write_cmd_msg_to_queue_(MC_setpoints_command msg); diff --git a/lib/interfaces/include/InverterInterface.tpp b/lib/interfaces/include/InverterInterface.tpp index 7dd835457..724c0f458 100644 --- a/lib/interfaces/include/InverterInterface.tpp +++ b/lib/interfaces/include/InverterInterface.tpp @@ -133,7 +133,8 @@ void InverterInterface::receive_status_msg(CAN_message_t &msg) // to get scalar for this. // torque_current_ = ((float)mc_status.get_torque_current() * id110_val_) / 16384.0; // iq torque_current_ = mc_status.get_torque_current(); - magnetizing_current_ = ((float)mc_status.get_magnetizing_current() * id110_val_) / 16384.0; // id + magnetizing_current_ = mc_status.get_magnetizing_current(); // id + // TODO enable this on the inverters // actual torque in Nm is from the signal we can add in from here: diff --git a/lib/interfaces/include/TelemetryInterface.h b/lib/interfaces/include/TelemetryInterface.h index c9b360eb9..c1a14a4a1 100644 --- a/lib/interfaces/include/TelemetryInterface.h +++ b/lib/interfaces/include/TelemetryInterface.h @@ -108,6 +108,12 @@ class TelemetryInterface InvInt_t *rl, InvInt_t *rr ); + void update_drivetrain_torque_filter_out_telem_CAN_msg( + InvInt_t *fl, + InvInt_t *fr, + InvInt_t *rl, + InvInt_t *rr + ); void update_penthouse_accum_CAN_msg( const AnalogConversion_s ¤t, const AnalogConversion_s &reference diff --git a/lib/interfaces/include/VectornavInterface.h b/lib/interfaces/include/VectornavInterface.h index edf525cd9..ec5a04c3a 100644 --- a/lib/interfaces/include/VectornavInterface.h +++ b/lib/interfaces/include/VectornavInterface.h @@ -45,10 +45,10 @@ class VNInterface void retrieve_uncompLinear_accel_CAN(CAN_message_t &recvd_msg); void retrieve_ypr_CAN(CAN_message_t &recvd_msg); void retrieve_lat_lon_CAN(CAN_message_t &recvd_msg); - void retrieve_gps_time_CAN(CAN_message_t &recvd_msg); + // void retrieve_gps_time_CAN(CAN_message_t &recvd_msg); void retrieve_vn_status_CAN(CAN_message_t &recvd_msg); void retrieve_vn_ecef_pos_xy_CAN(CAN_message_t &recvd_msg); - void retrieve_vn_ecef_pos_z_CAN(CAN_message_t &recvd_msg); + // void retrieve_vn_ecef_pos_z_CAN(CAN_message_t &recvd_msg); void receive_ang_rates_CAN(CAN_message_t &recvd_msg); // getters vector_nav get_vn_struct(); diff --git a/lib/interfaces/include/VectornavInterface.tpp b/lib/interfaces/include/VectornavInterface.tpp index be07a4e08..80e580eeb 100644 --- a/lib/interfaces/include/VectornavInterface.tpp +++ b/lib/interfaces/include/VectornavInterface.tpp @@ -48,12 +48,12 @@ void VNInterface::retrieve_lat_lon_CAN(CAN_message_t &recvd_msg) vn_data.longitude = HYTECH_vn_gps_lon_ro_fromS(lat_lon_data.vn_gps_lon_ro); } -template -void VNInterface::retrieve_gps_time_CAN(CAN_message_t &recvd_msg) { - VN_GPS_TIME_t gps_time_data; - Unpack_VN_GPS_TIME_hytech(&gps_time_data, recvd_msg.buf, recvd_msg.len); - vn_data.gps_time = gps_time_data.vn_gps_time; -} +// template +// void VNInterface::retrieve_gps_time_CAN(CAN_message_t &recvd_msg) { +// VN_GPS_TIME_MSG_CANID gps_time_data; +// Unpack_VN_GPS_TIME_MSG_hytech(&gps_time_data, recvd_msg.buf, recvd_msg.len); +// vn_data.gps_time = gps_time_data.vn_gps_time; +// } template void VNInterface::retrieve_vn_status_CAN(CAN_message_t &recvd_msg) { @@ -70,12 +70,12 @@ void VNInterface::retrieve_vn_ecef_pos_xy_CAN(CAN_message_t &recv vn_data.ecef_coords[1] = HYTECH_vn_ecef_pos_y_ro_fromS(ecef_xy.vn_ecef_pos_y_ro); } -template -void VNInterface::retrieve_vn_ecef_pos_z_CAN(CAN_message_t &recvd_msg) { - VN_ECEF_POS_Z_t ecef_z; - Unpack_VN_ECEF_POS_Z_hytech(&ecef_z, recvd_msg.buf, recvd_msg.len); - vn_data.ecef_coords[2] = HYTECH_vn_ecef_pos_z_ro_fromS(ecef_z.vn_ecef_pos_z_ro); -} +// template +// void VNInterface::retrieve_vn_ecef_pos_z_CAN(CAN_message_t &recvd_msg) { +// VN_ECEF_POS_Z_t ecef_z; +// Unpack_VN_ECEF_POS_Z_hytech(&ecef_z, recvd_msg.buf, recvd_msg.len); +// vn_data.ecef_coords[2] = HYTECH_vn_ecef_pos_z_ro_fromS(ecef_z.vn_ecef_pos_z_ro); +// } template void VNInterface::receive_ang_rates_CAN(CAN_message_t & recvd_msg) diff --git a/lib/interfaces/src/DrivebrainInterface.cpp b/lib/interfaces/src/DrivebrainInterface.cpp new file mode 100644 index 000000000..e69de29bb diff --git a/lib/interfaces/src/TelemetryInterface.cpp b/lib/interfaces/src/TelemetryInterface.cpp index b6c9ff0fb..fe4e8f183 100644 --- a/lib/interfaces/src/TelemetryInterface.cpp +++ b/lib/interfaces/src/TelemetryInterface.cpp @@ -165,14 +165,31 @@ void TelemetryInterface::update_drivetrain_torque_telem_CAN_msg( // TODO: change this to use actual torque values from inverter // Torque current just temporary for gearbox seal validation DRIVETRAIN_TORQUE_TELEM_t torque; - torque.fl_motor_torque_ro = HYTECH_fl_motor_torque_ro_toS(fl->get_torque_current()); - torque.fr_motor_torque_ro = HYTECH_fr_motor_torque_ro_toS(fr->get_torque_current()); - torque.rl_motor_torque_ro = HYTECH_rl_motor_torque_ro_toS(rl->get_torque_current()); - torque.rr_motor_torque_ro = HYTECH_rr_motor_torque_ro_toS(rr->get_torque_current()); + torque.fl_motor_torque = fl->get_torque_current(); + torque.fr_motor_torque = fr->get_torque_current(); + torque.rl_motor_torque = rl->get_torque_current(); + torque.rr_motor_torque = rr->get_torque_current(); enqueue_new_CAN(&torque, &Pack_DRIVETRAIN_TORQUE_TELEM_hytech); } +void TelemetryInterface::update_drivetrain_torque_filter_out_telem_CAN_msg( + InvInt_t* fl, + InvInt_t* fr, + InvInt_t* rl, + InvInt_t* rr) +{ + // TODO: change this to use actual torque values from inverter + // Torque current just temporary for gearbox seal validation + DRIVETRAIN_FILTER_OUT_TORQUE_TEL_t torque; + torque.fl_motor_torque = fl->get_mag_current(); + torque.fr_motor_torque = fr->get_mag_current(); + torque.rl_motor_torque = rl->get_mag_current(); + torque.rr_motor_torque = rr->get_mag_current(); + + enqueue_new_CAN(&torque, &Pack_DRIVETRAIN_FILTER_OUT_TORQUE_TEL_hytech); +} + // Pack_PENTHOUSE_ACCUM_MSG_hytech void TelemetryInterface::update_penthouse_accum_CAN_msg(const AnalogConversion_s ¤t, const AnalogConversion_s &reference) { @@ -307,6 +324,7 @@ void TelemetryInterface::tick(const AnalogConversionPacket_s<8> &adc1, update_drivetrain_err_status_CAN_msg(fl, fr, rl, rr); update_drivetrain_status_telem_CAN_msg(fl, fr, rl, rr, accel_implaus, brake_implaus, accel_per, brake_per); update_drivetrain_torque_telem_CAN_msg(fl, fr, rl, rr); + update_drivetrain_torque_filter_out_telem_CAN_msg(fl, fr, rl, rr); update_penthouse_accum_CAN_msg(adc1.conversions[channels_.current_channel], adc1.conversions[channels_.current_ref_channel]); diff --git a/lib/systems/include/Utility.h b/lib/shared_data/include/Utility.h similarity index 100% rename from lib/systems/include/Utility.h rename to lib/shared_data/include/Utility.h diff --git a/platformio.ini b/platformio.ini index 6d85fc724..0834adbb3 100644 --- a/platformio.ini +++ b/platformio.ini @@ -59,7 +59,7 @@ lib_deps = https://github.com/RCMast3r/spi_libs#2214fee https://github.com/tonton81/FlexCAN_T4#b928bcb https://github.com/RCMast3r/hytech_can#testing_new_inv_ids - https://github.com/hytech-racing/HT_CAN/releases/download/108/can_lib.tar.gz + https://github.com/hytech-racing/HT_CAN/releases/download/120/can_lib.tar.gz git+ssh://git@github.com/hytech-racing/CASE_lib.git#v49 [env:test_interfaces] diff --git a/src/main.cpp b/src/main.cpp index 9af88b81d..6656951fe 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -428,45 +428,45 @@ void loop() send_all_CAN_msgs(CAN3_txBuffer, &TELEM_CAN); // Basic debug prints - if (curr_tick.triggers.trigger5) - { - Serial.print("Steering system reported angle (deg): "); - Serial.println(steering_system.getSteeringSystemData().angle); - Serial.print("Steering system status: "); - Serial.println(static_cast(steering_system.getSteeringSystemData().status)); - Serial.print("Primary sensor angle: "); - Serial.println(steering1.convert().angle); - Serial.print("Secondary sensor angle: "); - Serial.print(a1.get().conversions[MCU15_STEERING_CHANNEL].conversion); - Serial.print(" raw: "); - Serial.println(a1.get().conversions[MCU15_STEERING_CHANNEL].raw); - Serial.print("Sensor divergence: "); - Serial.println(steering1.convert().angle - a1.get().conversions[MCU15_STEERING_CHANNEL].conversion); - Serial.println(); - Serial.println("Pedal outputs:"); - Serial.print("Accel 1 raw: "); - Serial.println(a1.get().conversions[MCU15_ACCEL1_CHANNEL].raw); - Serial.print("Accel 2 raw: "); - Serial.println(a1.get().conversions[MCU15_ACCEL2_CHANNEL].raw); - Serial.print("Accel percent: "); - Serial.println(pedals_system.getPedalsSystemDataCopy().accelPercent); - Serial.print("Brake 1 raw: "); - Serial.println(a1.get().conversions[MCU15_BRAKE1_CHANNEL].raw); - Serial.print("Brake 2 raw: "); - Serial.println(a1.get().conversions[MCU15_BRAKE2_CHANNEL].raw); - Serial.print("Brake percent: "); - Serial.println(pedals_system.getPedalsSystemDataCopy().brakePercent); - Serial.println(); - Serial.print("Derating factor: "); - Serial.println(ams_interface.get_acc_derate_factor()); - Serial.print("Filtered min cell voltage: "); - Serial.println(ams_interface.get_filtered_min_cell_voltage()); - Serial.print("Filtered max cell temp: "); - Serial.println(ams_interface.get_filtered_max_cell_temp()); - Serial.println(); - - Serial.println(); - } + // if (curr_tick.triggers.trigger5) + // { + // Serial.print("Steering system reported angle (deg): "); + // Serial.println(steering_system.getSteeringSystemData().angle); + // Serial.print("Steering system status: "); + // Serial.println(static_cast(steering_system.getSteeringSystemData().status)); + // Serial.print("Primary sensor angle: "); + // Serial.println(steering1.convert().angle); + // Serial.print("Secondary sensor angle: "); + // Serial.print(a1.get().conversions[MCU15_STEERING_CHANNEL].conversion); + // Serial.print(" raw: "); + // Serial.println(a1.get().conversions[MCU15_STEERING_CHANNEL].raw); + // Serial.print("Sensor divergence: "); + // Serial.println(steering1.convert().angle - a1.get().conversions[MCU15_STEERING_CHANNEL].conversion); + // Serial.println(); + // Serial.println("Pedal outputs:"); + // Serial.print("Accel 1 raw: "); + // Serial.println(a1.get().conversions[MCU15_ACCEL1_CHANNEL].raw); + // Serial.print("Accel 2 raw: "); + // Serial.println(a1.get().conversions[MCU15_ACCEL2_CHANNEL].raw); + // Serial.print("Accel percent: "); + // Serial.println(pedals_system.getPedalsSystemDataCopy().accelPercent); + // Serial.print("Brake 1 raw: "); + // Serial.println(a1.get().conversions[MCU15_BRAKE1_CHANNEL].raw); + // Serial.print("Brake 2 raw: "); + // Serial.println(a1.get().conversions[MCU15_BRAKE2_CHANNEL].raw); + // Serial.print("Brake percent: "); + // Serial.println(pedals_system.getPedalsSystemDataCopy().brakePercent); + // Serial.println(); + // Serial.print("Derating factor: "); + // Serial.println(ams_interface.get_acc_derate_factor()); + // Serial.print("Filtered min cell voltage: "); + // Serial.println(ams_interface.get_filtered_min_cell_voltage()); + // Serial.print("Filtered max cell temp: "); + // Serial.println(ams_interface.get_filtered_max_cell_temp()); + // Serial.println(); + + // Serial.println(); + // } } From 2c46b83944f22bf8c45b985fb84bb3c768e03843 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Thu, 5 Sep 2024 17:00:45 -0400 Subject: [PATCH 02/33] drivebrain controller creation --- lib/interfaces/include/DrivebrainInterface.h | 43 ++++++++++++-------- lib/interfaces/include/HytechCANInterface.h | 2 + lib/interfaces/src/DrivebrainInterface.cpp | 25 ++++++++++++ lib/shared_data/include/DrivebrainData.h | 14 +++++++ lib/shared_data/include/Utility.h | 17 ++++++++ lib/systems/include/DrivebrainController.h | 34 ++++++++++++++++ lib/systems/include/TorqueControllers.h | 3 +- lib/systems/src/DrivebrainController.cpp | 26 ++++++++++++ platformio.ini | 2 +- src/main.cpp | 4 +- 10 files changed, 150 insertions(+), 20 deletions(-) create mode 100644 lib/shared_data/include/DrivebrainData.h create mode 100644 lib/systems/include/DrivebrainController.h create mode 100644 lib/systems/src/DrivebrainController.cpp diff --git a/lib/interfaces/include/DrivebrainInterface.h b/lib/interfaces/include/DrivebrainInterface.h index 27c55f8ff..d495d3367 100644 --- a/lib/interfaces/include/DrivebrainInterface.h +++ b/lib/interfaces/include/DrivebrainInterface.h @@ -1,21 +1,30 @@ -// #ifndef __DRIVEBRAININTERFACE_H__ -// #define __DRIVEBRAININTERFACE_H__ -// #include "Utility.h" -// #include "MessageQueueDefine.h" +#ifndef __DRIVEBRAININTERFACE_H__ +#define __DRIVEBRAININTERFACE_H__ +#include "DrivebrainData.h" +#include "MessageQueueDefine.h" -// class DrivebrainInterface -// { - -// public: -// DrivebrainInterface(CANBufferType * msg_output_queue) -// { -// _msg_queue = msg_output_queue; -// } +class DrivebrainInterface +{ + public: + DrivebrainInterface(CANBufferType * msg_output_queue) + { + _msg_queue = msg_output_queue; + _current_drivebrain_data.last_speed_setpoint_receive_time_millis= -1; + _current_drivebrain_data.last_torque_lim_receive_time_millis= -1; + _current_drivebrain_data.speed_setpoints_rpm = {}; + _current_drivebrain_data.torque_limits_nm = {}; + } -// void receive_db_message(CAN_message_t &msg); -// private: -// CANBufferType * _msg_queue; + void receive_db_torque_lim_message(CAN_message_t &msg, unsigned long curr_millis); + void receive_db_speed_setpoint_message(CAN_message_t& msg, unsigned long curr_millis); + + DrivebrainData get_latest_db_data() {return _current_drivebrain_data; } + + private: -// } + DrivebrainData _current_drivebrain_data; + CANBufferType * _msg_queue; -// #endif // __DRIVEBRAININTERFACE_H__ \ No newline at end of file +}; + +#endif // __DRIVEBRAININTERFACE_H__ \ No newline at end of file diff --git a/lib/interfaces/include/HytechCANInterface.h b/lib/interfaces/include/HytechCANInterface.h index 6ae6e911c..35338a84e 100644 --- a/lib/interfaces/include/HytechCANInterface.h +++ b/lib/interfaces/include/HytechCANInterface.h @@ -14,6 +14,7 @@ #include "VectornavInterface.h" #include "HT08_CASE_types.h" #include "MessageQueueDefine.h" +#include "DrivebrainInterface.h" /* struct holding interfaces processed by process_ring_buffer() FL = MC1 @@ -32,6 +33,7 @@ struct CANInterfaces DashboardInterface *dash_interface; AMSInterface *ams_interface; SABInterface *sab_interface; + DrivebrainInterface *db_interface; }; // the goal with the can interface is that there exists a receive call that appends to a circular buffer diff --git a/lib/interfaces/src/DrivebrainInterface.cpp b/lib/interfaces/src/DrivebrainInterface.cpp index e69de29bb..12bb0b5d6 100644 --- a/lib/interfaces/src/DrivebrainInterface.cpp +++ b/lib/interfaces/src/DrivebrainInterface.cpp @@ -0,0 +1,25 @@ +#include "DrivebrainInterface.h" +#include "hytech.h" + +void DrivebrainInterface::receive_db_torque_lim_message(CAN_message_t& msg, unsigned long curr_millis) +{ + DRIVEBRAIN_TORQUE_LIM_INPUT_t db_data; + Unpack_DRIVEBRAIN_TORQUE_LIM_INPUT_hytech(&db_data, msg.buf, msg.len); + _current_drivebrain_data.last_torque_lim_receive_time_millis = curr_millis; + + auto fl = HYTECH_drivebrain_torque_fl_ro_fromS(db_data.drivebrain_torque_fl_ro); + auto fr = HYTECH_drivebrain_torque_fr_ro_fromS(db_data.drivebrain_torque_fr_ro); + auto rl = HYTECH_drivebrain_torque_rl_ro_fromS(db_data.drivebrain_torque_rl_ro); + auto rr = HYTECH_drivebrain_torque_rr_ro_fromS(db_data.drivebrain_torque_rr_ro); + _current_drivebrain_data.torque_limits_nm.construct(fl, fr, rl, rr); +} + +void DrivebrainInterface::receive_db_speed_setpoint_message(CAN_message_t& msg, unsigned long curr_millis) +{ + DRIVEBRAIN_SPEED_SET_INPUT_t db_data; + Unpack_DRIVEBRAIN_SPEED_SET_INPUT_hytech(&db_data, msg.buf, msg.len); + + _current_drivebrain_data.last_speed_setpoint_receive_time_millis = curr_millis; + _current_drivebrain_data.speed_setpoints_rpm.construct(db_data.drivebrain_set_rpm_fl, db_data.drivebrain_set_rpm_fr, db_data.drivebrain_set_rpm_rl, db_data.drivebrain_set_rpm_rr); + +} \ No newline at end of file diff --git a/lib/shared_data/include/DrivebrainData.h b/lib/shared_data/include/DrivebrainData.h new file mode 100644 index 000000000..0116eef7d --- /dev/null +++ b/lib/shared_data/include/DrivebrainData.h @@ -0,0 +1,14 @@ +#ifndef __DRIVEBRAINDATA_H__ +#define __DRIVEBRAINDATA_H__ + +#include "Utility.h" + +struct DrivebrainData +{ + unsigned long last_torque_lim_receive_time_millis; + unsigned long last_speed_setpoint_receive_time_millis; + veh_vec torque_limits_nm; + veh_vec speed_setpoints_rpm; +}; + +#endif // __DRIVEBRAINDATA_H__ \ No newline at end of file diff --git a/lib/shared_data/include/Utility.h b/lib/shared_data/include/Utility.h index dc2a9d508..1be627f6b 100644 --- a/lib/shared_data/include/Utility.h +++ b/lib/shared_data/include/Utility.h @@ -18,6 +18,23 @@ struct veh_vec T FR; T RL; T RR; + + void construct(T FL, T FR, T RL, T RR) + { + FL = FL; + FR = FR; + RL = RL; + RR = RR; + } + + /// @brief copy values to array in FL, FR, RL, RR order + void copy_to_arr(T(&arr_out)[4]) + { + arr_out[0]=FL; + arr_out[1]=FR; + arr_out[2]=RL; + arr_out[3]=RR; + } }; template diff --git a/lib/systems/include/DrivebrainController.h b/lib/systems/include/DrivebrainController.h new file mode 100644 index 000000000..5f8d35c46 --- /dev/null +++ b/lib/systems/include/DrivebrainController.h @@ -0,0 +1,34 @@ +#ifndef __DRIVEBRAINCONTROLLER_H__ +#define __DRIVEBRAINCONTROLLER_H__ + +#include "DrivebrainInterface.h" +#include "TorqueControllers.h" + +// TODO - [ ] 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 - [ ] set +class DrivebrainController : public TorqueController +{ +public: + void tick(const SysTick_s &sys_tick, DrivebrainData db_input); + + DrivebrainController(TorqueControllerOutput_s &writeout, unsigned long allowed_latency, unsigned long allowed_jitter) : _writeout(writeout) + { + _params = {allowed_jitter, allowed_latency}; + } + +private: + struct + { + unsigned long allowed_jitter; + unsigned long allowed_latency; + } _params; + + unsigned long _last_sent_torque_lim_millis = -1; + unsigned long _last_sent_speed_setpoint_millis = -1; + + TorqueControllerOutput_s &_writeout; +}; + +#endif // __DRIVEBRAINCONTROLLER_H__ \ No newline at end of file diff --git a/lib/systems/include/TorqueControllers.h b/lib/systems/include/TorqueControllers.h index 198d4c118..54d0ea39b 100644 --- a/lib/systems/include/TorqueControllers.h +++ b/lib/systems/include/TorqueControllers.h @@ -76,7 +76,8 @@ enum TorqueController_e TC_SLIP_LAUNCH = 4, TC_LOOKUP_LAUNCH = 5, TC_CASE_SYSTEM = 6, - TC_NUM_CONTROLLERS = 7, + TC_DRIVEBRAIN= 7, + TC_NUM_CONTROLLERS = 8, }; enum class LaunchStates_e diff --git a/lib/systems/src/DrivebrainController.cpp b/lib/systems/src/DrivebrainController.cpp new file mode 100644 index 000000000..cb9268aa6 --- /dev/null +++ b/lib/systems/src/DrivebrainController.cpp @@ -0,0 +1,26 @@ +#include "DrivebrainController.h" + +void DrivebrainController::tick(const SysTick_s &sys_tick, DrivebrainData db_input) +{ + _writeout.ready = false; + + bool speed_setpoint_too_latent = (::abs((int)(sys_tick.millis - db_input.last_speed_setpoint_receive_time_millis)) > (int)_params.allowed_latency); + bool torque_setpoint_too_latent = (::abs((int)(sys_tick.millis - db_input.last_torque_lim_receive_time_millis)) > (int)_params.allowed_latency); + bool msg_jitter_too_high = (::abs((int)(db_input.last_speed_setpoint_receive_time_millis - db_input.last_torque_lim_receive_time_millis)) > (int)_params.allowed_jitter); + + if (!(speed_setpoint_too_latent || torque_setpoint_too_latent || msg_jitter_too_high)) + { + _writeout.ready = true; + + _last_sent_speed_setpoint_millis = db_input.last_speed_setpoint_receive_time_millis; + _last_sent_torque_lim_millis = db_input.last_torque_lim_receive_time_millis; + + db_input.speed_setpoints_rpm.copy_to_arr(_writeout.command.speeds_rpm); + db_input.torque_limits_nm.copy_to_arr(_writeout.command.torqueSetpoints); + } + else + { + _writeout.ready = false; + _writeout.command = {}; // set command to all zeros if bad latency is apparent + } +} \ No newline at end of file diff --git a/platformio.ini b/platformio.ini index 0834adbb3..200bdb795 100644 --- a/platformio.ini +++ b/platformio.ini @@ -59,7 +59,7 @@ lib_deps = https://github.com/RCMast3r/spi_libs#2214fee https://github.com/tonton81/FlexCAN_T4#b928bcb https://github.com/RCMast3r/hytech_can#testing_new_inv_ids - https://github.com/hytech-racing/HT_CAN/releases/download/120/can_lib.tar.gz + https://github.com/hytech-racing/HT_CAN/releases/download/121/can_lib.tar.gz git+ssh://git@github.com/hytech-racing/CASE_lib.git#v49 [env:test_interfaces] diff --git a/src/main.cpp b/src/main.cpp index 6656951fe..a173aa722 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -24,6 +24,7 @@ #include "SABInterface.h" #include "VectornavInterface.h" #include "LoadCellInterface.h" +#include "DrivebrainInterface.h" /* Systems */ #include "SysClock.h" @@ -301,7 +302,8 @@ MCUStateMachine fsm(&buzzer, &drivetrain, &dashboard, &pedals_system // GROUPING STRUCTS (To limit parameter count in utilizing functions) // */ -CANInterfaces CAN_receive_interfaces = {&inv.fl, &inv.fr, &inv.rl, &inv.rr, &vn_interface, &dashboard, &ams_interface, &sab_interface}; +DrivebrainInterface db_interface(&CAN3_txBuffer); +CANInterfaces CAN_receive_interfaces = {&inv.fl, &inv.fr, &inv.rl, &inv.rr, &vn_interface, &dashboard, &ams_interface, &sab_interface, &db_interface}; /* FUNCTION DEFINITIONS From a8fff980ca355a2da8ec8be33a2cab18d0854a64 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Thu, 5 Sep 2024 17:59:42 -0400 Subject: [PATCH 03/33] added receiving of drivebrain messages --- lib/interfaces/include/HytechCANInterface.h | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/lib/interfaces/include/HytechCANInterface.h b/lib/interfaces/include/HytechCANInterface.h index 35338a84e..084722cff 100644 --- a/lib/interfaces/include/HytechCANInterface.h +++ b/lib/interfaces/include/HytechCANInterface.h @@ -181,6 +181,12 @@ void process_ring_buffer(BufferType &rx_buffer, const InterfaceType &interfaces, case VN_ANGULAR_RATE_CANID: interfaces.vn_interface->receive_ang_rates_CAN(recvd_msg); break; + case DRIVEBRAIN_TORQUE_LIM_INPUT_CANID: + interfaces.db_interface->receive_db_torque_lim_message(recvd_msg, curr_millis); + break; + case DRIVEBRAIN_SPEED_SET_INPUT_CANID: + interfaces.db_interface->receive_db_speed_setpoint_message(recvd_msg, curr_millis); + break; } } } From ed89e6a77d5a98b1fdeb6be47b6bcd97d97a4a66 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Thu, 5 Sep 2024 22:43:36 -0400 Subject: [PATCH 04/33] fixing pedals tests --- lib/mock_interfaces/DrivebrainInterface.h | 0 lib/systems/include/DrivebrainController.h | 4 +-- platformio.ini | 6 ++++- test/test_systems/main.cpp | 2 +- test/test_systems/pedals_system_test.h | 29 ++++++++++++++-------- 5 files changed, 26 insertions(+), 15 deletions(-) create mode 100644 lib/mock_interfaces/DrivebrainInterface.h diff --git a/lib/mock_interfaces/DrivebrainInterface.h b/lib/mock_interfaces/DrivebrainInterface.h new file mode 100644 index 000000000..e69de29bb diff --git a/lib/systems/include/DrivebrainController.h b/lib/systems/include/DrivebrainController.h index 5f8d35c46..14ad6dc09 100644 --- a/lib/systems/include/DrivebrainController.h +++ b/lib/systems/include/DrivebrainController.h @@ -3,16 +3,16 @@ #include "DrivebrainInterface.h" #include "TorqueControllers.h" +#include "DrivebrainData.h" // TODO - [ ] 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 - [ ] set class DrivebrainController : public TorqueController { public: void tick(const SysTick_s &sys_tick, DrivebrainData db_input); - + DrivebrainController(TorqueControllerOutput_s &writeout, unsigned long allowed_latency, unsigned long allowed_jitter) : _writeout(writeout) { _params = {allowed_jitter, allowed_latency}; diff --git a/platformio.ini b/platformio.ini index 200bdb795..2889f04dd 100644 --- a/platformio.ini +++ b/platformio.ini @@ -5,7 +5,11 @@ build_src_filter = -<**/*.c> -<**/*.cpp> build_unflags = -std=gnu++11 -build_flags = -std=c++17 -g + +build_flags = + -std=c++17 + -g + -D TESTING_SYSTEMS lib_ignore = interfaces-lib test_ignore= diff --git a/test/test_systems/main.cpp b/test/test_systems/main.cpp index a1d174e6d..82c7e7bba 100644 --- a/test/test_systems/main.cpp +++ b/test/test_systems/main.cpp @@ -2,7 +2,7 @@ #include #include "state_machine_test.h" -// #include "pedals_system_test.h" +#include "pedals_system_test.h" #include "torque_controller_mux_test.h" #include "drivetrain_system_test.h" #include "safety_system_test.h" diff --git a/test/test_systems/pedals_system_test.h b/test/test_systems/pedals_system_test.h index afbb8a121..3cc0df19f 100644 --- a/test/test_systems/pedals_system_test.h +++ b/test/test_systems/pedals_system_test.h @@ -273,12 +273,15 @@ TEST(PedalsSystemTesting, test_accel_and_brake_pressed_at_same_time_and_activati TEST(PedalsSystemTesting, test_implausibility_duration) { AnalogConversion_s test_accel1_val = {2583, 0.37, AnalogSensorStatus_e::ANALOG_SENSOR_GOOD}; - AnalogConversion_s test_accel2_val = {1068, 0.37, AnalogSensorStatus_e::ANALOG_SENSOR_GOOD}; - AnalogConversion_s test_brake_val = {1510, get_pedal_conversion_val(BRAKE1_PEDAL_MIN, BRAKE1_PEDAL_MAX, 1510), AnalogSensorStatus_e::ANALOG_SENSOR_GOOD}; + AnalogConversion_s test_accel2_val = {1068, 1.00, AnalogSensorStatus_e::ANALOG_SENSOR_GOOD}; + AnalogConversion_s test_brake_val = {1510, get_pedal_conversion_val(BRAKE1_PEDAL_MIN, BRAKE1_PEDAL_MAX, BRAKE1_PEDAL_MAX), AnalogSensorStatus_e::ANALOG_SENSOR_GOOD}; PedalsSystem pedals(get_real_accel_pedal_params(), get_real_brake_pedal_params()); auto data = pedals.evaluate_pedals(test_accel1_val, test_accel1_val, test_brake_val, test_brake_val, 1000); + + std::cout << data.accelPressed < Date: Thu, 5 Sep 2024 23:13:02 -0400 Subject: [PATCH 05/33] adding drivebrain interface empty mock header --- lib/mock_interfaces/DrivebrainInterface.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/lib/mock_interfaces/DrivebrainInterface.h b/lib/mock_interfaces/DrivebrainInterface.h index e69de29bb..ba6fb5ca0 100644 --- a/lib/mock_interfaces/DrivebrainInterface.h +++ b/lib/mock_interfaces/DrivebrainInterface.h @@ -0,0 +1,5 @@ +#ifndef __DRIVEBRAININTERFACE_H__ +#define __DRIVEBRAININTERFACE_H__ + + +#endif // __DRIVEBRAININTERFACE_H__ \ No newline at end of file From a8846658d5adc5175e267fbaef79b0d1d25e4058 Mon Sep 17 00:00:00 2001 From: sreekara Date: Fri, 6 Sep 2024 00:14:03 -0400 Subject: [PATCH 06/33] feat(drivebrain_controller): added tests for drivebrain controller latency and jitter --- .../test_systems/drivebrain_controller_test.h | 79 +++++++++++++++++++ test/test_systems/main.cpp | 1 + 2 files changed, 80 insertions(+) create mode 100644 test/test_systems/drivebrain_controller_test.h diff --git a/test/test_systems/drivebrain_controller_test.h b/test/test_systems/drivebrain_controller_test.h new file mode 100644 index 000000000..f97e18024 --- /dev/null +++ b/test/test_systems/drivebrain_controller_test.h @@ -0,0 +1,79 @@ +#ifndef DRIVEBRAIN_CONTROLLER_TEST +#define DRIVEBRAIN_CONTROLLER_TEST +#include +#include +#include + + +void runTick(unsigned int allowed_latency, unsigned int allowed_jitter, float last_torque_lim_receive_time_millis, float last_speed_setpoint_receive_time_millis, TorqueControllerOutput_s *torque_controller_output_s) { + DrivebrainController controller(*torque_controller_output_s, allowed_latency, allowed_jitter); + DrivebrainData data; + data.last_torque_lim_receive_time_millis = last_torque_lim_receive_time_millis; + data.last_speed_setpoint_receive_time_millis = last_speed_setpoint_receive_time_millis; + data.torque_limits_nm = {1, 1, 1, 1}; + data.speed_setpoints_rpm = {1, 1, 1, 1}; + + SysTick_s systick; + systick.millis = 1000; + systick.micros = 1000; + controller.tick(systick, data); +} + +TEST(DrivebrainControllerTesting, signals_sent_within_range) { + TorqueControllerOutput_s torque_controller_output_s = {}; + runTick(10, 10, 1001, 1009, &torque_controller_output_s); + + EXPECT_TRUE(torque_controller_output_s.ready); + EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 1); +} + +TEST(DrivebrainControllerTesting, speed_setpoint_too_latent) { + TorqueControllerOutput_s torque_controller_output_s = {}; + runTick(5, 5, 1001, 1006, &torque_controller_output_s); + + EXPECT_FALSE(torque_controller_output_s.ready); + EXPECT_NE(torque_controller_output_s.command.speeds_rpm[0], 1); +} + +TEST(DrivebrainControllerTesting, torque_setpoint_too_latent) { + TorqueControllerOutput_s torque_controller_output_s = {}; + runTick(5, 5, 1006, 1001, &torque_controller_output_s); + + EXPECT_FALSE(torque_controller_output_s.ready); + EXPECT_NE(torque_controller_output_s.command.speeds_rpm[0], 1); +} + +TEST(DrivebrainControllerTesting, msg_jitter_too_high) { + TorqueControllerOutput_s torque_controller_output_s = {}; + runTick(10, 5, 1001, 1009, &torque_controller_output_s); + + EXPECT_FALSE(torque_controller_output_s.ready); + EXPECT_NE(torque_controller_output_s.command.speeds_rpm[0], 1); +} + +TEST(DrivebrainControllerTesting, speed_setpoint_too_latent_and_torque_setpoint_too_latent) { + TorqueControllerOutput_s torque_controller_output_s = {}; + runTick(5, 10, 1001, 1015, &torque_controller_output_s); + + EXPECT_FALSE(torque_controller_output_s.ready); + EXPECT_NE(torque_controller_output_s.command.speeds_rpm[0], 1); +} + + +TEST(DrivebrainControllerTesting, speed_setpoint_too_latent_and_msg_jitter_too_high) { + TorqueControllerOutput_s torque_controller_output_s = {}; + runTick(10, 5, 1011, 1001, &torque_controller_output_s); + + EXPECT_FALSE(torque_controller_output_s.ready); + EXPECT_NE(torque_controller_output_s.command.speeds_rpm[0], 1); +} + +TEST(DrivebrainControllerTesting, torque_setpoint_too_latent_and_msg_jitter_too_high) { + TorqueControllerOutput_s torque_controller_output_s = {}; + runTick(10, 5, 1001, 1011, &torque_controller_output_s); + + EXPECT_FALSE(torque_controller_output_s.ready); + EXPECT_NE(torque_controller_output_s.command.speeds_rpm[0], 1); +} + +#endif DRIVEBRAIN_CONTROLLER_TEST \ No newline at end of file diff --git a/test/test_systems/main.cpp b/test/test_systems/main.cpp index 82c7e7bba..0753b5400 100644 --- a/test/test_systems/main.cpp +++ b/test/test_systems/main.cpp @@ -4,6 +4,7 @@ #include "state_machine_test.h" #include "pedals_system_test.h" #include "torque_controller_mux_test.h" +#include "drivebrain_controller_test.h" #include "drivetrain_system_test.h" #include "safety_system_test.h" // #include "test_CASE.h" From 91bf533d78b226b0c2289360e16c4a7e03aa8115 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Fri, 6 Sep 2024 00:16:36 -0400 Subject: [PATCH 07/33] adding in latching logic for the db controller --- lib/systems/include/DrivebrainController.h | 17 +- lib/systems/include/TorqueControllerMux.h | 15 +- lib/systems/include/TorqueControllers.h | 104 ++-- lib/systems/src/DrivebrainController.cpp | 15 +- lib/systems/src/TorqueControllerMux.cpp | 13 +- lib/systems/src/TorqueControllers.cpp | 92 ++-- .../test_systems/torque_controller_mux_test.h | 504 +++++++++--------- 7 files changed, 403 insertions(+), 357 deletions(-) diff --git a/lib/systems/include/DrivebrainController.h b/lib/systems/include/DrivebrainController.h index 14ad6dc09..83d316214 100644 --- a/lib/systems/include/DrivebrainController.h +++ b/lib/systems/include/DrivebrainController.h @@ -6,12 +6,22 @@ #include "DrivebrainData.h" // TODO - [ ] 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 - [ ] set +// and ensure that they are within tolerence to current sys-tick + +// TODO - [ ] 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 +// unless we switch to another controller and back again. in reality, the CAN line or ethernet conditions +// probably wont improve randomly during runtime so it will keep faulting. primarily this is just to make sure +// 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" + + class DrivebrainController : public TorqueController { public: - void tick(const SysTick_s &sys_tick, DrivebrainData db_input); + void tick(const SysTick_s &sys_tick, DrivebrainData db_input, bool is_active_controller); DrivebrainController(TorqueControllerOutput_s &writeout, unsigned long allowed_latency, unsigned long allowed_jitter) : _writeout(writeout) { @@ -25,6 +35,7 @@ class DrivebrainController : public TorqueController unsigned long allowed_latency; } _params; + bool _timing_failure = false; unsigned long _last_sent_torque_lim_millis = -1; unsigned long _last_sent_speed_setpoint_millis = -1; diff --git a/lib/systems/include/TorqueControllerMux.h b/lib/systems/include/TorqueControllerMux.h index f56731ba1..ac04971f5 100644 --- a/lib/systems/include/TorqueControllerMux.h +++ b/lib/systems/include/TorqueControllerMux.h @@ -12,6 +12,7 @@ #include "VectornavInterface.h" #include "LoadCellInterface.h" #include "TelemetryInterface.h" +#include "DrivebrainController.h" const float MAX_SPEED_FOR_MODE_CHANGE = 5.0; // m/s const float MAX_TORQUE_DELTA_FOR_MODE_CHANGE = 0.5; // Nm @@ -24,16 +25,17 @@ class TorqueControllerMux TorqueControllerSimple torqueControllerSimple_; TorqueControllerLoadCellVectoring torqueControllerLoadCellVectoring_; TorqueControllerSimpleLaunch torqueControllerSimpleLaunch_; - TorqueControllerSlipLaunch torqueControllerSlipLaunch_; + DrivebrainController _dbController; TorqueControllerCASEWrapper tcCASEWrapper_; + // Use this to map the dial to TCMUX modes std::unordered_map dialModeMap_ = { {DialMode_e::MODE_0, TorqueController_e::TC_SAFE_MODE}, {DialMode_e::MODE_1, TorqueController_e::TC_LOAD_CELL_VECTORING}, {DialMode_e::MODE_2, TorqueController_e::TC_CASE_SYSTEM}, {DialMode_e::MODE_3, TorqueController_e::TC_SIMPLE_LAUNCH}, - {DialMode_e::MODE_4, TorqueController_e::TC_SLIP_LAUNCH}, + {DialMode_e::MODE_4, TorqueController_e::TC_DRIVEBRAIN}, {DialMode_e::MODE_5, TorqueController_e::TC_NO_CONTROLLER}, }; std::unordered_map torqueLimitMap_ = { @@ -53,7 +55,7 @@ class TorqueControllerMux static_cast(&torqueControllerSimple_), static_cast(&torqueControllerLoadCellVectoring_), static_cast(&torqueControllerSimpleLaunch_), - static_cast(&torqueControllerSlipLaunch_), + static_cast(&_dbController), static_cast(&tcCASEWrapper_) }; @@ -73,7 +75,7 @@ class TorqueControllerMux , torqueControllerSimple_(controllerOutputs_[static_cast(TorqueController_e::TC_SAFE_MODE)]) , torqueControllerLoadCellVectoring_(controllerOutputs_[static_cast(TorqueController_e::TC_LOAD_CELL_VECTORING)]) , torqueControllerSimpleLaunch_(controllerOutputs_[static_cast(TorqueController_e::TC_SIMPLE_LAUNCH)]) - , torqueControllerSlipLaunch_(controllerOutputs_[static_cast(TorqueController_e::TC_SLIP_LAUNCH)]) + , _dbController(controllerOutputs_[static_cast(TorqueController_e::TC_DRIVEBRAIN)], 30, 10) , tcCASEWrapper_(controllerOutputs_[static_cast(TorqueController_e::TC_CASE_SYSTEM)]) , telemHandle_(telemInterface) {} @@ -86,7 +88,7 @@ class TorqueControllerMux , torqueControllerSimple_(controllerOutputs_[static_cast(TorqueController_e::TC_SAFE_MODE)], simpleTCRearTorqueScale, simpleTCRegenTorqueScale) , torqueControllerLoadCellVectoring_(controllerOutputs_[static_cast(TorqueController_e::TC_LOAD_CELL_VECTORING)], 1.0, simpleTCRegenTorqueScale) , torqueControllerSimpleLaunch_(controllerOutputs_[static_cast(TorqueController_e::TC_SIMPLE_LAUNCH)]) - , torqueControllerSlipLaunch_(controllerOutputs_[static_cast(TorqueController_e::TC_SLIP_LAUNCH)]) + , _dbController(controllerOutputs_[static_cast(TorqueController_e::TC_DRIVEBRAIN)], 30, 10) , tcCASEWrapper_(controllerOutputs_[static_cast(TorqueController_e::TC_CASE_SYSTEM)]) , telemHandle_(telemInterface) {} @@ -102,7 +104,8 @@ class TorqueControllerMux float accDerateFactor, bool dashboardTorqueModeButtonPressed, const vector_nav &vn_data, - const DrivetrainCommand_s &CASECommand + const DrivetrainCommand_s &CASECommand, + DrivebrainData db_input ); /// @brief apply corresponding limits on drivetrain command calculated by torque controller diff --git a/lib/systems/include/TorqueControllers.h b/lib/systems/include/TorqueControllers.h index 54d0ea39b..284412802 100644 --- a/lib/systems/include/TorqueControllers.h +++ b/lib/systems/include/TorqueControllers.h @@ -73,11 +73,9 @@ enum TorqueController_e TC_SAFE_MODE = 1, TC_LOAD_CELL_VECTORING = 2, TC_SIMPLE_LAUNCH = 3, - TC_SLIP_LAUNCH = 4, - TC_LOOKUP_LAUNCH = 5, - TC_CASE_SYSTEM = 6, - TC_DRIVEBRAIN= 7, - TC_NUM_CONTROLLERS = 8, + TC_CASE_SYSTEM = 4, + TC_DRIVEBRAIN= 5, + TC_NUM_CONTROLLERS = 6, }; enum class LaunchStates_e @@ -267,54 +265,54 @@ class TorqueControllerSimpleLaunch : public TorqueController, void calc_launch_algo(const vector_nav *vn_data) override; }; -class TorqueControllerSlipLaunch : public TorqueController, public BaseLaunchController -{ -private: - float slip_ratio_; - -public: - /*! - SLIP LAUNCH CONTROLLER - This launch controller is based off of a specified slip constant. It will at all times attempt - to keep the wheelspeed at this certain higher percent of the body velocity of the car to keep it - in constant slip - @param slip_ratio specified launch rate in m/s^2 - @param initial_speed_target the initial speed commanded to the wheels - */ - TorqueControllerSlipLaunch(TorqueControllerOutput_s &writeout, float slip_ratio, int16_t initial_speed_target) - : BaseLaunchController(writeout, initial_speed_target), - slip_ratio_(slip_ratio) {} - - TorqueControllerSlipLaunch(TorqueControllerOutput_s &writeout) : TorqueControllerSlipLaunch(writeout, DEFAULT_SLIP_RATIO, DEFAULT_LAUNCH_SPEED_TARGET) {} - - LaunchStates_e get_launch_state() override { return launch_state_; } - - void calc_launch_algo(const vector_nav *vn_data) override; -}; - -class TorqueControllerLookupLaunch : public TorqueController, BaseLaunchController -{ -private: - bool init_position = false; - -public: - /*! - Lookup Launch Controller - This launch controller is based off of a matlab and symlink generated lookup table. - This has been converted to a C array with some basic python code using the array index - as the input for the controller - @param slip_ratio specified launch rate in m/s^2 - @param initial_speed_target the initial speed commanded to the wheels - */ - TorqueControllerLookupLaunch(TorqueControllerOutput_s &writeout, int16_t initial_speed_target) - : BaseLaunchController(writeout, initial_speed_target) {} - - TorqueControllerLookupLaunch(TorqueControllerOutput_s &writeout) : TorqueControllerLookupLaunch(writeout, DEFAULT_LAUNCH_SPEED_TARGET) {} - - LaunchStates_e get_launch_state() override { return launch_state_; } - - void calc_launch_algo(const vector_nav *vn_data) override; -}; +// class TorqueControllerSlipLaunch : public TorqueController, public BaseLaunchController +// { +// private: +// float slip_ratio_; + +// public: +// /*! +// SLIP LAUNCH CONTROLLER +// This launch controller is based off of a specified slip constant. It will at all times attempt +// to keep the wheelspeed at this certain higher percent of the body velocity of the car to keep it +// in constant slip +// @param slip_ratio specified launch rate in m/s^2 +// @param initial_speed_target the initial speed commanded to the wheels +// */ +// TorqueControllerSlipLaunch(TorqueControllerOutput_s &writeout, float slip_ratio, int16_t initial_speed_target) +// : BaseLaunchController(writeout, initial_speed_target), +// slip_ratio_(slip_ratio) {} + +// TorqueControllerSlipLaunch(TorqueControllerOutput_s &writeout) : TorqueControllerSlipLaunch(writeout, DEFAULT_SLIP_RATIO, DEFAULT_LAUNCH_SPEED_TARGET) {} + +// LaunchStates_e get_launch_state() override { return launch_state_; } + +// void calc_launch_algo(const vector_nav *vn_data) override; +// }; + +// class TorqueControllerLookupLaunch : public TorqueController, BaseLaunchController +// { +// private: +// bool init_position = false; + +// public: +// /*! +// Lookup Launch Controller +// This launch controller is based off of a matlab and symlink generated lookup table. +// This has been converted to a C array with some basic python code using the array index +// as the input for the controller +// @param slip_ratio specified launch rate in m/s^2 +// @param initial_speed_target the initial speed commanded to the wheels +// */ +// TorqueControllerLookupLaunch(TorqueControllerOutput_s &writeout, int16_t initial_speed_target) +// : BaseLaunchController(writeout, initial_speed_target) {} + +// TorqueControllerLookupLaunch(TorqueControllerOutput_s &writeout) : TorqueControllerLookupLaunch(writeout, DEFAULT_LAUNCH_SPEED_TARGET) {} + +// LaunchStates_e get_launch_state() override { return launch_state_; } + +// void calc_launch_algo(const vector_nav *vn_data) override; +// }; class TorqueControllerCASEWrapper : public TorqueController { diff --git a/lib/systems/src/DrivebrainController.cpp b/lib/systems/src/DrivebrainController.cpp index cb9268aa6..1cbd819ce 100644 --- a/lib/systems/src/DrivebrainController.cpp +++ b/lib/systems/src/DrivebrainController.cpp @@ -1,17 +1,25 @@ #include "DrivebrainController.h" -void DrivebrainController::tick(const SysTick_s &sys_tick, DrivebrainData db_input) +void DrivebrainController::tick(const SysTick_s &sys_tick, DrivebrainData db_input, bool is_active_controller) { _writeout.ready = false; bool speed_setpoint_too_latent = (::abs((int)(sys_tick.millis - db_input.last_speed_setpoint_receive_time_millis)) > (int)_params.allowed_latency); bool torque_setpoint_too_latent = (::abs((int)(sys_tick.millis - db_input.last_torque_lim_receive_time_millis)) > (int)_params.allowed_latency); bool msg_jitter_too_high = (::abs((int)(db_input.last_speed_setpoint_receive_time_millis - db_input.last_torque_lim_receive_time_millis)) > (int)_params.allowed_jitter); + + bool timing_failure = (speed_setpoint_too_latent || torque_setpoint_too_latent || msg_jitter_too_high); + + // only in the case that this is the active controller do we want to clear our timing failure + if((!is_active_controller) && (!timing_failure) ) + { + // timing failure should be false here + _timing_failure = timing_failure; + } - if (!(speed_setpoint_too_latent || torque_setpoint_too_latent || msg_jitter_too_high)) + if (!timing_failure) { _writeout.ready = true; - _last_sent_speed_setpoint_millis = db_input.last_speed_setpoint_receive_time_millis; _last_sent_torque_lim_millis = db_input.last_torque_lim_receive_time_millis; @@ -20,6 +28,7 @@ void DrivebrainController::tick(const SysTick_s &sys_tick, DrivebrainData db_inp } else { + _timing_failure = true; _writeout.ready = false; _writeout.command = {}; // set command to all zeros if bad latency is apparent } diff --git a/lib/systems/src/TorqueControllerMux.cpp b/lib/systems/src/TorqueControllerMux.cpp index e6e04fa8b..5c5dfe347 100644 --- a/lib/systems/src/TorqueControllerMux.cpp +++ b/lib/systems/src/TorqueControllerMux.cpp @@ -1,6 +1,7 @@ #include "TorqueControllerMux.h" #include "Utility.h" #include "PhysicalParameters.h" +#include "DrivebrainController.h" void TorqueControllerMux::tick( const SysTick_s &tick, @@ -12,13 +13,16 @@ void TorqueControllerMux::tick( float accDerateFactor, bool dashboardTorqueModeButtonPressed, const vector_nav &vn_data, - const DrivetrainCommand_s &CASECommand) + const DrivetrainCommand_s &CASECommand, + DrivebrainData db_input) { // Tick all torque controllers torqueControllerSimple_.tick(tick, pedalsData, torqueLimitMap_[torqueLimit_]); torqueControllerLoadCellVectoring_.tick(tick, pedalsData, torqueLimitMap_[torqueLimit_], loadCellData); torqueControllerSimpleLaunch_.tick(tick, pedalsData, drivetrainData.measuredSpeeds, &vn_data); - torqueControllerSlipLaunch_.tick(tick, pedalsData, drivetrainData.measuredSpeeds, &vn_data); + + bool drivebrain_in_control = (muxMode_ == TorqueController_e::TC_DRIVEBRAIN); + _dbController.tick(tick, db_input, drivebrain_in_control); tcCASEWrapper_.tick( (TCCaseWrapperTick_s){ .command = CASECommand, @@ -106,13 +110,12 @@ void TorqueControllerMux::tick( // Apply setpoints value limits // Derating for endurance - - if (muxMode_ != TC_CASE_SYSTEM) + if (muxMode_ != TorqueController_e::TC_CASE_SYSTEM) { // Safety checks for CASE: CASE handles regen, torque, and power limit internally applyRegenLimit(&drivetrainCommand_, &drivetrainData); // Apply torque limit before power limit to not power limit - if ((muxMode_ != TC_SIMPLE_LAUNCH) && (muxMode_ != TC_SLIP_LAUNCH) && (muxMode_ != TC_LOOKUP_LAUNCH)) + if ((muxMode_ != TC_SIMPLE_LAUNCH)) { applyTorqueLimit(&drivetrainCommand_); } diff --git a/lib/systems/src/TorqueControllers.cpp b/lib/systems/src/TorqueControllers.cpp index 9497c9b61..96942c28a 100644 --- a/lib/systems/src/TorqueControllers.cpp +++ b/lib/systems/src/TorqueControllers.cpp @@ -291,53 +291,53 @@ void TorqueControllerSimpleLaunch::calc_launch_algo(const vector_nav* vn_data) { launch_speed_target_ = std::min((int)AMK_MAX_RPM, std::max(0, (int)launch_speed_target_)); } -void TorqueControllerSlipLaunch::calc_launch_algo(const vector_nav* vn_data) { - // accelerate at constant speed for a period of time to get body velocity up - // may want to make this the ht07 launch algo +// void TorqueControllerSlipLaunch::calc_launch_algo(const vector_nav* vn_data) { +// // accelerate at constant speed for a period of time to get body velocity up +// // may want to make this the ht07 launch algo - // makes sure that the car launches at the target launch speed - launch_speed_target_ = std::max(launch_speed_target_, (float)DEFAULT_LAUNCH_SPEED_TARGET); - - /* - New slip-ratio based launch algorithm by Luke Chen. The basic idea - is to always be pushing the car a certain 'slip_ratio_' faster than - the car is currently going, theoretically always keeping the car in slip - */ - // m/s - float new_speed_target = (1 + slip_ratio_) * (vn_data->velocity_x); - // rpm - new_speed_target *= METERS_PER_SECOND_TO_RPM; - // makes sure the car target speed never goes lower than prev. target - // allows for the vn to 'spool' up and us to get reliable vx data - launch_speed_target_ = std::max(launch_speed_target_, new_speed_target); -} - -void TorqueControllerLookupLaunch::calc_launch_algo(const vector_nav* vn_data) { - - launch_speed_target_ = std::max((float)DEFAULT_LAUNCH_SPEED_TARGET, launch_speed_target_); - - double dx = vn_data->ecef_coords[0] - initial_ecef_x_; - double dy = vn_data->ecef_coords[1] - initial_ecef_y_; - double dz = vn_data->ecef_coords[2] - initial_ecef_z_; - - double distance = sqrt((dx*dx) + (dy*dy) + (dz*dz)); - - /* - Distance-lookup launch algorithm. Takes in the vel_dist_lookup - generated from Luke's matlab/symlink to set speed targets based - on distance travelled from the start point. - This can also and may be better to replace with an integration - of body velocity. - */ - - uint32_t idx = (uint32_t)(distance * 10); // multiply by 10 to be used as index for meters in steps of 1/10 - idx = std::min(idx, (uint32_t)(sizeof(vel_dist_lookup) / sizeof(float))); - float mps_target = vel_dist_lookup[idx]; - - float new_speed_target = mps_target * METERS_PER_SECOND_TO_RPM; - launch_speed_target_ = std::max(launch_speed_target_, new_speed_target); - -} +// // makes sure that the car launches at the target launch speed +// launch_speed_target_ = std::max(launch_speed_target_, (float)DEFAULT_LAUNCH_SPEED_TARGET); + +// /* +// New slip-ratio based launch algorithm by Luke Chen. The basic idea +// is to always be pushing the car a certain 'slip_ratio_' faster than +// the car is currently going, theoretically always keeping the car in slip +// */ +// // m/s +// float new_speed_target = (1 + slip_ratio_) * (vn_data->velocity_x); +// // rpm +// new_speed_target *= METERS_PER_SECOND_TO_RPM; +// // makes sure the car target speed never goes lower than prev. target +// // allows for the vn to 'spool' up and us to get reliable vx data +// launch_speed_target_ = std::max(launch_speed_target_, new_speed_target); +// } + +// void TorqueControllerLookupLaunch::calc_launch_algo(const vector_nav* vn_data) { + +// launch_speed_target_ = std::max((float)DEFAULT_LAUNCH_SPEED_TARGET, launch_speed_target_); + +// double dx = vn_data->ecef_coords[0] - initial_ecef_x_; +// double dy = vn_data->ecef_coords[1] - initial_ecef_y_; +// double dz = vn_data->ecef_coords[2] - initial_ecef_z_; + +// double distance = sqrt((dx*dx) + (dy*dy) + (dz*dz)); + +// /* +// Distance-lookup launch algorithm. Takes in the vel_dist_lookup +// generated from Luke's matlab/symlink to set speed targets based +// on distance travelled from the start point. +// This can also and may be better to replace with an integration +// of body velocity. +// */ + +// uint32_t idx = (uint32_t)(distance * 10); // multiply by 10 to be used as index for meters in steps of 1/10 +// idx = std::min(idx, (uint32_t)(sizeof(vel_dist_lookup) / sizeof(float))); +// float mps_target = vel_dist_lookup[idx]; + +// float new_speed_target = mps_target * METERS_PER_SECOND_TO_RPM; +// launch_speed_target_ = std::max(launch_speed_target_, new_speed_target); + +// } void TorqueControllerCASEWrapper::tick(const TCCaseWrapperTick_s &intake) { diff --git a/test/test_systems/torque_controller_mux_test.h b/test/test_systems/torque_controller_mux_test.h index 64a63b5f0..fa6d4d7dd 100644 --- a/test/test_systems/torque_controller_mux_test.h +++ b/test/test_systems/torque_controller_mux_test.h @@ -113,7 +113,8 @@ TEST(TorqueControllerMuxTesting, test_torque_delta_prevents_mode_change) 1.0, false, vn_data, - (const DrivetrainCommand_s) {} + (const DrivetrainCommand_s) {}, + {} ); resulting_torque_command = torque_controller_mux.getDrivetrainCommand(); @@ -135,7 +136,8 @@ TEST(TorqueControllerMuxTesting, test_torque_delta_prevents_mode_change) 1.0, false, vn_data, - (const DrivetrainCommand_s) {} + (const DrivetrainCommand_s) {}, + {} ); // Now press the pedal again. Torque should be requested @@ -149,7 +151,8 @@ TEST(TorqueControllerMuxTesting, test_torque_delta_prevents_mode_change) 1.0, false, vn_data, - (const DrivetrainCommand_s) {} + (const DrivetrainCommand_s) {}, + {} ); resulting_torque_command = torque_controller_mux.getDrivetrainCommand(); @@ -229,7 +232,8 @@ TEST(TorqueControllerMuxTesting, test_speed_delta_prevents_mode_change) 1.0, false, vn_data, - (const DrivetrainCommand_s) {} + (const DrivetrainCommand_s) {}, + {} ); resulting_torque_command = torque_controller_mux.getDrivetrainCommand(); @@ -251,7 +255,8 @@ TEST(TorqueControllerMuxTesting, test_speed_delta_prevents_mode_change) 1.0, false, vn_data, - (const DrivetrainCommand_s) {} + (const DrivetrainCommand_s) {}, + {} ); // Now press the pedal. Torque should be requested @@ -265,7 +270,8 @@ TEST(TorqueControllerMuxTesting, test_speed_delta_prevents_mode_change) 1.0, false, vn_data, - (const DrivetrainCommand_s) {} + (const DrivetrainCommand_s) {}, + {} ); resulting_torque_command = torque_controller_mux.getDrivetrainCommand(); @@ -455,7 +461,8 @@ TEST(TorqueControllerMuxTesting, test_simple_launch_controller) { 1.0, false, vn_data, - (const DrivetrainCommand_s) {} + (const DrivetrainCommand_s) {}, + {} ); // change mode to mode 3 @@ -469,7 +476,8 @@ TEST(TorqueControllerMuxTesting, test_simple_launch_controller) { 1.0, false, vn_data, - (const DrivetrainCommand_s) {} + (const DrivetrainCommand_s) {}, + {} ); // tick again to calculate state switch @@ -483,7 +491,8 @@ TEST(TorqueControllerMuxTesting, test_simple_launch_controller) { 1.0, false, vn_data, - (const DrivetrainCommand_s) {} + (const DrivetrainCommand_s) {}, + {} ); LaunchStates_e launch_state = torque_controller_mux.activeController()->get_launch_state(); @@ -501,7 +510,8 @@ TEST(TorqueControllerMuxTesting, test_simple_launch_controller) { 1.0, false, vn_data, - (const DrivetrainCommand_s) {} + (const DrivetrainCommand_s) {}, + {} ); launch_state = torque_controller_mux.activeController()->get_launch_state(); @@ -519,7 +529,8 @@ TEST(TorqueControllerMuxTesting, test_simple_launch_controller) { 1.0, false, vn_data, - (const DrivetrainCommand_s) {} + (const DrivetrainCommand_s) {}, + {} ); launch_state = torque_controller_mux.activeController()->get_launch_state(); @@ -537,7 +548,8 @@ TEST(TorqueControllerMuxTesting, test_simple_launch_controller) { 1.0, false, vn_data, - (const DrivetrainCommand_s) {} + (const DrivetrainCommand_s) {}, + {} ); launch_state = torque_controller_mux.activeController()->get_launch_state(); @@ -554,7 +566,8 @@ TEST(TorqueControllerMuxTesting, test_simple_launch_controller) { 1.0, false, vn_data, - (const DrivetrainCommand_s) {} + (const DrivetrainCommand_s) {}, + {} ); DrivetrainCommand_s commanded = torque_controller_mux.getDrivetrainCommand(); @@ -572,7 +585,8 @@ TEST(TorqueControllerMuxTesting, test_simple_launch_controller) { 1.0, false, vn_data, - (const DrivetrainCommand_s) {} + (const DrivetrainCommand_s) {}, + {} ); launch_state = torque_controller_mux.activeController()->get_launch_state(); @@ -580,232 +594,240 @@ TEST(TorqueControllerMuxTesting, test_simple_launch_controller) { ASSERT_EQ(launch_state, LaunchStates_e::LAUNCH_NOT_READY); } -TEST(TorqueControllerMuxTesting, test_slip_launch_controller) { - SysClock clock = SysClock(); - SysTick_s cur_tick; - cur_tick = clock.tick(0); - TelemetryInterface telem_interface; - TorqueControllerMux torque_controller_mux = TorqueControllerMux(&telem_interface); - DrivetrainCommand_s resulting_torque_command; - - vector_nav vn_data{}; - - DrivetrainDynamicReport_s simulated_slow_drivetrain_dynamics = { - .measuredInverterFLPackVoltage = 550, - .measuredSpeeds = { - ((MAX_SPEED_FOR_MODE_CHANGE - 0.1f) * METERS_PER_SECOND_TO_RPM), - ((MAX_SPEED_FOR_MODE_CHANGE - 0.1f) * METERS_PER_SECOND_TO_RPM), - ((MAX_SPEED_FOR_MODE_CHANGE - 0.1f) * METERS_PER_SECOND_TO_RPM), - ((MAX_SPEED_FOR_MODE_CHANGE - 0.1f) * METERS_PER_SECOND_TO_RPM) - }, - .measuredTorques = {0.0, 0.0, 0.0, 0.0}, - .measuredTorqueCurrents = {0.0, 0.0, 0.0, 0.0}, - .measuredMagnetizingCurrents = {0.0, 0.0, 0.0, 0.0} - }; - - DrivetrainDynamicReport_s simulated_barely_launch_drivetrain_dynamics = { - .measuredInverterFLPackVoltage = 550, - .measuredSpeeds = {0, 2785.86, 0, 0}, - .measuredTorques = {0.0, 0.0, 0.0, 0.0}, - .measuredTorqueCurrents = {0.0, 0.0, 0.0, 0.0}, - .measuredMagnetizingCurrents = {0.0, 0.0, 0.0, 0.0} - }; - - // DrivetrainDynamicReport_s simulated_no_launch_drivetrain_dynamics = { - // .measuredInverterFLPackVoltage = 550, - // .measuredSpeeds = {0, 2786.86, 0, 0}, - // .measuredTorques = {0.0, 0.0, 0.0, 0.0}, - // .measuredTorqueCurrents = {0.0, 0.0, 0.0, 0.0}, - // .measuredMagnetizingCurrents = {0.0, 0.0, 0.0, 0.0} - // }; - - // DrivetrainDynamicReport_s simulated_fast_drivetrain_dynamics = { - // .measuredInverterFLPackVoltage = 550, - // .measuredSpeeds = { - // ((MAX_SPEED_FOR_MODE_CHANGE + 0.1f) * METERS_PER_SECOND_TO_RPM), - // ((MAX_SPEED_FOR_MODE_CHANGE + 0.1f) * METERS_PER_SECOND_TO_RPM), - // ((MAX_SPEED_FOR_MODE_CHANGE + 0.1f) * METERS_PER_SECOND_TO_RPM), - // ((MAX_SPEED_FOR_MODE_CHANGE + 0.1f) * METERS_PER_SECOND_TO_RPM) - // }, - // .measuredTorques = {0.0, 0.0, 0.0, 0.0}, - // .measuredTorqueCurrents = {0.0, 0.0, 0.0, 0.0}, - // .measuredMagnetizingCurrents = {0.0, 0.0, 0.0, 0.0} - // }; - - PedalsSystemData_s simulated_full_accel_press = { - .accelImplausible = false, - .brakeImplausible = false, - .brakePressed = false, - .brakeAndAccelPressedImplausibility = false, - .implausibilityExceededMaxDuration = false, - .accelPercent = 1.0, - .brakePercent = 0.0 - }; - - PedalsSystemData_s simulated_no_accel_press = { - .accelImplausible = false, - .brakeImplausible = false, - .brakePressed = false, - .brakeAndAccelPressedImplausibility = false, - .implausibilityExceededMaxDuration = false, - .accelPercent = 0.0, - .brakePercent = 0.0 - }; - - // PedalsSystemData_s simulated_accel_and_brake_press = { - // .accelImplausible = false, - // .brakeImplausible = false, - // .brakePressed = false, - // .brakeAndAccelPressedImplausibility = false, - // .implausibilityExceededMaxDuration = false, - // .accelPercent = 1.0, - // .brakePercent = 0.3 - // }; - - torque_controller_mux.tick( - cur_tick, - simulated_slow_drivetrain_dynamics, - simulated_no_accel_press, - (const SteeringSystemData_s) {}, - (const LoadCellInterfaceOutput_s) {}, - DialMode_e::MODE_0, - 1.0, - false, - vn_data, - (const DrivetrainCommand_s) {} - ); - - // change mode to mode 3 - torque_controller_mux.tick( - cur_tick, - simulated_slow_drivetrain_dynamics, - simulated_no_accel_press, - (const SteeringSystemData_s) {}, - (const LoadCellInterfaceOutput_s) {}, - DialMode_e::MODE_4, - 1.0, - false, - vn_data, - (const DrivetrainCommand_s) {} - ); - - // tick again to calculate state switch - torque_controller_mux.tick( - cur_tick, - simulated_slow_drivetrain_dynamics, - simulated_no_accel_press, - (const SteeringSystemData_s) {}, - (const LoadCellInterfaceOutput_s) {}, - DialMode_e::MODE_4, - 1.0, - false, - vn_data, - (const DrivetrainCommand_s) {} - ); - - LaunchStates_e launch_state = torque_controller_mux.activeController()->get_launch_state(); - - ASSERT_EQ(launch_state, LaunchStates_e::LAUNCH_READY); - - // press accel now with one wheelspeed barely under threshold - torque_controller_mux.tick( - clock.tick(100000), - simulated_barely_launch_drivetrain_dynamics, - simulated_full_accel_press, - (const SteeringSystemData_s) {}, - (const LoadCellInterfaceOutput_s) {}, - DialMode_e::MODE_4, - 1.0, - false, - vn_data, - (const DrivetrainCommand_s) {} - ); - - // tick TCMUX (10k us will hit 100hz) - torque_controller_mux.tick( - clock.tick(100000), - simulated_barely_launch_drivetrain_dynamics, - simulated_full_accel_press, - (const SteeringSystemData_s) {}, - (const LoadCellInterfaceOutput_s) {}, - DialMode_e::MODE_4, - 1.0, - false, - vn_data, - (const DrivetrainCommand_s) {} - ); - - launch_state = torque_controller_mux.activeController()->get_launch_state(); - - ASSERT_EQ(launch_state, LaunchStates_e::LAUNCHING); - - DrivetrainCommand_s commanded = torque_controller_mux.getDrivetrainCommand(); - ASSERT_EQ((int16_t)commanded.speeds_rpm[0], DEFAULT_LAUNCH_SPEED_TARGET); - - torque_controller_mux.tick( - clock.tick(const_accel_time * 1000), // const accel time sine launch - simulated_barely_launch_drivetrain_dynamics, - simulated_full_accel_press, - (const SteeringSystemData_s) {}, - (const LoadCellInterfaceOutput_s) {}, - DialMode_e::MODE_4, - 1.0, - false, - vn_data, - (const DrivetrainCommand_s) {} - ); - - launch_state = torque_controller_mux.activeController()->get_launch_state(); - - ASSERT_EQ(launch_state, LaunchStates_e::LAUNCHING); - - commanded = torque_controller_mux.getDrivetrainCommand(); - ASSERT_EQ((int16_t)commanded.speeds_rpm[0], DEFAULT_LAUNCH_SPEED_TARGET); - - // if velocity is less than the default speed, it should still go at launch speed - vn_data.velocity_x = 0; // m/s - - torque_controller_mux.tick( - clock.tick((const_accel_time * 1000) + 1000000), // 1 second after const accel time - simulated_barely_launch_drivetrain_dynamics, - simulated_full_accel_press, - (const SteeringSystemData_s) {}, - (const LoadCellInterfaceOutput_s) {}, - DialMode_e::MODE_4, - 1.0, - false, - vn_data, - (const DrivetrainCommand_s) {} - ); - - commanded = torque_controller_mux.getDrivetrainCommand(); - printf("lower vx_body: %.2f\n", (float)commanded.speeds_rpm[0] * RPM_TO_METERS_PER_SECOND); - ASSERT_EQ(commanded.speeds_rpm[0] , DEFAULT_LAUNCH_SPEED_TARGET); // should still be at initial launch target - - // this is approx. the speed the car would be going at 1500 rpm - // we should expect the next calculation to be approx. DEFAULT_SLIP_RATIO % higher than this - vn_data.velocity_x = DEFAULT_LAUNCH_SPEED_TARGET * RPM_TO_METERS_PER_SECOND; // m/s - - torque_controller_mux.tick( - clock.tick((const_accel_time * 1000) + 1000000), // 1 second after const accel time - simulated_barely_launch_drivetrain_dynamics, - simulated_full_accel_press, - (const SteeringSystemData_s) {}, - (const LoadCellInterfaceOutput_s) {}, - DialMode_e::MODE_4, - 1.0, - false, - vn_data, - (const DrivetrainCommand_s) {} - ); - - commanded = torque_controller_mux.getDrivetrainCommand(); - printf("launch vx_body: %.2f\n", (float)commanded.speeds_rpm[0] * RPM_TO_METERS_PER_SECOND); - ASSERT_EQ((int16_t)commanded.speeds_rpm[0] > (DEFAULT_LAUNCH_SPEED_TARGET * RPM_TO_METERS_PER_SECOND), true); - - - // this fails, but the value is close enough - // ASSERT_EQ((float)((int)((commanded.speeds_rpm[0] - 1500) * RPM_TO_METERS_PER_SECOND * 100)) / 100, 11.76f); -} +// TEST(TorqueControllerMuxTesting, test_slip_launch_controller) { +// SysClock clock = SysClock(); +// SysTick_s cur_tick; +// cur_tick = clock.tick(0); +// TelemetryInterface telem_interface; +// TorqueControllerMux torque_controller_mux = TorqueControllerMux(&telem_interface); +// DrivetrainCommand_s resulting_torque_command; + +// vector_nav vn_data{}; + +// DrivetrainDynamicReport_s simulated_slow_drivetrain_dynamics = { +// .measuredInverterFLPackVoltage = 550, +// .measuredSpeeds = { +// ((MAX_SPEED_FOR_MODE_CHANGE - 0.1f) * METERS_PER_SECOND_TO_RPM), +// ((MAX_SPEED_FOR_MODE_CHANGE - 0.1f) * METERS_PER_SECOND_TO_RPM), +// ((MAX_SPEED_FOR_MODE_CHANGE - 0.1f) * METERS_PER_SECOND_TO_RPM), +// ((MAX_SPEED_FOR_MODE_CHANGE - 0.1f) * METERS_PER_SECOND_TO_RPM) +// }, +// .measuredTorques = {0.0, 0.0, 0.0, 0.0}, +// .measuredTorqueCurrents = {0.0, 0.0, 0.0, 0.0}, +// .measuredMagnetizingCurrents = {0.0, 0.0, 0.0, 0.0} +// }; + +// DrivetrainDynamicReport_s simulated_barely_launch_drivetrain_dynamics = { +// .measuredInverterFLPackVoltage = 550, +// .measuredSpeeds = {0, 2785.86, 0, 0}, +// .measuredTorques = {0.0, 0.0, 0.0, 0.0}, +// .measuredTorqueCurrents = {0.0, 0.0, 0.0, 0.0}, +// .measuredMagnetizingCurrents = {0.0, 0.0, 0.0, 0.0} +// }; + +// // DrivetrainDynamicReport_s simulated_no_launch_drivetrain_dynamics = { +// // .measuredInverterFLPackVoltage = 550, +// // .measuredSpeeds = {0, 2786.86, 0, 0}, +// // .measuredTorques = {0.0, 0.0, 0.0, 0.0}, +// // .measuredTorqueCurrents = {0.0, 0.0, 0.0, 0.0}, +// // .measuredMagnetizingCurrents = {0.0, 0.0, 0.0, 0.0} +// // }; + +// // DrivetrainDynamicReport_s simulated_fast_drivetrain_dynamics = { +// // .measuredInverterFLPackVoltage = 550, +// // .measuredSpeeds = { +// // ((MAX_SPEED_FOR_MODE_CHANGE + 0.1f) * METERS_PER_SECOND_TO_RPM), +// // ((MAX_SPEED_FOR_MODE_CHANGE + 0.1f) * METERS_PER_SECOND_TO_RPM), +// // ((MAX_SPEED_FOR_MODE_CHANGE + 0.1f) * METERS_PER_SECOND_TO_RPM), +// // ((MAX_SPEED_FOR_MODE_CHANGE + 0.1f) * METERS_PER_SECOND_TO_RPM) +// // }, +// // .measuredTorques = {0.0, 0.0, 0.0, 0.0}, +// // .measuredTorqueCurrents = {0.0, 0.0, 0.0, 0.0}, +// // .measuredMagnetizingCurrents = {0.0, 0.0, 0.0, 0.0} +// // }; + +// PedalsSystemData_s simulated_full_accel_press = { +// .accelImplausible = false, +// .brakeImplausible = false, +// .brakePressed = false, +// .brakeAndAccelPressedImplausibility = false, +// .implausibilityExceededMaxDuration = false, +// .accelPercent = 1.0, +// .brakePercent = 0.0 +// }; + +// PedalsSystemData_s simulated_no_accel_press = { +// .accelImplausible = false, +// .brakeImplausible = false, +// .brakePressed = false, +// .brakeAndAccelPressedImplausibility = false, +// .implausibilityExceededMaxDuration = false, +// .accelPercent = 0.0, +// .brakePercent = 0.0 +// }; + +// // PedalsSystemData_s simulated_accel_and_brake_press = { +// // .accelImplausible = false, +// // .brakeImplausible = false, +// // .brakePressed = false, +// // .brakeAndAccelPressedImplausibility = false, +// // .implausibilityExceededMaxDuration = false, +// // .accelPercent = 1.0, +// // .brakePercent = 0.3 +// // }; + +// torque_controller_mux.tick( +// cur_tick, +// simulated_slow_drivetrain_dynamics, +// simulated_no_accel_press, +// (const SteeringSystemData_s) {}, +// (const LoadCellInterfaceOutput_s) {}, +// DialMode_e::MODE_0, +// 1.0, +// false, +// vn_data, +// (const DrivetrainCommand_s) {}, +// {} +// ); + +// // change mode to mode 3 +// torque_controller_mux.tick( +// cur_tick, +// simulated_slow_drivetrain_dynamics, +// simulated_no_accel_press, +// (const SteeringSystemData_s) {}, +// (const LoadCellInterfaceOutput_s) {}, +// DialMode_e::MODE_4, +// 1.0, +// false, +// vn_data, +// (const DrivetrainCommand_s) {}, +// {} +// ); + +// // tick again to calculate state switch +// torque_controller_mux.tick( +// cur_tick, +// simulated_slow_drivetrain_dynamics, +// simulated_no_accel_press, +// (const SteeringSystemData_s) {}, +// (const LoadCellInterfaceOutput_s) {}, +// DialMode_e::MODE_4, +// 1.0, +// false, +// vn_data, +// (const DrivetrainCommand_s) {}, +// {} +// ); + +// LaunchStates_e launch_state = torque_controller_mux.activeController()->get_launch_state(); + +// ASSERT_EQ(launch_state, LaunchStates_e::LAUNCH_READY); + +// // press accel now with one wheelspeed barely under threshold +// torque_controller_mux.tick( +// clock.tick(100000), +// simulated_barely_launch_drivetrain_dynamics, +// simulated_full_accel_press, +// (const SteeringSystemData_s) {}, +// (const LoadCellInterfaceOutput_s) {}, +// DialMode_e::MODE_4, +// 1.0, +// false, +// vn_data, +// (const DrivetrainCommand_s) {}, +// {} +// ); + +// // tick TCMUX (10k us will hit 100hz) +// torque_controller_mux.tick( +// clock.tick(100000), +// simulated_barely_launch_drivetrain_dynamics, +// simulated_full_accel_press, +// (const SteeringSystemData_s) {}, +// (const LoadCellInterfaceOutput_s) {}, +// DialMode_e::MODE_4, +// 1.0, +// false, +// vn_data, +// (const DrivetrainCommand_s) {}, +// {} +// ); + +// launch_state = torque_controller_mux.activeController()->get_launch_state(); + +// ASSERT_EQ(launch_state, LaunchStates_e::LAUNCHING); + +// DrivetrainCommand_s commanded = torque_controller_mux.getDrivetrainCommand(); +// ASSERT_EQ((int16_t)commanded.speeds_rpm[0], DEFAULT_LAUNCH_SPEED_TARGET); + +// torque_controller_mux.tick( +// clock.tick(const_accel_time * 1000), // const accel time sine launch +// simulated_barely_launch_drivetrain_dynamics, +// simulated_full_accel_press, +// (const SteeringSystemData_s) {}, +// (const LoadCellInterfaceOutput_s) {}, +// DialMode_e::MODE_4, +// 1.0, +// false, +// vn_data, +// (const DrivetrainCommand_s) {}, +// {} +// ); + +// launch_state = torque_controller_mux.activeController()->get_launch_state(); + +// ASSERT_EQ(launch_state, LaunchStates_e::LAUNCHING); + +// commanded = torque_controller_mux.getDrivetrainCommand(); +// ASSERT_EQ((int16_t)commanded.speeds_rpm[0], DEFAULT_LAUNCH_SPEED_TARGET); + +// // if velocity is less than the default speed, it should still go at launch speed +// vn_data.velocity_x = 0; // m/s + +// torque_controller_mux.tick( +// clock.tick((const_accel_time * 1000) + 1000000), // 1 second after const accel time +// simulated_barely_launch_drivetrain_dynamics, +// simulated_full_accel_press, +// (const SteeringSystemData_s) {}, +// (const LoadCellInterfaceOutput_s) {}, +// DialMode_e::MODE_4, +// 1.0, +// false, +// vn_data, +// (const DrivetrainCommand_s) {}, +// {} +// ); + +// commanded = torque_controller_mux.getDrivetrainCommand(); +// printf("lower vx_body: %.2f\n", (float)commanded.speeds_rpm[0] * RPM_TO_METERS_PER_SECOND); +// ASSERT_EQ(commanded.speeds_rpm[0] , DEFAULT_LAUNCH_SPEED_TARGET); // should still be at initial launch target + +// // this is approx. the speed the car would be going at 1500 rpm +// // we should expect the next calculation to be approx. DEFAULT_SLIP_RATIO % higher than this +// vn_data.velocity_x = DEFAULT_LAUNCH_SPEED_TARGET * RPM_TO_METERS_PER_SECOND; // m/s + +// torque_controller_mux.tick( +// clock.tick((const_accel_time * 1000) + 1000000), // 1 second after const accel time +// simulated_barely_launch_drivetrain_dynamics, +// simulated_full_accel_press, +// (const SteeringSystemData_s) {}, +// (const LoadCellInterfaceOutput_s) {}, +// DialMode_e::MODE_4, +// 1.0, +// false, +// vn_data, +// (const DrivetrainCommand_s) {}, +// {} +// ); + +// commanded = torque_controller_mux.getDrivetrainCommand(); +// printf("launch vx_body: %.2f\n", (float)commanded.speeds_rpm[0] * RPM_TO_METERS_PER_SECOND); +// ASSERT_EQ((int16_t)commanded.speeds_rpm[0] > (DEFAULT_LAUNCH_SPEED_TARGET * RPM_TO_METERS_PER_SECOND), true); + + +// // this fails, but the value is close enough +// // ASSERT_EQ((float)((int)((commanded.speeds_rpm[0] - 1500) * RPM_TO_METERS_PER_SECOND * 100)) / 100, 11.76f); +// } #endif /* TORQUE_CONTROLLER_MUX_TEST */ \ No newline at end of file From 882d6c075614b07ff0e1afd1dbbad8f26e901f8f Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Fri, 6 Sep 2024 00:22:18 -0400 Subject: [PATCH 08/33] added in drivebrain interface into main --- src/main.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main.cpp b/src/main.cpp index a173aa722..26e8b9550 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -662,7 +662,8 @@ void tick_all_systems(const SysTick_s ¤t_system_tick) ams_interface.get_acc_derate_factor(), dashboard.torqueModeButtonPressed(), vn_interface.get_vn_struct(), - controller_output); + controller_output, + db_interface.get_latest_db_data()); } void handle_ethernet_interface_comms() From 21e35b86dfb5ba274458b68d44ebf627bb4a6340 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Fri, 6 Sep 2024 00:26:18 -0400 Subject: [PATCH 09/33] ensuring that the regen and torque limits dont get applied to the drivebrain controller --- lib/systems/src/TorqueControllerMux.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/systems/src/TorqueControllerMux.cpp b/lib/systems/src/TorqueControllerMux.cpp index 5c5dfe347..2c49faf64 100644 --- a/lib/systems/src/TorqueControllerMux.cpp +++ b/lib/systems/src/TorqueControllerMux.cpp @@ -110,7 +110,7 @@ void TorqueControllerMux::tick( // Apply setpoints value limits // Derating for endurance - if (muxMode_ != TorqueController_e::TC_CASE_SYSTEM) + if ( (muxMode_ != TorqueController_e::TC_CASE_SYSTEM) && (muxMode_ != TorqueController_e::TC_DRIVEBRAIN) ) { // Safety checks for CASE: CASE handles regen, torque, and power limit internally applyRegenLimit(&drivetrainCommand_, &drivetrainData); From de1165b5f5de96ace2c7c8a1b743ef1755b71d7a Mon Sep 17 00:00:00 2001 From: sreekara Date: Fri, 6 Sep 2024 01:10:28 -0400 Subject: [PATCH 10/33] fix(drivebrain_controller): fixed drivebrain_controller failure and reset logic --- lib/systems/include/DrivebrainController.h | 2 +- lib/systems/src/DrivebrainController.cpp | 13 +++-- .../test_systems/drivebrain_controller_test.h | 53 +++++++++++++++---- 3 files changed, 50 insertions(+), 18 deletions(-) diff --git a/lib/systems/include/DrivebrainController.h b/lib/systems/include/DrivebrainController.h index 83d316214..6d0e72a4f 100644 --- a/lib/systems/include/DrivebrainController.h +++ b/lib/systems/include/DrivebrainController.h @@ -5,7 +5,7 @@ #include "TorqueControllers.h" #include "DrivebrainData.h" -// TODO - [ ] need to validate that the times that are apparent in the drivebrain data +// 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, diff --git a/lib/systems/src/DrivebrainController.cpp b/lib/systems/src/DrivebrainController.cpp index 1cbd819ce..0932e50a6 100644 --- a/lib/systems/src/DrivebrainController.cpp +++ b/lib/systems/src/DrivebrainController.cpp @@ -7,17 +7,16 @@ void DrivebrainController::tick(const SysTick_s &sys_tick, DrivebrainData db_inp bool speed_setpoint_too_latent = (::abs((int)(sys_tick.millis - db_input.last_speed_setpoint_receive_time_millis)) > (int)_params.allowed_latency); bool torque_setpoint_too_latent = (::abs((int)(sys_tick.millis - db_input.last_torque_lim_receive_time_millis)) > (int)_params.allowed_latency); bool msg_jitter_too_high = (::abs((int)(db_input.last_speed_setpoint_receive_time_millis - db_input.last_torque_lim_receive_time_millis)) > (int)_params.allowed_jitter); - - bool timing_failure = (speed_setpoint_too_latent || torque_setpoint_too_latent || msg_jitter_too_high); - + + bool timing_failure = (speed_setpoint_too_latent || torque_setpoint_too_latent || msg_jitter_too_high); + // only in the case that this is the active controller do we want to clear our timing failure - if((!is_active_controller) && (!timing_failure) ) + if ((!is_active_controller) && (!timing_failure)) { // timing failure should be false here - _timing_failure = timing_failure; + _timing_failure = false; } - - if (!timing_failure) + if (!timing_failure && (!_timing_failure)) { _writeout.ready = true; _last_sent_speed_setpoint_millis = db_input.last_speed_setpoint_receive_time_millis; diff --git a/test/test_systems/drivebrain_controller_test.h b/test/test_systems/drivebrain_controller_test.h index f97e18024..1228e5fb0 100644 --- a/test/test_systems/drivebrain_controller_test.h +++ b/test/test_systems/drivebrain_controller_test.h @@ -5,8 +5,8 @@ #include -void runTick(unsigned int allowed_latency, unsigned int allowed_jitter, float last_torque_lim_receive_time_millis, float last_speed_setpoint_receive_time_millis, TorqueControllerOutput_s *torque_controller_output_s) { - DrivebrainController controller(*torque_controller_output_s, allowed_latency, allowed_jitter); +void runTick(DrivebrainController *controller, float last_torque_lim_receive_time_millis, float last_speed_setpoint_receive_time_millis, bool is_active_controller) { + // DrivebrainController controller(*torque_controller_output_s, allowed_latency, allowed_jitter); DrivebrainData data; data.last_torque_lim_receive_time_millis = last_torque_lim_receive_time_millis; data.last_speed_setpoint_receive_time_millis = last_speed_setpoint_receive_time_millis; @@ -16,12 +16,13 @@ void runTick(unsigned int allowed_latency, unsigned int allowed_jitter, float la SysTick_s systick; systick.millis = 1000; systick.micros = 1000; - controller.tick(systick, data); + controller->tick(systick, data, is_active_controller); } TEST(DrivebrainControllerTesting, signals_sent_within_range) { TorqueControllerOutput_s torque_controller_output_s = {}; - runTick(10, 10, 1001, 1009, &torque_controller_output_s); + DrivebrainController controller(torque_controller_output_s, 10, 10); + runTick(&controller, 1001, 1009, true); EXPECT_TRUE(torque_controller_output_s.ready); EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 1); @@ -29,7 +30,8 @@ TEST(DrivebrainControllerTesting, signals_sent_within_range) { TEST(DrivebrainControllerTesting, speed_setpoint_too_latent) { TorqueControllerOutput_s torque_controller_output_s = {}; - runTick(5, 5, 1001, 1006, &torque_controller_output_s); + DrivebrainController controller(torque_controller_output_s, 5, 5); + runTick(&controller, 1001, 1006, true); EXPECT_FALSE(torque_controller_output_s.ready); EXPECT_NE(torque_controller_output_s.command.speeds_rpm[0], 1); @@ -37,7 +39,8 @@ TEST(DrivebrainControllerTesting, speed_setpoint_too_latent) { TEST(DrivebrainControllerTesting, torque_setpoint_too_latent) { TorqueControllerOutput_s torque_controller_output_s = {}; - runTick(5, 5, 1006, 1001, &torque_controller_output_s); + DrivebrainController controller(torque_controller_output_s, 5, 5); + runTick(&controller, 1006, 1001, true); EXPECT_FALSE(torque_controller_output_s.ready); EXPECT_NE(torque_controller_output_s.command.speeds_rpm[0], 1); @@ -45,7 +48,8 @@ TEST(DrivebrainControllerTesting, torque_setpoint_too_latent) { TEST(DrivebrainControllerTesting, msg_jitter_too_high) { TorqueControllerOutput_s torque_controller_output_s = {}; - runTick(10, 5, 1001, 1009, &torque_controller_output_s); + DrivebrainController controller(torque_controller_output_s, 10, 5); + runTick(&controller, 1001, 1009, true); EXPECT_FALSE(torque_controller_output_s.ready); EXPECT_NE(torque_controller_output_s.command.speeds_rpm[0], 1); @@ -53,7 +57,8 @@ TEST(DrivebrainControllerTesting, msg_jitter_too_high) { TEST(DrivebrainControllerTesting, speed_setpoint_too_latent_and_torque_setpoint_too_latent) { TorqueControllerOutput_s torque_controller_output_s = {}; - runTick(5, 10, 1001, 1015, &torque_controller_output_s); + DrivebrainController controller(torque_controller_output_s, 5, 10); + runTick(&controller, 1001, 1015, true); EXPECT_FALSE(torque_controller_output_s.ready); EXPECT_NE(torque_controller_output_s.command.speeds_rpm[0], 1); @@ -62,7 +67,8 @@ TEST(DrivebrainControllerTesting, speed_setpoint_too_latent_and_torque_setpoint_ TEST(DrivebrainControllerTesting, speed_setpoint_too_latent_and_msg_jitter_too_high) { TorqueControllerOutput_s torque_controller_output_s = {}; - runTick(10, 5, 1011, 1001, &torque_controller_output_s); + DrivebrainController controller(torque_controller_output_s, 10, 5); + runTick(&controller, 1011, 1001, true); EXPECT_FALSE(torque_controller_output_s.ready); EXPECT_NE(torque_controller_output_s.command.speeds_rpm[0], 1); @@ -70,10 +76,37 @@ TEST(DrivebrainControllerTesting, speed_setpoint_too_latent_and_msg_jitter_too_h TEST(DrivebrainControllerTesting, torque_setpoint_too_latent_and_msg_jitter_too_high) { TorqueControllerOutput_s torque_controller_output_s = {}; - runTick(10, 5, 1001, 1011, &torque_controller_output_s); + DrivebrainController controller(torque_controller_output_s, 10, 5); + runTick(&controller, 1001, 1011, true); EXPECT_FALSE(torque_controller_output_s.ready); EXPECT_NE(torque_controller_output_s.command.speeds_rpm[0], 1); } +TEST(DrivebrainControllerTesting, failing_stay_failing) { + TorqueControllerOutput_s torque_controller_output_s = {}; + DrivebrainController controller(torque_controller_output_s, 10, 5); + runTick(&controller, 1001, 1011, true); + EXPECT_FALSE(torque_controller_output_s.ready); + EXPECT_NE(torque_controller_output_s.command.speeds_rpm[0], 1); + + runTick(&controller, 1001, 1001, true); + EXPECT_FALSE(torque_controller_output_s.ready); + EXPECT_NE(torque_controller_output_s.command.speeds_rpm[0], 1); +} + +TEST(DrivebrainControllerTesting, failing_reset_success) { + TorqueControllerOutput_s torque_controller_output_s = {}; + DrivebrainController controller(torque_controller_output_s, 10, 5); + runTick(&controller, 1001, 1011, true); + EXPECT_FALSE(torque_controller_output_s.ready); + EXPECT_NE(torque_controller_output_s.command.speeds_rpm[0], 1); + + runTick(&controller, 1001, 1001, false); + + runTick(&controller, 1001, 1001, true); + EXPECT_TRUE(torque_controller_output_s.ready); + EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 1); +} + #endif DRIVEBRAIN_CONTROLLER_TEST \ No newline at end of file From 57c2a969be25ff634beee3b0dd07d017a61d65b2 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sat, 7 Sep 2024 03:02:15 -0400 Subject: [PATCH 11/33] adjusting logic; removing interaction with tc and giving drivebrain more responsbility --- lib/interfaces/include/DrivebrainInterface.h | 1 + lib/interfaces/src/DrivebrainInterface.cpp | 12 +- lib/systems/include/CASESystem.tpp | 118 +++++++++--------- lib/systems/include/TorqueControllerMux.h | 4 +- lib/systems/src/DrivebrainController.cpp | 27 +++- lib/systems/src/TorqueControllerMux.cpp | 4 +- src/main.cpp | 6 +- .../test_systems/drivebrain_controller_test.h | 38 +++--- 8 files changed, 118 insertions(+), 92 deletions(-) diff --git a/lib/interfaces/include/DrivebrainInterface.h b/lib/interfaces/include/DrivebrainInterface.h index d495d3367..85573e95d 100644 --- a/lib/interfaces/include/DrivebrainInterface.h +++ b/lib/interfaces/include/DrivebrainInterface.h @@ -8,6 +8,7 @@ class DrivebrainInterface public: DrivebrainInterface(CANBufferType * msg_output_queue) { + _msg_queue = msg_output_queue; _current_drivebrain_data.last_speed_setpoint_receive_time_millis= -1; _current_drivebrain_data.last_torque_lim_receive_time_millis= -1; diff --git a/lib/interfaces/src/DrivebrainInterface.cpp b/lib/interfaces/src/DrivebrainInterface.cpp index 12bb0b5d6..cd9980e13 100644 --- a/lib/interfaces/src/DrivebrainInterface.cpp +++ b/lib/interfaces/src/DrivebrainInterface.cpp @@ -11,7 +11,12 @@ void DrivebrainInterface::receive_db_torque_lim_message(CAN_message_t& msg, unsi auto fr = HYTECH_drivebrain_torque_fr_ro_fromS(db_data.drivebrain_torque_fr_ro); auto rl = HYTECH_drivebrain_torque_rl_ro_fromS(db_data.drivebrain_torque_rl_ro); auto rr = HYTECH_drivebrain_torque_rr_ro_fromS(db_data.drivebrain_torque_rr_ro); - _current_drivebrain_data.torque_limits_nm.construct(fl, fr, rl, rr); + + _current_drivebrain_data.torque_limits_nm.FL = (float)fl; + _current_drivebrain_data.torque_limits_nm.FR = (float)fr; + _current_drivebrain_data.torque_limits_nm.RL = (float)rl; + _current_drivebrain_data.torque_limits_nm.RR = (float)rr; + } void DrivebrainInterface::receive_db_speed_setpoint_message(CAN_message_t& msg, unsigned long curr_millis) @@ -20,6 +25,9 @@ void DrivebrainInterface::receive_db_speed_setpoint_message(CAN_message_t& msg, Unpack_DRIVEBRAIN_SPEED_SET_INPUT_hytech(&db_data, msg.buf, msg.len); _current_drivebrain_data.last_speed_setpoint_receive_time_millis = curr_millis; - _current_drivebrain_data.speed_setpoints_rpm.construct(db_data.drivebrain_set_rpm_fl, db_data.drivebrain_set_rpm_fr, db_data.drivebrain_set_rpm_rl, db_data.drivebrain_set_rpm_rr); + _current_drivebrain_data.speed_setpoints_rpm.FL = (float)db_data.drivebrain_set_rpm_fl; + _current_drivebrain_data.speed_setpoints_rpm.FR = (float)db_data.drivebrain_set_rpm_fr; + _current_drivebrain_data.speed_setpoints_rpm.RL = (float)db_data.drivebrain_set_rpm_rl; + _current_drivebrain_data.speed_setpoints_rpm.RR = (float)db_data.drivebrain_set_rpm_rr; } \ No newline at end of file diff --git a/lib/systems/include/CASESystem.tpp b/lib/systems/include/CASESystem.tpp index 3a6db48c7..4418a926e 100644 --- a/lib/systems/include/CASESystem.tpp +++ b/lib/systems/include/CASESystem.tpp @@ -200,71 +200,71 @@ DrivetrainCommand_s CASESystem::evaluate( // send these out at the send period - if ((tick.millis - last_controller_pt1_send_time_) >= (controller_send_period_ms_)) - { - enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_initia); - enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_norm_p); - enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_pow_pn); - enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_to); - enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_regen_); - enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_pid__p); - - last_controller_pt1_send_time_ = tick.millis; - } + // if ((tick.millis - last_controller_pt1_send_time_) >= (controller_send_period_ms_)) + // { + // enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_initia); + // enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_norm_p); + // enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_pow_pn); + // enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_to); + // enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_regen_); + // enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_pid__p); + + // last_controller_pt1_send_time_ = tick.millis; + // } - if (((tick.millis - last_controller_pt1_send_time_) >= (vehicle_math_offset_ms_ / 3)) && - ((tick.millis - last_controller_pt2_send_time_) > controller_send_period_ms_)) - { - enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_pid_ya); - enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_normal); - enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_pi); - enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs__p); - enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_st); - enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_pn); - - last_controller_pt2_send_time_ = tick.millis; - } + // if (((tick.millis - last_controller_pt1_send_time_) >= (vehicle_math_offset_ms_ / 3)) && + // ((tick.millis - last_controller_pt2_send_time_) > controller_send_period_ms_)) + // { + // enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_pid_ya); + // enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_normal); + // enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_pi); + // enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs__p); + // enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_st); + // enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_pn); + + // last_controller_pt2_send_time_ = tick.millis; + // } - if (((tick.millis - last_controller_pt2_send_time_) >= (vehicle_math_offset_ms_ / 3)) && - ((tick.millis - last_controller_pt3_send_time_) > controller_send_period_ms_)) - { + // if (((tick.millis - last_controller_pt2_send_time_) >= (vehicle_math_offset_ms_ / 3)) && + // ((tick.millis - last_controller_pt3_send_time_) > controller_send_period_ms_)) + // { - enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_sl); - enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_rege_p); - enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_torque); - enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_power_); - enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_powe_p); + // enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_sl); + // enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_rege_p); + // enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_torque); + // enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_power_); + // enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_powe_p); - last_controller_pt3_send_time_ = tick.millis; - } + // last_controller_pt3_send_time_ = tick.millis; + // } - if (((tick.millis - last_controller_pt1_send_time_) >= vehicle_math_offset_ms_) && - ((tick.millis - last_vehm_send_time_) > controller_send_period_ms_)) - { - enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_alpha_deg); - enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_sl); - enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_long_corner_); - enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_wheel_steer_); - enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_kin_desired_); - enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_beta_deg); - enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_wheel_lin_ve); - - last_vehm_send_time_ = tick.millis; - } + // if (((tick.millis - last_controller_pt1_send_time_) >= vehicle_math_offset_ms_) && + // ((tick.millis - last_vehm_send_time_) > controller_send_period_ms_)) + // { + // enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_alpha_deg); + // enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_sl); + // enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_long_corner_); + // enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_wheel_steer_); + // enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_kin_desired_); + // enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_beta_deg); + // enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_wheel_lin_ve); + + // last_vehm_send_time_ = tick.millis; + // } - if ((tick.millis - last_lowest_priority_controller_send_time_) >= (lowest_priority_controller_send_period_ms_)) - { - enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_boolea); - enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_co); - enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_yaw_pi); - enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_sa); - enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_di); - enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_rp); - enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_nl); - enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tc_pna); - - last_lowest_priority_controller_send_time_ = tick.millis; - } + // if ((tick.millis - last_lowest_priority_controller_send_time_) >= (lowest_priority_controller_send_period_ms_)) + // { + // enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_boolea); + // enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_co); + // enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_yaw_pi); + // enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_sa); + // enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_di); + // enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_rp); + // enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_nl); + // enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tc_pna); + + // last_lowest_priority_controller_send_time_ = tick.millis; + // } DrivetrainCommand_s command; diff --git a/lib/systems/include/TorqueControllerMux.h b/lib/systems/include/TorqueControllerMux.h index ac04971f5..a66bea9d7 100644 --- a/lib/systems/include/TorqueControllerMux.h +++ b/lib/systems/include/TorqueControllerMux.h @@ -75,7 +75,7 @@ class TorqueControllerMux , torqueControllerSimple_(controllerOutputs_[static_cast(TorqueController_e::TC_SAFE_MODE)]) , torqueControllerLoadCellVectoring_(controllerOutputs_[static_cast(TorqueController_e::TC_LOAD_CELL_VECTORING)]) , torqueControllerSimpleLaunch_(controllerOutputs_[static_cast(TorqueController_e::TC_SIMPLE_LAUNCH)]) - , _dbController(controllerOutputs_[static_cast(TorqueController_e::TC_DRIVEBRAIN)], 30, 10) + , _dbController(controllerOutputs_[static_cast(TorqueController_e::TC_DRIVEBRAIN)], 100, 10) , tcCASEWrapper_(controllerOutputs_[static_cast(TorqueController_e::TC_CASE_SYSTEM)]) , telemHandle_(telemInterface) {} @@ -88,7 +88,7 @@ class TorqueControllerMux , torqueControllerSimple_(controllerOutputs_[static_cast(TorqueController_e::TC_SAFE_MODE)], simpleTCRearTorqueScale, simpleTCRegenTorqueScale) , torqueControllerLoadCellVectoring_(controllerOutputs_[static_cast(TorqueController_e::TC_LOAD_CELL_VECTORING)], 1.0, simpleTCRegenTorqueScale) , torqueControllerSimpleLaunch_(controllerOutputs_[static_cast(TorqueController_e::TC_SIMPLE_LAUNCH)]) - , _dbController(controllerOutputs_[static_cast(TorqueController_e::TC_DRIVEBRAIN)], 30, 10) + , _dbController(controllerOutputs_[static_cast(TorqueController_e::TC_DRIVEBRAIN)], 200, 120) , tcCASEWrapper_(controllerOutputs_[static_cast(TorqueController_e::TC_CASE_SYSTEM)]) , telemHandle_(telemInterface) {} diff --git a/lib/systems/src/DrivebrainController.cpp b/lib/systems/src/DrivebrainController.cpp index 0932e50a6..a223bc3d2 100644 --- a/lib/systems/src/DrivebrainController.cpp +++ b/lib/systems/src/DrivebrainController.cpp @@ -2,12 +2,30 @@ void DrivebrainController::tick(const SysTick_s &sys_tick, DrivebrainData db_input, bool is_active_controller) { - _writeout.ready = false; + + _writeout.ready = true; + bool speed_setpoint_too_latent = (::abs((int)(sys_tick.millis - db_input.last_speed_setpoint_receive_time_millis)) > (int)_params.allowed_latency); bool torque_setpoint_too_latent = (::abs((int)(sys_tick.millis - db_input.last_torque_lim_receive_time_millis)) > (int)_params.allowed_latency); - bool msg_jitter_too_high = (::abs((int)(db_input.last_speed_setpoint_receive_time_millis - db_input.last_torque_lim_receive_time_millis)) > (int)_params.allowed_jitter); + auto speed_sp_recv = db_input.last_speed_setpoint_receive_time_millis; + auto t_sp_recv = db_input.last_torque_lim_receive_time_millis; + + bool msg_jitter_too_high; + + int diff = 0; + if(t_sp_recv > speed_sp_recv) + { + msg_jitter_too_high = ( (t_sp_recv - speed_sp_recv) > _params.allowed_jitter); + diff = (t_sp_recv - speed_sp_recv); + } else if (speed_sp_recv > t_sp_recv){ + diff = (speed_sp_recv - t_sp_recv); + msg_jitter_too_high = ( (speed_sp_recv - t_sp_recv) > _params.allowed_jitter); + } else { + msg_jitter_too_high = false; + } + bool timing_failure = (speed_setpoint_too_latent || torque_setpoint_too_latent || msg_jitter_too_high); // only in the case that this is the active controller do we want to clear our timing failure @@ -16,9 +34,9 @@ void DrivebrainController::tick(const SysTick_s &sys_tick, DrivebrainData db_inp // timing failure should be false here _timing_failure = false; } + if (!timing_failure && (!_timing_failure)) { - _writeout.ready = true; _last_sent_speed_setpoint_millis = db_input.last_speed_setpoint_receive_time_millis; _last_sent_torque_lim_millis = db_input.last_torque_lim_receive_time_millis; @@ -28,7 +46,6 @@ void DrivebrainController::tick(const SysTick_s &sys_tick, DrivebrainData db_inp else { _timing_failure = true; - _writeout.ready = false; - _writeout.command = {}; // set command to all zeros if bad latency is apparent + _writeout.command = {0.0f, 0.0f, 0.0f ,0.0f }; // set command to all zeros if bad latency is apparent } } \ No newline at end of file diff --git a/lib/systems/src/TorqueControllerMux.cpp b/lib/systems/src/TorqueControllerMux.cpp index 2c49faf64..951e519b1 100644 --- a/lib/systems/src/TorqueControllerMux.cpp +++ b/lib/systems/src/TorqueControllerMux.cpp @@ -90,9 +90,9 @@ void TorqueControllerMux::tick( // Check if the current controller is ready. If it has faulted, revert to safe mode // When the car goes below 5m/s, it will attempt to re-engage the faulted controller // It will stay in safe mode if the controller is still faulted - if (controllerOutputs_[static_cast(muxMode_)].ready == false) + if (!controllerOutputs_[static_cast(muxMode_)].ready) { - muxMode_ = TorqueController_e::TC_SAFE_MODE; + muxMode_ = TorqueController_e::TC_NO_CONTROLLER; } // Update TCMux status diff --git a/src/main.cpp b/src/main.cpp index 26e8b9550..58068cb2a 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -551,12 +551,12 @@ void tick_all_interfaces(const SysTick_s ¤t_system_tick) a1.get().conversions[MCU15_BRAKE2_CHANNEL], pedals_system.getMechBrakeActiveThreshold(), {}); + ams_interface.tick(current_system_tick); } if (t.trigger50) // 50Hz { steering1.sample(); - ams_interface.tick(current_system_tick); } if (t.trigger100) // 100Hz @@ -651,7 +651,7 @@ void tick_all_systems(const SysTick_s ¤t_system_tick) vn_interface.get_vn_struct().vn_status); // case_system.update_config_from_param_interface(param_interface); - + auto test = db_interface.get_latest_db_data(); torque_controller_mux.tick( current_system_tick, drivetrain.get_dynamic_data(), @@ -663,7 +663,7 @@ void tick_all_systems(const SysTick_s ¤t_system_tick) dashboard.torqueModeButtonPressed(), vn_interface.get_vn_struct(), controller_output, - db_interface.get_latest_db_data()); + test); } void handle_ethernet_interface_comms() diff --git a/test/test_systems/drivebrain_controller_test.h b/test/test_systems/drivebrain_controller_test.h index 1228e5fb0..9a8aee25d 100644 --- a/test/test_systems/drivebrain_controller_test.h +++ b/test/test_systems/drivebrain_controller_test.h @@ -33,8 +33,8 @@ TEST(DrivebrainControllerTesting, speed_setpoint_too_latent) { DrivebrainController controller(torque_controller_output_s, 5, 5); runTick(&controller, 1001, 1006, true); - EXPECT_FALSE(torque_controller_output_s.ready); - EXPECT_NE(torque_controller_output_s.command.speeds_rpm[0], 1); + // EXPECT_FALSE(torque_controller_output_s.ready); + EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 0); } TEST(DrivebrainControllerTesting, torque_setpoint_too_latent) { @@ -42,8 +42,8 @@ TEST(DrivebrainControllerTesting, torque_setpoint_too_latent) { DrivebrainController controller(torque_controller_output_s, 5, 5); runTick(&controller, 1006, 1001, true); - EXPECT_FALSE(torque_controller_output_s.ready); - EXPECT_NE(torque_controller_output_s.command.speeds_rpm[0], 1); + // EXPECT_FALSE(torque_controller_output_s.ready); + EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 0); } TEST(DrivebrainControllerTesting, msg_jitter_too_high) { @@ -51,8 +51,8 @@ TEST(DrivebrainControllerTesting, msg_jitter_too_high) { DrivebrainController controller(torque_controller_output_s, 10, 5); runTick(&controller, 1001, 1009, true); - EXPECT_FALSE(torque_controller_output_s.ready); - EXPECT_NE(torque_controller_output_s.command.speeds_rpm[0], 1); + // EXPECT_FALSE(torque_controller_output_s.ready); + EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 0); } TEST(DrivebrainControllerTesting, speed_setpoint_too_latent_and_torque_setpoint_too_latent) { @@ -60,8 +60,8 @@ TEST(DrivebrainControllerTesting, speed_setpoint_too_latent_and_torque_setpoint_ DrivebrainController controller(torque_controller_output_s, 5, 10); runTick(&controller, 1001, 1015, true); - EXPECT_FALSE(torque_controller_output_s.ready); - EXPECT_NE(torque_controller_output_s.command.speeds_rpm[0], 1); + // EXPECT_FALSE(torque_controller_output_s.ready); + EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 0); } @@ -70,8 +70,8 @@ TEST(DrivebrainControllerTesting, speed_setpoint_too_latent_and_msg_jitter_too_h DrivebrainController controller(torque_controller_output_s, 10, 5); runTick(&controller, 1011, 1001, true); - EXPECT_FALSE(torque_controller_output_s.ready); - EXPECT_NE(torque_controller_output_s.command.speeds_rpm[0], 1); + // EXPECT_FALSE(torque_controller_output_s.ready); + EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 0); } TEST(DrivebrainControllerTesting, torque_setpoint_too_latent_and_msg_jitter_too_high) { @@ -79,33 +79,33 @@ TEST(DrivebrainControllerTesting, torque_setpoint_too_latent_and_msg_jitter_too_ DrivebrainController controller(torque_controller_output_s, 10, 5); runTick(&controller, 1001, 1011, true); - EXPECT_FALSE(torque_controller_output_s.ready); - EXPECT_NE(torque_controller_output_s.command.speeds_rpm[0], 1); + // EXPECT_FALSE(torque_controller_output_s.ready); + EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 0); } TEST(DrivebrainControllerTesting, failing_stay_failing) { TorqueControllerOutput_s torque_controller_output_s = {}; DrivebrainController controller(torque_controller_output_s, 10, 5); runTick(&controller, 1001, 1011, true); - EXPECT_FALSE(torque_controller_output_s.ready); - EXPECT_NE(torque_controller_output_s.command.speeds_rpm[0], 1); + // EXPECT_FALSE(torque_controller_output_s.ready); + EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 0); runTick(&controller, 1001, 1001, true); - EXPECT_FALSE(torque_controller_output_s.ready); - EXPECT_NE(torque_controller_output_s.command.speeds_rpm[0], 1); + // EXPECT_FALSE(torque_controller_output_s.ready); + EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 0); } TEST(DrivebrainControllerTesting, failing_reset_success) { TorqueControllerOutput_s torque_controller_output_s = {}; DrivebrainController controller(torque_controller_output_s, 10, 5); runTick(&controller, 1001, 1011, true); - EXPECT_FALSE(torque_controller_output_s.ready); - EXPECT_NE(torque_controller_output_s.command.speeds_rpm[0], 1); + // EXPECT_FALSE(torque_controller_output_s.ready); + EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 0); runTick(&controller, 1001, 1001, false); runTick(&controller, 1001, 1001, true); - EXPECT_TRUE(torque_controller_output_s.ready); + // EXPECT_TRUE(torque_controller_output_s.ready); EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 1); } From 77b1e2b34965e39aa0cd57df6dfba1e55ca6547d Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Mon, 16 Sep 2024 17:05:26 -0400 Subject: [PATCH 12/33] fixing oopsie in drivebrain control mode and adding it to main --- lib/systems/src/DrivebrainController.cpp | 2 +- src/main.cpp | 12 +++++++++--- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/lib/systems/src/DrivebrainController.cpp b/lib/systems/src/DrivebrainController.cpp index 89a178cef..3710f8b2b 100644 --- a/lib/systems/src/DrivebrainController.cpp +++ b/lib/systems/src/DrivebrainController.cpp @@ -50,7 +50,7 @@ TorqueControllerOutput_s DrivebrainController::evaluate(const SharedCarState_s & db_input.speed_setpoints_rpm.copy_to_arr(output.command.speeds_rpm); - db_input.speed_setpoints_rpm.copy_to_arr(output.command.torqueSetpoints); + db_input.torque_limits_nm.copy_to_arr(output.command.torqueSetpoints); } else { diff --git a/src/main.cpp b/src/main.cpp index 128e7b404..767eb25e9 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -33,6 +33,7 @@ #include "SafetySystem.h" #include "DrivetrainSystem.h" #include "PedalsSystem.h" +#include "DrivebrainController.h" // #include "TorqueControllerMux.h" #include "TorqueControllerMux.h" @@ -304,13 +305,14 @@ TorqueControllerCASEWrapper case_wrapper(&case_system); // mode 3 TorqueControllerSimpleLaunch simple_launch; // mode 4 -TorqueControllerSlipLaunch slip_launch; +DrivebrainController db_controller(210, 210); + TCMuxType torque_controller_mux({static_cast(&tc_simple), static_cast(&tc_vec), static_cast(&case_wrapper), static_cast(&simple_launch), - static_cast(&slip_launch)}, - {false, false, true, false, false}); + static_cast(&db_controller)}, + {false, false, true, false, true}); /* Declare state machine */ MCUStateMachine fsm(&buzzer, &drivetrain, &dashboard, &pedals_system, &torque_controller_mux, &safety_system); @@ -492,6 +494,10 @@ void loop() Serial.print("Current TC error: "); Serial.println(static_cast(torque_controller_mux.get_tc_mux_status().current_error)); Serial.println(); + Serial.print("dial state: "); + Serial.println(static_cast(dashboard.getDialMode())); + Serial.println(db_interface.get_latest_db_data().torque_limits_nm.FL); + Serial.println(); Serial.println(); } } From 8d4a7f130a26046c46531a57f81a54b2a05b5eb3 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Mon, 16 Sep 2024 20:54:45 -0400 Subject: [PATCH 13/33] ready to test on car --- .../include/DrivebrainETHInterface.h | 21 +++++++++++++++ ...sgInterface.h => ProtobufMsgInterface.hpp} | 26 +++++++++---------- lib/interfaces/library.json | 3 ++- lib/interfaces/src/DrivebrainETHInterface.cpp | 8 ++++++ lib/systems/include/CASESystem.h | 2 +- platformio.ini | 6 +++-- src/main.cpp | 24 +++++++++++------ 7 files changed, 64 insertions(+), 26 deletions(-) create mode 100644 lib/interfaces/include/DrivebrainETHInterface.h rename lib/interfaces/include/{ProtobufMsgInterface.h => ProtobufMsgInterface.hpp} (59%) create mode 100644 lib/interfaces/src/DrivebrainETHInterface.cpp diff --git a/lib/interfaces/include/DrivebrainETHInterface.h b/lib/interfaces/include/DrivebrainETHInterface.h new file mode 100644 index 000000000..0ba8f0ea5 --- /dev/null +++ b/lib/interfaces/include/DrivebrainETHInterface.h @@ -0,0 +1,21 @@ +#ifndef __DRIVEBRAINETHINTERFACE_H__ +#define __DRIVEBRAINETHINTERFACE_H__ +#include "hytech_msgs.pb.h" +#include "DrivebrainData.h" + +class DrivebrainETHInterface +{ + public: + DrivebrainETHInterface() = default; + + void receive_pb_msg(const hytech_msgs_MCUData& msg_in); + + DrivebrainData_s get_latest_data() { return _latest_data; } + + private: + + DrivebrainData_s _latest_data = {}; + +}; + +#endif // __DRIVEBRAINETHINTERFACE_H__ \ No newline at end of file diff --git a/lib/interfaces/include/ProtobufMsgInterface.h b/lib/interfaces/include/ProtobufMsgInterface.hpp similarity index 59% rename from lib/interfaces/include/ProtobufMsgInterface.h rename to lib/interfaces/include/ProtobufMsgInterface.hpp index 4b24cf586..61f2b36fc 100644 --- a/lib/interfaces/include/ProtobufMsgInterface.h +++ b/lib/interfaces/include/ProtobufMsgInterface.hpp @@ -6,37 +6,39 @@ #include "pb_common.h" #include "circular_buffer.h" #include "NativeEthernet.h" +#include // #include "InterfaceParams.h" struct ETHInterfaces { }; -using recv_function_t = void (*)(const uint8_t* buffer, size_t packet_size, ETHInterfaces& interfaces); +// using recv_function_t = void (*)(const uint8_t* buffer, size_t packet_size, ETHInterfaces& interfaces); // this should be usable with arbitrary functions idk something -template -void handle_ethernet_socket_receive(EthernetUDP* socket, recv_function_t recv_function, ETHInterfaces& interfaces) +template +void handle_ethernet_socket_receive(EthernetUDP *socket, std::function recv_function, eth_interface &interface, const pb_msgdesc_t *desc_pointer) { int packet_size = socket->parsePacket(); - if(packet_size > 0) + if (packet_size > 0) { Serial.println("packet size"); Serial.println(packet_size); uint8_t buffer[buffer_size]; size_t read_bytes = socket->read(buffer, sizeof(buffer)); socket->read(buffer, UDP_TX_PACKET_MAX_SIZE); - recv_function(buffer, read_bytes, interfaces); + recv_function(buffer, read_bytes, interface, desc_pointer); } } template -bool handle_ethernet_socket_send_pb(IPAddress addr, uint16_t port, EthernetUDP* socket, const pb_struct& msg, const pb_msgdesc_t* msg_desc) +bool handle_ethernet_socket_send_pb(IPAddress addr, uint16_t port, EthernetUDP *socket, const pb_struct &msg, const pb_msgdesc_t *msg_desc) { socket->beginPacket(addr, port); uint8_t buffer[buffer_size]; pb_ostream_t stream = pb_ostream_from_buffer(buffer, sizeof(buffer)); - if (!pb_encode(&stream, msg_desc, &msg)) { + if (!pb_encode(&stream, msg_desc, &msg)) + { // You can handle error more explicitly by looking at stream.errmsg return false; } @@ -46,19 +48,15 @@ bool handle_ethernet_socket_send_pb(IPAddress addr, uint16_t port, EthernetUDP* return true; } - -template -std::pair recv_pb_stream_msg(const uint8_t *buffer, size_t packet_size, ETHInterfaces& interfaces, const pb_msgdesc_t * desc_pointer) +template +void recv_pb_stream_msg(const uint8_t *buffer, size_t packet_size, eth_interface &interface, const pb_msgdesc_t *desc_pointer) { pb_istream_t stream = pb_istream_from_buffer(buffer, packet_size); pb_msg_type msg = {}; if (pb_decode(&stream, desc_pointer, &msg)) { - return {msg, true}; + interface.receive_pb_msg(msg); } - return {msg, }; } - - #endif \ No newline at end of file diff --git a/lib/interfaces/library.json b/lib/interfaces/library.json index 202cee41a..359a0f9e7 100644 --- a/lib/interfaces/library.json +++ b/lib/interfaces/library.json @@ -5,7 +5,8 @@ "dependencies": { "can_lib": "*", "shared_data": "*", - "CASE_lib": "*" + "CASE_lib": "*", + "nanopb": "*" }, "frameworks": "*", "platforms": "*" diff --git a/lib/interfaces/src/DrivebrainETHInterface.cpp b/lib/interfaces/src/DrivebrainETHInterface.cpp new file mode 100644 index 000000000..fe4bf359e --- /dev/null +++ b/lib/interfaces/src/DrivebrainETHInterface.cpp @@ -0,0 +1,8 @@ +#include "DrivebrainETHInterface.h" + +#include +void DrivebrainETHInterface::receive_pb_msg(const hytech_msgs_MCUData& msg_in) +{ + Serial.println("msg recvd"); + _latest_data = {}; +} \ No newline at end of file diff --git a/lib/systems/include/CASESystem.h b/lib/systems/include/CASESystem.h index 5e5c962f2..737fb011f 100644 --- a/lib/systems/include/CASESystem.h +++ b/lib/systems/include/CASESystem.h @@ -7,7 +7,7 @@ #include "DrivetrainSystem.h" #include "SteeringSystem.h" #include "MCUStateMachine.h" -#include "ProtobufMsgInterface.h" +#include "ProtobufMsgInterface.hpp" // #include "ParameterInterface.h" struct CASEConfiguration diff --git a/platformio.ini b/platformio.ini index 0b3a02629..5c535e22e 100644 --- a/platformio.ini +++ b/platformio.ini @@ -4,7 +4,8 @@ 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-09-15T16_39_42/hytech_msgs_pb_lib.tar.gz + [env:test_env] platform = native test_framework = googletest @@ -14,7 +15,7 @@ build_src_filter = build_unflags = -std=gnu++11 build_flags = - -std=c++17 + -std=c++17 -g -D TESTING_SYSTEMS lib_ignore = @@ -57,6 +58,7 @@ upload_protocol = teensy-cli extra_scripts = pre:extra_script.py lib_deps = ${common.lib_deps_shared} + Nanopb SPI https://github.com/hytech-racing/shared_firmware_interfaces.git#shared_types https://github.com/tonton81/FlexCAN_T4 diff --git a/src/main.cpp b/src/main.cpp index 767eb25e9..01bdcf7e0 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -7,9 +7,10 @@ #include "HyTech_CAN.h" #include "MCU_rev15_defs.h" // #include "NativeEthernet.h" - +#include "pb.h" // /* Interfaces */ - +#include "DrivebrainETHInterface.h" +#include "ProtobufMsgInterface.hpp" #include "HytechCANInterface.h" #include "ThermistorInterface.h" #include "Teensy_ADC.h" @@ -34,7 +35,6 @@ #include "DrivetrainSystem.h" #include "PedalsSystem.h" #include "DrivebrainController.h" -// #include "TorqueControllerMux.h" #include "TorqueControllerMux.h" #include "CASESystem.h" @@ -42,7 +42,10 @@ #include "MCUStateMachine.h" #include "HT08_CASE.h" +#include "InterfaceParams.h" #include "PrintLogger.h" + +#include "hytech_msgs.pb.h" /* PARAMETER STRUCTS */ @@ -322,6 +325,7 @@ MCUStateMachine fsm(&buzzer, &drivetrain, &dashboard, &pedals_system // */ DrivebrainInterface db_interface(&CAN3_txBuffer); +DrivebrainETHInterface db_eth_interface; CANInterfaces CAN_receive_interfaces = {&inv.fl, &inv.fr, &inv.rl, &inv.rr, &vn_interface, &dashboard, &ams_interface, &sab_interface, &db_interface}; /* @@ -348,9 +352,9 @@ void setup() // initialize CAN communication init_all_CAN_devices(); - // Ethernet.begin(EthParams::default_MCU_MAC_address, EthParams::default_MCU_ip); - // protobuf_send_socket.begin(EthParams::default_protobuf_send_port); - // protobuf_recv_socket.begin(EthParams::default_protobuf_recv_port); + Ethernet.begin(EthParams::default_MCU_MAC_address, EthParams::default_MCU_ip); + protobuf_send_socket.begin(EthParams::default_protobuf_send_port); + protobuf_recv_socket.begin(EthParams::default_protobuf_recv_port); SPI.begin(); a1.init(); @@ -412,7 +416,7 @@ void loop() // get latest tick from sys clock SysTick_s curr_tick = sys_clock.tick(micros()); - // handle_ethernet_interface_comms(); + handle_ethernet_interface_comms(); // process received CAN messages process_ring_buffer(CAN2_rxBuffer, CAN_receive_interfaces, curr_tick.millis); @@ -687,5 +691,9 @@ void handle_ethernet_interface_comms() // 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_MCUData>(&protobuf_recv_socket, recv_boi, db_eth_interface, hytech_msgs_MCUData_fields); + } \ No newline at end of file From c02d1f1e6cf97b74bcf50864c7450f7ab6653cd4 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Mon, 16 Sep 2024 21:22:48 -0400 Subject: [PATCH 14/33] fixing header --- .../{ProtobufMsgInterface.hpp => ProtobufMsgInterface.h} | 0 lib/systems/include/CASESystem.h | 2 +- src/main.cpp | 2 +- 3 files changed, 2 insertions(+), 2 deletions(-) rename lib/interfaces/include/{ProtobufMsgInterface.hpp => ProtobufMsgInterface.h} (100%) diff --git a/lib/interfaces/include/ProtobufMsgInterface.hpp b/lib/interfaces/include/ProtobufMsgInterface.h similarity index 100% rename from lib/interfaces/include/ProtobufMsgInterface.hpp rename to lib/interfaces/include/ProtobufMsgInterface.h diff --git a/lib/systems/include/CASESystem.h b/lib/systems/include/CASESystem.h index 737fb011f..5e5c962f2 100644 --- a/lib/systems/include/CASESystem.h +++ b/lib/systems/include/CASESystem.h @@ -7,7 +7,7 @@ #include "DrivetrainSystem.h" #include "SteeringSystem.h" #include "MCUStateMachine.h" -#include "ProtobufMsgInterface.hpp" +#include "ProtobufMsgInterface.h" // #include "ParameterInterface.h" struct CASEConfiguration diff --git a/src/main.cpp b/src/main.cpp index 01bdcf7e0..0677dcdff 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -10,7 +10,7 @@ #include "pb.h" // /* Interfaces */ #include "DrivebrainETHInterface.h" -#include "ProtobufMsgInterface.hpp" +#include "ProtobufMsgInterface.h" #include "HytechCANInterface.h" #include "ThermistorInterface.h" #include "Teensy_ADC.h" From 200da818694612acf262fcbd50162647be18913e Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Tue, 17 Sep 2024 02:39:47 -0400 Subject: [PATCH 15/33] seemingly working ethernet integration --- include/InterfaceParams.h | 2 +- .../include/DrivebrainETHInterface.h | 17 +++++----- lib/interfaces/include/ProtobufMsgInterface.h | 13 +++----- lib/interfaces/src/DrivebrainETHInterface.cpp | 31 ++++++++++++++++--- lib/shared_data/include/Utility.h | 10 +++--- lib/systems/include/DrivebrainController.h | 2 +- lib/systems/src/DrivebrainController.cpp | 2 +- platformio.ini | 2 +- src/main.cpp | 22 ++++++++----- 9 files changed, 64 insertions(+), 37 deletions(-) diff --git a/include/InterfaceParams.h b/include/InterfaceParams.h index 0c55b6e92..0370d45f0 100644 --- a/include/InterfaceParams.h +++ b/include/InterfaceParams.h @@ -8,7 +8,7 @@ namespace EthParams {0x04, 0xe9, 0xe5, 0x10, 0x1f, 0x22}; const IPAddress default_MCU_ip(192, 168, 1, 30); - const IPAddress default_TCU_ip(192, 168, 1, 68); + const IPAddress default_TCU_ip(192, 168, 1, 69); const uint16_t default_protobuf_send_port = 2001; const uint16_t default_protobuf_recv_port = 2000; diff --git a/lib/interfaces/include/DrivebrainETHInterface.h b/lib/interfaces/include/DrivebrainETHInterface.h index 0ba8f0ea5..cc7e1c034 100644 --- a/lib/interfaces/include/DrivebrainETHInterface.h +++ b/lib/interfaces/include/DrivebrainETHInterface.h @@ -2,20 +2,19 @@ #define __DRIVEBRAINETHINTERFACE_H__ #include "hytech_msgs.pb.h" #include "DrivebrainData.h" - +#include "SharedDataTypes.h" class DrivebrainETHInterface { - public: - DrivebrainETHInterface() = default; - - void receive_pb_msg(const hytech_msgs_MCUData& msg_in); - - DrivebrainData_s get_latest_data() { return _latest_data; } +public: + DrivebrainETHInterface() = default; - private: + void receive_pb_msg(const hytech_msgs_MCUCommandData &msg_in, unsigned long curr_millis); - DrivebrainData_s _latest_data = {}; + hytech_msgs_MCUOutputData make_db_msg(const SharedCarState_s &shared_state); + DrivebrainData_s get_latest_data() { return _latest_data; } +private: + DrivebrainData_s _latest_data = {}; }; #endif // __DRIVEBRAINETHINTERFACE_H__ \ No newline at end of file diff --git a/lib/interfaces/include/ProtobufMsgInterface.h b/lib/interfaces/include/ProtobufMsgInterface.h index 61f2b36fc..328fdf6ed 100644 --- a/lib/interfaces/include/ProtobufMsgInterface.h +++ b/lib/interfaces/include/ProtobufMsgInterface.h @@ -13,21 +13,18 @@ struct ETHInterfaces { }; -// using recv_function_t = void (*)(const uint8_t* buffer, size_t packet_size, ETHInterfaces& interfaces); // this should be usable with arbitrary functions idk something template -void handle_ethernet_socket_receive(EthernetUDP *socket, std::function recv_function, eth_interface &interface, const pb_msgdesc_t *desc_pointer) +void handle_ethernet_socket_receive(const SysTick_s& tick, EthernetUDP *socket, std::function recv_function, eth_interface &interface, const pb_msgdesc_t *desc_pointer) { int packet_size = socket->parsePacket(); if (packet_size > 0) - { - Serial.println("packet size"); - Serial.println(packet_size); + { uint8_t buffer[buffer_size]; size_t read_bytes = socket->read(buffer, sizeof(buffer)); socket->read(buffer, UDP_TX_PACKET_MAX_SIZE); - recv_function(buffer, read_bytes, interface, desc_pointer); + recv_function(tick.millis, buffer, read_bytes, interface, desc_pointer); } } @@ -49,13 +46,13 @@ bool handle_ethernet_socket_send_pb(IPAddress addr, uint16_t port, EthernetUDP * } template -void recv_pb_stream_msg(const uint8_t *buffer, size_t packet_size, eth_interface &interface, const pb_msgdesc_t *desc_pointer) +void recv_pb_stream_msg(unsigned long curr_millis, const uint8_t *buffer, size_t packet_size, eth_interface &interface, const pb_msgdesc_t *desc_pointer) { pb_istream_t stream = pb_istream_from_buffer(buffer, packet_size); pb_msg_type msg = {}; if (pb_decode(&stream, desc_pointer, &msg)) { - interface.receive_pb_msg(msg); + interface.receive_pb_msg(msg, curr_millis); } } diff --git a/lib/interfaces/src/DrivebrainETHInterface.cpp b/lib/interfaces/src/DrivebrainETHInterface.cpp index fe4bf359e..030462577 100644 --- a/lib/interfaces/src/DrivebrainETHInterface.cpp +++ b/lib/interfaces/src/DrivebrainETHInterface.cpp @@ -1,8 +1,31 @@ #include "DrivebrainETHInterface.h" - +#include "SharedDataTypes.h" #include -void DrivebrainETHInterface::receive_pb_msg(const hytech_msgs_MCUData& msg_in) + +hytech_msgs_MCUOutputData DrivebrainETHInterface::make_db_msg(const SharedCarState_s &shared_state) +{ + 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]}; + return out; +} + +void DrivebrainETHInterface::receive_pb_msg(const hytech_msgs_MCUCommandData &msg_in, unsigned long curr_millis) { - Serial.println("msg recvd"); - _latest_data = {}; + // Serial.println("msg recvd in here"); + veh_vec nm_lim; + nm_lim.construct(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 speed_set; + speed_set.construct(msg_in.desired_rpms.FL, msg_in.desired_rpms.FR, msg_in.desired_rpms.RL, msg_in.desired_rpms.RR); + // Serial.println(msg_in.desired_rpms.FL); + + _latest_data.torque_limits_nm = nm_lim; + _latest_data.speed_setpoints_rpm = speed_set; + _latest_data.last_torque_lim_receive_time_millis = curr_millis; + _latest_data.last_speed_setpoint_receive_time_millis = curr_millis; } \ No newline at end of file diff --git a/lib/shared_data/include/Utility.h b/lib/shared_data/include/Utility.h index 1fbde62c9..66d0f2cb5 100644 --- a/lib/shared_data/include/Utility.h +++ b/lib/shared_data/include/Utility.h @@ -20,12 +20,12 @@ struct veh_vec T RL; T RR; - void construct(T FL, T FR, T RL, T RR) + void construct(T _FL, T _FR, T _RL, T _RR) { - FL = FL; - FR = FR; - RL = RL; - RR = RR; + FL = _FL; + FR = _FR; + RL = _RL; + RR = _RR; } /// @brief copy values to array in FL, FR, RL, RR order diff --git a/lib/systems/include/DrivebrainController.h b/lib/systems/include/DrivebrainController.h index 320462eb4..834df4a74 100644 --- a/lib/systems/include/DrivebrainController.h +++ b/lib/systems/include/DrivebrainController.h @@ -1,7 +1,7 @@ #ifndef __DRIVEBRAINCONTROLLER_H__ #define __DRIVEBRAINCONTROLLER_H__ -#include "DrivebrainInterface.h" + #include "TorqueControllers.h" #include "DrivebrainData.h" #include "BaseController.h" diff --git a/lib/systems/src/DrivebrainController.cpp b/lib/systems/src/DrivebrainController.cpp index 3710f8b2b..122a00168 100644 --- a/lib/systems/src/DrivebrainController.cpp +++ b/lib/systems/src/DrivebrainController.cpp @@ -7,7 +7,7 @@ TorqueControllerOutput_s DrivebrainController::evaluate(const SharedCarState_s & auto sys_tick = state.systick; auto db_input = state.db_data; - + bool speed_setpoint_too_latent = (::abs((int)(sys_tick.millis - db_input.last_speed_setpoint_receive_time_millis)) > (int)_params.allowed_latency); bool torque_setpoint_too_latent = (::abs((int)(sys_tick.millis - db_input.last_torque_lim_receive_time_millis)) > (int)_params.allowed_latency); diff --git a/platformio.ini b/platformio.ini index 5c535e22e..adc5e5ac3 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-09-15T16_39_42/hytech_msgs_pb_lib.tar.gz + https://github.com/hytech-racing/HT_proto/releases/download/2024-09-17T03_08_13/hytech_msgs_pb_lib.tar.gz [env:test_env] platform = native diff --git a/src/main.cpp b/src/main.cpp index 0677dcdff..a4cf22553 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -341,7 +341,7 @@ void tick_all_systems(const SysTick_s ¤t_system_tick); /* Reset inverters */ void drivetrain_reset(); -void handle_ethernet_interface_comms(); +void handle_ethernet_interface_comms(const SysTick_s& systick, const hytech_msgs_MCUOutputData& out_msg); /* SETUP @@ -416,7 +416,6 @@ void loop() // get latest tick from sys clock SysTick_s curr_tick = sys_clock.tick(micros()); - handle_ethernet_interface_comms(); // process received CAN messages process_ring_buffer(CAN2_rxBuffer, CAN_receive_interfaces, curr_tick.millis); @@ -435,9 +434,13 @@ void loop() load_cell_interface.getLoadCellForces(), pedals_system.getPedalsSystemData(), vn_interface.get_vn_struct(), - db_interface.get_latest_db_data(), + 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); // inverter procedure before entering state machine @@ -458,7 +461,8 @@ void loop() send_all_CAN_msgs(CAN3_txBuffer, &TELEM_CAN); // Basic debug prints - if (curr_tick.triggers.trigger5) + // if (curr_tick.triggers.trigger5) + if (false) { Serial.print("Steering system reported angle (deg): "); Serial.println(steering_system.getSteeringSystemData().angle); @@ -685,7 +689,7 @@ void tick_all_systems(const SysTick_s ¤t_system_tick) } -void handle_ethernet_interface_comms() +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. @@ -693,7 +697,11 @@ void handle_ethernet_interface_comms() // Serial.println("bruh"); - std::function recv_boi = &recv_pb_stream_msg; - handle_ethernet_socket_receive<1024, hytech_msgs_MCUData>(&protobuf_recv_socket, recv_boi, db_eth_interface, hytech_msgs_MCUData_fields); + 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) + { + 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 From 550754ce96b2785b5129d5afe1665e2964115f4f Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Wed, 18 Sep 2024 12:15:21 -0400 Subject: [PATCH 16/33] switching ethernet driver --- include/InterfaceParams.h | 5 ++++- lib/interfaces/include/ProtobufMsgInterface.h | 7 ++++--- platformio.ini | 1 - src/main.cpp | 3 ++- 4 files changed, 10 insertions(+), 6 deletions(-) diff --git a/include/InterfaceParams.h b/include/InterfaceParams.h index 0370d45f0..77d8f9e7c 100644 --- a/include/InterfaceParams.h +++ b/include/InterfaceParams.h @@ -1,6 +1,9 @@ #ifndef INTERFACEPARAMS #define INTERFACEPARAMS -#include "NativeEthernet.h" + +#include + +using namespace qindesign::network; namespace EthParams { diff --git a/lib/interfaces/include/ProtobufMsgInterface.h b/lib/interfaces/include/ProtobufMsgInterface.h index 328fdf6ed..490b43bb9 100644 --- a/lib/interfaces/include/ProtobufMsgInterface.h +++ b/lib/interfaces/include/ProtobufMsgInterface.h @@ -5,14 +5,15 @@ #include "pb_decode.h" #include "pb_common.h" #include "circular_buffer.h" -#include "NativeEthernet.h" +#include + #include -// #include "InterfaceParams.h" struct ETHInterfaces { }; +using namespace qindesign::network; // this should be usable with arbitrary functions idk something template @@ -23,7 +24,7 @@ void handle_ethernet_socket_receive(const SysTick_s& tick, EthernetUDP *socket, { uint8_t buffer[buffer_size]; size_t read_bytes = socket->read(buffer, sizeof(buffer)); - socket->read(buffer, UDP_TX_PACKET_MAX_SIZE); + socket->read(buffer, 1024); recv_function(tick.millis, buffer, read_bytes, interface, desc_pointer); } } diff --git a/platformio.ini b/platformio.ini index adc5e5ac3..1d508c33c 100644 --- a/platformio.ini +++ b/platformio.ini @@ -80,7 +80,6 @@ lib_ldf_mode = deep+ lib_deps = SPI Nanopb - https://github.com/vjmuzik/NativeEthernet.git https://github.com/tonton81/FlexCAN_T4#b928bcb https://github.com/RCMast3r/hytech_can#4ffbba4 https://github.com/hytech-racing/HT_params/releases/download/2024-05-07T06_59_33/ht_eth_pb_lib.tar.gz diff --git a/src/main.cpp b/src/main.cpp index a4cf22553..d97d07b82 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -6,7 +6,6 @@ #include "FlexCAN_T4.h" #include "HyTech_CAN.h" #include "MCU_rev15_defs.h" -// #include "NativeEthernet.h" #include "pb.h" // /* Interfaces */ #include "DrivebrainETHInterface.h" @@ -49,6 +48,8 @@ /* PARAMETER STRUCTS */ +using namespace qindesign::network; + const TelemetryInterfaceReadChannels telem_read_channels = { .accel1_channel = MCU15_ACCEL1_CHANNEL, From c61ec58fbafeeb1528cc4839125a6d02383376c6 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sat, 21 Sep 2024 11:29:33 -0400 Subject: [PATCH 17/33] 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 --- lib/interfaces/include/DrivebrainInterface.h | 30 -------- lib/interfaces/include/HytechCANInterface.h | 8 +-- lib/interfaces/include/InverterInterface.h | 6 +- lib/interfaces/include/InverterInterface.tpp | 6 +- lib/interfaces/include/ProtobufMsgInterface.h | 8 +-- lib/interfaces/src/DrivebrainETHInterface.cpp | 3 +- lib/interfaces/src/DrivebrainInterface.cpp | 33 --------- lib/interfaces/src/TelemetryInterface.cpp | 16 ++--- lib/shared_data/include/DrivebrainData.h | 3 +- lib/systems/include/DrivebrainController.h | 8 +-- lib/systems/include/DrivetrainSystem.tpp | 4 +- lib/systems/src/DrivebrainController.cpp | 27 +------ src/main.cpp | 4 +- .../test_systems/drivebrain_controller_test.h | 71 +++++-------------- 14 files changed, 46 insertions(+), 181 deletions(-) delete mode 100644 lib/interfaces/include/DrivebrainInterface.h delete mode 100644 lib/interfaces/src/DrivebrainInterface.cpp diff --git a/lib/interfaces/include/DrivebrainInterface.h b/lib/interfaces/include/DrivebrainInterface.h deleted file mode 100644 index f29d6475b..000000000 --- a/lib/interfaces/include/DrivebrainInterface.h +++ /dev/null @@ -1,30 +0,0 @@ -#ifndef __DRIVEBRAININTERFACE_H__ -#define __DRIVEBRAININTERFACE_H__ -#include "DrivebrainData.h" -#include "MessageQueueDefine.h" - -class DrivebrainInterface -{ - public: - DrivebrainInterface(CANBufferType * msg_output_queue) - { - _msg_queue = msg_output_queue; - _current_drivebrain_data.last_speed_setpoint_receive_time_millis= -1; - _current_drivebrain_data.last_torque_lim_receive_time_millis= -1; - _current_drivebrain_data.speed_setpoints_rpm = {}; - _current_drivebrain_data.torque_limits_nm = {}; - } - - void receive_db_torque_lim_message(CAN_message_t &msg, unsigned long curr_millis); - void receive_db_speed_setpoint_message(CAN_message_t& msg, unsigned long curr_millis); - - DrivebrainData_s get_latest_db_data() {return _current_drivebrain_data; } - - private: - - DrivebrainData_s _current_drivebrain_data; - CANBufferType * _msg_queue; - -}; - -#endif // __DRIVEBRAININTERFACE_H__ \ No newline at end of file diff --git a/lib/interfaces/include/HytechCANInterface.h b/lib/interfaces/include/HytechCANInterface.h index 492367a1b..8f3b4c7e2 100644 --- a/lib/interfaces/include/HytechCANInterface.h +++ b/lib/interfaces/include/HytechCANInterface.h @@ -33,7 +33,6 @@ struct CANInterfaces DashboardInterface *dash_interface; AMSInterface *ams_interface; SABInterface *sab_interface; - DrivebrainInterface *db_interface; }; // the goal with the can interface is that there exists a receive call that appends to a circular buffer @@ -168,7 +167,6 @@ void process_ring_buffer(BufferType &rx_buffer, const InterfaceType &interfaces, case VN_YPR_CANID: interfaces.vn_interface->retrieve_ypr_CAN(recvd_msg); break; - case VN_LAT_LON_CANID: interfaces.vn_interface->retrieve_lat_lon_CAN(recvd_msg); break; @@ -181,11 +179,7 @@ void process_ring_buffer(BufferType &rx_buffer, const InterfaceType &interfaces, case VN_ANGULAR_RATE_CANID: interfaces.vn_interface->receive_ang_rates_CAN(recvd_msg); break; - case DRIVEBRAIN_TORQUE_LIM_INPUT_CANID: - interfaces.db_interface->receive_db_torque_lim_message(recvd_msg, curr_millis); - break; - case DRIVEBRAIN_SPEED_SET_INPUT_CANID: - interfaces.db_interface->receive_db_speed_setpoint_message(recvd_msg, curr_millis); + default: break; } } diff --git a/lib/interfaces/include/InverterInterface.h b/lib/interfaces/include/InverterInterface.h index 8853becef..41312a1c1 100644 --- a/lib/interfaces/include/InverterInterface.h +++ b/lib/interfaces/include/InverterInterface.h @@ -119,8 +119,8 @@ class InverterInterface } int16_t get_speed() { return speed_; } - float get_torque_current() { return torque_current_; } - int16_t get_mag_current() { return magnetizing_current_; } + float get_motor_torque() { return actual_motor_torque_; } + int16_t get_commanded_torque() { return commanded_torque_; } float get_actual_torque() { return actual_torque_nm_; } uint16_t get_error_status(); MC_temps get_temps_msg() { return mc_temps_; } @@ -128,7 +128,7 @@ class InverterInterface private: float id110_val_; // for scaling to proper iq and id vals - int16_t torque_current_, magnetizing_current_; // iq and id in A respectively + int16_t actual_motor_torque_, commanded_torque_; float actual_torque_nm_; /* write setpoints message to the CAN buffer */ void write_cmd_msg_to_queue_(MC_setpoints_command msg); diff --git a/lib/interfaces/include/InverterInterface.tpp b/lib/interfaces/include/InverterInterface.tpp index 724c0f458..12aa71274 100644 --- a/lib/interfaces/include/InverterInterface.tpp +++ b/lib/interfaces/include/InverterInterface.tpp @@ -131,9 +131,9 @@ void InverterInterface::receive_status_msg(CAN_message_t &msg) // https://www.amk-motion.com/amk-dokucd/dokucd/en/content/resources/pdf-dateien/pdk_205481_kw26-s5-fse-4q_en_.pdf#page=83&zoom=100,76,82 // for the actual conversion. requires looking at the current params of the inverter // to get scalar for this. - // torque_current_ = ((float)mc_status.get_torque_current() * id110_val_) / 16384.0; // iq - torque_current_ = mc_status.get_torque_current(); - magnetizing_current_ = mc_status.get_magnetizing_current(); // id + // actual_motor_torque_ = ((float)mc_status.get_motor_torque() * id110_val_) / 16384.0; // iq + actual_motor_torque_ = mc_status.get_torque_current(); + commanded_torque_ = mc_status.get_magnetizing_current(); // id // TODO enable this on the inverters diff --git a/lib/interfaces/include/ProtobufMsgInterface.h b/lib/interfaces/include/ProtobufMsgInterface.h index 490b43bb9..3f874bc29 100644 --- a/lib/interfaces/include/ProtobufMsgInterface.h +++ b/lib/interfaces/include/ProtobufMsgInterface.h @@ -13,24 +13,22 @@ struct ETHInterfaces { }; -using namespace qindesign::network; - // this should be usable with arbitrary functions idk something template -void handle_ethernet_socket_receive(const SysTick_s& tick, EthernetUDP *socket, std::function recv_function, eth_interface &interface, const pb_msgdesc_t *desc_pointer) +void handle_ethernet_socket_receive(const SysTick_s& tick, qindesign::network::EthernetUDP *socket, std::function recv_function, eth_interface &interface, const pb_msgdesc_t *desc_pointer) { int packet_size = socket->parsePacket(); if (packet_size > 0) { uint8_t buffer[buffer_size]; size_t read_bytes = socket->read(buffer, sizeof(buffer)); - socket->read(buffer, 1024); + socket->read(buffer, buffer_size); recv_function(tick.millis, buffer, read_bytes, interface, desc_pointer); } } template -bool handle_ethernet_socket_send_pb(IPAddress addr, uint16_t port, EthernetUDP *socket, const pb_struct &msg, const pb_msgdesc_t *msg_desc) +bool handle_ethernet_socket_send_pb(IPAddress addr, uint16_t port, qindesign::network::EthernetUDP *socket, const pb_struct &msg, const pb_msgdesc_t *msg_desc) { socket->beginPacket(addr, port); uint8_t buffer[buffer_size]; diff --git a/lib/interfaces/src/DrivebrainETHInterface.cpp b/lib/interfaces/src/DrivebrainETHInterface.cpp index 030462577..4e1f90c66 100644 --- a/lib/interfaces/src/DrivebrainETHInterface.cpp +++ b/lib/interfaces/src/DrivebrainETHInterface.cpp @@ -26,6 +26,5 @@ void DrivebrainETHInterface::receive_pb_msg(const hytech_msgs_MCUCommandData &ms _latest_data.torque_limits_nm = nm_lim; _latest_data.speed_setpoints_rpm = speed_set; - _latest_data.last_torque_lim_receive_time_millis = curr_millis; - _latest_data.last_speed_setpoint_receive_time_millis = curr_millis; + _latest_data.last_receive_time_millis = curr_millis; } \ No newline at end of file diff --git a/lib/interfaces/src/DrivebrainInterface.cpp b/lib/interfaces/src/DrivebrainInterface.cpp deleted file mode 100644 index cd9980e13..000000000 --- a/lib/interfaces/src/DrivebrainInterface.cpp +++ /dev/null @@ -1,33 +0,0 @@ -#include "DrivebrainInterface.h" -#include "hytech.h" - -void DrivebrainInterface::receive_db_torque_lim_message(CAN_message_t& msg, unsigned long curr_millis) -{ - DRIVEBRAIN_TORQUE_LIM_INPUT_t db_data; - Unpack_DRIVEBRAIN_TORQUE_LIM_INPUT_hytech(&db_data, msg.buf, msg.len); - _current_drivebrain_data.last_torque_lim_receive_time_millis = curr_millis; - - auto fl = HYTECH_drivebrain_torque_fl_ro_fromS(db_data.drivebrain_torque_fl_ro); - auto fr = HYTECH_drivebrain_torque_fr_ro_fromS(db_data.drivebrain_torque_fr_ro); - auto rl = HYTECH_drivebrain_torque_rl_ro_fromS(db_data.drivebrain_torque_rl_ro); - auto rr = HYTECH_drivebrain_torque_rr_ro_fromS(db_data.drivebrain_torque_rr_ro); - - _current_drivebrain_data.torque_limits_nm.FL = (float)fl; - _current_drivebrain_data.torque_limits_nm.FR = (float)fr; - _current_drivebrain_data.torque_limits_nm.RL = (float)rl; - _current_drivebrain_data.torque_limits_nm.RR = (float)rr; - -} - -void DrivebrainInterface::receive_db_speed_setpoint_message(CAN_message_t& msg, unsigned long curr_millis) -{ - DRIVEBRAIN_SPEED_SET_INPUT_t db_data; - Unpack_DRIVEBRAIN_SPEED_SET_INPUT_hytech(&db_data, msg.buf, msg.len); - - _current_drivebrain_data.last_speed_setpoint_receive_time_millis = curr_millis; - _current_drivebrain_data.speed_setpoints_rpm.FL = (float)db_data.drivebrain_set_rpm_fl; - _current_drivebrain_data.speed_setpoints_rpm.FR = (float)db_data.drivebrain_set_rpm_fr; - _current_drivebrain_data.speed_setpoints_rpm.RL = (float)db_data.drivebrain_set_rpm_rl; - _current_drivebrain_data.speed_setpoints_rpm.RR = (float)db_data.drivebrain_set_rpm_rr; - -} \ No newline at end of file diff --git a/lib/interfaces/src/TelemetryInterface.cpp b/lib/interfaces/src/TelemetryInterface.cpp index 34c0b27fc..15b5f55a1 100644 --- a/lib/interfaces/src/TelemetryInterface.cpp +++ b/lib/interfaces/src/TelemetryInterface.cpp @@ -167,10 +167,10 @@ void TelemetryInterface::update_drivetrain_torque_telem_CAN_msg( // TODO: change this to use actual torque values from inverter // Torque current just temporary for gearbox seal validation DRIVETRAIN_TORQUE_TELEM_t torque; - torque.fl_motor_torque = fl->get_torque_current(); - torque.fr_motor_torque = fr->get_torque_current(); - torque.rl_motor_torque = rl->get_torque_current(); - torque.rr_motor_torque = rr->get_torque_current(); + torque.fl_motor_torque = fl->get_motor_torque(); + torque.fr_motor_torque = fr->get_motor_torque(); + torque.rl_motor_torque = rl->get_motor_torque(); + torque.rr_motor_torque = rr->get_motor_torque(); enqueue_new_CAN(&torque, &Pack_DRIVETRAIN_TORQUE_TELEM_hytech); } @@ -184,10 +184,10 @@ void TelemetryInterface::update_drivetrain_torque_filter_out_telem_CAN_msg( // TODO: change this to use actual torque values from inverter // Torque current just temporary for gearbox seal validation DRIVETRAIN_FILTER_OUT_TORQUE_TEL_t torque; - torque.fl_motor_torque = fl->get_mag_current(); - torque.fr_motor_torque = fr->get_mag_current(); - torque.rl_motor_torque = rl->get_mag_current(); - torque.rr_motor_torque = rr->get_mag_current(); + torque.fl_motor_torque = fl->get_commanded_torque(); + torque.fr_motor_torque = fr->get_commanded_torque(); + torque.rl_motor_torque = rl->get_commanded_torque(); + torque.rr_motor_torque = rr->get_commanded_torque(); enqueue_new_CAN(&torque, &Pack_DRIVETRAIN_FILTER_OUT_TORQUE_TEL_hytech); } diff --git a/lib/shared_data/include/DrivebrainData.h b/lib/shared_data/include/DrivebrainData.h index fa18fe4d6..d8f70a0ad 100644 --- a/lib/shared_data/include/DrivebrainData.h +++ b/lib/shared_data/include/DrivebrainData.h @@ -5,8 +5,7 @@ struct DrivebrainData_s { - unsigned long last_torque_lim_receive_time_millis; - unsigned long last_speed_setpoint_receive_time_millis; + unsigned long last_receive_time_millis; veh_vec torque_limits_nm; veh_vec speed_setpoints_rpm; }; diff --git a/lib/systems/include/DrivebrainController.h b/lib/systems/include/DrivebrainController.h index 834df4a74..7eb7b6e48 100644 --- a/lib/systems/include/DrivebrainController.h +++ b/lib/systems/include/DrivebrainController.h @@ -23,11 +23,11 @@ class DrivebrainController : public Controller { public: - DrivebrainController(unsigned long allowed_latency, unsigned long allowed_jitter, + DrivebrainController(unsigned long allowed_latency, float max_fault_clear_speed_m_s = 1.0, ControllerMode_e assigned_controller_mode = ControllerMode_e::MODE_4) { - _params = {allowed_jitter, allowed_latency, max_fault_clear_speed_m_s, assigned_controller_mode}; + _params = {allowed_latency, max_fault_clear_speed_m_s, assigned_controller_mode}; } TorqueControllerOutput_s evaluate(const SharedCarState_s &state); @@ -35,15 +35,13 @@ class DrivebrainController : public Controller private: struct { - unsigned long allowed_jitter; unsigned long allowed_latency; float max_fault_clear_speed_m_s; ControllerMode_e assigned_controller_mode; } _params; bool _timing_failure = false; - unsigned long _last_sent_torque_lim_millis = -1; - unsigned long _last_sent_speed_setpoint_millis = -1; + unsigned long _last_setpoint_millis = -1; }; #endif // __DRIVEBRAINCONTROLLER_H__ \ No newline at end of file diff --git a/lib/systems/include/DrivetrainSystem.tpp b/lib/systems/include/DrivetrainSystem.tpp index de01cc702..30565c739 100644 --- a/lib/systems/include/DrivetrainSystem.tpp +++ b/lib/systems/include/DrivetrainSystem.tpp @@ -233,8 +233,8 @@ DrivetrainDynamicReport_s DrivetrainSystem::get_dynamic_data() int inverter_ind = 0; for (auto inv_pointer : inverters_) { - auto iq = inv_pointer->get_torque_current(); // iq in A - auto id = inv_pointer->get_mag_current(); // id in A + auto iq = inv_pointer->get_motor_torque(); // iq in A + auto id = inv_pointer->get_commanded_torque(); // id in A dynamic_data_.measuredSpeeds[inverter_ind] = inv_pointer->get_speed(); dynamic_data_.measuredTorqueCurrents[inverter_ind] = iq; dynamic_data_.measuredMagnetizingCurrents[inverter_ind] = id; diff --git a/lib/systems/src/DrivebrainController.cpp b/lib/systems/src/DrivebrainController.cpp index 122a00168..c4c79441d 100644 --- a/lib/systems/src/DrivebrainController.cpp +++ b/lib/systems/src/DrivebrainController.cpp @@ -8,31 +8,13 @@ TorqueControllerOutput_s DrivebrainController::evaluate(const SharedCarState_s & auto sys_tick = state.systick; auto db_input = state.db_data; - bool speed_setpoint_too_latent = (::abs((int)(sys_tick.millis - db_input.last_speed_setpoint_receive_time_millis)) > (int)_params.allowed_latency); - bool torque_setpoint_too_latent = (::abs((int)(sys_tick.millis - db_input.last_torque_lim_receive_time_millis)) > (int)_params.allowed_latency); - - auto speed_sp_recv = db_input.last_speed_setpoint_receive_time_millis; - auto t_sp_recv = db_input.last_torque_lim_receive_time_millis; + bool message_too_latent = (::abs((int)(sys_tick.millis - db_input.last_receive_time_millis)) > (int)_params.allowed_latency); - bool msg_jitter_too_high; - int diff = 0; - if(t_sp_recv > speed_sp_recv) - { - msg_jitter_too_high = ( (t_sp_recv - speed_sp_recv) > _params.allowed_jitter); - diff = (t_sp_recv - speed_sp_recv); - } else if (speed_sp_recv > t_sp_recv){ - diff = (speed_sp_recv - t_sp_recv); - msg_jitter_too_high = ( (speed_sp_recv - t_sp_recv) > _params.allowed_jitter); - } else { - msg_jitter_too_high = false; - } - - bool timing_failure = (speed_setpoint_too_latent || torque_setpoint_too_latent || msg_jitter_too_high); + bool timing_failure = (message_too_latent); // only in the case that our speed is low enough (<1 m/s) do we want to clear the fault - bool is_active_controller = state.tc_mux_status.current_controller_mode_ == _params.assigned_controller_mode; if ((!is_active_controller) && (!timing_failure)) @@ -44,10 +26,7 @@ TorqueControllerOutput_s DrivebrainController::evaluate(const SharedCarState_s & TorqueControllerOutput_s output; if (!timing_failure && (!_timing_failure)) { - _last_sent_speed_setpoint_millis = db_input.last_speed_setpoint_receive_time_millis; - _last_sent_torque_lim_millis = db_input.last_torque_lim_receive_time_millis; - - + _last_setpoint_millis = db_input.last_receive_time_millis; db_input.speed_setpoints_rpm.copy_to_arr(output.command.speeds_rpm); db_input.torque_limits_nm.copy_to_arr(output.command.torqueSetpoints); diff --git a/src/main.cpp b/src/main.cpp index d97d07b82..2f95dd72d 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -325,9 +325,8 @@ MCUStateMachine fsm(&buzzer, &drivetrain, &dashboard, &pedals_system // GROUPING STRUCTS (To limit parameter count in utilizing functions) // */ -DrivebrainInterface db_interface(&CAN3_txBuffer); DrivebrainETHInterface db_eth_interface; -CANInterfaces CAN_receive_interfaces = {&inv.fl, &inv.fr, &inv.rl, &inv.rr, &vn_interface, &dashboard, &ams_interface, &sab_interface, &db_interface}; +CANInterfaces CAN_receive_interfaces = {&inv.fl, &inv.fr, &inv.rl, &inv.rr, &vn_interface, &dashboard, &ams_interface, &sab_interface}; /* FUNCTION DEFINITIONS @@ -505,7 +504,6 @@ void loop() Serial.println(); Serial.print("dial state: "); Serial.println(static_cast(dashboard.getDialMode())); - Serial.println(db_interface.get_latest_db_data().torque_limits_nm.FL); Serial.println(); Serial.println(); } diff --git a/test/test_systems/drivebrain_controller_test.h b/test/test_systems/drivebrain_controller_test.h index 7bb02d5d6..f2a2095ed 100644 --- a/test/test_systems/drivebrain_controller_test.h +++ b/test/test_systems/drivebrain_controller_test.h @@ -4,16 +4,15 @@ #include #include -auto runTick(DrivebrainController *controller, float last_torque_lim_receive_time_millis, float last_speed_setpoint_receive_time_millis, ControllerMode_e current_control_mode) +auto runTick(DrivebrainController *controller, float last_receive_time_millis, ControllerMode_e current_control_mode, unsigned long systick_millis) { DrivebrainData_s data; - data.last_torque_lim_receive_time_millis = last_torque_lim_receive_time_millis; - data.last_speed_setpoint_receive_time_millis = last_speed_setpoint_receive_time_millis; + data.last_receive_time_millis = last_receive_time_millis; data.torque_limits_nm = {1, 1, 1, 1}; data.speed_setpoints_rpm = {1, 1, 1, 1}; SysTick_s systick; - systick.millis = 1000; + systick.millis = systick_millis; systick.micros = 1000; TorqueControllerMuxStatus status = {}; @@ -24,75 +23,39 @@ auto runTick(DrivebrainController *controller, float last_torque_lim_receive_tim TEST(DrivebrainControllerTesting, signals_sent_within_range) { - DrivebrainController controller(10, 10); - auto torque_controller_output_s = runTick(&controller, 1001, 1009, ControllerMode_e::MODE_4); + DrivebrainController controller(10); + auto torque_controller_output_s = runTick(&controller, 1001, ControllerMode_e::MODE_4, 1002); EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 1); } -TEST(DrivebrainControllerTesting, speed_setpoint_too_latent) +TEST(DrivebrainControllerTesting, setpoint_too_latent) { - DrivebrainController controller(5, 5); - auto torque_controller_output_s = runTick(&controller, 1001, 1006, ControllerMode_e::MODE_4); + DrivebrainController controller(5); + auto torque_controller_output_s = runTick(&controller, 1006, ControllerMode_e::MODE_4, 1012); EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 0); } -TEST(DrivebrainControllerTesting, torque_setpoint_too_latent) -{ - DrivebrainController controller(5, 5); - auto torque_controller_output_s = runTick(&controller, 1006, 1001, ControllerMode_e::MODE_4); - - EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 0); -} - -TEST(DrivebrainControllerTesting, msg_jitter_too_high) -{ - DrivebrainController controller(10, 5); - auto torque_controller_output_s = runTick(&controller, 1001, 1009, ControllerMode_e::MODE_4); - EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 0); -} - -TEST(DrivebrainControllerTesting, speed_setpoint_too_latent_and_torque_setpoint_too_latent) -{ - DrivebrainController controller(5, 10); - auto torque_controller_output_s = runTick(&controller, 1001, 1015, ControllerMode_e::MODE_4); - EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 0); -} - -TEST(DrivebrainControllerTesting, speed_setpoint_too_latent_and_msg_jitter_too_high) -{ - DrivebrainController controller(10, 5); - auto torque_controller_output_s = runTick(&controller, 1011, 1001, ControllerMode_e::MODE_4); - - EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 0); -} - -TEST(DrivebrainControllerTesting, torque_setpoint_too_latent_and_msg_jitter_too_high) +TEST(DrivebrainControllerTesting, failing_stay_failing) { - DrivebrainController controller(10, 5); - auto torque_controller_output_s = runTick(&controller, 1001, 1011, ControllerMode_e::MODE_4); - + DrivebrainController controller(10); + auto torque_controller_output_s = runTick(&controller, 1011, ControllerMode_e::MODE_4, 1032); EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 0); -} -TEST(DrivebrainControllerTesting, failing_stay_failing) -{ - DrivebrainController controller(10, 5); - auto torque_controller_output_s = runTick(&controller, 1001, 1011, ControllerMode_e::MODE_4); + torque_controller_output_s = runTick(&controller, 1033, ControllerMode_e::MODE_4, 1033); EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 0); - torque_controller_output_s = runTick(&controller, 1001, 1001, ControllerMode_e::MODE_4); + torque_controller_output_s = runTick(&controller, 1034, ControllerMode_e::MODE_4, 1034); EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 0); } TEST(DrivebrainControllerTesting, failing_reset_success) { - DrivebrainController controller(10, 5); - auto torque_controller_output_s = runTick(&controller, 1001, 1011, ControllerMode_e::MODE_4); + DrivebrainController controller(10); + auto torque_controller_output_s = runTick(&controller, 1011, ControllerMode_e::MODE_4, 1022); EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 0); - torque_controller_output_s = runTick(&controller, 1001, 1001, ControllerMode_e::MODE_3); - - torque_controller_output_s = runTick(&controller, 1001, 1001, ControllerMode_e::MODE_4); + torque_controller_output_s = runTick(&controller, 1021, ControllerMode_e::MODE_3, 1023); + torque_controller_output_s = runTick(&controller, 1022, ControllerMode_e::MODE_4, 1024); EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 1); } From 53d653ad859d2271e40ac2cef500c2549146b0df Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sat, 21 Sep 2024 11:38:44 -0400 Subject: [PATCH 18/33] making the veh_vec constuctor actually a constructor --- lib/interfaces/include/HytechCANInterface.h | 1 - lib/interfaces/include/LoadCellInterface.h | 13 +++++--- lib/interfaces/src/DrivebrainETHInterface.cpp | 8 ++--- lib/shared_data/include/Utility.h | 30 ++++++++++--------- src/main.cpp | 1 - 5 files changed, 27 insertions(+), 26 deletions(-) diff --git a/lib/interfaces/include/HytechCANInterface.h b/lib/interfaces/include/HytechCANInterface.h index 8f3b4c7e2..b2c2249fa 100644 --- a/lib/interfaces/include/HytechCANInterface.h +++ b/lib/interfaces/include/HytechCANInterface.h @@ -14,7 +14,6 @@ #include "VectornavInterface.h" #include "HT08_CASE_types.h" #include "MessageQueueDefine.h" -#include "DrivebrainInterface.h" /* struct holding interfaces processed by process_ring_buffer() FL = MC1 diff --git a/lib/interfaces/include/LoadCellInterface.h b/lib/interfaces/include/LoadCellInterface.h index 7d5dc4d0c..f0e260f36 100644 --- a/lib/interfaces/include/LoadCellInterface.h +++ b/lib/interfaces/include/LoadCellInterface.h @@ -19,6 +19,14 @@ struct LoadCellInterfaceTick_s class LoadCellInterface { +public: + LoadCellInterface() { + loadCellConversions_ = veh_vec(); + loadCellForcesUnfiltered_ = veh_vec(); + loadCellForcesFiltered_ = veh_vec(); + } + void tick(const LoadCellInterfaceTick_s &intake); + LoadCellInterfaceOutput_s getLoadCellForces(); private: /* FIR filter designed with @@ -50,10 +58,7 @@ class LoadCellInterface veh_vec loadCellForcesUnfiltered_; veh_vec loadCellForcesFiltered_; bool FIRSaturated_ = false; -public: - LoadCellInterface() {} - void tick(const LoadCellInterfaceTick_s &intake); - LoadCellInterfaceOutput_s getLoadCellForces(); + }; #endif \ No newline at end of file diff --git a/lib/interfaces/src/DrivebrainETHInterface.cpp b/lib/interfaces/src/DrivebrainETHInterface.cpp index 4e1f90c66..ef092d2c5 100644 --- a/lib/interfaces/src/DrivebrainETHInterface.cpp +++ b/lib/interfaces/src/DrivebrainETHInterface.cpp @@ -16,13 +16,9 @@ hytech_msgs_MCUOutputData DrivebrainETHInterface::make_db_msg(const SharedCarSta void DrivebrainETHInterface::receive_pb_msg(const hytech_msgs_MCUCommandData &msg_in, unsigned long curr_millis) { - // Serial.println("msg recvd in here"); - veh_vec nm_lim; - nm_lim.construct(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 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 speed_set; - speed_set.construct(msg_in.desired_rpms.FL, msg_in.desired_rpms.FR, msg_in.desired_rpms.RL, msg_in.desired_rpms.RR); - // Serial.println(msg_in.desired_rpms.FL); + veh_vec 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; diff --git a/lib/shared_data/include/Utility.h b/lib/shared_data/include/Utility.h index 66d0f2cb5..d528a73f6 100644 --- a/lib/shared_data/include/Utility.h +++ b/lib/shared_data/include/Utility.h @@ -3,11 +3,11 @@ #include "PhysicalParameters.h" #include // Defines -const int FL = 0; -const int FR = 1; -const int RL = 2; -const int RR = 3; -const int NUM_MOTORS = 4; +const int FL = 0; +const int FR = 1; +const int RL = 2; +const int RR = 3; +const int NUM_MOTORS = 4; const int DEBOUNCE_MILLIS = 10; // milliseconds before registering another button press @@ -15,28 +15,30 @@ const int DEBOUNCE_MILLIS = 10; // milliseconds before registering another butto template struct veh_vec { +public: T FL; T FR; T RL; T RR; - void construct(T _FL, T _FR, T _RL, T _RR) +public: + veh_vec() = default; + veh_vec(T _FL, T _FR, T _RL, T _RR) { FL = _FL; FR = _FR; RL = _RL; RR = _RR; } - - /// @brief copy values to array in FL, FR, RL, RR order - void copy_to_arr(T(&arr_out)[4]) + + /// @brief copy values to array in FL, FR, RL, RR order + void copy_to_arr(T (&arr_out)[4]) { - arr_out[0]=FL; - arr_out[1]=FR; - arr_out[2]=RL; - arr_out[3]=RR; + arr_out[0] = FL; + arr_out[1] = FR; + arr_out[2] = RL; + arr_out[3] = RR; } - }; template diff --git a/src/main.cpp b/src/main.cpp index 2f95dd72d..97dff3083 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -24,7 +24,6 @@ #include "SABInterface.h" #include "VectornavInterface.h" #include "LoadCellInterface.h" -#include "DrivebrainInterface.h" #include "TorqueControllers.h" /* Systems */ From d857f814aa7c4d8f904fa8fd553aee108663a944 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sat, 21 Sep 2024 11:42:24 -0400 Subject: [PATCH 19/33] addressing comments --- lib/state_machine/include/MCUStateMachine.h | 1 - lib/state_machine/include/MCUStateMachine.tpp | 45 ------- lib/systems/include/CASESystem.tpp | 118 +++++++++--------- lib/systems/src/DrivebrainController.cpp | 1 - src/main.cpp | 2 + 5 files changed, 61 insertions(+), 106 deletions(-) diff --git a/lib/state_machine/include/MCUStateMachine.h b/lib/state_machine/include/MCUStateMachine.h index c92ad3812..791511dbe 100644 --- a/lib/state_machine/include/MCUStateMachine.h +++ b/lib/state_machine/include/MCUStateMachine.h @@ -73,7 +73,6 @@ class MCUStateMachine // IMDInterface *imd_; SafetySystem *safety_system_; TCMuxType *controller_mux_; - // RateLimitedLogger logger_; }; #include "MCUStateMachine.tpp" #endif /* MCUSTATEMACHINE */ diff --git a/lib/state_machine/include/MCUStateMachine.tpp b/lib/state_machine/include/MCUStateMachine.tpp index aa2ed32cc..8958c7b09 100644 --- a/lib/state_machine/include/MCUStateMachine.tpp +++ b/lib/state_machine/include/MCUStateMachine.tpp @@ -13,41 +13,6 @@ void MCUStateMachine::tick_state_machine(unsigned long curren case CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE: { - // Serial.println("eror in ts not active:"); - // Serial.println(safety_system_->get_software_is_ok()); - // Serial.println(); - // hal_println("tractive system not active state"); - - // auto data = pedals_->getPedalsSystemData(); - // auto mux_test = controller_mux_->getDrivetrainCommand(); - - // hal_println("speeds 1 through 4"); - // Serial.println(mux_test.speeds_rpm[0]); - // Serial.println(mux_test.speeds_rpm[1]); - // Serial.println(mux_test.speeds_rpm[2]); - // Serial.println(mux_test.speeds_rpm[3]); - - // hal_println("torqeus"); - // Serial.println(mux_test.torqueSetpoints[0]); - // Serial.println(mux_test.torqueSetpoints[1]); - // Serial.println(mux_test.torqueSetpoints[2]); - // Serial.println(mux_test.torqueSetpoints[3]); - // Serial.println(); - // Serial.print(data.brakeImplausible); - // Serial.print(" "); - // Serial.print(data.accelImplausible); - // Serial.print(" "); - // Serial.print(data.brakeAndAccelPressedImplausibility); - // Serial.print(" "); - - // Serial.println("accel, brake:"); - // Serial.print(data.accelPercent); - // Serial.print(" "); - // Serial.print(data.brakePercent); - // Serial.print(" \n"); - // Serial.print(data.implausibilityExceededMaxDuration); - // Serial.println(); - // if TS is above HV threshold, move to Tractive System Active drivetrain_->disable_no_pins(); if (drivetrain_->hv_over_threshold_on_drivetrain()) @@ -63,7 +28,6 @@ void MCUStateMachine::tick_state_machine(unsigned long curren { buzzer_->deactivate(); } - // hal_println("in tractive system active state"); // TODO migrate to new pedals system auto data = pedals_->getPedalsSystemData(); drivetrain_->disable_no_pins(); @@ -83,7 +47,6 @@ void MCUStateMachine::tick_state_machine(unsigned long curren case CAR_STATE::ENABLING_INVERTERS: { - // hal_println("in enabling inverters state"); if (!drivetrain_->hv_over_threshold_on_drivetrain()) { set_state_(CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE, current_millis); @@ -144,8 +107,6 @@ void MCUStateMachine::tick_state_machine(unsigned long curren if (safety_system_->get_software_is_ok() && !data.implausibilityExceededMaxDuration) { - // logger_.log_out("torque mode:", current_millis, 100); - // logger_.log_out(static_cast(dashboard_->getTorqueLimitMode()), current_millis, 100); drivetrain_->command_drivetrain(controller_mux_->getDrivetrainCommand(dashboard_->getDialMode(), dashboard_->getTorqueLimitMode(), current_car_state)); } else @@ -161,14 +122,11 @@ void MCUStateMachine::tick_state_machine(unsigned long curren template void MCUStateMachine::set_state_(CAR_STATE new_state, unsigned long curr_time) { - // hal_println("running exit logic"); - // hal_printf("\n to state: %i\n", new_state); handle_exit_logic_(current_state_, curr_time); current_state_ = new_state; - // hal_println("running entry logic"); handle_entry_logic_(new_state, curr_time); } @@ -215,13 +173,10 @@ void MCUStateMachine::handle_entry_logic_(CAR_STATE new_state { // make dashboard sound buzzer buzzer_->activate_buzzer(curr_time); - // hal_println("RTDS enabled"); break; } case CAR_STATE::READY_TO_DRIVE: { - // hal_printf("%i\n", curr_time); - // hal_println("Ready to drive"); break; } } diff --git a/lib/systems/include/CASESystem.tpp b/lib/systems/include/CASESystem.tpp index f8488d29c..1787cbb12 100644 --- a/lib/systems/include/CASESystem.tpp +++ b/lib/systems/include/CASESystem.tpp @@ -200,71 +200,71 @@ DrivetrainCommand_s CASESystem::evaluate( // send these out at the send period - // if ((tick.millis - last_controller_pt1_send_time_) >= (controller_send_period_ms_)) - // { - // enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_initia); - // enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_norm_p); - // enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_pow_pn); - // enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_to); - // enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_regen_); - // enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_pid__p); - - // last_controller_pt1_send_time_ = tick.millis; - // } + if ((tick.millis - last_controller_pt1_send_time_) >= (controller_send_period_ms_)) + { + enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_initia); + enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_norm_p); + enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_pow_pn); + enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_to); + enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_regen_); + enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_pid__p); + + last_controller_pt1_send_time_ = tick.millis; + } - // if (((tick.millis - last_controller_pt1_send_time_) >= (vehicle_math_offset_ms_ / 3)) && - // ((tick.millis - last_controller_pt2_send_time_) > controller_send_period_ms_)) - // { - // enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_pid_ya); - // enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_normal); - // enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_pi); - // enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs__p); - // enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_st); - // enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_pn); - - // last_controller_pt2_send_time_ = tick.millis; - // } + if (((tick.millis - last_controller_pt1_send_time_) >= (vehicle_math_offset_ms_ / 3)) && + ((tick.millis - last_controller_pt2_send_time_) > controller_send_period_ms_)) + { + enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_pid_ya); + enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_normal); + enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_pi); + enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs__p); + enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_st); + enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_pn); + + last_controller_pt2_send_time_ = tick.millis; + } - // if (((tick.millis - last_controller_pt2_send_time_) >= (vehicle_math_offset_ms_ / 3)) && - // ((tick.millis - last_controller_pt3_send_time_) > controller_send_period_ms_)) - // { + if (((tick.millis - last_controller_pt2_send_time_) >= (vehicle_math_offset_ms_ / 3)) && + ((tick.millis - last_controller_pt3_send_time_) > controller_send_period_ms_)) + { - // enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_sl); - // enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_rege_p); - // enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_torque); - // enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_power_); - // enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_powe_p); + enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_sl); + enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_rege_p); + enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_torque); + enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_power_); + enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_powe_p); - // last_controller_pt3_send_time_ = tick.millis; - // } + last_controller_pt3_send_time_ = tick.millis; + } - // if (((tick.millis - last_controller_pt1_send_time_) >= vehicle_math_offset_ms_) && - // ((tick.millis - last_vehm_send_time_) > controller_send_period_ms_)) - // { - // enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_alpha_deg); - // enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_sl); - // enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_long_corner_); - // enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_wheel_steer_); - // enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_kin_desired_); - // enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_beta_deg); - // enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_wheel_lin_ve); - - // last_vehm_send_time_ = tick.millis; - // } + if (((tick.millis - last_controller_pt1_send_time_) >= vehicle_math_offset_ms_) && + ((tick.millis - last_vehm_send_time_) > controller_send_period_ms_)) + { + enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_alpha_deg); + enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_sl); + enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_long_corner_); + enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_wheel_steer_); + enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_kin_desired_); + enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_beta_deg); + enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_wheel_lin_ve); + + last_vehm_send_time_ = tick.millis; + } - // if ((tick.millis - last_lowest_priority_controller_send_time_) >= (lowest_priority_controller_send_period_ms_)) - // { - // enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_boolea); - // enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_co); - // enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_yaw_pi); - // enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_sa); - // enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_di); - // enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_rp); - // enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_nl); - // enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tc_pna); - - // last_lowest_priority_controller_send_time_ = tick.millis; - // } + if ((tick.millis - last_lowest_priority_controller_send_time_) >= (lowest_priority_controller_send_period_ms_)) + { + enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_boolea); + enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_co); + enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_yaw_pi); + enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_sa); + enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_di); + enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_rp); + enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_nl); + enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tc_pna); + + last_lowest_priority_controller_send_time_ = tick.millis; + } DrivetrainCommand_s command; diff --git a/lib/systems/src/DrivebrainController.cpp b/lib/systems/src/DrivebrainController.cpp index c4c79441d..6b1884fc0 100644 --- a/lib/systems/src/DrivebrainController.cpp +++ b/lib/systems/src/DrivebrainController.cpp @@ -1,5 +1,4 @@ #include "DrivebrainController.h" -#include // for std::copy TorqueControllerOutput_s DrivebrainController::evaluate(const SharedCarState_s &state) diff --git a/src/main.cpp b/src/main.cpp index 97dff3083..9b294833b 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -568,6 +568,7 @@ void tick_all_interfaces(const SysTick_s ¤t_system_tick) PedalsSystemData_s data2 = pedals_system.getPedalsSystemDataCopy(); front_thermistors_interface.tick(mcu_adc.get().conversions[MCU15_THERM_FL_CHANNEL], mcu_adc.get().conversions[MCU15_THERM_FR_CHANNEL]); + // TODO pass in the shared state instead telem_interface.tick( a1.get(), a2.get(), @@ -587,6 +588,7 @@ 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().current_error); + ams_interface.tick(current_system_tick); } From 03ccfbb5576aed358b9f61910d7e553b13a6811e Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sat, 21 Sep 2024 11:44:56 -0400 Subject: [PATCH 20/33] a little cleanup --- src/main.cpp | 37 ------------------------------------- 1 file changed, 37 deletions(-) diff --git a/src/main.cpp b/src/main.cpp index 9b294833b..51129cc56 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -631,46 +631,12 @@ void tick_all_systems(const SysTick_s ¤t_system_tick) a1.get().conversions[MCU15_BRAKE1_CHANNEL], a1.get().conversions[MCU15_BRAKE2_CHANNEL]); - // accel 1 only accel 2 dead, brake normal - // pedals_system.tick( - // current_system_tick, - // a1.get().conversions[MCU15_ACCEL1_CHANNEL], - // a1.get().conversions[MCU15_ACCEL1_CHANNEL], - // a1.get().conversions[MCU15_BRAKE1_CHANNEL], - // a1.get().conversions[MCU15_BRAKE2_CHANNEL]); - // accel 2 only accel 1 dead, brake normal - // pedals_system.tick( - // current_system_tick, - // a1.get().conversions[MCU15_ACCEL2_CHANNEL], - // a1.get().conversions[MCU15_ACCEL2_CHANNEL], - // a1.get().conversions[MCU15_BRAKE1_CHANNEL], - // a1.get().conversions[MCU15_BRAKE2_CHANNEL]); - // brake 1 only brake 2 dead, accel normal - // pedals_system.tick( - // current_system_tick, - // a1.get().conversions[MCU15_ACCEL1_CHANNEL], - // a1.get().conversions[MCU15_ACCEL2_CHANNEL], - // a1.get().conversions[MCU15_BRAKE1_CHANNEL], - // a1.get().conversions[MCU15_BRAKE1_CHANNEL]); - // brake 2 only brake 1 dead, accel normal - // pedals_system.tick( - // current_system_tick, - // a1.get().conversions[MCU15_ACCEL1_CHANNEL], - // a1.get().conversions[MCU15_ACCEL2_CHANNEL], - // a1.get().conversions[MCU15_BRAKE2_CHANNEL], - // a1.get().conversions[MCU15_BRAKE2_CHANNEL]); - // tick steering system steering_system.tick( (SteeringSystemTick_s){ .tick = current_system_tick, .secondaryConversion = a1.get().conversions[MCU15_STEERING_CHANNEL]}); - // Serial.println("Steering angle"); - // Serial.println(steering_system.getSteeringSystemData().angle); - // Serial.println("Steering status"); - // Serial.println(static_cast(steering_system.getSteeringSystemData().status)); - // tick drivetrain system drivetrain.tick(current_system_tick); // // tick torque controller mux @@ -693,9 +659,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"); - 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); From 19650a3ecdf17ec84fc3783f08e9b4f163ecbd16 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sun, 29 Sep 2024 14:51:04 -0400 Subject: [PATCH 21/33] added compiler macro flag for debug prints --- platformio.ini | 1 + src/main.cpp | 8 +++++++- 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/platformio.ini b/platformio.ini index 1d508c33c..b0e4d2444 100644 --- a/platformio.ini +++ b/platformio.ini @@ -43,6 +43,7 @@ build_unflags = -std=gnu++11 build_flags = -std=c++17 -D TEENSY_OPT_SMALLEST_CODE + ; -D ENABLE_DEBUG_PRINTS check_tool = clangtidy check_src_filters = + diff --git a/src/main.cpp b/src/main.cpp index 51129cc56..c087dd20f 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,3 +1,9 @@ +#ifdef ENABLE_DEBUG_PRINTS +#define DEBUG_PRINTS true +#else +#define DEBUG_PRINTS false +#endif + /* Include files */ /* System Includes*/ #include @@ -461,7 +467,7 @@ void loop() // Basic debug prints // if (curr_tick.triggers.trigger5) - if (false) + if (DEBUG_PRINTS) { Serial.print("Steering system reported angle (deg): "); Serial.println(steering_system.getSteeringSystemData().angle); From f858024b940a3c3bc663283a381c1e7a0d9f0528 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sun, 29 Sep 2024 15:05:16 -0400 Subject: [PATCH 22/33] added in type alias --- lib/shared_data/include/SharedDataTypes.h | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/lib/shared_data/include/SharedDataTypes.h b/lib/shared_data/include/SharedDataTypes.h index 92ce0ab05..60f80757c 100644 --- a/lib/shared_data/include/SharedDataTypes.h +++ b/lib/shared_data/include/SharedDataTypes.h @@ -6,6 +6,9 @@ #include "SharedFirmwareTypes.h" #include "DrivebrainData.h" +using speed_rpm = float; +using torque_nm = float; + enum class TorqueLimit_e { TCMUX_FULL_TORQUE = 0, @@ -45,8 +48,8 @@ struct PedalsSystemData_s struct DrivetrainDynamicReport_s { uint16_t measuredInverterFLPackVoltage; - float measuredSpeeds[NUM_MOTORS]; // rpm - float measuredTorques[NUM_MOTORS]; + speed_rpm measuredSpeeds[NUM_MOTORS]; // rpm + torque_nm measuredTorques[NUM_MOTORS]; float measuredTorqueCurrents[NUM_MOTORS]; float measuredMagnetizingCurrents[NUM_MOTORS]; }; From c0201efcc28b0e91a885f71af60d2b58f4c68db4 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Thu, 3 Oct 2024 21:33:09 -0400 Subject: [PATCH 23/33] adding in better handling of timing for drivebrain message latency --- .../include/DrivebrainETHInterface.h | 6 +++++- lib/interfaces/src/DrivebrainETHInterface.cpp | 7 ++++++- lib/shared_data/include/DrivebrainData.h | 5 ++++- lib/systems/src/DrivebrainController.cpp | 14 ++++++++++++-- platformio.ini | 2 +- .../test_systems/drivebrain_controller_test.h | 19 ++++++++++--------- 6 files changed, 38 insertions(+), 15 deletions(-) diff --git a/lib/interfaces/include/DrivebrainETHInterface.h b/lib/interfaces/include/DrivebrainETHInterface.h index cc7e1c034..e3a3a0ee2 100644 --- a/lib/interfaces/include/DrivebrainETHInterface.h +++ b/lib/interfaces/include/DrivebrainETHInterface.h @@ -6,7 +6,10 @@ class DrivebrainETHInterface { public: - DrivebrainETHInterface() = default; + DrivebrainETHInterface(){ + _latest_data.last_receive_time_millis = -1; + _latest_data.DB_prev_MCU_recv_millis = -1; + }; void receive_pb_msg(const hytech_msgs_MCUCommandData &msg_in, unsigned long curr_millis); @@ -14,6 +17,7 @@ class DrivebrainETHInterface DrivebrainData_s get_latest_data() { return _latest_data; } private: + DrivebrainData_s _latest_data = {}; }; diff --git a/lib/interfaces/src/DrivebrainETHInterface.cpp b/lib/interfaces/src/DrivebrainETHInterface.cpp index ef092d2c5..29aadf493 100644 --- a/lib/interfaces/src/DrivebrainETHInterface.cpp +++ b/lib/interfaces/src/DrivebrainETHInterface.cpp @@ -11,6 +11,9 @@ hytech_msgs_MCUOutputData DrivebrainETHInterface::make_db_msg(const SharedCarSta shared_state.drivetrain_data.measuredSpeeds[1], shared_state.drivetrain_data.measuredSpeeds[2], 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; return out; } @@ -22,5 +25,7 @@ void DrivebrainETHInterface::receive_pb_msg(const hytech_msgs_MCUCommandData &ms _latest_data.torque_limits_nm = nm_lim; _latest_data.speed_setpoints_rpm = speed_set; - _latest_data.last_receive_time_millis = curr_millis; + _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/shared_data/include/DrivebrainData.h b/lib/shared_data/include/DrivebrainData.h index d8f70a0ad..35cada9bf 100644 --- a/lib/shared_data/include/DrivebrainData.h +++ b/lib/shared_data/include/DrivebrainData.h @@ -5,7 +5,10 @@ struct DrivebrainData_s { - unsigned long last_receive_time_millis; + /// @brief the latest time that the MCU received a message w.r.t the current tick's millis + int64_t last_receive_time_millis = -1; + /// @brief the latest MCU last_receive_time_millis that the drivebrain received + int64_t DB_prev_MCU_recv_millis = -1; veh_vec torque_limits_nm; veh_vec speed_setpoints_rpm; }; diff --git a/lib/systems/src/DrivebrainController.cpp b/lib/systems/src/DrivebrainController.cpp index 6b1884fc0..91d0e10e8 100644 --- a/lib/systems/src/DrivebrainController.cpp +++ b/lib/systems/src/DrivebrainController.cpp @@ -7,10 +7,20 @@ TorqueControllerOutput_s DrivebrainController::evaluate(const SharedCarState_s & auto sys_tick = state.systick; auto db_input = state.db_data; - bool message_too_latent = (::abs((int)(sys_tick.millis - db_input.last_receive_time_millis)) > (int)_params.allowed_latency); + // cases for timing_failure: + // 1 if the last_receive_time_millis < 0, then we have not received any messages from the db (initialized at -1 in constructor) + bool no_messages_received = db_input.last_receive_time_millis < 0; + + // 2 if the DB_prev_MCU_recv_millis < 0, then the drivebrain has not received a time from the MCU + // (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); + + // 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); - bool timing_failure = (message_too_latent); + bool timing_failure = (message_too_latent || no_messages_received || drivebrain_has_not_received_time); // only in the case that our speed is low enough (<1 m/s) do we want to clear the fault diff --git a/platformio.ini b/platformio.ini index b0e4d2444..3e6a35567 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-09-17T03_08_13/hytech_msgs_pb_lib.tar.gz + https://github.com/hytech-racing/HT_proto/releases/download/2024-10-04T01_14_21/hytech_msgs_pb_lib.tar.gz [env:test_env] platform = native diff --git a/test/test_systems/drivebrain_controller_test.h b/test/test_systems/drivebrain_controller_test.h index f2a2095ed..c90af8628 100644 --- a/test/test_systems/drivebrain_controller_test.h +++ b/test/test_systems/drivebrain_controller_test.h @@ -4,10 +4,11 @@ #include #include -auto runTick(DrivebrainController *controller, float last_receive_time_millis, ControllerMode_e current_control_mode, unsigned long systick_millis) +auto runTick(DrivebrainController *controller, float last_DB_receive_time_millis, float last_receive_time_millis, ControllerMode_e current_control_mode, unsigned long systick_millis) { DrivebrainData_s data; data.last_receive_time_millis = last_receive_time_millis; + data.DB_prev_MCU_recv_millis = last_DB_receive_time_millis; data.torque_limits_nm = {1, 1, 1, 1}; data.speed_setpoints_rpm = {1, 1, 1, 1}; @@ -24,38 +25,38 @@ auto runTick(DrivebrainController *controller, float last_receive_time_millis, C TEST(DrivebrainControllerTesting, signals_sent_within_range) { DrivebrainController controller(10); - auto torque_controller_output_s = runTick(&controller, 1001, ControllerMode_e::MODE_4, 1002); + auto torque_controller_output_s = runTick(&controller, 998, 1001, ControllerMode_e::MODE_4, 1002); EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 1); } TEST(DrivebrainControllerTesting, setpoint_too_latent) { DrivebrainController controller(5); - auto torque_controller_output_s = runTick(&controller, 1006, ControllerMode_e::MODE_4, 1012); + auto torque_controller_output_s = runTick(&controller, 800, 1006, ControllerMode_e::MODE_4, 1012); EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 0); } TEST(DrivebrainControllerTesting, failing_stay_failing) { DrivebrainController controller(10); - auto torque_controller_output_s = runTick(&controller, 1011, ControllerMode_e::MODE_4, 1032); + auto torque_controller_output_s = runTick(&controller, 200, 1011, ControllerMode_e::MODE_4, 1032); EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 0); - torque_controller_output_s = runTick(&controller, 1033, ControllerMode_e::MODE_4, 1033); + torque_controller_output_s = runTick(&controller, 200, 1033, ControllerMode_e::MODE_4, 1033); EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 0); - torque_controller_output_s = runTick(&controller, 1034, ControllerMode_e::MODE_4, 1034); + torque_controller_output_s = runTick(&controller, 400, 1034, ControllerMode_e::MODE_4, 1034); EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 0); } TEST(DrivebrainControllerTesting, failing_reset_success) { DrivebrainController controller(10); - auto torque_controller_output_s = runTick(&controller, 1011, ControllerMode_e::MODE_4, 1022); + auto torque_controller_output_s = runTick(&controller, 300, 1011, ControllerMode_e::MODE_4, 1022); EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 0); - torque_controller_output_s = runTick(&controller, 1021, ControllerMode_e::MODE_3, 1023); - torque_controller_output_s = runTick(&controller, 1022, ControllerMode_e::MODE_4, 1024); + torque_controller_output_s = runTick(&controller, 1020, 1021, ControllerMode_e::MODE_3, 1023); + torque_controller_output_s = runTick(&controller, 1022, 1022, ControllerMode_e::MODE_4, 1024); EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 1); } From 59fd58614d2c472d7f1d89f7c96497937b74c89b Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Thu, 3 Oct 2024 23:00:55 -0400 Subject: [PATCH 24/33] updated to latest HT-prot --- platformio.ini | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/platformio.ini b/platformio.ini index 3e6a35567..f7a98a088 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-04T01_14_21/hytech_msgs_pb_lib.tar.gz + https://github.com/hytech-racing/HT_proto/releases/download/2024-10-04T01_43_38/hytech_msgs_pb_lib.tar.gz [env:test_env] platform = native From 2c789725dd0cd3bc2a2aa5dcd7474e4db28011aa Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Fri, 4 Oct 2024 19:12:44 -0400 Subject: [PATCH 25/33] updating pedal params and to latest HT_proto --- include/MCU_rev15_defs.h | 10 +++++----- platformio.ini | 2 +- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/include/MCU_rev15_defs.h b/include/MCU_rev15_defs.h index a11ee99dc..b651397eb 100644 --- a/include/MCU_rev15_defs.h +++ b/include/MCU_rev15_defs.h @@ -52,14 +52,14 @@ const unsigned long TELEM_CAN_BAUDRATE = 500000; // All of these values are the PEDAL min/max // the sensor min/max that trip implaus are calculated // in the PedalsSystem constructor -const int ACCEL1_PEDAL_MAX = 3491; +const int ACCEL1_PEDAL_MAX = 3490; const int ACCEL2_PEDAL_MAX = 189; -const int ACCEL1_PEDAL_MIN = 2244; -const int ACCEL2_PEDAL_MIN = 1405; +const int ACCEL1_PEDAL_MIN = 2646; +const int ACCEL2_PEDAL_MIN = 1030; -const int BRAKE1_PEDAL_MAX = 1945; -const int BRAKE2_PEDAL_MAX = 1742; +const int BRAKE1_PEDAL_MAX = 2022; +const int BRAKE2_PEDAL_MAX = 1668; const int BRAKE1_PEDAL_MIN = 1192; const int BRAKE2_PEDAL_MIN = 2476; diff --git a/platformio.ini b/platformio.ini index f7a98a088..4f2a8d8d9 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-04T01_43_38/hytech_msgs_pb_lib.tar.gz + https://github.com/hytech-racing/HT_proto/releases/download/2024-10-04T04_27_55/hytech_msgs_pb_lib.tar.gz [env:test_env] platform = native From a78510726e02aaa30c366b60a09fb86da5ba7d40 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Thu, 10 Oct 2024 14:50:03 -0400 Subject: [PATCH 26/33] added in loadcell data to the output message and added in more metrics for drivebrain comms --- lib/interfaces/include/LoadCellInterface.h | 3 ++ lib/interfaces/src/DrivebrainETHInterface.cpp | 20 ++++++++----- lib/interfaces/src/LoadCellInterface.cpp | 11 ++++++-- lib/shared_data/include/SharedDataTypes.h | 28 +++++++++++++++++++ lib/systems/include/DrivebrainController.h | 4 +++ lib/systems/src/DrivebrainController.cpp | 22 +++++++++++++-- platformio.ini | 2 +- src/main.cpp | 17 ++++++----- 8 files changed, 86 insertions(+), 21 deletions(-) diff --git a/lib/interfaces/include/LoadCellInterface.h b/lib/interfaces/include/LoadCellInterface.h index f0e260f36..5cfd37947 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 29aadf493..3626541c1 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 b6aa570f3..b3a032ee0 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 60f80757c..da99b8baf 100644 --- 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 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 index 7eb7b6e48..2f81eb2ca 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_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 index 91d0e10e8..26d0deb6f 100644 --- a/lib/systems/src/DrivebrainController.cpp +++ b/lib/systems/src/DrivebrainController.cpp @@ -1,5 +1,5 @@ #include "DrivebrainController.h" - +// #include 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 index 4f2a8d8d9..f3d3ab21c 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-07T23_13_48/hytech_msgs_pb_lib.tar.gz [env:test_env] platform = native diff --git a/src/main.cpp b/src/main.cpp index c087dd20f..860ebcd2c 100644 --- 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 ¤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 @@ -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 ¤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() @@ -658,10 +662,9 @@ 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. @@ -669,7 +672,7 @@ void handle_ethernet_interface_comms(const SysTick_s& systick, const hytech_msgs 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.trigger100) { handle_ethernet_socket_send_pb(EthParams::default_TCU_ip, EthParams::default_protobuf_send_port, &protobuf_send_socket, out_msg, hytech_msgs_MCUOutputData_fields); } From 75dd8cd4c91d1139c4661a9bd303d8dd86837d2d Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Thu, 10 Oct 2024 15:49:23 -0400 Subject: [PATCH 27/33] added in ability to control the car even when failing drivebrain timing --- lib/interfaces/src/DrivebrainETHInterface.cpp | 2 +- lib/systems/include/DrivebrainController.h | 8 +- lib/systems/src/DrivebrainController.cpp | 4 +- lib/systems/src/SimpleController.cpp | 1 - .../test_systems/drivebrain_controller_test.h | 101 ++++++++++-------- 5 files changed, 65 insertions(+), 51 deletions(-) diff --git a/lib/interfaces/src/DrivebrainETHInterface.cpp b/lib/interfaces/src/DrivebrainETHInterface.cpp index 3626541c1..c65b952fd 100644 --- a/lib/interfaces/src/DrivebrainETHInterface.cpp +++ b/lib/interfaces/src/DrivebrainETHInterface.cpp @@ -6,7 +6,7 @@ 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]; diff --git a/lib/systems/include/DrivebrainController.h b/lib/systems/include/DrivebrainController.h index 2f81eb2ca..77831c1f3 100644 --- a/lib/systems/include/DrivebrainController.h +++ b/lib/systems/include/DrivebrainController.h @@ -1,12 +1,12 @@ #ifndef __DRIVEBRAINCONTROLLER_H__ #define __DRIVEBRAINCONTROLLER_H__ - -#include "TorqueControllers.h" +#include +// #include "TorqueControllers.h" #include "DrivebrainData.h" #include "BaseController.h" #include "SharedDataTypes.h" - +#include // 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 @@ -26,6 +26,7 @@ class DrivebrainController : public Controller 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_rec_time =0; _worst_latency_so_far = -1; @@ -46,6 +47,7 @@ class DrivebrainController : public Controller int64_t _worst_latency_so_far; bool _timing_failure = false; unsigned long _last_setpoint_millis = -1; + TorqueControllerSimple _emergency_control; }; #endif // __DRIVEBRAINCONTROLLER_H__ \ No newline at end of file diff --git a/lib/systems/src/DrivebrainController.cpp b/lib/systems/src/DrivebrainController.cpp index 26d0deb6f..ec3c040c4 100644 --- a/lib/systems/src/DrivebrainController.cpp +++ b/lib/systems/src/DrivebrainController.cpp @@ -1,6 +1,5 @@ #include "DrivebrainController.h" // #include - TorqueControllerOutput_s DrivebrainController::evaluate(const SharedCarState_s &state) { @@ -59,7 +58,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; } \ No newline at end of file diff --git a/lib/systems/src/SimpleController.cpp b/lib/systems/src/SimpleController.cpp index 08e308680..f068ae87d 100644 --- a/lib/systems/src/SimpleController.cpp +++ b/lib/systems/src/SimpleController.cpp @@ -1,5 +1,4 @@ #include "SimpleController.h" - void TorqueControllerSimple::tick(const PedalsSystemData_s &pedalsData) { diff --git a/test/test_systems/drivebrain_controller_test.h b/test/test_systems/drivebrain_controller_test.h index c90af8628..4a7019253 100644 --- a/test/test_systems/drivebrain_controller_test.h +++ b/test/test_systems/drivebrain_controller_test.h @@ -1,63 +1,76 @@ #ifndef DRIVEBRAIN_CONTROLLER_TEST #define DRIVEBRAIN_CONTROLLER_TEST -#include #include #include +#include + +auto runTick(DrivebrainController *controller, + float last_DB_receive_time_millis, float last_receive_time_millis, + ControllerMode_e current_control_mode, + unsigned long systick_millis, float brakePercent, + float accelPercent) { + DrivebrainData_s data; + data.last_receive_time_millis = last_receive_time_millis; + data.DB_prev_MCU_recv_millis = last_DB_receive_time_millis; + data.torque_limits_nm = {1, 1, 1, 1}; + data.speed_setpoints_rpm = {1, 1, 1, 1}; + + SysTick_s systick; + systick.millis = systick_millis; + systick.micros = 1000; -auto runTick(DrivebrainController *controller, float last_DB_receive_time_millis, float last_receive_time_millis, ControllerMode_e current_control_mode, unsigned long systick_millis) -{ - DrivebrainData_s data; - data.last_receive_time_millis = last_receive_time_millis; - data.DB_prev_MCU_recv_millis = last_DB_receive_time_millis; - data.torque_limits_nm = {1, 1, 1, 1}; - data.speed_setpoints_rpm = {1, 1, 1, 1}; - - SysTick_s systick; - systick.millis = systick_millis; - systick.micros = 1000; - - TorqueControllerMuxStatus status = {}; - status.current_controller_mode_ = current_control_mode; - SharedCarState_s state(systick, {}, {}, {}, {}, {}, data, status); - return controller->evaluate(state); + TorqueControllerMuxStatus status = {}; + status.current_controller_mode_ = current_control_mode; + PedalsSystemData_s pedals_data = {}; + pedals_data.regenPercent = brakePercent; + pedals_data.accelPercent = accelPercent; + SharedCarState_s state(systick, {/*steering data*/}, {/*drivetrain_data*/}, + {/*loadcell_data*/}, {/*raw_loadcell_data*/}, + pedals_data, {}, data, status); + return controller->evaluate(state); } -TEST(DrivebrainControllerTesting, signals_sent_within_range) -{ - DrivebrainController controller(10); - auto torque_controller_output_s = runTick(&controller, 998, 1001, ControllerMode_e::MODE_4, 1002); - EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 1); +TEST(DrivebrainControllerTesting, signals_sent_within_range) { + DrivebrainController controller(10); + auto torque_controller_output_s = + runTick(&controller, 998, 1001, ControllerMode_e::MODE_4, 1002, 0.0f, 0.0f); + EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 1); } -TEST(DrivebrainControllerTesting, setpoint_too_latent) -{ - DrivebrainController controller(5); - auto torque_controller_output_s = runTick(&controller, 800, 1006, ControllerMode_e::MODE_4, 1012); - EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 0); +TEST(DrivebrainControllerTesting, setpoint_too_latent_still_in_control) { + DrivebrainController controller(5); + auto torque_controller_output_s = runTick( + &controller, 800, 1006, ControllerMode_e::MODE_4, 1012, 1.0f, 0.0f); + EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 0); + EXPECT_FLOAT_EQ(torque_controller_output_s.command.torqueSetpoints[0], PhysicalParameters::MAX_REGEN_TORQUE); } -TEST(DrivebrainControllerTesting, failing_stay_failing) -{ - DrivebrainController controller(10); - auto torque_controller_output_s = runTick(&controller, 200, 1011, ControllerMode_e::MODE_4, 1032); - EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 0); +TEST(DrivebrainControllerTesting, failing_stay_failing) { + DrivebrainController controller(10); + auto torque_controller_output_s = runTick( + &controller, 200, 1011, ControllerMode_e::MODE_4, 1032, 0.01f, 0.0f); + EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 0); - torque_controller_output_s = runTick(&controller, 200, 1033, ControllerMode_e::MODE_4, 1033); - EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 0); + torque_controller_output_s = runTick( + &controller, 200, 1033, ControllerMode_e::MODE_4, 1033, 0.01f, 0.0f); + EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 0); - torque_controller_output_s = runTick(&controller, 400, 1034, ControllerMode_e::MODE_4, 1034); - EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 0); + torque_controller_output_s = runTick( + &controller, 400, 1034, ControllerMode_e::MODE_4, 1034, 0.01f, 0.0f); + EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 0); } -TEST(DrivebrainControllerTesting, failing_reset_success) -{ - DrivebrainController controller(10); - auto torque_controller_output_s = runTick(&controller, 300, 1011, ControllerMode_e::MODE_4, 1022); - EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 0); +TEST(DrivebrainControllerTesting, failing_reset_success) { + DrivebrainController controller(10); + auto torque_controller_output_s = + runTick(&controller, 300, 1011, ControllerMode_e::MODE_4, 1022, 0.01f, 0.0f); + EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 0); - torque_controller_output_s = runTick(&controller, 1020, 1021, ControllerMode_e::MODE_3, 1023); - torque_controller_output_s = runTick(&controller, 1022, 1022, ControllerMode_e::MODE_4, 1024); - EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 1); + torque_controller_output_s = + runTick(&controller, 1020, 1021, ControllerMode_e::MODE_3, 1023, 0.01f, 0); + torque_controller_output_s = + runTick(&controller, 1022, 1022, ControllerMode_e::MODE_4, 1024, 0.01f, 0); + EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 1); } #endif \ No newline at end of file From e30fa72a63a810d35a7dd975e93c854927883dba Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Thu, 10 Oct 2024 15:51:24 -0400 Subject: [PATCH 28/33] added test for ensuring control even when failing --- .../test_systems/drivebrain_controller_test.h | 20 +++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/test/test_systems/drivebrain_controller_test.h b/test/test_systems/drivebrain_controller_test.h index 4a7019253..ab3f6023e 100644 --- a/test/test_systems/drivebrain_controller_test.h +++ b/test/test_systems/drivebrain_controller_test.h @@ -60,6 +60,26 @@ TEST(DrivebrainControllerTesting, failing_stay_failing) { EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 0); } +TEST(DrivebrainControllerTesting, failing_in_control) { + DrivebrainController controller(10); + auto torque_controller_output_s = runTick( + &controller, 200, 1011, ControllerMode_e::MODE_4, 1032, 0.01f, 0.0f); + EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 0); + + torque_controller_output_s = runTick( + &controller, 200, 1033, ControllerMode_e::MODE_4, 1033, 0.01f, 0.0f); + EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 0); + + torque_controller_output_s = runTick( + &controller, 400, 1034, ControllerMode_e::MODE_4, 1034, 0.01f, 0.0f); + EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 0); + + torque_controller_output_s = runTick( + &controller, 400, 1034, ControllerMode_e::MODE_4, 1034, 0.0f, 1.0f); + EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 20000); + EXPECT_FLOAT_EQ(torque_controller_output_s.command.torqueSetpoints[0], PhysicalParameters::AMK_MAX_TORQUE); +} + TEST(DrivebrainControllerTesting, failing_reset_success) { DrivebrainController controller(10); auto torque_controller_output_s = From 161226df9c021a5a74e33798cea0564f8e10e267 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Thu, 10 Oct 2024 16:14:31 -0400 Subject: [PATCH 29/33] added timing failure status --- lib/interfaces/src/DrivebrainETHInterface.cpp | 2 ++ lib/shared_data/include/DrivebrainData.h | 1 + lib/shared_data/include/SharedDataTypes.h | 7 +++++-- lib/systems/include/DrivebrainController.h | 1 + platformio.ini | 2 +- src/main.cpp | 3 ++- test/test_systems/drivebrain_controller_test.h | 2 +- 7 files changed, 13 insertions(+), 5 deletions(-) diff --git a/lib/interfaces/src/DrivebrainETHInterface.cpp b/lib/interfaces/src/DrivebrainETHInterface.cpp index c65b952fd..f948da2d5 100644 --- a/lib/interfaces/src/DrivebrainETHInterface.cpp +++ b/lib/interfaces/src/DrivebrainETHInterface.cpp @@ -21,6 +21,8 @@ hytech_msgs_MCUOutputData DrivebrainETHInterface::make_db_msg(const SharedCarSta 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; } diff --git a/lib/shared_data/include/DrivebrainData.h b/lib/shared_data/include/DrivebrainData.h index 35cada9bf..220c4b2cf 100644 --- a/lib/shared_data/include/DrivebrainData.h +++ b/lib/shared_data/include/DrivebrainData.h @@ -13,4 +13,5 @@ struct DrivebrainData_s veh_vec speed_setpoints_rpm; }; + #endif // __DRIVEBRAINDATA_H__ \ No newline at end of file diff --git a/lib/shared_data/include/SharedDataTypes.h b/lib/shared_data/include/SharedDataTypes.h index da99b8baf..04aae87ff 100644 --- a/lib/shared_data/include/SharedDataTypes.h +++ b/lib/shared_data/include/SharedDataTypes.h @@ -146,6 +146,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, @@ -175,7 +176,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), @@ -184,7 +186,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) } diff --git a/lib/systems/include/DrivebrainController.h b/lib/systems/include/DrivebrainController.h index 77831c1f3..9b9cefe38 100644 --- a/lib/systems/include/DrivebrainController.h +++ b/lib/systems/include/DrivebrainController.h @@ -34,6 +34,7 @@ class DrivebrainController : public Controller } TorqueControllerOutput_s evaluate(const SharedCarState_s &state); + bool get_timing_failure_status() { return _timing_failure; } private: struct diff --git a/platformio.ini b/platformio.ini index f3d3ab21c..29fbc0dfa 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-07T23_13_48/hytech_msgs_pb_lib.tar.gz + https://github.com/hytech-racing/HT_proto/releases/download/2024-10-10T19_58_06/hytech_msgs_pb_lib.tar.gz [env:test_env] platform = native diff --git a/src/main.cpp b/src/main.cpp index 860ebcd2c..f41120655 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -439,7 +439,8 @@ void loop() pedals_system.getPedalsSystemData(), vn_interface.get_vn_struct(), db_eth_interface.get_latest_data(), - torque_controller_mux.get_tc_mux_status()); + torque_controller_mux.get_tc_mux_status(), + db_controller.get_timing_failure_status()); hytech_msgs_MCUOutputData out_eth_msg = db_eth_interface.make_db_msg(car_state_inst); diff --git a/test/test_systems/drivebrain_controller_test.h b/test/test_systems/drivebrain_controller_test.h index ab3f6023e..a6cada9df 100644 --- a/test/test_systems/drivebrain_controller_test.h +++ b/test/test_systems/drivebrain_controller_test.h @@ -26,7 +26,7 @@ auto runTick(DrivebrainController *controller, pedals_data.accelPercent = accelPercent; SharedCarState_s state(systick, {/*steering data*/}, {/*drivetrain_data*/}, {/*loadcell_data*/}, {/*raw_loadcell_data*/}, - pedals_data, {}, data, status); + pedals_data, {}, data, status, {}); return controller->evaluate(state); } From 3ca6d5364b934a505ff9db7ee92fea3251ffa8e5 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sun, 13 Oct 2024 12:50:16 -0400 Subject: [PATCH 30/33] added docs for the drivebrain controller --- lib/interfaces/src/DrivebrainETHInterface.cpp | 6 +-- lib/systems/include/DrivebrainController.h | 49 ++++++++++++++++--- lib/systems/src/DrivebrainController.cpp | 18 +++---- src/main.cpp | 1 + 4 files changed, 52 insertions(+), 22 deletions(-) diff --git a/lib/interfaces/src/DrivebrainETHInterface.cpp b/lib/interfaces/src/DrivebrainETHInterface.cpp index f948da2d5..0a8684817 100644 --- a/lib/interfaces/src/DrivebrainETHInterface.cpp +++ b/lib/interfaces/src/DrivebrainETHInterface.cpp @@ -7,13 +7,13 @@ 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.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, @@ -29,9 +29,7 @@ hytech_msgs_MCUOutputData DrivebrainETHInterface::make_db_msg(const SharedCarSta void DrivebrainETHInterface::receive_pb_msg(const hytech_msgs_MCUCommandData &msg_in, unsigned long curr_millis) { veh_vec 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 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; diff --git a/lib/systems/include/DrivebrainController.h b/lib/systems/include/DrivebrainController.h index 9b9cefe38..d418ee107 100644 --- a/lib/systems/include/DrivebrainController.h +++ b/lib/systems/include/DrivebrainController.h @@ -2,15 +2,15 @@ #define __DRIVEBRAINCONTROLLER_H__ #include -// #include "TorqueControllers.h" #include "DrivebrainData.h" #include "BaseController.h" #include "SharedDataTypes.h" #include + // 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,28 +19,65 @@ // 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_rec_time =0; + _last_worst_latency_rec_time = 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; diff --git a/lib/systems/src/DrivebrainController.cpp b/lib/systems/src/DrivebrainController.cpp index ec3c040c4..062294637 100644 --- a/lib/systems/src/DrivebrainController.cpp +++ b/lib/systems/src/DrivebrainController.cpp @@ -1,5 +1,5 @@ #include "DrivebrainController.h" -// #include + TorqueControllerOutput_s DrivebrainController::evaluate(const SharedCarState_s &state) { @@ -12,32 +12,26 @@ TorqueControllerOutput_s DrivebrainController::evaluate(const SharedCarState_s & // 2 if the DB_prev_MCU_recv_millis < 0, then the drivebrain has not received a time from the MCU // (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) + // 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); - + _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); - // only in the case that our speed is low enough (<1 m/s) do we want to clear the fault + // only in the case that we are not the active controller yet do we want to clear the fault bool is_active_controller = state.tc_mux_status.current_controller_mode_ == _params.assigned_controller_mode; diff --git a/src/main.cpp b/src/main.cpp index f41120655..77d936035 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -442,6 +442,7 @@ void loop() 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); From 5f887edbb5e0d022ffc9f0e45dcc2a55cfca08db Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sun, 13 Oct 2024 12:52:52 -0400 Subject: [PATCH 31/33] fixing build --- src/main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main.cpp b/src/main.cpp index 77d936035..fef30eaef 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -313,7 +313,7 @@ TorqueControllerCASEWrapper 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(&tc_simple), static_cast(&tc_vec), From 9deea295c36d9247042822fa7b39627604856e5f Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sun, 20 Oct 2024 16:09:49 -0400 Subject: [PATCH 32/33] adding in safety check to drivetrain system for over-limit torque limit --- lib/systems/include/DrivetrainSystem.h | 5 +++-- lib/systems/include/DrivetrainSystem.tpp | 10 +++++++++- test/test_systems/drivetrain_system_test.h | 10 +++++----- 3 files changed, 17 insertions(+), 8 deletions(-) diff --git a/lib/systems/include/DrivetrainSystem.h b/lib/systems/include/DrivetrainSystem.h index 7afb108ca..039ed23b8 100644 --- a/lib/systems/include/DrivetrainSystem.h +++ b/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 &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 &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" diff --git a/lib/systems/include/DrivetrainSystem.tpp b/lib/systems/include/DrivetrainSystem.tpp index 30565c739..d3909af09 100644 --- a/lib/systems/include/DrivetrainSystem.tpp +++ b/lib/systems/include/DrivetrainSystem.tpp @@ -148,7 +148,15 @@ void DrivetrainSystem::command_drivetrain(const DrivetrainCommand_ int index = 0; for (auto inv_pointer : inverters_) { - inv_pointer->handle_command({data.torqueSetpoints[index], data.speeds_rpm[index]}); + float setpoint = 0; + if(data.torqueSetpoints[index] > max_torque_setpoint_nm_) + { + setpoint = max_torque_setpoint_nm_; + } else { + setpoint = data.torqueSetpoints[index]; + } + + inv_pointer->handle_command({setpoint, data.speeds_rpm[index]}); index++; } // last_general_cmd_time_ = curr_system_millis_; diff --git a/test/test_systems/drivetrain_system_test.h b/test/test_systems/drivetrain_system_test.h index ad48cb90e..85628766c 100644 --- a/test/test_systems/drivetrain_system_test.h +++ b/test/test_systems/drivetrain_system_test.h @@ -179,17 +179,17 @@ TEST(DrivetrainSystemTesting, test_drivetrain_inverter_comms) SysClock clock; auto micros = 1000000; dt.tick(clock.tick(micros)); - dt.command_drivetrain({{1000.0, 1001.0, 1002.0, 1003.0}, {2000.0, 2001.0, 2002.0, 2003.0}}); + dt.command_drivetrain({{1000.0, 1001.0, 1002.0, 1003.0}, {10.0, 11.0, 12.0, 13.0}}); EXPECT_EQ(inv_fl.speed_setpoint_rpm_, 1000.0); - EXPECT_EQ(inv_fl.torque_setpoint_nm_, 2000.0); + EXPECT_EQ(inv_fl.torque_setpoint_nm_, 10.0); - EXPECT_EQ(inv_fr.torque_setpoint_nm_, 2001.0); + EXPECT_EQ(inv_fr.torque_setpoint_nm_, 11.0); EXPECT_EQ(inv_fr.speed_setpoint_rpm_, 1001.0); - EXPECT_EQ(inv_rl.torque_setpoint_nm_, 2002.0); + EXPECT_EQ(inv_rl.torque_setpoint_nm_, 12.0); EXPECT_EQ(inv_rl.speed_setpoint_rpm_, 1002.0); - EXPECT_EQ(inv_rr.torque_setpoint_nm_, 2003.0); + EXPECT_EQ(inv_rr.torque_setpoint_nm_, 13.0); EXPECT_EQ(inv_rr.speed_setpoint_rpm_, 1003.0); // testing to ensure that these extra general commands dont get through the period filter From 65af6d74150cb963ebb41eed8b55100e0680c4b4 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sun, 20 Oct 2024 18:04:14 -0400 Subject: [PATCH 33/33] removed duplicate type alias --- lib/shared_data/include/SharedDataTypes.h | 3 --- 1 file changed, 3 deletions(-) diff --git a/lib/shared_data/include/SharedDataTypes.h b/lib/shared_data/include/SharedDataTypes.h index 7ef31a2b6..1c389f03e 100644 --- a/lib/shared_data/include/SharedDataTypes.h +++ b/lib/shared_data/include/SharedDataTypes.h @@ -9,9 +9,6 @@ using speed_rpm = float; using torque_nm = float; -using speed_rpm = float; -using torque_nm = float; - /// @brief Defines modes of torque limit to be processed in torque limit map for exact values. enum class TorqueLimit_e {