From 5c038a3b43247a94ada3baeeb4e093f6b4ed3826 Mon Sep 17 00:00:00 2001 From: kamilritz Date: Tue, 15 Oct 2019 14:52:13 +0200 Subject: [PATCH] Add stop*Fusion function --- EKF/control.cpp | 18 ++++++------ EKF/ekf.h | 33 ++++++++++++++++++++++ EKF/ekf_helper.cpp | 70 ++++++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 111 insertions(+), 10 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 0b797f9287..c2988b9995 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -392,9 +392,7 @@ void Ekf::controlExternalVisionFusion() && ((_time_last_imu - _time_last_ext_vision) > (uint64_t)_params.reset_timeout_max)) { // Turn off EV fusion mode if no data has been received - _control_status.flags.ev_pos = false; - _control_status.flags.ev_vel = false; - _control_status.flags.ev_yaw = false; + stopEvFusion(); ECL_INFO_TIMESTAMPED("EKF External Vision Data Stopped"); } @@ -464,12 +462,12 @@ void Ekf::controlOpticalFlowFusion() // Handle cases where we are using optical flow but are no longer able to because data is old // or its use has been inhibited. if (_control_status.flags.opt_flow) { - if (_inhibit_flow_use) { - _control_status.flags.opt_flow = false; - _time_last_of_fuse = 0; + if (_inhibit_flow_use) { + stopFlowFusion(); + _time_last_of_fuse = 0; } else if ((_time_last_imu - _time_last_of_fuse) > (uint64_t)_params.reset_timeout_max) { - _control_status.flags.opt_flow = false; + stopFlowFusion(); } } @@ -642,7 +640,7 @@ void Ekf::controlGpsFusion() // Handle the case where we are using GPS and another source of aiding and GPS is failing checks if (_control_status.flags.gps && gps_checks_failing && (_control_status.flags.opt_flow || _control_status.flags.ev_pos || _control_status.flags.ev_vel)) { - _control_status.flags.gps = false; + stopGpsFusion(); // Reset position state to external vision if we are going to use absolute values if (_control_status.flags.ev_pos && !(_params.fusion_mode & MASK_ROTATE_EV)) { resetPosition(); @@ -740,12 +738,12 @@ void Ekf::controlGpsFusion() } } else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)10e6)) { - _control_status.flags.gps = false; + stopGpsFusion(); ECL_WARN_TIMESTAMPED("EKF GPS data stopped"); } else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)1e6) && (_control_status.flags.opt_flow || _control_status.flags.ev_pos || _control_status.flags.ev_vel)) { // Handle the case where we are fusing another position source along GPS, // stop waiting for GPS after 1 s of lost signal - _control_status.flags.gps = false; + stopGpsFusion(); ECL_WARN_TIMESTAMPED("EKF GPS data stopped, using only EV or OF"); } } diff --git a/EKF/ekf.h b/EKF/ekf.h index 5c2ece43f6..bf3b8499d9 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -789,4 +789,37 @@ private: // sensor measurement float calculate_synthetic_mag_z_measurement(Vector3f mag_meas, Vector3f mag_earth_predicted); + // stop GPS fusion + void stopGpsFusion(); + + // stop GPS position fusion + void stopGpsPosFusion(); + + // stop GPS velocity fusion + void stopGpsVelFusion(); + + // stop GPS yaw fusion + void stopGpsYawFusion(); + + // stop external vision fusion + void stopEvFusion(); + + // stop external vision position fusion + void stopEvPosFusion(); + + // stop external vision velocity fusion + void stopEvVelFusion(); + + // stop external vision yaw fusion + void stopEvYawFusion(); + + // stop auxiliary horizontal velocity fusion + void stopAuxVelFusion(); + + // stop optical flow fusion + void stopFlowFusion(); + + // Helper function to set velPos fault status + void setVelPosFaultStatus(const int index, const bool status); + }; diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 624b078645..a54a886335 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -1870,3 +1870,73 @@ float Ekf::kahanSummation(float sum_previous, float input, float &accumulator) c accumulator = (t - sum_previous) - y; return t; } + + +void Ekf::stopGpsFusion() +{ + stopGpsPosFusion(); + stopGpsVelFusion(); + stopGpsYawFusion(); +} + +void Ekf::stopGpsPosFusion() +{ + _control_status.flags.gps = false; + _control_status.flags.gps_hgt = false; + memset(_gps_vel_pos_innov+3,0.0f, sizeof(float)*3); + memset(_gps_vel_pos_innov_var+3,0.0f, sizeof(float)*3); + memset(_gps_vel_pos_test_ratio+HPOS,0.0f, sizeof(float)*2); +} + +void Ekf::stopGpsVelFusion() +{ + memset(_gps_vel_pos_innov,0.0f, sizeof(float)*3); + memset(_gps_vel_pos_innov_var,0.0f, sizeof(float)*3); + memset(_gps_vel_pos_test_ratio,0.0f, sizeof(float)*2); +} + +void Ekf::stopGpsYawFusion() +{ + _control_status.flags.gps_yaw = false; +} + +void Ekf::stopEvFusion() +{ + stopEvPosFusion(); + stopEvVelFusion(); + stopEvYawFusion(); +} + +void Ekf::stopEvPosFusion() +{ + _control_status.flags.ev_pos = false; + memset(_ev_vel_pos_innov+3,0.0f, sizeof(float)*3); + memset(_ev_vel_pos_innov_var+3,0.0f, sizeof(float)*3); + memset(_ev_vel_pos_test_ratio+HPOS,0.0f, sizeof(float)*2); +} + +void Ekf::stopEvVelFusion() +{ + _control_status.flags.ev_vel = false; + memset(_ev_vel_pos_innov,0.0f, sizeof(float)*3); + memset(_ev_vel_pos_innov_var,0.0f, sizeof(float)*3); + memset(_ev_vel_pos_test_ratio,0.0f, sizeof(float)*2);} + +void Ekf::stopEvYawFusion() +{ + _control_status.flags.ev_yaw = false; + +} + +void Ekf::stopAuxVelFusion() +{ + // TODO: Add proper handling of auxiliar velocity fusion +} + +void Ekf::stopFlowFusion() +{ + _control_status.flags.opt_flow = false; + memset(_flow_innov,0.0f,sizeof(_flow_innov)); + memset(_flow_innov_var,0.0f,sizeof(_flow_innov_var)); + memset(&_optflow_test_ratio,0.0f,sizeof(_optflow_test_ratio)); +}