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

AP_AHRS: add a runtime flag for always using EKF

Browse Source 
The AHRS_EKF_USE_ALWAYS define is used to force EKF to be always
used. It is defined only for building ArduCopter. Change it to be a
runtime flag. Keep its default value still based on the original define,
once the Copter uses it the define will be removed.

The motivation is to move vehicle specifc code out of the general
libraries. This patch shouldn't change behavior.
This commit is contained in:
Caio Marcelo de Oliveira Filho 2015-10-13 15:08:49 -03:00 committed by Andrew Tridgell
parent e87db31149
commit 9d2e3157fe
2 changed files with 34 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -488,12 +488,9 @@ bool AP_AHRS_NavEKF::get_relative_position_NED(Vector3f &vec) const
uint8_t AP_AHRS_NavEKF::ekf_type(void) const
{
uint8_t type = _ekf_type;
#if AHRS_EKF_USE_ALWAYS
// on copters always use an EKF
if (type == 0) {
if (always_use_EKF() && type == 0) {
type = 1;
}
#endif
// check for invalid type
if (type > 2) {
@ -515,17 +512,15 @@ AP_AHRS_NavEKF::EKF_TYPE AP_AHRS_NavEKF::active_EKF_type(void) const
if (!ekf1_started) {
return EKF_TYPE_NONE;
}
#if AHRS_EKF_USE_ALWAYS
uint8_t ekf_faults;
EKF1.getFilterFaults(ekf_faults);
if (ekf_faults == 0) {
if (always_use_EKF()) {
uint8_t ekf_faults;
EKF1.getFilterFaults(ekf_faults);
if (ekf_faults == 0) {
ret = EKF_TYPE1;
}
} else if (EKF1.healthy()) {
ret = EKF_TYPE1;
}
#else
if (EKF1.healthy()) {
ret = EKF_TYPE1;
}
#endif
break;
}
@ -534,17 +529,15 @@ AP_AHRS_NavEKF::EKF_TYPE AP_AHRS_NavEKF::active_EKF_type(void) const
if (!ekf2_started) {
return EKF_TYPE_NONE;
}
#if AHRS_EKF_USE_ALWAYS
uint8_t ekf2_faults;
EKF2.getFilterFaults(ekf2_faults);
if (ekf2_faults == 0) {
if (always_use_EKF()) {
uint8_t ekf2_faults;
EKF2.getFilterFaults(ekf2_faults);
if (ekf2_faults == 0) {
ret = EKF_TYPE2;
}
} else if (EKF2.healthy()) {
ret = EKF_TYPE2;
}
#else
if (EKF2.healthy()) {
ret = EKF_TYPE2;
}
#endif
break;
}
}

21
libraries/AP_AHRS/AP_AHRS_NavEKF.h
View File

@ -36,12 +36,24 @@
class AP_AHRS_NavEKF : public AP_AHRS_DCM
{
public:
enum Flags {
FLAG_NONE = 0,
FLAG_ALWAYS_USE_EKF = 0x1,
};
// Constructor
AP_AHRS_NavEKF(AP_InertialSensor &ins, AP_Baro &baro, AP_GPS &gps, RangeFinder &rng,
NavEKF &_EKF1, NavEKF2 &_EKF2) :
NavEKF &_EKF1, NavEKF2 &_EKF2,
#if AHRS_EKF_USE_ALWAYS
Flags flags = FLAG_ALWAYS_USE_EKF
#else
Flags flags = FLAG_NONE
#endif
) :
AP_AHRS_DCM(ins, baro, gps),
EKF1(_EKF1),
EKF2(_EKF2)
EKF2(_EKF2),
_flags(flags)
{
}
@ -189,6 +201,10 @@ private:
enum EKF_TYPE {EKF_TYPE_NONE, EKF_TYPE1, EKF_TYPE2};
EKF_TYPE active_EKF_type(void) const;
bool always_use_EKF() const {
return _flags & FLAG_ALWAYS_USE_EKF;
}
NavEKF &EKF1;
NavEKF2 &EKF2;
bool ekf1_started = false;
@ -201,6 +217,7 @@ private:
Vector3f _accel_ef_ekf_blended;
const uint16_t startup_delay_ms = 1000;
uint32_t start_time_ms = 0;
Flags _flags;
uint8_t ekf_type(void) const;
void update_DCM(void);