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

AP_NavEKF: Load levelling scheduler - first attempt

This commit is contained in:
Paul Riseborough 2013-12-30 19:58:24 +11:00 committed by Andrew Tridgell
parent 68283b7aa1
commit 406fb31a57
2 changed files with 35 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -26,8 +26,10 @@ NavEKF::NavEKF(const AP_AHRS &ahrs, AP_Baro &baro) :
covTimeStepMax(0.07f), // maximum time (sec) between covariance prediction updates covTimeStepMax(0.07f), // maximum time (sec) between covariance prediction updates
covDelAngMax(0.05f), // maximum delta angle between covariance prediction updates covDelAngMax(0.05f), // maximum delta angle between covariance prediction updates
TASmsecTgt(250), // target interal between airspeed measurement updates TASmsecTgt(250), // target interal between airspeed measurement updates
MAGmsecTgt(300), // target interval between magnetometer measurement updates TASmsecMax(500), // maximum allowed interal between airspeed measurement updates
HGTmsecTgt(250), // maximum interval between height measurement updates MAGmsecTgt(333), // target interval between magnetometer measurement updates
MAGmsecMax(500), // maximum allowed interval between magnetometer measurement updates
HGTmsecMax(250), // maximum interval between height measurement updates
msecVelDelay(230), // msec of GPS velocity delay msecVelDelay(230), // msec of GPS velocity delay
msecPosDelay(210), // msec of GPS position delay msecPosDelay(210), // msec of GPS position delay
msecHgtDelay(350), // msec of barometric height delay msecHgtDelay(350), // msec of barometric height delay
@ -156,7 +158,7 @@ void NavEKF::UpdateFilter()
// perform a covariance prediction if the total delta angle has exceeded the limit // perform a covariance prediction if the total delta angle has exceeded the limit
// or the time limit will be exceeded at the next IMU update // or the time limit will be exceeded at the next IMU update
// Do not predict covariance if magnetometer fusion still needs to be performed // Do not predict covariance if magnetometer fusion still needs to be performed
if (((dt >= (covTimeStepMax - dtIMU)) || (summedDelAng.length() > covDelAngMax)))// && !magFuseStep) if ((((dt >= (covTimeStepMax - dtIMU)) || (summedDelAng.length() > covDelAngMax))) && !magFuseRequired)
{ {
CovariancePrediction(); CovariancePrediction();
covPredStep = true; covPredStep = true;
@ -191,8 +193,8 @@ void NavEKF::SelectVelPosFusion()
fusePosData = false; fusePosData = false;
} }
// Fuse height measurements at the same time as GPS measurements for efficiency // Fuse height measurements at the same time as GPS measurements for efficiency
// Don't wait longer than HGTmsecTgt msec between height updates // Don't wait longer than HGTmsecMax msec between height updates
if (statesInitialised && (newDataGps || ((IMUmsec - HGTmsecPrev) >= HGTmsecTgt))) if (statesInitialised && (newDataGps || ((IMUmsec - HGTmsecPrev) >= HGTmsecMax)))
{ {
HGTmsecPrev = IMUmsec; HGTmsecPrev = IMUmsec;
fuseHgtData = true; fuseHgtData = true;
@ -212,13 +214,11 @@ void NavEKF::SelectVelPosFusion()
void NavEKF::SelectMagFusion() void NavEKF::SelectMagFusion()
{ {
readMagData(); readMagData();
// Fuse Magnetometer Measurements // Fuse Magnetometer Measurements - hold off if covariance prediction has occurred, unless maximum time interval exceeded
if (statesInitialised && useCompass && newDataMag) if (statesInitialised && useCompass && newDataMag && ((!covPredStep && ((IMUmsec - MAGmsecPrev) >= MAGmsecTgt)) || ((IMUmsec - MAGmsecPrev) >= MAGmsecMax)))
//if (statesInitialised && useCompass && newDataMag && ((IMUmsec - MAGmsecPrev) >= MAGmsecTgt))
{ {
MAGmsecPrev = IMUmsec; MAGmsecPrev = IMUmsec;
fuseMagData = true; fuseMagData = true;
//::printf("Fuse Mag: (%.3f %.3f %.3f)\n",magData.x,magData.y,magData.z);
} }
else else
{ {
@ -234,9 +234,8 @@ void NavEKF::SelectMagFusion()
void NavEKF::SelectTasFusion() void NavEKF::SelectTasFusion()
{ {
readAirSpdData(); readAirSpdData();
// Fuse Airspeed Measurements // Fuse Airspeed Measurements - hold off if covariance prediction or magnetometer fusion has been performed, unless maximum time interval exceeded
if (statesInitialised && useAirspeed && !onGround && newDataTas) if (statesInitialised && useAirspeed && !onGround && newDataTas && ((!covPredStep && !magFusePerformed && ((IMUmsec - TASmsecPrev) >= TASmsecTgt)) || ((IMUmsec - TASmsecPrev) >= TASmsecMax)))
//if (statesInitialised && useAirspeed && !onGround && newDataTas && !covPredStep && !magFuseStep && ((IMUmsec - TASmsecPrev) >= TASmsecTgt))
{ {
TASmsecPrev = IMUmsec; TASmsecPrev = IMUmsec;
FuseAirspeed(); FuseAirspeed();
@ -1460,8 +1459,9 @@ void NavEKF::FuseMagnetometer()
// reset the observation index to 0 (we start by fusing the X // reset the observation index to 0 (we start by fusing the X
// measurement) // measurement)
obsIndex = 0; obsIndex = 0;
// Set flag to indicate to other processes that magnetometer fusion is performed on this time step // set flags to indicate to other processes that fusion has been perfomred and is required on the next time step
magFuseStep = true; magFusePerformed = true;
magFuseRequired = true;
} }
else if (obsIndex == 1) // we are now fusing the Y measurement else if (obsIndex == 1) // we are now fusing the Y measurement
{ {
@ -1509,8 +1509,9 @@ void NavEKF::FuseMagnetometer()
Kfusion[22] = SK_MY[0]*(P[22][22] + P[22][0]*SH_MAG[2] + P[22][1]*SH_MAG[1] + P[22][2]*SH_MAG[0] - P[22][3]*SK_MY[2] - P[22][19]*SK_MY[1] - P[22][18]*SK_MY[3] + P[22][20]*SK_MY[4]); Kfusion[22] = SK_MY[0]*(P[22][22] + P[22][0]*SH_MAG[2] + P[22][1]*SH_MAG[1] + P[22][2]*SH_MAG[0] - P[22][3]*SK_MY[2] - P[22][19]*SK_MY[1] - P[22][18]*SK_MY[3] + P[22][20]*SK_MY[4]);
Kfusion[23] = SK_MY[0]*(P[23][22] + P[23][0]*SH_MAG[2] + P[23][1]*SH_MAG[1] + P[23][2]*SH_MAG[0] - P[23][3]*SK_MY[2] - P[23][19]*SK_MY[1] - P[23][18]*SK_MY[3] + P[23][20]*SK_MY[4]); Kfusion[23] = SK_MY[0]*(P[23][22] + P[23][0]*SH_MAG[2] + P[23][1]*SH_MAG[1] + P[23][2]*SH_MAG[0] - P[23][3]*SK_MY[2] - P[23][19]*SK_MY[1] - P[23][18]*SK_MY[3] + P[23][20]*SK_MY[4]);
varInnovMag[1] = 1.0f/SK_MY[0]; varInnovMag[1] = 1.0f/SK_MY[0];
// Set flag to indicate to other processes that magnetometer fusion is performed on this time step // set flags to indicate to other processes that fusion has been perfomred and is required on the next time step
magFuseStep = true; magFusePerformed = true;
magFuseRequired = true;
} }
else if (obsIndex == 2) // we are now fusing the Z measurement else if (obsIndex == 2) // we are now fusing the Z measurement
{ {
@ -1559,8 +1560,9 @@ void NavEKF::FuseMagnetometer()
Kfusion[22] = SK_MZ[0]*(P[22][23] + P[22][0]*SH_MAG[1] + P[22][3]*SH_MAG[0] - P[22][1]*SK_MZ[2] + P[22][2]*SK_MZ[3] + P[22][20]*SK_MZ[1] + P[22][18]*SK_MZ[5] - P[22][19]*SK_MZ[4]); Kfusion[22] = SK_MZ[0]*(P[22][23] + P[22][0]*SH_MAG[1] + P[22][3]*SH_MAG[0] - P[22][1]*SK_MZ[2] + P[22][2]*SK_MZ[3] + P[22][20]*SK_MZ[1] + P[22][18]*SK_MZ[5] - P[22][19]*SK_MZ[4]);
Kfusion[23] = SK_MZ[0]*(P[23][23] + P[23][0]*SH_MAG[1] + P[23][3]*SH_MAG[0] - P[23][1]*SK_MZ[2] + P[23][2]*SK_MZ[3] + P[23][20]*SK_MZ[1] + P[23][18]*SK_MZ[5] - P[23][19]*SK_MZ[4]); Kfusion[23] = SK_MZ[0]*(P[23][23] + P[23][0]*SH_MAG[1] + P[23][3]*SH_MAG[0] - P[23][1]*SK_MZ[2] + P[23][2]*SK_MZ[3] + P[23][20]*SK_MZ[1] + P[23][18]*SK_MZ[5] - P[23][19]*SK_MZ[4]);
varInnovMag[2] = 1.0f/SK_MZ[0]; varInnovMag[2] = 1.0f/SK_MZ[0];
// Set flag to indicate to other processes that magnetometer fusion is performed on this time step // set flags to indicate to other processes that fusion has been perfomred and is not required on the next time step
magFuseStep = true; magFusePerformed = true;
magFuseRequired = false;
} }
// Calculate the measurement innovation // Calculate the measurement innovation
innovMag[obsIndex] = MagPred[obsIndex] - magData[obsIndex]; innovMag[obsIndex] = MagPred[obsIndex] - magData[obsIndex];
@ -1624,8 +1626,9 @@ void NavEKF::FuseMagnetometer()
} }
else else
{ {
// clear flag to indicate to other processes that magnetometer fusion is not performed on this time step // set flags to indicate to other processes that fusion has not been performed and is not required on the next time step
magFuseStep = false; magFusePerformed = false;
magFuseRequired = false;
} }
} }
@ -1881,6 +1884,13 @@ void NavEKF::getEulerAngles(Vector3f &euler)
quat2eul(euler, q); quat2eul(euler, q);
} }
void NavEKF::getVelNED(Vector3f &vel)
{
vel.x = states[4];
vel.y = states[5];
vel.z = states[6];
}
void NavEKF::calcposNE(float lat, float lon) void NavEKF::calcposNE(float lat, float lon)
{ {
posNE[0] = RADIUS_OF_EARTH * (lat - latRef); posNE[0] = RADIUS_OF_EARTH * (lat - latRef);

7
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -193,15 +193,18 @@ private:
const float covTimeStepMax; // maximum time allowed between covariance predictions const float covTimeStepMax; // maximum time allowed between covariance predictions
const float covDelAngMax; // maximum delta angle between covariance predictions const float covDelAngMax; // maximum delta angle between covariance predictions
bool covPredStep; // boolean set to true when a covariance prediction step has been performed bool covPredStep; // boolean set to true when a covariance prediction step has been performed
bool magFuseStep; // boolean set to true when magnetometer fusion is being performed bool magFusePerformed; // boolean set to true when magnetometer fusion has been perfomred in that time step
bool magFuseRequired; // boolean set to true when magnetometer fusion will be perfomred in the next time step
bool posVelFuseStep; // boolean set to true when position and velocity fusion is being performed bool posVelFuseStep; // boolean set to true when position and velocity fusion is being performed
bool tasFuseStep; // boolean set to true when airspeed fusion is being performed bool tasFuseStep; // boolean set to true when airspeed fusion is being performed
uint32_t TASmsecPrev; // time stamp of last TAS fusion step uint32_t TASmsecPrev; // time stamp of last TAS fusion step
const uint32_t TASmsecTgt; // target interval between TAS fusion steps const uint32_t TASmsecTgt; // target interval between TAS fusion steps
const uint32_t TASmsecMax; // maximum allowed interval between TAS fusion steps
uint32_t MAGmsecPrev; // time stamp of last compass fusion step uint32_t MAGmsecPrev; // time stamp of last compass fusion step
const uint32_t MAGmsecTgt; // target interval between compass fusion steps const uint32_t MAGmsecTgt; // target interval between compass fusion steps
const uint32_t MAGmsecMax; // maximum allowed interval between compass fusion steps
uint32_t HGTmsecPrev; // time stamp of last height measurement fusion step uint32_t HGTmsecPrev; // time stamp of last height measurement fusion step
const uint32_t HGTmsecTgt; // target interval between height measurement fusion steps const uint32_t HGTmsecMax; // maximum allowed interval between height measurement fusion steps
// Estimated time delays (msec) for different measurements relative to IMU // Estimated time delays (msec) for different measurements relative to IMU
const uint32_t msecVelDelay; const uint32_t msecVelDelay;