From 9bb6080a73b8ef54c8cb36e7ac241faaea6a5c78 Mon Sep 17 00:00:00 2001 From: wb-eugenia Date: Fri, 13 Mar 2026 19:38:42 +0100 Subject: [PATCH] fix(firmware): propagate gy85 i2c failures --- .../9_1_1_C_Cpp_Libraries/GY_85_HAL.c | 288 +++++++++++------- .../9_1_1_C_Cpp_Libraries/GY_85_HAL.h | 5 +- .../9_1_3_C_Cpp_Code/main.cpp | 35 ++- 3 files changed, 192 insertions(+), 136 deletions(-) diff --git a/9_Firmware/9_1_Microcontroller/9_1_1_C_Cpp_Libraries/GY_85_HAL.c b/9_Firmware/9_1_Microcontroller/9_1_1_C_Cpp_Libraries/GY_85_HAL.c index 2c735fd..6f6c8e9 100644 --- a/9_Firmware/9_1_Microcontroller/9_1_1_C_Cpp_Libraries/GY_85_HAL.c +++ b/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; - -// ---------------- Internal Functions ---------------- // -static void GY85_SetAccelerometer(void); -static void GY85_ReadAccelerometer(GY85_t *imu); -static void GY85_SetCompass(void); -static void GY85_ReadCompass(GY85_t *imu); -static void GY85_SetGyro(void); -static void GY85_GyroCalibrate(void); -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, ®, 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, ®, 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; + for(uint8_t i = 0; i < 10; i++) + { + HAL_Delay(10); + if (!GY85_ReadGyro(&imu)) { + return false; + } + tmpx += imu.gx; + tmpy += imu.gy; + tmpz += imu.gz; } - g_offx = tmpx / 10; - g_offy = tmpy / 10; - g_offz = tmpz / 10; -} - -static void GY85_ReadGyro(GY85_t *imu) -{ - uint8_t reg = 0x1B; - uint8_t buf[8]; - HAL_I2C_Master_Transmit(&hi2c3, ITG3200_ADDR, ®, 1, HAL_MAX_DELAY); - HAL_I2C_Master_Receive(&hi2c3, ITG3200_ADDR, buf, 8, HAL_MAX_DELAY); - - 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; -} + g_offx = tmpx / 10; + g_offy = tmpy / 10; + g_offz = tmpz / 10; + return true; +} + +static bool GY85_ReadGyro(GY85_t *imu) +{ + uint8_t buf[8]; + + 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, ®, 1, HAL_MAX_DELAY) != HAL_OK) { + return false; + } + + return HAL_I2C_Master_Receive(&hi2c3, address, buffer, length, HAL_MAX_DELAY) == HAL_OK; +} diff --git a/9_Firmware/9_1_Microcontroller/9_1_1_C_Cpp_Libraries/GY_85_HAL.h b/9_Firmware/9_1_Microcontroller/9_1_1_C_Cpp_Libraries/GY_85_HAL.h index 9af4e50..085572e 100644 --- a/9_Firmware/9_1_Microcontroller/9_1_1_C_Cpp_Libraries/GY_85_HAL.h +++ b/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 #include #define ADXL345_ADDR (0x53 << 1) @@ -18,8 +19,8 @@ typedef struct { } GY85_t; // Public API -void GY85_Init(void); -void GY85_Update(GY85_t *imu); // reads all sensors and stores in struct +bool GY85_Init(void); +bool GY85_Update(GY85_t *imu); // reads all sensors and stores in struct #endif diff --git a/9_Firmware/9_1_Microcontroller/9_1_3_C_Cpp_Code/main.cpp b/9_Firmware/9_1_Microcontroller/9_1_3_C_Cpp_Code/main.cpp index 0daa07d..b158751 100644 --- a/9_Firmware/9_1_Microcontroller/9_1_3_C_Cpp_Code/main.cpp +++ b/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 - static uint32_t last_imu_check = 0; - if (HAL_GetTick() - last_imu_check > 10000) { - GY85_Update(&imu); - if (isnan(imu.ax) || isnan(imu.ay) || isnan(imu.az)) { - current_error = ERROR_IMU_COMM; - } - last_imu_check = HAL_GetTick(); + // 4. Check IMU Communication + static uint32_t last_imu_check = 0; + if (HAL_GetTick() - last_imu_check > 10000) { + if (!GY85_Update(&imu)) { + current_error = ERROR_IMU_COMM; + } + 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 - GY85_Init(); - for(int i=0; i<10;i++){ - GY85_Update(&imu); - - ax = imu.ax; + + +// Initialize module IMU + if (!GY85_Init()) { + Error_Handler(); + } + for(int i=0; i<10;i++){ + if (!GY85_Update(&imu)) { + Error_Handler(); + } + + ax = imu.ax; ay = imu.ay; az = imu.az; gx = -imu.gx;