Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-02 14:08:45 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF : Prevent bad user parameter causing incorrect GPS fusion

Browse Source
This commit is contained in:
Paul Riseborough 2014-03-13 20:10:17 +11:00 committed by Andrew Tridgell
parent a24bfc1b8a
commit d0828d9c15
1 changed files with 12 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -2888,7 +2888,17 @@ void NavEKF::readGpsData()
// read the NED velocity from the GPS
velNED[0] = _ahrs->get_gps()->velocity_north();
velNED[1] = _ahrs->get_gps()->velocity_east();
velNED[2] = _ahrs->get_gps()->velocity_down();
// Check if GPS can output vertical velocity and set value and GPS fusion mode accordingly
if (_ahrs->get_gps()->have_vertical_velocity()) {
velNED[2] = _ahrs->get_gps()->velocity_down();
} else {
velNED[2] = 0;
// vertical velocity should not be fused
if (_fusionModeGPS == 0) {
_fusionModeGPS = 1;
}
}
// read latitutde and longitude from GPS and convert to NE position
struct Location gpsloc;
@ -2897,6 +2907,7 @@ void NavEKF::readGpsData()
Vector2f posdiff = location_diff(_ahrs->get_home(), gpsloc);
posNE[0] = posdiff.x;
posNE[1] = posdiff.y;
}
}