diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index bcddce73cb..74c6001b27 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -2022,3 +2022,12 @@ void NavEKF3::writeDefaultAirSpeed(float airspeed, float uncertainty) } } } + +// returns true when the yaw angle has been aligned +bool NavEKF3::yawAlignmentComplete(void) const +{ + if (!core) { + return false; + } + return core[primary].have_aligned_yaw(); +} diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.h b/libraries/AP_NavEKF3/AP_NavEKF3.h index d448fe6bc4..7be9f70572 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3.h @@ -373,6 +373,9 @@ public: // parameter conversion void convert_parameters(); + // returns true when the yaw angle has been aligned + bool yawAlignmentComplete(void) const; + private: uint8_t num_cores; // number of allocated cores uint8_t primary; // current primary core