Skip to content

Commit

Permalink
EKF: Decouple range finder use criteria checking and selection
Browse files Browse the repository at this point in the history
  • Loading branch information
@priseborough
priseborough committed May 7, 2018
1 parent 6708bec commit 0c0a660
Show file tree
Hide file tree
Showing 2 changed files with 24 additions and 22 deletions.
40 changes: 20 additions & 20 deletions EKF/control.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -354,7 +354,7 @@ void Ekf::controlOpticalFlowFusion()

} else {
bool good_gps_aiding = _control_status.flags.gps && ((_time_last_imu - _last_gps_fail_us) > (uint64_t)6e6);
if (good_gps_aiding && !_in_range_aid_mode) {
if (good_gps_aiding && !_range_aid_mode_enabled) {
// Detect the special case where we are in flight, are using good quality GPS and speed and range has exceeded
// limits for use of range finder for height
_time_bad_motion_us = _imu_sample_delayed.time_us;
Expand Down Expand Up @@ -861,11 +861,13 @@ void Ekf::controlHeightFusion()
_range_sample_delayed.rng += pos_offset_earth(2) / _R_rng_to_earth_2_2;
}

rangeAidConditionsMet();

_range_aid_mode_selected = (_params.range_aid == 1) && _range_aid_mode_enabled;

if (_params.vdist_sensor_type == VDIST_SENSOR_BARO) {
_in_range_aid_mode = rangeAidConditionsMet(_in_range_aid_mode);

if (_in_range_aid_mode && _range_data_ready && !_rng_hgt_faulty) {
if (_range_aid_mode_selected && _range_data_ready && !_rng_hgt_faulty) {
setControlRangeHeight();
_fuse_height = true;

Expand All @@ -883,7 +885,7 @@ void Ekf::controlHeightFusion()
} else if (!_in_range_aid_mode && _baro_data_ready && !_baro_hgt_faulty) {
setControlBaroHeight();
_fuse_height = true;
_in_range_aid_mode = false;
_range_aid_mode_enabled = false;

// we have just switched to using baro height, we don't need to set a height sensor offset
// since we track a separate _baro_hgt_offset
Expand All @@ -905,7 +907,7 @@ void Ekf::controlHeightFusion()
} else if (_control_status.flags.gps_hgt && _gps_data_ready && !_gps_hgt_faulty) {
// switch to gps if there was a reset to gps
_fuse_height = true;
_in_range_aid_mode = false;
_range_aid_mode_enabled = false;

// we have just switched to using gps height, calculate height sensor offset such that current
// measurment matches our current height estimate
Expand Down Expand Up @@ -951,9 +953,8 @@ void Ekf::controlHeightFusion()

// Determine if GPS should be used as the height source
if (_params.vdist_sensor_type == VDIST_SENSOR_GPS) {
_in_range_aid_mode = rangeAidConditionsMet(_in_range_aid_mode);

if (_in_range_aid_mode && _range_data_ready && !_rng_hgt_faulty) {
if (_range_aid_mode_selected && _range_data_ready && !_rng_hgt_faulty) {
setControlRangeHeight();
_fuse_height = true;

Expand All @@ -971,7 +972,7 @@ void Ekf::controlHeightFusion()
} else if (!_in_range_aid_mode && _gps_data_ready && !_gps_hgt_faulty) {
setControlGPSHeight();
_fuse_height = true;
_in_range_aid_mode = false;
_range_aid_mode_enabled = false;

// we have just switched to using gps height, calculate height sensor offset such that current
// measurment matches our current height estimate
Expand All @@ -982,7 +983,7 @@ void Ekf::controlHeightFusion()
} else if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) {
// switch to baro if there was a reset to baro
_fuse_height = true;
_in_range_aid_mode = false;
_range_aid_mode_enabled = false;

// we have just switched to using baro height, we don't need to set a height sensor offset
// since we track a separate _baro_hgt_offset
Expand All @@ -997,7 +998,7 @@ void Ekf::controlHeightFusion()
if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) {
// switch to baro if there was a reset to baro
_fuse_height = true;
_in_range_aid_mode = false;
_range_aid_mode_enabled = false;

// we have just switched to using baro height, we don't need to set a height sensor offset
// since we track a separate _baro_hgt_offset
Expand Down Expand Up @@ -1035,19 +1036,18 @@ void Ekf::controlHeightFusion()

}

bool Ekf::rangeAidConditionsMet(bool in_range_aid_mode)
void Ekf::rangeAidConditionsMet()
{
// if the parameter for range aid is enabled we allow to switch from using the primary height source to using range finder as height source
// under the following conditions
// 1) we are not further than max_range_for_dual_fusion away from the ground
// 2) our ground speed is not higher than max_vel_for_dual_fusion
// 1) we are not further than max_hagl_for_range_aid away from the ground
// 2) our ground speed is not higher than max_vel_for_range_aid
// 3) Our terrain estimate is stable (needs better checks)
// 4) We are in-air
if (_params.range_aid && _control_status.flags.in_air) {
if (_control_status.flags.in_air) {
// check if we should use range finder measurements to estimate height, use hysteresis to avoid rapid switching
bool use_range_finder;

if (in_range_aid_mode) {
if (_range_aid_mode_enabled) {
use_range_finder = (_terrain_vpos - _state.pos(2) < _params.max_hagl_for_range_aid) && get_terrain_valid();

} else {
Expand All @@ -1062,7 +1062,7 @@ bool Ekf::rangeAidConditionsMet(bool in_range_aid_mode)
if (horz_vel_valid) {
float ground_vel = sqrtf(_state.vel(0) * _state.vel(0) + _state.vel(1) * _state.vel(1));

if (in_range_aid_mode) {
if (_range_aid_mode_enabled) {
use_range_finder &= ground_vel < _params.max_vel_for_range_aid;

} else {
Expand All @@ -1076,7 +1076,7 @@ bool Ekf::rangeAidConditionsMet(bool in_range_aid_mode)
}

// use hysteresis to check for hagl
if (in_range_aid_mode) {
if (_range_aid_mode_enabled) {
use_range_finder &= ((_hagl_innov * _hagl_innov / (sq(_params.range_aid_innov_gate) * _hagl_innov_var)) < 1.0f);

} else {
Expand All @@ -1085,10 +1085,10 @@ bool Ekf::rangeAidConditionsMet(bool in_range_aid_mode)
use_range_finder &= ((_hagl_innov * _hagl_innov / (sq(_params.range_aid_innov_gate) * _hagl_innov_var)) < 0.01f);
}

return use_range_finder;
_range_aid_mode_enabled = use_range_finder;

} else {
return false;
_range_aid_mode_enabled = false;
}
}

Expand Down
6 changes: 4 additions & 2 deletions EKF/ekf.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -432,7 +432,8 @@ class Ekf : public EstimatorInterface
bool _bad_vert_accel_detected{false}; ///< true when bad vertical accelerometer data has been detected

// variables used to control range aid functionality
bool _in_range_aid_mode{false}; ///< true when range finder is to be used as the height reference instead of the primary height sensor
bool _range_aid_mode_enabled{false}; ///< true when range finder can be used as the height reference instead of the primary height sensor
bool _range_aid_mode_selected{false}; ///< true when range finder is being used as the height reference instead of the primary height sensor

// variables used to check for "stuck" rng data
float _rng_check_min_val{0.0f}; ///< minimum value for new rng measurement when being stuck
Expand Down Expand Up @@ -584,7 +585,8 @@ class Ekf : public EstimatorInterface
// control for combined height fusion mode (implemented for switching between baro and range height)
void controlHeightFusion();

bool rangeAidConditionsMet(bool in_range_aid_mode);
// determine if flight condition is suitable so use range finder instead of the primary height senor
void rangeAidConditionsMet();

// check for "stuck" range finder measurements when rng was not valid for certain period
void checkForStuckRange();
Expand Down

0 comments on commit 0c0a660

Please sign in to comment.