Feat/r2d pump lockout

.gitmodules

@@ -4,3 +4,4 @@
 [submodule "src/Middlewares/SUFST/can-defs"]
 	path = src/Middlewares/SUFST/can-defs
 	url = https://github.com/sufst/can-defs
+	branch = feat/r2d-pump-lockout

src/SUFST/Inc/Services/pm100.h

@@ -13,6 +13,7 @@
 #define PM100_H
 
 #include <can_c.h>
+#include <can_s.h>
 #include <rtcan.h>
 #include <stdint.h>
 #include <tx_api.h>
@@ -32,6 +33,8 @@
 
 #define PM100_RX_QUEUE_SIZE           10 // 10 items
 
+#define PUMP_RUNNING_THRESHOLD        8 // Voltage/V
+
 /**
  * @brief   PM100 context
  */
@@ -49,6 +52,7 @@ typedef struct
     struct can_c_pm100_temperature_set_1_t temp1;
     struct can_c_pm100_temperature_set_2_t temp2;
     struct can_c_pm100_temperature_set_3_t temp3;
+    struct can_s_pdm_out_voltage_t vout;
     uint16_t error;
     const config_pm100_t* config_ptr;
 } pm100_context_t;
@@ -69,4 +73,6 @@ int16_t pm100_max_inverter_temp(pm100_context_t* pm100_ptr);
 status_t pm100_disable(pm100_context_t* pm100_ptr);
 status_t pm100_request_torque(pm100_context_t* pm100_ptr, uint16_t torque);
 
+bool pm100_check_pumps_running(pm100_context_t* pm100_ptr);
+
 #endif

src/SUFST/Inc/config.h

@@ -46,6 +46,7 @@ typedef struct {
      config_thread_t thread;                 // control thread config
      uint32_t schedule_ticks;                // number of ticks between runs of the control loop thread
      bool r2d_requires_brake;                // whether or not the brake needs to be pressed for R2D activation
+     bool r2d_requires_pump;                 // whether or not the pump needs to be running for R2D activation
      uint32_t ts_ready_timeout_ticks;        // ticks after which waiting for TS ready times out
      uint32_t ts_ready_poll_ticks;           // how often to poll input when waiting for TS ready
      uint32_t precharge_timeout_ticks;       // ticks after which waiting for precharge times out

src/SUFST/Src/Services/ctrl.c

@@ -242,12 +242,14 @@ void ctrl_state_machine_tick(ctrl_context_t* ctrl_ptr)
 
     // TS is ready, can initiate pre-charge sequence
     // TS on LED turns solid
+    // powers on pumps in preparation for r2d
     case (CTRL_STATE_PRECHARGE_WAIT):
     {
         const uint32_t charge_time = tx_time_get() - ctrl_ptr->precharge_start;
 
         if (pm100_is_precharged(ctrl_ptr->pm100_ptr))
         {
+            ctrl_ptr->pump_pwr = 1;
             next_state = CTRL_STATE_R2D_WAIT;
             dash_clear_buttons(dash_ptr);
             LOG_INFO("Precharge complete\n");
@@ -264,6 +266,7 @@ void ctrl_state_machine_tick(ctrl_context_t* ctrl_ptr)
 
     // pre-charge is complete, wait for R2D signal
     // also wait for brake to be fully pressed (if enabled)
+    // also wait for pumps to be running (if enabled)
     case (CTRL_STATE_R2D_WAIT):
     {
         if (!trc_ready())
@@ -295,12 +298,15 @@ void ctrl_state_machine_tick(ctrl_context_t* ctrl_ptr)
                           ? (ctrl_ptr->bps_reading > BPS_ON_THRESH)
                           : 1;
 
+                r2d &= (config_ptr->r2d_requires_pump)
+                           ? pm100_check_pumps_running(ctrl_ptr->pm100_ptr)
+                           : 1;
+
                 if (r2d)
                 {
                     dash_set_r2d_led_state(dash_ptr, GPIO_PIN_SET);
                     pm100_disable(ctrl_ptr->pm100_ptr);
                     rtds_activate(ctrl_ptr->rtds_config_ptr);
-                    ctrl_ptr->pump_pwr = 1;
 
                     next_state = CTRL_STATE_TS_ON;
 

src/SUFST/Src/Services/pm100.c

@@ -127,18 +127,37 @@ void pm100_thread_entry(ULONG input)
     pm100_context_t* pm100_ptr = (pm100_context_t*) input;
     const config_pm100_t* config_ptr = pm100_ptr->config_ptr;
 
-    // set up RTCAN subscriptions
-    uint32_t subscriptions[] = {CAN_C_PM100_INTERNAL_STATES_FRAME_ID,
-                                CAN_C_PM100_FAULT_CODES_FRAME_ID,
-                                CAN_C_PM100_TEMPERATURE_SET_1_FRAME_ID,
-                                CAN_C_PM100_TEMPERATURE_SET_2_FRAME_ID,
-                                CAN_C_PM100_TEMPERATURE_SET_3_FRAME_ID};
-
-    for (uint32_t i = 0; i < sizeof(subscriptions) / sizeof(subscriptions[0]);
+    // set up RTCAN CAN C subscriptions
+    uint32_t can_c_subscriptions[] = {CAN_C_PM100_INTERNAL_STATES_FRAME_ID,
+                                      CAN_C_PM100_FAULT_CODES_FRAME_ID,
+                                      CAN_C_PM100_TEMPERATURE_SET_1_FRAME_ID,
+                                      CAN_C_PM100_TEMPERATURE_SET_2_FRAME_ID,
+                                      CAN_C_PM100_TEMPERATURE_SET_3_FRAME_ID};
+
+    for (uint32_t i = 0;
+         i < sizeof(can_c_subscriptions) / sizeof(can_c_subscriptions[0]);
          i++)
     {
         rtcan_status_t status = rtcan_subscribe(pm100_ptr->rtcan_c_ptr,
-                                                subscriptions[i],
+                                                can_c_subscriptions[i],
+                                                &pm100_ptr->can_rx_queue);
+
+        if (status != RTCAN_OK)
+        {
+            // TODO: update broadcast states with error
+            tx_thread_terminate(&pm100_ptr->thread);
+        }
+    }
+
+    // set up RTCAN CAN S subscriptions
+    uint32_t can_s_subscriptions[] = {CAN_S_PDM_OUT_VOLTAGE_FRAME_ID};
+
+    for (uint32_t i = 0;
+         i < sizeof(can_s_subscriptions) / sizeof(can_s_subscriptions[0]);
+         i++)
+    {
+        rtcan_status_t status = rtcan_subscribe(pm100_ptr->rtcan_s_ptr,
+                                                can_s_subscriptions[i],
                                                 &pm100_ptr->can_rx_queue);
 
         if (status != RTCAN_OK)
@@ -166,7 +185,6 @@ void pm100_thread_entry(ULONG input)
         {
             pm100_ptr->broadcasts_valid = true;
             process_broadcast(pm100_ptr, msg_ptr);
-            rtcan_msg_consumed(pm100_ptr->rtcan_c_ptr, msg_ptr);
         }
         else
         {
@@ -191,6 +209,8 @@ void process_broadcast(pm100_context_t* pm100_ptr, const rtcan_msg_t* msg_ptr)
                                            msg_ptr->data,
                                            msg_ptr->length);
 
+        rtcan_msg_consumed(pm100_ptr->rtcan_c_ptr, msg_ptr);
+
         break;
     }
 
@@ -213,6 +233,8 @@ void process_broadcast(pm100_context_t* pm100_ptr, const rtcan_msg_t* msg_ptr)
             (void) pm100_disable(pm100_ptr);
         }
 
+        rtcan_msg_consumed(pm100_ptr->rtcan_c_ptr, msg_ptr);
+
         break;
     }
 
@@ -223,6 +245,8 @@ void process_broadcast(pm100_context_t* pm100_ptr, const rtcan_msg_t* msg_ptr)
                                              msg_ptr->data,
                                              msg_ptr->length);
 
+        rtcan_msg_consumed(pm100_ptr->rtcan_c_ptr, msg_ptr);
+
         break;
     }
 
@@ -232,6 +256,8 @@ void process_broadcast(pm100_context_t* pm100_ptr, const rtcan_msg_t* msg_ptr)
                                              msg_ptr->data,
                                              msg_ptr->length);
 
+        rtcan_msg_consumed(pm100_ptr->rtcan_c_ptr, msg_ptr);
+
         break;
     }
 
