Ekf: add set_min_required_gps_health_time() to allow configure minimum GPS health time

This does not change the default of 10 seconds.
This commit is contained in:
Beat Küng
2019-05-15 09:00:24 +01:00
committed by Lorenz Meier
parent 770cd3c5cc
commit 54ac147ae8
2 changed files with 6 additions and 2 deletions
+4
View File
@@ -261,6 +261,9 @@ public:
// use the latest IMU data at the current time horizon.
Quatf calculate_quaternion() const;
// set minimum continuous period without GPS fail required to mark a healthy GPS status
void set_min_required_gps_health_time(uint32_t time_us) { _min_gps_health_time_us = time_us; }
private:
static constexpr uint8_t _k_num_states{24}; ///< number of EKF states
@@ -408,6 +411,7 @@ private:
uint64_t _last_gps_fail_us{0}; ///< last system time in usec that the GPS failed it's checks
uint64_t _last_gps_pass_us{0}; ///< last system time in usec that the GPS passed it's checks
float _gps_error_norm{1.0f}; ///< normalised gps error
uint32_t _min_gps_health_time_us{10000000}; ///< GPS is marked as healthy only after this amount of time
// Variables used to publish the WGS-84 location of the EKF local NED origin
uint64_t _last_gps_origin_time_us{0}; ///< time the origin was last set (uSec)
+2 -2
View File
@@ -242,6 +242,6 @@ bool Ekf::gps_is_good(const gps_message &gps)
_last_gps_pass_us = _time_last_imu;
}
// continuous period without fail of 10 seconds required to return a healthy status
return (_time_last_imu - _last_gps_fail_us > (uint64_t)1e7);
// 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;
}