diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index ae5fe84f3d..1ad45f68b5 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -33,6 +33,9 @@ AP_AHRS_NavEKF::AP_AHRS_NavEKF(AP_InertialSensor &ins, AP_Baro &baro, AP_GPS &gp AP_AHRS_DCM(ins, baro, gps), EKF2(_EKF2), EKF3(_EKF3), + _ekf2_started(false), + _ekf3_started(false), + _force_ekf(false), _ekf_flags(flags) { _dcm_matrix.identity(); @@ -116,19 +119,19 @@ void AP_AHRS_NavEKF::update_DCM(void) void AP_AHRS_NavEKF::update_EKF2(void) { - if (!ekf2_started) { + if (!_ekf2_started) { // wait 1 second for DCM to output a valid tilt error estimate 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) { + if (AP_HAL::millis() - start_time_ms > startup_delay_ms || _force_ekf) { + _ekf2_started = EKF2.InitialiseFilter(); + if (_force_ekf) { return; } } } - if (ekf2_started) { + if (_ekf2_started) { EKF2.UpdateFilter(); if (active_EKF_type() == EKF_TYPE2) { Vector3f eulers; @@ -181,19 +184,19 @@ void AP_AHRS_NavEKF::update_EKF2(void) void AP_AHRS_NavEKF::update_EKF3(void) { - if (!ekf3_started) { + if (!_ekf3_started) { // wait 1 second for DCM to output a valid tilt error estimate 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) { + if (AP_HAL::millis() - start_time_ms > startup_delay_ms || _force_ekf) { + _ekf3_started = EKF3.InitialiseFilter(); + if (_force_ekf) { return; } } } - if (ekf3_started) { + if (_ekf3_started) { EKF3.UpdateFilter(); if (active_EKF_type() == EKF_TYPE3) { Vector3f eulers; @@ -301,11 +304,11 @@ void AP_AHRS_NavEKF::reset(bool recover_eulers) { AP_AHRS_DCM::reset(recover_eulers); _dcm_attitude(roll, pitch, yaw); - if (ekf2_started) { - ekf2_started = EKF2.InitialiseFilter(); + if (_ekf2_started) { + _ekf2_started = EKF2.InitialiseFilter(); } - if (ekf3_started) { - ekf3_started = EKF3.InitialiseFilter(); + if (_ekf3_started) { + _ekf3_started = EKF3.InitialiseFilter(); } } @@ -314,11 +317,11 @@ void AP_AHRS_NavEKF::reset_attitude(const float &_roll, const float &_pitch, con { AP_AHRS_DCM::reset_attitude(_roll, _pitch, _yaw); _dcm_attitude(roll, pitch, yaw); - if (ekf2_started) { - ekf2_started = EKF2.InitialiseFilter(); + if (_ekf2_started) { + _ekf2_started = EKF2.InitialiseFilter(); } - if (ekf3_started) { - ekf3_started = EKF3.InitialiseFilter(); + if (_ekf3_started) { + _ekf3_started = EKF3.InitialiseFilter(); } } @@ -437,7 +440,7 @@ bool AP_AHRS_NavEKF::get_secondary_attitude(Vector3f &eulers) case EKF_TYPE_NONE: // EKF is secondary EKF2.getEulerAngles(-1, eulers); - return ekf2_started; + return _ekf2_started; case EKF_TYPE2: @@ -457,7 +460,7 @@ bool AP_AHRS_NavEKF::get_secondary_position(struct Location &loc) case EKF_TYPE_NONE: // EKF is secondary EKF2.getLLH(loc); - return ekf2_started; + return _ekf2_started; case EKF_TYPE2: @@ -767,7 +770,7 @@ AP_AHRS_NavEKF::EKF_TYPE AP_AHRS_NavEKF::active_EKF_type(void) const case 2: { // do we have an EKF2 yet? - if (!ekf2_started) { + if (!_ekf2_started) { return EKF_TYPE_NONE; } if (always_use_EKF()) { @@ -784,7 +787,7 @@ AP_AHRS_NavEKF::EKF_TYPE AP_AHRS_NavEKF::active_EKF_type(void) const case 3: { // do we have an EKF3 yet? - if (!ekf3_started) { + if (!_ekf3_started) { return EKF_TYPE_NONE; } if (always_use_EKF()) { @@ -878,7 +881,7 @@ bool AP_AHRS_NavEKF::healthy(void) const return AP_AHRS_DCM::healthy(); case 2: { - bool ret = ekf2_started && EKF2.healthy(); + bool ret = _ekf2_started && EKF2.healthy(); if (!ret) { return false; } @@ -893,7 +896,7 @@ bool AP_AHRS_NavEKF::healthy(void) const } case 3: { - bool ret = ekf3_started && EKF3.healthy(); + bool ret = _ekf3_started && EKF3.healthy(); if (!ret) { return false; } @@ -931,11 +934,11 @@ bool AP_AHRS_NavEKF::initialised(void) const case 2: default: // initialisation complete 10sec after ekf has started - return (ekf2_started && (AP_HAL::millis() - start_time_ms > AP_AHRS_NAVEKF_SETTLE_TIME_MS)); + return (_ekf2_started && (AP_HAL::millis() - start_time_ms > AP_AHRS_NAVEKF_SETTLE_TIME_MS)); case 3: // initialisation complete 10sec after ekf has started - return (ekf3_started && (AP_HAL::millis() - start_time_ms > AP_AHRS_NAVEKF_SETTLE_TIME_MS)); + return (_ekf3_started && (AP_HAL::millis() - start_time_ms > AP_AHRS_NAVEKF_SETTLE_TIME_MS)); #if CONFIG_HAL_BOARD == HAL_BOARD_SITL case EKF_TYPE_SITL: diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h index f2a9789217..cdba902262 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.h @@ -221,7 +221,7 @@ public: bool getGpsGlitchStatus(); // used by Replay to force start at right timestamp - void force_ekf_start(void) { force_ekf = true; } + void force_ekf_start(void) { _force_ekf = true; } // is the EKF backend doing its own sensor logging? bool have_ekf_logging(void) const override; @@ -248,10 +248,9 @@ private: NavEKF2 &EKF2; NavEKF3 &EKF3; - bool ekf1_started:1; - bool ekf2_started:1; - bool ekf3_started:1; - bool force_ekf:1; + bool _ekf2_started; + bool _ekf3_started; + bool _force_ekf; Matrix3f _dcm_matrix; Vector3f _dcm_attitude; Vector3f _gyro_bias;