diff --git a/EKF/ekf.h b/EKF/ekf.h index 7b17449979..d94e4f2b09 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -167,6 +167,16 @@ public: */ void get_imu_vibe_metrics(float vibe[3]); + /* + First argument returns GPS drift metrics in the following array locations + 0 : Horizontal position drift rate (m/s) + 1 : Vertical position drift rate (m/s) + 2 : Filtered horizontal velocity (m/s) + Second argument returns true when IMU movement is blocking the drift calculation + Function returns true if the metrics have been updated and not returned previously by this function + */ + bool get_gps_drift_metrics(float drift[3], bool *blocked); + // return true if the global position estimate is valid bool global_position_is_valid(); diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 778661c8c0..de92e5ee9a 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -987,6 +987,25 @@ void Ekf::get_imu_vibe_metrics(float vibe[3]) memcpy(vibe, _vibe_metrics, 3 * sizeof(float)); } +/* + First argument returns GPS drift metrics in the following array locations + 0 : Horizontal position drift rate (m/s) + 1 : Vertical position drift rate (m/s) + 2 : Filtered horizontal velocity (m/s) + Second argument returns true when IMU movement is blocking the drift calculation + Function returns true if the metrics have been updated and not returned previously by this function +*/ +bool Ekf::get_gps_drift_metrics(float drift[3], bool *blocked) +{ + memcpy(drift, _gps_drift_metrics, 3 * sizeof(float)); + *blocked = !_vehicle_at_rest; + if (_gps_drift_updated) { + _gps_drift_updated = false; + return true; + } + return false; +} + // get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) { diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 5d2fc982b9..d240bdff6f 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -140,6 +140,16 @@ public: */ virtual void get_imu_vibe_metrics(float vibe[3]) = 0; + /* + First argument returns GPS drift metrics in the following array locations + 0 : Horizontal position drift rate (m/s) + 1 : Vertical position drift rate (m/s) + 2 : Filtered horizontal velocity (m/s) + Second argument returns true when IMU movement is blocking the drift calculation + Function returns true if the metrics have been updated and not returned previously by this function + */ + virtual bool get_gps_drift_metrics(float drift[3], bool *blocked) = 0; + // get the ekf WGS-84 origin position and height and the system time it was last set // return true if the origin is valid virtual bool get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt) = 0; @@ -480,8 +490,13 @@ protected: // [0] Level of coning vibration in the IMU delta angles (rad^2) // [1] high frequency vibraton level in the IMU delta angle data (rad) // [2] high frequency vibration level in the IMU delta velocity data (m/s) + float _gps_drift_metrics[3] {}; // Array containing GPS drift metrics + // [0] Horizontal position drift rate (m/s) + // [1] Vertical position drift rate (m/s) + // [2] Filtered horizontal velocity (m/s) bool _vehicle_at_rest{false}; // true when the vehicle is at rest uint64_t _time_last_move_detect_us{0}; // timestamp of last movement detection event in microseconds + bool _gps_drift_updated{false}; // true when _gps_drift_metrics has been updated and is ready for retrieval // data buffer instances RingBuffer _imu_buffer; diff --git a/EKF/gps_checks.cpp b/EKF/gps_checks.cpp index 9f91c3c9e0..871b4d1efa 100644 --- a/EKF/gps_checks.cpp +++ b/EKF/gps_checks.cpp @@ -163,8 +163,8 @@ bool Ekf::gps_is_good(struct gps_message *gps) _gpsDriftVelE = velE * filter_coef + _gpsDriftVelE * (1.0f - filter_coef); // Calculate the horizontal drift speed and fail if too high - float drift_speed = sqrtf(_gpsDriftVelN * _gpsDriftVelN + _gpsDriftVelE * _gpsDriftVelE); - _gps_check_fail_status.flags.hdrift = (drift_speed > _params.req_hdrift); + _gps_drift_metrics[0] = sqrtf(_gpsDriftVelN * _gpsDriftVelN + _gpsDriftVelE * _gpsDriftVelE); + _gps_check_fail_status.flags.hdrift = (_gps_drift_metrics[0] > _params.req_hdrift); // Calculate the vertical drift velocity and limit to 10x the threshold float vz_drift_limit = 10.0f * _params.req_vdrift; @@ -175,7 +175,8 @@ bool Ekf::gps_is_good(struct gps_message *gps) _gps_drift_velD = velD * filter_coef + _gps_drift_velD * (1.0f - filter_coef); // Fail if the vertical drift speed is too high - _gps_check_fail_status.flags.vdrift = (fabsf(_gps_drift_velD) > _params.req_vdrift); + _gps_drift_metrics[1] = fabsf(_gps_drift_velD); + _gps_check_fail_status.flags.vdrift = (_gps_drift_metrics[1] > _params.req_vdrift); // Check the magnitude of the filtered horizontal GPS velocity float vxy_drift_limit = 10.0f * _params.req_hdrift; @@ -183,8 +184,10 @@ bool Ekf::gps_is_good(struct gps_message *gps) float gps_velE = fminf(fmaxf(gps->vel_ned[1], -vxy_drift_limit), vxy_drift_limit); _gps_velN_filt = gps_velN * filter_coef + _gps_velN_filt * (1.0f - filter_coef); _gps_velE_filt = gps_velE * filter_coef + _gps_velE_filt * (1.0f - filter_coef); - float horiz_speed = sqrtf(_gps_velN_filt * _gps_velN_filt + _gps_velE_filt * _gps_velE_filt); - _gps_check_fail_status.flags.hspeed = (horiz_speed > _params.req_hdrift); + _gps_drift_metrics[2] = sqrtf(_gps_velN_filt * _gps_velN_filt + _gps_velE_filt * _gps_velE_filt); + _gps_check_fail_status.flags.hspeed = (_gps_drift_metrics[2] > _params.req_hdrift); + + _gps_drift_updated = true; } else if (_control_status.flags.in_air) { // These checks are always declared as passed when flying @@ -192,6 +195,11 @@ bool Ekf::gps_is_good(struct gps_message *gps) _gps_check_fail_status.flags.hdrift = false; _gps_check_fail_status.flags.vdrift = false; _gps_check_fail_status.flags.hspeed = false; + _gps_drift_updated = false; + + } else { + // This is the case where the vehicle is on ground and IMU movement is blocking the drift calculation + _gps_drift_updated = true; }