Merge pull request #39 from waterloo-rocketry/vn-uart-testing

VN Serial Ingestion, Logging, and CAN

STM32Cube/.cproject

@@ -47,11 +47,11 @@
 								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.optimization.level.1681767496" name="Optimization level" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.optimization.level" useByScannerDiscovery="false"/>
 								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.definedsymbols.2001593663" name="Define symbols (-D)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.definedsymbols" useByScannerDiscovery="false" valueType="definedSymbols">
 									<listOptionValue builtIn="false" value="DEBUG"/>
-									<listOptionValue builtIn="false" value="TEST_MODE"/>
 									<listOptionValue builtIn="false" value="USE_HAL_DRIVER"/>
 									<listOptionValue builtIn="false" value="STM32_THREAD_SAFE_STRATEGY=4"/>
 									<listOptionValue builtIn="false" value="BOARD_UNIQUE_ID=0x08"/>
 									<listOptionValue builtIn="false" value="STM32H733xx"/>
+									<listOptionValue builtIn="false" value="TEST_MODE"/>
 								</option>
 								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.includepaths.2062664197" name="Include paths (-I)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.includepaths" useByScannerDiscovery="false" valueType="includePath">
 									<listOptionValue builtIn="false" value="../Core/Inc"/>
@@ -143,15 +143,15 @@
 						</toolChain>
 					</folderInfo>
 					<sourceEntries>
+						<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="printf"/>
+						<entry excluding="dspic*|pic18*|tests|mcp*" flags="VALUE_WORKSPACE_PATH" kind="sourcePath" name="canlib"/>
+						<entry excluding="c/src/vn/sensors|c/src/vn/sensors.c|c/src/vn/xplat|c/examples|c/help|c/include/vn/sensors|c/include/vn/xplat|c/projects|python|help" flags="VALUE_WORKSPACE_PATH" kind="sourcePath" name="vnproglib"/>
+						<entry excluding="Examples|Python" flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Fusion"/>
 						<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Core"/>
-						<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Drivers"/>
 						<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="FATFS"/>
-						<entry excluding="Examples|Python" flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Fusion"/>
 						<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Middlewares"/>
 						<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Tasks"/>
-						<entry excluding="dspic*|pic18*|tests|mcp*" flags="VALUE_WORKSPACE_PATH" kind="sourcePath" name="canlib"/>
-						<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="printf"/>
-						<entry excluding="c/src/vn/sensors|c/src/vn/sensors.c|c/src/vn/xplat|c/examples|c/help|c/include/vn/sensors|c/include/vn/xplat|c/projects|python|help" flags="VALUE_WORKSPACE_PATH" kind="sourcePath" name="vnproglib"/>
+						<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Drivers"/>
 					</sourceEntries>
 				</configuration>
 			</storageModule>

STM32Cube/.gitignore

@@ -0,0 +1 @@
+/Debug/

STM32Cube/Core/Inc/FreeRTOSConfig.h

@@ -74,6 +74,7 @@
 #define configUSE_16_BIT_TICKS                   0
 #define configUSE_MUTEXES                        1
 #define configQUEUE_REGISTRY_SIZE                8
+#define configCHECK_FOR_STACK_OVERFLOW           1
 #define configUSE_RECURSIVE_MUTEXES              1
 #define configUSE_COUNTING_SEMAPHORES            1
 #define configUSE_PORT_OPTIMISED_TASK_SELECTION  0

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 ---------------------------------------------------------*/
@@ -211,6 +212,7 @@ int main(void)
   flightPhaseInit();
   healthCheckInit();
   controllerInit();
+  state_est_init();
 
   otitsRegister(test_defaultTaskPass, TEST_SOURCE_DEFAULT, "DefaultPass");
   otitsRegister(test_defaultTaskFail, TEST_SOURCE_DEFAULT, "DefaultFail");
@@ -225,14 +227,14 @@ 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(trajectory_task, "traj", 512, NULL, (UBaseType_t) osPriorityNormal, &trajectoryTaskHandle);
-  xReturned &= xTaskCreate(logTask, "Logging", 1024, NULL, (UBaseType_t) osPriorityBelowNormal, &logTaskhandle);
+  xReturned &= xTaskCreate(logTask, "Logging", 1024, NULL, (UBaseType_t) osPriorityNormal, &logTaskhandle);
   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);
+  xReturned &= xTaskCreate(controlTask, "Controller", 2000, NULL, (UBaseType_t) osPriorityBelowNormal, &controllerHandle);
+  xReturned &= xTaskCreate(flightPhaseTask, "Flight Phase", 2000, NULL, (UBaseType_t) osPriorityAboveNormal, &controllerHandle);
 #ifdef TEST_MODE
   xReturned &= xTaskCreate(otitsTask, "oTITS", 512, NULL, (UBaseType_t) osPriorityNormal, &oTITSHandle);
 #endif
@@ -1021,13 +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";
-		printf_(buffer);
+		//printf_(buffer);
 		//logDebug(0, "test messsssage");
 		HAL_GPIO_TogglePin(GPIOB, GPIO_PIN_12);
+		//printf_("hello world\n");
 		osDelay(1000);
+
 	}
   /* USER CODE END 5 */
 }

STM32Cube/Tasks/can_handler.h

