Fix GPS watchdog and get data (#1352)

### Changelist 
<!-- Give a list of the changes covered in this PR. This will help both
you and the reviewer keep this PR within scope. -->

- Enable GPS can messages that are used and are received by SBG,
broadcast them, and enable sbg io
- Rewrote rtos queue architecture in callback and in read function
- No more watchdog or cpu timing issues

### 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. -->
- Ran HV and in drive state, extract SD data, stable, deterministic can
messages received
### Resolved Tickets
<!-- Link any tickets that this PR resolves. -->
-
[FIRM-14](https://ubcformulaelectric.atlassian.net/jira/software/c/projects/FIRM/boards/37?selectedIssue=FIRM-14)


[FIRM-14]:
https://ubcformulaelectric.atlassian.net/browse/FIRM-14?atlOrigin=eyJpIjoiNWRkNTljNzYxNjVmNDY3MDlhMDU5Y2ZhYzA5YTRkZjUiLCJwIjoiZ2l0aHViLWNvbS1KU1cifQ

---------

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

can_bus/quadruna/VC/VC_alerts.json

@@ -79,6 +79,10 @@
         "BrakeAppsDisagreement": {
             "id": 564,
             "description": "Brakes and apps are active at same time."
+        },
+        "SbgInitFailed": {
+            "id": 565,
+            "description": "Initializing the SBG Ellipse GPS failed"
         }
     },
     "faults": {
@@ -105,11 +109,6 @@
         "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_enum.json

@@ -4,11 +4,11 @@
     "VC_INVERTER_ON_STATE": 1,
     "VC_DRIVE_STATE": 2
   },
-
-  "VcSbgStatus": {
-    "VC_SOLUTION_COMPUTED": 0,
-    "INSUFFICIENT_READINGS": 1, 
-    "INTERNAL_ERROR": 2, 
-    "LIMIT_EXCEEDED": 3
+  "VcEkfStatus": {
+    "UNINITIALIZED": 0,
+    "VERTICAL_GYRO": 1,
+    "AHRS": 2,
+    "VELOCITY": 3,
+    "POSITION": 4
   }
-}
+}

can_bus/quadruna/VC/VC_tx.json

@@ -85,21 +85,21 @@
         }
     },
     "INVL_ReadWriteParamCommand": {
-        "msg_id": 33, 
+        "msg_id": 33,
         "cycle_time": null,
         "signals": {
             "INVL_CommandParameterAddress": {
-                "start_bit": 0, 
+                "start_bit": 0,
                 "bits": 16,
                 "start_value": 20
             },
             "INVL_CommandReadWrite": {
-                "start_bit": 16, 
+                "start_bit": 16,
                 "enum": "InverterReadWriteCommand",
                 "start_value": 1
             },
             "INVL_CommandData": {
-                "start_bit": 32, 
+                "start_bit": 32,
                 "bits": 16,
                 "start_value": 0
             }
@@ -165,21 +165,21 @@
         }
     },
     "INVR_ReadWriteParamCommand": {
-        "msg_id": 83, 
+        "msg_id": 83,
         "cycle_time": null,
         "signals": {
             "INVR_CommandParameterAddress": {
-                "start_bit": 0, 
+                "start_bit": 0,
                 "bits": 16,
                 "start_value": 20
             },
             "INVR_CommandReadWrite": {
-                "start_bit": 16, 
+                "start_bit": 16,
                 "enum": "InverterReadWriteCommand",
                 "start_value": 1
             },
             "INVR_CommandData": {
-                "start_bit": 32, 
+                "start_bit": 32,
                 "bits": 16,
                 "start_value": 0
             }
@@ -204,7 +204,6 @@
         }
     },
     "EllipseStatus": {
-        "disabled": true,
         "msg_id": 223,
         "cycle_time": 100,
         "description": "Status of the SBG Ellipse N sensor.",
@@ -214,11 +213,13 @@
             },
             "EllipseComStatusBitmask": {
                 "bits": 32
+            },
+            "EkfSolutionMode": {
+                "enum": "VcEkfStatus"
             }
         }
     },
     "EllipseTime": {
-        "disabled": true,
         "msg_id": 209,
         "cycle_time": 100,
         "description": "Timestamp from the SBG Ellipse N sensor.",
@@ -282,7 +283,6 @@
         }
     },
     "EllipseEulerAngles": {
-        "disabled": true,
         "msg_id": 222,
         "cycle_time": 1000,
         "description": "Euler angles from the SBG Ellipse N sensor.",
@@ -488,20 +488,23 @@
             "RegenEnabled": {
                 "bits": 1
             },
-            "TorqueVectoringEnabled":{
+            "TorqueVectoringEnabled": {
                 "bits": 1
+            },
+            "VehicleVelocity": {
+                "resolution": 0.01,
+                "min": -150,
+                "max": 150,
+                "unit": "km/h"
             }
         }
     },
