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