diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index f9f11f81bf..f0d4966451 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -505,7 +505,7 @@ void NavEKF::ResetHeight(void) // this function is used to initialise the filter whilst moving, using the AHRS DCM solution // it should NOT be used to re-initialise after a timeout as DCM will also be corrupted -void NavEKF::InitialiseFilterDynamic(void) +bool NavEKF::InitialiseFilterDynamic(void) { // this forces healthy() to be false so that when we ask for ahrs // attitude we get the DCM attitude regardless of the state of AHRS_EKF_USE @@ -513,7 +513,7 @@ void NavEKF::InitialiseFilterDynamic(void) // If we are a plane and don't have GPS lock then don't initialise if (assume_zero_sideslip() && _ahrs->get_gps().status() < AP_GPS::GPS_OK_FIX_3D) { - return; + return false; } // Set re-used variables to zero @@ -571,16 +571,18 @@ void NavEKF::InitialiseFilterDynamic(void) // initialise IMU pre-processing states readIMUData(); + + return true; } // Initialise the states from accelerometer and magnetometer data (if present) // This method can only be used when the vehicle is static -void NavEKF::InitialiseFilterBootstrap(void) +bool NavEKF::InitialiseFilterBootstrap(void) { // If we are a plane and don't have GPS lock then don't initialise if (assume_zero_sideslip() && _ahrs->get_gps().status() < AP_GPS::GPS_OK_FIX_3D) { statesInitialised = false; - return; + return false; } // set re-used variables to zero @@ -664,6 +666,8 @@ void NavEKF::InitialiseFilterBootstrap(void) // initialise IMU pre-processing states readIMUData(); + + return true; } // Update Filter States - this should be called whenever new IMU data is available diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h index 2366e6befd..22eca6e982 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.h +++ b/libraries/AP_NavEKF/AP_NavEKF.h @@ -90,11 +90,11 @@ public: // This function is used to initialise the filter whilst moving, using the AHRS DCM solution // It should NOT be used to re-initialise after a timeout as DCM will also be corrupted - void InitialiseFilterDynamic(void); + bool InitialiseFilterDynamic(void); // Initialise the states from accelerometer and magnetometer data (if present) // This method can only be used when the vehicle is static - void InitialiseFilterBootstrap(void); + bool InitialiseFilterBootstrap(void); // Update Filter States - this should be called whenever new IMU data is available void UpdateFilter(void);