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

AP_NavEKF: added static mode for pre-arm and bench testing

Browse Source
This commit is contained in:
Paul Riseborough 2014-01-04 15:20:14 +11:00 committed by Andrew Tridgell
parent ab08a5c7d6
commit 619fffec3e
2 changed files with 27 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -27,7 +27,7 @@ NavEKF::NavEKF(const AP_AHRS *ahrs, AP_Baro &baro) :
_baro(baro),
useAirspeed(true),
useCompass(true),
fusionModeGPS(1), // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity
fusionModeGPS(0), // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs, no velocity, 3 = Force postion and velocity measurements to zero (only used during pre-arm or ground testing)
covTimeStepMax(0.07f), // maximum time (sec) between covariance prediction updates
covDelAngMax(0.05f), // maximum delta angle between covariance prediction updates
yawVarScale(2.0f), // scale factor applied to yaw gyro errors when on ground
@ -35,6 +35,7 @@ NavEKF::NavEKF(const AP_AHRS *ahrs, AP_Baro &baro) :
MAGmsecMax(333), // maximum allowed interval between magnetometer measurement updates
HGTmsecMax(1000), // maximum interval between height measurement updates
fuseMeNow(true), // forces fusion to occur on the IMU frame that data arrives
staticMode(false), // staticMode forces position and velocity fusion with zero values
msecVelDelay(230), // msec of GPS velocity delay
msecPosDelay(210), // msec of GPS position delay
msecHgtDelay(350), // msec of barometric height delay
@ -95,6 +96,9 @@ void NavEKF::ResetPosition(void)
states[8] = posNE[1];
states[9] = - _baro.get_altitude();
// set static mode to force positon and velocity measurements to zero
staticMode = true;
}
void NavEKF::InitialiseFilter(void)
@ -259,9 +263,18 @@ void NavEKF::SelectVelPosFusion()
// if no GPS
else if ((IMUmsec - HGTmsecPrev) >= HGTmsecMax)
{
// Static mode is used for pre-arm and bench testing and allows operation
// without GPS
if (staticMode) {
fuseVelData = true;
fusePosData = true;
fuseHgtData = true;
}
else {
fuseVelData = false;
fusePosData = false;
fuseHgtData = true;
}
HGTmsecPrev = IMUmsec;
}
else
@ -1071,6 +1084,12 @@ void NavEKF::FuseVelPosNED()
for (uint8_t i=0; i<=2; i++) observation[i] = velNED[i];
for (uint8_t i=3; i<=4; i++) observation[i] = posNE[i-3];
observation[5] = -hgtMea;
// zero observations if in static mode (used for pre-arm and bench testing)
if (staticMode) {
for (uint8_t i=0; i<=2; 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
velErr = _gpsVelVarAccScale*accNavMag;
@ -1157,7 +1176,7 @@ void NavEKF::FuseVelPosNED()
}
// Set range for sequential fusion of velocity and position measurements depending
// on which data is available and its health
if (fuseVelData && fusionModeGPS == 0 && velHealth)
if ((fuseVelData && fusionModeGPS == 0 && velHealth) || staticMode)
{
fuseData[0] = true;
fuseData[1] = true;
@ -1168,12 +1187,12 @@ void NavEKF::FuseVelPosNED()
fuseData[0] = true;
fuseData[1] = true;
}
if (fusePosData && fusionModeGPS <= 2 && posHealth)
if ((fusePosData && fusionModeGPS <= 2 && posHealth) || staticMode)
{
fuseData[3] = true;
fuseData[4] = true;
}
if (fuseHgtData && hgtHealth)
if ((fuseHgtData && hgtHealth) || staticMode)
{
fuseData[5] = true;
}

1
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -262,6 +262,7 @@ private:
uint32_t HGTmsecPrev; // time stamp of last height measurement fusion step
const uint32_t HGTmsecMax; // maximum allowed interval between height measurement fusion steps
const bool fuseMeNow; // boolean to force fusion whenever data arrives
bool staticMode; // boolean to force positio and velocity measurements to zero for pre-arm or bench testing
// last time compass was updated
uint32_t lastMagUpdate;