Adi/gps error handling

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