tested; added bounds checking on D and I terms

STM32Cube/Core/Src/main.c

@@ -28,7 +28,6 @@
 #include "millis.h"
 #include "ICM-20948.h"
 
-#include "vn_handler.h"
 #include "log.h"
 #include "controller.h"
 #include "flight_phase.h"
@@ -38,6 +37,7 @@
 #include "can_handler.h"
 #include "otits.h"
 #include "sdmmc.h"
+#include "vn_handler.h"
 /* USER CODE END Includes */
 
 /* Private typedef -----------------------------------------------------------*/
@@ -118,6 +118,7 @@ static void MX_TIM2_Init(void);
 void StartDefaultTask(void *argument);
 
 /* USER CODE BEGIN PFP */
+//void HAL_FDCAN_RxFifo1Callback(FDCAN_HandleTypeDef *hfdcan, uint32_t RxFifo0ITs);
 /* USER CODE END PFP */
 
 /* Private user code ---------------------------------------------------------*/
@@ -226,12 +227,12 @@ int main(void)
   BaseType_t xReturned = pdPASS;
 
   //dunno if casting from CMSIS priorities is valid
-  //xReturned &= xTaskCreate(vnIMUHandler, "VN Task", 2000, NULL, (UBaseType_t) osPriorityNormal, &VNTaskHandle);
+  xReturned &= xTaskCreate(vnIMUHandler, "VN Task", 2000, NULL, (UBaseType_t) osPriorityNormal, &VNTaskHandle);
   xReturned &= xTaskCreate(canHandlerTask, "CAN handler", 2000, NULL, (UBaseType_t) osPriorityNormal, &canhandlerhandle);
-  //xReturned &= xTaskCreate(stateEstTask, "StateEst", 512, NULL, (UBaseType_t) osPriorityNormal, &stateEstTaskHandle);
+  xReturned &= xTaskCreate(stateEstTask, "StateEst", 512, NULL, (UBaseType_t) osPriorityNormal, &stateEstTaskHandle);
   xReturned &= xTaskCreate(trajectory_task, "traj", 512, NULL, (UBaseType_t) osPriorityNormal, &trajectoryTaskHandle);
   xReturned &= xTaskCreate(logTask, "Logging", 1024, NULL, (UBaseType_t) osPriorityNormal, &logTaskhandle);
-  //xReturned &= xTaskCreate(healthCheckTask, "health checks", 512, NULL, (UBaseType_t) osPriorityNormal, &healthChecksTaskHandle);
+  xReturned &= xTaskCreate(healthCheckTask, "health checks", 512, NULL, (UBaseType_t) osPriorityNormal, &healthChecksTaskHandle);
   xReturned &= xTaskCreate(controlTask, "Controller", 2000, NULL, (UBaseType_t) osPriorityBelowNormal, &controllerHandle);
   xReturned &= xTaskCreate(flightPhaseTask, "Flight Phase", 2000, NULL, (UBaseType_t) osPriorityAboveNormal, &controllerHandle);
 #ifdef TEST_MODE
@@ -1022,19 +1023,22 @@ void StartDefaultTask(void *argument)
 {
   /* USER CODE BEGIN 5 */
 	/* Infinite loop */
+//	uint32_t index = 0;
+//	uint8_t temp_buf[1];
+//	uint8_t rx_buf[100] = {0};
+//	int isSizeRxed = 0;
+//	uint16_t size = 0;
+
+	//HAL_UART_Receive_DMA(&huart4, rx_buf, 1);
 	for(;;)
 	{
-		//char buffer[] = "hello world!\r\n";
+		char buffer[] = "hello world!\r\n";
 		//printf_(buffer);
-    float time = millis_();
-    uint32_t alt =  0.0292*time*time*time - 6.7446*time*time + 315.51*time + 5400.9;
-    AltTime altStruct = {alt, time};
-    FusionEuler angles = {0.2, 0.2, 0.2}; //Need Rad ~ 11.5 deg
-    xQueueOverwrite(angleQueue, &angles);
-    xQueueSend(altQueue, &altStruct, 10);
-
-		//HAL_GPIO_TogglePin(GPIOB, GPIO_PIN_12);
-		osDelay(100);
+		//logDebug(0, "test messsssage");
+		HAL_GPIO_TogglePin(GPIOB, GPIO_PIN_12);
+		//printf_("hello world\n");
+		osDelay(1000);
+
 	}
   /* USER CODE END 5 */
 }

