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

AP_AHRS: added selection of EKF type using AHRS_EKF_TYPE

This commit is contained in:
Andrew Tridgell 2015-09-23 11:40:25 +10:00
parent ed25c85d21
commit 7ba45444a2
4 changed files with 379 additions and 118 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_AHRS/AP_AHRS.cpp
View File

@ -18,10 +18,6 @@
#include <AP_HAL/AP_HAL.h>
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

8
libraries/AP_AHRS/AP_AHRS.h
View File

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

442
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

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

31
libraries/AP_AHRS/AP_AHRS_NavEKF.h
View File

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