drive state required for PCU drive

Author: ZacharyBer

PCU/Core/User/Inc/FEB_CAN_BMS.h

@@ -64,5 +64,6 @@ float FEB_CAN_BMS_getMaxTemperature(void);
 void FEB_CAN_BMS_Init(void);
 void FEB_CAN_HEARTBEAT_Transmit(void);
 void FEB_CAN_BMS_ProcessHeartbeat(void);
+bool FEB_CAN_BMS_InDriveState(void);
 
 #endif /* INC_FEB_CAN_BMS_H_ */

PCU/Core/User/Src/FEB_CAN_BMS.c

@@ -1,5 +1,9 @@
 #include "FEB_CAN_BMS.h"
 #include "feb_uart_log.h"
+#include <stdbool.h>
+
+/* Timeout for BMS CAN communication (ms) */
+#define BMS_STATE_TIMEOUT_MS 500
 
 /* Global BMS message data */
 BMS_MESSAGE_TYPE BMS_MESSAGE;
@@ -140,3 +144,19 @@ void FEB_CAN_BMS_ProcessHeartbeat(void)
     FEB_CAN_HEARTBEAT_Transmit();
   }
 }
+
+bool FEB_CAN_BMS_InDriveState(void)
+{
+  // Check for recent BMS communication
+  if (BMS_MESSAGE.last_rx_timestamp == 0)
+  {
+    return false; // Never received BMS data
+  }
+
+  if (HAL_GetTick() - BMS_MESSAGE.last_rx_timestamp > BMS_STATE_TIMEOUT_MS)
+  {
+    return false; // BMS communication timeout
+  }
+
+  return BMS_MESSAGE.state == FEB_SM_ST_DRIVE;
+}

PCU/Core/User/Src/FEB_RMS.c

@@ -1,5 +1,6 @@
 #include "FEB_RMS.h"
 #include "FEB_ADC.h"
+#include "FEB_CAN_BMS.h"
 #include "FEB_CAN_RMS.h"
 #include "FEB_RMS_Config.h"
 #include "FEB_Debug.h"
@@ -29,6 +30,13 @@ void FEB_RMS_Setup(void)
 
 void FEB_RMS_Process(void)
 {
+  // Require BMS to be in drive state before enabling inverter
+  if (!FEB_CAN_BMS_InDriveState())
+  {
+    LOG_W(TAG_RMS, "Cannot enable RMS: BMS not in drive state (state=%d)", BMS_MESSAGE.state);
+    return;
+  }
+
   if (!RMS_CONTROL_MESSAGE.enabled)
   {
     LOG_I(TAG_RMS, "Sending RMS disable commands to clear lockout");
@@ -175,13 +183,20 @@ float FEB_RMS_GetMaxTorque(void)
  */
 void FEB_RMS_Torque(void)
 {
+  // Auto-disable RMS if BMS leaves drive state or communication is lost
+  if (DRIVE_STATE && !FEB_CAN_BMS_InDriveState())
+  {
+    LOG_W(TAG_RMS, "BMS left drive state or timeout, disabling RMS");
+    FEB_RMS_Disable();
+  }
 
   // Read latest sensor data
   FEB_ADC_GetAPPSData(&APPS_Data);
   FEB_ADC_GetBrakeData(&Brake_Data);
 
-  // Check plausibility and safety conditions
-  bool sensors_plausible = true; // OVERRIDE: bypass implausibility for testing
+  // Check plausibility and safety conditions (require BMS in drive state)
+  bool bms_in_drive = FEB_CAN_BMS_InDriveState();
+  bool sensors_plausible = bms_in_drive && DRIVE_STATE;
 
   // Log any safety violations
   if (!sensors_plausible)