#23 - state machine skeleton (#34)

* #23 - state machine skeleton

---------

Co-authored-by: RChandler234 <root@DESKTOP-DEAD2D4>

CM7/Core/Inc/state_machine.h

@@ -0,0 +1,34 @@
+#ifndef STATE_MACHINE_H
+#define STATE_MACHINE_H
+
+#include "cmsis_os.h"
+
+// TODO: add actual list of states
+typedef enum
+{
+    IDLE_START,
+    CHARGE_BOOT_CAP,
+    OFFSET_CALIB,
+    CLEAR,
+    START,
+    SWITCH_OVER,
+    START_RUN,
+    RUN,
+    ANY_STOP,
+    STOP_IDLE,
+    FAULT_NOW,
+    MAX_FUNC_STATES
+} state_t;
+
+extern osThreadId_t sm_director_handle;
+extern const osThreadAttr_t sm_director_attributes;
+
+void vStateMachineDirector(void *pv_params);
+
+/* Adds a functional state transition to be processed */
+int queue_state(state_t new_state);
+
+/* Retrieves the current functional state */
+state_t get_state();
+
+#endif

CM7/Core/Src/state_machine.c

@@ -0,0 +1,63 @@
+#include "state_machine.h"
+#include <stdbool.h>
+#include <stdio.h>
+
+#define STATE_TRANS_QUEUE_SIZE 16
+
+/* Internal State of Vehicle */
+static state_t cerberus_state;
+
+/* State Transition Map */
+// TODO: fill out state transition map
+static const bool valid_trans_to_from[MAX_FUNC_STATES][MAX_FUNC_STATES] = {
+    /*BOOT  READY   DRIVING FAULTED*/
+    {true}};
+
+osThreadId_t sm_director_handle;
+const osThreadAttr_t sm_director_attributes = {
+    .name = "State Machine Director",
+    .stack_size = 128 * 4,
+    .priority = (osPriority_t)osPriorityAboveNormal3,
+};
+
+static osMessageQueueId_t state_trans_queue;
+
+int queue_state(state_t new_state)
+{
+    if (!state_trans_queue)
+        return 1;
+
+    return osMessageQueuePut(state_trans_queue, &new_state, 0U, 0U);
+}
+
+state_t get_state()
+{
+    return cerberus_state;
+}
+
+void vStateMachineDirector(void *pv_params)
+{
+    cerberus_state = IDLE_START;
+
+    state_trans_queue = osMessageQueueNew(STATE_TRANS_QUEUE_SIZE, sizeof(state_t), NULL);
+
+    state_t new_state;
+
+    for (;;)
+    {
+        if (osOK != osMessageQueueGet(state_trans_queue, &new_state, NULL, 50))
+        {
+            // TODO queue fault, low criticality
+            continue;
+        }
+
+        if (!valid_trans_to_from[new_state][cerberus_state])
+        {
+            // TODO queue fault, low criticality
+            continue;
+        }
+
+        // transition state via LUT ?
+        cerberus_state = new_state;
+    }
+}

Makefile/CM7/Makefile

@@ -42,6 +42,7 @@ C_SOURCES =  \
 ../../CM7/Core/Src/stm32h7xx_hal_msp.c \
 ../../CM7/Core/Src/gatedriver.c \
 ../../CM7/Core/Src/foc_ctrl.c \
+../../CM7/Core/Src/state_machine.c \
 ../../CM7/Core/Src/fault.c \
 ../../CM7/Core/Src/ssi_encoder.c \
 ../../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_cortex.c \