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

SensorRangeFinder: add distBottom function to get the vertical distance

Browse Source
This commit is contained in:
bresch
2020-04-03 11:01:23 +02:00
committed by Mathieu Bresciani
parent a998da1781
commit 572ad2df0a
5 changed files with 26 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+4 -3
View File
@@ -1001,7 +1001,7 @@ void Ekf::controlHeightFusion()
_hgt_sensor_offset = _terrain_vpos;
} else if (_control_status.flags.in_air) {
_hgt_sensor_offset = _range_sensor.getCosTilt() * _range_sensor.getRange() + _state.pos(2);
_hgt_sensor_offset = _range_sensor.getDistBottom() + _state.pos(2);
} else {
_hgt_sensor_offset = _params.rng_gnd_clearance;
@@ -1161,10 +1161,11 @@ void Ekf::controlHeightFusion()
Vector2f rng_hgt_innov_gate;
Vector3f rng_hgt_obs_var;
// use range finder with tilt correction
_rng_hgt_innov(2) = _state.pos(2) - (-math::max(_range_sensor.getRange() * _range_sensor.getCosTilt(),
_rng_hgt_innov(2) = _state.pos(2) - (-math::max(_range_sensor.getDistBottom(),
_params.rng_gnd_clearance)) - _hgt_sensor_offset;
// observation variance - user parameter defined
rng_hgt_obs_var(2) = fmaxf((sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sensor.getRange())) * sq(_range_sensor.getCosTilt()), 0.01f);
rng_hgt_obs_var(2) = fmaxf(sq(_params.range_noise)
+ sq(_params.range_noise_scaler * _range_sensor.getDistBottom()), 0.01f);
// innovation gate size
rng_hgt_innov_gate(1) = fmaxf(_params.range_innov_gate, 1.0f);
// fuse height information