fix(firmware): propagate gy85 i2c failures

9_Firmware/9_1_Microcontroller/9_1_1_C_Cpp_Libraries/GY_85_HAL.c

@@ -1,121 +1,173 @@
-#include "GY_85_HAL.h"
+#include "GY_85_HAL.h"
+
+static int16_t g_offx = 0, g_offy = 0, g_offz = 0;
+
+// ---------------- Internal Functions ---------------- //
+static bool GY85_SetAccelerometer(void);
+static bool GY85_ReadAccelerometer(GY85_t *imu);
+static bool GY85_SetCompass(void);
+static bool GY85_ReadCompass(GY85_t *imu);
+static bool GY85_SetGyro(void);
+static bool GY85_GyroCalibrate(void);
+static bool GY85_ReadGyro(GY85_t *imu);
+static bool GY85_WriteRegister(uint16_t address, uint8_t reg, uint8_t value);
+static bool GY85_ReadRegisters(uint16_t address, uint8_t reg, uint8_t *buffer, uint16_t length);
+
+// ---------------- Initialization ---------------- //
+bool GY85_Init(void)
+{
+    if (!GY85_SetAccelerometer()) {
+        return false;
+    }
+    if (!GY85_SetCompass()) {
+        return false;
+    }
+    return GY85_SetGyro();
+}
+
+// ---------------- Update All Sensors ---------------- //
+bool GY85_Update(GY85_t *imu)
+{
+    GY85_t next_sample;
+
+    if (imu == NULL) {
+        return false;
+    }
+
+    next_sample = *imu;
+
+    if (!GY85_ReadAccelerometer(&next_sample)) {
+        return false;
+    }
+    if (!GY85_ReadCompass(&next_sample)) {
+        return false;
+    }
+    if (!GY85_ReadGyro(&next_sample)) {
+        return false;
+    }
+
+    *imu = next_sample;
+    return true;
+}
+
+// ---------------- Accelerometer ---------------- //
+static bool GY85_SetAccelerometer(void)
+{
+    if (!GY85_WriteRegister(ADXL345_ADDR, 0x31, 0x01)) {
+        return false;
+    }
+
+    return GY85_WriteRegister(ADXL345_ADDR, 0x2D, 0x08);
+}
+
+static bool GY85_ReadAccelerometer(GY85_t *imu)
+{
+    uint8_t buf[6];
+
+    if (imu == NULL || !GY85_ReadRegisters(ADXL345_ADDR, 0x32, buf, sizeof(buf))) {
+        return false;
+    }
+
+    imu->ax = (int16_t)((buf[1] << 8) | buf[0]);
+    imu->ay = (int16_t)((buf[3] << 8) | buf[2]);
+    imu->az = (int16_t)((buf[5] << 8) | buf[4]);
+    return true;
+}
+
+// ---------------- Compass ---------------- //
+static bool GY85_SetCompass(void)
+{
+    return GY85_WriteRegister(HMC5883_ADDR, 0x02, 0x00);
+}
+
+static bool GY85_ReadCompass(GY85_t *imu)
+{
+    uint8_t buf[6];
+
+    if (imu == NULL || !GY85_ReadRegisters(HMC5883_ADDR, 0x03, buf, sizeof(buf))) {
+        return false;
+    }
+
+    imu->mx = (int16_t)((buf[0] << 8) | buf[1]);
+    imu->mz = (int16_t)((buf[2] << 8) | buf[3]);
+    imu->my = (int16_t)((buf[4] << 8) | buf[5]);
+    return true;
+}
+
+// ---------------- Gyroscope ---------------- //
+static bool GY85_SetGyro(void)
+{
+    if (!GY85_WriteRegister(ITG3200_ADDR, 0x3E, 0x00)) {
+        return false;
+    }
+
+    if (!GY85_WriteRegister(ITG3200_ADDR, 0x15, 0x07)) {
+        return false;
+    }
+
+    if (!GY85_WriteRegister(ITG3200_ADDR, 0x16, 0x1E)) {
+        return false;
+    }
+
+    if (!GY85_WriteRegister(ITG3200_ADDR, 0x17, 0x00)) {
+        return false;
+    }
+
+    HAL_Delay(10);
+    return GY85_GyroCalibrate();
+}
+
+static bool GY85_GyroCalibrate(void)
+{
+    int32_t tmpx = 0, tmpy = 0, tmpz = 0;
+    GY85_t imu;
 
-static int16_t g_offx = 0, g_offy = 0, g_offz = 0;
+    for(uint8_t i = 0; i < 10; i++)
-
+    {
-// ---------------- Internal Functions ---------------- //
+        HAL_Delay(10);
-static void GY85_SetAccelerometer(void);
+        if (!GY85_ReadGyro(&imu)) {
-static void GY85_ReadAccelerometer(GY85_t *imu);
+            return false;
-static void GY85_SetCompass(void);
+        }
-static void GY85_ReadCompass(GY85_t *imu);
+        tmpx += imu.gx;
-static void GY85_SetGyro(void);
+        tmpy += imu.gy;
-static void GY85_GyroCalibrate(void);
+        tmpz += imu.gz;
-static void GY85_ReadGyro(GY85_t *imu);
-
-// ---------------- Initialization ---------------- //
-void GY85_Init(void)
-{
-    GY85_SetAccelerometer();
-    GY85_SetCompass();
-    GY85_SetGyro();
-}
-
-// ---------------- Update All Sensors ---------------- //
-void GY85_Update(GY85_t *imu)
-{
-    GY85_ReadAccelerometer(imu);
-    GY85_ReadCompass(imu);
-    GY85_ReadGyro(imu);
-}
-
-// ---------------- Accelerometer ---------------- //
-static void GY85_SetAccelerometer(void)
-{
-    uint8_t data[2];
-    data[0] = 0x31; data[1] = 0x01;
-    HAL_I2C_Master_Transmit(&hi2c3, ADXL345_ADDR, data, 2, HAL_MAX_DELAY);
-
-    data[0] = 0x2D; data[1] = 0x08;
-    HAL_I2C_Master_Transmit(&hi2c3, ADXL345_ADDR, data, 2, HAL_MAX_DELAY);
-}
-
-static void GY85_ReadAccelerometer(GY85_t *imu)
-{
-    uint8_t reg = 0x32;
-    uint8_t buf[6];
-    HAL_I2C_Master_Transmit(&hi2c3, ADXL345_ADDR, &reg, 1, HAL_MAX_DELAY);
-    HAL_I2C_Master_Receive(&hi2c3, ADXL345_ADDR, buf, 6, HAL_MAX_DELAY);
-
-    imu->ax = (int16_t)((buf[1] << 8) | buf[0]);
-    imu->ay = (int16_t)((buf[3] << 8) | buf[2]);
-    imu->az = (int16_t)((buf[5] << 8) | buf[4]);
-}
-
-// ---------------- Compass ---------------- //
-static void GY85_SetCompass(void)
-{
-    uint8_t data[2] = {0x02, 0x00};
-    HAL_I2C_Master_Transmit(&hi2c3, HMC5883_ADDR, data, 2, HAL_MAX_DELAY);
-}
-
-static void GY85_ReadCompass(GY85_t *imu)
-{
-    uint8_t reg = 0x03;
-    uint8_t buf[6];
-    HAL_I2C_Master_Transmit(&hi2c3, HMC5883_ADDR, &reg, 1, HAL_MAX_DELAY);
-    HAL_I2C_Master_Receive(&hi2c3, HMC5883_ADDR, buf, 6, HAL_MAX_DELAY);
-
-    imu->mx = (int16_t)((buf[0] << 8) | buf[1]);
-    imu->mz = (int16_t)((buf[2] << 8) | buf[3]);
-    imu->my = (int16_t)((buf[4] << 8) | buf[5]);
-}
-
-// ---------------- Gyroscope ---------------- //
-static void GY85_SetGyro(void)
-{
-    uint8_t data[2];
-    data[0] = 0x3E; data[1] = 0x00;
-    HAL_I2C_Master_Transmit(&hi2c3, ITG3200_ADDR, data, 2, HAL_MAX_DELAY);
-
-    data[0] = 0x15; data[1] = 0x07;
-    HAL_I2C_Master_Transmit(&hi2c3, ITG3200_ADDR, data, 2, HAL_MAX_DELAY);
-
-    data[0] = 0x16; data[1] = 0x1E;
-    HAL_I2C_Master_Transmit(&hi2c3, ITG3200_ADDR, data, 2, HAL_MAX_DELAY);
-
-    data[0] = 0x17; data[1] = 0x00;
-    HAL_I2C_Master_Transmit(&hi2c3, ITG3200_ADDR, data, 2, HAL_MAX_DELAY);
-
-    HAL_Delay(10);
-    GY85_GyroCalibrate();
-}
-
-static void GY85_GyroCalibrate(void)
-{
-    int32_t tmpx = 0, tmpy = 0, tmpz = 0;
-    GY85_t imu;
-
-    for(uint8_t i = 0; i < 10; i++)
-    {
-        HAL_Delay(10);
-        GY85_ReadGyro(&imu);
-        tmpx += imu.gx;
-        tmpy += imu.gy;
-        tmpz += imu.gz;
     }
 
-    g_offx = tmpx / 10;
+    g_offx = tmpx / 10;
-    g_offy = tmpy / 10;
+    g_offy = tmpy / 10;
-    g_offz = tmpz / 10;
+    g_offz = tmpz / 10;
-}
+    return true;
-
+}
-static void GY85_ReadGyro(GY85_t *imu)
+
-{
+static bool GY85_ReadGyro(GY85_t *imu)
-    uint8_t reg = 0x1B;
+{
-    uint8_t buf[8];
+    uint8_t buf[8];
-    HAL_I2C_Master_Transmit(&hi2c3, ITG3200_ADDR, &reg, 1, HAL_MAX_DELAY);
+
-    HAL_I2C_Master_Receive(&hi2c3, ITG3200_ADDR, buf, 8, HAL_MAX_DELAY);
+    if (imu == NULL || !GY85_ReadRegisters(ITG3200_ADDR, 0x1B, buf, sizeof(buf))) {
-
+        return false;
-    imu->gx = ((int16_t)((buf[2] << 8) | buf[3])) - g_offx;
+    }
-    imu->gy = ((int16_t)((buf[4] << 8) | buf[5])) - g_offy;
+
-    imu->gz = ((int16_t)((buf[6] << 8) | buf[7])) - g_offz;
+    imu->gx = ((int16_t)((buf[2] << 8) | buf[3])) - g_offx;
-}
+    imu->gy = ((int16_t)((buf[4] << 8) | buf[5])) - g_offy;
+    imu->gz = ((int16_t)((buf[6] << 8) | buf[7])) - g_offz;
+    return true;
+}
+
+static bool GY85_WriteRegister(uint16_t address, uint8_t reg, uint8_t value)
+{
+    uint8_t data[2] = {reg, value};
+    return HAL_I2C_Master_Transmit(&hi2c3, address, data, sizeof(data), HAL_MAX_DELAY) == HAL_OK;
+}
+
+static bool GY85_ReadRegisters(uint16_t address, uint8_t reg, uint8_t *buffer, uint16_t length)
+{
+    if (buffer == NULL) {
+        return false;
+    }
+
+    if (HAL_I2C_Master_Transmit(&hi2c3, address, &reg, 1, HAL_MAX_DELAY) != HAL_OK) {
+        return false;
+    }
+
+    return HAL_I2C_Master_Receive(&hi2c3, address, buffer, length, HAL_MAX_DELAY) == HAL_OK;
+}

9_Firmware/9_1_Microcontroller/9_1_1_C_Cpp_Libraries/GY_85_HAL.h

@@ -2,6 +2,7 @@
 #define __GY_85_HAL_H__
 
 #include "stm32f7xx_hal.h"   // change depending on MCU family
+#include <stdbool.h>
 #include <stdint.h>
 
 #define ADXL345_ADDR     (0x53 << 1)
@@ -18,8 +19,8 @@ typedef struct {
 } GY85_t;
 
 // Public API
-void GY85_Init(void);
+bool GY85_Init(void);
-void GY85_Update(GY85_t *imu);   // reads all sensors and stores in struct
+bool GY85_Update(GY85_t *imu);   // reads all sensors and stores in struct
 
 #endif
 

9_Firmware/9_1_Microcontroller/9_1_3_C_Cpp_Code/main.cpp

@@ -617,14 +617,13 @@ SystemError_t checkSystemHealth(void) {
         }
     }
 
