zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

EKF add clarity brackets to avoid potential confusion 

- fixes https://github.com/PX4/ecl/issues/555
master
Daniel Agar 6 years ago
parent
721f5e61a5
commit
a5e6191ba7
4 changed files with 36 additions and 36 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 44
      EKF/control.cpp
  2. 10
      EKF/ekf_helper.cpp
  3. 14
      EKF/estimator_interface.cpp
  4. 4
      EKF/terrain_estimator.cpp

44
EKF/control.cpp
Unescape 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();

10
EKF/ekf_helper.cpp
Unescape 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;

14
EKF/estimator_interface.cpp
Unescape 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;

4
EKF/terrain_estimator.cpp
Unescape 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;

Write Preview
Loading…
Cancel
Save