Adi/gps error handling (#1399)

### Changelist 
Added IMU functions for obtaining angular velocity in io_imu.c
Added velocity calculations in app_sbgEllipse.c
Added getter function for EKF Solution Mode in io_sbgEllipse.c
Added new CAN messages for calculated velocity, IMU angular velocity,
and EKF Solution Mode

### Testing Done
Velocity Calculation was tested on the car, corrected formula
accordingly.
IMU outputs reasonable angular velocities based on second VC, more
rigorous testing should be done in the future.
Wrote unit tests for velocity calculation.

### Resolved Tickets
<!-- Link any tickets that this PR resolves. -->

---------

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

can_bus/quadruna/VC/VC_tx.json

@@ -496,6 +496,12 @@
                 "min": -150,
                 "max": 150,
                 "unit": "km/h"
+            },
+            "VehicleVelocityCalculated": {
+                "resolution": 0.01,
+                "min": -150,
+                "max": 150,
+                "unit": "km/h"
             }
         }
     },
@@ -691,10 +697,10 @@
             }
         }
     },
-    "ImuLinearAcceleration": {
+    "ImuData": {
         "msg_id": 225,
         "cycle_time": 100,
-        "description": "Linear acceleration in x, y and z direction from the IMU",
+        "description": "Linear Acceleration and Angular Velocity in x, y and z direction from the IMU",
         "signals": {
             "ImuAccelerationX": {
                 "resolution": 0.1,
@@ -713,6 +719,24 @@
                 "min": 0,
                 "max": 20,
                 "unit": "m/s"
+            },
+            "ImuAngularVelocityRoll": {
+                "resolution": 0.1,
+                "min": -360,
+                "max": 360,
+                "unit": "deg/s"
+            },
+            "ImuAngularVelocityPitch": {
+                "resolution": 0.1,
+                "min": -360,
+                "max": 360,
+                "unit": "deg/s"
+            },
+            "ImuAngularVelocityYaw": {
+                "resolution": 0.1,
+                "min": -360,
+                "max": 360,
+                "unit": "deg/s"
             }
         }
     },

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

@@ -1,9 +1,11 @@
 #include <assert.h>
 #include "app_sbgEllipse.h"
 #include "app_canTx.h"
+#include "app_canRx.h"
 #include "app_utils.h"
 #include "app_units.h"
-#include "io_sbgEllipse.h"
+#include "app_vehicleDynamicsConstants.h"
+#include "io_log.h"
 
 void app_sbgEllipse_broadcast()
 {
@@ -21,10 +23,15 @@ void app_sbgEllipse_broadcast()
     // const uint32_t timestamp_us = io_sbgEllipse_getTimestampUs();
     // app_canTx_VC_EllipseTimestamp_set(timestamp_us);
 
-    // 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;
+    VelocityData velocity_calculated;
+
+    // calculating velocity data based on wheel speed
+    app_sbgEllipse_calculateVelocity(&velocity_calculated);
+
+    // EKF
+    float ekf_vel_N = io_sbgEllipse_getEkfNavVelocityData()->north;
+    float ekf_vel_E = io_sbgEllipse_getEkfNavVelocityData()->east;
+    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;
@@ -38,9 +45,21 @@ void app_sbgEllipse_broadcast()
     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));
+    const float vehicle_velocity            = sqrtf(SQUARE(ekf_vel_N) + SQUARE(ekf_vel_E) + SQUARE(ekf_vel_D));
+    const float vehicle_velocity_calculated = MPS_TO_KMH(velocity_calculated.north);
+
+    uint32_t ekf_sol_mode = io_sbgEllipse_getEkfSolutionMode();
+
+    if (ekf_sol_mode < NUM_VC_EKF_STATUS_CHOICES)
+    {
+        app_canTx_VC_EkfSolutionMode_set((VcEkfStatus)ekf_sol_mode);
+    }
 
-    app_canTx_VC_VehicleVelocity_set(MPS_TO_KMH(vehicle_velocity));
+    // determines when to use calculated or gps velocity, will be externed later
+    // bool        use_calculated_velocity = ekf_sol_mode == POSITION;
+
+    app_canTx_VC_VehicleVelocity_set(vehicle_velocity);
+    app_canTx_VC_VehicleVelocityCalculated_set(vehicle_velocity_calculated);
 
     // Position EKF
     // const double ekf_pos_lat  = io_sbgEllipse_getEkfNavPositionData()->latitude;
