fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 06:03:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

fix formatting

Browse Source
This commit is contained in:
ChristophTobler
2017-06-15 09:10:43 +02:00
parent 5036967343
commit 1fed209804
1 changed files with 35 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+35 -35
View File
@@ -704,9 +704,9 @@ void Ekf::controlHeightFusion()
if (_params.vdist_sensor_type == VDIST_SENSOR_BARO) {
_in_range_aid_mode = rangeAidConditionsMet(_in_range_aid_mode);
_in_range_aid_mode = rangeAidConditionsMet(_in_range_aid_mode);
if (_in_range_aid_mode && _range_data_ready && !_rng_hgt_faulty) {
if (_in_range_aid_mode && _range_data_ready && !_rng_hgt_faulty) {
setControlRangeHeight();
_fuse_height = true;
@@ -720,7 +720,7 @@ void Ekf::controlHeightFusion()
}
}
} else if (_baro_data_ready && !_baro_hgt_faulty && !_in_range_aid_mode) {
} else if (_baro_data_ready && !_baro_hgt_faulty && !_in_range_aid_mode) {
setControlBaroHeight();
_fuse_height = true;
@@ -740,9 +740,9 @@ 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);
_in_range_aid_mode = rangeAidConditionsMet(_in_range_aid_mode);
if (_in_range_aid_mode && _range_data_ready && !_rng_hgt_faulty) {
if (_in_range_aid_mode && _range_data_ready && !_rng_hgt_faulty) {
setControlRangeHeight();
_fuse_height = true;
@@ -756,7 +756,7 @@ void Ekf::controlHeightFusion()
}
}
} else if (_gps_data_ready && !_gps_hgt_faulty && !_in_range_aid_mode) {
} else if (_gps_data_ready && !_gps_hgt_faulty && !_in_range_aid_mode) {
setControlGPSHeight();
_fuse_height = true;
@@ -802,39 +802,40 @@ bool Ekf::rangeAidConditionsMet(bool in_range_aid_mode)
// 2) our ground speed is not higher than max_vel_for_dual_fusion
// 3) Our terrain estimate is stable (needs better checks)
if (_params.range_aid) {
// 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) {
use_range_finder = (_terrain_vpos - _state.pos(2) < _params.max_hagl_for_range_aid) && _terrain_initialised;
} else {
// if we were not using range aid in the previous iteration then require the current height above terrain to be
// smaller than 70 % of the maximum allowed ground distance for range aid
use_range_finder = (_terrain_vpos - _state.pos(2) < 0.7f * _params.max_hagl_for_range_aid) && _terrain_initialised;
}
// 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) {
use_range_finder = (_terrain_vpos - _state.pos(2) < _params.max_hagl_for_range_aid) && _terrain_initialised;
} else {
// if we were not using range aid in the previous iteration then require the current height above terrain to be
// smaller than 70 % of the maximum allowed ground distance for range aid
use_range_finder = (_terrain_vpos - _state.pos(2) < 0.7f * _params.max_hagl_for_range_aid) && _terrain_initialised;
}
bool horz_vel_valid = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow)
&& (_fault_status.value == 0);
&& (_fault_status.value == 0);
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) {
use_range_finder &= ground_vel < _params.max_vel_for_range_aid;
if (in_range_aid_mode) {
use_range_finder &= ground_vel < _params.max_vel_for_range_aid;
} else {
// if we were not using range aid in the previous iteration then require the ground velocity to be
// smaller than 70 % of the maximum allowed ground velocity for range aid
use_range_finder &= ground_vel < 0.7f * _params.max_vel_for_range_aid;
}
} else {
// if we were not using range aid in the previous iteration then require the ground velocity to be
// smaller than 70 % of the maximum allowed ground velocity for range aid
use_range_finder &= ground_vel < 0.7f * _params.max_vel_for_range_aid;
}
} else {
use_range_finder = false;
}
use_range_finder &= ((_hagl_innov * _hagl_innov / (sq(_params.range_aid_innov_gate) * _hagl_innov_var)) < 1.0f);
use_range_finder &= ((_hagl_innov * _hagl_innov / (sq(_params.range_aid_innov_gate) * _hagl_innov_var)) < 1.0f);
return use_range_finder;
} else {
return false;
}
@@ -876,15 +877,14 @@ void Ekf::controlAirDataFusion()
void Ekf::controlBetaFusion()
{
// control activation and initialisation/reset of wind states required for synthetic sideslip fusion fusion
// control activation and initialisation/reset of wind states required for synthetic sideslip fusion fusion
// If both airspeed and sideslip fusion have timed out then we no longer have valid wind estimates
bool sideslip_timed_out = _time_last_imu - _time_last_beta_fuse > 10e6;
bool airspeed_timed_out = _time_last_imu - _time_last_arsp_fuse > 10e6;
if(_control_status.flags.wind && airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & MASK_USE_DRAG)){
_control_status.flags.wind = false;
}
// If both airspeed and sideslip fusion have timed out then we no longer have valid wind estimates
bool sideslip_timed_out = _time_last_imu - _time_last_beta_fuse > 10e6;
bool airspeed_timed_out = _time_last_imu - _time_last_arsp_fuse > 10e6;
if(_control_status.flags.wind && airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & MASK_USE_DRAG)) {
_control_status.flags.wind = false;
}
// Perform synthetic sideslip fusion when in-air and sideslip fuson had been enabled externally in addition to the following criteria:
@@ -907,10 +907,10 @@ void Ekf::controlBetaFusion()
resetWindCovariance();
}
fuseSideslip();
fuseSideslip();
}
}