diff --git a/src/imu/imu.cpp b/src/imu/imu.cpp index 348f462..809d37e 100644 --- a/src/imu/imu.cpp +++ b/src/imu/imu.cpp @@ -53,8 +53,14 @@ void setupICM(void) bool getAccelerometer(float *acc_x, float *acc_y, float *acc_z) { #if SF_PLATFORM == SF_PLATFORM_PARTICLE - ICM_20948_AGMT_t agmt = myICM.getAGMT(); + myICM.getAGMT(); + if (myICM.status != ICM_20948_Stat_Ok) + { + FLOG_AddError(FLOG_ICM_FAIL, 0); + return false; + } + ICM_20948_AGMT_t agmt = myICM.agmt; *acc_x = getAccMG(agmt.acc.axes.x, agmt.fss.a); *acc_y = getAccMG(agmt.acc.axes.y, agmt.fss.a); *acc_z = getAccMG(agmt.acc.axes.z, agmt.fss.a); @@ -65,7 +71,14 @@ bool getAccelerometer(float *acc_x, float *acc_y, float *acc_z) bool getGyroscope(float *gyr_x, float *gyr_y, float *gyr_z) { #if SF_PLATFORM == SF_PLATFORM_PARTICLE - ICM_20948_AGMT_t agmt = myICM.getAGMT(); + myICM.getAGMT(); + + if (myICM.status != ICM_20948_Stat_Ok) + { + FLOG_AddError(FLOG_ICM_FAIL, 0); + return false; + } + ICM_20948_AGMT_t agmt = myICM.agmt; *gyr_x = getGyrDPS(agmt.gyr.axes.x, agmt.fss.g); *gyr_y = getGyrDPS(agmt.gyr.axes.y, agmt.fss.g); @@ -77,7 +90,14 @@ bool getGyroscope(float *gyr_x, float *gyr_y, float *gyr_z) bool getMagnetometer(float *mag_x, float *mag_y, float *mag_z) { #if SF_PLATFORM == SF_PLATFORM_PARTICLE - ICM_20948_AGMT_t agmt = myICM.getAGMT(); + myICM.getAGMT(); + + if (myICM.status != ICM_20948_Stat_Ok) + { + FLOG_AddError(FLOG_ICM_FAIL, 0); + return false; + } + ICM_20948_AGMT_t agmt = myICM.agmt; *mag_x = getMagUT(agmt.mag.axes.x); *mag_y = getMagUT(agmt.mag.axes.y);