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

ekf2: use ground as ekf altitude datum when in range primary hgt mode

Browse Source
This commit is contained in:
bresch
2021-12-10 15:18:10 +01:00
committed by Mathieu Bresciani
parent 5e790da634
commit 9147d5ea5c
2 changed files with 11 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/control.cpp
+4 -17
View File
@@ -712,7 +712,7 @@ void Ekf::controlHeightSensorTimeouts()
} else if (_range_sensor.isHealthy()) { } else if (_range_sensor.isHealthy()) {
// Fallback to rangefinder data if available // Fallback to rangefinder data if available
setControlRangeHeight(); startRngHgtFusion();
request_height_reset = true; request_height_reset = true;
failing_height_source = "ev"; failing_height_source = "ev";
new_height_source = "rng"; new_height_source = "rng";
@@ -825,22 +825,9 @@ void Ekf::controlHeightFusion()
break; break;
case VDIST_SENSOR_RANGE: case VDIST_SENSOR_RANGE:
if (_range_sensor.isDataHealthy()) { if (!_control_status.flags.rng_hgt) {
setControlRangeHeight(); if (_range_sensor.isDataHealthy()) {
startRngHgtFusion();
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 = _range_sensor.getDistBottom() + _state.pos(2);
} else {
_hgt_sensor_offset = _params.rng_gnd_clearance;
}
} }
} }
src/modules/ekf2/EKF/ekf_helper.cpp
+7 -13
View File
@@ -260,24 +260,18 @@ void Ekf::resetHeight()
// reset the vertical position // reset the vertical position
if (_control_status.flags.rng_hgt) { if (_control_status.flags.rng_hgt) {
float dist_bottom;
// a fallback from any other height source to rangefinder happened if (_control_status.flags.in_air) {
if (!_control_status_prev.flags.rng_hgt) { dist_bottom = _range_sensor.getDistBottom();
if (_control_status.flags.in_air && isTerrainEstimateValid()) {
_hgt_sensor_offset = _terrain_vpos;
} else if (_control_status.flags.in_air) {
_hgt_sensor_offset = _range_sensor.getDistBottom() + _state.pos(2);
} else {
_hgt_sensor_offset = _params.rng_gnd_clearance;
}
} else {
// use the parameter rng_gnd_clearance if on ground to avoid a noisy offset initialization (e.g. sonar)
dist_bottom = _params.rng_gnd_clearance;
} }
// update the state and associated variance // update the state and associated variance
resetVerticalPositionTo(_hgt_sensor_offset - _range_sensor.getDistBottom()); resetVerticalPositionTo(-dist_bottom + _hgt_sensor_offset);
// the state variance is the same as the observation // the state variance is the same as the observation
P.uncorrelateCovarianceSetVariance<1>(9, sq(_params.range_noise)); P.uncorrelateCovarianceSetVariance<1>(9, sq(_params.range_noise));