@@ -241,6 +267,19 @@ void process_broadcast(pm100_context_t* pm100_ptr, const rtcan_msg_t* msg_ptr)
                                              msg_ptr->data,
                                              msg_ptr->length);
 
+        rtcan_msg_consumed(pm100_ptr->rtcan_c_ptr, msg_ptr);
+
+        break;
+    }
+
+    case CAN_S_PDM_OUT_VOLTAGE_FRAME_ID:
+    {
+        can_s_pdm_out_voltage_unpack(&pm100_ptr->vout,
+                                     msg_ptr->data,
+                                     msg_ptr->length);
+
+        rtcan_msg_consumed(pm100_ptr->rtcan_s_ptr, msg_ptr);
+
         break;
     }
 
@@ -434,3 +473,9 @@ status_t pm100_request_torque(pm100_context_t* pm100_ptr, uint16_t torque)
 
     return status;
 }
+
+bool pm100_check_pumps_running(pm100_context_t* pm100_ptr)
+{
+    return (&pm100_ptr->vout.pdm_output_4_voltage > PUMP_RUNNING_THRESHOLD)
+           && (&pm100_ptr->vout.pdm_output_5_voltage > PUMP_RUNNING_THRESHOLD);
+}

src/SUFST/Src/config.c

@@ -91,6 +91,7 @@ static const config_t config_instance = {
         },
         .schedule_ticks = SECONDS_TO_TICKS(0.01), // 100Hz control loop
         .r2d_requires_brake = true,
+        .r2d_requires_pump = true,
         .bps_on_threshold = 5,
 	    .apps_bps_low_threshold = 5,
 	    .apps_bps_high_threshold = 20,