From a5e6191ba748913d3aad1a03bc1eb9052c76a72b Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 29 Jan 2019 16:17:08 -0500 Subject: [PATCH] EKF add clarity brackets to avoid potential confusion - fixes https://github.com/PX4/ecl/issues/555 --- EKF/control.cpp | 44 ++++++++++++++++++------------------- EKF/ekf_helper.cpp | 10 ++++----- EKF/estimator_interface.cpp | 14 ++++++------ EKF/terrain_estimator.cpp | 4 ++-- 4 files changed, 36 insertions(+), 36 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 2ede3541de..82642bd717 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -172,7 +172,7 @@ void Ekf::controlExternalVisionFusion() && _control_status.flags.yaw_align) { // 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 _control_status.flags.ev_pos = true; ECL_INFO("EKF commencing external vision position fusion"); @@ -248,7 +248,7 @@ void Ekf::controlExternalVisionFusion() // determine if we should start using the height observations if (_params.vdist_sensor_type == VDIST_SENSOR_EV) { // 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(); resetHeight(); } @@ -310,9 +310,9 @@ void Ekf::controlExternalVisionFusion() _vel_pos_innov[4] = _state.pos(1) - _ev_sample_delayed.posNED(1); // 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 - if (_time_last_imu - _time_last_of_fuse > (uint64_t)1E6) { + if ((_time_last_imu - _time_last_of_fuse) > (uint64_t)1E6) { resetVelocity(); } @@ -343,7 +343,7 @@ void Ekf::controlExternalVisionFusion() } else if (_control_status.flags.ev_pos && (_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 _control_status.flags.ev_pos = false; @@ -420,7 +420,7 @@ void Ekf::controlOpticalFlowFusion() _control_status.flags.opt_flow = false; _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; } @@ -465,7 +465,7 @@ void Ekf::controlOpticalFlowFusion() && !_control_status.flags.gps && !_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) { 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)) { // 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 - 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(); _last_known_posNE(0) = _state.pos(0); _last_known_posNE(1) = _state.pos(1); @@ -516,7 +516,7 @@ void Ekf::controlGpsFusion() && ISFINITE(_gps_sample_delayed.yaw) && _control_status.flags.tilt_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()) { // flag the yaw as aligned @@ -598,13 +598,13 @@ void Ekf::controlGpsFusion() if (_control_status.flags.gps) { // We are relying on aiding to constrain drift so after a specified time // with no aiding we need to do something - 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_vel_fuse > _params.reset_timeout_max) - && (_time_last_imu - _time_last_of_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_vel_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 - 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) { // 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 // since takeoff. 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) && _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 // 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) { // 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 // 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 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 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)) { _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 // 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 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 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)) { _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: // 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 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; // 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 if ((_time_last_imu - _time_last_fake_gps) > (uint64_t)4e5) { resetPosition(); diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 6ffa53cd0e..8956546384 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -1322,11 +1322,11 @@ bool Ekf::global_position_is_valid() void Ekf::update_deadreckoning_status() { 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_vel_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 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); + && (((_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_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 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_dead_reckoning = !velPosAiding && !optFlowAiding && !airDataAiding; diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 0dc1f0b18c..fd981c0e12 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -176,7 +176,7 @@ void EstimatorInterface::setMagData(uint64_t time_usec, float (&data)[3]) } // 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; 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 - if (time_usec - _time_last_baro > _min_obs_interval_us) { + if ((time_usec - _time_last_baro) > _min_obs_interval_us) { baroSample baro_sample_new; 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 - if (time_usec - _time_last_airspeed > _min_obs_interval_us) { + if ((time_usec - _time_last_airspeed) > _min_obs_interval_us) { airspeedSample airspeed_sample_new; airspeed_sample_new.true_airspeed = true_airspeed; 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 - if (time_usec - _time_last_range > _min_obs_interval_us) { + if ((time_usec - _time_last_range) > _min_obs_interval_us) { rangeSample range_sample_new; range_sample_new.rng = data; 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 - 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% // of min arrival interval because too much data is being lost 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 - 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; // 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; @@ -471,7 +471,7 @@ void EstimatorInterface::setAuxVelData(uint64_t time_usec, float (&data)[2], flo } // 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; auxvel_sample_new.time_us = time_usec - _params.auxvel_delay_ms * 1000; diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp index d54b9639ce..2073b8ae10 100644 --- a/EKF/terrain_estimator.cpp +++ b/EKF/terrain_estimator.cpp @@ -149,7 +149,7 @@ void Ekf::fuseHagl() _innov_check_fail_status.flags.reject_hagl = false; } else { // 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_var = obs_variance; } else { @@ -171,7 +171,7 @@ bool Ekf::get_terrain_valid() // determine terrain validity 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;