|
|
|
@ -471,14 +471,14 @@ void Ekf::controlOpticalFlowFusion()
@@ -471,14 +471,14 @@ void Ekf::controlOpticalFlowFusion()
|
|
|
|
|
if (!_inhibit_flow_use && _control_status.flags.opt_flow) { |
|
|
|
|
// inhibit use of optical flow if motion is unsuitable and we are not reliant on it for flight navigation
|
|
|
|
|
bool preflight_motion_not_ok = !_control_status.flags.in_air && ((_imu_sample_delayed.time_us - _time_good_motion_us) > (uint64_t)1E5); |
|
|
|
|
bool flight_motion_not_ok = _control_status.flags.in_air && !areRangeAidConditionsMet(); |
|
|
|
|
bool flight_motion_not_ok = _control_status.flags.in_air && !isRangeAidSuitable(); |
|
|
|
|
if ((preflight_motion_not_ok || flight_motion_not_ok) && !flow_required) { |
|
|
|
|
_inhibit_flow_use = true; |
|
|
|
|
} |
|
|
|
|
} else if (_inhibit_flow_use && !_control_status.flags.opt_flow){ |
|
|
|
|
// allow use of optical flow if motion is suitable or we are reliant on it for flight navigation
|
|
|
|
|
bool preflight_motion_ok = !_control_status.flags.in_air && ((_imu_sample_delayed.time_us - _time_bad_motion_us) > (uint64_t)5E6); |
|
|
|
|
bool flight_motion_ok = _control_status.flags.in_air && areRangeAidConditionsMet(); |
|
|
|
|
bool flight_motion_ok = _control_status.flags.in_air && isRangeAidSuitable(); |
|
|
|
|
if (preflight_motion_ok || flight_motion_ok || flow_required) { |
|
|
|
|
_inhibit_flow_use = false; |
|
|
|
|
} |
|
|
|
@ -1011,8 +1011,8 @@ void Ekf::controlHeightFusion()
@@ -1011,8 +1011,8 @@ void Ekf::controlHeightFusion()
|
|
|
|
|
{ |
|
|
|
|
// set control flags for the desired primary height source
|
|
|
|
|
|
|
|
|
|
checkRangeAidConditionsMet(); |
|
|
|
|
_range_aid_mode_selected = (_params.range_aid == 1) && areRangeAidConditionsMet(); |
|
|
|
|
checkRangeAidSuitability(); |
|
|
|
|
_range_aid_mode_selected = (_params.range_aid == 1) && isRangeAidSuitable(); |
|
|
|
|
|
|
|
|
|
if (_params.vdist_sensor_type == VDIST_SENSOR_BARO) { |
|
|
|
|
|
|
|
|
@ -1177,7 +1177,7 @@ void Ekf::controlHeightFusion()
@@ -1177,7 +1177,7 @@ void Ekf::controlHeightFusion()
|
|
|
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void Ekf::checkRangeAidConditionsMet() |
|
|
|
|
void Ekf::checkRangeAidSuitability() |
|
|
|
|
{ |
|
|
|
|
const bool horz_vel_valid = _control_status.flags.gps |
|
|
|
|
|| _control_status.flags.ev_pos |
|
|
|
@ -1190,23 +1190,23 @@ void Ekf::checkRangeAidConditionsMet()
@@ -1190,23 +1190,23 @@ void Ekf::checkRangeAidConditionsMet()
|
|
|
|
|
&& horz_vel_valid) { |
|
|
|
|
// check if we can use range finder measurements to estimate height, use hysteresis to avoid rapid switching
|
|
|
|
|
// Note that the 0.7 coefficients and the innovation check are arbitrary values but work well in practice
|
|
|
|
|
const bool is_in_range = _range_aid_mode_enabled |
|
|
|
|
const bool is_in_range = _is_range_aid_suitable |
|
|
|
|
? (_terrain_vpos - _state.pos(2) < _params.max_hagl_for_range_aid) |
|
|
|
|
: (_terrain_vpos - _state.pos(2) < _params.max_hagl_for_range_aid * 0.7f); |
|
|
|
|
|
|
|
|
|
const float ground_vel = sqrtf(_state.vel(0) * _state.vel(0) + _state.vel(1) * _state.vel(1)); |
|
|
|
|
const bool is_below_max_speed = _range_aid_mode_enabled |
|
|
|
|
const bool is_below_max_speed = _is_range_aid_suitable |
|
|
|
|
? ground_vel < _params.max_vel_for_range_aid |
|
|
|
|
: ground_vel < _params.max_vel_for_range_aid * 0.7f; |
|
|
|
|
|
|
|
|
|
const bool is_hagl_stable = _range_aid_mode_enabled |
|
|
|
|
const bool is_hagl_stable = _is_range_aid_suitable |
|
|
|
|
? ((_hagl_innov * _hagl_innov / (sq(_params.range_aid_innov_gate) * _hagl_innov_var)) < 1.0f) |
|
|
|
|
: ((_hagl_innov * _hagl_innov / (sq(_params.range_aid_innov_gate) * _hagl_innov_var)) < 0.01f); |
|
|
|
|
|
|
|
|
|
_range_aid_mode_enabled = is_in_range && is_below_max_speed && is_hagl_stable; |
|
|
|
|
_is_range_aid_suitable = is_in_range && is_below_max_speed && is_hagl_stable; |
|
|
|
|
|
|
|
|
|
} else { |
|
|
|
|
_range_aid_mode_enabled = false; |
|
|
|
|
_is_range_aid_suitable = false; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|