dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

control: rename _range_aid_enabled to _is_range_aid_suitable 

rename rangeAidConditionsMet to checkRangeAidSuitability
This commit is contained in:
bresch 2019-10-14 14:28:18 +02:00 committed by Mathieu Bresciani
parent fac69d07a3
commit e09e3e17a1
2 changed files with 13 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
EKF/control.cpp
View File

@ -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;
}
}

6
EKF/ekf.h
View File

@ -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();