diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 1857f0aab0..5e3c9f258e 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -48,7 +48,8 @@ Ekf::Ekf(): _fuse_height(false), _fuse_pos(false), _fuse_vel(false), - _mag_fuse_index(0) + _mag_fuse_index(0), + _time_last_fake_gps(0) { _earth_rate_NED.setZero(); _R_prev = matrix::Dcm(); @@ -84,9 +85,9 @@ bool Ekf::update() // measurement updates if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) { - //fuseHeading(); - fuseMag(_mag_fuse_index); - _mag_fuse_index = (_mag_fuse_index + 1) % 3; + fuseHeading(); + //fuseMag(_mag_fuse_index); + //_mag_fuse_index = (_mag_fuse_index + 1) % 3; } if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) { @@ -96,6 +97,9 @@ bool Ekf::update() if (_gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed)) { _fuse_pos = true; _fuse_vel = true; + } else if (_time_last_imu - _time_last_gps > 2000000 && _time_last_imu - _time_last_fake_gps > 70000) { + _fuse_vel = true; + _gps_sample_delayed.vel.setZero(); } @@ -142,14 +146,17 @@ bool Ekf::initialiseFilter(void) roll = -asinf(accel_init(1) / cosf(pitch)); } - matrix::Euler euler_init(roll, pitch, 0.0f); - _state.quat_nominal = Quaternion(euler_init); - magSample mag_init = _mag_buffer.get_newest(); if (mag_init.time_us == 0) { return false; } + float yaw_init = atan2f(mag_init.mag(1), mag_init.mag(0)); + + matrix::Euler euler_init(roll, pitch, yaw_init); + _state.quat_nominal = Quaternion(euler_init); + + matrix::Dcm R_to_earth(euler_init); _state.mag_I = R_to_earth * mag_init.mag; diff --git a/EKF/ekf.h b/EKF/ekf.h index 561010de57..519b83dc9d 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -66,6 +66,8 @@ private: uint8_t _mag_fuse_index; // counter for sequential mag axis fusion + uint64_t _time_last_fake_gps; + Vector3f _earth_rate_NED; matrix::Dcm _R_prev; diff --git a/EKF/estimator_base.cpp b/EKF/estimator_base.cpp index d4cb36ae1c..fe2dd95ee4 100644 --- a/EKF/estimator_base.cpp +++ b/EKF/estimator_base.cpp @@ -69,7 +69,7 @@ void EstimatorBase::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, uint64 _time_last_imu = time_usec; - if (_imu_time_last > 0) { + if (_time_last_imu > 0) { _dt_imu_avg = 0.8f * _dt_imu_avg + 0.2f * dt; } @@ -170,6 +170,8 @@ void EstimatorBase::setGpsData(uint64_t time_usec, struct gps_message *gps) gps_sample_new.time_us -= FILTER_UPDATE_PERRIOD_MS * 1000 / 2; _time_last_gps = gps_sample_new.time_us; + _last_valid_gps_time_us = hrt_absolute_time(); + gps_sample_new.time_us = math::max(gps_sample_new.time_us, _imu_sample_delayed.time_us); @@ -261,8 +263,8 @@ void EstimatorBase::initialiseVariables(uint64_t time_usec) _params.baro_delay_ms = 0; _params.gps_delay_ms = 200; _params.airspeed_delay_ms = 0; - _params.requiredEph = 200; - _params.requiredEpv = 200; + _params.requiredEph = 500; + _params.requiredEpv = 800; _params.gyro_noise = 1e-3f; _params.accel_noise = 1e-1f; _params.gyro_bias_p_noise = 1e-5f; @@ -311,7 +313,7 @@ void EstimatorBase::initialiseVariables(uint64_t time_usec) _gps_speed_valid = false; _mag_healthy = false; - _in_air = true; // XXX get this flag from the application + _in_air = false; // XXX get this flag from the application _time_last_imu = 0; _time_last_gps = 0; @@ -335,20 +337,38 @@ void EstimatorBase::initialiseGPS(struct gps_message *gps) map_projection_init(&_posRef, lat, lon); _gps_alt_ref = gps->alt / 1e3f; _gps_initialised = true; + _last_gps_origin_time_us = hrt_absolute_time(); } } bool EstimatorBase::gps_is_good(struct gps_message *gps) { // go through apm implementation of calcGpsGoodToAlign for fancier checks - if ((gps->fix_type >= 3) && (gps->eph < _params.requiredEph) && (gps->epv < _params.requiredEpv)) { - return true; + // Use a stricter check for initialisation than during flight to avoid complete loss of GPS + if (_gps_initialised) { + if ((gps->fix_type >= 3) && (gps->eph < _params.requiredEph * 2) && (gps->epv < _params.requiredEpv * 2)) { + return true; + } else { + return false; + } } else { - return false; + if ((gps->fix_type >= 3) && (gps->eph < _params.requiredEph) && (gps->epv < _params.requiredEpv)) { + return true; + + } else { + return false; + } } } +bool EstimatorBase::position_is_valid() +{ + // return true if the position estimate is valid + // TOTO implement proper check based on published GPS accuracy, innovaton consistency checks and timeout status + return _gps_initialised && (hrt_absolute_time() - _last_valid_gps_time_us) < 5e6; +} + void EstimatorBase::printStoredIMU() { printf("---------Printing IMU data buffer------------\n"); diff --git a/EKF/estimator_base.h b/EKF/estimator_base.h index 98a9e8a1fa..ee606c1143 100644 --- a/EKF/estimator_base.h +++ b/EKF/estimator_base.h @@ -190,6 +190,8 @@ protected: float _dt_imu_avg; uint64_t _imu_time_last; + uint64_t _last_valid_gps_time_us; + imuSample _imu_sample_delayed; imuSample _imu_down_sampled; Quaternion @@ -206,9 +208,6 @@ protected: outputSample _output_new; imuSample _imu_sample_new; - struct map_projection_reference_s _posRef; - float _gps_alt_ref; - uint64_t _imu_ticks; @@ -266,6 +265,8 @@ public: void printGps(struct gpsSample *data); void printStoredGps(); + bool position_is_valid(); + void copy_quaternion(float *quat) { for (unsigned i = 0; i < 4; i++) { quat[i] = _output_new.quat_nominal(i); @@ -284,4 +285,8 @@ public: void copy_timestamp(uint64_t *time_us) { *time_us = _imu_time_last; } + + uint64_t _last_gps_origin_time_us; + struct map_projection_reference_s _posRef; + float _gps_alt_ref; };