diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp index 217f7b863f1..6c1c7de3594 100644 --- a/libraries/AP_Compass/AP_Compass.cpp +++ b/libraries/AP_Compass/AP_Compass.cpp @@ -981,7 +981,11 @@ Compass::read(void) if (_learn == LEARN_INFLIGHT && learn != nullptr) { learn->update(); } - return healthy(); + bool ret = healthy(); + if (ret && _log_bit != (uint32_t)-1 && AP::logger().should_log(_log_bit) && !AP::ahrs().have_ekf_logging()) { + AP::logger().Write_Compass(); + } + return ret; } uint8_t diff --git a/libraries/AP_Compass/AP_Compass.h b/libraries/AP_Compass/AP_Compass.h index 50d62531d2a..8026603e70e 100644 --- a/libraries/AP_Compass/AP_Compass.h +++ b/libraries/AP_Compass/AP_Compass.h @@ -143,6 +143,9 @@ public: bool compass_cal_requires_reboot() const { return _cal_complete_requires_reboot; } bool is_calibrating() const; + // indicate which bit in LOG_BITMASK indicates we should log compass readings + void set_log_bit(uint32_t log_bit) { _log_bit = log_bit; } + /* handle an incoming MAG_CAL command */ @@ -404,6 +407,9 @@ private: // first-time-around flag used by offset nulling bool _null_init_done; + // stores which bit is used to indicate we should log compass readings + uint32_t _log_bit = -1; + // used by offset correction static const uint8_t _mag_history_size = 20;