dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

EKF: Don't start using GPS for height until all validity checks have passed 

Consolidate intermittent data checks, improve variable and clarify usage.
This commit is contained in:
Paul Riseborough 2019-05-30 08:53:29 +10:00 committed by Paul Riseborough
parent cef2ba5ab9
commit 7612fa40ed
3 changed files with 15 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
EKF/control.cpp
View File

@ -80,12 +80,12 @@ void Ekf::controlFusionModes()
}
// check faultiness (before pop_first_older_than) to see if we can change back to original height sensor
// 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);
const gpsSample &gps_init = _gps_buffer.get_newest();
_gps_hgt_faulty = !((_time_last_imu - gps_init.time_us) < 2 * GPS_MAX_INTERVAL);
_gps_hgt_intermittent = !((_time_last_imu - 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);
@ -115,7 +115,7 @@ void Ekf::controlFusionModes()
Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
_range_sample_delayed.rng += pos_offset_earth(2) / _R_rng_to_earth_2_2;
}
// We don't fuse flow data immediately because we have to wait for the mid integration point to fall behind the fusion time horizon.
// This means we stop looking for new data until the old data has been fused.
if (!_flow_data_ready) {
@ -697,7 +697,7 @@ void Ekf::controlGpsFusion()
_control_status.flags.gps = false;
ECL_WARN("EKF GPS data stopped");
} else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)1e6) && (_control_status.flags.opt_flow || _control_status.flags.ev_pos)) {
// Handle the case where we are fusing another position source along GPS,
// Handle the case where we are fusing another position source along GPS,
// stop waiting for GPS after 1 s of lost signal
_control_status.flags.gps = false;
ECL_WARN("EKF GPS data stopped, using only EV or OF");
@ -754,7 +754,6 @@ void Ekf::controlHeightSensorTimeouts()
if (_control_status.flags.baro_hgt) {
// check if GPS height is available
const gpsSample &gps_init = _gps_buffer.get_newest();
bool gps_hgt_available = ((_time_last_imu - gps_init.time_us) < 2 * GPS_MAX_INTERVAL);
bool gps_hgt_accurate = (gps_init.vacc < _params.req_vacc);
const baroSample &baro_init = _baro_buffer.get_newest();
@ -764,10 +763,10 @@ void Ekf::controlHeightSensorTimeouts()
bool prev_bad_vert_accel = (_time_last_imu - _time_bad_vert_accel < BADACC_PROBATION);
// reset to GPS if adequate GPS data is available and the timeout cannot be blamed on IMU data
bool reset_to_gps = gps_hgt_available && gps_hgt_accurate && !_gps_hgt_faulty && !prev_bad_vert_accel;
bool reset_to_gps = !_gps_hgt_intermittent && gps_hgt_accurate && !prev_bad_vert_accel;
// reset to GPS if GPS data is available and there is no Baro data
reset_to_gps = reset_to_gps || (gps_hgt_available && !baro_hgt_available);
reset_to_gps = reset_to_gps || (!_gps_hgt_intermittent && !baro_hgt_available);
// reset to Baro if we are not doing a GPS reset and baro data is available
bool reset_to_baro = !reset_to_gps && baro_hgt_available;
@ -776,9 +775,6 @@ void Ekf::controlHeightSensorTimeouts()
// set height sensor health
_baro_hgt_faulty = true;
// declare the GPS height healthy
_gps_hgt_faulty = false;
// reset the height mode
setControlGPSHeight();
@ -809,7 +805,6 @@ void Ekf::controlHeightSensorTimeouts()
if (_control_status.flags.gps_hgt) {
// check if GPS height is available
const gpsSample &gps_init = _gps_buffer.get_newest();
bool gps_hgt_available = ((_time_last_imu - gps_init.time_us) < 2 * GPS_MAX_INTERVAL);
bool gps_hgt_accurate = (gps_init.vacc < _params.req_vacc);
// check the baro height source for consistency and freshness
@ -822,14 +817,13 @@ void Ekf::controlHeightSensorTimeouts()
bool reset_to_baro = baro_data_consistent && baro_data_fresh && !_baro_hgt_faulty && !gps_hgt_accurate;
// if GPS height is unavailable and baro data is available, reset height to baro
reset_to_baro = reset_to_baro || (!gps_hgt_available && baro_data_fresh);
reset_to_baro = reset_to_baro || (_gps_hgt_intermittent && baro_data_fresh);
// if we cannot switch to baro and GPS data is available, reset height to GPS
bool reset_to_gps = !reset_to_baro && gps_hgt_available;
bool reset_to_gps = !reset_to_baro && !_gps_hgt_intermittent;
if (reset_to_baro) {
// set height sensor health
_gps_hgt_faulty = true;
_baro_hgt_faulty = false;
// reset the height mode
@ -840,9 +834,6 @@ void Ekf::controlHeightSensorTimeouts()
ECL_WARN("EKF gps hgt timeout - reset to baro");
} else if (reset_to_gps) {
// set height sensor health
_gps_hgt_faulty = false;
// reset the height mode
setControlGPSHeight();
@ -999,7 +990,7 @@ void Ekf::controlHeightFusion()
}
}
} else if (_control_status.flags.gps_hgt && _gps_data_ready && !_gps_hgt_faulty) {
} else if (_control_status.flags.gps_hgt && _gps_data_ready && !_gps_hgt_intermittent) {
// switch to gps if there was a reset to gps
_fuse_height = true;
@ -1063,7 +1054,7 @@ void Ekf::controlHeightFusion()
}
}
} else if (!_range_aid_mode_selected && _gps_data_ready && !_gps_hgt_faulty) {
} else if (!_range_aid_mode_selected && _gps_data_ready && !_gps_hgt_intermittent && _gps_checks_passed) {
setControlGPSHeight();
_fuse_height = true;

5
EKF/ekf.h
View File

@ -415,6 +415,7 @@ private:
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
bool _gps_checks_passed{false}; ///> true when all active GPS checks have passed
// 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)
@ -463,9 +464,9 @@ private:
float _dt_last_range_update_filt_us{0.0f}; ///< filtered value of the delta time elapsed since the last range measurement came into the filter (uSec)
bool _hagl_valid{false}; ///< true when the height above ground estimate is valid
// height sensor fault status
// height sensor status
bool _baro_hgt_faulty{false}; ///< true if valid baro data is unavailable for use
bool _gps_hgt_faulty{false}; ///< true if valid gps height data is unavailable for use
bool _gps_hgt_intermittent{false}; ///< true if gps height into the buffer is intermittent
bool _rng_hgt_faulty{false}; ///< true if valid range finder height data is unavailable for use
int _primary_hgt_source{VDIST_SENSOR_BARO}; ///< specifies primary source of height data

4
EKF/gps_checks.cpp
View File

@ -59,8 +59,8 @@
bool Ekf::collect_gps(const gps_message &gps)
{
// Run GPS checks always
bool gps_checks_pass = gps_is_good(gps);
if (!_NED_origin_initialised && gps_checks_pass) {
_gps_checks_passed = gps_is_good(gps);
if (!_NED_origin_initialised && _gps_checks_passed) {
// If we have good GPS data set the origin's WGS-84 position to the last gps fix
double lat = gps.lat / 1.0e7;
double lon = gps.lon / 1.0e7;