Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF : Position and Velocity reset bugfix

This commit is contained in:
Paul Riseborough 2014-02-16 20:42:55 +11:00 committed by Andrew Tridgell
parent b22dc706b2
commit 275ef86f86
1 changed files with 29 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -284,24 +284,39 @@ void NavEKF::SetStaticMode(bool setting) {
void NavEKF::ResetPosition(void) void NavEKF::ResetPosition(void)
{ {
// read the GPS if (staticMode) {
readGpsData(); states[7] = 0;
// write to state vector states[8] = 0;
states[7] = posNE[0]; } else if (_ahrs->get_gps()->status() >= GPS::GPS_OK_FIX_3D) {
states[8] = posNE[1]; // read the GPS
readGpsData();
// write to state vector
states[7] = posNE[0];
states[8] = posNE[1];
}
} }
void NavEKF::ResetVelocity(void) void NavEKF::ResetVelocity(void)
{ {
// read the GPS if (staticMode) {
readGpsData(); if (_fusionModeGPS <= 1) {
// write to state vector states[4] = 0;
if (_fusionModeGPS <= 1) { states[5] = 0;
states[4] = velNED[0]; }
states[5] = velNED[1]; if (_fusionModeGPS <= 0) {
} states[6] = 0;
if (_fusionModeGPS <= 0) { }
states[6] = velNED[2]; } else if (_ahrs->get_gps()->status() >= GPS::GPS_OK_FIX_3D) {
// read the GPS
readGpsData();
// write to state vector
if (_fusionModeGPS <= 1) {
states[4] = velNED[0];
states[5] = velNED[1];
}
if (_fusionModeGPS <= 0) {
states[6] = velNED[2];
}
} }
} }