@@ -76,3 +95,25 @@ void app_sbgEllipse_broadcast()
     app_canTx_VC_EulerAnglesPitch_set(euler_pitch);
     app_canTx_VC_EulerAnglesYaw_set(euler_yaw);
 }
+
+void app_sbgEllipse_calculateVelocity(VelocityData *velocity)
+{
+    // These velocity calculations are not going to be super accurate because it
+    // currently does not compute a proper relative y-axis velocity because no yaw rate
+
+    const float rightMotorRPM = (float)-app_canRx_INVR_MotorSpeed_get();
+    const float leftMotorRPM  = (float)app_canRx_INVL_MotorSpeed_get();
+
+    float leftWheelVelocity  = MOTOR_RPM_TO_KMH(leftMotorRPM);
+    float rightWheelVelocity = MOTOR_RPM_TO_KMH(rightMotorRPM);
+
+    float velocityX = (leftWheelVelocity + rightWheelVelocity) / 2.0f;
+
+    // This is technically velocity in the x-axis as it is relative
+    velocity->north = velocityX;
+
+    // This is technically velocity in the y-axis as it is relative
+    velocity->east = 0;
+
+    velocity->down = 0;
+}

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

@@ -1,7 +1,14 @@
 #pragma once
 
 #include <stdint.h>
+#include "io_sbgEllipse.h"
+
 /*
  * Broadcast sensor outputs over CAN.
  */
 void app_sbgEllipse_broadcast(void);
+
+/*
+ * Calculate vehicle velocity based on motor RPM
+ */
+void app_sbgEllipse_calculateVelocity(VelocityData *velocity);

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

@@ -50,6 +50,21 @@ void app_allStates_runOnTick100Hz(void)
         app_canTx_VC_ImuAccelerationZ_set(lin_accel_z);
     }
 
+    float angular_velocity_roll  = 0.0f;
+    float angular_velocity_pitch = 0.0f;
+    float angular_velocity_yaw   = 0.0f;
+
+    bool has_ang_vel_roll  = io_imu_getAngularVelocityRoll(&angular_velocity_roll);
+    bool has_ang_vel_pitch = io_imu_getAngularVelocityPitch(&angular_velocity_pitch);
+    bool has_ang_vel_yaw   = io_imu_getAngularVelocityYaw(&angular_velocity_yaw);
+
+    if (has_ang_vel_roll && has_ang_vel_pitch && has_ang_vel_yaw)
+    {
+        app_canTx_VC_ImuAngularVelocityRoll_set(angular_velocity_roll);
+        app_canTx_VC_ImuAngularVelocityPitch_set(angular_velocity_pitch);
+        app_canTx_VC_ImuAngularVelocityYaw_set(angular_velocity_yaw);
+    }
+
     app_heartbeatMonitor_checkIn(&hb_monitor);
 
     if (heartbeat_cycles <= IGNORE_HEARTBEAT_CYCLES) // TODO make this part of the heartbeat monitor

firmware/quadruna/VC/src/io/io_imu.c

@@ -3,8 +3,10 @@
 #include <math.h>
 #include <stdio.h>
 
-// Default sensitivity for LSM6DSM is 0.061 mg/digit
-const float SENSITIVITY = 0.061f;
+// Default accelerometer sensitivity for LSM6DSM is 0.061 mg/digit
+const float ACCEL_SENSITIVITY = 0.061f;
+// Default gyroscope sensitivity for LSM6DSM is 8.75 mdeg/digit
+const float GYRO_SENSITIVITY = 8.75f;
 
 extern I2C_HandleTypeDef hi2c2;
 static I2cInterface      imu = { .i2c_handle = &hi2c2, .target_address = 0x6B, .timeout_ms = 100 };
