From 6fa13c78061cf19f386df3b1aef1e5d33460c0fd Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 16 Apr 2016 12:41:50 +1000 Subject: [PATCH 1/4] EKF: publish control mode status --- EKF/estimator_interface.h | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 6579c8eeeb..11119e39a2 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -211,6 +211,12 @@ public: virtual void get_accel_bias(float *bias) = 0; + // get EKF mode status + void get_control_mode(uint16_t *val) + { + *val = _control_status.value; + } + protected: parameters _params; // filter parameters From 31bf342fc1d0bab3229012b288d4d2873720c220 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 16 Apr 2016 13:38:40 +1000 Subject: [PATCH 2/4] EKF: publish GPS check status --- EKF/ekf.h | 3 +++ EKF/ekf_helper.cpp | 6 ++++++ EKF/estimator_interface.h | 3 +++ 3 files changed, 12 insertions(+) diff --git a/EKF/ekf.h b/EKF/ekf.h index 5fca7e4c65..cdfd72d6c1 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -123,6 +123,9 @@ public: void get_accel_bias(float *bias) {*bias = _state.accel_z_bias;} + // get GPS check status + void get_gps_check_status(uint16_t *_gps_check_fail_status); + private: static const uint8_t _k_num_states = 24; diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 929b3b2429..bf9c9edfdd 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -316,6 +316,12 @@ void Ekf::get_heading_innov_var(float *heading_innov_var) memcpy(heading_innov_var, &_heading_innov_var, sizeof(float)); } +// get GPS check status +void Ekf::get_gps_check_status(uint16_t *val) +{ + memcpy(val, &_gps_check_fail_status, sizeof(uint16_t)); +} + // get the state vector at the delayed time horizon void Ekf::get_state_delayed(float *state) { diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 11119e39a2..ed8cf5a9a3 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -217,6 +217,9 @@ public: *val = _control_status.value; } + // get GPS check status + virtual void get_gps_check_status(uint16_t *val) = 0; + protected: parameters _params; // filter parameters From 82920da2327533696fe879a6dc8dfc4d3bf32432 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 16 Apr 2016 13:56:50 +1000 Subject: [PATCH 3/4] EKF: strengthen checking of local position status provides immediate status reporting when dropping out of optical flow mode --- EKF/estimator_interface.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 876b3030e9..8a4f8fb6a1 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -364,5 +364,5 @@ void EstimatorInterface::unallocate_buffers() bool EstimatorInterface::local_position_is_valid() { // return true if the position estimate is valid - return ((_time_last_imu - _time_last_optflow) < 5e6) || global_position_is_valid(); + return (((_time_last_imu - _time_last_optflow) < 5e6) && _control_status.flags.opt_flow) || global_position_is_valid(); } From 2fa8a11a293405fcea9ff93ae602d28a27f97b76 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 19 Apr 2016 19:07:21 +1000 Subject: [PATCH 4/4] EKF: replace unnecessary memcopy --- EKF/ekf_helper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index bf9c9edfdd..09377f9537 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -319,7 +319,7 @@ void Ekf::get_heading_innov_var(float *heading_innov_var) // get GPS check status void Ekf::get_gps_check_status(uint16_t *val) { - memcpy(val, &_gps_check_fail_status, sizeof(uint16_t)); + *val = _gps_check_fail_status.value; } // get the state vector at the delayed time horizon