VC GPS Fix (#1213)

### Summary
<!-- Quick summary of changes, optional -->
- Add new GPS functionality, wanting to use EKF in mode `
SBG_ECOM_SOL_MODE_NAV_POSITION ` to measure vehicle speed
- Fix GPS callback on Quadruna
- Tested and generally works but triggers watchdog sometimes, haven't
had time to test it before 2024 Comp so will comment out

### TODO (For the future)
- Test ekf while in motion. Values at the moment are suspicious
- Figure out watchdog issue
### Changelist 
<!-- Give a list of the changes covered in this PR. This will help both
you and the reviewer keep this PR within scope. -->

### Testing Done
<!-- Outline the testing that was done to demonstrate the changes are
solid. This could be unit tests, integration tests, testing on the car,
etc. Include relevant code snippets, screenshots, etc as needed. -->

### Resolved Issues
<!-- Link any issues that this PR resolved like so: `Resolves #1, #2,
and #5` (Note: Using this format, Github will automatically close the
issue(s) when this PR is merged in). -->

### Checklist
*Please change `[ ]` to `[x]` when you are ready.*
- [ ] I have read and followed the code conventions detailed in
[README.md](../README.md) (*This will save time for both you and the
reviewer!*).
- [ ] If this pull request is longer then **500** lines, I have provided
*explicit* justification in the summary above explaining why I *cannot*
break this up into multiple pull requests (*Small PR's are faster and
less painful for everyone involved!*).

---------

Co-authored-by: Kyle Mackenzie <[email protected]>
Co-authored-by: Gus Tahara-Edmonds <[email protected]>

can_bus/quadruna/VC/VC_alerts.json

@@ -105,6 +105,11 @@
         "RightInverterFault": {
             "id": 556,
             "description": "Right inverter entered fault state."
-        }
+        },
+        "SBGModeFault":  {
+            "disabled": true,
+            "id" :561,
+            "description" : "Ensure mode is Nominal mode for EKF data used"
+        }  
     }
 }

can_bus/quadruna/VC/VC_tx.json

