forked from Archive/PX4-Autopilot
Robustify timestamp checks (#729)
* Robustify timestamp checks * Remove lowerbound on sensor timestamp * Add tests for fusion timeouts * Inline and const time check functions * Rename dead_reckoning time variable
This commit is contained in:
kritz
Paul Riseborough
parent
f20726d47f
commit
ee859e092a
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
||||
|
|
|
|||
12
EKF/ekf.h
12
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();
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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));
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
)
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -47,6 +47,19 @@ public:
|
|||
EkfWrapper(std::shared_ptr<Ekf> 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;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
|
||||
|
|
|
|||
|
|
@ -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(); }
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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 <ka.ritz@hotmail.com>
|
||||
*/
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#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<Ekf>()},
|
||||
_sensor_simulator(_ekf),
|
||||
_ekf_wrapper(_ekf) {};
|
||||
|
||||
std::shared_ptr<Ekf> _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());
|
||||
}
|
||||
Loading…
Reference in New Issue