From 394fa953b8cf7ece8afaf7692ffedcf689898e8b Mon Sep 17 00:00:00 2001 From: bresch Date: Tue, 19 Oct 2021 15:22:30 +0200 Subject: [PATCH] EKF: get status on yaw alignement in flight 1s after the in air mag reset is performed, the final yaw alignment is complete and the yaw estimate can be used for yaw control. --- src/modules/ekf2/EKF/ekf.h | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index aa6c7ce0a6..cc1dfbde20 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -215,6 +215,16 @@ public: bool isTerrainEstimateValid() const { return _hagl_valid; }; + bool isYawFinalAlignComplete() const + { + const bool is_using_mag = (_control_status.flags.mag_3D || _control_status.flags.mag_hdg); + const bool is_mag_alignment_in_flight_complete = is_using_mag + && _control_status.flags.mag_aligned_in_flight + && ((_imu_sample_delayed.time_us - _flt_mag_align_start_time) > (uint64_t)1e6); + return _control_status.flags.yaw_align + && (is_mag_alignment_in_flight_complete || !is_using_mag); + } + uint8_t getTerrainEstimateSensorBitfield() const { return _hagl_sensor_status.value; } // get the estimated terrain vertical position relative to the NED origin