@@ -162,6 +162,7 @@
         }
     },
     "EllipseStatus": {
+        "disabled": true,
         "msg_id": 223,
         "cycle_time": 100,
         "description": "Status of the SBG Ellipse N sensor.",
@@ -175,6 +176,7 @@
         }
     },
     "EllipseTime": {
+        "disabled": true,
         "msg_id": 209,
         "cycle_time": 100,
         "description": "Timestamp from the SBG Ellipse N sensor.",
@@ -186,6 +188,7 @@
         }
     },
     "EllipseAcceleration": {
+        "disabled": true,
         "msg_id": 201,
         "cycle_time": 1000,
         "description": "Acceleration from the SBG Ellipse N sensor.",
@@ -211,6 +214,7 @@
         }
     },
     "EllipseAngularVelocity": {
+        "disabled": true,
         "msg_id": 203,
         "cycle_time": 1000,
         "description": "Angular velocity from the SBG Ellipse N sensor.",
@@ -236,6 +240,7 @@
         }
     },
     "EllipseEulerAngles": {
+        "disabled": true,
         "msg_id": 222,
         "cycle_time": 1000,
         "description": "Euler angles from the SBG Ellipse N sensor.",
@@ -447,6 +452,7 @@
         }
     },
     "GpsPosInfo": {
+        "disabled": true,
         "msg_id": 217,
         "cycle_time": 100,
         "description": "Car position info",
@@ -493,6 +499,7 @@
         }
     },
     "GpsVelInfo": {
+        "disabled": true,
         "msg_id": 218,
         "cycle_time": 100,
         "description": "Car Velocity info",

firmware/quadruna/VC/src/app/app_sbgEllipse.c

@@ -3,43 +3,44 @@
 #include "app_canTx.h"
 #include "io_sbgEllipse.h"
 
+// TODO: Not using Ellipse GPS for Comp 2024
 void app_sbgEllipse_broadcast()
 {
-    // Status msg
-    const uint16_t general_status = io_sbgEllipse_getGeneralStatus();
-    const uint32_t com_status     = io_sbgEllipse_getComStatus();
-
-    app_canTx_VC_EllipseGeneralStatusBitmask_set(general_status);
-    app_canTx_VC_EllipseComStatusBitmask_set(com_status);
-
-    // Time msg
-    const uint32_t timestamp_us = io_sbgEllipse_getTimestampUs();
-    app_canTx_VC_EllipseTimestamp_set(timestamp_us);
-
-    // Acceleration msg
-    const float forward_accel  = io_sbgEllipse_getSensorOutput(ELLIPSE_OUTPUT_ACCELERATION_X);
-    const float lateral_accel  = io_sbgEllipse_getSensorOutput(ELLIPSE_OUTPUT_ACCELERATION_Y);
-    const float vertical_accel = io_sbgEllipse_getSensorOutput(ELLIPSE_OUTPUT_ACCELERATION_Z);
-
-    app_canTx_VC_AccelerationForward_set(forward_accel);
-    app_canTx_VC_AccelerationLateral_set(lateral_accel);
-    app_canTx_VC_AccelerationVertical_set(vertical_accel);
-
-    // Angular velocity msg
-    const float ang_vel_roll  = io_sbgEllipse_getSensorOutput(ELLIPSE_OUTPUT_ANGULAR_VELOCITY_ROLL);
-    const float ang_vel_pitch = io_sbgEllipse_getSensorOutput(ELLIPSE_OUTPUT_ANGULAR_VELOCITY_PITCH);
-    const float ang_vel_yaw   = io_sbgEllipse_getSensorOutput(ELLIPSE_OUTPUT_ANGULAR_VELOCITY_YAW);
-
-    app_canTx_VC_AngularVelocityRoll_set((int)ang_vel_roll);
-    app_canTx_VC_AngularVelocityPitch_set((int)ang_vel_pitch);
-    app_canTx_VC_AngularVelocityYaw_set((int)ang_vel_yaw);
-
-    // Euler angles msg
-    const float euler_roll  = io_sbgEllipse_getSensorOutput(ELLIPSE_OUTPUT_EULER_ROLL);
-    const float euler_pitch = io_sbgEllipse_getSensorOutput(ELLIPSE_OUTPUT_EULER_PITCH);
-    const float euler_yaw   = io_sbgEllipse_getSensorOutput(ELLIPSE_OUTPUT_EULER_YAW);
-
-    app_canTx_VC_EulerAnglesRoll_set(euler_roll);
-    app_canTx_VC_EulerAnglesPitch_set(euler_pitch);
-    app_canTx_VC_EulerAnglesYaw_set(euler_yaw);
+    // // Status msg
+    // const uint16_t general_status = io_sbgEllipse_getGeneralStatus();
+    // const uint32_t com_status     = io_sbgEllipse_getComStatus();
+
+    // app_canTx_VC_EllipseGeneralStatusBitmask_set(general_status);
+    // app_canTx_VC_EllipseComStatusBitmask_set(com_status);
+
+    // // Time msg
+    // const uint32_t timestamp_us = io_sbgEllipse_getTimestampUs();
+    // app_canTx_VC_EllipseTimestamp_set(timestamp_us);
+
+    // // Acceleration msg
+    // const float forward_accel  = io_sbgEllipse_getImuAccelerations()->x;
+    // const float lateral_accel  = io_sbgEllipse_getImuAccelerations()->y;
+    // const float vertical_accel = io_sbgEllipse_getImuAccelerations()->z;
+
+    // app_canTx_VC_AccelerationForward_set(forward_accel);
+    // app_canTx_VC_AccelerationLateral_set(lateral_accel);
+    // app_canTx_VC_AccelerationVertical_set(vertical_accel);
+
+    // // Angular velocity msg
+    // const float ang_vel_roll  = io_sbgEllipse_getImuAngularVelocities()->roll;
+    // const float ang_vel_pitch = io_sbgEllipse_getImuAngularVelocities()->pitch;
+    // const float ang_vel_yaw   = io_sbgEllipse_getImuAngularVelocities()->yaw;
+
+    // app_canTx_VC_AngularVelocityRoll_set((int)ang_vel_roll);
+    // app_canTx_VC_AngularVelocityPitch_set((int)ang_vel_pitch);
+    // app_canTx_VC_AngularVelocityYaw_set((int)ang_vel_yaw);
+
+    // // Euler angles msg
+    // const float euler_roll  = io_sbgEllipse_getEulerAngles()->roll;
+    // const float euler_pitch = io_sbgEllipse_getEulerAngles()->pitch;
+    // const float euler_yaw   = io_sbgEllipse_getEulerAngles()->yaw;
+
+    // app_canTx_VC_EulerAnglesRoll_set(euler_roll);
+    // app_canTx_VC_EulerAnglesPitch_set(euler_pitch);
+    // app_canTx_VC_EulerAnglesYaw_set(euler_yaw);
 }

firmware/quadruna/VC/src/app/states/app_allStates.c

@@ -49,8 +49,9 @@ void app_allStates_runOnTick100Hz(void)
     else
         app_heartbeatMonitor_broadcastFaults();
 
-    io_sbgEllipse_handleLogs();
-    app_sbgEllipse_broadcast();
+    // Comment out for now - SBG Ellipse is not currently used.
+    // io_sbgEllipse_handleLogs();
+    // app_sbgEllipse_broadcast();
 
     // Set status to false (which blocks drive) if either inverter is faulted, or another board has set a fault.
 }

