Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-19 14:25:20 +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);
}
static bool isMagnetometerHealthy(void)
{
return (mag.magADC[X] != 0) && (mag.magADC[Y] != 0) && (mag.magADC[Z] != 0);
}
static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
{
static uint32_t previousIMUUpdateTime;
@ -422,9 +417,11 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
useAcc = true;
}
if (sensors(SENSOR_MAG) && isMagnetometerHealthy()) {
#ifdef USE_MAG
if (sensors(SENSOR_MAG) && compassIsHealthy()) {
useMag = true;
}
#endif
#if defined(USE_GPS)
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

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

@ -262,6 +262,11 @@ bool compassInit(void)
return true;
}
bool compassIsHealthy(void)
{
return (mag.magADC[X] != 0) && (mag.magADC[Y] != 0) && (mag.magADC[Z] != 0);
}
void compassUpdate(timeUs_t currentTimeUs)
{
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
#include "common/time.h"
#include "drivers/io_types.h"
#include "drivers/sensor.h"
#include "common/time.h"
#include "pg/pg.h"
#include "sensors/sensors.h"
@ -56,6 +56,7 @@ typedef struct compassConfig_s {
PG_DECLARE(compassConfig_t, compassConfig);
bool compassInit(void);
bool compassIsHealthy(void);
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 micros(void) { return 0; }
bool compassIsHealthy(void) { return true; }
bool isBaroCalibrationComplete(void) { return true; }
void performBaroCalibrationCycle(void) {}
int32_t baroCalculateAltitude(void) { return 0; }