fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 01:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Add GPS height option and improve height recovery

Browse Source
This commit is contained in:
Paul Riseborough
2016-03-16 10:40:51 +11:00
parent 63b0cf4360
commit 6b2e2dba90
10 changed files with 261 additions and 112 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/gps_checks.cpp
+11 -1
View File
@@ -59,7 +59,7 @@ bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps)
if (!_NED_origin_initialised) {
// we have good GPS data so can now set the origin's WGS-84 position
if (gps_is_good(gps) && !_NED_origin_initialised) {
printf("gps is good - setting EKF origin\n");
printf("EKF gps is good - setting origin\n");
// Set the origin's WGS-84 position to the last gps fix
double lat = gps->lat / 1.0e7;
double lon = gps->lon / 1.0e7;
@@ -81,6 +81,16 @@ bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps)
// save the horizontal and vertical position uncertainty of the origin
_gps_origin_eph = gps->eph;
_gps_origin_epv = gps->epv;
// if the user has selected GPS as the primary height source, switch across to using it
if (_primary_hgt_source == VDIST_SENSOR_GPS) {
printf("EKF switching to GPS height\n");
_control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = true;
_control_status.flags.rng_hgt = false;
// zero the sensor offset
_hgt_sensor_offset = 0.0f;
}
}
}