From 42da834e763022d2c2025c0269e6b2d25e3b4dba Mon Sep 17 00:00:00 2001 From: Garrett Date: Tue, 5 Apr 2022 09:02:02 -0700 Subject: [PATCH] Control flow test modes run w/o sensors + global config --- include/IMU.cpp | 1 + include/comm.h | 4 +- include/commandFunctions.h | 1 + include/global.h | 28 ++++++++ include/sensors.h | 1 + include/supportFunctions.h | 4 +- include/validation_tests.h | 1 + src/main.cpp | 138 ++++++++++++++++++++----------------- src/sensors.cpp | 59 ++++++++-------- src/validation_tests.cpp | 63 +++++++++++++---- 10 files changed, 194 insertions(+), 106 deletions(-) create mode 100644 include/global.h diff --git a/include/IMU.cpp b/include/IMU.cpp index b682c5d..e9b6b54 100644 --- a/include/IMU.cpp +++ b/include/IMU.cpp @@ -19,6 +19,7 @@ * 3. To print debug data to the serial port there are helper functions in this file. * Make sure you setup Serial first. ***************************************************************/ +#include #include "ICM_20949.h" // Click here to get the library: http://librarymanager/All#SparkFun_ICM_20949_IMU #include #include "wiring_private.h" diff --git a/include/comm.h b/include/comm.h index 2c8ed57..3840ce8 100644 --- a/include/comm.h +++ b/include/comm.h @@ -1,6 +1,7 @@ #ifndef __COMM_H__ #define __COMM_H__ +#include #include "CRC16.h" #include @@ -40,7 +41,8 @@ enum Status : uint8_t STATUS_OK = 0xaa, // "Heartbeat" STATUS_HELLO = 0xaf, // Sent upon system init STATUS_ADCS_ERROR = 0xf0, // Sent upon runtime error - STATUS_COMM_ERROR = 0x99 // Sent upon invalid communication + STATUS_COMM_ERROR = 0x99, // Sent upon invalid communication + STATUS_FUDGED = 0x00 // Data is not real, just test output }; /** diff --git a/include/commandFunctions.h b/include/commandFunctions.h index da579ca..2ca4a07 100644 --- a/include/commandFunctions.h +++ b/include/commandFunctions.h @@ -2,6 +2,7 @@ #define __COMMAND_FUNCTIONS_H__ +#include #include "comm.h" #include "sensors.h" #include "DRV_10970.h" diff --git a/include/global.h b/include/global.h new file mode 100644 index 0000000..536e953 --- /dev/null +++ b/include/global.h @@ -0,0 +1,28 @@ +#ifndef GLOBAL_H +#define GLOBAL_H + +/* + * Contains all preprocessor definitions that should be held(included) commonly across all source and header + * files in the project. + * + * Notable entries will include: + * 1. Sensor configurations (number of IMUs) + * 2. Peripheral interface enables (Serial, I2C, UART) + * 3. Debug/Test defines for test modes + */ + +/* SENSOR CONFIGURATIONS */ +#define TWO_IMUS 0 + +/* PERIPHERAL DEFINITIONS */ +#define SERCOM_USB Serial +#define SERCOM_UART Serial1 +#define SERCOM_I2C Wire + +/* DEBUG/TEST DEFINITIONS */ +#define TEST_CONTROL_FLOW 1 /* if non-zero then assume satellite is connected but no sensors */ +#define RTOS_TEST_SUITE 0 /* if non-zero then rtos test tasks in validation_tests.h will be created */ +#define DEBUG 1 /* if non-zero then debug statements will be printed over USB serial */ + + +#endif \ No newline at end of file diff --git a/include/sensors.h b/include/sensors.h index 13ec7c0..9f40127 100644 --- a/include/sensors.h +++ b/include/sensors.h @@ -1,6 +1,7 @@ #ifndef __SENSORS_H__ #define __SENSORS_H__ +#include #include "comm.h" #include "ICM_20948.h" #include "INA209.h" diff --git a/include/supportFunctions.h b/include/supportFunctions.h index 0f83fb4..9e7c2ce 100644 --- a/include/supportFunctions.h +++ b/include/supportFunctions.h @@ -1,6 +1,8 @@ #ifndef __SUPPORT_FUNCTIONS_H__ #define __SUPPORT_FUNCTIONS_H__ +#include + /* These are functions that support the main command functions in one way * or another they are called upon in the commandFunctions.cpp file */ /* I tried to make these as self-explanatory as possible by name @@ -18,7 +20,7 @@ #define MODE_TEST_SMPLTUMBLE 6 /* starting in spin, attempt to stop spinning */ #define MODE_TEST_ORIENT 7 /* try orienting the adcs system */ -// #define TWO_IMUS +//#define TWO_IMUS //Parses cmd and calls appropriate function void doCmd(char *cmd); diff --git a/include/validation_tests.h b/include/validation_tests.h index d27a0fb..5a17fdf 100644 --- a/include/validation_tests.h +++ b/include/validation_tests.h @@ -5,6 +5,7 @@ * see supportFunctions.h */ +#include #include #include #include diff --git a/src/main.cpp b/src/main.cpp index 6ba8d7a..cb59854 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,4 +1,5 @@ // Our own headers +#include /* definitions of interfaces and test modes */ #include "samd51_pin_definitions.h" #include "comm.h" /* data packets and UART transmission functions */ #include "sensors.h" /* read from sensors into heartbeat packet */ @@ -7,9 +8,6 @@ #include "DRV_10970.h" #include "validation_tests.h" // tests for functionality -// only include this if test functions are desired -//#include - // Arduino library headers #include "INA209.h" #include "ICM_20948.h" @@ -17,11 +15,6 @@ // Standard C/C++ library headers #include -// TESTING DEFINES ////////////////////////////////////////////////////////////// -#define DEBUG /* print the serial debug messages over USB connection */ -#define TEST_RTOS /* if this is defined, then test tasks will be available */ -///////////////////////////////////////////////////////////////////////////////// - /* NON-RTOS GLOBAL VARIABLES ================================================ */ /** @@ -79,9 +72,9 @@ QueueHandle_t modeQ; * setup function as if it were main. Since setup runs only once, it * essentially behaves the same as main. */ -int main(void) +void setup(void) { - #ifdef DEBUG + #if DEBUG /** * Initialize USB connection to computer. Used to print debug messages. * Baud rate: 115200 @@ -101,7 +94,7 @@ int main(void) */ SERCOM_UART.begin(115200, SERIAL_8O1); while (!SERCOM_UART); // wait for initialization to complete - #ifdef DEBUG + #if DEBUG SERCOM_USB.write("UART interface initialized\r\n"); #endif @@ -122,8 +115,9 @@ int main(void) data_packet.setStatus(STATUS_HELLO); data_packet.computeCRC(); data_packet.send(); + - vTaskStartScheduler(); // start the RTOS + vTaskStartScheduler(); // start the RTOS // should never be reached if everything goes right while (1) @@ -133,7 +127,6 @@ int main(void) data_packet.send(); vTaskDelay(1000 / portTICK_PERIOD_MS); } - return 0; } /* @@ -144,9 +137,12 @@ int main(void) */ void loop(){ /* SAMD51 USBCore.cpp sets enable bit during sleep standby mode so USB needs to be disabled if in DEBUG*/ - #ifdef DEBUG - USB->HOST.CTRLA.reg &= ~USB_CTRLA_ENABLE; // disable USB in standby sleep mode... + #if !TEST_CONTROL_FLOW + #if DEBUG + USB->HOST.CTRLA.reg &= ~USB_CTRLA_ENABLE; // disable USB in standby sleep mode... + #endif #endif + // SLEEPRDY bit must be set to sleep if(PM_INTFLAG_SLEEPRDY) __WFI(); // enter low power mode when running Idle task to enter low power mode @@ -157,7 +153,6 @@ void loop(){ /* Initialize all the GPIO pins and the builtin LED */ void init_hardware(void){ - Serial.println("INIT HARDWARE\n"); // INIT SLEEP REG // configure power manager (PM) to enter STANDBY mode on _WFI() PM->SLEEPCFG.bit.SLEEPMODE = 0x4; @@ -174,6 +169,10 @@ void init_hardware(void){ pinMode(10, OUTPUT); analogWrite(10, 0); // set the PWM pin to 0% + + #if DEBUG + SERCOM_USB.println("HARDWARE INITIALIZED"); + #endif } /* @@ -214,56 +213,62 @@ void init_PWM_50kHz(void){ TCC0->CTRLA.bit.ENABLE = 1; // Enable timer TCC0 while (TCC0->SYNCBUSY.bit.ENABLE); // Wait for synchronization + + #if DEBUG + SERCOM_USB.println("PWM INITIALIZED"); + #endif } /* Setup the sensor objects */ void init_sensors(void){ - /** - * Initialize first IMU - * Address: 0x69 or 0x68 - */ - IMU1.begin(SERCOM_I2C, AD0_VAL); - while (IMU1.status != ICM_20948_Stat_Ok); // wait for initialization to - // complete - #ifdef DEBUG - SERCOM_USB.write("IMU1 initialized\r\n"); - #endif - - #ifdef TWO_IMUS + #if !TEST_CONTROL_FLOW /** - * Initialize second IMU - * Address: 0x68 or 0x69 - */ - IMU2.begin(SERCOM_I2C, AD0_VAL^1); // initialize other IMU with opposite - // value for bit 0 - while (IMU2.status != ICM_20948_Stat_Ok); // wait for initialization to - // complete - #ifdef DEBUG - SERCOM_USB.write("IMU2 initialized\r\n"); + * Initialize first IMU + * Address: 0x69 or 0x68 + */ + IMU1.begin(SERCOM_I2C, AD0_VAL); + while (IMU1.status != ICM_20948_Stat_Ok); // wait for initialization to + // complete + #if DEBUG + SERCOM_USB.write("IMU1 initialized\r\n"); #endif + + #if TWO_IMUS + /** + * Initialize second IMU + * Address: 0x68 or 0x69 + */ + IMU2.begin(SERCOM_I2C, AD0_VAL^1); // initialize other IMU with opposite + // value for bit 0 + while (IMU2.status != ICM_20948_Stat_Ok); // wait for initialization to + // complete + #if DEBUG + SERCOM_USB.write("IMU2 initialized\r\n"); + #endif + #endif + + /** + * Write default settings to INA209 + * Reset: no + // * Bus voltage range: 32V + * PGA gain: /8 + * PGA range: +-320mV + * ADC resolution: 12 bits + * ADC conversion time: 532us + * Mode: shunt and bus, continuous + */ + ina209.writeCfgReg(0x399f); + + /** + * Calibrate INA209 + * Current LSB: 100uA + */ + ina209.writeCal(0x7fff); #endif - /** - * Write default settings to INA209 - * Reset: no - // * Bus voltage range: 32V - * PGA gain: /8 - * PGA range: +-320mV - * ADC resolution: 12 bits - * ADC conversion time: 532us - * Mode: shunt and bus, continuous - */ - ina209.writeCfgReg(0x399f); - - /** - * Calibrate INA209 - * Current LSB: 100uA - */ - ina209.writeCal(0x7fff); - - #ifdef DEBUG - SERCOM_USB.write("INA209 initialized\r\n"); + #if DEBUG + SERCOM_USB.println("SENSORS INITIALIZED"); #endif } @@ -278,15 +283,18 @@ void init_rtos_architecture(void){ 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 + #if DEBUG + SERCOM_USB.println("INITIALIZED COMMAND MONITOR"); + #endif // TESTS - #ifdef TEST_RTOS + #if RTOS_TEST_SUITE + #if DEBUG + SERCOM_USB.println("REQUESTED: INITIALIZE RTOS TEST SUITE"); + #endif create_test_tasks(); // if we are in test mode, create the tasks #endif - #ifdef DEBUG - SERCOM_USB.write("Tasks created\r\n"); - #endif } /* RTOS TASK DEFINITIONS ==================================================== */ @@ -309,7 +317,7 @@ static void readUART(void *pvParameters) ADCSdata response; uint8_t mode; - #ifdef DEBUG + #if DEBUG char cmd_str[8]; // used to print command value to serial monitor #endif @@ -335,7 +343,7 @@ static void readUART(void *pvParameters) response.send(); } - #ifdef DEBUG + #if DEBUG // convert int to string for USB monitoring sprintf(cmd_str, "0x%02x", cmd_packet.getCommand()); @@ -407,7 +415,7 @@ void state_machine_transition(TEScommand cmand){ default: // do nothing command_is_valid = false; - #ifdef DEBUG + #if DEBUG Serial.println("HIT AN UNKNOWN OR UNIMPLEMENTED COMMAND"); #endif } @@ -415,7 +423,7 @@ void state_machine_transition(TEScommand cmand){ if(command_is_valid){ xQueueOverwrite(modeQ, (void*)&mode); // enter specified mode - #ifdef DEBUG + #if DEBUG char cmd_str[8]; // used to print command value to serial monitor // convert int to string for USB monitoring sprintf(cmd_str, "0x%02x", cmand.getCommand()); diff --git a/src/sensors.cpp b/src/sensors.cpp index 05154ed..bf099e9 100644 --- a/src/sensors.cpp +++ b/src/sensors.cpp @@ -9,41 +9,46 @@ extern INA209 ina209; void readIMU(ADCSdata &data_packet) { + #if TEST_CONTROL_FLOW + data_packet.setIMUdata(0, 0, 0, 0, 0, 0); // use default values + data_packet.setStatus(STATUS_FUDGED); // tell the satellite this data isn't real + return; // don't do anything else + #endif + ICM_20948_I2C *sensor_ptr1 = &IMU1; // IMU data can only be accessed through // a pointer -#ifdef TWO_IMUS - ICM_20948_I2C *sensor_ptr2 = &IMU2; // IMU data can only be accessed through - // a pointer -#endif + #if TWO_IMUS + ICM_20948_I2C *sensor_ptr2 = &IMU2; // IMU data can only be accessed through a pointer + #endif float mx, my, mz, gx, gy, gz; if (IMU1.dataReady()) IMU1.getAGMT(); // acquires data from sensor -#ifdef TWO_IMUS - if (IMU2.dataReady()) - IMU2.getAGMT(); -#endif - -#ifdef TWO_IMUS - // extract data from IMU object - mx = (sensor_ptr1->magY() + sensor_ptr2->magY()) / 2; - my = (sensor_ptr1->magX() + sensor_ptr2->magX()) / 2; - mz = (sensor_ptr1->magZ() + sensor_ptr2->magZ()) / -2; - - gx = (sensor_ptr1->gyrY() + sensor_ptr2->gyrY()) / 2; - gy = (sensor_ptr1->gyrX() + sensor_ptr2->gyrX()) / 2; - gz = (sensor_ptr1->gyrZ() + sensor_ptr2->gyrZ()) / -2; -#else - mx = sensor_ptr1->magY(); - my = sensor_ptr1->magX(); - mz = sensor_ptr1->magZ() * -1; - - gx = sensor_ptr1->gyrY(); - gy = sensor_ptr1->gyrX(); - gz = sensor_ptr1->gyrZ() * -1; -#endif + #if TWO_IMUS + if (IMU2.dataReady()) + IMU2.getAGMT(); + #endif + + #if TWO_IMUS + // extract data from IMU object + mx = (sensor_ptr1->magY() + sensor_ptr2->magY()) / 2; + my = (sensor_ptr1->magX() + sensor_ptr2->magX()) / 2; + mz = (sensor_ptr1->magZ() + sensor_ptr2->magZ()) / -2; + + gx = (sensor_ptr1->gyrY() + sensor_ptr2->gyrY()) / 2; + gy = (sensor_ptr1->gyrX() + sensor_ptr2->gyrX()) / 2; + gz = (sensor_ptr1->gyrZ() + sensor_ptr2->gyrZ()) / -2; + #else + mx = sensor_ptr1->magY(); + my = sensor_ptr1->magX(); + mz = sensor_ptr1->magZ() * -1; + + gx = sensor_ptr1->gyrY(); + gy = sensor_ptr1->gyrX(); + gz = sensor_ptr1->gyrZ() * -1; + #endif data_packet.setIMUdata(mx, my, mz, gx, gy, gz); } diff --git a/src/validation_tests.cpp b/src/validation_tests.cpp index c5b01c4..01d1f30 100644 --- a/src/validation_tests.cpp +++ b/src/validation_tests.cpp @@ -15,7 +15,7 @@ void create_test_tasks(void){ xTaskCreate(simple_orient, "SIMPLE ORIENT", 256, NULL, 1, NULL); #ifdef DEBUG - SERCOM_USB.println("Created test task"); + SERCOM_USB.println("\tINITIALIZED RTOS TEST SUITE"); #endif } @@ -52,8 +52,8 @@ static void writeUART(void *pvParameters) data_packet.computeCRC(); data_packet.send(); // send to TES #ifdef DEBUG - // SERCOM_USB.write("Wrote to UART\r\n"); - // printScaledAGMT(&IMU1); + SERCOM_USB.write("[writeUART] wrote to UART\r\n"); + //printScaledAGMT(&IMU1); #endif data_packet.clear(); @@ -71,24 +71,47 @@ static void writeUART(void *pvParameters) */ void basic_motion(void* pvParameters){ uint8_t mode; - const int pwm_sig = 255 * 0.05; // 5% - const int MAX_TEST_SPD = 10; + int multiplier = 0.00; + int pwm_sig = 255 * multiplier; // 0% + const int MAX_TEST_SPD = 10; // upper limit is 10 degrees per second/1.667 rpm - const TickType_t FREQ = 1000 / portTICK_PERIOD_MS; + const TickType_t FREQ = 2000 / portTICK_PERIOD_MS; while(true){ + #if DEBUG + SERCOM_USB.println("[BASIC MOTION] checked mode"); + #endif + xQueuePeek(modeQ, (void*)&mode, (TickType_t)0); if(mode == CMD_TST_BASIC_MOTION){ float rot_vel_z = IMU1.gyrZ(); - //------------------------------------------------------------------ - /// @brief This class describes a write. - /// - Serial.write("USB interface initialized\r\n"); - - if( rot_vel_z < MAX_TEST_SPD){ + if( rot_vel_z < MAX_TEST_SPD && multiplier < 1 ){ // as long as we are spinning slower than our goal, continue + pwm_sig = 255 * multiplier; flywhl.run(FWD, pwm_sig); + + if(multiplier < 1){ multiplier += 0.01; } + #if DEBUG + else{ + Serial.println("[BASIC MOTION] multiplier hit ceiling"); + } + #endif + }else{ // stop the test + + flywhl.stop(); + + #if DEBUG + if( rot_vel_z > MAX_TEST_SPD ){ + SERCOM_USB.print("[BASIC MOTION] rotational velocity greater than MAX_TEST_SPD ( "); + SERCOM_USB.print(MAX_TEST_SPD); + SERCOM_USB.println(" deg/s )"); + }else if (multiplier >= 1){ + SERCOM_USB.print("[BASIC MOTION] multiplier hit ceiling but velocity still "); + SERCOM_USB.print(rot_vel_z); + SERCOM_USB.println(" deg/s"); + } + #endif } } @@ -108,6 +131,9 @@ void basic_heartbeat(void* pvParameters){ ADCSdata dat; while(true){ + #if DEBUG + SERCOM_USB.println("[BASIC HEARTBEAT] checked mode"); + #endif xQueuePeek(modeQ, (void*)&mode, (TickType_t)0); if(mode == 0){ @@ -138,6 +164,9 @@ void basic_attitude_determination(void* pvParameters){ uint8_t mode; while(true){ + #if DEBUG + SERCOM_USB.println("[BASIC AD] checked mode"); + #endif xQueuePeek(modeQ, &mode, 0); if(mode == CMD_TST_BASIC_AD){ @@ -159,6 +188,9 @@ void basic_attitude_determination(void* pvParameters){ void basic_attitude_control(void* pvParameters){ uint8_t mode; while(true){ + #if DEBUG + SERCOM_USB.println("[BASIC AC] checked mode"); + #endif xQueuePeek(modeQ, &mode, 0); if(mode == CMD_TST_BASIC_AC){ @@ -178,6 +210,9 @@ void basic_attitude_control(void* pvParameters){ void simple_detumble(void* pvParameters){ uint8_t mode; while(true){ + #if DEBUG + SERCOM_USB.println("[BASIC DETUMBLE] checked mode"); + #endif xQueuePeek(modeQ, &mode, 0); if(mode == CMD_TST_SIMPLE_DETUMBLE){ @@ -195,6 +230,10 @@ void simple_detumble(void* pvParameters){ void simple_orient(void* pvParameters){ uint8_t mode; while(true){ + #if DEBUG + SERCOM_USB.println("[BASIC ORIENT] checked mode"); + #endif + xQueuePeek(modeQ, &mode, 0); if(mode == CMD_TST_SIMPLE_ORIENT){