mirror of https://github.com/ArduPilot/ardupilot
AP_NavEKF: Load levelling scheduler - first attempt
This commit is contained in:
parent
68283b7aa1
commit
406fb31a57
|
@ -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);
|
||||||
|
|
|
@ -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;
|
||||||
|
|
Loading…
Reference in New Issue