diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index f01c1297b9..9dc3780b49 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -46,8 +46,8 @@ #include // interface and maths for accelerometer calibration #include // ArduPilot Mega Inertial Sensor (accel & gyro) Library #include -#include #include +#include #include // Mission command library #include // PID library #include // PID library (2-axis) @@ -202,9 +202,9 @@ private: AP_RPM rpm_sensor; // Inertial Navigation EKF - NavEKF EKF{&ahrs, barometer, rangefinder}; NavEKF2 EKF2{&ahrs, barometer, rangefinder}; - AP_AHRS_NavEKF ahrs{ins, barometer, gps, rangefinder, EKF, EKF2, AP_AHRS_NavEKF::FLAG_ALWAYS_USE_EKF}; + NavEKF3 EKF3{&ahrs, barometer, rangefinder}; + AP_AHRS_NavEKF ahrs{ins, barometer, gps, rangefinder, EKF2, EKF3, AP_AHRS_NavEKF::FLAG_ALWAYS_USE_EKF}; #if CONFIG_HAL_BOARD == HAL_BOARD_SITL SITL::SITL sitl; diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp index 31345f32d5..14126bbcdf 100644 --- a/ArduCopter/Log.cpp +++ b/ArduCopter/Log.cpp @@ -378,9 +378,9 @@ void Copter::Log_Write_Attitude() DataFlash.Log_Write_Attitude(ahrs, targets); #if OPTFLOW == ENABLED - DataFlash.Log_Write_EKF2(ahrs,optflow.enabled()); + DataFlash.Log_Write_EKF(ahrs,optflow.enabled()); #else - DataFlash.Log_Write_EKF2(ahrs,false); + DataFlash.Log_Write_EKF(ahrs,false); #endif DataFlash.Log_Write_AHRS2(ahrs); #if CONFIG_HAL_BOARD == HAL_BOARD_SITL diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index bb428ec999..567c9398cb 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -852,14 +852,14 @@ const AP_Param::Info Copter::var_info[] = { // @Path: ../libraries/AP_RCMapper/AP_RCMapper.cpp GOBJECT(rcmap, "RCMAP_", RCMapper), - // @Group: EKF_ - // @Path: ../libraries/AP_NavEKF/AP_NavEKF.cpp - GOBJECTN(EKF, NavEKF, "EKF_", NavEKF), - // @Group: EK2_ // @Path: ../libraries/AP_NavEKF2/AP_NavEKF2.cpp GOBJECTN(EKF2, NavEKF2, "EK2_", NavEKF2), + // @Group: EK3_ + // @Path: ../libraries/AP_NavEKF3/AP_NavEKF3.cpp + GOBJECTN(EKF3, NavEKF3, "EK3_", NavEKF3), + // @Group: MIS_ // @Path: ../libraries/AP_Mission/AP_Mission.cpp GOBJECT(mission, "MIS_", AP_Mission), diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index bf190bc663..b42fe30ca5 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -51,9 +51,10 @@ public: k_param_software_type, k_param_ins_old, // *** Deprecated, remove with next eeprom number change k_param_ins, // libraries/AP_InertialSensor variables - k_param_NavEKF2_old, // deprecated + k_param_NavEKF2_old, // deprecated - remove k_param_NavEKF2, k_param_g2, // 2nd block of parameters + k_param_NavEKF3, // simulation k_param_sitl = 10, @@ -110,7 +111,7 @@ public: k_param_angle_rate_max, // remove k_param_rssi_range, // unused, replaced by rssi_ library parameters k_param_rc_feel_rp, - k_param_NavEKF, // Extended Kalman Filter Inertial Navigation Group + k_param_NavEKF, // deprecated - remove k_param_mission, // mission library k_param_rc_13, k_param_rc_14, diff --git a/ArduCopter/make.inc b/ArduCopter/make.inc index 2770398f38..b8c4eeb4ce 100644 --- a/ArduCopter/make.inc +++ b/ArduCopter/make.inc @@ -14,7 +14,6 @@ LIBRARIES += AP_Math LIBRARIES += AP_InertialSensor LIBRARIES += AP_AccelCal LIBRARIES += AP_AHRS -LIBRARIES += AP_NavEKF LIBRARIES += AP_NavEKF2 LIBRARIES += AP_Mission LIBRARIES += AP_Rally