Fix inverter on state (#1314)

### Changelist 
<!-- Give a list of the changes covered in this PR. This will help both
you and the reviewer keep this PR within scope. -->

Currently, "inverter on" state is not being respected. This means the
inverters are not actually given time to boot up before HV is applied.
This is probably fine, since we've been running this for a while, but we
should still fix.

### Testing Done
<!-- Outline the testing that was done to demonstrate the changes are
solid. This could be unit tests, integration tests, testing on the car,
etc. Include relevant code snippets, screenshots, etc as needed. -->

Just unit testing ATM.

---------

Co-authored-by: Edwin <20777515+Lucien950@users.noreply.github.com>

firmware/quadruna/BMS/src/app/states/app_initState.c

@@ -52,7 +52,6 @@ static void initStateRunOnTick100Hz(void)
             const bool charger_connected         = app_canRx_BRUSA_IsConnected_get();
             const bool cell_balancing_enabled    = app_canRx_Debug_CellBalancingRequest_get();
             const bool external_charging_request = app_canRx_Debug_StartCharging_get();
-            const bool charging_override_fault   = app_canRx_Debug_FaultEncounteredOverride_get();
             const bool clear_brusa_latch         = app_canRx_Debug_ClearChargerLatchedFault_get();
 
             app_canTx_BMS_ClearLatch_set(clear_brusa_latch);

firmware/quadruna/BMS/src/app/states/app_inverterOnState.c

@@ -4,23 +4,15 @@
 #include "app_utils.h"
 #include "app_timer.h"
 
-#define CHARGING_MILLISECONDS 200
-
 static TimerChannel timer;
-static bool         has_time_passed;
 
 static void inverterOnStateRunOnEntry(void)
 {
     app_canTx_BMS_State_set(BMS_INVERTER_ON_STATE);
-    app_timer_init(&timer, CHARGING_MILLISECONDS);
+    app_timer_init(&timer, INVERTER_BOOTUP_TIME_MS);
     app_timer_restart(&timer);
 }
 
-void app_inverterOnState_init()
-{
-    has_time_passed = false;
-}
-
 static void inverterOnStateRunOnTick1Hz(void)
 {
     app_allStates_runOnTick1Hz();
@@ -32,10 +24,9 @@ static void inverterOnStateRunOnTick100Hz(void)
     {
         TimerState timer_state = app_timer_updateAndGetState(&timer);
 
-        if (timer_state == TIMER_STATE_EXPIRED || has_time_passed)
+        if (timer_state == TIMER_STATE_EXPIRED)
         {
             app_stateMachine_setNextState(app_prechargeState_get());
-            has_time_passed = true;
         }
     }
 }

firmware/quadruna/BMS/src/app/states/app_inverterOnState.h

@@ -2,13 +2,10 @@
 
 #include "app_stateMachine.h"
 
+#define INVERTER_BOOTUP_TIME_MS (200U)
+
 /**
  * Get a pointer to the Inverter State.
  * @return A pointer to the Inverter State.
  */
 const State *app_inverterOnState_get(void);
-
-/**
- * Reset hasTimePassed bool
- */
-void app_inverterOnState_init(void);

firmware/quadruna/BMS/src/tasks.c

@@ -333,7 +333,6 @@ void tasks_init(void)
     app_canTx_init();
     app_canRx_init();
 
-    app_inverterOnState_init();
     app_accumulator_init();
     app_tractiveSystem_init();
 

firmware/quadruna/BMS/test/test_bmsBaseStateMachineTest.h

@@ -52,7 +52,6 @@ class BmsBaseStateMachineTest : public BaseStateMachineTest
             heartbeatMonitorChecklist, heartbeatGetters, heartbeatUpdaters, &app_canTx_BMS_Heartbeat_set,
             heartbeatFaultSetters, heartbeatFaultGetters);
 
-        app_inverterOnState_init();
         app_accumulator_init();
         app_tractiveSystem_init();
         app_thermistors_init();

