diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index df79800342..5d432f9f5e 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -611,7 +611,6 @@ NavEKF2::NavEKF2() AP_Param::setup_object_defaults(this, var_info); } -#include // Initialise the filter bool NavEKF2::InitialiseFilter(void) @@ -1571,8 +1570,6 @@ uint32_t NavEKF2::getLastPosDownReset(float &posDelta) // update the yaw reset data to capture changes due to a lane switch void NavEKF2::updateLaneSwitchYawResetData(uint8_t new_primary, uint8_t old_primary) { - // AP_DAL::log_updateLaneSwitchYawResetData(new_primary, old_primary); - Vector3f eulers_old_primary, eulers_new_primary; float old_yaw_delta; @@ -1599,8 +1596,6 @@ void NavEKF2::updateLaneSwitchYawResetData(uint8_t new_primary, uint8_t old_prim // update the position reset data to capture changes due to a lane switch void NavEKF2::updateLaneSwitchPosResetData(uint8_t new_primary, uint8_t old_primary) { - // AP_DAL::log_updateLaneSwitchPosResetData(new_primary, old_primary); - Vector2f pos_old_primary, pos_new_primary, old_pos_delta; // If core position reset data has been consumed reset delta to zero @@ -1628,8 +1623,6 @@ void NavEKF2::updateLaneSwitchPosResetData(uint8_t new_primary, uint8_t old_prim // new primary EKF update has been run void NavEKF2::updateLaneSwitchPosDownResetData(uint8_t new_primary, uint8_t old_primary) { - // AP_DAL::log_updateLaneSwitchPosResetData(new_primary, old_primary); - float posDownOldPrimary, posDownNewPrimary, oldPosDownDelta; // If core position reset data has been consumed reset delta to zero diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.h b/libraries/AP_NavEKF2/AP_NavEKF2.h index 8333e875af..86dab18caf 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2.h @@ -314,9 +314,6 @@ public: // allow the enable flag to be set by Replay void set_enable(bool enable) { _enable.set_enable(enable); } - // are we doing sensor logging inside the EKF? - bool have_ekf_logging(void) const { return false; } - // get timing statistics structure void getTimingStatistics(int8_t instance, struct ekf_timing &timing) const; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp index d03117cf3e..8fdb0a99c5 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp @@ -4,8 +4,6 @@ #include "AP_NavEKF2_core.h" #include -#include - extern const AP_HAL::HAL& hal; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp index edeca280cf..f16311fe9b 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp @@ -4,8 +4,6 @@ #include "AP_NavEKF2_core.h" #include -#include - extern const AP_HAL::HAL& hal; /******************************************************** diff --git a/libraries/AP_NavEKF2/AP_NavEKF_GyroBias.cpp b/libraries/AP_NavEKF2/AP_NavEKF_GyroBias.cpp index b1272e33c9..f2b73f110a 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF_GyroBias.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF_GyroBias.cpp @@ -5,8 +5,6 @@ #include "AP_NavEKF2_core.h" #include -#include - extern const AP_HAL::HAL& hal; // reset the body axis gyro bias states to zero and re-initialise the corresponding covariances