fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

EV height reset (#379)

Browse Source 
* fix typo

* EKF: use baro if it was reset to baro from ev

* EKF: set vert_pos_reset if resetting to ev hgt

otherwise the position controller will not reset the setpoint -> leading to unwanted altitude changes
This commit is contained in:
ChristophTobler 2018-01-15 10:15:56 +01:00 committed by Paul Riseborough
parent 9e16e51d3a
commit cdbca91e79
2 changed files with 18 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
EKF/control.cpp
View File

@ -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;

3
EKF/ekf_helper.cpp
View File

@ -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);
}
}