STM32Cube/Tasks/controller.c

@@ -27,7 +27,8 @@ void controlTask(void *argument)
 {
 	float apogeeEstimate;
 	controller_t airbrakesController;
-	airbrakesController.target_altitude = 9000;
+	airbrakesController.last_error = 0;
+	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
@@ -41,8 +42,14 @@ void controlTask(void *argument)
 			airbrakesController.error = airbrakesController.target_altitude - apogeeEstimate;
 			airbrakesController.controller_term_P = airbrakesController.error * CONTROLLER_GAIN_P;
 			float dt = (millis_() - last_ms) / 1000.0; //time delay in s
-			airbrakesController.controller_term_I = airbrakesController.controller_term_I + CONTROLLER_GAIN_I * airbrakesController.error * dt; //Add some time measure here
-			airbrakesController.controller_term_D = CONTROLLER_GAIN_D * (airbrakesController.error - airbrakesController.last_error)/ dt; //here too
+			airbrakesController.controller_term_I = airbrakesController.controller_term_I + CONTROLLER_GAIN_I * airbrakesController.error * dt;
+			if(airbrakesController.controller_term_I > CONTROLLER_I_SATMAX) airbrakesController.controller_term_I = CONTROLLER_I_SATMAX;
+			if(airbrakesController.controller_term_I < -CONTROLLER_I_SATMAX) airbrakesController.controller_term_I = -CONTROLLER_I_SATMAX;
+
+			//prevent divide by 0 errors
+			if(dt < 0.0000001) airbrakesController.controller_term_D = 0;
+			else airbrakesController.controller_term_D = CONTROLLER_GAIN_D * (airbrakesController.error - airbrakesController.last_error) / dt;
+
 			last_ms = millis_();
 			airbrakesController.last_error = airbrakesController.error;
 
@@ -52,15 +59,15 @@ void controlTask(void *argument)
 			if(extension > CONTROLLER_MAX_EXTENSION) extension = CONTROLLER_MAX_EXTENSION;
 			if(extension < CONTROLLER_MIN_EXTENSION) extension = CONTROLLER_MIN_EXTENSION;
 
+			printf_("extension: %f\n", extension);
 			xQueueOverwrite(extQueue, &extension); //make the new extension value available to trajectory prediction
-
 			if(extensionAllowed())
 			{
 				can_msg_t msg;
 				build_actuator_cmd_analog( (uint32_t) millis_(), ACTUATOR_AIRBRAKES_SERVO, (uint8_t) (100 * extension), &msg);
 				xQueueSend(busQueue, &msg, 5); //If we are in the coast phase, command the airbrakes servo to the target extension value
 			}
-			printf_("extension: %f", extension);
+
 		}
 	}
 }

STM32Cube/Tasks/controller.h

@@ -11,12 +11,13 @@
 #include "FreeRTOS.h"
 #include "queue.h"
 
-#define CONTROLLER_GAIN_P 0.05
-#define CONTROLLER_GAIN_I 0.01
+#define CONTROLLER_GAIN_P 0.00005
+#define CONTROLLER_GAIN_I 0.001
 #define CONTROLLER_GAIN_D 0.0
 #define ALTITUDE_TARGET_FT 20000
 #define CONTROLLER_MAX_EXTENSION 1.0
 #define CONTROLLER_MIN_EXTENSION 0.0
+#define CONTROLLER_I_SATMAX 10.0
 
 #define MIN_CONTROLLER_Hz 1
 #define CONTROLLER_DELAY_TICKS 1000 / MIN_CONTROLLER_Hz / portTICK_RATE_MS