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

AP_AHRS: added API for forcing EKF to start

Browse Source 
used by Replay to sync start times
This commit is contained in:
Andrew Tridgell 2016-05-03 16:34:05 +10:00
parent f92279f436
commit 07060051cf
2 changed files with 14 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -118,8 +118,11 @@ void AP_AHRS_NavEKF::update_EKF1(void)
start_time_ms = AP_HAL::millis();
}
// slight extra delay on EKF1 to prioritise EKF2 for memory
if (AP_HAL::millis() - start_time_ms > startup_delay_ms + 100U) {
if (AP_HAL::millis() - start_time_ms > startup_delay_ms + 100U || force_ekf) {
ekf1_started = EKF1.InitialiseFilterDynamic();
if (force_ekf) {
return;
}
}
}
if (ekf1_started) {
@ -189,8 +192,11 @@ 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) {
if (AP_HAL::millis() - start_time_ms > startup_delay_ms || force_ekf) {
ekf2_started = EKF2.InitialiseFilter();
if (force_ekf) {
return;
}
}
}
if (ekf2_started) {

8
libraries/AP_AHRS/AP_AHRS_NavEKF.h
View File

@ -217,6 +217,9 @@ public:
bool getGpsGlitchStatus();
// used by Replay to force start at right timestamp
void force_ekf_start(void) { force_ekf = true; }
private:
enum EKF_TYPE {EKF_TYPE_NONE=0,
#if AP_AHRS_WITH_EKF1
@ -235,8 +238,9 @@ private:
NavEKF &EKF1;
NavEKF2 &EKF2;
bool ekf1_started = false;
bool ekf2_started = false;
bool ekf1_started:1;
bool ekf2_started:1;
bool force_ekf:1;
Matrix3f _dcm_matrix;
Vector3f _dcm_attitude;
Vector3f _gyro_bias;