diff --git a/EKF/control.cpp b/EKF/control.cpp index 299917a710..098cd8f368 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -229,7 +229,7 @@ void Ekf::controlExternalVisionFusion() // determine if we should start using the height observations if (_params.vdist_sensor_type == VDIST_SENSOR_EV) { - // don't start using EV data unless daa is arriving frequently + // don't start using EV data unless data is arriving frequently if (!_control_status.flags.ev_hgt && (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL)) { setControlEVHeight(); resetHeight(); @@ -933,6 +933,21 @@ void Ekf::controlHeightFusion() } } + // Determine if we rely on EV height but switched to baro + if (_params.vdist_sensor_type == VDIST_SENSOR_EV) { + if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) { + // switch to baro if there was a reset to baro + _fuse_height = true; + _in_range_aid_mode = false; + + // we have just switched to using baro height, we don't need to set a height sensor offset + // since we track a separate _baro_hgt_offset + if (_control_status_prev.flags.baro_hgt != _control_status.flags.baro_hgt) { + _hgt_sensor_offset = 0.0f; + } + } + } + // calculate a filtered offset between the baro origin and local NED origin if we are not using the baro as a height reference if (!_control_status.flags.baro_hgt && _baro_data_ready) { float local_time_step = 1e-6f * _delta_time_baro_us; diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index d15ca376b5..e18c904a01 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -240,6 +240,8 @@ void Ekf::resetHeight() int32_t dt_newest = ev_newest.time_us - _imu_sample_delayed.time_us; int32_t dt_delayed = _ev_sample_delayed.time_us - _imu_sample_delayed.time_us; + vert_pos_reset = true; + if (std::abs(dt_newest) < std::abs(dt_delayed)) { _state.pos(2) = ev_newest.posNED(2); @@ -1592,4 +1594,3 @@ void Ekf::get_ekf2ev_quaternion(float *quat) quat[i] = quat_ekf2ev(i); } } -