-    // 4. Check IMU Communication
+    // 4. Check IMU Communication
-    static uint32_t last_imu_check = 0;
+    static uint32_t last_imu_check = 0;
-    if (HAL_GetTick() - last_imu_check > 10000) {
+    if (HAL_GetTick() - last_imu_check > 10000) {
-        GY85_Update(&imu);
+        if (!GY85_Update(&imu)) {
-        if (isnan(imu.ax) || isnan(imu.ay) || isnan(imu.az)) {
+            current_error = ERROR_IMU_COMM;
-            current_error = ERROR_IMU_COMM;
+        }
-        }
+        last_imu_check = HAL_GetTick();
-        last_imu_check = HAL_GetTick();
     }
 
     // 5. Check BMP180 Communication
@@ -1274,14 +1273,18 @@ int main(void)
   HAL_Delay(100);
   HAL_GPIO_WritePin(EN_P_3V3_FPGA_GPIO_Port,EN_P_3V3_FPGA_Pin,GPIO_PIN_SET);
   HAL_Delay(100);
-
+
-
+
-// Initialize module IMU
+// Initialize module IMU
-  GY85_Init();
+  if (!GY85_Init()) {
-  for(int i=0; i<10;i++){
+      Error_Handler();
-  GY85_Update(&imu);
+  }
-
+  for(int i=0; i<10;i++){
-  ax = imu.ax;
+  if (!GY85_Update(&imu)) {
+      Error_Handler();
+  }
+
+  ax = imu.ax;
   ay = imu.ay;
   az = imu.az;
   gx = -imu.gx;