fix(firmware): propagate gy85 i2c failures

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

@@ -3,93 +3,121 @@
 static int16_t g_offx = 0, g_offy = 0, g_offz = 0;
 
 // ---------------- Internal Functions ---------------- //
-static void GY85_SetAccelerometer(void);
+static bool GY85_SetAccelerometer(void);
-static void GY85_ReadAccelerometer(GY85_t *imu);
+static bool GY85_ReadAccelerometer(GY85_t *imu);
-static void GY85_SetCompass(void);
+static bool GY85_SetCompass(void);
-static void GY85_ReadCompass(GY85_t *imu);
+static bool GY85_ReadCompass(GY85_t *imu);
-static void GY85_SetGyro(void);
+static bool GY85_SetGyro(void);
-static void GY85_GyroCalibrate(void);
+static bool GY85_GyroCalibrate(void);
-static void GY85_ReadGyro(GY85_t *imu);
+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 ---------------- //
-void GY85_Init(void)
+bool GY85_Init(void)
 {
-    GY85_SetAccelerometer();
+    if (!GY85_SetAccelerometer()) {
-    GY85_SetCompass();
+        return false;
-    GY85_SetGyro();
+    }
+    if (!GY85_SetCompass()) {
+        return false;
+    }
+    return GY85_SetGyro();
 }
 
 // ---------------- Update All Sensors ---------------- //
-void GY85_Update(GY85_t *imu)
+bool GY85_Update(GY85_t *imu)
 {
-    GY85_ReadAccelerometer(imu);
+    GY85_t next_sample;
-    GY85_ReadCompass(imu);
+
-    GY85_ReadGyro(imu);
+    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 void GY85_SetAccelerometer(void)
+static bool GY85_SetAccelerometer(void)
 {
-    uint8_t data[2];
+    if (!GY85_WriteRegister(ADXL345_ADDR, 0x31, 0x01)) {
-    data[0] = 0x31; data[1] = 0x01;
+        return false;
-    HAL_I2C_Master_Transmit(&hi2c3, ADXL345_ADDR, data, 2, HAL_MAX_DELAY);
+    }
 
-    data[0] = 0x2D; data[1] = 0x08;
+    return GY85_WriteRegister(ADXL345_ADDR, 0x2D, 0x08);
-    HAL_I2C_Master_Transmit(&hi2c3, ADXL345_ADDR, data, 2, HAL_MAX_DELAY);
 }
 
-static void GY85_ReadAccelerometer(GY85_t *imu)
+static bool 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);
+    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 void GY85_SetCompass(void)
+static bool GY85_SetCompass(void)
 {
-    uint8_t data[2] = {0x02, 0x00};
+    return GY85_WriteRegister(HMC5883_ADDR, 0x02, 0x00);
-    HAL_I2C_Master_Transmit(&hi2c3, HMC5883_ADDR, data, 2, HAL_MAX_DELAY);
 }
 
-static void GY85_ReadCompass(GY85_t *imu)
+static bool 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);
+    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 void GY85_SetGyro(void)
+static bool GY85_SetGyro(void)
 {
-    uint8_t data[2];
+    if (!GY85_WriteRegister(ITG3200_ADDR, 0x3E, 0x00)) {
-    data[0] = 0x3E; data[1] = 0x00;
+        return false;
-    HAL_I2C_Master_Transmit(&hi2c3, ITG3200_ADDR, data, 2, HAL_MAX_DELAY);
+    }
 
-    data[0] = 0x15; data[1] = 0x07;
+    if (!GY85_WriteRegister(ITG3200_ADDR, 0x15, 0x07)) {
-    HAL_I2C_Master_Transmit(&hi2c3, ITG3200_ADDR, data, 2, HAL_MAX_DELAY);
+        return false;
+    }
 
-    data[0] = 0x16; data[1] = 0x1E;
+    if (!GY85_WriteRegister(ITG3200_ADDR, 0x16, 0x1E)) {
-    HAL_I2C_Master_Transmit(&hi2c3, ITG3200_ADDR, data, 2, HAL_MAX_DELAY);
+        return false;
+    }
 
-    data[0] = 0x17; data[1] = 0x00;
+    if (!GY85_WriteRegister(ITG3200_ADDR, 0x17, 0x00)) {
-    HAL_I2C_Master_Transmit(&hi2c3, ITG3200_ADDR, data, 2, HAL_MAX_DELAY);
+        return false;
+    }
 
     HAL_Delay(10);
-    GY85_GyroCalibrate();
+    return GY85_GyroCalibrate();
 }
 
-static void GY85_GyroCalibrate(void)
+static bool GY85_GyroCalibrate(void)
 {
     int32_t tmpx = 0, tmpy = 0, tmpz = 0;
     GY85_t imu;
@@ -97,7 +125,9 @@ static void GY85_GyroCalibrate(void)
     for(uint8_t i = 0; i < 10; i++)
     {
         HAL_Delay(10);
-        GY85_ReadGyro(&imu);
+        if (!GY85_ReadGyro(&imu)) {
+            return false;
+        }
         tmpx += imu.gx;
         tmpy += imu.gy;
         tmpz += imu.gz;
@@ -106,16 +136,38 @@ static void GY85_GyroCalibrate(void)
     g_offx = tmpx / 10;
     g_offy = tmpy / 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];
-    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;
+    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

@@ -620,8 +620,7 @@ SystemError_t checkSystemHealth(void) {
     // 4. Check IMU Communication
     static uint32_t last_imu_check = 0;
     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;
         }
         last_imu_check = HAL_GetTick();
@@ -1277,9 +1276,13 @@ int main(void)
 
 
 // Initialize module IMU
-  GY85_Init();
+  if (!GY85_Init()) {
+      Error_Handler();
+  }
   for(int i=0; i<10;i++){
-  GY85_Update(&imu);
+  if (!GY85_Update(&imu)) {
+      Error_Handler();
+  }
 
   ax = imu.ax;
   ay = imu.ay;