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

AP_AHRS: Add EKF3 and remove EKF1

Browse Source
This commit is contained in:
priseborough 2016-06-28 17:36:12 +10:00 committed by Andrew Tridgell
parent 38ad778ff1
commit 33a7c682a3
3 changed files with 183 additions and 460 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_AHRS/AP_AHRS.cpp
View File

@ -120,8 +120,8 @@ const AP_Param::GroupInfo AP_AHRS::var_info[] = {
#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. 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
// @Description: This controls which NavEKF Kalman filter version is used for attitude and position estimation
// @Values: 0:Disabled,2:Enable EKF2,3:Enable EKF3
// @User: Advanced
AP_GROUPINFO("EKF_TYPE", 14, AP_AHRS, _ekf_type, 2),
#endif

607
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

File diff suppressed because it is too large Load Diff

32
libraries/AP_AHRS/AP_AHRS_NavEKF.h
View File

@ -29,7 +29,6 @@
#endif
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150
#include <AP_NavEKF/AP_NavEKF.h>
#include <AP_NavEKF2/AP_NavEKF2.h>
#include <AP_NavEKF3/AP_NavEKF3.h>
#include <AP_NavEKF/AP_Nav_Common.h> // definitions shared by inertial and ekf nav filters
@ -37,19 +36,6 @@
#define AP_AHRS_NAVEKF_AVAILABLE 1
#define AP_AHRS_NAVEKF_SETTLE_TIME_MS 20000 // time in milliseconds the ekf needs to settle after being started
/*
we are too close to running out of flash on px4, so disable
it. Leave it enabled on V4 for now as that has sufficient flash
space
*/
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 && (defined(CONFIG_ARCH_BOARD_PX4FMU_V1) || defined(CONFIG_ARCH_BOARD_PX4FMU_V2))
#define AP_AHRS_WITH_EKF1 0
#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN && !defined(CONFIG_ARCH_BOARD_VRBRAIN_V54)
#define AP_AHRS_WITH_EKF1 0
#else
#define AP_AHRS_WITH_EKF1 1
#endif
class AP_AHRS_NavEKF : public AP_AHRS_DCM
{
public:
@ -60,7 +46,7 @@ public:
// Constructor
AP_AHRS_NavEKF(AP_InertialSensor &ins, AP_Baro &baro, AP_GPS &gps, RangeFinder &rng,
NavEKF &_EKF1, NavEKF2 &_EKF2, NavEKF3 &_EKF3, Flags flags = FLAG_NONE);
NavEKF2 &_EKF2, NavEKF3 &_EKF3, Flags flags = FLAG_NONE);
// return the smoothed gyro vector corrected for drift
const Vector3f &get_gyro(void) const override;
@ -100,13 +86,6 @@ public:
bool use_compass(void);
// 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;
}
NavEKF2 &get_NavEKF2(void) {
return EKF2;
}
@ -255,11 +234,8 @@ public:
private:
enum EKF_TYPE {EKF_TYPE_NONE=0,
#if AP_AHRS_WITH_EKF1
EKF_TYPE1=1,
#endif
EKF_TYPE2=2,
EKF_TYPE3=3
EKF_TYPE3=3,
EKF_TYPE2=2
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
,EKF_TYPE_SITL=10
#endif
@ -270,7 +246,6 @@ private:
return _ekf_flags & FLAG_ALWAYS_USE_EKF;
}
NavEKF &EKF1;
NavEKF2 &EKF2;
NavEKF3 &EKF3;
bool ekf1_started:1;
@ -289,7 +264,6 @@ private:
uint8_t ekf_type(void) const;
void update_DCM(void);
void update_EKF1(void);
void update_EKF2(void);
void update_EKF3(void);