aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF : Enable staticMode to be set externally

Browse Source
This commit is contained in:
Paul Riseborough 2014-01-31 09:39:11 +11:00 committed by Andrew Tridgell
parent 3617c65af7
commit 5c3dea28dc
2 changed files with 16 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -89,21 +89,19 @@ bool NavEKF::healthy(void) const
return true; return true;
} }
void NavEKF::SetStaticMode(bool setting) {
staticMode = !setting;
}
void NavEKF::ResetPosition(void) void NavEKF::ResetPosition(void)
{ {
// read the GPS // read the GPS
readGpsData(); readGpsData();
// read the barometer
readHgtData(); readHgtData();
// write to state vector // write to state vector
states[7] = posNE[0]; states[7] = posNE[0];
states[8] = posNE[1]; states[8] = posNE[1];
states[9] = - _baro.get_altitude(); states[9] = -hgtMea;
// set static mode to force positon and velocity measurements to zero
staticMode = true;
} }
void NavEKF::InitialiseFilter(void) void NavEKF::InitialiseFilter(void)
@ -1073,8 +1071,6 @@ void NavEKF::FuseVelPosNED()
// zero observations if in static mode (used for pre-arm and bench testing) // zero observations if in static mode (used for pre-arm and bench testing)
if (staticMode) { if (staticMode) {
for (uint8_t i=0; i<=5; i++) observation[i] = 0.0f; for (uint8_t i=0; i<=5; i++) observation[i] = 0.0f;
// cancel static mode (it needs to be set every frame if required)
staticMode = false;
} }
// additional error in GPS velocity caused by manoeuvring // additional error in GPS velocity caused by manoeuvring
@ -1851,12 +1847,12 @@ void NavEKF::CovarianceInit()
P[1][1] = 0.25f*sq(radians(1.0f)); P[1][1] = 0.25f*sq(radians(1.0f));
P[2][2] = 0.25f*sq(radians(1.0f)); P[2][2] = 0.25f*sq(radians(1.0f));
P[3][3] = 0.25f*sq(radians(10.0f)); P[3][3] = 0.25f*sq(radians(10.0f));
P[4][4] = sq(0.7f); P[4][4] = 1.0e-9;
P[5][5] = P[4][4]; P[5][5] = P[4][4];
P[6][6] = sq(0.7f); P[6][6] = P[4][4];
P[7][7] = sq(15.0f); P[7][7] = sq(_gpsHorizPosNoise);
P[8][8] = P[7][7]; P[8][8] = P[7][7];
P[9][9] = sq(5.0f); P[9][9] = sq(_gpsVertPosNoise);
P[10][10] = sq(radians(0.1f * dtIMU)); P[10][10] = sq(radians(0.1f * dtIMU));
P[11][11] = P[10][10]; P[11][11] = P[10][10];
P[12][12] = P[10][10]; P[12][12] = P[10][10];

3
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -77,6 +77,9 @@ public:
// Reset the position and height states // Reset the position and height states
void ResetPosition(void); void ResetPosition(void);
// inhibits position and velocity aittude corrections when set to false
void SetStaticMode(bool setting);
// Update Filter States - this should be called whenever new IMU data is available // Update Filter States - this should be called whenever new IMU data is available
void UpdateFilter(void); void UpdateFilter(void);