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

EKF add clarity brackets to avoid potential confusion

Browse Source 
- fixes https://github.com/PX4/ecl/issues/555
This commit is contained in:
Daniel Agar
2019-01-30 09:15:53 -05:00
parent 721f5e61a5
commit a5e6191ba7
4 changed files with 36 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+22 -22
View File
@@ -172,7 +172,7 @@ void Ekf::controlExternalVisionFusion()
&& _control_status.flags.yaw_align) { && _control_status.flags.yaw_align) {
// check for a exernal vision measurement that has fallen behind the fusion time horizon // check for a exernal vision measurement that has fallen behind the fusion time horizon
if (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL) { if ((_time_last_imu - _time_last_ext_vision) < (2 * EV_MAX_INTERVAL)) {
// turn on use of external vision measurements for position // turn on use of external vision measurements for position
_control_status.flags.ev_pos = true; _control_status.flags.ev_pos = true;
ECL_INFO("EKF commencing external vision position fusion"); ECL_INFO("EKF commencing external vision position fusion");
@@ -248,7 +248,7 @@ void Ekf::controlExternalVisionFusion()
// determine if we should start using the height observations // determine if we should start using the height observations
if (_params.vdist_sensor_type == VDIST_SENSOR_EV) { if (_params.vdist_sensor_type == VDIST_SENSOR_EV) {
// don't start using EV data unless data is arriving frequently // don't start using EV data unless data is arriving frequently
if (!_control_status.flags.ev_hgt && (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL)) { if (!_control_status.flags.ev_hgt && ((_time_last_imu - _time_last_ext_vision) < (2 * EV_MAX_INTERVAL))) {
setControlEVHeight(); setControlEVHeight();
resetHeight(); resetHeight();
} }
@@ -310,9 +310,9 @@ void Ekf::controlExternalVisionFusion()
_vel_pos_innov[4] = _state.pos(1) - _ev_sample_delayed.posNED(1); _vel_pos_innov[4] = _state.pos(1) - _ev_sample_delayed.posNED(1);
// check if we have been deadreckoning too long // check if we have been deadreckoning too long
if (_time_last_imu - _time_last_pos_fuse > _params.reset_timeout_max) { if ((_time_last_imu - _time_last_pos_fuse) > _params.reset_timeout_max) {
// don't reset velocity if we have another source of aiding constraining it // don't reset velocity if we have another source of aiding constraining it
if (_time_last_imu - _time_last_of_fuse > (uint64_t)1E6) { if ((_time_last_imu - _time_last_of_fuse) > (uint64_t)1E6) {
resetVelocity(); resetVelocity();
} }
@@ -343,7 +343,7 @@ void Ekf::controlExternalVisionFusion()
} else if (_control_status.flags.ev_pos } else if (_control_status.flags.ev_pos
&& (_time_last_imu >= _time_last_ext_vision) && (_time_last_imu >= _time_last_ext_vision)
&& (_time_last_imu - _time_last_ext_vision > (uint64_t)_params.reset_timeout_max)) { && ((_time_last_imu - _time_last_ext_vision) > (uint64_t)_params.reset_timeout_max)) {
// Turn off EV fusion mode if no data has been received // Turn off EV fusion mode if no data has been received
_control_status.flags.ev_pos = false; _control_status.flags.ev_pos = false;
@@ -420,7 +420,7 @@ void Ekf::controlOpticalFlowFusion()
_control_status.flags.opt_flow = false; _control_status.flags.opt_flow = false;
_time_last_of_fuse = 0; _time_last_of_fuse = 0;
} else if (_time_last_imu - _time_last_of_fuse > (uint64_t)_params.reset_timeout_max) { } else if ((_time_last_imu - _time_last_of_fuse) > (uint64_t)_params.reset_timeout_max) {
_control_status.flags.opt_flow = false; _control_status.flags.opt_flow = false;
} }
@@ -465,7 +465,7 @@ void Ekf::controlOpticalFlowFusion()
&& !_control_status.flags.gps && !_control_status.flags.gps
&& !_control_status.flags.ev_pos) { && !_control_status.flags.ev_pos) {
bool do_reset = _time_last_imu - _time_last_of_fuse > _params.reset_timeout_max; bool do_reset = ((_time_last_imu - _time_last_of_fuse) > _params.reset_timeout_max);
if (do_reset) { if (do_reset) {
resetVelocity(); resetVelocity();
@@ -496,7 +496,7 @@ void Ekf::controlOpticalFlowFusion()
if (_flow_data_ready && (_imu_sample_delayed.time_us > _flow_sample_delayed.time_us - uint32_t(1e6f * _flow_sample_delayed.dt) / 2)) { if (_flow_data_ready && (_imu_sample_delayed.time_us > _flow_sample_delayed.time_us - uint32_t(1e6f * _flow_sample_delayed.dt) / 2)) {
// Fuse optical flow LOS rate observations into the main filter only if height above ground has been updated recently // Fuse optical flow LOS rate observations into the main filter only if height above ground has been updated recently
// but use a relaxed time criteria to enable it to coast through bad range finder data // but use a relaxed time criteria to enable it to coast through bad range finder data
if (_control_status.flags.opt_flow && (_time_last_imu - _time_last_hagl_fuse < (uint64_t)10e6)) { if (_control_status.flags.opt_flow && ((_time_last_imu - _time_last_hagl_fuse) < (uint64_t)10e6)) {
fuseOptFlow(); fuseOptFlow();
_last_known_posNE(0) = _state.pos(0); _last_known_posNE(0) = _state.pos(0);
_last_known_posNE(1) = _state.pos(1); _last_known_posNE(1) = _state.pos(1);
@@ -516,7 +516,7 @@ void Ekf::controlGpsFusion()
&& ISFINITE(_gps_sample_delayed.yaw) && ISFINITE(_gps_sample_delayed.yaw)
&& _control_status.flags.tilt_align && _control_status.flags.tilt_align
&& (!_control_status.flags.gps_yaw || !_control_status.flags.yaw_align) && (!_control_status.flags.gps_yaw || !_control_status.flags.yaw_align)
&& (_time_last_imu - _time_last_gps < 2 * GPS_MAX_INTERVAL)) { && ((_time_last_imu - _time_last_gps) < (2 * GPS_MAX_INTERVAL))) {
if (resetGpsAntYaw()) { if (resetGpsAntYaw()) {
// flag the yaw as aligned // flag the yaw as aligned
@@ -598,13 +598,13 @@ void Ekf::controlGpsFusion()
if (_control_status.flags.gps) { if (_control_status.flags.gps) {
// We are relying on aiding to constrain drift so after a specified time // We are relying on aiding to constrain drift so after a specified time
// with no aiding we need to do something // with no aiding we need to do something
bool do_reset = (_time_last_imu - _time_last_pos_fuse > _params.reset_timeout_max) bool do_reset = ((_time_last_imu - _time_last_pos_fuse) > _params.reset_timeout_max)
&& (_time_last_imu - _time_last_delpos_fuse > _params.reset_timeout_max) && ((_time_last_imu - _time_last_delpos_fuse) > _params.reset_timeout_max)
&& (_time_last_imu - _time_last_vel_fuse > _params.reset_timeout_max) && ((_time_last_imu - _time_last_vel_fuse) > _params.reset_timeout_max)
&& (_time_last_imu - _time_last_of_fuse > _params.reset_timeout_max); && ((_time_last_imu - _time_last_of_fuse) > _params.reset_timeout_max);
// We haven't had an absolute position fix for a longer time so need to do something // We haven't had an absolute position fix for a longer time so need to do something
do_reset = do_reset || (_time_last_imu - _time_last_pos_fuse > 2 * _params.reset_timeout_max); do_reset = do_reset || ((_time_last_imu - _time_last_pos_fuse) > (2 * _params.reset_timeout_max));
if (do_reset) { if (do_reset) {
// use GPS velocity data to check and correct yaw angle if a FW vehicle // use GPS velocity data to check and correct yaw angle if a FW vehicle
@@ -974,7 +974,7 @@ void Ekf::controlHeightFusion()
// Turn off ground effect compensation if it times out or sufficient height has been gained // Turn off ground effect compensation if it times out or sufficient height has been gained
// since takeoff. // since takeoff.
if (_control_status.flags.gnd_effect) { if (_control_status.flags.gnd_effect) {
if ((_time_last_imu - _time_last_gnd_effect_on > GNDEFFECT_TIMEOUT) || if (((_time_last_imu - _time_last_gnd_effect_on) > GNDEFFECT_TIMEOUT) ||
(((_last_on_ground_posD - _state.pos(2)) > _params.gnd_effect_max_hgt) && (((_last_on_ground_posD - _state.pos(2)) > _params.gnd_effect_max_hgt) &&
_control_status.flags.in_air)) { _control_status.flags.in_air)) {
@@ -1211,7 +1211,7 @@ void Ekf::checkRangeDataValidity()
// Check for "stuck" range finder measurements when range was not valid for certain period // Check for "stuck" range finder measurements when range was not valid for certain period
// This handles a failure mode observed with some lidar sensors // This handles a failure mode observed with some lidar sensors
if (_range_sample_delayed.time_us - _time_last_rng_ready > (uint64_t)10e6 && if (((_range_sample_delayed.time_us - _time_last_rng_ready) > (uint64_t)10e6) &&
_control_status.flags.in_air) { _control_status.flags.in_air) {
// require a variance of rangefinder values to check for "stuck" measurements // require a variance of rangefinder values to check for "stuck" measurements
@@ -1244,8 +1244,8 @@ void Ekf::controlAirDataFusion()
// control activation and initialisation/reset of wind states required for airspeed fusion // control activation and initialisation/reset of wind states required for airspeed fusion
// If both airspeed and sideslip fusion have timed out and we are not using a drag observation model then we no longer have valid wind estimates // If both airspeed and sideslip fusion have timed out and we are not using a drag observation model then we no longer have valid wind estimates
bool airspeed_timed_out = _time_last_imu - _time_last_arsp_fuse > (uint64_t)10e6; bool airspeed_timed_out = ((_time_last_imu - _time_last_arsp_fuse) > (uint64_t)10e6);
bool sideslip_timed_out = _time_last_imu - _time_last_beta_fuse > (uint64_t)10e6; bool sideslip_timed_out = ((_time_last_imu - _time_last_beta_fuse) > (uint64_t)10e6);
if (_control_status.flags.wind && airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & MASK_USE_DRAG)) { if (_control_status.flags.wind && airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & MASK_USE_DRAG)) {
_control_status.flags.wind = false; _control_status.flags.wind = false;
@@ -1287,8 +1287,8 @@ 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 and we are not using a drag observation model then we no longer have valid wind estimates // If both airspeed and sideslip fusion have timed out and we are not using a drag observation model then we no longer have valid wind estimates
bool sideslip_timed_out = _time_last_imu - _time_last_beta_fuse > (uint64_t)10e6; bool sideslip_timed_out = ((_time_last_imu - _time_last_beta_fuse) > (uint64_t)10e6);
bool airspeed_timed_out = _time_last_imu - _time_last_arsp_fuse > (uint64_t)10e6; bool airspeed_timed_out = ((_time_last_imu - _time_last_arsp_fuse) > (uint64_t)10e6);
if (_control_status.flags.wind && airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & MASK_USE_DRAG)) { if (_control_status.flags.wind && airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & MASK_USE_DRAG)) {
_control_status.flags.wind = false; _control_status.flags.wind = false;
@@ -1297,7 +1297,7 @@ void Ekf::controlBetaFusion()
// Perform synthetic sideslip fusion when in-air and sideslip fuson had been enabled externally in addition to the following criteria: // Perform synthetic sideslip fusion when in-air and sideslip fuson had been enabled externally in addition to the following criteria:
// Suffient time has lapsed sice the last fusion // Suffient time has lapsed sice the last fusion
bool beta_fusion_time_triggered = _time_last_imu - _time_last_beta_fuse > _params.beta_avg_ft_us; bool beta_fusion_time_triggered = ((_time_last_imu - _time_last_beta_fuse) > _params.beta_avg_ft_us);
if (beta_fusion_time_triggered && _control_status.flags.fuse_beta && _control_status.flags.in_air) { if (beta_fusion_time_triggered && _control_status.flags.fuse_beta && _control_status.flags.in_air) {
// If starting wind state estimation, reset the wind states and covariances before fusing any data // If starting wind state estimation, reset the wind states and covariances before fusing any data
@@ -1593,7 +1593,7 @@ void Ekf::controlVelPosFusion()
_using_synthetic_position = true; _using_synthetic_position = true;
// Fuse synthetic position observations every 200msec // Fuse synthetic position observations every 200msec
if ((_time_last_imu - _time_last_fake_gps > (uint64_t)2e5) || _fuse_height) { if (((_time_last_imu - _time_last_fake_gps) > (uint64_t)2e5) || _fuse_height) {
// Reset position and velocity states if we re-commence this aiding method // Reset position and velocity states if we re-commence this aiding method
if ((_time_last_imu - _time_last_fake_gps) > (uint64_t)4e5) { if ((_time_last_imu - _time_last_fake_gps) > (uint64_t)4e5) {
resetPosition(); resetPosition();
EKF/ekf_helper.cpp
+5 -5
View File
@@ -1322,11 +1322,11 @@ bool Ekf::global_position_is_valid()
void Ekf::update_deadreckoning_status() void Ekf::update_deadreckoning_status()
{ {
bool velPosAiding = (_control_status.flags.gps || _control_status.flags.ev_pos) bool velPosAiding = (_control_status.flags.gps || _control_status.flags.ev_pos)
&& ((_time_last_imu - _time_last_pos_fuse <= _params.no_aid_timeout_max) && (((_time_last_imu - _time_last_pos_fuse) <= _params.no_aid_timeout_max)
|| (_time_last_imu - _time_last_vel_fuse <= _params.no_aid_timeout_max) || ((_time_last_imu - _time_last_vel_fuse) <= _params.no_aid_timeout_max)
|| (_time_last_imu - _time_last_delpos_fuse <= _params.no_aid_timeout_max)); || ((_time_last_imu - _time_last_delpos_fuse) <= _params.no_aid_timeout_max));
bool optFlowAiding = _control_status.flags.opt_flow && (_time_last_imu - _time_last_of_fuse <= _params.no_aid_timeout_max); bool optFlowAiding = _control_status.flags.opt_flow && ((_time_last_imu - _time_last_of_fuse) <= _params.no_aid_timeout_max);
bool airDataAiding = _control_status.flags.wind && (_time_last_imu - _time_last_arsp_fuse <= _params.no_aid_timeout_max) && (_time_last_imu - _time_last_beta_fuse <= _params.no_aid_timeout_max); bool airDataAiding = _control_status.flags.wind && ((_time_last_imu - _time_last_arsp_fuse) <= _params.no_aid_timeout_max) && ((_time_last_imu - _time_last_beta_fuse) <= _params.no_aid_timeout_max);
_is_wind_dead_reckoning = !velPosAiding && !optFlowAiding && airDataAiding; _is_wind_dead_reckoning = !velPosAiding && !optFlowAiding && airDataAiding;
_is_dead_reckoning = !velPosAiding && !optFlowAiding && !airDataAiding; _is_dead_reckoning = !velPosAiding && !optFlowAiding && !airDataAiding;
EKF/estimator_interface.cpp
+7 -7
View File
@@ -176,7 +176,7 @@ void EstimatorInterface::setMagData(uint64_t time_usec, float (&data)[3])
} }
// limit data rate to prevent data being lost // limit data rate to prevent data being lost
if (time_usec - _time_last_mag > _min_obs_interval_us) { if ((time_usec - _time_last_mag) > _min_obs_interval_us) {
magSample mag_sample_new; magSample mag_sample_new;
mag_sample_new.time_us = time_usec - _params.mag_delay_ms * 1000; mag_sample_new.time_us = time_usec - _params.mag_delay_ms * 1000;
@@ -269,7 +269,7 @@ void EstimatorInterface::setBaroData(uint64_t time_usec, float data)
} }
// limit data rate to prevent data being lost // limit data rate to prevent data being lost
if (time_usec - _time_last_baro > _min_obs_interval_us) { if ((time_usec - _time_last_baro) > _min_obs_interval_us) {
baroSample baro_sample_new; baroSample baro_sample_new;
baro_sample_new.hgt = data; baro_sample_new.hgt = data;
@@ -302,7 +302,7 @@ void EstimatorInterface::setAirspeedData(uint64_t time_usec, float true_airspeed
} }
// limit data rate to prevent data being lost // limit data rate to prevent data being lost
if (time_usec - _time_last_airspeed > _min_obs_interval_us) { if ((time_usec - _time_last_airspeed) > _min_obs_interval_us) {
airspeedSample airspeed_sample_new; airspeedSample airspeed_sample_new;
airspeed_sample_new.true_airspeed = true_airspeed; airspeed_sample_new.true_airspeed = true_airspeed;
airspeed_sample_new.eas2tas = eas2tas; airspeed_sample_new.eas2tas = eas2tas;
@@ -332,7 +332,7 @@ void EstimatorInterface::setRangeData(uint64_t time_usec, float data)
} }
// limit data rate to prevent data being lost // limit data rate to prevent data being lost
if (time_usec - _time_last_range > _min_obs_interval_us) { if ((time_usec - _time_last_range) > _min_obs_interval_us) {
rangeSample range_sample_new; rangeSample range_sample_new;
range_sample_new.rng = data; range_sample_new.rng = data;
range_sample_new.time_us = time_usec - _params.range_delay_ms * 1000; range_sample_new.time_us = time_usec - _params.range_delay_ms * 1000;
@@ -361,7 +361,7 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl
} }
// limit data rate to prevent data being lost // limit data rate to prevent data being lost
if (time_usec - _time_last_optflow > _min_obs_interval_us) { if ((time_usec - _time_last_optflow) > _min_obs_interval_us) {
// check if enough integration time and fail if integration time is less than 50% // check if enough integration time and fail if integration time is less than 50%
// of min arrival interval because too much data is being lost // of min arrival interval because too much data is being lost
float delta_time = 1e-6f * (float)flow->dt; float delta_time = 1e-6f * (float)flow->dt;
@@ -433,7 +433,7 @@ void EstimatorInterface::setExtVisionData(uint64_t time_usec, ext_vision_message
} }
// limit data rate to prevent data being lost // limit data rate to prevent data being lost
if (time_usec - _time_last_ext_vision > _min_obs_interval_us) { if ((time_usec - _time_last_ext_vision) > _min_obs_interval_us) {
extVisionSample ev_sample_new; extVisionSample ev_sample_new;
// calculate the system time-stamp for the mid point of the integration period // calculate the system time-stamp for the mid point of the integration period
ev_sample_new.time_us = time_usec - _params.ev_delay_ms * 1000; ev_sample_new.time_us = time_usec - _params.ev_delay_ms * 1000;
@@ -471,7 +471,7 @@ void EstimatorInterface::setAuxVelData(uint64_t time_usec, float (&data)[2], flo
} }
// limit data rate to prevent data being lost // limit data rate to prevent data being lost
if (time_usec - _time_last_auxvel > _min_obs_interval_us) { if ((time_usec - _time_last_auxvel) > _min_obs_interval_us) {
auxVelSample auxvel_sample_new; auxVelSample auxvel_sample_new;
auxvel_sample_new.time_us = time_usec - _params.auxvel_delay_ms * 1000; auxvel_sample_new.time_us = time_usec - _params.auxvel_delay_ms * 1000;
EKF/terrain_estimator.cpp
+2 -2
View File
@@ -149,7 +149,7 @@ void Ekf::fuseHagl()
_innov_check_fail_status.flags.reject_hagl = false; _innov_check_fail_status.flags.reject_hagl = false;
} else { } else {
// If we have been rejecting range data for too long, reset to measurement // If we have been rejecting range data for too long, reset to measurement
if (_time_last_imu - _time_last_hagl_fuse > (uint64_t)10E6) { if ((_time_last_imu - _time_last_hagl_fuse) > (uint64_t)10E6) {
_terrain_vpos = _state.pos(2) + meas_hagl; _terrain_vpos = _state.pos(2) + meas_hagl;
_terrain_var = obs_variance; _terrain_var = obs_variance;
} else { } else {
@@ -171,7 +171,7 @@ bool Ekf::get_terrain_valid()
// determine terrain validity // determine terrain validity
void Ekf::update_terrain_valid() void Ekf::update_terrain_valid()
{ {
if (_terrain_initialised && (_time_last_imu - _time_last_hagl_fuse < (uint64_t)5e6)) { if (_terrain_initialised && ((_time_last_imu - _time_last_hagl_fuse) < (uint64_t)5e6)) {
_hagl_valid = true; _hagl_valid = true;