From a1e67396f4f933c96a41909eac23e51741ef0f2d Mon Sep 17 00:00:00 2001 From: Roman Date: Tue, 28 Feb 2017 10:42:52 +0100 Subject: [PATCH] ekf: added methods for setting control height flags Signed-off-by: Roman --- EKF/ekf.h | 12 ++++++++++++ EKF/ekf_helper.cpp | 36 ++++++++++++++++++++++++++++++++++++ 2 files changed, 48 insertions(+) diff --git a/EKF/ekf.h b/EKF/ekf.h index 017ae3a449..37b4c6ed52 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -507,6 +507,18 @@ private: return var * var; } + // set control flags to use baro height + void setControlBaroHeight(); + + // set control flags to use range height + void setControlRangeHeight(); + + // set control flags to use GPS height + void setControlGPSHeight(); + + // set control flags to use external vision height + void setControlEVHeight(); + // zero the specified range of rows in the state covariance matrix void zeroRows(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last); diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 47122d2e81..49989e30b0 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -1162,3 +1162,39 @@ void Ekf::initialiseQuatCovariances(Vector3f &rot_vec_var) } } + +void Ekf::setControlBaroHeight() +{ + _control_status.flags.baro_hgt = true; + + _control_status.flags.gps_hgt = false; + _control_status.flags.rng_hgt = false; + _control_status.flags.ev_hgt = false; +} + +void Ekf::setControlRangeHeight() +{ + _control_status.flags.rng_hgt = true; + + _control_status.flags.baro_hgt = false; + _control_status.flags.gps_hgt = false; + _control_status.flags.ev_hgt = false; +} + +void Ekf::setControlGPSHeight() +{ + _control_status.flags.gps_hgt = true; + + _control_status.flags.baro_hgt = false; + _control_status.flags.rng_hgt = false; + _control_status.flags.ev_hgt = false; +} + +void Ekf::setControlEVHeight() +{ + _control_status.flags.ev_hgt = true; + + _control_status.flags.baro_hgt = false; + _control_status.flags.gps_hgt = false; + _control_status.flags.rng_hgt = false; +}