mirror of https://github.com/ArduPilot/ardupilot
Blimp: move error logging of sensor health into AP_Baro
This commit is contained in:
parent
5a4963e4f8
commit
8783bb9638
|
@ -278,7 +278,6 @@ Blimp::Blimp(void)
|
||||||
flightmode(&mode_manual)
|
flightmode(&mode_manual)
|
||||||
{
|
{
|
||||||
// init sensor error logging flags
|
// init sensor error logging flags
|
||||||
sensor_health.baro = true;
|
|
||||||
sensor_health.compass = true;
|
sensor_health.compass = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -189,7 +189,6 @@ private:
|
||||||
|
|
||||||
// sensor health for logging
|
// sensor health for logging
|
||||||
struct {
|
struct {
|
||||||
uint8_t baro : 1; // true if baro is healthy
|
|
||||||
uint8_t compass : 1; // true if compass is healthy
|
uint8_t compass : 1; // true if compass is healthy
|
||||||
} sensor_health;
|
} sensor_health;
|
||||||
|
|
||||||
|
|
|
@ -232,20 +232,13 @@ tuning_max : tune_max
|
||||||
logger.WriteBlock(&pkt_tune, sizeof(pkt_tune));
|
logger.WriteBlock(&pkt_tune, sizeof(pkt_tune));
|
||||||
}
|
}
|
||||||
|
|
||||||
// logs when baro or compass becomes unhealthy
|
// logs when compass becomes unhealthy
|
||||||
void Blimp::Log_Sensor_Health()
|
void Blimp::Log_Sensor_Health()
|
||||||
{
|
{
|
||||||
if (!should_log(MASK_LOG_ANY)) {
|
if (!should_log(MASK_LOG_ANY)) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
// check baro
|
|
||||||
if (sensor_health.baro != barometer.healthy()) {
|
|
||||||
sensor_health.baro = barometer.healthy();
|
|
||||||
AP::logger().Write_Error(LogErrorSubsystem::BARO,
|
|
||||||
(sensor_health.baro ? LogErrorCode::ERROR_RESOLVED : LogErrorCode::UNHEALTHY));
|
|
||||||
}
|
|
||||||
|
|
||||||
// check compass
|
// check compass
|
||||||
if (sensor_health.compass != compass.healthy()) {
|
if (sensor_health.compass != compass.healthy()) {
|
||||||
sensor_health.compass = compass.healthy();
|
sensor_health.compass = compass.healthy();
|
||||||
|
|
Loading…
Reference in New Issue