From d446f66105d816a3ccccd82381ceb2c391035b0f Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 7 Jul 2017 17:18:53 +1000 Subject: [PATCH] EKF: Add a fixed wing mode with setter function --- EKF/common.h | 1 + EKF/control.cpp | 12 +++++------- EKF/estimator_interface.h | 3 +++ 3 files changed, 9 insertions(+), 7 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index 84c92be36d..e96c32ff88 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -412,6 +412,7 @@ union filter_control_status_u { uint32_t ev_hgt : 1; // 14 - true when height data from external vision measurements is being fused uint32_t fuse_beta : 1; // 15 - true when synthetic sideslip measurements are being fused uint32_t update_mag_states_only : 1; // 16 - true when only the magnetometer states are updated by the magnetometer + uint32_t fixed_wing : 1; // 17 - true when the vehicle is operating as a fixed wing vehicle } flags; uint32_t value; }; diff --git a/EKF/control.cpp b/EKF/control.cpp index b340439886..4e6addb9d6 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -1050,14 +1050,12 @@ void Ekf::controlMagFusion() if (!_control_status.flags.mag_3D) { if (!_flt_mag_align_complete) { // If we are flying a vehicle that flies forward, eg plane, then we can use the GPS course to check and correct the heading - // if we are doing wind estimation and using the zero sideslip assumotion, then it is reasonable to assume the plane is flying forward - bool zeroSideslipValid = _control_status.flags.fuse_beta; - if (zeroSideslipValid) { - _flt_mag_align_complete = _control_status.flags.yaw_align = realignYawGPS(); - + if (_control_status.flags.fixed_wing) { + _control_status.flags.yaw_align = realignYawGPS(); + _flt_mag_align_complete = _control_status.flags.yaw_align; } else { - _flt_mag_align_complete = _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); - + _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); + _flt_mag_align_complete = _control_status.flags.yaw_align; } } else { // reset the mag field covariances diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 7a21dbbb15..bc9954ddda 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -181,6 +181,9 @@ public: // set vehicle landed status data void set_in_air_status(bool in_air) {_control_status.flags.in_air = in_air;} + // set vehicle is fixed wing status + void set_is_fixed_wing(bool is_fixed_wing) {_control_status.flags.fixed_wing = is_fixed_wing;} + // set flag if synthetic sideslip measurement should be fused void set_fuse_beta_flag(bool fuse_beta) {_control_status.flags.fuse_beta = fuse_beta;}