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

AP_AHRS_NavEKF: fix ekf variable naming and initialisation

This commit is contained in:
priseborough 2016-12-19 15:38:44 +11:00 committed by Randy Mackay
parent c8bdf2fab8
commit e2757c17c8
2 changed files with 33 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

55
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -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:

9
libraries/AP_AHRS/AP_AHRS_NavEKF.h
View File

@ -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;