|
|
@ -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(); |
|
|
|