From a4fe2b1e7206c97f1498de2ac875eb671cc45a38 Mon Sep 17 00:00:00 2001 From: bresch Date: Tue, 11 May 2021 12:21:05 +0200 Subject: [PATCH] yaw_align: let the mag control logic do the alignement and resets --- EKF/control.cpp | 9 +-------- EKF/ekf.cpp | 4 +++- 2 files changed, 4 insertions(+), 9 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 097004215f..528391b8e0 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -53,11 +53,10 @@ void Ekf::controlFusionModes() // whilst we are aligning the tilt, monitor the variances const Vector3f angle_err_var_vec = calcRotVecVariances(); - // Once the tilt variances have reduced to equivalent of 3deg uncertainty, re-set the yaw and magnetic field states + // Once the tilt variances have reduced to equivalent of 3deg uncertainty // and declare the tilt alignment complete if ((angle_err_var_vec(0) + angle_err_var_vec(1)) < sq(math::radians(3.0f))) { _control_status.flags.tilt_align = true; - _control_status.flags.yaw_align = resetMagHeading(_mag_lpf.getState()); // TODO: is this needed? // send alignment status message to the console const char* height_source = nullptr; @@ -435,12 +434,6 @@ void Ekf::controlOpticalFlowFusion() && !_control_status.flags.opt_flow // we are not yet using flow data && !_inhibit_flow_use) { - // If the heading is not aligned, reset the yaw and magnetic field states - // TODO: ekf2 should always try to align itself if not already aligned - if (!_control_status.flags.yaw_align) { - _control_status.flags.yaw_align = resetMagHeading(_mag_lpf.getState()); - } - // If the heading is valid and use is not inhibited , start using optical flow aiding if (_control_status.flags.yaw_align) { // set the flag and reset the fusion timeout diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 6ca20cb017..72b705da7e 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -203,7 +203,9 @@ bool Ekf::initialiseFilter() } // calculate the initial magnetic field and yaw alignment - _control_status.flags.yaw_align = resetMagHeading(_mag_lpf.getState(), false, false); + // but do not mark the yaw alignement complete as it needs to be + // reset once the leveling phase is done + resetMagHeading(_mag_lpf.getState(), false, false); // initialise the state covariance matrix now we have starting values for all the states initialiseCovariance();