firmware/quadruna/VC/src/cubemx/Inc/stm32h7xx_it.h

@@ -54,6 +54,7 @@ extern "C"
     void UsageFault_Handler(void);
     void DebugMon_Handler(void);
     void DMA1_Stream0_IRQHandler(void);
+    void DMA1_Stream1_IRQHandler(void);
     void FDCAN1_IT0_IRQHandler(void);
     void FDCAN1_IT1_IRQHandler(void);
     void TIM3_IRQHandler(void);

firmware/quadruna/VC/src/cubemx/Src/main.c

@@ -64,6 +64,7 @@ UART_HandleTypeDef huart7;
 UART_HandleTypeDef huart1;
 UART_HandleTypeDef huart2;
 UART_HandleTypeDef huart3;
+DMA_HandleTypeDef  hdma_usart2_rx;
 
 /* Definitions for Task100Hz */
 osThreadId_t         Task100HzHandle;
@@ -918,7 +919,8 @@ static void MX_USART2_UART_Init(void)
     huart2.Init.OverSampling           = UART_OVERSAMPLING_16;
     huart2.Init.OneBitSampling         = UART_ONE_BIT_SAMPLE_DISABLE;
     huart2.Init.ClockPrescaler         = UART_PRESCALER_DIV1;
-    huart2.AdvancedInit.AdvFeatureInit = UART_ADVFEATURE_NO_INIT;
+    huart2.AdvancedInit.AdvFeatureInit = UART_ADVFEATURE_RXOVERRUNDISABLE_INIT;
+    huart2.AdvancedInit.OverrunDisable = UART_ADVFEATURE_OVERRUN_DISABLE;
     if (HAL_UART_Init(&huart2) != HAL_OK)
     {
         Error_Handler();
@@ -998,6 +1000,9 @@ static void MX_DMA_Init(void)
     /* DMA1_Stream0_IRQn interrupt configuration */
     HAL_NVIC_SetPriority(DMA1_Stream0_IRQn, 5, 0);
     HAL_NVIC_EnableIRQ(DMA1_Stream0_IRQn);
+    /* DMA1_Stream1_IRQn interrupt configuration */
+    HAL_NVIC_SetPriority(DMA1_Stream1_IRQn, 5, 0);
+    HAL_NVIC_EnableIRQ(DMA1_Stream1_IRQn);
 }
 
 /**

firmware/quadruna/VC/src/cubemx/Src/stm32h7xx_hal_msp.c

@@ -26,6 +26,8 @@
 /* USER CODE END Includes */
 extern DMA_HandleTypeDef hdma_adc1;
 
+extern DMA_HandleTypeDef hdma_usart2_rx;
+
 /* Private typedef -----------------------------------------------------------*/
 /* USER CODE BEGIN TD */
 
@@ -714,6 +716,25 @@ void HAL_UART_MspInit(UART_HandleTypeDef *huart)
         GPIO_InitStruct.Alternate = GPIO_AF7_USART2;
         HAL_GPIO_Init(GPIOD, &GPIO_InitStruct);
 
+        /* USART2 DMA Init */
+        /* USART2_RX Init */
+        hdma_usart2_rx.Instance                 = DMA1_Stream1;
+        hdma_usart2_rx.Init.Request             = DMA_REQUEST_USART2_RX;
+        hdma_usart2_rx.Init.Direction           = DMA_PERIPH_TO_MEMORY;
+        hdma_usart2_rx.Init.PeriphInc           = DMA_PINC_DISABLE;
+        hdma_usart2_rx.Init.MemInc              = DMA_MINC_ENABLE;
+        hdma_usart2_rx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
+        hdma_usart2_rx.Init.MemDataAlignment    = DMA_MDATAALIGN_BYTE;
+        hdma_usart2_rx.Init.Mode                = DMA_CIRCULAR;
+        hdma_usart2_rx.Init.Priority            = DMA_PRIORITY_LOW;
+        hdma_usart2_rx.Init.FIFOMode            = DMA_FIFOMODE_DISABLE;
+        if (HAL_DMA_Init(&hdma_usart2_rx) != HAL_OK)
+        {
+            Error_Handler();
+        }
+
+        __HAL_LINKDMA(huart, hdmarx, hdma_usart2_rx);
+
         /* USER CODE BEGIN USART2_MspInit 1 */
 
         /* USER CODE END USART2_MspInit 1 */
@@ -826,6 +847,8 @@ void HAL_UART_MspDeInit(UART_HandleTypeDef *huart)
         */
         HAL_GPIO_DeInit(GPIOD, GPIO_PIN_5 | GPIO_PIN_6);
 
+        /* USART2 DMA DeInit */
+        HAL_DMA_DeInit(huart->hdmarx);
         /* USER CODE BEGIN USART2_MspDeInit 1 */
 
         /* USER CODE END USART2_MspDeInit 1 */

