Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-26 17:55:28 +03:00
Code Issues Wiki Activity

hardware status monitoring - generic interface

Browse source
This commit is contained in:
Krzysztof Matula 2016-11-03 00:06:53 +01:00 • committed by Konstantin Sharlaimov (DigitalEntity)
parent d4efddcbf4
commit ab29fb8f9f
7 changed files with 40 additions and 11 deletions
Download patch file Download diff file Expand all files Collapse all files

1
Makefile
View file

@ -452,6 +452,7 @@ COMMON_SRC = \
sensors/compass.c \ sensors/compass.c \
sensors/gyro.c \ sensors/gyro.c \
sensors/initialisation.c \ sensors/initialisation.c \
sensors/diagnostics.c \
$(CMSIS_SRC) \ $(CMSIS_SRC) \
$(DEVICE_STDPERIPH_SRC) $(DEVICE_STDPERIPH_SRC)

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

@ -423,11 +423,6 @@ static int imuCalculateAccelerometerConfidence(void)
return (nearness > MAX_ACC_SQ_NEARNESS) ? 0 : MAX_ACC_SQ_NEARNESS - nearness; return (nearness > MAX_ACC_SQ_NEARNESS) ? 0 : MAX_ACC_SQ_NEARNESS - nearness;
} }
static bool isMagnetometerHealthy(void)
{
return (mag.magADC[X] != 0) && (mag.magADC[Y] != 0) && (mag.magADC[Z] != 0);
}
static void imuCalculateEstimatedAttitude(float dT) static void imuCalculateEstimatedAttitude(float dT)
{ {
const bool canUseMAG = sensors(SENSOR_MAG) && isMagnetometerHealthy(); const bool canUseMAG = sensors(SENSOR_MAG) && isMagnetometerHealthy();

4
src/main/io/ledstrip.c
View file

@ -533,10 +533,8 @@ static void applyLedWarningLayer(bool updateNow, timeUs_t *timer)
warningFlags |= 1 << WARNING_FAILSAFE; warningFlags |= 1 << WARNING_FAILSAFE;
if (!ARMING_FLAG(ARMED) && !ARMING_FLAG(OK_TO_ARM)) if (!ARMING_FLAG(ARMED) && !ARMING_FLAG(OK_TO_ARM))
warningFlags |= 1 << WARNING_ARMING_DISABLED; warningFlags |= 1 << WARNING_ARMING_DISABLED;
#ifdef MAG if (!isHardwareHealthy())
if (masterConfig.mag_hardware != MAG_NONE && !compassIsWorking())
warningFlags |= 1 << WARNING_HW_ERROR; warningFlags |= 1 << WARNING_HW_ERROR;
#endif
} }
*timer += LED_STRIP_HZ(10); *timer += LED_STRIP_HZ(10);
} }

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

@ -63,9 +63,9 @@ bool compassInit(const compassConfig_t *compassConfig)
return ret; return ret;
} }
bool compassIsWorking(void) bool isCompassHealthy(void)
{ {
return (magADC[X] | magADC[Y] | magADC[Z]) != 0; return (magADC[X] != 0) && (magADC[Y] != 0) && (magADC[Z] != 0);
} }
bool isCompassReady(void) bool isCompassReady(void)

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

@ -55,4 +55,4 @@ bool compassInit(const compassConfig_t *compassConfig);
union flightDynamicsTrims_u; union flightDynamicsTrims_u;
void compassUpdate(timeUs_t currentTimeUs, union flightDynamicsTrims_u *magZero); void compassUpdate(timeUs_t currentTimeUs, union flightDynamicsTrims_u *magZero);
bool isCompassReady(void); bool isCompassReady(void);
bool compassIsWorking(void); bool isCompassHealthy(void);

27
src/main/sensors/diagnostics.c Normal file
View file

@ -0,0 +1,27 @@
#include <stdbool.h>
#include <stdint.h>
#include "platform.h"
#include "build/debug.h"
#include "common/axis.h"
#include "common/maths.h"
#include "flight/navigation_rewrite.h"
#include "config/config.h"
#include "config/config_master.h"
#include "sensors/sensors.h"
#include "sensors/compass.h"
bool isHardwareHealthy(void)
{
#ifdef MAG
if (masterConfig.mag_hardware != MAG_NONE && !isCompassHealthy())
return false;
#endif
return true;
}

8
src/main/sensors/diagnostics.h Normal file
View file

@ -0,0 +1,8 @@
/**
* Collects hardware failure information from devices.
* No hardware checking should be actually performed,
* rather already known hardware status should be returned.
*/
bool isHardwareHealthy(void);