diff --git a/EKF/control.cpp b/EKF/control.cpp index f740930927..d9d9bfd43a 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -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() { // 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() } -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() && 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; } } diff --git a/EKF/ekf.h b/EKF/ekf.h index 2de8ddeac5..7f6da0fba5 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -478,7 +478,7 @@ private: bool _bad_vert_accel_detected{false}; ///< true when bad vertical accelerometer data has been detected // variables used to control range aid functionality - bool _range_aid_mode_enabled{false}; ///< true when range finder can be used in flight as the height reference instead of the primary height sensor + bool _is_range_aid_suitable{false}; ///< true when range finder can be used in flight 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 range finder validity data @@ -649,8 +649,8 @@ private: void controlHeightFusion(); // determine if flight condition is suitable to use range finder instead of the primary height sensor - void checkRangeAidConditionsMet(); - bool areRangeAidConditionsMet() { return _range_aid_mode_enabled; } + void checkRangeAidSuitability(); + bool isRangeAidSuitable() { return _is_range_aid_suitable; } // check for "stuck" range finder measurements when rng was not valid for certain period void checkRangeDataValidity();