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

check if terrain valid instead of initialized

Browse Source
This commit is contained in:
ChristophTobler
2017-07-17 16:43:23 +02:00
parent eae0522dc2
commit e429ecab17
1 changed files with 5 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+5 -5
View File
@@ -734,7 +734,7 @@ void Ekf::controlHeightFusion()
// we have just switched to using range finder, calculate height sensor offset such that current // we have just switched to using range finder, calculate height sensor offset such that current
// measurment matches our current height estimate // measurment matches our current height estimate
if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) { if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) {
if (_terrain_initialised) { if (get_terrain_valid()) {
_hgt_sensor_offset = _terrain_vpos; _hgt_sensor_offset = _terrain_vpos;
} else { } else {
_hgt_sensor_offset = _R_rng_to_earth_2_2 * _range_sample_delayed.rng + _state.pos(2); _hgt_sensor_offset = _R_rng_to_earth_2_2 * _range_sample_delayed.rng + _state.pos(2);
@@ -774,7 +774,7 @@ void Ekf::controlHeightFusion()
// measurment matches our current height estimate // measurment matches our current height estimate
if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) { if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) {
// use the parameter rng_gnd_clearance if on ground to avoid a noisy offset initialization (e.g. sonar) // use the parameter rng_gnd_clearance if on ground to avoid a noisy offset initialization (e.g. sonar)
if (_control_status.flags.in_air && _terrain_initialised) { if (_control_status.flags.in_air && get_terrain_valid()) {
_hgt_sensor_offset = _terrain_vpos; _hgt_sensor_offset = _terrain_vpos;
} else if (_control_status.flags.in_air) { } else if (_control_status.flags.in_air) {
@@ -807,7 +807,7 @@ void Ekf::controlHeightFusion()
// we have just switched to using range finder, calculate height sensor offset such that current // we have just switched to using range finder, calculate height sensor offset such that current
// measurment matches our current height estimate // measurment matches our current height estimate
if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) { if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) {
if (_terrain_initialised) { if (get_terrain_valid()) {
_hgt_sensor_offset = _terrain_vpos; _hgt_sensor_offset = _terrain_vpos;
} else { } else {
_hgt_sensor_offset = _R_rng_to_earth_2_2 * _range_sample_delayed.rng + _state.pos(2); _hgt_sensor_offset = _R_rng_to_earth_2_2 * _range_sample_delayed.rng + _state.pos(2);
@@ -875,12 +875,12 @@ bool Ekf::rangeAidConditionsMet(bool in_range_aid_mode)
// check if we should use range finder measurements to estimate height, use hysteresis to avoid rapid switching // check if we should use range finder measurements to estimate height, use hysteresis to avoid rapid switching
bool use_range_finder; bool use_range_finder;
if (in_range_aid_mode) { if (in_range_aid_mode) {
use_range_finder = (_terrain_vpos - _state.pos(2) < _params.max_hagl_for_range_aid) && _terrain_initialised; use_range_finder = (_terrain_vpos - _state.pos(2) < _params.max_hagl_for_range_aid) && get_terrain_valid();
} else { } else {
// if we were not using range aid in the previous iteration then require the current height above terrain to be // if we were not using range aid in the previous iteration then require the current height above terrain to be
// smaller than 70 % of the maximum allowed ground distance for range aid // smaller than 70 % of the maximum allowed ground distance for range aid
use_range_finder = (_terrain_vpos - _state.pos(2) < 0.7f * _params.max_hagl_for_range_aid) && _terrain_initialised; use_range_finder = (_terrain_vpos - _state.pos(2) < 0.7f * _params.max_hagl_for_range_aid) && get_terrain_valid();
} }
bool horz_vel_valid = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow) bool horz_vel_valid = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow)