fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 06:13:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF control height fusion add switch statement for all dist sensor types

Browse Source
This commit is contained in:
Daniel Agar
2020-03-03 11:29:07 +01:00
committed by Mathieu Bresciani
parent 7c179b39b8
commit 4fab6057a7
1 changed files with 32 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+32 -27
View File
@@ -932,8 +932,12 @@ void Ekf::controlHeightFusion()
checkRangeAidSuitability();
_range_aid_mode_selected = (_params.range_aid == 1) && isRangeAidSuitable();
if (_params.vdist_sensor_type == VDIST_SENSOR_BARO) {
switch (_params.vdist_sensor_type) {
default:
ECL_ERR("Invalid height mode: %d", _params.vdist_sensor_type);
// FALLTHROUGH
case VDIST_SENSOR_BARO:
if (_range_aid_mode_selected && _range_data_ready && _rng_hgt_valid) {
setControlRangeHeight();
_fuse_height = true;
@@ -977,45 +981,44 @@ void Ekf::controlHeightFusion()
_hgt_sensor_offset = _gps_sample_delayed.hgt - _gps_alt_ref + _state.pos(2);
}
}
}
// set the height data source to range if requested
if ((_params.vdist_sensor_type == VDIST_SENSOR_RANGE) && _rng_hgt_valid) {
setControlRangeHeight();
_fuse_height = _range_data_ready;
break;
// we have just switched to using range finder, calculate height sensor offset such that current
// measurement matches our current height estimate
if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) {
case VDIST_SENSOR_RANGE:
if (_range_data_ready && _rng_hgt_valid) {
setControlRangeHeight();
_fuse_height = _range_data_ready;
} else if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) {
// we have just switched to using range finder, calculate height sensor offset such that current
// measurement matches our current height estimate
// use the parameter rng_gnd_clearance if on ground to avoid a noisy offset initialization (e.g. sonar)
if (_control_status.flags.in_air && isTerrainEstimateValid()) {
_hgt_sensor_offset = _terrain_vpos;
} else if (_control_status.flags.in_air) {
_hgt_sensor_offset = _R_rng_to_earth_2_2 * _range_sample_delayed.rng + _state.pos(2);
} else {
_hgt_sensor_offset = _params.rng_gnd_clearance;
}
} else if (_baro_data_ready && !_baro_hgt_faulty) {
setControlBaroHeight();
_fuse_height = true;
// 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;
}
}
} else if ((_params.vdist_sensor_type == VDIST_SENSOR_RANGE) && _baro_data_ready && !_baro_hgt_faulty) {
setControlBaroHeight();
_fuse_height = true;
break;
// 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;
}
}
// Determine if GPS should be used as the height source
if (_params.vdist_sensor_type == VDIST_SENSOR_GPS) {
case VDIST_SENSOR_GPS:
// Determine if GPS should be used as the height source
if (_range_aid_mode_selected && _range_data_ready && _rng_hgt_valid) {
setControlRangeHeight();
_fuse_height = true;
@@ -1051,10 +1054,10 @@ void Ekf::controlHeightFusion()
_hgt_sensor_offset = 0.0f;
}
}
}
// Determine if we rely on EV height but switched to baro
if (_params.vdist_sensor_type == VDIST_SENSOR_EV) {
break;
case VDIST_SENSOR_EV:
// don't start using EV data unless data is arriving frequently
if (!_control_status.flags.ev_hgt && isRecent(_time_last_ext_vision, 2 * EV_MAX_INTERVAL)) {
@@ -1078,6 +1081,8 @@ void Ekf::controlHeightFusion()
if (_control_status.flags.ev_hgt) {
_fuse_height = true;
}
break;
}
// calculate a filtered offset between the baro origin and local NED origin if we are not using the baro as a height reference