mirror of https://github.com/ArduPilot/ardupilot
AP_NavEKF2: Remove unused variables and improve variable names
This commit is contained in:
parent
4a7714e15d
commit
9c5e48e7e9
|
@ -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
|
||||
|
|
|
@ -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.
|
||||
Vector8 SQ; // intermediate variables used to calculate predicted covariance matrix
|
||||
Vector23 SPP; // intermediate variables used to calculate predicted covariance matrix
|
||||
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,11 +655,10 @@ 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
|
||||
bool prevFilterArmed; // vehicleArmed from previous frame
|
||||
bool filterArmed; // true when the vehicle is disarmed
|
||||
bool prevFilterArmed; // vehicleArmed from previous frame
|
||||
struct Location EKF_origin; // LLH origin of the NED axis system - do not change unless filter is reset
|
||||
bool validOrigin; // true when the EKF origin is valid
|
||||
float gpsSpdAccuracy; // estimated speed accuracy in m/s returned by the UBlox GPS receiver
|
||||
|
@ -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,23 +710,21 @@ 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
|
||||
float auxFlowObsInnovVar; // innovation variance for optical flow observations from 1-state terrain offset estimator
|
||||
Vector2 flowRadXYcomp; // motion compensated optical flow angular rates(rad/sec)
|
||||
Vector2 flowRadXY; // raw (non motion compensated) optical flow angular rates (rad/sec)
|
||||
Vector2 flowRadXYcomp; // motion compensated optical flow angular rates(rad/sec)
|
||||
Vector2 flowRadXY; // raw (non motion compensated) optical flow angular rates (rad/sec)
|
||||
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
|
||||
Matrix3f Tbn_flow; // transformation matrix from body to nav axes at the middle of the optical flow sample period
|
||||
Vector2 varInnovOptFlow; // optical flow innovations variances (rad/sec)^2
|
||||
Vector2 innovOptFlow; // optical flow LOS innovations (rad/sec)
|
||||
Vector2 varInnovOptFlow; // optical flow innovations variances (rad/sec)^2
|
||||
Vector2 innovOptFlow; // optical flow LOS innovations (rad/sec)
|
||||
float Popt; // Optical flow terrain height state covariance (m^2)
|
||||
float terrainState; // terrain position state (m)
|
||||
float prevPosN; // north position at last measurement
|
||||
|
@ -751,7 +735,7 @@ private:
|
|||
float rngMea; // range finder measurement (m)
|
||||
bool inhibitGndState; // true when the terrain position state is to remain constant
|
||||
uint32_t prevFlowFuseTime_ms; // time both flow measurement components passed their innovation consistency checks
|
||||
Vector2 flowTestRatio; // square of optical flow innovations divided by fail threshold used by main filter where >1.0 is a fail
|
||||
Vector2 flowTestRatio; // square of optical flow innovations divided by fail threshold used by main filter where >1.0 is a fail
|
||||
float auxFlowTestRatio; // sum of squares of optical flow innovation divided by fail threshold used by 1-state terrain offset estimator
|
||||
float R_LOS; // variance of optical flow rate measurements (rad/sec)^2
|
||||
float auxRngTestRatio; // square of range finder innovations divided by fail threshold used by main filter where >1.0 is a fail
|
||||
|
@ -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
|
||||
|
|
Loading…
Reference in New Issue