firmware/quadruna/BMS/test/test_stateMachine.cpp

@@ -560,9 +560,10 @@ TEST_F(BmsStateMachineTest, check_precharge_state_transitions_and_air_plus_statu
                          },
                          {
                              // Slow precharge, fails
-                             .air_negative_closes         = true,
-                             .initial_ts_voltage          = 0.0,
-                             .precharge_duration          = PRECHARGE_COMPLETION_UPPER_BOUND + 30,
+                             .air_negative_closes = true,
+                             .initial_ts_voltage  = 0.0,
+                             .precharge_duration  = PRECHARGE_COMPLETION_UPPER_BOUND + INVERTER_BOOTUP_TIME_MS +
+                                                   20U, // Allow inverter on state to complete again
                              .expect_precharge_starts     = true,
                              .expect_precharge_successful = false,
                          } };
@@ -582,7 +583,7 @@ TEST_F(BmsStateMachineTest, check_precharge_state_transitions_and_air_plus_statu
         if (test_params[i].expect_precharge_starts)
         {
             // Precharge should start
-            LetTimePass(210U);
+            LetTimePass(INVERTER_BOOTUP_TIME_MS + 10U);
             ASSERT_EQ(app_prechargeState_get(), app_stateMachine_getCurrentState());
             ASSERT_EQ(fake_io_airs_closePositive_callCount(), 0);
 

firmware/quadruna/VC/src/app/app_faultCheck.c

@@ -7,7 +7,13 @@
 
 static Signal apps_brake_disagreement_signal;
 
-bool app_boardFaultCheck(void)
+void app_faultCheck_init(void)
+{
+    app_signal_init(
+        &apps_brake_disagreement_signal, APPS_BRAKE_DISAGREEMENT_TIME_TO_FAULT, APPS_BRAKE_DISAGREEMENT_TIME_TO_CLEAR);
+}
+
+bool app_faultCheck_checkBoards(void)
 {
     const bool bms_fault  = app_canAlerts_BoardHasFault(BMS_ALERT_BOARD);
     const bool vc_fault   = app_canAlerts_BoardHasFault(VC_ALERT_BOARD);
@@ -17,7 +23,7 @@ bool app_boardFaultCheck(void)
     return (bms_fault || vc_fault || fsm_fault || crit_fault);
 }
 
-bool app_inverterFaultCheck()
+bool app_faultCheck_checkInverters()
 {
     const bool left_inverter_fault  = app_canRx_INVL_VsmState_get() == INVERTER_VSM_BLINK_FAULT_CODE_STATE;
     const bool right_inverter_fault = app_canRx_INVR_VsmState_get() == INVERTER_VSM_BLINK_FAULT_CODE_STATE;
@@ -27,13 +33,7 @@ bool app_inverterFaultCheck()
     return (left_inverter_fault || right_inverter_fault);
 }
 
-void app_bspd_init(void)
-{
-    app_signal_init(
-        &apps_brake_disagreement_signal, APPS_BRAKE_DISAGREEMENT_TIME_TO_FAULT, APPS_BRAKE_DISAGREEMENT_TIME_TO_CLEAR);
-}
-
-bool app_bspdWarningCheck(float papps_pedal_percentage, float sapps_pedal_percentage)
+bool app_faultCheck_checkSoftwareBspd(float papps_pedal_percentage, float sapps_pedal_percentage)
 {
     // Accelerator Brake Plausibility (bad user input safety issues)
     // Protect against brake/apps active at same time

firmware/quadruna/VC/src/app/app_faultCheck.h

@@ -2,25 +2,25 @@
 
 #include <stdbool.h>
 
+#define APPS_BRAKE_DISAGREEMENT_TIME_TO_FAULT (10U)
+#define APPS_BRAKE_DISAGREEMENT_TIME_TO_CLEAR (10U)
+
 /**
- * Check if any baord has faulted
+ * Initialize apps internal signals.
  */
-bool app_boardFaultCheck();
+void app_faultCheck_init(void);
 
 /**
- * Check either right or left inverters have faulted
+ * Check if any baord has faulted
  */
-bool app_inverterFaultCheck();
+bool app_faultCheck_checkBoards();
 
 /**
- * Initialize apps internal signals.
+ * Check either right or left inverters have faulted
  */
-void app_bspd_init(void);
+bool app_faultCheck_checkInverters();
 
 /**
  * Check if brakes and apps are active at same time.
  */
-bool app_bspdWarningCheck(float papps_pedal_percentage, float sapps_pedal_percentage);
-
-#define APPS_BRAKE_DISAGREEMENT_TIME_TO_FAULT (10U)
-#define APPS_BRAKE_DISAGREEMENT_TIME_TO_CLEAR (10U)
+bool app_faultCheck_checkSoftwareBspd(float papps_pedal_percentage, float sapps_pedal_percentage);

firmware/quadruna/VC/src/app/app_powerManager.c

@@ -1,6 +1,5 @@
 #include "io_efuse.h"
 #include "app_powerManager.h"
-#include "io_pcm.h"
 #include "app_canTx.h"
 
 static PowerStateConfig power_manager_config;
@@ -12,8 +11,6 @@ void app_powerManager_updateConfig(PowerStateConfig new_power_manager_config)
     {
         io_efuse_setChannel((EfuseChannel)efuse, power_manager_config.efuses[efuse]);
     }
-
-    io_pcm_set(power_manager_config.pcm);
 }
 
 PowerStateConfig app_powerManager_getConfig(void)

firmware/quadruna/VC/src/app/app_powerManager.h

@@ -4,7 +4,6 @@
 typedef struct
 {
     bool efuses[NUM_EFUSE_CHANNELS];
-    bool pcm;
 } PowerStateConfig;
 
 void app_powerManager_updateConfig(PowerStateConfig power_manager_config);

firmware/quadruna/VC/src/app/states/app_allStates.c

@@ -2,6 +2,7 @@
 // app
 #include "app_sbgEllipse.h"
 #include "app_canTx.h"
+#include "app_canRx.h"
 #include "app_canAlerts.h"
 #include "app_heartbeatMonitor.h"
 #include "app_lowVoltageBattery.h"
@@ -21,6 +22,10 @@ static uint16_t heartbeat_cycles = 0;
 
 void app_allStates_runOnTick100Hz(void)
 {
+    // Enable PCM if HV up.
+    const bool bms_in_drive = app_canRx_BMS_State_get() == BMS_DRIVE_STATE;
+    io_pcm_set(bms_in_drive);
+
     app_lowVoltageBattery_broadcast();
     app_shdnLoop_broadcast();
     app_currentSensing_broadcast();

firmware/quadruna/VC/src/app/states/app_driveState.c

@@ -34,7 +34,6 @@ static const PowerStateConfig power_manager_drive_init = {
         [EFUSE_CHANNEL_TELEM] = true,
         [EFUSE_CHANNEL_BUZZER] = true,
     },
-    .pcm = true,
 };
 
 void transmitTorqueRequests(float apps_pedal_percentage)
@@ -104,16 +103,14 @@ static void driveStateRunOnTick1Hz(void)
 static void driveStateRunOnTick100Hz(void)
 {
     // All states module checks for faults, and returns whether or not a fault was detected.
-    const bool any_board_has_fault = app_boardFaultCheck();
-    const bool inverter_has_fault  = app_inverterFaultCheck();
+    const bool any_board_has_fault = app_faultCheck_checkBoards();
+    const bool inverter_has_fault  = app_faultCheck_checkInverters();
     const bool all_states_ok       = !(any_board_has_fault || inverter_has_fault);
 
     const bool start_switch_off          = app_canRx_CRIT_StartSwitch_get() == SWITCH_OFF;
     const bool bms_not_in_drive          = app_canRx_BMS_State_get() != BMS_DRIVE_STATE;
     bool       exit_drive_to_init        = bms_not_in_drive;
     bool       exit_drive_to_inverter_on = !all_states_ok || start_switch_off;
-    float      apps_pedal_percentage     = app_canRx_FSM_PappsMappedPedalPercentage_get() * 0.01f;
-    float      sapps_pedal_percentage    = app_canRx_FSM_SappsMappedPedalPercentage_get() * 0.01f;
 
     if (exit_drive_to_init)
     {
@@ -134,14 +131,16 @@ static void driveStateRunOnTick100Hz(void)
     }
 
     // regen switched pedal percentage from [0, 100] to [0.0, 1.0] to [-0.3, 0.7] and then scaled to [-1,1]
+    float apps_pedal_percentage  = app_canRx_FSM_PappsMappedPedalPercentage_get() * 0.01f;
+    float sapps_pedal_percentage = app_canRx_FSM_SappsMappedPedalPercentage_get() * 0.01f;
     if (regen_switch_enabled)
     {
         apps_pedal_percentage  = app_regen_pedalRemapping(apps_pedal_percentage);
         sapps_pedal_percentage = app_regen_pedalRemapping(sapps_pedal_percentage);
     }
 
     app_canTx_VC_MappedPedalPercentage_set(apps_pedal_percentage);
-    if (app_bspdWarningCheck(apps_pedal_percentage, sapps_pedal_percentage))
+    if (app_faultCheck_checkSoftwareBspd(apps_pedal_percentage, sapps_pedal_percentage))
     {
         // If bspd warning is true, set torque to 0.0
         app_canTx_VC_LeftInverterTorqueCommand_set(0.0f);

firmware/quadruna/VC/src/app/states/app_initState.c

@@ -19,7 +19,6 @@ static const PowerStateConfig power_manager_shutdown_init = {
         [EFUSE_CHANNEL_TELEM] = true,
         [EFUSE_CHANNEL_BUZZER] = false,
     },
-    .pcm = false,
 };
 
 static void initStateRunOnEntry(void)
@@ -43,11 +42,12 @@ static void initStateRunOnEntry(void)
 
 static void initStateRunOnTick100Hz(void)
 {
-    const bool any_board_has_fault = app_boardFaultCheck();
-    const bool inverter_has_fault  = app_inverterFaultCheck();
-    const bool all_states_ok       = !(any_board_has_fault || inverter_has_fault);
-
-    if (app_canRx_BMS_State_get() == BMS_DRIVE_STATE && all_states_ok)
+    // Turn on inverters when requested to do so by the BMS (so we can power them up before HV is applied).
+    // Also, turn them on if HV is already up/coming up.
+    const bool enable_inverters = app_canRx_BMS_State_get() == BMS_INVERTER_ON_STATE ||
+                                  app_canRx_BMS_State_get() == BMS_PRECHARGE_STATE ||
+                                  app_canRx_BMS_State_get() == BMS_DRIVE_STATE;
+    if (enable_inverters)
     {
         app_stateMachine_setNextState(app_inverterOnState_get());
     }

firmware/quadruna/VC/src/app/states/app_inverterOnState.c

@@ -19,7 +19,6 @@ static const PowerStateConfig power_manager_inverter_init = {
         [EFUSE_CHANNEL_TELEM] = true,
         [EFUSE_CHANNEL_BUZZER] = false,
     },
-    .pcm = true,
 };
 
 static bool prev_start_switch_pos;
@@ -42,19 +41,23 @@ static void inverterOnStateRunOnEntry(void)
 
 static void inverterOnStateRunOnTick100Hz(void)
 {
-    const bool any_board_has_fault = app_boardFaultCheck();
-    const bool inverter_has_fault  = app_inverterFaultCheck();
+    const bool any_board_has_fault = app_faultCheck_checkBoards();
+    const bool inverter_has_fault  = app_faultCheck_checkInverters();
     const bool all_states_ok       = !(any_board_has_fault || inverter_has_fault);
 
     const bool curr_start_switch_on     = app_canRx_CRIT_StartSwitch_get();
     const bool was_start_switch_enabled = !prev_start_switch_pos && curr_start_switch_on;
     const bool is_brake_actuated        = app_canRx_FSM_BrakeActuated_get();
 
-    if (app_canRx_BMS_State_get() != BMS_DRIVE_STATE)
+    const bool bms_ready_for_drive = app_canRx_BMS_State_get() == BMS_DRIVE_STATE;
+    const bool hv_support_lost =
+        app_canRx_BMS_State_get() == BMS_INIT_STATE || app_canRx_BMS_State_get() == BMS_FAULT_STATE;
+
+    if (hv_support_lost)
     {
         app_stateMachine_setNextState(app_initState_get());
     }
-    else if (is_brake_actuated && was_start_switch_enabled && all_states_ok)
+    else if (all_states_ok && bms_ready_for_drive && is_brake_actuated && was_start_switch_enabled)
     {
         // Transition to drive state when start-up conditions are passed (see
         // EV.10.4.3):

firmware/quadruna/VC/src/tasks.c

@@ -428,7 +428,7 @@ void tasks_init(void)
     app_canTx_VC_Hash_set(GIT_COMMIT_HASH);
     app_canTx_VC_Clean_set(GIT_COMMIT_CLEAN);
 
-    app_bspd_init();
+    app_faultCheck_init();
 
     // enable these for inverter programming
     // hw_gpio_writePin(&inv_l_program, true);

firmware/quadruna/VC/test/test_stateMachine.cpp

@@ -251,7 +251,7 @@ TEST_F(VCStateMachineTest, BMS_causes_drive_to_inverter_on)
 
     app_canRx_BMS_State_update(BMS_INVERTER_ON_STATE);
     LetTimePass(100);
-    EXPECT_EQ(app_initState_get(), app_stateMachine_getCurrentState());
+    EXPECT_EQ(app_inverterOnState_get(), app_stateMachine_getCurrentState());
 }
 
 TEST_F(VCStateMachineTest, BMS_causes_drive_to_inverter_on_to_init)
@@ -262,7 +262,7 @@ TEST_F(VCStateMachineTest, BMS_causes_drive_to_inverter_on_to_init)
 
     app_canRx_BMS_State_update(BMS_PRECHARGE_STATE);
     LetTimePass(100);
-    EXPECT_EQ(app_initState_get(), app_stateMachine_getCurrentState());
+    EXPECT_EQ(app_inverterOnState_get(), app_stateMachine_getCurrentState());
 
     app_canRx_BMS_State_update(BMS_INIT_STATE);
     LetTimePass(100);

firmware/quadruna/VC/test/test_vcBaseStateMachineTest.h

@@ -55,7 +55,7 @@ class VcBaseStateMachineTest : public BaseStateMachineTest
         // Disable heartbeat monitor in the nominal case. To use representative heartbeat behavior,
         // re-enable the heartbeat monitor.
         app_heartbeatMonitor_blockFaults(true);
-        app_bspd_init();
+        app_faultCheck_init();
 
         memset(&fake_sensor_data, 0U, sizeof(fake_sensor_data));
 

firmware/quadruna/VC/test/vehicle_dynamics/test_regen.cpp

@@ -1,20 +1,15 @@
 #include <gtest/gtest.h>
+#include <assert.h>
+
+#include "test_vcBaseStateMachineTest.h"
 
 extern "C"
 {
-#include "test_vcBaseStateMachineTest.h"
-#include "app_utils.h"
 #include "app_units.h"
-#include "app_canTx.h"
-#include "app_canRx.h"
-#include "app_canAlerts.h"
-#include "app_canUtils.h"
 #include "app_regen.h"
 #include "app_powerLimiting.h"
 #include "app_vehicleDynamicsConstants.h"
 #include "app_vehicleDynamics.h"
-#include "math.h"
-#include <assert.h>
 }
 
 class TestRegen : public VcBaseStateMachineTest