Merge pull request #73 from waterloo-rocketry/traj/control-rework

Remove extQueue, modify controller extension
waterloo-rocketry · Jul 24, 2024 · b64a625 · b64a625
2 parents a9aa370 + aa14b35
commit b64a625
Show file tree

Hide file tree

Showing 3 changed files with 8 additions and 13 deletions.
diff --git a/STM32Cube/Tasks/controller.c b/STM32Cube/Tasks/controller.c
@@ -34,7 +34,6 @@ void controlTask(void *argument)
 	airbrakesController.target_altitude = 7000;
 	float last_ms = millis_();
 	float initial_extension = 0;
-	xQueueOverwrite(extQueue, &initial_extension); //put a valid element in the queue so that trajectory prediction can actually run
 
   /* Infinite loop */
 	for(;;)
@@ -65,14 +64,14 @@ void controlTask(void *argument)
 			airbrakesController.last_error = airbrakesController.error;
 
 			float output = airbrakesController.controller_term_P + airbrakesController.controller_term_I - airbrakesController.controller_term_D;
-			float extension = 1.0 - output; //invert the controller output
+			float extension = 0.5 - output; //invert the controller output - if we are undershooting retract, and if we are overshooting, extend
 
 			if(extension > CONTROLLER_MAX_EXTENSION) extension = CONTROLLER_MAX_EXTENSION;
 			if(extension < CONTROLLER_MIN_EXTENSION) extension = CONTROLLER_MIN_EXTENSION;
 
-			//printf_("extension: %f\n", extension);
+			printf_("extension: %f\n", extension);
 			logInfo("controller", "ext: %d", extension * 100);
-			xQueueOverwrite(extQueue, &extension); //make the new extension value available to trajectory prediction
+
 			if(extensionAllowed())
 			{
 				can_msg_t msg;

diff --git a/STM32Cube/Tasks/trajectory.c b/STM32Cube/Tasks/trajectory.c
@@ -6,6 +6,7 @@
  */
 
 #include <math.h>
+#include <limits.h>
 
 #include "Fusion.h"
 
@@ -25,7 +26,6 @@
 
 xQueueHandle altQueue;
 xQueueHandle angleQueue;
-xQueueHandle extQueue;
 
 float rocket_area(float extension);
 float velocity_derivative(float force, float mass);
@@ -272,17 +272,16 @@ Otits_Result_t test_apogeeQueue() {
 
 void trajectory_task(void * argument){    
     float prev_time = -1;
-    uint16_t prev_alt = 0xFFFF;
+    int32_t prev_alt = INT_MAX;
 
     for(;;)
     {
         AltTime altTime;
         FusionEuler angles;
-        float ext;
+        const float ext = 0.5;
         if(xQueueReceive(altQueue, &altTime, 10) == pdTRUE) {
-            if(xQueuePeek(extQueue, &ext, 10)== pdTRUE) {
                 if(xQueuePeek(angleQueue, &angles, 100) == pdTRUE) {
-                    if(prev_alt != 0xFFFF) {
+                    if(prev_alt != INT_MAX) {
                         float vely = (altTime.alt-prev_alt)*1000.0/(altTime.time-prev_time);
                         float velx = vely*tan(angles.angle.pitch);
                         float apogee = get_max_altitude(vely,velx, altTime.alt, ext, ROCKET_BURNOUT_MASS);
@@ -292,15 +291,13 @@ void trajectory_task(void * argument){
                     prev_alt = altTime.alt;
                     prev_time = altTime.time;
                 }
-            }
 
         }
-      //  vTaskDelay(20); // TODO: for testing so this blocks
+        vTaskDelay(20); // TODO: for testing so this blocks
     }
 }
 void trajectory_init(){
     altQueue = xQueueCreate(1, sizeof(AltTime));
     angleQueue = xQueueCreate(1, sizeof(FusionEuler));
-    extQueue = xQueueCreate(1, sizeof(float));
     otitsRegister(test_apogeeQueue, TEST_SOURCE_TRAJ, "apogeeQ");
 }
diff --git a/STM32Cube/Tasks/trajectory.h b/STM32Cube/Tasks/trajectory.h
@@ -13,7 +13,6 @@
 
 extern xQueueHandle altQueue;
 extern xQueueHandle angleQueue;
-extern xQueueHandle extQueue;
 
 void trajectory_task(void * argument);
 void trajectory_init(void);