dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Height source vision: Fallback to rangefinder if available (#994)

This commit is contained in:
Eike 2021-03-31 14:24:38 +02:00 committed by GitHub
parent da06f25216
commit 5d34d7a24e
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 31 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
EKF/control.cpp
View File

@ -787,7 +787,9 @@ void Ekf::controlHeightSensorTimeouts()
const bool continuous_bad_accel_hgt = isTimedOut(_time_good_vert_accel, (uint64_t)_params.bad_acc_reset_delay_us);
// check if height has been inertial deadreckoning for too long
const bool hgt_fusion_timeout = isTimedOut(_time_last_hgt_fuse, (uint64_t)5e6);
// in vision hgt mode check for vision data
const bool hgt_fusion_timeout = isTimedOut(_time_last_hgt_fuse, (uint64_t)5e6) ||
(_control_status.flags.ev_hgt && !isRecent(_time_last_ext_vision, 5 * EV_MAX_INTERVAL));
if (hgt_fusion_timeout || continuous_bad_accel_hgt) {
@ -876,6 +878,13 @@ void Ekf::controlHeightSensorTimeouts()
failing_height_source = "ev";
new_height_source = "ev";
// Fallback to rangefinder data if available
} else if (_range_sensor.isHealthy()) {
setControlRangeHeight();
request_height_reset = true;
failing_height_source = "ev";
new_height_source = "rng";
} else if (!_baro_hgt_faulty) {
startBaroHgtFusion();
@ -1061,11 +1070,13 @@ void Ekf::controlHeightFusion()
case VDIST_SENSOR_EV:
// don't start using EV data unless data is arriving frequently
// don't start using EV data unless data is arriving frequently, do not reset if pref mode was height
if (!_control_status.flags.ev_hgt && isRecent(_time_last_ext_vision, 2 * EV_MAX_INTERVAL)) {
fuse_height = true;
setControlEVHeight();
resetHeight();
if (!_control_status_prev.flags.rng_hgt) {
resetHeight();
}
}
if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) {

14
EKF/ekf_helper.cpp
View File

@ -262,6 +262,20 @@ void Ekf::resetHeight()
// reset the vertical position
if (_control_status.flags.rng_hgt) {
// a fallback from any other height source to rangefinder happened
if(!_control_status_prev.flags.rng_hgt) {
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;
}
}
// update the state and associated variance
resetVerticalPositionTo(_hgt_sensor_offset - _range_sensor.getDistBottom());

5
test/test_EKF_fusionLogic.cpp
View File

@ -379,6 +379,7 @@ TEST_F(EkfFusionLogicTest, doVisionHeightFusion)
_sensor_simulator.runSeconds(12);
// THEN: EKF should stop to intend to use vision height
// TODO: This is not happening
EXPECT_TRUE(_ekf_wrapper.isIntendingVisionHeightFusion()); // TODO: Needs to change
EXPECT_FALSE(_ekf_wrapper.isIntendingVisionHeightFusion());
EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion());
}