firmware/quadruna/VC/src/cubemx/Src/stm32h7xx_it.c

@@ -60,6 +60,7 @@ extern DMA_HandleTypeDef   hdma_adc1;
 extern ADC_HandleTypeDef   hadc3;
 extern FDCAN_HandleTypeDef hfdcan1;
 extern TIM_HandleTypeDef   htim3;
+extern DMA_HandleTypeDef   hdma_usart2_rx;
 extern UART_HandleTypeDef  huart7;
 extern TIM_HandleTypeDef   htim6;
 
@@ -179,6 +180,20 @@ void DMA1_Stream0_IRQHandler(void)
     /* USER CODE END DMA1_Stream0_IRQn 1 */
 }
 
+/**
+ * @brief This function handles DMA1 stream1 global interrupt.
+ */
+void DMA1_Stream1_IRQHandler(void)
+{
+    /* USER CODE BEGIN DMA1_Stream1_IRQn 0 */
+
+    /* USER CODE END DMA1_Stream1_IRQn 0 */
+    HAL_DMA_IRQHandler(&hdma_usart2_rx);
+    /* USER CODE BEGIN DMA1_Stream1_IRQn 1 */
+
+    /* USER CODE END DMA1_Stream1_IRQn 1 */
+}
+
 /**
  * @brief This function handles FDCAN1 interrupt 0.
  */

firmware/quadruna/VC/src/cubemx/VC.ioc

@@ -80,7 +80,26 @@ Dma.ADC1.0.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT
 Dma.ADC1.0.SyncRequestNumber=1
 Dma.ADC1.0.SyncSignalID=NONE
 Dma.Request0=ADC1
-Dma.RequestsNb=1
+Dma.Request1=USART2_RX
+Dma.RequestsNb=2
+Dma.USART2_RX.1.Direction=DMA_PERIPH_TO_MEMORY
+Dma.USART2_RX.1.EventEnable=DISABLE
+Dma.USART2_RX.1.FIFOMode=DMA_FIFOMODE_DISABLE
+Dma.USART2_RX.1.Instance=DMA1_Stream1
+Dma.USART2_RX.1.MemDataAlignment=DMA_MDATAALIGN_BYTE
+Dma.USART2_RX.1.MemInc=DMA_MINC_ENABLE
+Dma.USART2_RX.1.Mode=DMA_CIRCULAR
+Dma.USART2_RX.1.PeriphDataAlignment=DMA_PDATAALIGN_BYTE
+Dma.USART2_RX.1.PeriphInc=DMA_PINC_DISABLE
+Dma.USART2_RX.1.Polarity=HAL_DMAMUX_REQ_GEN_RISING
+Dma.USART2_RX.1.Priority=DMA_PRIORITY_LOW
+Dma.USART2_RX.1.RequestNumber=1
+Dma.USART2_RX.1.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode,SignalID,Polarity,RequestNumber,SyncSignalID,SyncPolarity,SyncEnable,EventEnable,SyncRequestNumber
+Dma.USART2_RX.1.SignalID=NONE
+Dma.USART2_RX.1.SyncEnable=DISABLE
+Dma.USART2_RX.1.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT
+Dma.USART2_RX.1.SyncRequestNumber=1
+Dma.USART2_RX.1.SyncSignalID=NONE
 FDCAN1.AutoRetransmission=ENABLE
 FDCAN1.CalculateBaudRateNominal=500000
 FDCAN1.CalculateTimeBitNominal=2000
@@ -210,6 +229,7 @@ MxDb.Version=DB.6.0.92
 NVIC.ADC3_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
 NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
 NVIC.DMA1_Stream0_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
+NVIC.DMA1_Stream1_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
 NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
 NVIC.FDCAN1_IT0_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
 NVIC.FDCAN1_IT1_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
@@ -631,7 +651,8 @@ USART1.BaudRate=57600
 USART1.IPParameters=VirtualMode-Asynchronous,BaudRate
 USART1.VirtualMode-Asynchronous=VM_ASYNC
 USART2.BaudRate=SBG_ELLIPSE_GPS_BAUD_RATE
-USART2.IPParameters=VirtualMode-Asynchronous,BaudRate
+USART2.IPParameters=VirtualMode-Asynchronous,BaudRate,OverrunDisableParam
+USART2.OverrunDisableParam=ADVFEATURE_OVERRUN_DISABLE
 USART2.VirtualMode-Asynchronous=VM_ASYNC
 USART3.IPParameters=VirtualMode-Asynchronous
 USART3.VirtualMode-Asynchronous=VM_ASYNC

firmware/quadruna/VC/src/cubemx/VC.ioc.md5

@@ -1 +1 @@
-b3531f304282bafc01eb3f14081ed474
+731a9fa27895e1523a89e29d3db89def