From 4511b9ff5e0a75c6eb4912b813a62f23ea2cd707 Mon Sep 17 00:00:00 2001 From: kamilritz Date: Tue, 17 Sep 2019 14:29:51 +0200 Subject: [PATCH] Add missing checks for flags.ev_vel --- EKF/control.cpp | 24 +++++++++++++----------- EKF/ekf_helper.cpp | 11 ++++++++--- EKF/estimator_interface.cpp | 2 +- EKF/gps_checks.cpp | 2 +- 4 files changed, 23 insertions(+), 16 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 0fa5fe10b3..3daa0aa49b 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -453,7 +453,7 @@ void Ekf::controlOpticalFlowFusion() // Check if we are in-air and require optical flow to control position drift bool flow_required = _control_status.flags.in_air && (_is_dead_reckoning // is doing inertial dead-reckoning so must constrain drift urgently - || (_control_status.flags.opt_flow && !_control_status.flags.gps && !_control_status.flags.ev_pos) // is completely reliant on optical flow + || (_control_status.flags.opt_flow && !_control_status.flags.gps && !_control_status.flags.ev_pos && !_control_status.flags.ev_vel) // is completely reliant on optical flow || (_control_status.flags.gps && (_gps_error_norm > gps_err_norm_lim))); // is using GPS, but GPS is bad if (!_inhibit_flow_use && _control_status.flags.opt_flow) { @@ -504,7 +504,7 @@ void Ekf::controlOpticalFlowFusion() _time_last_of_fuse = _time_last_imu; // if we are not using GPS or external vision aiding, then the velocity and position states and covariances need to be set - const bool flow_aid_only = !(_control_status.flags.gps || _control_status.flags.ev_pos); + const bool flow_aid_only = !(_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.ev_vel); if (flow_aid_only) { resetVelocity(); resetPosition(); @@ -521,7 +521,8 @@ void Ekf::controlOpticalFlowFusion() // handle the case when we have optical flow, are reliant on it, but have not been using it for an extended period if (_control_status.flags.opt_flow && !_control_status.flags.gps - && !_control_status.flags.ev_pos) { + && !_control_status.flags.ev_pos + && !_control_status.flags.ev_vel) { bool do_reset = ((_time_last_imu - _time_last_of_fuse) > _params.reset_timeout_max); @@ -714,7 +715,7 @@ void Ekf::controlGpsFusion() // calculate observation process noise float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f); - if (_control_status.flags.opt_flow || _control_status.flags.ev_pos) { + if (_control_status.flags.opt_flow || _control_status.flags.ev_pos || _control_status.flags.ev_vel) { // if we are using other sources of aiding, then relax the upper observation // noise limit which prevents bad GPS perturbing the position estimate _posObsNoiseNE = fmaxf(_gps_sample_delayed.hacc, lower_limit); @@ -743,7 +744,7 @@ 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; ECL_WARN("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)) { + } 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; @@ -1186,7 +1187,7 @@ void Ekf::rangeAidConditionsMet() can_use_range_finder = (_terrain_vpos - _state.pos(2) < 0.7f * _params.max_hagl_for_range_aid) && get_terrain_valid(); } - bool horz_vel_valid = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow) + bool horz_vel_valid = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.ev_vel || _control_status.flags.opt_flow) && (_fault_status.value == 0); if (horz_vel_valid) { @@ -1471,7 +1472,7 @@ void Ekf::controlMagFusion() _yaw_angle_observable = _accel_lpf_NE.norm() > 2.0f * _params.mag_acc_gate; } - _yaw_angle_observable = _yaw_angle_observable && (_control_status.flags.gps || _control_status.flags.ev_pos); + _yaw_angle_observable = _yaw_angle_observable && (_control_status.flags.gps || _control_status.flags.ev_pos); // Do we have to add ev_vel here? // check if there is enough yaw rotation to make the mag bias states observable if (!_mag_bias_observable && (fabsf(_yaw_rate_lpf_ef) > _params.mag_yaw_rate_gate)) { @@ -1610,9 +1611,10 @@ void Ekf::controlMagFusion() // is available, assume that we are operating indoors and the magnetometer should not be used. bool user_selected = (_params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR); bool not_using_gps = !(_params.fusion_mode & MASK_USE_GPS) || !_control_status.flags.gps; - bool not_using_evpos = !(_params.fusion_mode & MASK_USE_EVPOS) || !_control_status.flags.ev_pos; - bool not_selected_evyaw = !(_params.fusion_mode & MASK_USE_EVYAW); - if (user_selected && not_using_gps && not_using_evpos && not_selected_evyaw) { + bool not_using_evpos = !(_params.fusion_mode & MASK_USE_EVPOS) || !_control_status.flags.ev_pos; + bool not_using_evvel = !(_params.fusion_mode & MASK_USE_EVVEL) || !_control_status.flags.ev_vel; + bool not_selected_evyaw = !(_params.fusion_mode & MASK_USE_EVYAW); + if (user_selected && not_using_gps && not_using_evpos && not_using_evvel && not_selected_evyaw) { _mag_use_inhibit = true; } else { _mag_use_inhibit = false; @@ -1722,7 +1724,7 @@ void Ekf::controlVelPosFusion() void Ekf::controlAuxVelFusion() { bool data_ready = _auxvel_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_auxvel_sample_delayed); - bool primary_aiding = _control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow; + bool primary_aiding = _control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.ev_vel || _control_status.flags.opt_flow; if (data_ready && primary_aiding) { _fuse_hor_vel = _fuse_vert_vel = _fuse_pos = _fuse_height = false; diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 2431274848..44bda2b391 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -53,7 +53,7 @@ bool Ekf::resetVelocity() Vector3f vel_before_reset = _state.vel; // reset EKF states - if (_control_status.flags.gps) { + if (_control_status.flags.gps && _gps_check_fail_status.value==0) { // this reset is only called if we have new gps data at the fusion time horizon _state.vel = _gps_sample_delayed.vel; @@ -1074,6 +1074,11 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) vel_err_conservative = math::max(vel_err_conservative, sqrtf(sq(_vel_pos_innov[0]) + sq(_vel_pos_innov[1]))); } + if (_control_status.flags.ev_vel) { + // What is the right thing to do here +// vel_err_conservative = math::max(vel_err_conservative, sqrtf(sq(_vel_pos_innov[0]) + sq(_vel_pos_innov[1]))); + } + hvel_err = math::max(hvel_err, vel_err_conservative); } @@ -1105,7 +1110,7 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl bool relying_on_rangefinder = _control_status.flags.rng_hgt && !_params.range_aid; - bool relying_on_optical_flow = _control_status.flags.opt_flow && !(_control_status.flags.gps || _control_status.flags.ev_pos); + bool relying_on_optical_flow = _control_status.flags.opt_flow && !(_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.ev_vel); // Do not require limiting by default *vxy_max = NAN; @@ -1192,7 +1197,7 @@ void Ekf::get_ekf_soln_status(uint16_t *status) ekf_solution_status soln_status; soln_status.flags.attitude = _control_status.flags.tilt_align && _control_status.flags.yaw_align && (_fault_status.value == 0); - soln_status.flags.velocity_horiz = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow || (_control_status.flags.fuse_beta && _control_status.flags.fuse_aspd)) && (_fault_status.value == 0); + soln_status.flags.velocity_horiz = (_control_status.flags.gps || _control_status.flags.ev_pos|| _control_status.flags.ev_vel || _control_status.flags.opt_flow || (_control_status.flags.fuse_beta && _control_status.flags.fuse_aspd)) && (_fault_status.value == 0); soln_status.flags.velocity_vert = (_control_status.flags.baro_hgt || _control_status.flags.ev_hgt || _control_status.flags.gps_hgt || _control_status.flags.rng_hgt) && (_fault_status.value == 0); soln_status.flags.pos_horiz_rel = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow) && (_fault_status.value == 0); soln_status.flags.pos_horiz_abs = (_control_status.flags.gps || _control_status.flags.ev_pos) && (_fault_status.value == 0); diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 2aad6881ba..48325e1bc5 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -384,7 +384,7 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl flow_magnitude_good = (flow_rate_magnitude <= _flow_max_rate); } - bool relying_on_flow = !_control_status.flags.gps && !_control_status.flags.ev_pos; + bool relying_on_flow = !_control_status.flags.gps && !_control_status.flags.ev_pos && !_control_status.flags.ev_vel; // check quality metric bool flow_quality_good = (flow->quality >= _params.flow_qual_min); diff --git a/EKF/gps_checks.cpp b/EKF/gps_checks.cpp index bbd3d09a3f..829f88c7fd 100644 --- a/EKF/gps_checks.cpp +++ b/EKF/gps_checks.cpp @@ -67,7 +67,7 @@ bool Ekf::collect_gps(const gps_message &gps) map_projection_init_timestamped(&_pos_ref, lat, lon, _time_last_imu); // if we are already doing aiding, correct for the change in position since the EKF started navigationg - if (_control_status.flags.opt_flow || _control_status.flags.gps || _control_status.flags.ev_pos) { + if (_control_status.flags.opt_flow || _control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.ev_vel) { double est_lat, est_lon; map_projection_reproject(&_pos_ref, -_state.pos(0), -_state.pos(1), &est_lat, &est_lon); map_projection_init_timestamped(&_pos_ref, est_lat, est_lon, _time_last_imu);