diff --git a/EKF/control.cpp b/EKF/control.cpp index 0648c43b97..01ecd2c736 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -86,10 +86,10 @@ void Ekf::controlFusionModes() // check for intermittent data (before pop_first_older_than) const baroSample &baro_init = _baro_buffer.get_newest(); - _baro_hgt_faulty = !((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL); + _baro_hgt_faulty = !isRecent(baro_init.time_us, 2 * BARO_MAX_INTERVAL); const gpsSample &gps_init = _gps_buffer.get_newest(); - _gps_hgt_intermittent = !((_time_last_imu - gps_init.time_us) < 2 * GPS_MAX_INTERVAL); + _gps_hgt_intermittent = !isRecent(gps_init.time_us, 2 * GPS_MAX_INTERVAL); // check for arrival of new sensor data at the fusion time horizon _gps_data_ready = _gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed); @@ -143,7 +143,7 @@ void Ekf::controlFusionModes() // check if we should fuse flow data for terrain estimation if (!_flow_for_terrain_data_ready && _flow_data_ready && _control_status.flags.in_air) { // only fuse flow for terrain if range data hasn't been fused for 5 seconds - _flow_for_terrain_data_ready = (_time_last_imu - _time_last_hagl_fuse) > 5 * 1000 * 1000; + _flow_for_terrain_data_ready = isTimedOut(_time_last_hagl_fuse, (uint64_t)5E6); // only fuse flow for terrain if the main filter is not fusing flow and we are using gps _flow_for_terrain_data_ready &= (!_control_status.flags.opt_flow && _control_status.flags.gps); } @@ -192,7 +192,7 @@ void Ekf::controlExternalVisionFusion() if (_control_status.flags.tilt_align && _control_status.flags.yaw_align) { // check for a external vision measurement that has fallen behind the fusion time horizon - if ((_time_last_imu - _time_last_ext_vision) < (2 * EV_MAX_INTERVAL)) { + if (isRecent(_time_last_ext_vision, 2 * EV_MAX_INTERVAL)) { // turn on use of external vision measurements for position if (_params.fusion_mode & MASK_USE_EVPOS && !_control_status.flags.ev_pos) { _control_status.flags.ev_pos = true; @@ -212,7 +212,7 @@ void Ekf::controlExternalVisionFusion() // external vision yaw aiding selection logic if (!_control_status.flags.gps && (_params.fusion_mode & MASK_USE_EVYAW) && !_control_status.flags.ev_yaw && _control_status.flags.tilt_align) { // don't start using EV data unless daa is arriving frequently - if (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL) { + if (isRecent(_time_last_ext_vision, 2 * EV_MAX_INTERVAL)) { // reset the yaw angle to the value from the observation quaternion // get the roll, pitch, yaw estimates from the quaternion states Eulerf euler_init(_state.quat_nominal); @@ -325,9 +325,10 @@ void Ekf::controlExternalVisionFusion() ev_pos_obs_var(1) = fmaxf(ev_pos_var(1, 0), sq(0.01f)); // check if we have been deadreckoning too long - if ((_time_last_imu - _time_last_hor_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) && ((_time_last_imu - _time_last_hor_vel_fuse) > (uint64_t)1E6)) { + if (isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)) { + // only reset velocity if we have no another source of aiding constraining it + if (isTimedOut(_time_last_of_fuse, (uint64_t)1E6) && + isTimedOut(_time_last_hor_vel_fuse, (uint64_t)1E6)) { resetVelocity(); } @@ -366,9 +367,10 @@ void Ekf::controlExternalVisionFusion() _ev_vel_innov = _state.vel - vel_aligned; // check if we have been deadreckoning too long - if ((_time_last_imu - _time_last_hor_vel_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) && ((_time_last_imu - _time_last_hor_pos_fuse) > (uint64_t)1E6)) { + if (isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)) { + // only reset velocity if we have no another source of aiding constraining it + if (isTimedOut(_time_last_of_fuse, (uint64_t)1E6) && + isTimedOut(_time_last_hor_pos_fuse, (uint64_t)1E6)) { resetVelocity(); } } @@ -387,8 +389,7 @@ void Ekf::controlExternalVisionFusion() } } else if ((_control_status.flags.ev_pos || _control_status.flags.ev_vel) - && (_time_last_imu >= _time_last_ext_vision) - && ((_time_last_imu - _time_last_ext_vision) > (uint64_t)_params.reset_timeout_max)) { + && isTimedOut(_time_last_ext_vision, (uint64_t)_params.reset_timeout_max)) { // Turn off EV fusion mode if no data has been received stopEvFusion(); @@ -466,7 +467,7 @@ void Ekf::controlOpticalFlowFusion() stopFlowFusion(); _time_last_of_fuse = 0; - } else if ((_time_last_imu - _time_last_of_fuse) > (uint64_t)_params.reset_timeout_max) { + } else if (isTimedOut(_time_last_of_fuse, (uint64_t)_params.reset_timeout_max)) { stopFlowFusion(); } @@ -508,7 +509,7 @@ void Ekf::controlOpticalFlowFusion() // handle the case when we have optical flow, are reliant on it, but have not been using it for an extended period if (isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow)) { - bool do_reset = ((_time_last_imu - _time_last_of_fuse) > _params.reset_timeout_max); + bool do_reset = isTimedOut(_time_last_of_fuse, _params.reset_timeout_max); if (do_reset) { resetVelocity(); @@ -539,7 +540,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 && isTimedOut(_time_last_hagl_fuse, (uint64_t)10e6)) { fuseOptFlow(); _last_known_posNE(0) = _state.pos(0); _last_known_posNE(1) = _state.pos(1); @@ -559,7 +560,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))) { + && isRecent(_time_last_gps, 2 * GPS_MAX_INTERVAL)) { if (resetGpsAntYaw()) { // flag the yaw as aligned @@ -584,8 +585,8 @@ void Ekf::controlGpsFusion() // Determine if we should use GPS aiding for velocity and horizontal position // To start using GPS we need angular alignment completed, the local NED origin set and GPS data that has not failed checks recently - bool gps_checks_passing = (_time_last_imu - _last_gps_fail_us > (uint64_t)5e6); - bool gps_checks_failing = (_time_last_imu - _last_gps_pass_us > (uint64_t)5e6); + bool gps_checks_passing = isTimedOut(_last_gps_fail_us, (uint64_t)5e6); + bool gps_checks_failing = isTimedOut(_last_gps_pass_us, (uint64_t)5e6); if ((_params.fusion_mode & MASK_USE_GPS) && !_control_status.flags.gps) { if (_control_status.flags.tilt_align && _NED_origin_initialised && gps_checks_passing) { // If the heading is not aligned, reset the yaw and magnetic field states @@ -649,13 +650,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_hor_pos_fuse) > _params.reset_timeout_max) - && ((_time_last_imu - _time_last_delpos_fuse) > _params.reset_timeout_max) - && ((_time_last_imu - _time_last_hor_vel_fuse) > _params.reset_timeout_max) - && ((_time_last_imu - _time_last_of_fuse) > _params.reset_timeout_max); + bool do_reset = isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max) + && isTimedOut(_time_last_delpos_fuse, _params.reset_timeout_max) + && isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max) + && isTimedOut(_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_hor_pos_fuse) > (2 * _params.reset_timeout_max)); + do_reset = do_reset || isTimedOut(_time_last_hor_pos_fuse, 2 * _params.reset_timeout_max); if (do_reset) { // use GPS velocity data to check and correct yaw angle if a FW vehicle @@ -772,17 +773,17 @@ void Ekf::controlHeightSensorTimeouts() // declare a bad vertical acceleration measurement and make the declaration persist // for a minimum of 10 seconds if (_bad_vert_accel_detected) { - _bad_vert_accel_detected = (_time_last_imu - _time_bad_vert_accel < BADACC_PROBATION); + _bad_vert_accel_detected = isRecent(_time_bad_vert_accel, BADACC_PROBATION); } else { _bad_vert_accel_detected = bad_vert_accel; } // check if height is continuously failing because of accel errors - bool continuous_bad_accel_hgt = ((_time_last_imu - _time_good_vert_accel) > (unsigned)_params.bad_acc_reset_delay_us); + bool continuous_bad_accel_hgt = isTimedOut(_time_good_vert_accel, (uint64_t)_params.bad_acc_reset_delay_us); // check if height has been inertial deadreckoning for too long - bool hgt_fusion_timeout = ((_time_last_imu - _time_last_hgt_fuse) > (uint64_t)5e6); + bool hgt_fusion_timeout = isTimedOut(_time_last_hgt_fuse, (uint64_t)5e6); if (hgt_fusion_timeout || continuous_bad_accel_hgt) { @@ -795,10 +796,10 @@ void Ekf::controlHeightSensorTimeouts() const bool gps_hgt_accurate = (gps_init.vacc < _params.req_vacc); const baroSample &baro_init = _baro_buffer.get_newest(); - const bool baro_data_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL); + const bool baro_data_available = isRecent(baro_init.time_us, 2 * BARO_MAX_INTERVAL); // check for inertial sensing errors in the last 10 seconds - const bool prev_bad_vert_accel = (_time_last_imu - _time_bad_vert_accel < BADACC_PROBATION); + const bool prev_bad_vert_accel = isRecent(_time_bad_vert_accel, BADACC_PROBATION); // reset to GPS if adequate GPS data is available and the timeout cannot be blamed on IMU data const bool reset_to_gps = !_gps_hgt_intermittent && @@ -833,7 +834,7 @@ void Ekf::controlHeightSensorTimeouts() // check the baro height source for consistency and freshness const baroSample &baro_init = _baro_buffer.get_newest(); - const bool baro_data_fresh = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL); + const bool baro_data_fresh = isRecent(baro_init.time_us, 2 * BARO_MAX_INTERVAL); const float baro_innov = _state.pos(2) - (_hgt_sensor_offset - baro_init.hgt + _baro_hgt_offset); const bool baro_data_consistent = fabsf(baro_innov) < (sq(_params.baro_noise) + P(9,9)) * sq(_params.baro_innov_gate); @@ -865,7 +866,7 @@ void Ekf::controlHeightSensorTimeouts() // check if baro data is available const baroSample &baro_init = _baro_buffer.get_newest(); - const bool baro_data_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL); + const bool baro_data_available = isRecent(baro_init.time_us, 2 * BARO_MAX_INTERVAL); if (_rng_hgt_valid) { @@ -890,11 +891,11 @@ void Ekf::controlHeightSensorTimeouts() if (_control_status.flags.ev_hgt) { // check if vision data is available const extVisionSample &ev_init = _ext_vision_buffer.get_newest(); - const bool ev_data_available = ((_time_last_imu - ev_init.time_us) < 2 * EV_MAX_INTERVAL); + const bool ev_data_available = isRecent(ev_init.time_us, 2 * EV_MAX_INTERVAL); // check if baro data is available const baroSample &baro_init = _baro_buffer.get_newest(); - const bool baro_data_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL); + const bool baro_data_available = isRecent(baro_init.time_us, 2 * BARO_MAX_INTERVAL); if (ev_data_available) { setControlEVHeight(); @@ -960,7 +961,7 @@ void Ekf::controlHeightFusion() // Turn off ground effect compensation if it times out if (_control_status.flags.gnd_effect) { - if ((_time_last_imu - _time_last_gnd_effect_on > GNDEFFECT_TIMEOUT)) { + if (isTimedOut(_time_last_gnd_effect_on, GNDEFFECT_TIMEOUT)) { _control_status.flags.gnd_effect = false; } @@ -1056,7 +1057,7 @@ void Ekf::controlHeightFusion() 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 && isRecent(_time_last_ext_vision, 2 * EV_MAX_INTERVAL)) { _fuse_height = true; setControlEVHeight(); resetHeight(); @@ -1090,7 +1091,7 @@ void Ekf::controlHeightFusion() _baro_hgt_offset += local_time_step * math::constrain(offset_rate_correction, -0.1f, 0.1f); } - if ((_time_last_imu - _time_last_hgt_fuse) > 2 * RNG_MAX_INTERVAL && _control_status.flags.rng_hgt + if (isTimedOut(_time_last_hgt_fuse, 2 * RNG_MAX_INTERVAL) && _control_status.flags.rng_hgt && (!_range_data_ready || !_rng_hgt_valid)) { // If we are supposed to be using range finder data as the primary height sensor, have missed or rejected measurements @@ -1218,8 +1219,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 - const bool airspeed_timed_out = ((_time_last_imu - _time_last_arsp_fuse) > (uint64_t)10e6); - const bool sideslip_timed_out = ((_time_last_imu - _time_last_beta_fuse) > (uint64_t)10e6); + const bool airspeed_timed_out = isTimedOut(_time_last_arsp_fuse, (uint64_t)10e6); + const bool sideslip_timed_out = isTimedOut(_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; @@ -1261,8 +1262,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 - const bool sideslip_timed_out = ((_time_last_imu - _time_last_beta_fuse) > (uint64_t)10e6); - const bool airspeed_timed_out = ((_time_last_imu - _time_last_arsp_fuse) > (uint64_t)10e6); + const bool sideslip_timed_out = isTimedOut(_time_last_beta_fuse, (uint64_t)10e6); + const bool airspeed_timed_out = isTimedOut(_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; @@ -1271,7 +1272,7 @@ void Ekf::controlBetaFusion() // Perform synthetic sideslip fusion when in-air and sideslip fuson had been enabled externally in addition to the following criteria: // Sufficient 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 = isTimedOut(_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 @@ -1325,14 +1326,14 @@ void Ekf::controlFakePosFusion() _using_synthetic_position = true; // Fuse synthetic position observations every 200msec - if (((_time_last_imu - _time_last_fake_pos) > (uint64_t)2e5) || _fuse_height) { + if (isTimedOut(_time_last_fake_pos, (uint64_t)2e5) || _fuse_height) { Vector3f fake_pos_obs_var; Vector2f fake_pos_innov_gate; // Reset position and velocity states if we re-commence this aiding method - if ((_time_last_imu - _time_last_fake_pos) > (uint64_t)4e5) { + if (isTimedOut(_time_last_fake_pos, (uint64_t)4e5)) { resetPosition(); resetVelocity(); _fuse_hpos_as_odom = false; diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index 18a75e104a..29db22d16a 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -811,7 +811,7 @@ void Ekf::fixCovarianceErrors(bool force_symmetry) // if we have failed for 7 seconds continuously, reset the accel bias covariances to fix bad conditioning of // the covariance matrix but preserve the variances (diagonals) to allow bias learning to continue - if (_time_last_imu - _time_acc_bias_check > (uint64_t)7e6) { + if (isTimedOut(_time_acc_bias_check, (uint64_t)7e6)) { P.uncorrelateCovariance<3>(13); diff --git a/EKF/ekf.h b/EKF/ekf.h index 9501f61753..ea1609ec12 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -331,7 +331,7 @@ private: bool _tas_data_ready{false}; ///< true when new true airspeed data has fallen behind the fusion time horizon and is available to be fused bool _flow_for_terrain_data_ready{false}; /// same flag as "_flow_data_ready" but used for separate terrain estimator - uint64_t _time_ins_deadreckon_start{0}; ///< amount of time we have been doing inertial only deadreckoning (uSec) + uint64_t _time_last_aiding{0}; ///< amount of time we have been doing inertial only deadreckoning (uSec) bool _using_synthetic_position{false}; ///< true if we are using a synthetic position to constrain drift uint64_t _time_last_hor_pos_fuse{0}; ///< time the last fusion of horizontal position measurements was performed (uSec) @@ -785,6 +785,16 @@ private: // sensor measurement float calculate_synthetic_mag_z_measurement(const Vector3f& mag_meas, const Vector3f& mag_earth_predicted); + bool isTimedOut(uint64_t last_sensor_timestamp, uint64_t timeout_period) const + { + return last_sensor_timestamp + timeout_period < _time_last_imu; + } + + bool isRecent(uint64_t sensor_timestamp, uint64_t acceptance_interval) const + { + return sensor_timestamp + acceptance_interval > _time_last_imu; + } + void stopGpsFusion(); void stopGpsPosFusion(); diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 2e5da68ea3..1dea56d18e 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -246,7 +246,7 @@ void Ekf::resetHeight() // initialize vertical position with newest baro measurement const baroSample &baro_newest = _baro_buffer.get_newest(); - if (_time_last_imu - baro_newest.time_us < 2 * BARO_MAX_INTERVAL) { + if (isRecent(baro_newest.time_us, 2 * BARO_MAX_INTERVAL)) { _state.pos(2) = _hgt_sensor_offset - baro_newest.hgt + _baro_hgt_offset; // the state variance is the same as the observation @@ -260,7 +260,7 @@ void Ekf::resetHeight() } else if (_control_status.flags.gps_hgt) { // initialize vertical position and velocity with newest gps measurement - if (_time_last_imu - gps_newest.time_us < 2 * GPS_MAX_INTERVAL) { + if (isRecent(gps_newest.time_us, 2 * GPS_MAX_INTERVAL)) { _state.pos(2) = _hgt_sensor_offset - gps_newest.hgt + _gps_alt_ref; // the state variance is the same as the observation @@ -296,7 +296,7 @@ void Ekf::resetHeight() } // reset the vertical velocity state - if (_control_status.flags.gps && (_time_last_imu - gps_newest.time_us < 2 * GPS_MAX_INTERVAL)) { + if (_control_status.flags.gps && isRecent(gps_newest.time_us, 2 * GPS_MAX_INTERVAL)) { // If we are using GPS, then use it to reset the vertical velocity _state.vel(2) = gps_newest.vel(2); @@ -1386,22 +1386,23 @@ bool Ekf::global_position_is_valid() void Ekf::update_deadreckoning_status() { bool velPosAiding = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.ev_vel) - && (((_time_last_imu - _time_last_hor_pos_fuse) <= _params.no_aid_timeout_max) - || ((_time_last_imu - _time_last_hor_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); + && (isRecent(_time_last_hor_pos_fuse, _params.no_aid_timeout_max) + || isRecent(_time_last_hor_vel_fuse, _params.no_aid_timeout_max) + || isRecent(_time_last_delpos_fuse, _params.no_aid_timeout_max)); + bool optFlowAiding = _control_status.flags.opt_flow && isRecent(_time_last_of_fuse, _params.no_aid_timeout_max); + bool airDataAiding = _control_status.flags.wind && + isRecent(_time_last_arsp_fuse, _params.no_aid_timeout_max) && + isRecent(_time_last_beta_fuse, _params.no_aid_timeout_max); _is_wind_dead_reckoning = !velPosAiding && !optFlowAiding && airDataAiding; _is_dead_reckoning = !velPosAiding && !optFlowAiding && !airDataAiding; - // record the time we start inertial dead reckoning if (!_is_dead_reckoning) { - _time_ins_deadreckon_start = _time_last_imu - _params.no_aid_timeout_max; + _time_last_aiding = _time_last_imu - _params.no_aid_timeout_max; } // report if we have been deadreckoning for too long - _deadreckon_time_exceeded = ((_time_last_imu - _time_ins_deadreckon_start) > (unsigned)_params.valid_timeout_max); + _deadreckon_time_exceeded = isTimedOut(_time_last_aiding, (uint64_t)_params.valid_timeout_max); } // calculate the inverse rotation matrix from a quaternion rotation @@ -1748,7 +1749,6 @@ float Ekf::kahanSummation(float sum_previous, float input, float &accumulator) c return t; } - void Ekf::stopGpsFusion() { stopGpsPosFusion(); diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index a7439dca63..bbc6ba5a87 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -196,7 +196,6 @@ void EstimatorInterface::setGpsData(const gps_message &gps) gps_sample_new.time_us = gps.time_usec - _params.gps_delay_ms * 1000; gps_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2; - gps_sample_new.time_us = math::max(gps_sample_new.time_us, _imu_sample_delayed.time_us); gps_sample_new.vel = gps.vel_ned; @@ -267,7 +266,6 @@ void EstimatorInterface::setBaroData(const baroSample &baro_sample) baro_sample_new.time_us = 1000 * (_baro_timestamp_sum / _baro_sample_count); baro_sample_new.time_us -= _params.baro_delay_ms * 1000; baro_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2; - baro_sample_new.time_us = math::max(baro_sample_new.time_us, _imu_sample_delayed.time_us); _baro_buffer.push(baro_sample_new); diff --git a/EKF/gps_checks.cpp b/EKF/gps_checks.cpp index e070f1caaa..e198ffdd8c 100644 --- a/EKF/gps_checks.cpp +++ b/EKF/gps_checks.cpp @@ -137,7 +137,7 @@ bool Ekf::gps_is_good(const gps_message &gps) // Calculate time lapsed since last update, limit to prevent numerical errors and calculate a lowpass filter coefficient const float filt_time_const = 10.0f; - const float dt = fminf(fmaxf(float(_time_last_imu - _gps_pos_prev.timestamp) * 1e-6f, 0.001f), filt_time_const); + const float dt = fminf(fmaxf(float(int64_t(_time_last_imu) - int64_t(_gps_pos_prev.timestamp)) * 1e-6f, 0.001f), filt_time_const); const float filter_coef = dt / filt_time_const; // The following checks are only valid when the vehicle is at rest @@ -232,5 +232,5 @@ bool Ekf::gps_is_good(const gps_message &gps) } // continuous period without fail of x seconds required to return a healthy status - return _time_last_imu - _last_gps_fail_us > (uint64_t)_min_gps_health_time_us; + return isTimedOut(_last_gps_fail_us, (uint64_t)_min_gps_health_time_us); } diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp index db4a535fca..f1e89fe23b 100644 --- a/EKF/terrain_estimator.cpp +++ b/EKF/terrain_estimator.cpp @@ -57,7 +57,7 @@ bool Ekf::initHagl() initialized = true; } else if (_rng_hgt_valid - && (_time_last_imu - latest_measurement.time_us) < (uint64_t)2e5 + && isRecent(latest_measurement.time_us, (uint64_t)2e5) && _R_rng_to_earth_2_2 > _params.range_cos_max_tilt) { // if we have a fresh measurement, use it to initialise the terrain estimator _terrain_vpos = _state.pos(2) + latest_measurement.rng * _R_rng_to_earth_2_2; @@ -168,7 +168,7 @@ void Ekf::fuseHagl() } 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 (isTimedOut(_time_last_hagl_fuse, (uint64_t)10E6)) { _terrain_vpos = _state.pos(2) + meas_hagl; _terrain_var = obs_variance; @@ -290,12 +290,12 @@ bool Ekf::isTerrainEstimateValid() const void Ekf::updateTerrainValidity() { // we have been fusing range finder measurements in the last 5 seconds - const bool recent_range_fusion = (_time_last_imu - _time_last_hagl_fuse) < (uint64_t)5e6; + const bool recent_range_fusion = isRecent(_time_last_hagl_fuse, (uint64_t)5e6); // we have been fusing optical flow measurements for terrain estimation within the last 5 seconds // this can only be the case if the main filter does not fuse optical flow - const bool recent_flow_for_terrain_fusion = ((_time_last_imu - _time_last_of_fuse) < (uint64_t)5e6) - && !_control_status.flags.opt_flow; + const bool recent_flow_for_terrain_fusion = isRecent(_time_last_of_fuse, (uint64_t)5e6) + && !_control_status.flags.opt_flow; _hagl_valid = (_terrain_initialised && (recent_range_fusion || recent_flow_for_terrain_fusion)); } diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 783da7873f..9d41a3aa0a 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -44,6 +44,7 @@ set(SRCS test_AlphaFilter.cpp test_EKF_fusionLogic.cpp test_EKF_initialization.cpp + test_EKF_gps.cpp test_EKF_externalVision.cpp test_EKF_airspeed.cpp ) diff --git a/test/sensor_simulator/ekf_wrapper.cpp b/test/sensor_simulator/ekf_wrapper.cpp index b95bb842be..d4a1a28760 100644 --- a/test/sensor_simulator/ekf_wrapper.cpp +++ b/test/sensor_simulator/ekf_wrapper.cpp @@ -10,6 +10,54 @@ EkfWrapper::~EkfWrapper() { } +void EkfWrapper::setBaroHeight() +{ + _ekf_params->vdist_sensor_type = VDIST_SENSOR_BARO; +} + +bool EkfWrapper::isIntendingBaroHeightFusion() const +{ + filter_control_status_u control_status; + _ekf->get_control_mode(&control_status.value); + return control_status.flags.baro_hgt; +} + +void EkfWrapper::setGpsHeight() +{ + _ekf_params->vdist_sensor_type = VDIST_SENSOR_GPS; +} + +bool EkfWrapper::isIntendingGpsHeightFusion() const +{ + filter_control_status_u control_status; + _ekf->get_control_mode(&control_status.value); + return control_status.flags.gps_hgt; +} + +void EkfWrapper::setRangeHeight() +{ + _ekf_params->vdist_sensor_type = VDIST_SENSOR_RANGE; +} + +bool EkfWrapper::isIntendingRangeHeightFusion() const +{ + filter_control_status_u control_status; + _ekf->get_control_mode(&control_status.value); + return control_status.flags.rng_hgt; +} + +void EkfWrapper::setVisionHeight() +{ + _ekf_params->vdist_sensor_type = VDIST_SENSOR_EV; +} + +bool EkfWrapper::isIntendingVisionHeightFusion() const +{ + filter_control_status_u control_status; + _ekf->get_control_mode(&control_status.value); + return control_status.flags.ev_hgt; +} + void EkfWrapper::enableGpsFusion() { _ekf_params->fusion_mode |= MASK_USE_GPS; diff --git a/test/sensor_simulator/ekf_wrapper.h b/test/sensor_simulator/ekf_wrapper.h index 5a980c7919..764a00c8aa 100644 --- a/test/sensor_simulator/ekf_wrapper.h +++ b/test/sensor_simulator/ekf_wrapper.h @@ -47,6 +47,19 @@ public: EkfWrapper(std::shared_ptr ekf); ~EkfWrapper(); + + void setBaroHeight(); + bool isIntendingBaroHeightFusion() const; + + void setGpsHeight(); + bool isIntendingGpsHeightFusion() const; + + void setRangeHeight(); + bool isIntendingRangeHeightFusion() const; + + void setVisionHeight(); + bool isIntendingVisionHeightFusion() const; + void enableGpsFusion(); void disableGpsFusion(); bool isIntendingGpsFusion() const; diff --git a/test/sensor_simulator/gps.cpp b/test/sensor_simulator/gps.cpp index bf18a4b4dd..ca05d30247 100644 --- a/test/sensor_simulator/gps.cpp +++ b/test/sensor_simulator/gps.cpp @@ -44,6 +44,21 @@ void Gps::setVelocity(const Vector3f& vel) _gps_data.vel_ned = vel; } +void Gps::setFixType(int n) +{ + _gps_data.fix_type = n; +} + +void Gps::setNumberOfSatellites(int n) +{ + _gps_data.nsats = n; +} + +void Gps::setPdop(float pdop) +{ + _gps_data.pdop = pdop; +} + void Gps::stepHeightByMeters(float hgt_change) { _gps_data.alt += hgt_change * 1e3f; diff --git a/test/sensor_simulator/gps.h b/test/sensor_simulator/gps.h index 931846e555..021e439acc 100644 --- a/test/sensor_simulator/gps.h +++ b/test/sensor_simulator/gps.h @@ -57,6 +57,9 @@ public: void setLatitude(int32_t lat); void setLongitude(int32_t lon); void setVelocity(const Vector3f& vel); + void setFixType(int n); + void setNumberOfSatellites(int n); + void setPdop(float pdop); gps_message getDefaultGpsData(); diff --git a/test/sensor_simulator/sensor_simulator.h b/test/sensor_simulator/sensor_simulator.h index 4cb0c3d191..fee44cc41d 100644 --- a/test/sensor_simulator/sensor_simulator.h +++ b/test/sensor_simulator/sensor_simulator.h @@ -78,6 +78,9 @@ public: void runSeconds(float duration_seconds); void runMicroseconds(uint32_t duration); + void startBaro(){ _baro.start(); } + void stopBaro(){ _baro.stop(); } + void startGps(){ _gps.start(); } void stopGps(){ _gps.stop(); } diff --git a/test/test_EKF_fusionLogic.cpp b/test/test_EKF_fusionLogic.cpp index 11c1783c51..f9c0554364 100644 --- a/test/test_EKF_fusionLogic.cpp +++ b/test/test_EKF_fusionLogic.cpp @@ -68,6 +68,22 @@ class EkfFusionLogicTest : public ::testing::Test { }; +TEST_F(EkfFusionLogicTest, doNoFusion) +{ + // GIVEN: a tilt and heading aligned filter + // WHEN: having no aiding source EKF should not have a valid position estimate + + // TODO: for the first 5 second it still has some valid local position + // that needs to change + EXPECT_TRUE(_ekf->local_position_is_valid()); + + _sensor_simulator.runSeconds(4); + + // THEN: Local and global position should not be valid + EXPECT_FALSE(_ekf->local_position_is_valid()); + EXPECT_FALSE(_ekf->global_position_is_valid()); +} + TEST_F(EkfFusionLogicTest, doGpsFusion) { // GIVEN: a tilt and heading aligned filter @@ -153,7 +169,7 @@ TEST_F(EkfFusionLogicTest, doFlowFusion) _sensor_simulator.startFlow(); _sensor_simulator.runSeconds(4); - // THEN: EKF should intend to fuse flow measurements + // THEN: EKF should not intend to fuse flow measurements EXPECT_FALSE(_ekf_wrapper.isIntendingFlowFusion()); // THEN: Local and global position should not be valid EXPECT_FALSE(_ekf->local_position_is_valid()); @@ -171,7 +187,7 @@ TEST_F(EkfFusionLogicTest, doFlowFusion) EXPECT_FALSE(_ekf->local_position_is_valid()); EXPECT_FALSE(_ekf->global_position_is_valid()); - // WHEN: Flow data is send and we enable flow fusion + // WHEN: Flow data is sent and we enable flow fusion _sensor_simulator.startFlow(); _ekf_wrapper.enableFlowFusion(); _sensor_simulator.runSeconds(10); @@ -181,4 +197,172 @@ TEST_F(EkfFusionLogicTest, doFlowFusion) // THEN: Local and global position should be valid EXPECT_TRUE(_ekf->local_position_is_valid()); EXPECT_FALSE(_ekf->global_position_is_valid()); + + // WHEN: Stop sending flow data + _sensor_simulator.stopFlow(); + _sensor_simulator.runSeconds(10); + + // THEN: EKF should not intend to fuse flow measurements + EXPECT_TRUE(_ekf_wrapper.isIntendingFlowFusion()); // TODO: change to false + // THEN: Local and global position should not be valid + EXPECT_FALSE(_ekf->local_position_is_valid()); + EXPECT_FALSE(_ekf->global_position_is_valid()); +} + +TEST_F(EkfFusionLogicTest, doVisionPositionFusion) +{ + // WHEN: allow vision position to be fused and we send vision data + _ekf_wrapper.enableExternalVisionPositionFusion(); + _sensor_simulator.startExternalVision(); + _sensor_simulator.runSeconds(4); + + // THEN: EKF should intend to fuse vision position estimate + // and we have a valid local position estimate + EXPECT_TRUE(_ekf_wrapper.isIntendingExternalVisionPositionFusion()); + EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionVelocityFusion()); + EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionHeadingFusion()); + EXPECT_TRUE(_ekf->local_position_is_valid()); + EXPECT_FALSE(_ekf->global_position_is_valid()); + + // WHEN: stop sending vision data + _sensor_simulator.stopExternalVision(); + _sensor_simulator.runSeconds(7); + + // THEN: EKF should stop to intend to fuse vision position estimate + // and EKF should not have a valid local position estimate anymore + EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionPositionFusion()); + EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionVelocityFusion()); + EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionHeadingFusion()); + EXPECT_FALSE(_ekf->local_position_is_valid()); + EXPECT_FALSE(_ekf->global_position_is_valid()); +} + +TEST_F(EkfFusionLogicTest, doVisionVelocityFusion) +{ + // WHEN: allow vision position to be fused and we send vision data + _ekf_wrapper.enableExternalVisionVelocityFusion(); + _sensor_simulator.startExternalVision(); + _sensor_simulator.runSeconds(4); + + // THEN: EKF should intend to fuse vision position estimate + // and we have a valid local position estimate + EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionPositionFusion()); + EXPECT_TRUE(_ekf_wrapper.isIntendingExternalVisionVelocityFusion()); + EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionHeadingFusion()); + EXPECT_TRUE(_ekf->local_position_is_valid()); + EXPECT_FALSE(_ekf->global_position_is_valid()); + + // WHEN: stop sending vision data + _sensor_simulator.stopExternalVision(); + _sensor_simulator.runSeconds(7); + + // THEN: EKF should stop to intend to fuse vision position estimate + // and EKF should not have a valid local position estimate anymore + EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionPositionFusion()); + EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionVelocityFusion()); + EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionHeadingFusion()); + EXPECT_FALSE(_ekf->local_position_is_valid()); + EXPECT_FALSE(_ekf->global_position_is_valid()); +} + +TEST_F(EkfFusionLogicTest, doVisionHeadingFusion) +{ + // WHEN: allow vision position to be fused and we send vision data + _ekf_wrapper.enableExternalVisionHeadingFusion(); + _sensor_simulator.startExternalVision(); + _sensor_simulator.runSeconds(4); + + // THEN: EKF should intend to fuse vision heading estimates + // and we should not have a valid local position estimate + EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionPositionFusion()); + EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionVelocityFusion()); + EXPECT_TRUE(_ekf_wrapper.isIntendingExternalVisionHeadingFusion()); + EXPECT_FALSE(_ekf->local_position_is_valid()); + EXPECT_FALSE(_ekf->global_position_is_valid()); + + // WHEN: stop sending vision data + _sensor_simulator.stopExternalVision(); + _sensor_simulator.runSeconds(6); + + // THEN: EKF should stop to intend to fuse vision position estimate + // and EKF should not have a valid local position estimate anymore + EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionPositionFusion()); + EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionVelocityFusion()); + EXPECT_TRUE(_ekf_wrapper.isIntendingExternalVisionHeadingFusion()); + // TODO: it is still intending to fuse ev_yaw. There should be some fallback to mag if possible + EXPECT_FALSE(_ekf->local_position_is_valid()); + EXPECT_FALSE(_ekf->global_position_is_valid()); +} + +TEST_F(EkfFusionLogicTest, doBaroHeightFusion) +{ + // GIVEN: EKF that receives baro data + + // THEN: EKF should intend to fuse baro by default + EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion()); + + // WHEN: stop sending baro data + _sensor_simulator.stopBaro(); + _sensor_simulator.runSeconds(6); + + // THEN: EKF should stop to intend to use baro hgt + // TODO: We have no fall back in balce + EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion()); // TODO: Needs to change +} + + +TEST_F(EkfFusionLogicTest, doGpsHeightFusion) +{ + // WHEN: commanding GPS height and sending GPS data + _ekf_wrapper.setGpsHeight(); + _sensor_simulator.startGps(); + _sensor_simulator.runSeconds(11); + + // THEN: EKF should intend to fuse gps height + EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeightFusion()); + + // WHEN: stop sending gps data + _sensor_simulator.stopGps(); + _sensor_simulator.runSeconds(11); // TODO: We have to wait way too long + + // THEN: EKF should stop to intend to use gps height + EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeightFusion()); +} + + +TEST_F(EkfFusionLogicTest, doRangeHeightFusion) +{ + // WHEN: commanding range height and sending range data + _ekf_wrapper.setRangeHeight(); + _sensor_simulator.startRangeFinder(); + _sensor_simulator.runSeconds(2); + + // THEN: EKF should intend to fuse range height + EXPECT_TRUE(_ekf_wrapper.isIntendingRangeHeightFusion()); + + // WHEN: stop sending range data + _sensor_simulator.stopRangeFinder(); + _sensor_simulator.runSeconds(2); + + // THEN: EKF should stop to intend to use range height + EXPECT_FALSE(_ekf_wrapper.isIntendingRangeHeightFusion()); +} + +TEST_F(EkfFusionLogicTest, doVisionHeightFusion) +{ + // WHEN: commanding vision height and sending vision data + _ekf_wrapper.setVisionHeight(); + _sensor_simulator.startExternalVision(); + _sensor_simulator.runSeconds(2); + + // THEN: EKF should intend to fuse vision height + EXPECT_TRUE(_ekf_wrapper.isIntendingVisionHeightFusion()); + + // WHEN: stop sending vision data + _sensor_simulator.stopExternalVision(); + _sensor_simulator.runSeconds(12); + + // THEN: EKF should stop to intend to use vision height + // TODO: This is not happening + EXPECT_TRUE(_ekf_wrapper.isIntendingVisionHeightFusion()); // TODO: Needs to change } diff --git a/test/test_EKF_gps.cpp b/test/test_EKF_gps.cpp new file mode 100644 index 0000000000..3675e3d258 --- /dev/null +++ b/test/test_EKF_gps.cpp @@ -0,0 +1,85 @@ +/**************************************************************************** + * + * Copyright (c) 2019 ECL Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * Test the gps fusion + * @author Kamil Ritz + */ + +#include +#include "EKF/ekf.h" +#include "sensor_simulator/sensor_simulator.h" +#include "sensor_simulator/ekf_wrapper.h" + + +class EkfGpsTest : public ::testing::Test { + public: + + EkfGpsTest(): ::testing::Test(), + _ekf{std::make_shared()}, + _sensor_simulator(_ekf), + _ekf_wrapper(_ekf) {}; + + std::shared_ptr _ekf; + SensorSimulator _sensor_simulator; + EkfWrapper _ekf_wrapper; + + // Setup the Ekf with synthetic measurements + void SetUp() override + { + _ekf->init(0); + _sensor_simulator.runSeconds(2); + _ekf_wrapper.enableGpsFusion(); + _sensor_simulator.startGps(); + _sensor_simulator.runSeconds(11); + } + + // Use this method to clean up any memory, network etc. after each test + void TearDown() override + { + } +}; + +TEST_F(EkfGpsTest, gpsTimeout) +{ + // GIVEN:EKF that fuses GPS + + // WHEN: setting the PDOP to high + _sensor_simulator._gps.setNumberOfSatellites(3); + + // THEN: EKF should stop fusing GPS + _sensor_simulator.runSeconds(20); + + // TODO: this is not happening as expected + EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion()); +}