diff --git a/include/commandFunctions.h b/include/commandFunctions.h index 0020230..da579ca 100644 --- a/include/commandFunctions.h +++ b/include/commandFunctions.h @@ -9,9 +9,9 @@ * functions (located in supportFunctions.cpp) to properally execute */ //This tests the sensors and makes sure they are reading correctly -void testFun(); +void testFun(void* pvParameters); //This function has the satellite do nothing until something is in UART -void standby(); +void standby(void* pvParameters); //This corrdinates the satellite's rotation void orient(const char *direction); diff --git a/include/supportFunctions.h b/include/supportFunctions.h index f1265ff..0f83fb4 100644 --- a/include/supportFunctions.h +++ b/include/supportFunctions.h @@ -6,9 +6,17 @@ /* I tried to make these as self-explanatory as possible by name * but I also added little descriptions anyways -Kristie */ +// TODO: write test tasks for each of the test modes defined in supportFunctions.h + // descriptive names for mode values #define MODE_STANDBY 0 -#define MODE_TEST 1 +#define MODE_TEST 1 /* send heartbeat packet every second, eventually more */ +#define MODE_TEST_HRTBT 2 /* send heartbeat packet every second */ +#define MODE_TEST_MOTION 3 /* start spinning flywheel, stop once rotation speed is above threshold */ +#define MODE_TEST_AD 4 /* calculate and output result of attitude determination */ +#define MODE_TEST_AC 5 /* attempt to spin the satellite a pre-determined amount */ +#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 diff --git a/include/validation_tests.h b/include/validation_tests.h index 9616e19..d1eb9be 100644 --- a/include/validation_tests.h +++ b/include/validation_tests.h @@ -1,14 +1,31 @@ #ifndef VALIDATION_TEST_H #define VALIDATION_TEST_H +/* + * These tasks are defined for testing individual parts of the ADCS system. To enable a specific test mode, set that as the ADCS mode. For list of operation modes + * see supportFunctions.h + */ -#include "DRV_10970.h" -#include "ICM_20948.h" +#include +#include #include #include +#include /* data packet and transmission functions */ +#include /* read from sensors */ +#include /* ADCS operation modes */ extern DRV10970 flywhl; extern ICM_20948_I2C IMU1; +extern QueueHandle_t modeQ; /* state machine publishes mode here */ -void basic_motion1(void* pvParameters); +void create_test_tasks(void); // create all RTOS tasks for testing + +// TEST TASKS ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +static void writeUART(void *pvParameters); // transmit heartbeat every second, active when mode is TEST_MODE +void basic_motion(void* pvParameters); // test the flywheel to make sure it spins, active for MODE_TEST_HRTBT +void basic_heartbeat(void* pvParameters); // test reading sensors and transmitting heartbeat to the main system, MODE_TEST_MOTION +void basic_attitude_determination(void* pvParameters); // test attitude determination, MODE_TEST_AD +void basic_attitude_control(void* pvParameters); // test attitude control, MODE_TEST_AC +void simple_detumble(void* pvParameters); // test basic ability to stop system from spinning, MODE_TEST_SMPLTUMBLE +void simple_orient(void* pvParameters); // test ability to orient the system, MODE_TEST_ORIENT #endif diff --git a/lib/DRV10970/src/DRV_10970.h b/lib/DRV10970/src/DRV_10970.h index c317b03..fee9973 100644 --- a/lib/DRV10970/src/DRV_10970.h +++ b/lib/DRV10970/src/DRV_10970.h @@ -11,13 +11,14 @@ #define DRV_10970_H #include +#include // default pinout for the SAMD51 -const int DRV_FG = 6, // frequency/rpm indication pin - DRV_FR = 9, // motor direction pin - DRV_BRKMOD = 0, // brake mode (coast/brake), not currently available - DRV_PWM = 10, // pwm output pin - DRV_RD = 5; // lock indication pin +const int DRV_FG = FLYWHL_RPM, // frequency/rpm indication pin + DRV_FR = FLYWHL_DIR, // motor direction pin + DRV_BRKMOD = FLYWHL_BRKMOD, // brake mode (coast/brake), not currently available + DRV_PWM = FLYWHL_PWM, // pwm output pin + DRV_RD = FLYWHL_LOCKED; // lock indication pin enum MotorDirection {REV=0, FWD=1}; diff --git a/src/commandFunctions.cpp b/src/commandFunctions.cpp index 14bda04..d45ec93 100644 --- a/src/commandFunctions.cpp +++ b/src/commandFunctions.cpp @@ -11,14 +11,14 @@ extern ICM_20948_I2C IMU2; extern DRV10970* DRV; //This tests the sensors and makes sure they are reading correctly -void testFun() +void testFun(void* pvParameters) { //Call test/test.cpp? return; } //This will loop indefinately until there is something in UART -void standby() +void standby(void* pvParameters) { //Delay then return? //In standby, auto try and detumble? Since this is the first mode... diff --git a/src/main.cpp b/src/main.cpp index f62c900..ff40b09 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,10 +1,11 @@ // Our own headers -#include "comm.h" -#include "sensors.h" +#include "adcs_pin_definitions.h" +#include "comm.h" /* data packets and UART transmission functions */ +#include "sensors.h" /* read from sensors into heartbeat packet */ #include "supportFunctions.h" #include "commandFunctions.h" #include "DRV_10970.h" -#include "validation_tests.h" +#include "validation_tests.h" // tests for functionality // only include this if test functions are desired //#include @@ -16,8 +17,11 @@ // Standard C/C++ library headers #include -// if defined, enables debug print statements over USB to the serial monitor -#define DEBUG +// TESTING DEFINES ////////////////////////////////////////////////////////////// +#define DEBUG /* print the serial debug messages over USB connection */ +#define TESTHEART /* test if can transmit heartbeat */ +//#define TESTMOTION /* test if can spin flywheel */ +///////////////////////////////////////////////////////////////////////////////// /* NON-RTOS GLOBAL VARIABLES ================================================ */ @@ -70,15 +74,15 @@ static void writeUART(void *pvParameters); int main(void) { #ifdef DEBUG - /** - * Initialize USB connection to computer. Used to print debug messages. - * Baud rate: 115200 - * Data bits: 8 - * Parity: none - */ - SERCOM_USB.begin(115200); - while (!SERCOM_USB); // wait for initialization to complete - SERCOM_USB.write("USB interface initialized\r\n"); + /** + * Initialize USB connection to computer. Used to print debug messages. + * Baud rate: 115200 + * Data bits: 8 + * Parity: none + */ + SERCOM_USB.begin(115200); + while (!SERCOM_USB); // wait for initialization to complete + SERCOM_USB.write("USB interface initialized\r\n"); #endif /** @@ -90,7 +94,7 @@ int main(void) SERCOM_UART.begin(115200, SERIAL_8O1); while (!SERCOM_UART); // wait for initialization to complete #ifdef DEBUG - SERCOM_USB.write("UART interface initialized\r\n"); + SERCOM_USB.write("UART interface initialized\r\n"); #endif /** @@ -146,20 +150,26 @@ 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; //while(PM->SLEEPCFG.bit.SLEEPMODE != 0x4); // wait for register to set, cannot sleep while(!PM_INTFLAG_SLEEPRDY); // SLEEPRDY flag must be set before __WFI() + // INIT GPIO // enable LED pinMode(LED_BUILTIN, OUTPUT); digitalWrite(LED_BUILTIN, HIGH); pinMode(9, OUTPUT); - digitalWrite(9, HIGH); + digitalWrite(9, HIGH); // set the direction pin HIGH?? pinMode(10, OUTPUT); - analogWrite(10, 127); + analogWrite(10, 0); // set the PWM pin to 0% + + // INIT INTERRUPTS + // TODO: setup UART interrupt in init_hardware + } @@ -214,21 +224,21 @@ void init_sensors(void){ while (IMU1.status != ICM_20948_Stat_Ok); // wait for initialization to // complete #ifdef DEBUG - SERCOM_USB.write("IMU1 initialized\r\n"); + SERCOM_USB.write("IMU1 initialized\r\n"); #endif #ifdef TWO_IMUS - /** - * Initialize second IMU - * Address: 0x68 or 0x69 - */ - IMU2.begin(SERCOM_I2C, AD0_VAL^1); // initialize other IMU with opposite + /** + * 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 + while (IMU2.status != ICM_20948_Stat_Ok); // wait for initialization to // complete - #ifdef DEBUG - SERCOM_USB.write("IMU2 initialized\r\n"); - #endif + #ifdef DEBUG + SERCOM_USB.write("IMU2 initialized\r\n"); + #endif #endif /** @@ -250,7 +260,7 @@ void init_sensors(void){ ina209.writeCal(0x7fff); #ifdef DEBUG - SERCOM_USB.write("INA209 initialized\r\n"); + SERCOM_USB.write("INA209 initialized\r\n"); #endif } @@ -263,12 +273,18 @@ 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(writeUART, "Write UART", 2048, NULL, 1, NULL); - //xTaskCreate(basic_motion1, "BASIC MOTION TEST", 256, NULL, 1, NULL); + xTaskCreate(readUART, "Read UART", 2048, NULL, 1, NULL); + //xTaskCreate(writeUART, "Write UART", 2048, NULL, 1, NULL); // test function to send heartbeat every half-second + + // TESTS + #ifdef TESTHEART + xTaskCreate(basic_heartbeat, "BASIC HEARTBEAT TEST", 256, NULL, 1, NULL); + #elif TESTMOTION + xTaskCreate(basic_motion, "BASIC MOTION TEST", 256, NULL, 1, NULL); + #endif #ifdef DEBUG - SERCOM_USB.write("Tasks created\r\n"); + SERCOM_USB.write("Tasks created\r\n"); #endif } @@ -295,7 +311,7 @@ static void readUART(void *pvParameters) uint8_t mode; #ifdef DEBUG - char cmd_str[8]; // used to print command value to serial monitor + char cmd_str[8]; // used to print command value to serial monitor #endif while (1) @@ -348,47 +364,3 @@ static void readUART(void *pvParameters) // vTaskDelay(1000 / portTICK_PERIOD_MS); } } - -/** - * @brief - * Reads magnetometer and gyroscope values from IMU and writes them to UART - * every 0.5 seconds while ADCS is in test mode. - * - * @param[in] pvParameters Unused but required by FreeRTOS. Program will not - * compile without this parameter. When a task is instantiated from this - * function, a set of initialization arguments or NULL is passed in as - * pvParameters, so pvParameters must be declared even if it is not used. - * - * @return None - */ -static void writeUART(void *pvParameters) -{ - uint8_t mode; - ADCSdata data_packet; - - #ifdef DEBUG - char mode_str[8]; - #endif - - while (1) - { - xQueuePeek(modeQ, (void*)&mode, (TickType_t)0); - - if (mode == MODE_TEST) - { - data_packet.setStatus(STATUS_OK); - readIMU(data_packet); - readINA(data_packet); - data_packet.computeCRC(); - data_packet.send(); // send to TES - #ifdef DEBUG - // SERCOM_USB.write("Wrote to UART\r\n"); - // printScaledAGMT(&IMU1); - #endif - - data_packet.clear(); - } - - vTaskDelay(1000 / portTICK_PERIOD_MS); - } -} diff --git a/src/supportFunctions.cpp b/src/supportFunctions.cpp index c7192be..36a9591 100644 --- a/src/supportFunctions.cpp +++ b/src/supportFunctions.cpp @@ -12,11 +12,11 @@ void doCmd(char *cmd) { if (cmd[0] == 0xa0) //Binary: 10100000 { - testFun(); + testFun(NULL); } else if (cmd[0] == 0xc0) //Binary: 11000000 { - standby(); + standby(NULL); } //For now, the direction doesn't matter, but it's set up for later else if (cmd[0] == 0xe0) //Binary: 11100000 diff --git a/src/validation_tests.cpp b/src/validation_tests.cpp index 05d646f..a12e26c 100644 --- a/src/validation_tests.cpp +++ b/src/validation_tests.cpp @@ -2,7 +2,53 @@ * These are the tests used to validate the correct operation of our system. */ -#include "validation_tests.h" +#include + +void create_test_tasks(void); // create all RTOS tasks for testing + +/** + * @brief + * Reads magnetometer and gyroscope values from IMU and writes them to UART + * every 0.5 seconds while ADCS is in test mode. + * + * @param[in] pvParameters Unused but required by FreeRTOS. Program will not + * compile without this parameter. When a task is instantiated from this + * function, a set of initialization arguments or NULL is passed in as + * pvParameters, so pvParameters must be declared even if it is not used. + * + * @return None + */ +static void writeUART(void *pvParameters) +{ + uint8_t mode; + ADCSdata data_packet; + + #ifdef DEBUG + char mode_str[8]; + #endif + + while (1) + { + xQueuePeek(modeQ, (void*)&mode, (TickType_t)0); + + if (mode == MODE_TEST) + { + data_packet.setStatus(STATUS_OK); + readIMU(data_packet); + readINA(data_packet); + data_packet.computeCRC(); + data_packet.send(); // send to TES + #ifdef DEBUG + // SERCOM_USB.write("Wrote to UART\r\n"); + // printScaledAGMT(&IMU1); + #endif + + data_packet.clear(); + } + + vTaskDelay(1000 / portTICK_PERIOD_MS); + } +} /* * Assume start from start, then begin increasing flywheel RPM until the system begins to spin. @@ -10,22 +56,134 @@ * 1. check the IMU every loop and if the rotation rate is above MAX_TEST_SPD, stop revving * 2. RPM only go up if some input is received... */ -void basic_motion1(void* pvParameters){ +void basic_motion(void* pvParameters){ + uint8_t mode; const int pwm_sig = 255 * 0.05; // 5% const int MAX_TEST_SPD = 10; const TickType_t FREQ = 1000 / portTICK_PERIOD_MS; while(true){ - float rot_vel_z = IMU1.gyrZ(); + xQueuePeek(modeQ, (void*)&mode, (TickType_t)0); - Serial.write("USB interface initialized\r\n"); + if(mode == MODE_TEST_MOTION){ + float rot_vel_z = IMU1.gyrZ(); - if( rot_vel_z < MAX_TEST_SPD){ - flywhl.run(FWD, pwm_sig); + Serial.write("USB interface initialized\r\n"); + + if( rot_vel_z < MAX_TEST_SPD){ + flywhl.run(FWD, pwm_sig); + } } vTaskDelay(FREQ); } } + + +/* + * Reads all sensors periodically and transmits the data back to the main system via UART. + * 1. Tests the ADCSdata class in comm.h + * 2. Tests reading from sensors and data packet construction + */ +void basic_heartbeat(void* pvParameters){ + uint8_t mode; + ADCSdata dat; + + while(true){ + xQueuePeek(modeQ, (void*)&mode, (TickType_t)0); + + if(mode == MODE_TEST_HRTBT){ + // read all sensors + readIMU(dat); + readINA(dat); // TODO: reading INA may have an issue currently + + // TODO: read sunsensors + + // send data to satellite... + dat.computeCRC(); + dat.send(); + dat.clear(); + } + + vTaskDelay(pdMS_TO_TICKS(1000)); // transmit once per second + } +} + + + +/** + * @brief calculate and output result of attitude determination, MODE_TEST_AD + * + * @param pvParameters The pv parameters + */ +void basic_attitude_determination(void* pvParameters){ + uint8_t mode; + + while(true){ + xQueuePeek(modeQ, &mode, 0); + + if(mode == MODE_TEST_AD){ + // TODO: write the attitude determination test in validation_tests.cpp + // TODO: read IMU + // TODO: use geographic lib to get ideal + // TODO: calculate the vector... + + } + vTaskDelay(pdMS_TO_TICKS(1000)); + } +} + +/** + * @brief attempt to spin the satellite a pre-determined amount, MODE_TEST_AC + * + * @param pvParameters The pv parameters + */ +void basic_attitude_control(void* pvParameters){ + uint8_t mode; + while(true){ + xQueuePeek(modeQ, &mode, 0); + + if(mode == MODE_TEST_AC){ + // TODO: write the attitude control test in validation_tests.cpp + } + + vTaskDelay(pdMS_TO_TICKS(1000)); + } +} + +/** + * @brief test basic ability to stop system from spinning, MODE_TEST_SMPLTUMBLE + + * + * @param pvParameters The pv parameters + */ +void simple_detumble(void* pvParameters){ + uint8_t mode; + while(true){ + xQueuePeek(modeQ, &mode, 0); + + if(mode == MODE_TEST_AD){ + // TODO: write the detumble test in validation_tests.cpp + } + vTaskDelay(pdMS_TO_TICKS(1000)); + } +} + +/** + * @brief test ability to orient the system, MODE_TEST_ORIENT + * + * @param pvParameters The pv parameters + */ +void simple_orient(void* pvParameters){ + uint8_t mode; + while(true){ + xQueuePeek(modeQ, &mode, 0); + + if(mode == MODE_TEST_AD){ + // TODO: write the orient test in validation_tests.cpp + } + vTaskDelay(pdMS_TO_TICKS(1000)); + } +} \ No newline at end of file