fix(drivebrain_controller): fixed drivebrain_controller failure and r…

…eset logic

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, 

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;

test/test_systems/drivebrain_controller_test.h

@@ -5,8 +5,8 @@
 #include <DrivebrainData.h>
 
 
-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,44 +16,49 @@ 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);
 }
 
 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);
 }
 
 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);
 }
 
 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);
 }
 
 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,18 +67,46 @@ 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);
 }
 
 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