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

ekf: improve range primary height source

Browse Source 
allow range fusion to start with fake measurements if the sensor is
providing bad measurements because too close to the ground
This commit is contained in:
bresch
2021-12-10 15:18:10 +01:00
committed by Mathieu Bresciani
parent 9147d5ea5c
commit 66290de745
2 changed files with 12 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/control.cpp
+11 -12
View File
@@ -825,6 +825,17 @@ void Ekf::controlHeightFusion()
break;
case VDIST_SENSOR_RANGE:
// If we are supposed to be using range finder data as the primary height sensor, have bad range measurements
// and are on the ground, then synthesise a measurement at the expected on ground value
if (!_control_status.flags.in_air
&& !_range_sensor.isDataHealthy()
&& _range_sensor.isRegularlySendingData()
&& _range_sensor.isDataReady()) {
_range_sensor.setRange(_params.rng_gnd_clearance);
_range_sensor.setValidity(true); // bypass the checks
}
if (!_control_status.flags.rng_hgt) {
if (_range_sensor.isDataHealthy()) {
startRngHgtFusion();
@@ -887,18 +898,6 @@ void Ekf::controlHeightFusion()
if (_range_sensor.isDataHealthy()) {
fuseRngHgt();
} else if (!_control_status.flags.in_air
&& _range_sensor.isRegularlySendingData()
&& isTimedOut(_time_last_hgt_fuse, 2 * RNG_MAX_INTERVAL)) {
// If we are supposed to be using range finder data as the primary height sensor, have missed or rejected measurements
// and are on the ground, then synthesise a measurement at the expected on ground value
_range_sensor.setRange(_params.rng_gnd_clearance);
_range_sensor.setDataReadiness(true);
_range_sensor.setValidity(true); // bypass the checks
fuseRngHgt();
}
} else if (_control_status.flags.ev_hgt) {
src/modules/ekf2/EKF/sensor_range_finder.hpp
+1
View File
@@ -58,6 +58,7 @@ public:
void runChecks(uint64_t current_time_us, const matrix::Dcmf &R_to_earth);
bool isHealthy() const override { return _is_sample_valid; }
bool isDataHealthy() const override { return _is_sample_ready && _is_sample_valid; }
bool isDataReady() const { return _is_sample_ready; }
bool isRegularlySendingData() const override { return _is_regularly_sending_data; }
void setSample(const rangeSample &sample)