diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp index fd7a46468f..85129552bb 100644 --- a/libraries/AP_Baro/AP_Baro.cpp +++ b/libraries/AP_Baro/AP_Baro.cpp @@ -604,6 +604,20 @@ void AP_Baro::init(void) #endif } +bool AP_Baro::should_df_log() const +{ + DataFlash_Class *instance = DataFlash_Class::instance(); + if (instance == nullptr) { + return false; + } + if (_log_baro_bit == (uint32_t)-1) { + return false; + } + if (!instance->should_log(_log_baro_bit)) { + return false; + } + return true; +} /* call update on all drivers @@ -671,6 +685,11 @@ void AP_Baro::update(void) } } } + + // logging + if (should_df_log() && !AP::ahrs().have_ekf_logging()) { + DataFlash_Class::instance()->Log_Write_Baro(); + } } /* diff --git a/libraries/AP_Baro/AP_Baro.h b/libraries/AP_Baro/AP_Baro.h index 16a9ebc6ed..1e0e9e7db8 100644 --- a/libraries/AP_Baro/AP_Baro.h +++ b/libraries/AP_Baro/AP_Baro.h @@ -174,6 +174,10 @@ public: uint8_t get_filter_range() const { return _filter_range; } + // indicate which bit in LOG_BITMASK indicates baro logging enabled + void set_log_baro_bit(uint32_t bit) { _log_baro_bit = bit; } + bool should_df_log() const; + private: // singleton static AP_Baro *_instance; @@ -188,6 +192,8 @@ private: // what is the primary sensor at the moment? uint8_t _primary; + uint32_t _log_baro_bit = -1; + struct sensor { baro_type_t type; // 0 for air pressure (default), 1 for water pressure uint32_t last_update_ms; // last update time in ms