-    "GpsPosInfo": {
+    "EllipseEkfNavPosition": {
         "disabled": true,
         "msg_id": 217,
         "cycle_time": 100,
         "description": "Car position info",
         "signals": {
-            "PositionStatus": {
-                "enum": "VcSbgStatus"
-            },
             "Latitude": {
                 "resolution": 0.00005,
                 "min": 47,
@@ -540,49 +543,45 @@
             }
         }
     },
-    "GpsVelInfo": {
-        "disabled": true,
+    "EllipseEkfNavVelocity": {
         "msg_id": 218,
         "cycle_time": 100,
         "description": "Car Velocity info",
         "signals": {
-            "VelocityStatus": {
-                "enum": "VcSbgStatus"
-            },
             "VelocityNorth": {
-                "resolution": 0.1,
-                "min": 0,
-                "max": 120,
+                "resolution": 0.05,
+                "min": -35,
+                "max": 35,
                 "unit": "m/s"
             },
             "VelocityNorthAccuracy": {
                 "resolution": 0.1,
                 "min": 0,
-                "max": 20,
+                "max": 35,
                 "unit": "m/s"
             },
             "VelocityEast": {
-                "resolution": 0.1,
-                "min": 0,
-                "max": 120,
+                "resolution": 0.05,
+                "min": -35,
+                "max": 35,
                 "unit": "m/s"
             },
             "VelocityEastAccuracy": {
                 "resolution": 0.1,
                 "min": 0,
-                "max": 20,
+                "max": 35,
                 "unit": "m/s"
             },
             "VelocityDown": {
-                "resolution": 0.1,
-                "min": 0,
-                "max": 120,
+                "resolution": 0.05,
+                "min": -35,
+                "max": 35,
                 "unit": "m/s"
             },
             "VelocityDownAccuracy": {
                 "resolution": 0.1,
                 "min": 0,
-                "max": 20,
+                "max": 35,
                 "unit": "m/s"
             }
         }
@@ -764,7 +763,7 @@
         "cycle_time": 100,
         "description": "Pedal percentage info",
         "signals": {
-            "MappedPedalPercentage" :{
+            "MappedPedalPercentage": {
                 "min": -1,
                 "max": 1,
                 "resolution": 0.01,

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

@@ -1,23 +1,55 @@
 #include <assert.h>
 #include "app_sbgEllipse.h"
 #include "app_canTx.h"
+#include "app_utils.h"
+#include "app_units.h"
 #include "io_sbgEllipse.h"
 
-// TODO: Not using Ellipse GPS for Comp 2024
 void app_sbgEllipse_broadcast()
 {
-    // // Status msg
+    /* Enable these back when you turn this on in the SBG, otherwise it's still sending
+       CAN messages because another message in the signal is being used */
+
+    // 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
+    // Time msg
     // const uint32_t timestamp_us = io_sbgEllipse_getTimestampUs();
     // app_canTx_VC_EllipseTimestamp_set(timestamp_us);
 
-    // // Acceleration msg
+    // Velocity EKF
+    const float ekf_vel_N = io_sbgEllipse_getEkfNavVelocityData()->north;
+    const float ekf_vel_E = io_sbgEllipse_getEkfNavVelocityData()->east;
+    const float ekf_vel_D = io_sbgEllipse_getEkfNavVelocityData()->down;
+
+    const float ekf_vel_N_accuracy = io_sbgEllipse_getEkfNavVelocityData()->north_std_dev;
+    const float ekf_vel_E_accuracy = io_sbgEllipse_getEkfNavVelocityData()->east_std_dev;
+    const float ekf_vel_D_accuracy = io_sbgEllipse_getEkfNavVelocityData()->down_std_dev;
+
+    app_canTx_VC_VelocityNorth_set(ekf_vel_N);
+    app_canTx_VC_VelocityEast_set(ekf_vel_E);
+    app_canTx_VC_VelocityDown_set(ekf_vel_D);
+
+    app_canTx_VC_VelocityNorthAccuracy_set(ekf_vel_N_accuracy);
+    app_canTx_VC_VelocityEastAccuracy_set(ekf_vel_E_accuracy);
+    app_canTx_VC_VelocityDownAccuracy_set(ekf_vel_D_accuracy);
+
+    const float vehicle_velocity = sqrtf(SQUARE(ekf_vel_N) + SQUARE(ekf_vel_E) + SQUARE(ekf_vel_D));
+
+    app_canTx_VC_VehicleVelocity_set(MPS_TO_KMH(vehicle_velocity));
+
+    // Position EKF
+    // const double ekf_pos_lat  = io_sbgEllipse_getEkfNavPositionData()->latitude;
+    // const double ekf_pos_long = io_sbgEllipse_getEkfNavPositionData()->longitude;
+
+    // app_canTx_VC_Latitude_set((float)ekf_pos_lat);
+    // app_canTx_VC_Longtitude_set((float)ekf_pos_long);
+
+    // 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;
@@ -26,7 +58,7 @@ void app_sbgEllipse_broadcast()
     // app_canTx_VC_AccelerationLateral_set(lateral_accel);
     // app_canTx_VC_AccelerationVertical_set(vertical_accel);
 
-    // // Angular velocity msg
+    // 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;
@@ -35,12 +67,12 @@ void app_sbgEllipse_broadcast()
     // 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;
+    // Euler angles msg
+    const float euler_roll  = io_sbgEllipse_getEkfEulerAngles()->roll;
+    const float euler_pitch = io_sbgEllipse_getEkfEulerAngles()->pitch;
+    const float euler_yaw   = io_sbgEllipse_getEkfEulerAngles()->yaw;
 
-    // app_canTx_VC_EulerAnglesRoll_set(euler_roll);
-    // app_canTx_VC_EulerAnglesPitch_set(euler_pitch);
-    // app_canTx_VC_EulerAnglesYaw_set(euler_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/app_sbgEllipse.h

@@ -1,7 +1,6 @@
 #pragma once
 
 #include <stdint.h>
-
 /*
  * Broadcast sensor outputs over CAN.
  */

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

@@ -55,9 +55,8 @@ void app_allStates_runOnTick100Hz(void)
     else
         app_heartbeatMonitor_broadcastFaults();
 
-    // Comment out for now - SBG Ellipse is not currently used.
-    // io_sbgEllipse_handleLogs();
-    // app_sbgEllipse_broadcast();
+    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/app/states/app_driveState.c

@@ -115,6 +115,9 @@ static void driveStateRunOnTick100Hz(void)
     regen_switch_is_on                   = app_canRx_CRIT_RegenSwitch_get() == SWITCH_ON;
     bool turn_regen_led                  = regen_switch_is_on && !prev_regen_switch_val;
 
+    /* TODO: Vehicle dyanmics people need to make sure to do a check if sensor init failed
+       or not before using closed loop features */
+
     // Regen + TV LEDs and update warnings
     if (turn_regen_led)
     {