From 7ba45444a2ab668d954a04e8337ef6a32f7dbbb8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 23 Sep 2015 11:40:25 +1000 Subject: [PATCH] AP_AHRS: added selection of EKF type using AHRS_EKF_TYPE --- libraries/AP_AHRS/AP_AHRS.cpp | 16 +- libraries/AP_AHRS/AP_AHRS.h | 8 +- libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 442 +++++++++++++++++++++------ libraries/AP_AHRS/AP_AHRS_NavEKF.h | 31 +- 4 files changed, 379 insertions(+), 118 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 1b48237776..edf11ed9d6 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -18,10 +18,6 @@ #include extern const AP_HAL::HAL& hal; -#if AHRS_EKF_USE_ALWAYS -const int8_t AP_AHRS::_ekf_use; -#endif - // table of user settable parameters const AP_Param::GroupInfo AP_AHRS::var_info[] PROGMEM = { // index 0 and 1 are for old parameters that are no longer not used @@ -115,13 +111,15 @@ const AP_Param::GroupInfo AP_AHRS::var_info[] PROGMEM = { // NOTE: index 12 was for GPS_DELAY, but now removed, fixed delay // of 1 was found to be the best choice -#if AP_AHRS_NAVEKF_AVAILABLE && !AHRS_EKF_USE_ALWAYS - // @Param: EKF_USE + // 13 was the old EKF_USE + +#if AP_AHRS_NAVEKF_AVAILABLE + // @Param: EKF_TYPE // @DisplayName: Use NavEKF Kalman filter for attitude and position estimation - // @Description: This controls whether the NavEKF Kalman filter is used for attitude and position estimation and whether fallback to the DCM algorithm is allowed - // @Values: 0:Disabled,1:Enabled, 2:Enabled - No Fallback + // @Description: This controls whether the NavEKF Kalman filter is used for attitude and position estimation and whether fallback to the DCM algorithm is allowed. Note that on copters "disabled" is not available, and will be the same as "enabled - no fallback" + // @Values: 0:Disabled,1:Enabled,2:Enable EKF2 // @User: Advanced - AP_GROUPINFO("EKF_USE", 13, AP_AHRS, _ekf_use, AHRS_EKF_USE_DEFAULT), + AP_GROUPINFO("EKF_TYPE", 14, AP_AHRS, _ekf_type, 1), #endif AP_GROUPEND diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index f5ab2da3d3..708cef2753 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -53,6 +53,7 @@ #define EKF_DO_NOT_USE 0 // Prevents the EKF from being used by the flight controllers #define EKF_USE_WITH_FALLBACK 1 // Uses the EKF unless its solution is unhealthy or not initialised. This allows sensor errors to cause reversion. #define EKF_USE_WITHOUT_FALLBACK 2 // Uses the EKF unless it encounters numerical processing errors or isn't iniitalised. Sensor errors will not cause reversion. +#define EKF_USE_SECONDARY 3 // Use 2nd EKF if available enum AHRS_VehicleClass { AHRS_VEHICLE_UNKNOWN, @@ -379,12 +380,7 @@ protected: AP_Int8 _board_orientation; AP_Int8 _gps_minsats; AP_Int8 _gps_delay; - -#if AHRS_EKF_USE_ALWAYS - static const int8_t _ekf_use = EKF_USE_WITHOUT_FALLBACK; -#else - AP_Int8 _ekf_use; -#endif + AP_Int8 _ekf_type; // flags structure struct ahrs_flags { diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index 0c2d47f839..746ea6728c 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -60,10 +60,18 @@ void AP_AHRS_NavEKF::reset_gyro_drift(void) AP_AHRS_DCM::reset_gyro_drift(); // reset the EKF gyro bias states - EKF.resetGyroBias(); + EKF1.resetGyroBias(); + EKF2.resetGyroBias(); } void AP_AHRS_NavEKF::update(void) +{ + update_DCM(); + update_EKF1(); + update_EKF2(); +} + +void AP_AHRS_NavEKF::update_DCM(void) { // we need to restore the old DCM attitude values as these are // used internally in DCM to calculate error values for gyro drift @@ -77,22 +85,25 @@ void AP_AHRS_NavEKF::update(void) // keep DCM attitude available for get_secondary_attitude() _dcm_attitude(roll, pitch, yaw); +} - if (!ekf_started) { +void AP_AHRS_NavEKF::update_EKF1(void) +{ + if (!ekf1_started) { // wait 1 second for DCM to output a valid tilt error estimate if (start_time_ms == 0) { start_time_ms = hal.scheduler->millis(); } if (hal.scheduler->millis() - start_time_ms > startup_delay_ms) { - ekf_started = EKF.InitialiseFilterDynamic(); + ekf1_started = EKF1.InitialiseFilterDynamic(); } } - if (ekf_started) { - EKF.UpdateFilter(); - EKF.getRotationBodyToNED(_dcm_matrix); - if (using_EKF()) { + if (ekf1_started) { + EKF1.UpdateFilter(); + EKF1.getRotationBodyToNED(_dcm_matrix); + if (using_EKF() == EKF_TYPE1) { Vector3f eulers; - EKF.getEulerAngles(eulers); + EKF1.getEulerAngles(eulers); roll = eulers.x; pitch = eulers.y; yaw = eulers.z; @@ -101,7 +112,7 @@ void AP_AHRS_NavEKF::update(void) update_trig(); // keep _gyro_bias for get_gyro_drift() - EKF.getGyroBias(_gyro_bias); + EKF1.getGyroBias(_gyro_bias); _gyro_bias = -_gyro_bias; // calculate corrected gryo estimate for get_gyro() @@ -119,7 +130,7 @@ void AP_AHRS_NavEKF::update(void) _gyro_estimate += _gyro_bias; float abias1, abias2; - EKF.getAccelZBias(abias1, abias2); + EKF1.getAccelZBias(abias1, abias2); // update _accel_ef_ekf for (uint8_t i=0; i<_ins.get_accel_count(); i++) { @@ -136,7 +147,77 @@ void AP_AHRS_NavEKF::update(void) if(_ins.use_accel(0) && _ins.use_accel(1)) { float IMU1_weighting; - EKF.getIMU1Weighting(IMU1_weighting); + EKF1.getIMU1Weighting(IMU1_weighting); + _accel_ef_ekf_blended = _accel_ef_ekf[0] * IMU1_weighting + _accel_ef_ekf[1] * (1.0f-IMU1_weighting); + } else { + _accel_ef_ekf_blended = _accel_ef_ekf[_ins.get_primary_accel()]; + } + } + } +} + + +void AP_AHRS_NavEKF::update_EKF2(void) +{ + if (!ekf2_started) { + // wait 1 second for DCM to output a valid tilt error estimate + if (start_time_ms == 0) { + start_time_ms = hal.scheduler->millis(); + } + if (hal.scheduler->millis() - start_time_ms > startup_delay_ms) { + ekf2_started = EKF2.InitialiseFilter(); + } + } + if (ekf2_started) { + EKF2.UpdateFilter(); + EKF2.getRotationBodyToNED(_dcm_matrix); + if (using_EKF() == EKF_TYPE2) { + Vector3f eulers; + EKF2.getEulerAngles(eulers); + roll = eulers.x; + pitch = eulers.y; + yaw = eulers.z; + + update_cd_values(); + update_trig(); + + // keep _gyro_bias for get_gyro_drift() + EKF2.getGyroBias(_gyro_bias); + _gyro_bias = -_gyro_bias; + + // calculate corrected gryo estimate for get_gyro() + _gyro_estimate.zero(); + uint8_t healthy_count = 0; + for (uint8_t i=0; i<_ins.get_gyro_count(); i++) { + if (_ins.get_gyro_health(i) && healthy_count < 2) { + _gyro_estimate += _ins.get_gyro(i); + healthy_count++; + } + } + if (healthy_count > 1) { + _gyro_estimate /= healthy_count; + } + _gyro_estimate += _gyro_bias; + + float abias1, abias2; + EKF2.getAccelZBias(abias1, abias2); + + // update _accel_ef_ekf + for (uint8_t i=0; i<_ins.get_accel_count(); i++) { + Vector3f accel = _ins.get_accel(i); + if (i==0) { + accel.z -= abias1; + } else if (i==1) { + accel.z -= abias2; + } + if (_ins.get_accel_health(i)) { + _accel_ef_ekf[i] = _dcm_matrix * accel; + } + } + + if(_ins.use_accel(0) && _ins.use_accel(1)) { + float IMU1_weighting; + EKF2.getIMU1Weighting(IMU1_weighting); _accel_ef_ekf_blended = _accel_ef_ekf[0] * IMU1_weighting + _accel_ef_ekf[1] * (1.0f-IMU1_weighting); } else { _accel_ef_ekf_blended = _accel_ef_ekf[_ins.get_primary_accel()]; @@ -148,7 +229,7 @@ void AP_AHRS_NavEKF::update(void) // accelerometer values in the earth frame in m/s/s const Vector3f &AP_AHRS_NavEKF::get_accel_ef(uint8_t i) const { - if(!using_EKF()) { + if (using_EKF() == EKF_TYPE_NONE) { return AP_AHRS_DCM::get_accel_ef(i); } return _accel_ef_ekf[i]; @@ -157,7 +238,7 @@ const Vector3f &AP_AHRS_NavEKF::get_accel_ef(uint8_t i) const // blended accelerometer values in the earth frame in m/s/s const Vector3f &AP_AHRS_NavEKF::get_accel_ef_blended(void) const { - if(!using_EKF()) { + if (using_EKF() == EKF_TYPE_NONE) { return AP_AHRS_DCM::get_accel_ef_blended(); } return _accel_ef_ekf_blended; @@ -166,8 +247,11 @@ const Vector3f &AP_AHRS_NavEKF::get_accel_ef_blended(void) const void AP_AHRS_NavEKF::reset(bool recover_eulers) { AP_AHRS_DCM::reset(recover_eulers); - if (ekf_started) { - ekf_started = EKF.InitialiseFilterBootstrap(); + if (ekf1_started) { + ekf1_started = EKF1.InitialiseFilterBootstrap(); + } + if (ekf2_started) { + ekf2_started = EKF2.InitialiseFilter(); } } @@ -175,8 +259,11 @@ void AP_AHRS_NavEKF::reset(bool recover_eulers) void AP_AHRS_NavEKF::reset_attitude(const float &_roll, const float &_pitch, const float &_yaw) { AP_AHRS_DCM::reset_attitude(_roll, _pitch, _yaw); - if (ekf_started) { - ekf_started = EKF.InitialiseFilterBootstrap(); + if (ekf1_started) { + ekf1_started = EKF1.InitialiseFilterBootstrap(); + } + if (ekf2_started) { + ekf2_started = EKF2.InitialiseFilter(); } } @@ -184,11 +271,26 @@ void AP_AHRS_NavEKF::reset_attitude(const float &_roll, const float &_pitch, con bool AP_AHRS_NavEKF::get_position(struct Location &loc) const { Vector3f ned_pos; - if (using_EKF() && EKF.getLLH(loc) && EKF.getPosNED(ned_pos)) { - // fixup altitude using relative position from AHRS home, not - // EKF origin - loc.alt = get_home().alt - ned_pos.z*100; - return true; + switch (using_EKF()) { + case EKF_TYPE1: + if (EKF1.getLLH(loc) && EKF1.getPosNED(ned_pos)) { + // fixup altitude using relative position from AHRS home, not + // EKF origin + loc.alt = get_home().alt - ned_pos.z*100; + return true; + } + break; + case EKF_TYPE2: + if (EKF2.getLLH(loc) && EKF2.getPosNED(ned_pos)) { + // fixup altitude using relative position from AHRS home, not + // EKF origin + loc.alt = get_home().alt - ned_pos.z*100; + return true; + } + break; + + default: + break; } return AP_AHRS_DCM::get_position(loc); } @@ -207,13 +309,20 @@ float AP_AHRS_NavEKF::get_error_yaw(void) const // return a wind estimation vector, in m/s Vector3f AP_AHRS_NavEKF::wind_estimate(void) { - if (!using_EKF()) { - // EKF does not estimate wind speed when there is no airspeed - // sensor active - return AP_AHRS_DCM::wind_estimate(); - } Vector3f wind; - EKF.getWind(wind); + switch (using_EKF()) { + case EKF_TYPE_NONE: + wind = AP_AHRS_DCM::wind_estimate(); + break; + + case EKF_TYPE1: + EKF1.getWind(wind); + break; + + case EKF_TYPE2: + EKF2.getWind(wind); + break; + } return wind; } @@ -227,56 +336,72 @@ bool AP_AHRS_NavEKF::airspeed_estimate(float *airspeed_ret) const // true if compass is being used bool AP_AHRS_NavEKF::use_compass(void) { - if (using_EKF()) { - return EKF.use_compass(); + switch (using_EKF()) { + case EKF_TYPE_NONE: + break; + case EKF_TYPE1: + return EKF1.use_compass(); + case EKF_TYPE2: + return EKF2.use_compass(); } - return AP_AHRS_DCM::use_compass(); + return AP_AHRS_DCM::use_compass(); } // return secondary attitude solution if available, as eulers in radians bool AP_AHRS_NavEKF::get_secondary_attitude(Vector3f &eulers) { - if (using_EKF()) { - // return DCM attitude + switch (using_EKF()) { + case EKF_TYPE_NONE: + // EKF is secondary + EKF1.getEulerAngles(eulers); + return ekf1_started; + + case EKF_TYPE1: + case EKF_TYPE2: + default: + // DCM is secondary eulers = _dcm_attitude; return true; } - if (ekf_started) { - // EKF is secondary - EKF.getEulerAngles(eulers); - return true; - } - // no secondary available - return false; } // return secondary position solution if available bool AP_AHRS_NavEKF::get_secondary_position(struct Location &loc) { - if (using_EKF()) { + switch (using_EKF()) { + case EKF_TYPE_NONE: + // EKF is secondary + EKF1.getLLH(loc); + return ekf1_started; + + case EKF_TYPE1: + case EKF_TYPE2: + default: // return DCM position AP_AHRS_DCM::get_position(loc); return true; } - if (ekf_started) { - // EKF is secondary - EKF.getLLH(loc); - return true; - } - // no secondary available - return false; } // EKF has a better ground speed vector estimate Vector2f AP_AHRS_NavEKF::groundspeed_vector(void) { - if (!using_EKF()) { - return AP_AHRS_DCM::groundspeed_vector(); - } Vector3f vec; - EKF.getVelNED(vec); - return Vector2f(vec.x, vec.y); + + switch (using_EKF()) { + case EKF_TYPE_NONE: + return AP_AHRS_DCM::groundspeed_vector(); + + case EKF_TYPE1: + default: + EKF1.getVelNED(vec); + return Vector2f(vec.x, vec.y); + + case EKF_TYPE2: + EKF2.getVelNED(vec); + return Vector2f(vec.x, vec.y); + } } void AP_AHRS_NavEKF::set_home(const Location &loc) @@ -287,62 +412,137 @@ void AP_AHRS_NavEKF::set_home(const Location &loc) // return true if inertial navigation is active bool AP_AHRS_NavEKF::have_inertial_nav(void) const { - return using_EKF(); + return using_EKF() != EKF_TYPE_NONE; } // return a ground velocity in meters/second, North/East/Down // order. Must only be called if have_inertial_nav() is true bool AP_AHRS_NavEKF::get_velocity_NED(Vector3f &vec) const { - if (using_EKF()) { - EKF.getVelNED(vec); + switch (using_EKF()) { + case EKF_TYPE_NONE: + return false; + + case EKF_TYPE1: + default: + EKF1.getVelNED(vec); + return true; + + case EKF_TYPE2: + EKF2.getVelNED(vec); return true; } - return false; } // return a relative ground position in meters/second, North/East/Down // order. Must only be called if have_inertial_nav() is true bool AP_AHRS_NavEKF::get_relative_position_NED(Vector3f &vec) const { - if (using_EKF()) { - return EKF.getPosNED(vec); + switch (using_EKF()) { + case EKF_TYPE_NONE: + return false; + + case EKF_TYPE1: + default: + return EKF1.getPosNED(vec); + + case EKF_TYPE2: + return EKF2.getPosNED(vec); } - return false; } -bool AP_AHRS_NavEKF::using_EKF(void) const +/* + canonicalise _ekf_type, forcing it to be 0, 1 or 2 + */ +uint8_t AP_AHRS_NavEKF::ekf_type(void) const { - uint8_t ekf_faults; - EKF.getFilterFaults(ekf_faults); - // If EKF is started we switch away if it reports unhealthy. This could be due to bad - // sensor data. If EKF reversion is inhibited, we only switch across if the EKF encounters - // an internal processing error, but not for bad sensor data. - bool ret = ekf_started && ((_ekf_use == EKF_USE_WITH_FALLBACK && EKF.healthy()) || (_ekf_use == EKF_USE_WITHOUT_FALLBACK && ekf_faults == 0)); - if (!ret) { - return false; + uint8_t type = _ekf_type; +#if AHRS_EKF_USE_ALWAYS + // on copters always use an EKF + if (type == 0) { + type = 1; + } +#endif + + // check for invalid type + if (type > 2) { + type = 1; + } + return type; +} + +AP_AHRS_NavEKF::EKF_TYPE AP_AHRS_NavEKF::using_EKF(void) const +{ + EKF_TYPE ret = EKF_TYPE_NONE; + + switch (ekf_type()) { + case 0: + return EKF_TYPE_NONE; + + case 1: { + // do we have an EKF yet? + if (!ekf1_started) { + return EKF_TYPE_NONE; + } +#if AHRS_EKF_USE_ALWAYS + uint8_t ekf_faults; + EKF1.getFilterFaults(ekf_faults); + if (ekf_faults == 0) { + ret = EKF_TYPE1; + } +#else + if (EKF1.healthy()) { + ret = EKF_TYPE1; + } +#endif + break; } - if (_vehicle_class == AHRS_VEHICLE_FIXED_WING || - _vehicle_class == AHRS_VEHICLE_GROUND) { + case 2: { + // do we have an EKF2 yet? + if (!ekf2_started) { + return EKF_TYPE_NONE; + } +#if AHRS_EKF_USE_ALWAYS + uint8_t ekf2_faults; + EKF2.getFilterFaults(ekf2_faults); + if (ekf2_faults == 0) { + ret = EKF_TYPE2; + } +#else + if (EKF2.healthy()) { + ret = EKF_TYPE2; + } +#endif + break; + } + } + + if (ret != EKF_TYPE_NONE && + (_vehicle_class == AHRS_VEHICLE_FIXED_WING || + _vehicle_class == AHRS_VEHICLE_GROUND)) { nav_filter_status filt_state; - EKF.getFilterStatus(filt_state); + if (ret == EKF_TYPE1) { + EKF1.getFilterStatus(filt_state); + } else { + EKF2.getFilterStatus(filt_state); + } if (hal.util->get_soft_armed() && !filt_state.flags.using_gps && _gps.status() >= AP_GPS::GPS_OK_FIX_3D) { // if the EKF is not fusing GPS and we have a 3D lock, then // plane and rover would prefer to use the GPS position from // DCM. This is a safety net while some issues with the EKF // get sorted out - return false; + return EKF_TYPE_NONE; } if (hal.util->get_soft_armed() && filt_state.flags.const_pos_mode) { - return false; + return EKF_TYPE_NONE; } if (!filt_state.flags.attitude || !filt_state.flags.horiz_vel || !filt_state.flags.vert_vel || !filt_state.flags.horiz_pos_abs || !filt_state.flags.vert_pos) { - return false; + return EKF_TYPE_NONE; } } return ret; @@ -356,68 +556,130 @@ bool AP_AHRS_NavEKF::healthy(void) const // If EKF is started we switch away if it reports unhealthy. This could be due to bad // sensor data. If EKF reversion is inhibited, we only switch across if the EKF encounters // an internal processing error, but not for bad sensor data. - if (_ekf_use != EKF_DO_NOT_USE) { - bool ret = ekf_started && EKF.healthy(); + switch (ekf_type()) { + case 0: + return AP_AHRS_DCM::healthy(); + + case 1: { + bool ret = ekf1_started && EKF1.healthy(); if (!ret) { return false; } if ((_vehicle_class == AHRS_VEHICLE_FIXED_WING || _vehicle_class == AHRS_VEHICLE_GROUND) && - !using_EKF()) { + using_EKF() != EKF_TYPE1) { // on fixed wing we want to be using EKF to be considered // healthy if EKF is enabled return false; } return true; } + + case 2: { + bool ret = ekf2_started && EKF2.healthy(); + if (!ret) { + return false; + } + if ((_vehicle_class == AHRS_VEHICLE_FIXED_WING || + _vehicle_class == AHRS_VEHICLE_GROUND) && + using_EKF() != EKF_TYPE2) { + // on fixed wing we want to be using EKF to be considered + // healthy if EKF is enabled + return false; + } + return true; + } + } + return AP_AHRS_DCM::healthy(); } void AP_AHRS_NavEKF::set_ekf_use(bool setting) { -#if !AHRS_EKF_USE_ALWAYS - _ekf_use.set(setting); -#endif + _ekf_type.set(setting?1:0); } // true if the AHRS has completed initialisation bool AP_AHRS_NavEKF::initialised(void) const { - // initialisation complete 10sec after ekf has started - return (ekf_started && (hal.scheduler->millis() - start_time_ms > AP_AHRS_NAVEKF_SETTLE_TIME_MS)); + switch (ekf_type()) { + case 0: + return true; + + case 1: + default: + // initialisation complete 10sec after ekf has started + return (ekf1_started && (hal.scheduler->millis() - start_time_ms > AP_AHRS_NAVEKF_SETTLE_TIME_MS)); + + case 2: + // initialisation complete 10sec after ekf has started + return (ekf2_started && (hal.scheduler->millis() - start_time_ms > AP_AHRS_NAVEKF_SETTLE_TIME_MS)); + } }; // write optical flow data to EKF void AP_AHRS_NavEKF::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas) { - EKF.writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas); + EKF1.writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas); + EKF2.writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas); } // inhibit GPS useage uint8_t AP_AHRS_NavEKF::setInhibitGPS(void) { - return EKF.setInhibitGPS(); + switch (ekf_type()) { + case 0: + case 1: + default: + return EKF1.setInhibitGPS(); + + case 2: + return EKF2.setInhibitGPS(); + } } // get speed limit void AP_AHRS_NavEKF::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) { - EKF.getEkfControlLimits(ekfGndSpdLimit,ekfNavVelGainScaler); + switch (ekf_type()) { + case 0: + case 1: + default: + EKF1.getEkfControlLimits(ekfGndSpdLimit,ekfNavVelGainScaler); + break; + + case 2: + EKF2.getEkfControlLimits(ekfGndSpdLimit,ekfNavVelGainScaler); + break; + } } // get compass offset estimates // true if offsets are valid bool AP_AHRS_NavEKF::getMagOffsets(Vector3f &magOffsets) { - bool status = EKF.getMagOffsets(magOffsets); - return status; + switch (ekf_type()) { + case 0: + case 1: + default: + return EKF1.getMagOffsets(magOffsets); + + case 2: + return EKF2.getMagOffsets(magOffsets); + } } // report any reason for why the backend is refusing to initialise const char *AP_AHRS_NavEKF::prearm_failure_reason(void) const { - if (_ekf_use != EKF_DO_NOT_USE) { - return EKF.prearm_failure_reason(); + switch (ekf_type()) { + case 0: + return nullptr; + case 1: + return EKF1.prearm_failure_reason(); + case 2: + // not implemented yet + return nullptr; } return nullptr; } diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h index 395f242656..3674f6a016 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.h @@ -36,13 +36,10 @@ class AP_AHRS_NavEKF : public AP_AHRS_DCM public: // Constructor AP_AHRS_NavEKF(AP_InertialSensor &ins, AP_Baro &baro, AP_GPS &gps, RangeFinder &rng, - NavEKF &_EKF, NavEKF2 &_EKF2) : + NavEKF &_EKF1, NavEKF2 &_EKF2) : AP_AHRS_DCM(ins, baro, gps), - EKF(_EKF), - EKF2(_EKF2), - ekf_started(false), - startup_delay_ms(1000), - start_time_ms(0) + EKF1(_EKF1), + EKF2(_EKF2) { } @@ -80,8 +77,9 @@ AP_AHRS_NavEKF(AP_InertialSensor &ins, AP_Baro &baro, AP_GPS &gps, RangeFinder & // true if compass is being used bool use_compass(void); - NavEKF &get_NavEKF(void) { return EKF; } - const NavEKF &get_NavEKF_const(void) const { return EKF; } + // we will need to remove these to fully hide which EKF we are using + NavEKF &get_NavEKF(void) { return EKF1; } + const NavEKF &get_NavEKF_const(void) const { return EKF1; } // return secondary attitude solution if available, as eulers in radians bool get_secondary_attitude(Vector3f &eulers); @@ -131,19 +129,26 @@ AP_AHRS_NavEKF(AP_InertialSensor &ins, AP_Baro &baro, AP_GPS &gps, RangeFinder & const char *prearm_failure_reason(void) const override; private: - bool using_EKF(void) const; + enum EKF_TYPE {EKF_TYPE_NONE, EKF_TYPE1, EKF_TYPE2}; + EKF_TYPE using_EKF(void) const; - NavEKF &EKF; + NavEKF &EKF1; NavEKF2 &EKF2; - bool ekf_started; + bool ekf1_started = false; + bool ekf2_started = false; Matrix3f _dcm_matrix; Vector3f _dcm_attitude; Vector3f _gyro_bias; Vector3f _gyro_estimate; Vector3f _accel_ef_ekf[INS_MAX_INSTANCES]; Vector3f _accel_ef_ekf_blended; - const uint16_t startup_delay_ms; - uint32_t start_time_ms; + const uint16_t startup_delay_ms = 1000; + uint32_t start_time_ms = 0; + + uint8_t ekf_type(void) const; + void update_DCM(void); + void update_EKF1(void); + void update_EKF2(void); }; #endif