@@ -13,8 +15,9 @@ bool io_imu_init()
 {
     if (hw_i2c_isTargetReady(&imu))
     {
-        uint8_t buffer = 0x40; // turn on accelerometer to normal mode
-        return hw_i2c_memWrite(&imu, 0x10, &buffer, 1);
+        uint8_t buffer             = 0x40; // turn on accelerometer/gyroscope to normal mode
+        bool activated_imu_sensors = hw_i2c_memWrite(&imu, 0x10, &buffer, 1) && hw_i2c_memWrite(&imu, 0x11, &buffer, 1);
+        return activated_imu_sensors;
     }
     return false;
 }
@@ -31,7 +34,7 @@ bool io_imu_getLinearAccelerationX(float *x_acceleration)
 
     // Convert raw value to acceleration in m/s^2
     int16_t x_raw   = (int16_t)(x_data[1] << 8 | x_data[0]);
-    *x_acceleration = x_raw * SENSITIVITY * 9.81f / 1000.0f;
+    *x_acceleration = x_raw * ACCEL_SENSITIVITY * 9.81f / 1000.0f;
     return true;
 }
 
@@ -47,7 +50,7 @@ bool io_imu_getLinearAccelerationY(float *y_acceleration)
 
     // Convert raw value to acceleration in m/s^2
     int16_t y_raw   = (int16_t)(y_data[1] << 8 | y_data[0]);
-    *y_acceleration = y_raw * SENSITIVITY * 9.81f / 1000.0f;
+    *y_acceleration = y_raw * ACCEL_SENSITIVITY * 9.81f / 1000.0f;
     return true;
 }
 
@@ -63,6 +66,54 @@ bool io_imu_getLinearAccelerationZ(float *z_acceleration)
 
     // Convert raw value to acceleration in m/s^2 and subtract force of gravity
     int16_t z_raw   = (int16_t)(z_data[1] << 8 | z_data[0]);
-    *z_acceleration = z_raw * SENSITIVITY * 9.81f / 1000.0f - 9.81f;
+    *z_acceleration = z_raw * ACCEL_SENSITIVITY * 9.81f / 1000.0f - 9.81f;
+    return true;
+}
+
+bool io_imu_getAngularVelocityRoll(float *roll_velocity)
+{
+    uint8_t roll_data[2];
+    bool    is_read_successful = hw_i2c_memRead(&imu, 0x22, roll_data, 2);
+
+    if (!is_read_successful)
+    {
+        return false;
+    }
+
+    // Convert raw value to angular velocity (degrees per second or radians per second as required)
+    int16_t roll_raw = (int16_t)(roll_data[1] << 8 | roll_data[0]);
+    *roll_velocity   = roll_raw * GYRO_SENSITIVITY / 1000.0f;
+    return true;
+}
+
+bool io_imu_getAngularVelocityPitch(float *pitch_velocity)
+{
+    uint8_t pitch_data[2];
+    bool    is_read_successful = hw_i2c_memRead(&imu, 0x24, pitch_data, 2);
+
+    if (!is_read_successful)
+    {
+        return false;
+    }
+
+    // Convert raw value to angular velocity (degrees per second or radians per second as required)
+    int16_t pitch_raw = (int16_t)(pitch_data[1] << 8 | pitch_data[0]);
+    *pitch_velocity   = pitch_raw * GYRO_SENSITIVITY / 1000.0f;
+    return true;
+}
+
+bool io_imu_getAngularVelocityYaw(float *yaw_velocity)
+{
+    uint8_t yaw_data[2];
+    bool    is_read_successful = hw_i2c_memRead(&imu, 0x26, yaw_data, 2);
+
+    if (!is_read_successful)
+    {
+        return false;
+    }
+
+    // Convert raw value to angular velocity (degrees per second or radians per second as required)
+    int16_t yaw_raw = (int16_t)(yaw_data[1] << 8 | yaw_data[0]);
+    *yaw_velocity   = yaw_raw * GYRO_SENSITIVITY / 1000.0f;
     return true;
 }

firmware/quadruna/VC/src/io/io_imu.h

@@ -29,3 +29,24 @@ bool io_imu_getLinearAccelerationY(float *y_acceleration);
  * @return if successful read
  */
 bool io_imu_getLinearAccelerationZ(float *z_acceleration);
