control: rename _range_aid_enabled to _is_range_aid_suitable

rename rangeAidConditionsMet to checkRangeAidSuitability
This commit is contained in:
bresch
2019-10-15 11:10:36 +02:00
committed by Mathieu Bresciani
parent fac69d07a3
commit e09e3e17a1
2 changed files with 13 additions and 13 deletions
+10 -10
View File
@@ -471,14 +471,14 @@ void Ekf::controlOpticalFlowFusion()
if (!_inhibit_flow_use && _control_status.flags.opt_flow) { 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 // 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 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) { if ((preflight_motion_not_ok || flight_motion_not_ok) && !flow_required) {
_inhibit_flow_use = true; _inhibit_flow_use = true;
} }
} else if (_inhibit_flow_use && !_control_status.flags.opt_flow){ } 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 // 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 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) { if (preflight_motion_ok || flight_motion_ok || flow_required) {
_inhibit_flow_use = false; _inhibit_flow_use = false;
} }
@@ -1011,8 +1011,8 @@ void Ekf::controlHeightFusion()
{ {
// set control flags for the desired primary height source // set control flags for the desired primary height source
checkRangeAidConditionsMet(); checkRangeAidSuitability();
_range_aid_mode_selected = (_params.range_aid == 1) && areRangeAidConditionsMet(); _range_aid_mode_selected = (_params.range_aid == 1) && isRangeAidSuitable();
if (_params.vdist_sensor_type == VDIST_SENSOR_BARO) { 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 const bool horz_vel_valid = _control_status.flags.gps
|| _control_status.flags.ev_pos || _control_status.flags.ev_pos
@@ -1190,23 +1190,23 @@ void Ekf::checkRangeAidConditionsMet()
&& horz_vel_valid) { && horz_vel_valid) {
// check if we can use range finder measurements to estimate height, use hysteresis to avoid rapid switching // 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 // 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)
: (_terrain_vpos - _state.pos(2) < _params.max_hagl_for_range_aid * 0.7f); : (_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 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
: ground_vel < _params.max_vel_for_range_aid * 0.7f; : 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)) < 1.0f)
: ((_hagl_innov * _hagl_innov / (sq(_params.range_aid_innov_gate) * _hagl_innov_var)) < 0.01f); : ((_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 { } else {
_range_aid_mode_enabled = false; _is_range_aid_suitable = false;
} }
} }
+3 -3
View File
@@ -478,7 +478,7 @@ private:
bool _bad_vert_accel_detected{false}; ///< true when bad vertical accelerometer data has been detected bool _bad_vert_accel_detected{false}; ///< true when bad vertical accelerometer data has been detected
// variables used to control range aid functionality // 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 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 // variables used to check range finder validity data
@@ -649,8 +649,8 @@ private:
void controlHeightFusion(); void controlHeightFusion();
// determine if flight condition is suitable to use range finder instead of the primary height sensor // determine if flight condition is suitable to use range finder instead of the primary height sensor
void checkRangeAidConditionsMet(); void checkRangeAidSuitability();
bool areRangeAidConditionsMet() { return _range_aid_mode_enabled; } bool isRangeAidSuitable() { return _is_range_aid_suitable; }
// check for "stuck" range finder measurements when rng was not valid for certain period // check for "stuck" range finder measurements when rng was not valid for certain period
void checkRangeDataValidity(); void checkRangeDataValidity();