Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-21 07:15:18 +03:00
Code Issues Wiki Activity

Moved compass health check into compass.c

Browse source
This commit is contained in:
Martin Budden 2018-01-20 11:59:20 +00:00
parent ded85bfce1
commit 3116ef4008
4 changed files with 13 additions and 9 deletions
Download patch file Download diff file Expand all files Collapse all files

9
src/main/flight/imu.c
View file

@ -402,11 +402,6 @@ static bool imuIsAccelerometerHealthy(void)
return (81 < accMagnitude) && (accMagnitude < 121); return (81 < accMagnitude) && (accMagnitude < 121);
} }
static bool isMagnetometerHealthy(void)
{
return (mag.magADC[X] != 0) && (mag.magADC[Y] != 0) && (mag.magADC[Z] != 0);
}
static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs) static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
{ {
static uint32_t previousIMUUpdateTime; static uint32_t previousIMUUpdateTime;
@ -422,9 +417,11 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
useAcc = true; useAcc = true;
} }
if (sensors(SENSOR_MAG) && isMagnetometerHealthy()) { #ifdef USE_MAG
if (sensors(SENSOR_MAG) && compassIsHealthy()) {
useMag = true; useMag = true;
} }
#endif
#if defined(USE_GPS) #if defined(USE_GPS)
else if (STATE(FIXED_WING) && sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5 && gpsSol.groundSpeed >= 300) { else if (STATE(FIXED_WING) && sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5 && gpsSol.groundSpeed >= 300) {
// In case of a fixed-wing aircraft we can use GPS course over ground to correct heading // In case of a fixed-wing aircraft we can use GPS course over ground to correct heading

7
src/main/sensors/compass.c
View file

@ -262,6 +262,11 @@ bool compassInit(void)
return true; return true;
} }
bool compassIsHealthy(void)
{
return (mag.magADC[X] != 0) && (mag.magADC[Y] != 0) && (mag.magADC[Z] != 0);
}
void compassUpdate(timeUs_t currentTimeUs) void compassUpdate(timeUs_t currentTimeUs)
{ {
static timeUs_t tCal = 0; static timeUs_t tCal = 0;
@ -310,4 +315,4 @@ void compassUpdate(timeUs_t currentTimeUs)
} }
} }
} }
#endif #endif // USE_MAG

5
src/main/sensors/compass.h
View file

@ -17,9 +17,9 @@
#pragma once #pragma once
#include "common/time.h"
#include "drivers/io_types.h" #include "drivers/io_types.h"
#include "drivers/sensor.h" #include "drivers/sensor.h"
#include "common/time.h"
#include "pg/pg.h" #include "pg/pg.h"
#include "sensors/sensors.h" #include "sensors/sensors.h"
@ -56,6 +56,7 @@ typedef struct compassConfig_s {
PG_DECLARE(compassConfig_t, compassConfig); PG_DECLARE(compassConfig_t, compassConfig);
bool compassInit(void); bool compassIsHealthy(void);
void compassUpdate(timeUs_t currentTime); void compassUpdate(timeUs_t currentTime);
bool compassInit(void);

1
src/test/unit/flight_imu_unittest.cc
View file

@ -234,6 +234,7 @@ bool sensors(uint32_t mask)
uint32_t millis(void) { return 0; } uint32_t millis(void) { return 0; }
uint32_t micros(void) { return 0; } uint32_t micros(void) { return 0; }
bool compassIsHealthy(void) { return true; }
bool isBaroCalibrationComplete(void) { return true; } bool isBaroCalibrationComplete(void) { return true; }
void performBaroCalibrationCycle(void) {} void performBaroCalibrationCycle(void) {}
int32_t baroCalculateAltitude(void) { return 0; } int32_t baroCalculateAltitude(void) { return 0; }