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