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

AP_NavEKF2: Remove unused variables and improve variable names

This commit is contained in:
Paul Riseborough 2015-09-23 22:53:14 +10:00 committed by Andrew Tridgell
parent 4a7714e15d
commit 9c5e48e7e9
2 changed files with 51 additions and 80 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

72
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -317,7 +317,7 @@ void NavEKF2_core::SelectVelPosFusion()
ResetVelocity();
CovarianceInit();
// record the fail time
lastPosFailTime = imuSampleTime_ms;
lastPosFailTime_ms = imuSampleTime_ms;
// Reset the normalised innovation to avoid false failing the bad position fusion test
posTestRatio = 0.0f;
}
@ -331,8 +331,8 @@ void NavEKF2_core::SelectVelPosFusion()
// If we haven't received height data for a while, then declare the height data as being timed out
// set timeout period based on whether we have vertical GPS velocity available to constrain drift
hgtRetryTime = (useGpsVertVel && !velTimeout) ? frontend.hgtRetryTimeMode0_ms : frontend.hgtRetryTimeMode12_ms;
if (imuSampleTime_ms - lastHgtReceived_ms > hgtRetryTime) {
hgtRetryTime_ms = (useGpsVertVel && !velTimeout) ? frontend.hgtRetryTimeMode0_ms : frontend.hgtRetryTimeMode12_ms;
if (imuSampleTime_ms - lastHgtReceived_ms > hgtRetryTime_ms) {
hgtTimeout = true;
}
@ -359,7 +359,7 @@ void NavEKF2_core::SelectVelPosFusion()
} else {
gpsRetryTime = frontend.gpsRetryTimeNoTAS_ms;
}
if ((posTestRatio > 2.0f) && ((imuSampleTime_ms - lastPosFailTime) < gpsRetryTime) && ((imuSampleTime_ms - lastPosFailTime) > gpsRetryTime/2) && fusePosData) {
if ((posTestRatio > 2.0f) && ((imuSampleTime_ms - lastPosFailTime_ms) < gpsRetryTime) && ((imuSampleTime_ms - lastPosFailTime_ms) > gpsRetryTime/2) && fusePosData) {
lastGpsAidBadTime_ms = imuSampleTime_ms;
gpsAidingBad = true;
}
@ -424,7 +424,7 @@ void NavEKF2_core::SelectTasFusion()
// ensure that the covariance prediction is up to date before fusing data
if (!covPredStep) CovariancePrediction();
FuseAirspeed();
TASmsecPrev = imuSampleTime_ms;
prevTasStep_ms = imuSampleTime_ms;
tasDataWaiting = false;
newDataTas = false;
}
@ -436,9 +436,9 @@ void NavEKF2_core::SelectTasFusion()
void NavEKF2_core::SelectBetaFusion()
{
// set true when the fusion time interval has triggered
bool f_timeTrigger = ((imuSampleTime_ms - BETAmsecPrev) >= frontend.betaAvg_ms);
bool f_timeTrigger = ((imuSampleTime_ms - prevBetaStep_ms) >= frontend.betaAvg_ms);
// set true when use of synthetic sideslip fusion is necessary because we have limited sensor data or are dead reckoning position
bool f_required = !(use_compass() && useAirspeed() && ((imuSampleTime_ms - lastPosPassTime) < frontend.gpsRetryTimeNoTAS_ms));
bool f_required = !(use_compass() && useAirspeed() && ((imuSampleTime_ms - lastPosPassTime_ms) < frontend.gpsRetryTimeNoTAS_ms));
// set true when sideslip fusion is feasible (requires zero sideslip assumption to be valid and use of wind states)
bool f_feasible = (assume_zero_sideslip() && !inhibitWindStates);
// use synthetic sideslip fusion if feasible, required and enough time has lapsed since the last fusion
@ -446,7 +446,7 @@ void NavEKF2_core::SelectBetaFusion()
// ensure that the covariance prediction is up to date before fusing data
if (!covPredStep) CovariancePrediction();
FuseSideslip();
BETAmsecPrev = imuSampleTime_ms;
prevBetaStep_ms = imuSampleTime_ms;
}
}
@ -1373,13 +1373,13 @@ void NavEKF2_core::FuseVelPosNED()
posTestRatio = (sq(innovVelPos[3]) + sq(innovVelPos[4])) / maxPosInnov2;
posHealth = ((posTestRatio < 1.0f) || badIMUdata);
// declare a timeout condition if we have been too long without data or not aiding
posTimeout = (((imuSampleTime_ms - lastPosPassTime) > gpsRetryTime) || PV_AidingMode == AID_NONE);
posTimeout = (((imuSampleTime_ms - lastPosPassTime_ms) > gpsRetryTime) || PV_AidingMode == AID_NONE);
// use position data if healthy, timed out, or in constant position mode
if (posHealth || posTimeout || constPosMode) {
posHealth = true;
// only reset the failed time and do glitch timeout checks if we are doing full aiding
if (PV_AidingMode == AID_ABSOLUTE) {
lastPosPassTime = imuSampleTime_ms;
lastPosPassTime_ms = imuSampleTime_ms;
// if timed out or outside the specified uncertainty radius, increment the offset applied to GPS data to compensate for large GPS position jumps
if (posTimeout || ((varInnovVelPos[3] + varInnovVelPos[4]) > sq(float(frontend._gpsGlitchRadiusMax)))) {
gpsPosGlitchOffsetNE.x += innovVelPos[3];
@ -1393,7 +1393,7 @@ void NavEKF2_core::FuseVelPosNED()
// don't fuse data on this time step
fusePosData = false;
// record the fail time
lastPosFailTime = imuSampleTime_ms;
lastPosFailTime_ms = imuSampleTime_ms;
// Reset the normalised innovation to avoid false failing the bad position fusion test
posTestRatio = 0.0f;
}
@ -1429,12 +1429,12 @@ void NavEKF2_core::FuseVelPosNED()
// fail if the ratio is greater than 1
velHealth = ((velTestRatio < 1.0f) || badIMUdata);
// declare a timeout if we have not fused velocity data for too long or not aiding
velTimeout = (((imuSampleTime_ms - lastVelPassTime) > gpsRetryTime) || PV_AidingMode == AID_NONE);
velTimeout = (((imuSampleTime_ms - lastVelPassTime_ms) > gpsRetryTime) || PV_AidingMode == AID_NONE);
// if data is healthy or in constant velocity mode we fuse it
if (velHealth || velTimeout || constVelMode) {
velHealth = true;
// restart the timeout count
lastVelPassTime = imuSampleTime_ms;
lastVelPassTime_ms = imuSampleTime_ms;
} else if (velTimeout && !posHealth && PV_AidingMode == AID_ABSOLUTE) {
// if data is not healthy and timed out and position is unhealthy and we are using aiding, we reset the velocity, but do not fuse data on this time step
ResetVelocity();
@ -1455,11 +1455,11 @@ void NavEKF2_core::FuseVelPosNED()
hgtTestRatio = sq(innovVelPos[5]) / (sq(frontend._hgtInnovGate) * varInnovVelPos[5]);
// fail if the ratio is > 1, but don't fail if bad IMU data
hgtHealth = ((hgtTestRatio < 1.0f) || badIMUdata);
hgtTimeout = (imuSampleTime_ms - lastHgtPassTime) > hgtRetryTime;
hgtTimeout = (imuSampleTime_ms - lastHgtPassTime_ms) > hgtRetryTime_ms;
// Fuse height data if healthy or timed out or in constant position mode
if (hgtHealth || hgtTimeout || constPosMode) {
hgtHealth = true;
lastHgtPassTime = imuSampleTime_ms;
lastHgtPassTime_ms = imuSampleTime_ms;
// if timed out, reset the height, but do not fuse data on this time step
if (hgtTimeout) {
ResetHeight();
@ -2065,13 +2065,13 @@ void NavEKF2_core::FuseAirspeed()
// fail if the ratio is > 1, but don't fail if bad IMU data
tasHealth = ((tasTestRatio < 1.0f) || badIMUdata);
tasTimeout = (imuSampleTime_ms - lastTasPassTime) > frontend.tasRetryTime_ms;
tasTimeout = (imuSampleTime_ms - lastTasPassTime_ms) > frontend.tasRetryTime_ms;
// test the ratio before fusing data, forcing fusion if airspeed and position are timed out as we have no choice but to try and use airspeed to constrain error growth
if (tasHealth || (tasTimeout && posTimeout)) {
// restart the counter
lastTasPassTime = imuSampleTime_ms;
lastTasPassTime_ms = imuSampleTime_ms;
// zero the attitude error state - by definition it is assumed to be zero before each observaton fusion
@ -3361,9 +3361,10 @@ void NavEKF2_core::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGa
}
// return weighting of first IMU in blending function
// This filter always uses the primary IMU, so a weighting of 1 is returned
void NavEKF2_core::getIMU1Weighting(float &ret) const
{
ret = IMU1_weighting;
ret = 1.0f;
}
// return the individual Z-accel bias estimates in m/s^2
@ -3878,9 +3879,9 @@ void NavEKF2_core::readHgtData()
// check for new magnetometer data and update store measurements if available
void NavEKF2_core::readMagData()
{
if (use_compass() && _ahrs->get_compass()->last_update_usec() != lastMagUpdate) {
if (use_compass() && _ahrs->get_compass()->last_update_usec() != lastMagUpdate_ms) {
// store time of last measurement update
lastMagUpdate = _ahrs->get_compass()->last_update_usec();
lastMagUpdate_ms = _ahrs->get_compass()->last_update_usec();
// estimate of time magnetometer measurement was taken, allowing for delays
magMeasTime_ms = imuSampleTime_ms - frontend.magDelay_ms;
@ -3939,7 +3940,6 @@ void NavEKF2_core::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRa
// This filter uses a different definition of optical flow rates to the sensor with a positive optical flow rate produced by a
// negative rotation about that axis. For example a positive rotation of the flight vehicle about its X (roll) axis would produce a negative X flow rate
flowMeaTime_ms = imuSampleTime_ms;
flowQuality = rawFlowQuality;
// calculate bias errors on flow sensor gyro rates, but protect against spikes in data
// reset the accumulated body delta angle and time
// don't do the calculation if not enough time lapsed for a reliable body rate measurement
@ -4151,16 +4151,15 @@ void NavEKF2_core::InitialiseVariables()
// initialise time stamps
imuSampleTime_ms = hal.scheduler->millis();
lastHealthyMagTime_ms = imuSampleTime_ms;
TASmsecPrev = imuSampleTime_ms;
BETAmsecPrev = imuSampleTime_ms;
lastMagUpdate = 0;
prevTasStep_ms = imuSampleTime_ms;
prevBetaStep_ms = imuSampleTime_ms;
lastMagUpdate_ms = 0;
lastHgtReceived_ms = imuSampleTime_ms;
lastVelPassTime = imuSampleTime_ms;
lastPosPassTime = imuSampleTime_ms;
lastPosFailTime = 0;
lastHgtPassTime = imuSampleTime_ms;
lastTasPassTime = imuSampleTime_ms;
lastStateStoreTime_ms = imuSampleTime_ms;
lastVelPassTime_ms = imuSampleTime_ms;
lastPosPassTime_ms = imuSampleTime_ms;
lastPosFailTime_ms = 0;
lastHgtPassTime_ms = imuSampleTime_ms;
lastTasPassTime_ms = imuSampleTime_ms;
lastTimeGpsReceived_ms = 0;
secondLastGpsTime_ms = 0;
lastDecayTime_ms = imuSampleTime_ms;
@ -4189,10 +4188,6 @@ void NavEKF2_core::InitialiseVariables()
secondMagYawInit = false;
dtIMUavg = 0.0025f;
dt = 0;
lastGyroBias.zero();
lastAngRate.zero();
lastAccel1.zero();
lastAccel2.zero();
velDotNEDfilt.zero();
summedDelAng.zero();
summedDelVel.zero();
@ -4204,7 +4199,6 @@ void NavEKF2_core::InitialiseVariables()
memset(&processNoise[0], 0, sizeof(processNoise));
flowDataValid = false;
newDataRng = false;
flowFusePerformed = false;
fuseOptFlowData = false;
Popt = 0.0f;
terrainState = 0.0f;
@ -4228,14 +4222,12 @@ void NavEKF2_core::InitialiseVariables()
hgtRate = 0.0f;
mag_state.q0 = 1;
mag_state.DCM.identity();
IMU1_weighting = 0.5f;
onGround = true;
manoeuvring = false;
yawAligned = false;
inhibitWindStates = true;
inhibitMagStates = true;
gndOffsetValid = false;
flowXfailed = false;
validOrigin = false;
takeoffExpectedSet_ms = 0;
expectGndEffectTakeoff = false;
@ -4426,7 +4418,7 @@ void NavEKF2_core::getFilterStatus(nav_filter_status &status) const
status.flags.takeoff_detected = takeOffDetected; // takeoff for optical flow navigation has been detected
status.flags.takeoff = expectGndEffectTakeoff; // The EKF has been told to expect takeoff and is in a ground effect mitigation mode
status.flags.touchdown = expectGndEffectTouchdown; // The EKF has been told to detect touchdown and is in a ground effect mitigation mode
status.flags.using_gps = (imuSampleTime_ms - lastPosPassTime) < 4000;
status.flags.using_gps = (imuSampleTime_ms - lastPosPassTime_ms) < 4000;
}
// send an EKF_STATUS message to GCS
@ -4567,9 +4559,9 @@ void NavEKF2_core::performArmingChecks()
lastTimeGpsReceived_ms = imuSampleTime_ms;
secondLastGpsTime_ms = imuSampleTime_ms;
// reset the last valid position fix time to prevent unwanted activation of GPS glitch logic
lastPosPassTime = imuSampleTime_ms;
lastPosPassTime_ms = imuSampleTime_ms;
// reset the fail time to prevent premature reporting of loss of position accruacy
lastPosFailTime = 0;
lastPosFailTime_ms = 0;
}
}
// Reset all position, velocity and covariance

41
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -591,7 +591,6 @@ private:
Vector3f correctedDelVel; // delta velocities along the XYZ body axes for weighted average of IMU1 and IMU2 corrected for errors (m/s)
Vector3f summedDelAng; // corrected & summed delta angles about the xyz body axes (rad)
Vector3f summedDelVel; // corrected & summed delta velocities along the XYZ body axes (m/s)
Vector3f lastGyroBias; // previous gyro bias vector used by filter divergence check
Matrix3f prevTnb; // previous nav to body transformation used for INS earth rotation compensation
ftype accNavMag; // magnitude of navigation accel - used to adjust GPS obs variance (m/s^2)
ftype accNavMagHoriz; // magnitude of navigation accel in horizontal plane (m/s^2)
@ -612,48 +611,36 @@ private:
Vector3f varInnovMag; // innovation variance output from fusion of X,Y,Z compass measurements
ftype innovVtas; // innovation output from fusion of airspeed measurements
ftype varInnovVtas; // innovation variance output from fusion of airspeed measurements
bool fuseVtasData; // boolean true when airspeed data is to be fused
float VtasMeas; // true airspeed measurement (m/s)
bool covPredStep; // boolean set to true when a covariance prediction step has been 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 tasFuseStep; // boolean set to true when airspeed fusion is being performed
uint32_t TASmsecPrev; // time stamp of last TAS fusion step
uint32_t BETAmsecPrev; // time stamp of last synthetic sideslip fusion step
uint32_t MAGmsecPrev; // time stamp of last compass fusion step
uint32_t HGTmsecPrev; // time stamp of last height measurement fusion step
uint32_t prevTasStep_ms; // time stamp of last TAS fusion step
uint32_t prevBetaStep_ms; // time stamp of last synthetic sideslip fusion step
bool constPosMode; // true when fusing a constant position to maintain attitude reference for planned operation without GPS or optical flow data
uint32_t lastMagUpdate; // last time compass was updated
uint32_t lastMagUpdate_ms; // last time compass was updated
Vector3f velDotNED; // rate of change of velocity in NED frame
Vector3f velDotNEDfilt; // low pass filtered velDotNED
uint32_t imuSampleTime_ms; // time that the last IMU value was taken
bool newDataMag; // true when new magnetometer data has arrived
bool newDataTas; // true when new airspeed data has arrived
bool tasDataWaiting; // true when new airspeed data is waiting to be fused
uint32_t lastHgtReceived_ms; // time last time we received height data
uint16_t hgtRetryTime; // time allowed without use of height measurements before a height timeout is declared
uint32_t lastVelPassTime; // time stamp when GPS velocity measurement last passed innovation consistency check (msec)
uint32_t lastPosPassTime; // time stamp when GPS position measurement last passed innovation consistency check (msec)
uint32_t lastPosFailTime; // time stamp when GPS position measurement last failed innovation consistency check (msec)
uint32_t lastHgtPassTime; // time stamp when height measurement last passed innovation consistency check (msec)
uint32_t lastTasPassTime; // time stamp when airspeed measurement last passed innovation consistency check (msec)
uint32_t lastStateStoreTime_ms; // time of last state vector storage
uint16_t hgtRetryTime_ms; // time allowed without use of height measurements before a height timeout is declared
uint32_t lastVelPassTime_ms; // time stamp when GPS velocity measurement last passed innovation consistency check (msec)
uint32_t lastPosPassTime_ms; // time stamp when GPS position measurement last passed innovation consistency check (msec)
uint32_t lastPosFailTime_ms; // time stamp when GPS position measurement last failed innovation consistency check (msec)
uint32_t lastHgtPassTime_ms; // time stamp when height measurement last passed innovation consistency check (msec)
uint32_t lastTasPassTime_ms; // time stamp when airspeed measurement last passed innovation consistency check (msec)
uint32_t lastTimeGpsReceived_ms;// last time we recieved GPS data
uint32_t timeAtLastAuxEKF_ms; // last time the auxilliary filter was run to fuse range or optical flow measurements
uint32_t secondLastGpsTime_ms; // time of second last GPS fix used to determine how long since last update
uint32_t lastHealthyMagTime_ms; // time the magnetometer was last declared healthy
uint32_t ekfStartTime_ms; // time the EKF was started (msec)
Vector3f lastAngRate; // angular rate from previous IMU sample used for trapezoidal integrator
Vector3f lastAccel1; // acceleration from previous IMU1 sample used for trapezoidal integrator
Vector3f lastAccel2; // acceleration from previous IMU2 sample used for trapezoidal integrator
Matrix24 nextP; // Predicted covariance matrix before addition of process noise to diagonals
Vector24 processNoise; // process noise added to diagonals of predicted covariance matrix
Vector25 SF; // intermediate variables used to calculate predicted covariance matrix
Vector5 SG; // intermediate variables used to calculate predicted covariance matrix
Vector8 SQ; // intermediate variables used to calculate predicted covariance matrix
Vector23 SPP; // intermediate variables used to calculate predicted covariance matrix
float IMU1_weighting; // Weighting applied to use of IMU1. Varies between 0 and 1.
bool yawAligned; // true when the yaw angle has been aligned
Vector2f gpsPosGlitchOffsetNE; // offset applied to GPS data in the NE direction to compensate for rapid changes in GPS solution
Vector2f lastKnownPositionNE; // last known position
@ -668,7 +655,6 @@ private:
bool firstArmComplete; // true when first transition out of static mode has been performed after start up
bool firstMagYawInit; // true when the first post takeoff initialisation of earth field and yaw angle has been performed
bool secondMagYawInit; // true when the second post takeoff initialisation of earth field and yaw angle has been performed
bool flowTimeout; // true when optical flow measurements have time out
Vector2f gpsVelGlitchOffset; // Offset applied to the GPS velocity when the gltch radius is being decayed back to zero
bool gpsNotAvailable; // bool true when valid GPS data is not available
bool filterArmed; // true when the vehicle is disarmed
@ -681,7 +667,7 @@ private:
bool gpsAidingBad; // true when GPS position measurements have been consistently rejected by the filter
uint32_t lastGpsAidBadTime_ms; // time in msec gps aiding was last detected to be bad
float posDownAtArming; // flight vehicle vertical position at arming used as a reference point
bool highYawRate; // true when the vehicle is doing rapid yaw rotation where gyro scel factor errors could cause loss of heading reference
bool highYawRate; // true when the vehicle is doing rapid yaw rotation where gyro scale factor errors could cause loss of heading reference
float yawRateFilt; // filtered yaw rate used to determine when the vehicle is doing rapid yaw rotation where gyro scel factor errors could cause loss of heading reference
bool useGpsVertVel; // true if GPS vertical velocity should be used
float yawResetAngle; // Change in yaw angle due to last in-flight yaw reset in radians. A positive value means the yaw angle has increased.
@ -724,7 +710,6 @@ private:
of_elements ofDataDelayed; // OF data at the fusion time horizon
uint8_t ofStoreIndex; // OF data storage index
bool newDataFlow; // true when new optical flow data has arrived
bool flowFusePerformed; // true when optical flow fusion has been performed in that time step
bool flowDataValid; // true while optical flow data is still fresh
bool fuseOptFlowData; // this boolean causes the last optical flow measurement to be fused
float auxFlowObsInnov; // optical flow rate innovation from 1-state terrain offset estimator
@ -734,7 +719,6 @@ private:
uint32_t flowValidMeaTime_ms; // time stamp from latest valid flow measurement (msec)
uint32_t rngValidMeaTime_ms; // time stamp from latest valid range measurement (msec)
uint32_t flowMeaTime_ms; // time stamp from latest flow measurement (msec)
uint8_t flowQuality; // unsigned integer representing quality of optical flow data. 255 is maximum quality.
uint32_t gndHgtValidTime_ms; // time stamp from last terrain offset state update (msec)
Vector3f omegaAcrossFlowTime; // body angular rates averaged across the optical flow sample period
Matrix3f Tnb_flow; // transformation matrix from nav to body axes at the middle of the optical flow sample period
@ -766,7 +750,6 @@ private:
};
AidingMode PV_AidingMode; // Defines the preferred mode for aiding of velocity and position estimates from the INS
bool gndOffsetValid; // true when the ground offset state can still be considered valid
bool flowXfailed; // true when the X optical flow measurement has failed the innovation consistency check
Vector3f delAngBodyOF; // bias corrected delta angle of the vehicle IMU measured summed across the time since the last OF measurement
float delTimeOF; // time that delAngBodyOF is summed across
@ -783,10 +766,6 @@ private:
float rangeAtArming; // range finder measurement when armed
uint32_t timeAtArming_ms; // time in msec that the vehicle armed
// IMU processing
float dtDelVel;
float dtDelVel2;
// baro ground effect
bool expectGndEffectTakeoff; // external state from ArduCopter - takeoff expected
uint32_t takeoffExpectedSet_ms; // system time at which expectGndEffectTakeoff was set