aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Compass: fixed compass logging when first compass unhealthy

Browse Source 
keep logging compasses if any are healthy
This commit is contained in:
Andrew Tridgell 2020-08-28 14:08:48 +10:00
parent 80ef6fa7f2
commit 37866ec3cf
1 changed files with 6 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_Compass/AP_Compass.cpp
View File

@ -1483,8 +1483,10 @@ Compass::read(void)
_backends[i]->read(); _backends[i]->read();
} }
uint32_t time = AP_HAL::millis(); uint32_t time = AP_HAL::millis();
bool any_healthy = false;
for (StateIndex i(0); i < COMPASS_MAX_INSTANCES; i++) { for (StateIndex i(0); i < COMPASS_MAX_INSTANCES; i++) {
_state[i].healthy = (time - _state[i].last_update_ms < 500); _state[i].healthy = (time - _state[i].last_update_ms < 500);
any_healthy |= _state[i].healthy;
} }
#if COMPASS_LEARN_ENABLED #if COMPASS_LEARN_ENABLED
if (_learn == LEARN_INFLIGHT && !learn_allocated) { if (_learn == LEARN_INFLIGHT && !learn_allocated) {
@ -1494,14 +1496,13 @@ Compass::read(void)
if (_learn == LEARN_INFLIGHT && learn != nullptr) { if (_learn == LEARN_INFLIGHT && learn != nullptr) {
learn->update(); learn->update();
} }
bool ret = healthy(); #endif
if (ret && _log_bit != (uint32_t)-1 && AP::logger().should_log(_log_bit) && !AP::ahrs().have_ekf_logging()) { #ifndef HAL_NO_LOGGING
if (any_healthy && _log_bit != (uint32_t)-1 && AP::logger().should_log(_log_bit) && !AP::ahrs().have_ekf_logging()) {
AP::logger().Write_Compass(); AP::logger().Write_Compass();
} }
return ret;
#else
return healthy();
#endif #endif
return healthy();
} }
uint8_t uint8_t