@@ -14,7 +14,6 @@ extern "C" {
 extern FDCAN_HandleTypeDef hfdcan1;
 extern xQueueHandle busQueue;
 
-
 void can_handle_rx(const can_msg_t *message, uint32_t timestamp);
 void canHandlerTask(void *argument);
 void canHandlerInit(void);

STM32Cube/Tasks/log.c

@@ -162,24 +162,29 @@ void logTask(void *argument) {
     FRESULT res = FR_OK;
 
     res |= f_mount(&fs, "", 0);
+
     res |= f_mkdir(logsPath);
+
     initUniqueLogFileName();
 
     FIL logfile;
     res |= f_open(&logfile, logFileName, FA_CREATE_ALWAYS);
+
     res |= f_close(&logfile);
 
+
     if (res != FR_OK && res != FR_EXIST) {
         // flag init error
     }
 
     // wait for a full buffer to appear in the queue; timeout is long - queues are not expected to fill up super quickly
     for (;;) {
 		if (xQueueReceive(fullBuffersQueue, &bufferToPrint, 1000000) == pdPASS) {
-            	(void)f_open(&logfile, logFileName, FA_OPEN_APPEND | FA_WRITE);
+				FRESULT result = FR_OK;
+			    result |= f_open(&logfile, logFileName, FA_OPEN_APPEND | FA_WRITE);
                 // buffers fill from 0, so `index` conveniently indicates how many chars of data there are to print
-            	(void)f_write(&logfile, bufferToPrint->buffer, bufferToPrint->index, NULL);
-            	(void)f_close(&logfile);
+			    result |= f_write(&logfile, bufferToPrint->buffer, bufferToPrint->index, NULL);
+			    result |= f_close(&logfile);
             	// uart print for testing
             	// !!!! Ensure the timeout (rn 3000) is long enough to transmit a whole log chunk !!!
             	// HAL_UART_Transmit(&huart4, bufferToPrint->buffer, bufferToPrint->index, 3000);

STM32Cube/Tasks/state_estimation.c

@@ -5,8 +5,7 @@
  *      Author: joedo
  */
 
-#include "FreeRTOS.h"
-#include "task.h"
+
 #include "ICM-20948.h"
 #include "Fusion.h"
 #include "vn_handler.h"
@@ -17,14 +16,36 @@
 
 #define TASK_DELAY_TICKS 20 // TODO: what is actual delay time?
 #define SAMPLE_RATE 100 // replace this with actual sample rate
-#define USE_ICM 1
+#define USE_ICM 0
 
 extern xQueueHandle angleQueue;
 
+QueueHandle_t IMUDataHandle;
+
+bool unpackIMUData(FusionVector *gyroscope, FusionVector *accelerometer, FusionVector *magnetometer, uint32_t *deltaTimems)
+{
+	rawIMUPacked data;
+	if(xQueueReceive(IMUDataHandle, &data, 50) == pdTRUE)
+	{
+		*gyroscope = data.gyroscope;
+		*accelerometer = data.accelerometer;
+		*magnetometer = data.magnetometer;
+		*deltaTimems = data.deltaTimems;
+		return true;
+	}
+	return false;
+}
+
+void state_est_init()
+{
+	IMUDataHandle = xQueueCreate(1, sizeof(rawIMUPacked));
+}
+
 void stateEstTask(void *arguments) {
     TickType_t xLastWakeTime = xTaskGetTickCount();
     float previousTimestamp = 0;
 
+
     // Define calibration (replace with actual calibration data if available)
     //all of these have this missing braces error..... that seems to just be a bug...?
     const FusionMatrix gyroscopeMisalignment = {
@@ -123,16 +144,12 @@ void stateEstTask(void *arguments) {
         previousTimestamp = timestamp;
 
 #else
-        //wait for the VN data buffer mutex to be available
-        if(xSemaphoreTake(vnIMUResultMutex, 100) != pdTRUE) {
-            //we timed out, something is holding onto the mutex
-        }
-
         uint32_t deltaTimeMS;
-        writeIMUData(&gyroscope, &accelerometer, &magnetometer, &deltaTimeMS);
-        float deltaTime = (float) deltaTimeMS / 1000.0; //yes I realized deltaTime was a float in s after the fact sue me
-
-        xSemaphoreGive(vnIMUResultMutex);
+		 if(unpackIMUData(&gyroscope, &accelerometer, &magnetometer, &deltaTimeMS) == false)
+		 {
+			 logError("state estimation", "Failed to get VN raw IMU data");
+		 }
+		 float deltaTime = (float) deltaTimeMS / 1000.0; //yes I realized deltaTime was a float in s after the fact sue me
 #endif
 
         // Apply calibration

STM32Cube/Tasks/state_estimation.h

@@ -9,7 +9,21 @@
 #define STATE_ESTIMATION_H_
 
 #include "stm32h7xx_hal.h"
+#include "Fusion.h"
+#include "FreeRTOS.h"
+#include "queue.h"
+
+typedef struct
+{
+	uint32_t deltaTimems;
+	FusionVector gyroscope;
+	FusionVector accelerometer;
+	FusionVector magnetometer;
+} rawIMUPacked;
+
+extern QueueHandle_t IMUDataHandle;
 
 void stateEstTask(void *arguments);
+void state_est_init();
 
 #endif /* STATE_ESTIMATION_H_ */