Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_AHRS: delay EKF start until logging ready 

for replay to work reliably we need all the parameters output before
we init the EKF.

Co-authored-by: Peter Barker <pbarker@barker.dropbear.id.au>
This commit is contained in:
Andrew Tridgell 2020-11-06 10:30:50 +11:00
parent 096aab9388
commit 31fbb59384
2 changed files with 23 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -26,6 +26,7 @@
#include <AP_GPS/AP_GPS.h>
#include <AP_Baro/AP_Baro.h>
#include <AP_InternalError/AP_InternalError.h>
#include <AP_Logger/AP_Logger.h>
#include <AP_Notify/AP_Notify.h>
#include <AP_Vehicle/AP_Vehicle_Type.h>
@ -194,12 +195,19 @@ void AP_AHRS_NavEKF::update_EKF2(void)
if (start_time_ms == 0) {
start_time_ms = AP_HAL::millis();
}
if (AP_HAL::millis() - start_time_ms > startup_delay_ms || _force_ekf) {
_ekf2_started = EKF2.InitialiseFilter();
if (_force_ekf) {
return;
// if we're doing Replay logging then don't allow any data
// into the EKF yet. Don't allow it to block us for long.
if (!hal.util->was_watchdog_reset()) {
if (AP_HAL::millis() - start_time_ms < 5000) {
if (!AP::logger().allow_start_ekf()) {
return;
}
}
}
if (AP_HAL::millis() - start_time_ms > startup_delay_ms) {
_ekf2_started = EKF2.InitialiseFilter();
}
}
if (_ekf2_started) {
EKF2.UpdateFilter();
@ -268,12 +276,18 @@ void AP_AHRS_NavEKF::update_EKF3(void)
if (start_time_ms == 0) {
start_time_ms = AP_HAL::millis();
}
if (AP_HAL::millis() - start_time_ms > startup_delay_ms || _force_ekf) {
_ekf3_started = EKF3.InitialiseFilter();
if (_force_ekf) {
return;
// if we're doing Replay logging then don't allow any data
// into the EKF yet. Don't allow it to block us for long.
if (!hal.util->was_watchdog_reset()) {
if (AP_HAL::millis() - start_time_ms < 5000) {
if (!AP::logger().allow_start_ekf()) {
return;
}
}
}
if (AP_HAL::millis() - start_time_ms > startup_delay_ms) {
_ekf3_started = EKF3.InitialiseFilter();
}
}
if (_ekf3_started) {
EKF3.UpdateFilter();

6
libraries/AP_AHRS/AP_AHRS_NavEKF.h
View File

@ -277,9 +277,6 @@ public:
bool getGpsGlitchStatus() const;
// used by Replay to force start at right timestamp
void force_ekf_start(void) { _force_ekf = true; }
// is the EKF backend doing its own sensor logging?
bool have_ekf_logging(void) const override;
@ -348,8 +345,7 @@ private:
bool _ekf3_started;
void update_EKF3(void);
#endif
bool _force_ekf;
// rotation from vehicle body to NED frame
Matrix3f _dcm_matrix;
Vector3f _dcm_attitude;