Test modes defined/tasks created! Added debug statements for initial …

…command flow check.
Idaho-ADCS · Apr 1, 2022 · ced63dd · ced63dd
1 parent daabf91
commit ced63dd
Show file tree

Hide file tree

Showing 4 changed files with 50 additions and 17 deletions.
diff --git a/include/comm.h b/include/comm.h
@@ -17,10 +17,17 @@
 // command values
 enum Command : uint8_t
 {
-	CMD_DESATURATE 	= 0x00,	   // bring everything to a stop, maybe turn off?
+	CMD_DESATURATE 	= 0x00,	   	// bring everything to a stop, maybe turn off?
 	CMD_STANDBY 	= 0xc0,
-	CMD_TEST    	= 0xa0,
-	CMD_ORIENT_DEFAULT = 0x80, // should be orienting to something like X+
+
+	CMD_TEST    	= 0xa0,		// transmit heartbeat signal regularly
+	CMD_TST_BASIC_MOTION = 0xa1, // test how much force needed to rotate
+	CMD_TST_BASIC_AD = 0xa2,	// test attitude determination
+	CMD_TST_BASIC_AC = 0xa3, 	// test attitude control
+	CMD_TST_SIMPLE_DETUMBLE = 0xa4,	// test simplistic detumble 
+	CMD_TST_SIMPLE_ORIENT 	= 0xa5,	// test simplistic orientation
+
+	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,

diff --git a/include/validation_tests.h b/include/validation_tests.h
@@ -17,7 +17,7 @@ extern DRV10970 flywhl;
 extern ICM_20948_I2C IMU1;
 extern QueueHandle_t modeQ;     /* state machine publishes mode here */
 
-void create_test_tasks(void); // create all RTOS tasks for testing
+void create_test_tasks(); // create all RTOS tasks for testing
 
 // TEST TASKS //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 static void writeUART(void *pvParameters);              // transmit heartbeat every second, active when mode is TEST_MODE

diff --git a/src/main.cpp b/src/main.cpp
@@ -19,8 +19,7 @@
 
 // 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 */
+#define TEST_RTOS 		/* if this is defined, then test tasks will be available */
 /////////////////////////////////////////////////////////////////////////////////
 
 /* NON-RTOS GLOBAL VARIABLES ================================================ */
@@ -281,10 +280,8 @@ void init_rtos_architecture(void){
     //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);
+    #ifdef TEST_RTOS
+    	create_test_tasks(); // if we are in test mode, create the tasks
     #endif
 
 	#ifdef DEBUG
@@ -381,6 +378,16 @@ void state_machine_transition(TEScommand cmand){
 			// do test command stuff
 			break;
 
+		// WARNING: if you switch out of a test mode the test will still be on the stack
+		// 	on the plus side, the task won't run though
+		case CMD_TST_BASIC_MOTION:
+		case CMD_TST_BASIC_AD:
+		case CMD_TST_BASIC_AC:
+		case CMD_TST_SIMPLE_DETUMBLE:
+		case CMD_TST_SIMPLE_ORIENT:
+			create_test_tasks();
+			break;
+
 		case CMD_STANDBY:
 			// print heartbeat regularly turn off actuators
 			if(magnetorquer_on){
@@ -408,6 +415,17 @@ void state_machine_transition(TEScommand cmand){
 	if(command_is_valid){
 		xQueueOverwrite(modeQ, (void*)&mode);  // enter specified mode
 
+		#ifdef 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());
+
+            // print command value to USB
+            SERCOM_USB.print("Command set:");
+            SERCOM_USB.print(cmd_str);
+            SERCOM_USB.print("\r\n");
+        #endif
+
 		// TODO: SM init the new mode, maybe turn off an unneeded actuator? clear some data?
 	}
 

diff --git a/src/validation_tests.cpp b/src/validation_tests.cpp
@@ -8,7 +8,15 @@
  * Create all RTOS tasks for testing
  */
 void create_test_tasks(void){
-    // TODO: validation_tests.cpp -> create rtos test tasks
+    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
+        SERCOM_USB.println("Created test task");
+    #endif
 }
 
 /**
@@ -71,7 +79,7 @@ void basic_motion(void* pvParameters){
     while(true){
         xQueuePeek(modeQ, (void*)&mode, (TickType_t)0);
 
-        if(mode == MODE_TEST_MOTION){
+        if(mode == CMD_TST_BASIC_MOTION){
             float rot_vel_z = IMU1.gyrZ();
 
             //------------------------------------------------------------------
@@ -102,7 +110,7 @@ void basic_heartbeat(void* pvParameters){
     while(true){
         xQueuePeek(modeQ, (void*)&mode, (TickType_t)0);
 
-        if(mode == MODE_TEST_HRTBT){
+        if(mode == 0){
             // read all sensors
             readIMU(dat);
             readINA(dat); // TODO: reading INA may have an issue currently
@@ -132,7 +140,7 @@ void basic_attitude_determination(void* pvParameters){
     while(true){
         xQueuePeek(modeQ, &mode, 0);
 
-        if(mode == MODE_TEST_AD){
+        if(mode == CMD_TST_BASIC_AD){
             // TODO: write the attitude determination test in validation_tests.cpp
             // TODO: read IMU
             // TODO: use geographic lib to get ideal
@@ -153,7 +161,7 @@ void basic_attitude_control(void* pvParameters){
     while(true){
         xQueuePeek(modeQ, &mode, 0);
 
-        if(mode == MODE_TEST_AC){
+        if(mode == CMD_TST_BASIC_AC){
             // TODO: write the attitude control test in validation_tests.cpp
         }
 
@@ -172,7 +180,7 @@ void simple_detumble(void* pvParameters){
     while(true){
         xQueuePeek(modeQ, &mode, 0);
 
-        if(mode == MODE_TEST_AD){
+        if(mode == CMD_TST_SIMPLE_DETUMBLE){
             // TODO: write the detumble test in validation_tests.cpp
         }
         vTaskDelay(pdMS_TO_TICKS(1000));
@@ -189,7 +197,7 @@ void simple_orient(void* pvParameters){
     while(true){
         xQueuePeek(modeQ, &mode, 0);
 
-        if(mode == MODE_TEST_AD){
+        if(mode == CMD_TST_SIMPLE_ORIENT){
             // TODO: write the orient test in validation_tests.cpp
         }
         vTaskDelay(pdMS_TO_TICKS(1000));