mirror of https://github.com/ArduPilot/ardupilot
AP_Baro: do dataflash logging as part of update
This commit is contained in:
parent
332560bdf7
commit
a5dc87e2af
|
@ -604,6 +604,20 @@ void AP_Baro::init(void)
|
||||||
#endif
|
#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
|
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();
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
|
|
|
@ -174,6 +174,10 @@ public:
|
||||||
|
|
||||||
uint8_t get_filter_range() const { return _filter_range; }
|
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:
|
private:
|
||||||
// singleton
|
// singleton
|
||||||
static AP_Baro *_instance;
|
static AP_Baro *_instance;
|
||||||
|
@ -188,6 +192,8 @@ private:
|
||||||
// what is the primary sensor at the moment?
|
// what is the primary sensor at the moment?
|
||||||
uint8_t _primary;
|
uint8_t _primary;
|
||||||
|
|
||||||
|
uint32_t _log_baro_bit = -1;
|
||||||
|
|
||||||
struct sensor {
|
struct sensor {
|
||||||
baro_type_t type; // 0 for air pressure (default), 1 for water pressure
|
baro_type_t type; // 0 for air pressure (default), 1 for water pressure
|
||||||
uint32_t last_update_ms; // last update time in ms
|
uint32_t last_update_ms; // last update time in ms
|
||||||
|
|
Loading…
Reference in New Issue