diff --git a/lib/interfaces/src/DrivebrainETHInterface.cpp b/lib/interfaces/src/DrivebrainETHInterface.cpp index 3626541c..1c751e7a 100644 --- a/lib/interfaces/src/DrivebrainETHInterface.cpp +++ b/lib/interfaces/src/DrivebrainETHInterface.cpp @@ -6,14 +6,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.brakePercent; - + 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, @@ -21,15 +20,15 @@ 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; } 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/shared_data/include/SharedDataTypes.h b/lib/shared_data/include/SharedDataTypes.h index fc2b1492..1c389f03 100644 --- a/lib/shared_data/include/SharedDataTypes.h +++ b/lib/shared_data/include/SharedDataTypes.h @@ -157,6 +157,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, @@ -186,7 +187,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), @@ -195,7 +197,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 ce3e0da5..9b159a46 100644 --- a/lib/systems/include/DrivebrainController.h +++ b/lib/systems/include/DrivebrainController.h @@ -1,16 +1,16 @@ #ifndef __DRIVEBRAINCONTROLLER_H__ #define __DRIVEBRAINCONTROLLER_H__ - -#include "TorqueControllers.h" +#include #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,26 +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_timestamp = 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; @@ -46,6 +85,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/include/DrivetrainSystem.h b/lib/systems/include/DrivetrainSystem.h index 7afb108c..039ed23b 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 87eb3085..3c5cf3b0 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.inverter_torque_limit[index], data.speeds_rpm[index]}); + float setpoint = 0; + if(data.inverter_torque_limit[index] > max_torque_setpoint_nm_) + { + setpoint = max_torque_setpoint_nm_; + } else { + setpoint = data.inverter_torque_limit[index]; + } + + inv_pointer->handle_command({setpoint, data.speeds_rpm[index]}); index++; } // last_general_cmd_time_ = curr_system_millis_; diff --git a/lib/systems/src/DrivebrainController.cpp b/lib/systems/src/DrivebrainController.cpp index 82278ba8..3c8d2dce 100644 --- a/lib/systems/src/DrivebrainController.cpp +++ b/lib/systems/src/DrivebrainController.cpp @@ -25,9 +25,7 @@ TorqueControllerOutput_s DrivebrainController::evaluate(const SharedCarState_s & 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); } @@ -54,7 +52,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/src/main.cpp b/src/main.cpp index 4e7090c6..9cbe128f 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 @@ -11,8 +11,7 @@ #include "FlexCAN_T4.h" #include "HyTech_CAN.h" #include "MCU_rev15_defs.h" -// #include "NativeEthernet.h" - +#include "pb.h" // /* Interfaces */ #include "DrivebrainETHInterface.h" #include "ProtobufMsgInterface.h" @@ -53,6 +52,7 @@ /* PARAMETER STRUCTS */ +using namespace qindesign::network; const TelemetryInterfaceReadChannels telem_read_channels = { .accel1_channel = MCU15_ACCEL1_CHANNEL, @@ -312,7 +312,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), @@ -431,21 +431,23 @@ void loop() // single source of truth for the state of the car. // no systems or interfaces should write directly to this. SharedCarState_s car_state_inst(curr_tick, - steering_system.getSteeringSystemData(), - drivetrain.get_dynamic_data(), - load_cell_interface.getLoadCellForces(), - pedals_system.getPedalsSystemData(), - vn_interface.get_vn_struct(), - db_eth_interface.get_latest_data(), - torque_controller_mux.get_tc_mux_status()); - + steering_system.getSteeringSystemData(), + drivetrain.get_dynamic_data(), + load_cell_interface.getLoadCellForces(), + load_cell_interface.get_latest_raw_data(), + pedals_system.getPedalsSystemData(), + vn_interface.get_vn_struct(), + db_eth_interface.get_latest_data(), + 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); tick_all_systems(curr_tick); - - // logger.log_out(static_cast(torque_controller_mux.get_tc_mux_status().current_controller_mode_), curr_tick.millis, 100); + // inverter procedure before entering state machine // reset inverters if (dashboard.inverterResetButtonPressed() && drivetrain.drivetrain_error_occured()) @@ -505,6 +507,9 @@ void loop() Serial.print("Current TC error: "); Serial.println(static_cast(torque_controller_mux.get_tc_mux_status().active_error)); Serial.println(); + Serial.print("dial state: "); + Serial.println(static_cast(dashboard.getDialMode())); + Serial.println(); Serial.println(); } } @@ -587,6 +592,9 @@ void tick_all_interfaces(const SysTick_s ¤t_system_tick) a1.get().conversions[MCU15_BRAKE2_CHANNEL], pedals_system.getMechBrakeActiveThreshold(), torque_controller_mux.get_tc_mux_status().active_error); + + ams_interface.tick(current_system_tick); + } if (t.trigger50) // 50Hz @@ -660,9 +668,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"); - // handle_ethernet_socket_receive(&protobuf_recv_socket, &recv_pb_stream_union_msg, ethernet_interfaces); std::function recv_boi = &recv_pb_stream_msg; handle_ethernet_socket_receive<1024, hytech_msgs_MCUCommandData>(systick, &protobuf_recv_socket, recv_boi, db_eth_interface, hytech_msgs_MCUCommandData_fields); diff --git a/test/test_systems/drivebrain_controller_test.h b/test/test_systems/drivebrain_controller_test.h index defffe45..84a609af 100644 --- a/test/test_systems/drivebrain_controller_test.h +++ b/test/test_systems/drivebrain_controller_test.h @@ -1,63 +1,96 @@ #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}; -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.active_controller_mode = current_control_mode; - SharedCarState_s state(systick, {}, {}, {}, {}, {}, data, status); - return controller->evaluate(state); + SysTick_s systick; + systick.millis = systick_millis; + systick.micros = 1000; + + TorqueControllerMuxStatus status = {}; + status.active_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.inverter_torque_limit[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, 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); +} + +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, 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, 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); - 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.inverter_torque_limit[0], PhysicalParameters::AMK_MAX_TORQUE); } -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 diff --git a/test/test_systems/drivetrain_system_test.h b/test/test_systems/drivetrain_system_test.h index ad48cb90..85628766 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