FlintyLemming/PLFM_RADAR
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Merge pull request #7 from walidb212/fix/gy85-i2c-status

Browse Source 
fix(firmware): propagate gy85 i2c failures
This commit is contained in:
NawfalMotii79
2026-03-15 01:57:28 +00:00
committed by GitHub
parent eca26f413d 9bb6080a73
commit 74d5a76abb
3 changed files with 192 additions and 136 deletions
Download Patch File Download Diff File Expand all files Collapse all files
9_Firmware/9_1_Microcontroller/9_1_1_C_Cpp_Libraries/GY_85_HAL.c
+170 -118
View File
@@ -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, &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;
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, &reg, 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, &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
+3 -2
View File
@@ -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);
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
9_Firmware/9_1_Microcontroller/9_1_3_C_Cpp_Code/main.cpp
+19 -16
View File
@@ -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;