Fwxv 171 power fsm implementation (#170)

* Inital commit

* Updated a few sequence states

* Power FSM implemented with very janky status checks

* Test OFF to MAIN

* Updated power fsm test

* Updated yamls

* Updated tests

* Fixed segfault

* Format

* Format again

* Can data header file and update power_fsm_sequence

* yay it is segfaulting again

* Test is passing

* Format

* Integrate watchdog changes

* fix fsm context

* Updated test and can data

* Format

* Address comments

* Add can recv checking

* Updated something

* Added notify. Test failing

* Updated fsm shared mem

* Band-aid solution for context

* Clean up

* Reverted unwanted change

* Remove prv from header files

* Update Power FSM shared memory

* Add small brake threshold

* Format

* Fix build error

---------

Co-authored-by: ShiCheng Lu <[email protected]>

libraries/codegen/boards/bms_carrier.yaml

@@ -11,7 +11,7 @@
 ---
   Messages:
     bps_heartbeat:
-      id: 0
+      id: 64
       target:
         centre_console:
           watchdog: 0
@@ -78,6 +78,8 @@
       target:
         bms_carrier:
           watchdog: 0
+        centre_console:
+          watchdog: 0
       signals:
         hv:
           length: 8

libraries/codegen/boards/centre_console.yaml

@@ -10,6 +10,30 @@
 
 ---
   Messages:
+    set_bms_power:
+      id: 4
+      target:
+        power_distribution_front:
+          watchdog: 0
+        power_distribution_rear:
+          watchdog: 0
+      critical: true
+      signals:
+        bms_power_on_notification:
+          length: 8
+
+    set_power_state:
+      id: 5
+      target:
+        power_distribution_front:
+          watchdog: 0
+        power_distribution_rear:
+          watchdog: 0
+      critical: true
+      signals:
+        turn_on_everything_notification:
+          length: 8
+
     set_relay_states:
       id: 1
       target:

libraries/codegen/boards/motor_controller.yaml

@@ -93,4 +93,12 @@
           length: 8
         precharge_status:
           length: 8
-
+
+    precharge_completed:
+      id: 16
+      target:
+        centre_console:
+          watchdog: 0
+      signals:
+        notification:
+          length: 8

libraries/codegen/boards/pedal.yaml

@@ -15,6 +15,8 @@
       target:
         motor_controller:
           watchdog: 0
+        centre_console:
+          watchdog: 0
       signals:
         throttle_output:
           length: 32

libraries/codegen/templates/_getters.h.jinja

@@ -10,6 +10,6 @@
 #define get_{{message}}_{{signal}}() \
     g_rx_struct.{{message}}_{{signal}}
     {% endfor %}
-#define get_received_{{message}} \
+#define get_received_{{message}}() \
     g_rx_struct.received_{{message}}
 {% endfor %}

projects/centre_console/inc/cc_hw_defs.h

@@ -11,7 +11,7 @@
 
 // Button Inputs and backlit LEDS
 #define CC_BTN_PUSH_START \
-  { .port = NUM_GPIO_PORTS, .pin = GPIO_PINS_PER_PORT }
+  { .port = NUM_GPIO_PORTS - 1, .pin = GPIO_PINS_PER_PORT - 1 }
 #define CC_BTN_STATE_DRIVE \
   { .port = NUM_GPIO_PORTS, .pin = GPIO_PINS_PER_PORT }
 #define CC_BTN_STATE_NEUTRAL \

projects/centre_console/inc/fsm_shared_mem.h

@@ -1,5 +1,6 @@
 #pragma once
 #include "fsm.h"
+#include "fsm_shared_mem.h"
 #include "log.h"
 #include "semaphore.h"
 
@@ -10,10 +11,23 @@ typedef struct FSMStorage {
   Mutex mutex;
 } FSMStorage;
 
+// Initialize fsm shared memory
 void fsm_shared_mem_init();
+
+// Set Power FSM state in shared memory
 void fsm_shared_mem_set_power_state(StateId state);
+
+// Get Power FSM state
 StateId fsm_shared_mem_get_power_state();
+
+// Set Power FSM Error Code
 void fsm_shared_mem_set_power_error_code(StatusCode code);
+
+// Get Power FSM Error Code
 StatusCode fsm_shared_mem_get_power_error_code();
+
+// Set Drive FSM Error Code
 void fsm_shared_mem_set_drive_error_code(StatusCode code);
+
+// Get Drive FSM Error Code
 StatusCode fsm_shared_mem_get_drive_error_code();

projects/centre_console/inc/power_fsm.h

@@ -2,16 +2,20 @@
 
 #include "delay.h"
 #include "fsm.h"
+#include "fsm_shared_mem.h"
 #include "gpio.h"
 #include "log.h"
 #include "power_fsm_sequence.h"
 #include "task.h"
 
-#define NUM_CENTRE_CONSOLE_POWER_STATES 14
-#define NUM_CENTRE_CONSOLE_POWER_TRANSITIONS 40
-DECLARE_FSM(centre_console_power_fsm);
+#define NUM_POWER_STATES 14
+#define NUM_POWER_TRANSITIONS 40
 
-typedef enum MciFsmStateId {
+#define START_BUTTON_EVENT 0
+
+DECLARE_FSM(power);
+
+typedef enum PowerFsmStateId {
   POWER_FSM_STATE_OFF = 0,
   // -> MAIN Sequence
   POWER_FSM_CONFIRM_AUX_STATUS,
@@ -32,6 +36,13 @@ typedef enum MciFsmStateId {
   POWER_FSM_DISCHARGE_PRECHARGE,
   POWER_FSM_TURN_OFF_EVERYTHING,
   POWER_FSM_OPEN_RELAYS
-} MciFsmStateId;
+} PowerFsmStateId;
+
+typedef struct PowerFsmContext {
+  PowerFsmStateId latest_state;
+  PowerFsmStateId target_state;
+} PowerFsmContext;
+
+extern PowerFsmContext power_context;
 
-StatusCode init_power_fsm(void);
+StatusCode init_power_fsm(PowerFsmStateId inital_state);

projects/centre_console/inc/power_fsm_can_data.h

@@ -0,0 +1,49 @@
+#pragma once
+
+// Aux battery status bits
+// https://uwmidsun.atlassian.net/wiki/spaces/ELEC/pages/3149398021/FWXV+Power+Select+Design
+#define AUX_STATUS_BITS 0x04
+#define AUX_FAULT_BITS 0xE0
+
+// PD messages (Todo)
+#define PD_FRONT_FAULT 0x00
+#define PD_REAR_FAULT 0x00
+
+// BMS send power notification (Todo)
+#define SET_BMS_POWER_NOTIFY 0x01
+
+// BPS hearbeat success message (Todo)
+#define BPS_HEARTBEAT 0x00
+
+// Relay setters (Todo)
+#define SET_CLOSE_RELAY_STATE_MASK 0x01
+#define SET_CLOSE_RELAY_STATE_STATE 0x01
+
+// Relay closed status (Todo)
+#define CLOSE_HV_STATUS 0x01
+#define CLOSE_GND_STATUS 0x01
+
+// DCDC status bits
+// https://uwmidsun.atlassian.net/wiki/spaces/ELEC/pages/3149398021/FWXV+Power+Select+Design
+#define DCDC_STATUS_BITS 0x02
+#define DCDC_FAULT_BITS 0x1C
+
+// PD send turn on everything notification (Todo)
+#define SET_TURN_ON_EVERYTHING_NOTIFICATION 0x00
+
+// MCI send ready to drive (Todo)
+#define SET_READY_TO_DRIVE 0x01
+
+// MCI set discharge precharge (Todo)
+#define SET_DISCHARGE_PRECHARGE 0x01
+
+// MCI precharge completed
+#define PRECHARGE_COMPLETED_NOTIFCIATION 0x01
+
+// Relay setters (Todo)
+#define SET_OPEN_RELAY_STATE_MASK 0x00
+#define SET_OPEN_RELAY_STATE_STATE 0x00
+
+// Relay open status (Todo)
+#define OPEN_HV_STATUS 0x01
+#define OPEN_GND_STATUS 0x01

projects/centre_console/inc/power_fsm_sequence.h

@@ -1,46 +1,43 @@
-#ifndef PROJECTS_CENTRE_CONSOLE_INC_POWER_FSM_SEQUENCE_H_
-#define PROJECTS_CENTRE_CONSOLE_INC_POWER_FSM_SEQUENCE_H_
+#pragma once
 
 #include "fsm.h"
 
 // Tell power select to check any aux faults before enabling power
-void prv_power_fsm_confirm_aux_status_input(Fsm *fsm, void *context);
-void prv_power_fsm_confirm_aux_status_output(void *context);
+void power_fsm_confirm_aux_status_output(void *context);
+void power_fsm_confirm_aux_status_input(Fsm *fsm, void *context);
 
 // Tell Power distribution to power on BMS board
-void prv_power_fsm_send_pd_bms_input(Fsm *fsm, void *context);
-void prv_power_fsm_send_pd_bms_output(void *context);
+void power_fsm_send_pd_bms_output(void *context);
+void power_fsm_send_pd_bms_input(Fsm *fsm, void *context);
 
 // Confirms battery checks, waits for ack
-void prv_power_fsm_confirm_battery_status_input(Fsm *fsm, void *context);
-void prv_power_fsm_confirm_battery_status_output(void *context);
+void power_fsm_confirm_battery_status_output(void *context);
+void power_fsm_confirm_battery_status_input(Fsm *fsm, void *context);
 
 // Transmits to BMS to close relays
-void prv_power_fsm_close_battery_relays_input(Fsm *fsm, void *context);
-void prv_power_fsm_close_battery_relays_output(void *context);
+void power_fsm_close_battery_relays_output(void *context);
+void power_fsm_close_battery_relays_input(Fsm *fsm, void *context);
 
 // Power Select confirms DCDC
-void prv_power_fsm_confirm_dc_dc_input(Fsm *fsm, void *context);
-void prv_power_fsm_confirm_dc_dc_output(void *context);
+void power_fsm_confirm_dc_dc_output(void *context);
+void power_fsm_confirm_dc_dc_input(Fsm *fsm, void *context);
 
 // Power Distribution enables all boards
-void prv_power_fsm_turn_on_everything_input(Fsm *fsm, void *context);
-void prv_power_fsm_turn_on_everything_output(void *context);
+void power_fsm_turn_on_everything_output(void *context);
+void power_fsm_turn_on_everything_input(Fsm *fsm, void *context);
 
 // Sends “ready to drive” to MCI
-void prv_power_fsm_power_main_complete_input(Fsm *fsm, void *context);
-void prv_power_fsm_power_main_complete_output(void *context);
+void power_fsm_power_main_complete_output(void *context);
+void power_fsm_power_main_complete_input(Fsm *fsm, void *context);
 
 // Send message to MCI to discharge precharge
-void prv_power_fsm_discharge_precharge_input(Fsm *fsm, void *context);
-void prv_power_fsm_discharge_precharge_output(void *context);
+void power_fsm_discharge_precharge_output(void *context);
+void power_fsm_discharge_precharge_input(Fsm *fsm, void *context);
 
 // Tell Power distribution to turn off the relevant boards
-void prv_power_fsm_turn_off_everything_input(Fsm *fsm, void *context);
-void prv_power_fsm_turn_off_everything_output(void *context);
+void power_fsm_turn_off_everything_output(void *context);
+void power_fsm_turn_off_everything_input(Fsm *fsm, void *context);
 
 // Tell BMS to open the relays
-void prv_power_fsm_open_relays_input(Fsm *fsm, void *context);
-void prv_power_fsm_open_relays_output(void *context);
-
-#endif  // PROJECTS_CENTRE_CONSOLE_INC_POWER_FSM_SEQUENCE_H_
+void power_fsm_open_relays_output(void *context);
+void power_fsm_open_relays_input(Fsm *fsm, void *context);

projects/centre_console/src/main.c

@@ -31,7 +31,7 @@ void run_fast_cycle() {}
 void run_medium_cycle() {
   run_can_rx_cycle();
   fsm_run_cycle(drive);
-  fsm_run_cycle(centre_console_power_fsm);
+  fsm_run_cycle(power);
   wait_tasks(2);
   run_can_tx_cycle();
 }
@@ -47,7 +47,7 @@ int main() {
   fsm_shared_mem_init();
 
   init_drive_fsm();
-  init_power_fsm();
+  init_power_fsm(POWER_FSM_STATE_OFF);
 
   init_master_task();