+
+/**
+ * @brief Gets the angular velocity in the roll direction
+ * @param roll_velocity Pointer to store the roll angular velocity
+ * @return True if the read is successful, false otherwise
+ */
+bool io_imu_getAngularVelocityRoll(float *roll_velocity);
+
+/**
+ * @brief Gets the angular velocity in the pitch direction
+ * @param pitch_velocity Pointer to store the pitch angular velocity
+ * @return True if the read is successful, false otherwise
+ */
+bool io_imu_getAngularVelocityPitch(float *pitch_velocity);
+
+/**
+ * @brief Gets the angular velocity in the yaw direction
+ * @param yaw_velocity Pointer to store the yaw angular velocity
+ * @return True if the read is successful, false otherwise
+ */
+bool io_imu_getAngularVelocityYaw(float *yaw_velocity);

firmware/quadruna/VC/src/io/io_sbgEllipse.c

@@ -10,6 +10,8 @@
 #include "sbgECom.h"
 #include "interfaces/sbgInterfaceSerial.h"
 #include "app_canTx.h"
+#include "app_canUtils.h"
+
 #include "io_time.h"
 #include "io_log.h"
 
@@ -211,23 +213,9 @@ static void io_sbgEllipse_processMsg_status(const SbgBinaryLogData *log_data)
 static void io_sbgEllipse_processMsg_EkfNavVelandPos(const SbgBinaryLogData *log_data)
 {
     // TODO: uncomment after initial testing, if this occurs skip reading data
-    // app_canAlerts_VC_Fault_SBGModeFault_set(sbgEComLogEkfGetSolutionMode(log_data->ekfNavData.status) !=
-    // SBG_ECOM_SOL_MODE_NAV_POSITION);
-
-    app_canTx_VC_EkfSolutionMode_set((VcEkfStatus)sbgEComLogEkfGetSolutionMode(log_data->ekfNavData.status));
-
-    // uint32_t status = log_data->ekfNavData.status;
-
-    // if (sbgEComLogEkfGetSolutionMode(log_data->ekfNavData.status) != SBG_ECOM_SOL_MODE_NAV_POSITION)
-    // {
-    //     uint32_t status = log_data->ekfNavData.status;
 
-    //     bool is_velocity_valid = (status & SBG_ECOM_SOL_VELOCITY_VALID) != 0;
-    //     bool is_position_valid = (status & SBG_ECOM_SOL_POSITION_VALID) != 0;
-
-    //     if (!is_velocity_valid & !is_position_valid)
-    //         return;
-    // }
+    // obtaining ekf solution mode from sbg ellipse and setting it in status field
+    sensor_data.ekf_solution_status = sbgEComLogEkfGetSolutionMode(log_data->ekfNavData.status);
 
     // velocity data in m/s
     sensor_data.ekf_nav_data.velocity.north = log_data->ekfNavData.velocity[0];
@@ -318,6 +306,11 @@ uint32_t io_sbgEllipse_getOverflowCount(void)
     return sbg_queue_overflow_count;
 }
 
+uint32_t io_sbgEllipse_getEkfSolutionMode(void)
+{
+    return sensor_data.ekf_solution_status;
+}
+
 Vector3 *io_sbgEllipse_getImuAccelerations()
 {
     return &sensor_data.imu_data.acceleration;

firmware/quadruna/VC/src/io/io_sbgEllipse.h

@@ -75,6 +75,7 @@ typedef struct
     EkfEulerPacketData ekf_euler_data;
     StatusPacketData   status_data;
     EkfNavPacketData   ekf_nav_data;
+    uint32_t           ekf_solution_status;
 } SensorData;
 
 #ifdef TARGET_EMBEDDED
@@ -114,6 +115,12 @@ uint32_t io_sbgEllipse_getComStatus(void);
  */
 uint32_t io_sbgEllipse_getOverflowCount(void);
 
+/*
+ * Get EKF Solution Mode Status
+ * @return the ekf solution mode
+ */
+uint32_t io_sbgEllipse_getEkfSolutionMode(void);
+
 /**
  * Get the IMU accelerations as a struct pointer with fields:
  * - float x: Forward acceleration in m/s^2