Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 18:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF : Position and Velocity reset bugfix

Browse Source
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)
{
// read the GPS
readGpsData();
// write to state vector
states[7] = posNE[0];
states[8] = posNE[1];
if (staticMode) {
states[7] = 0;
states[8] = 0;
} else if (_ahrs->get_gps()->status() >= GPS::GPS_OK_FIX_3D) {
// read the GPS
readGpsData();
// write to state vector
states[7] = posNE[0];
states[8] = posNE[1];
}
}
void NavEKF::ResetVelocity(void)
{
// 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];
if (staticMode) {
if (_fusionModeGPS <= 1) {
states[4] = 0;
states[5] = 0;
}
if (_fusionModeGPS <= 0) {
states[6] = 0;
}
} 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];
}
}
}