diff --git a/include/comm.h b/include/comm.h index 7b9c626..f789ff9 100644 --- a/include/comm.h +++ b/include/comm.h @@ -13,7 +13,7 @@ // packet sizes in bytes #define COMMAND_LEN 4 -#define PACKET_LEN 12 +#define PACKET_LEN 14 // command values enum Command : uint8_t @@ -94,6 +94,7 @@ class TEScommand * */ void addByte(uint8_t b); + void copyBytes(uint8_t *bytes); // add this line bool isFull(); uint8_t getCommand(); bool checkCRC(); @@ -113,7 +114,7 @@ class ADCSdata // Data can be accessed as fields - used to build packet uint8_t _status; fixed5_3_t _voltage; - int8_t _current; + int16_t _current; uint8_t _speed; int8_t _magX; int8_t _magY; diff --git a/include/global.h b/include/global.h index 861e6d2..649961c 100644 --- a/include/global.h +++ b/include/global.h @@ -22,7 +22,7 @@ #define SERCOM_I2C Wire /* DEBUG/TEST DEFINITIONS */ -#define TEST_CONTROL_FLOW 1 /* if non-zero then assume satellite is connected but no sensors */ +#define TEST_CONTROL_FLOW 0 /* 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 */ @@ -38,29 +38,31 @@ static void print_global_config(void){ // test modes SERCOM_USB.print("DEBUG ["); if(DEBUG){ - SERCOM_USB.print("ON]"); + SERCOM_USB.println("ON]"); }else{ - SERCOM_USB.print("OFF]"); + SERCOM_USB.println("OFF]"); } SERCOM_USB.print("TEST CONTROL FLOW ["); if(TEST_CONTROL_FLOW){ - SERCOM_USB.print("ON]"); + SERCOM_USB.println("ON]"); }else{ - SERCOM_USB.print("OFF]"); + SERCOM_USB.println("OFF]"); } SERCOM_USB.print("RTOS_TEST_SUITE ["); if(RTOS_TEST_SUITE){ - SERCOM_USB.print("ON]"); + SERCOM_USB.println("ON]"); }else{ - SERCOM_USB.print("OFF]"); + SERCOM_USB.println("OFF]"); } // two imus SERCOM_USB.print("TWO_IMUS ["); - if(SERCOM_USB){ - SERCOM_USB.print("ON]"); + if(TWO_IMUS){ + SERCOM_USB.println("ON]"); + }else{ + SERCOM_USB.println("OFF]"); } SERCOM_USB.println(); } diff --git a/src/comm.cpp b/src/comm.cpp index 276c6e7..a0560ed 100644 --- a/src/comm.cpp +++ b/src/comm.cpp @@ -23,6 +23,14 @@ void TEScommand::addByte(uint8_t b) } } +void TEScommand::copyBytes(uint8_t *bytes) +{ + for (int i = 0; i < COMMAND_LEN; i++) + { + _data[i] = bytes[i]; + } +} + bool TEScommand::isFull() { return _full; diff --git a/src/main.cpp b/src/main.cpp index c2edc5e..0425144 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -75,6 +75,8 @@ QueueHandle_t modeQ; */ void setup(void) { + init_hardware(); // setup gpio pins + #if DEBUG /** * Initialize USB connection to computer. Used to print debug messages. @@ -96,6 +98,7 @@ void setup(void) */ SERCOM_UART.begin(115200, SERIAL_8O1); while (!SERCOM_UART); // wait for initialization to complete + SERCOM_UART.setTimeout(10); #if DEBUG SERCOM_USB.write("UART interface initialized\r\n"); #endif @@ -107,7 +110,6 @@ void setup(void) SERCOM_I2C.begin(); SERCOM_I2C.setClock(400000); - init_hardware(); // setup gpio pins //init_PWM_50kHz(); init_sensors(); // init the sensor interfaces init_rtos_architecture(); // create rtos tasks and stuff @@ -119,7 +121,7 @@ void setup(void) data_packet.send(); - vTaskStartScheduler(); // start the RTOS + vTaskStartScheduler(); // start the RTOS // should never be reached if everything goes right while (1) @@ -141,7 +143,8 @@ void loop(){ /* SAMD51 USBCore.cpp sets enable bit during sleep standby mode so USB needs to be disabled if in DEBUG*/ #if !TEST_CONTROL_FLOW #if DEBUG - USB->HOST.CTRLA.reg &= ~USB_CTRLA_ENABLE; // disable USB in standby sleep mode... + // TODO: UNCOMMENT THIS SO SLEEP MODE IS ENTERED IN IDLE TASK + //USB->HOST.CTRLA.reg &= ~USB_CTRLA_ENABLE; // disable USB in standby sleep mode... #endif #endif @@ -172,9 +175,9 @@ void init_hardware(void){ pinMode(10, OUTPUT); analogWrite(10, 0); // set the PWM pin to 0% - #if DEBUG - SERCOM_USB.println("HARDWARE INITIALIZED"); - #endif + //#if DEBUG + // SERCOM_USB.println("HARDWARE INITIALIZED"); + //#endif } /* @@ -283,8 +286,8 @@ void init_rtos_architecture(void){ uint8_t mode = MODE_TEST; xQueueSend(modeQ, (void*)&mode, (TickType_t)0); - xTaskCreate(readUART, "Read UART", 2048, NULL, 1, readUART_h); - xTaskCreate(heartbeat, "HEARTBEAT", 2048, NULL, 1, NULL); // test function to send heartbeat every half-second + xTaskCreate(readUART, "Read UART", 256, NULL, 2, readUART_h); + // TODO: xTaskCreate(heartbeat, "HEARTBEAT", 256, NULL, 1, NULL); // test function to send heartbeat every half-second #if DEBUG SERCOM_USB.println("INITIALIZED COMMAND MONITOR"); @@ -295,7 +298,7 @@ void init_rtos_architecture(void){ #if DEBUG SERCOM_USB.println("REQUESTED: INITIALIZE RTOS TEST SUITE"); #endif - create_test_tasks(); // if we are in test mode, create the tasks + // TODO: create_test_tasks(); // if we are in test mode, create the tasks #endif } @@ -319,22 +322,57 @@ static void readUART(void *pvParameters) TEScommand cmd_packet; ADCSdata response; uint8_t mode; + uint8_t rx_buf[COMMAND_LEN]; + int rx_bytes; #if DEBUG - char cmd_str[8]; // used to print command value to serial monitor + char debug_str[8]; // used to print command value to serial monitor #endif while (1) { - if (SERCOM_UART.available()) // at least one byte is in the UART - { // receive buffer + //SERCOM_USB.println("READ UART ALIVE"); + #if DEBUG + + SERCOM_USB.println("[READ UART] checked uart buffer"); + if(!SERCOM_UART){ + SERCOM_USB.println("\tUART IS DEAD"); + } + + #endif - // copy one byte out of UART receive buffer - cmd_packet.addByte((uint8_t)SERCOM_UART.read()); + int rx_len = SERCOM_UART.available(); - if (cmd_packet.isFull()) // full command packet received + if (rx_len > 0) // at least one byte is in the UART + { // receive buffer + + #if DEBUG + sprintf(debug_str, "%d", rx_len); + SERCOM_USB.write("[command rx]\tDetected "); + SERCOM_USB.write(debug_str); + SERCOM_USB.write(" bytes in UART rx buffer\r\n"); + #endif + + rx_bytes = SERCOM_UART.readBytes(rx_buf, COMMAND_LEN); + + #ifdef DEBUG + sprintf(debug_str, "%d", rx_bytes); + SERCOM_USB.write("[command rx]\tReceived "); + SERCOM_USB.write(debug_str); + SERCOM_USB.write(" bytes:"); + for (int i = 0; i < rx_bytes; i++) + { + sprintf(debug_str, " %02x", rx_buf[i]); + SERCOM_USB.write(debug_str); + } + SERCOM_USB.write("\r\n"); + #endif + + if (rx_bytes == COMMAND_LEN) // full command packet received { - if (cmd_packet.checkCRC()) + cmd_packet.copyBytes(rx_buf); + state_machine_transition(cmd_packet); // publish mode, get ready to enter it too + /*if (cmd_packet.checkCRC()) { state_machine_transition(cmd_packet); // publish mode, get ready to enter it too } @@ -344,23 +382,8 @@ static void readUART(void *pvParameters) response.setStatus(STATUS_COMM_ERROR); response.computeCRC(); response.send(); - } - - #if DEBUG - // 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"); - - if (cmd_packet.getCommand() == CMD_HEARTBEAT) - SERCOM_USB.print("Entering heartbeat only mode\r\n"); - - if (cmd_packet.getCommand() == CMD_STANDBY) - SERCOM_USB.print("Entering standby mode\r\n"); - #endif } } @@ -391,6 +414,10 @@ static void heartbeat(void *pvParameters) while (1) { + #if DEBUG + SERCOM_USB.println("[HEARTBEAT] checked mode"); + #endif + xQueuePeek(modeQ, (void*)&mode, (TickType_t)0); if (mode == CMD_HEARTBEAT) @@ -400,9 +427,9 @@ static void heartbeat(void *pvParameters) readINA(data_packet); data_packet.computeCRC(); data_packet.send(); // send to TES - #ifdef DEBUG + #if DEBUG SERCOM_USB.write("[HEARTBEAT] wrote to UART\r\n"); - //printScaledAGMT(&IMU1); + if(!TEST_CONTROL_FLOW) printScaledAGMT(&IMU1); #endif data_packet.clear(); @@ -420,8 +447,9 @@ void state_machine_transition(TEScommand cmand){ uint8_t mode = cmand.getCommand(); uint8_t curr_mode = CMD_STANDBY; // get the current state to compare against - xQueuePeek(modeQ, (void*)curr_mode, 0); + xQueuePeek(modeQ, &curr_mode, 0); // make sure we are entering a new state + if(mode == curr_mode){ // if not, exit return; } @@ -440,7 +468,9 @@ void state_machine_transition(TEScommand cmand){ case CMD_TST_BASIC_AC: case CMD_TST_SIMPLE_DETUMBLE: case CMD_TST_SIMPLE_ORIENT: - create_test_tasks(); + #if RTOS_TEST_SUITE + create_test_tasks(); + #endif break; case CMD_STANDBY: diff --git a/src/sensors.cpp b/src/sensors.cpp index bf099e9..76c06da 100644 --- a/src/sensors.cpp +++ b/src/sensors.cpp @@ -68,16 +68,6 @@ void readINA(ADCSdata &data_packet) current = i_raw / 10; data_packet.setINAdata(voltage, current); - - char debug_str[32]; - sprintf(debug_str, "%d", v_raw); - SERCOM_USB.write("Bus voltage: "); - SERCOM_USB.write(debug_str); - SERCOM_USB.write(" mV\r\n"); - sprintf(debug_str, "%d", i_raw*100); - SERCOM_USB.write("Shunt current: "); - SERCOM_USB.write(debug_str); - SERCOM_USB.write(" uA\r\n"); } void printPaddedInt16b(int16_t val) diff --git a/src/validation_tests.cpp b/src/validation_tests.cpp index 7e60512..64b0e9c 100644 --- a/src/validation_tests.cpp +++ b/src/validation_tests.cpp @@ -8,13 +8,17 @@ * Create all RTOS tasks for testing */ void create_test_tasks(void){ + #if DEBUG + SERCOM_USB.println("\tINITIALIZING RTOS TEST SUITE"); + #endif + xTaskCreate(basic_motion, "BASIC MOTION", 256, NULL, 1, NULL); xTaskCreate(basic_attitude_determination, "BASIC AD", 256, NULL, 1, NULL); xTaskCreate(basic_attitude_control, "BASIC AC", 256, NULL, 1, NULL); xTaskCreate(simple_detumble, "SIMPLE DETUMBLE", 256, NULL, 1, NULL); xTaskCreate(simple_orient, "SIMPLE ORIENT", 256, NULL, 1, NULL); - - #ifdef DEBUG + + #if DEBUG SERCOM_USB.println("\tINITIALIZED RTOS TEST SUITE"); #endif } @@ -150,8 +154,20 @@ void basic_attitude_control(void* pvParameters){ */ void simple_detumble(void* pvParameters){ - uint8_t mode; - uint8_t pwm = 10; + uint8_t mode; // last received ADCS mode + + const int target_rot_vel = 0; // rotational velocity we want to maintain + const int step_size = 1; // minimum step size to take when error is present + int num_steps = 0; + + const float P = 0.25; // proportional component + int I = 0; // integral component + int D = 0; // derivative component + + int prev_error = 0; // error from last loop iteration + int error = 0; // assumed stationary at start + + int pwm_output = 0; // init at zero, signed to represent direction while(true){ #if DEBUG @@ -160,14 +176,56 @@ void simple_detumble(void* pvParameters){ xQueuePeek(modeQ, &mode, 0); if(mode == CMD_TST_SIMPLE_DETUMBLE){ + // calculate error + ICM_20948_I2C *sensor_ptr1 = &IMU1; // IMU data can only be accessed through + if (IMU1.dataReady()) + IMU1.getAGMT(); // acquires data from sensor + float rot_vel_z = sensor_ptr1->gyrZ(); + + error = rot_vel_z - target_rot_vel; // difference between current state and target state + // proportional term calculation + float p_term = error * P; + // integral term accumulation + I += error; + int i_term = I; + // derivative term + int d_term = I/num_steps; + // output + pwm_output = p_term; //+ i_term + d_term; + if(pwm_output > 255){ // cap output + pwm_output = 255; + } - float rot_vel_z = IMU1.gyrZ(); - if(rot_vel_z > 0){ // spinning clockwise - flywhl.run(REV, pwm); + num_steps++; - }else if(rot_vel_z < 0){ // spinning counter-clockwise - flywhl.run(FWD, pwm); + // unsigned pwm to motor with direction + if(error > 0){ + flywhl.run(REV, pwm_output); + + }else if(error < 0){ + flywhl.run(FWD, pwm_output); + + }else{ + // TODO: desaturate with magnetorquers if |pwm_output| == 255 or in state of equilibrium + // for the meantime, just keep doing the same thing + } + + if(DEBUG){ + SERCOM_USB.println("====== PID LOOP ======"); + SERCOM_USB.print("IMU VELOCITY = "); + SERCOM_USB.print(rot_vel_z); + SERCOM_USB.println(" degrees/sec"); + + SERCOM_USB.print("ERROR = "); + SERCOM_USB.println(error); + + SERCOM_USB.print("PWM OUTPUT = "); + SERCOM_USB.println(pwm_output); + SERCOM_USB.println("======================"); } + + prev_error = error; + } vTaskDelay(pdMS_TO_TICKS(1000)); }