From 2de95636725341fc51917d03bf073598604374bf Mon Sep 17 00:00:00 2001 From: Garrett Date: Thu, 31 Mar 2022 15:11:21 -0700 Subject: [PATCH] Added State Machine, removed UART interrupt, added all possible commands --- include/comm.h | 8 ++- lib/DRV10970/src/DRV_10970.h | 2 +- src/main.cpp | 124 ++++++++++++++++++++++++----------- 3 files changed, 94 insertions(+), 40 deletions(-) diff --git a/include/comm.h b/include/comm.h index 2e0be27..b28453d 100644 --- a/include/comm.h +++ b/include/comm.h @@ -17,8 +17,14 @@ // command values enum Command : uint8_t { + CMD_DESATURATE = 0x00, // bring everything to a stop, maybe turn off? CMD_STANDBY = 0xc0, - CMD_TEST = 0xa0 + CMD_TEST = 0xa0, + CMD_ORIENT_DEFAULT = 0x80, // should be orienting to something like X+ + CMD_ORIENT_X_POS= 0xe0, + CMD_ORIENT_Y_POS= 0xe1, + CMD_ORIENT_X_NEG= 0xe2, + CMD_ORIENT_Y_NEG= 0xe3 }; // data packet status codes diff --git a/lib/DRV10970/src/DRV_10970.h b/lib/DRV10970/src/DRV_10970.h index fee9973..4894c1b 100644 --- a/lib/DRV10970/src/DRV_10970.h +++ b/lib/DRV10970/src/DRV_10970.h @@ -11,7 +11,7 @@ #define DRV_10970_H #include -#include +#include // default pinout for the SAMD51 const int DRV_FG = FLYWHL_RPM, // frequency/rpm indication pin diff --git a/src/main.cpp b/src/main.cpp index ff40b09..db3862a 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,5 +1,5 @@ // Our own headers -#include "adcs_pin_definitions.h" +#include "samd51_pin_definitions.h" #include "comm.h" /* data packets and UART transmission functions */ #include "sensors.h" /* read from sensors into heartbeat packet */ #include "supportFunctions.h" @@ -41,28 +41,37 @@ INA209 ina209(0x40); DRV10970 flywhl; /* RTOS GLOBAL VARIABLES ==================================================== */ +bool flywhl_is_spinning = false; // flywheel flag, if set then flywheel is spinning +bool magnetorquer_on = false; // magnetorquer flag, if set then it is on so don't read magnetometer -/** - * @brief - * FreeRTOS queue that stores the current mode of the ADCS. - * Possible values: - * MODE_STANDBY (0) - * MODE_TEST (1) - */ -QueueHandle_t modeQ; +/* ISRs ===================================================================== */ +// none currently -/* HELPER FUNCTIONS ========================================================= */ +/* CORE FUNCTIONS =========================================================== */ +void state_machine_transition(uint8_t command); // takes the new command and changes system mode(modeQ) to reflect that new state +/* HELPER FUNCTIONS ========================================================= */ void init_hardware(void); void init_PWM_50kHz(void); // set PWM output on a pin to 50kHz instead of default void init_sensors(void); void init_rtos_architecture(void); /* RTOS TASK DECLARATIONS =================================================== */ - static void readUART(void *pvParameters); static void writeUART(void *pvParameters); +/* RTOS HANDLES ============================================================= */ +TaskHandle_t* readUART_h; + +/** + * @brief + * FreeRTOS queue that stores the current mode of the ADCS. + * Possible values: + * MODE_STANDBY (0) + * MODE_TEST (1) + */ +QueueHandle_t modeQ; + /* "MAIN" =================================================================== */ /** @@ -166,11 +175,6 @@ void init_hardware(void){ pinMode(10, OUTPUT); analogWrite(10, 0); // set the PWM pin to 0% - - // INIT INTERRUPTS - // TODO: setup UART interrupt in init_hardware - - } /* @@ -273,7 +277,7 @@ void init_rtos_architecture(void){ uint8_t mode = MODE_TEST; xQueueSend(modeQ, (void*)&mode, (TickType_t)0); - xTaskCreate(readUART, "Read UART", 2048, NULL, 1, NULL); + xTaskCreate(readUART, "Read UART", 2048, NULL, 1, readUART_h); //xTaskCreate(writeUART, "Write UART", 2048, NULL, 1, NULL); // test function to send heartbeat every half-second // TESTS @@ -301,8 +305,6 @@ void init_rtos_architecture(void){ * pvParameters, so pvParameters must be declared even if it is not used. * * @return None - * - * TODO: Remove polling and invoke this task using an interrupt instead. */ static void readUART(void *pvParameters) { @@ -326,12 +328,7 @@ static void readUART(void *pvParameters) { if (cmd_packet.checkCRC()) { - // process command if CRC is valid - if (cmd_packet.getCommand() == CMD_TEST) - mode = MODE_TEST; - - if (cmd_packet.getCommand() == CMD_STANDBY) - mode = MODE_STANDBY; + state_machine_transition(cmd_packet); // publish mode, get ready to enter it too } else { @@ -341,26 +338,77 @@ static void readUART(void *pvParameters) response.send(); } - xQueueOverwrite(modeQ, (void*)&mode); // enter specified mode - #ifdef DEBUG - // convert int to string for USB monitoring - sprintf(cmd_str, "0x%02x", cmd_packet.getCommand()); + // convert int to string for USB monitoring + sprintf(cmd_str, "0x%02x", cmd_packet.getCommand()); - // print command value to USB - SERCOM_USB.print("Command received: "); - SERCOM_USB.print(cmd_str); - SERCOM_USB.print("\r\n"); + // print command value to USB + SERCOM_USB.print("Command received: "); + SERCOM_USB.print(cmd_str); + SERCOM_USB.print("\r\n"); - if (cmd_packet.getCommand() == CMD_TEST) - SERCOM_USB.print("Entering test mode\r\n"); + if (cmd_packet.getCommand() == CMD_TEST) + SERCOM_USB.print("Entering test mode\r\n"); - if (cmd_packet.getCommand() == CMD_STANDBY) - SERCOM_USB.print("Entering standby mode\r\n"); + if (cmd_packet.getCommand() == CMD_STANDBY) + SERCOM_USB.print("Entering standby mode\r\n"); #endif } } - // vTaskDelay(1000 / portTICK_PERIOD_MS); + vTaskDelay(1000 / portTICK_PERIOD_MS); } } + +/* + * Handles transition from the current mode (modeQ) to the new mode selected by the input command. + */ +void state_machine_transition(TEScommand cmand){ + // set new state from the command + uint8_t mode = cmand.getCommand(); + uint8_t curr_mode; + // get the current state to compare against + xQueuePeek(modeQ, curr_mode, 0); + // make sure we are entering a new state + if(mode == curr_mode){ // if not, exit + return; + } + + bool command_is_valid = true; + + switch(mode){ + case CMD_TEST: + // do test command stuff + break; + + case CMD_STANDBY: + // print heartbeat regularly turn off actuators + if(magnetorquer_on){ + // TODO: SM turn magnetorquer off + } + if(flywhl_is_spinning){ + // TODO: SM turn off the flywheel + } + break; + + // TODO: SM fill out the other modes with functional code + case ORIENT_DEFAULT: // should be orienting to something like X+ + case ORIENT_X_POS: + case ORIENT_Y_POS: + case ORIENT_X_NEG: + case ORIENT_Y_NEG: + + default: // do nothing + command_is_valid = false; + #ifdef DEBUG + Serial.println("HIT AN UNKNOWN OR UNIMPLEMENTED COMMAND"); + #endif + } + + if(command_is_valid){ + xQueueOverwrite(modeQ, (void*)&mode); // enter specified mode + + // TODO: SM init the new mode, maybe turn off an unneeded actuator? clear some data? + } + +}