uint32_ttimeTasReceived_ms;// time last TAS data was received (msec)
boolgpsGoodToAlign;// true when the GPS quality can be used to initialise the navigation system
uint32_tmagYawResetTimer_ms;// timer in msec used to track how long good magnetometer data is failing innovation consistency checks
boolconsistentMagData;// true when the magnetometers are passing consistency checks
boolmotorsArmed;// true when the motors have been armed
boolprevMotorsArmed;// value of motorsArmed from previous frame
boolposVelFusionDelayed;// true when the position and velocity fusion has been delayed
booloptFlowFusionDelayed;// true when the optical flow fusion has been delayed
boolairSpdFusionDelayed;// true when the air speed fusion has been delayed
boolsideSlipFusionDelayed;// true when the sideslip fusion has been delayed
Vector3flastMagOffsets;// Last magnetometer offsets from COMPASS_ parameters. Used to detect parameter changes.
boollastMagOffsetsValid;// True when lastMagOffsets has been initialized
Vector2fposResetNE;// Change in North/East position due to last in-flight reset in metres. Returned by getLastPosNorthEastReset
uint32_tlastPosReset_ms;// System time at which the last position reset occurred. Returned by getLastPosNorthEastReset
Vector2fvelResetNE;// Change in North/East velocity due to last in-flight reset in metres/sec. Returned by getLastVelNorthEastReset
uint32_tlastVelReset_ms;// System time at which the last velocity reset occurred. Returned by getLastVelNorthEastReset
floatposResetD;// Change in Down position due to last in-flight reset in metres. Returned by getLastPosDowntReset
uint32_tlastPosResetD_ms;// System time at which the last position reset occurred. Returned by getLastPosDownReset
floatyawTestRatio;// square of magnetometer yaw angle innovation divided by fail threshold
QuaternionprevQuatMagReset;// Quaternion from the last time the magnetic field state reset condition test was performed
uint8_tfusionHorizonOffset;// number of IMU samples that the fusion time horizon has been shifted to prevent multiple EKF instances fusing data at the same time
floathgtInnovFiltState;// state used for fitering of the height innovations used for pre-flight checks
uint8_tmagSelectIndex;// Index of the magnetometer that is being used by the EKF
boolrunUpdates;// boolean true when the EKF updates can be run
uint32_tframesSincePredict;// number of frames lapsed since EKF instance did a state prediction
boolstartPredictEnabled;// boolean true when the frontend has given permission to start a new state prediciton cycele
uint8_tlocalFilterTimeStep_ms;// average number of msec between filter updates
floatposDownObsNoise;// observation noise variance on the vertical position used by the state and covariance update step (m^2)
Vector3fdelAngCorrected;// corrected IMU delta angle vector at the EKF time horizon (rad)
Vector3fdelVelCorrected;// corrected IMU delta velocity vector at the EKF time horizon (m/s)
boolmagFieldLearned;// true when the magnetic field has been learned
Vector3fearthMagFieldVar;// NED earth mag field variances for last learned field (mGauss^2)
Vector3fbodyMagFieldVar;// XYZ body mag field variances for last learned field (mGauss^2)
booldelAngBiasLearned;// true when the gyro bias has been learned
nav_filter_statusfilterStatus;// contains the status of various filter outputs
floatekfOriginHgtVar;// Variance of the the EKF WGS-84 origin height estimate (m^2)
uint32_tlastOriginHgtTime_ms;// last time the ekf's WGS-84 origin height was corrected
Vector3foutputTrackError;// attitude (rad), velocity (m/s) and position (m) tracking error magnitudes from the output observer
Vector3fvelOffsetNED;// This adds to the earth frame velocity estimate at the IMU to give the velocity at the body origin (m/s)
Vector3fposOffsetNED;// This adds to the earth frame position estimate at the IMU to give the position at the body origin (m)
// variables used to calculate a vertical velocity that is kinematically consistent with the verical position
floatposDownDerivative;// Rate of chage of vertical position (dPosD/dt) in m/s. This is the first time derivative of PosD.
floatposDown;// Down position state used in calculation of posDownRate
// variables used by the pre-initialisation GPS checks
structLocationgpsloc_prev;// LLH location of previous GPS measurement
uint32_tlastPreAlignGpsCheckTime_ms;// last time in msec the GPS quality was checked during pre alignment checks
floatgpsDriftNE;// amount of drift detected in the GPS position during pre-flight GPs checks
Vector2innovOptFlow;// optical flow LOS innovations (rad/sec)
floatPopt;// Optical flow terrain height state covariance (m^2)
floatterrainState;// terrain position state (m)
floatprevPosN;// north position at last measurement
floatprevPosE;// east position at last measurement
floatvarInnovRng;// range finder observation innovation variance (m^2)
floatinnovRng;// range finder observation innovation (m)
floathgtMea;// height measurement derived from either baro, gps or range finder data (m)
boolinhibitGndState;// true when the terrain position state is to remain constant
uint32_tprevFlowFuseTime_ms;// time both flow measurement components passed their innovation consistency checks
Vector2flowTestRatio;// square of optical flow innovations divided by fail threshold used by main filter where >1.0 is a fail
floatauxFlowTestRatio;// sum of squares of optical flow innovation divided by fail threshold used by 1-state terrain offset estimator
floatR_LOS;// variance of optical flow rate measurements (rad/sec)^2
floatauxRngTestRatio;// square of range finder innovations divided by fail threshold used by main filter where >1.0 is a fail
Vector2fflowGyroBias;// bias error of optical flow sensor gyro output
boolrangeDataToFuse;// true when valid range finder height data has arrived at the fusion time horizon.
boolbaroDataToFuse;// true when valid baro height finder data has arrived at the fusion time horizon.
boolgpsDataToFuse;// true when valid GPS data has arrived at the fusion time horizon.
boolmagDataToFuse;// true when valid magnetometer data has arrived at the fusion time horizon
Vector2fheldVelNE;// velocity held when no aiding is available
enumAidingMode{AID_ABSOLUTE=0,// GPS or some other form of absolute position reference aiding is being used (optical flow may also be used in parallel) so position estimates are absolute.
AID_NONE=1,// no aiding is being used so only attitude and height estimates are available. Either constVelMode or constPosMode must be used to constrain tilt drift.
AID_RELATIVE=2// only optical flow aiding is being used so position estimates will be relative
};
AidingModePV_AidingMode;// Defines the preferred mode for aiding of velocity and position estimates from the INS
AidingModePV_AidingModePrev;// Value of PV_AidingMode from the previous frame - used to detect transitions
boolgpsInhibit;// externally set flag informing the EKF not to use the GPS
boolgndOffsetValid;// true when the ground offset state can still be considered valid
Vector3fdelAngBodyOF;// bias corrected delta angle of the vehicle IMU measured summed across the time since the last OF measurement
floatdelTimeOF;// time that delAngBodyOF is summed across
Vector3faccelPosOffset;// position of IMU accelerometer unit in body frame (m)
// Range finder
floatbaroHgtOffset;// offset applied when when switching to use of Baro height
floatrngOnGnd;// Expected range finder reading in metres when vehicle is on ground
floatstoredRngMeas[2][3];// Ringbuffer of stored range measurements for dual range sensors
uint32_tstoredRngMeasTime_ms[2][3];// Ringbuffers of stored range measurement times for dual range sensors
uint32_tlastRngMeasTime_ms;// Timestamp of last range measurement
uint8_trngMeasIndex[2];// Current range measurement ringbuffer index for dual range sensors
boolterrainHgtStable;// true when the terrain height is stable enough to be used as a height reference
uint32_tterrainHgtStableSet_ms;// system time at which terrainHgtStable was set
// Range Beacon Sensor Fusion
obs_ring_buffer_t<rng_bcn_elements>storedRangeBeacon;// Beacon range buffer
rng_bcn_elementsrngBcnDataNew;// Range beacon data at the current time horizon
rng_bcn_elementsrngBcnDataDelayed;// Range beacon data at the fusion time horizon
uint8_trngBcnStoreIndex;// Range beacon data storage index
uint32_tlastRngBcnPassTime_ms;// time stamp when the range beacon measurement last passed innvovation consistency checks (msec)
floatrngBcnTestRatio;// Innovation test ratio for range beacon measurements
boolrngBcnHealth;// boolean true if range beacon measurements have passed innovation consistency check
boolrngBcnTimeout;// boolean true if range beacon measurements have faled innovation consistency checks for too long
floatvarInnovRngBcn;// range beacon observation innovation variance (m^2)
floatinnovRngBcn;// range beacon observation innovation (m)
uint32_tlastTimeRngBcn_ms[10];// last time we received a range beacon measurement (msec)
boolrngBcnDataToFuse;// true when there is new range beacon data to fuse
Vector3fbeaconVehiclePosNED;// NED position estimate from the beacon system (NED)
floatbeaconVehiclePosErr;// estimated position error from the beacon system (m)
uint32_trngBcnLast3DmeasTime_ms;// last time the beacon system returned a 3D fix (msec)
boolrngBcnGoodToAlign;// true when the range beacon systems 3D fix can be used to align the filter
uint8_tlastRngBcnChecked;// index of the last range beacon checked for data
Vector3freceiverPos;// receiver NED position (m) - alignment 3 state filter
floatreceiverPosCov[3][3];// Receiver position covariance (m^2) - alignment 3 state filter (
boolrngBcnAlignmentStarted;// True when the initial position alignment using range measurements has started
boolrngBcnAlignmentCompleted;// True when the initial position alignment using range measurements has finished
uint8_tlastBeaconIndex;// Range beacon index last read - used during initialisation of the 3-state filter
Vector3frngBcnPosSum;// Sum of range beacon NED position (m) - used during initialisation of the 3-state filter
uint8_tnumBcnMeas;// Number of beacon measurements - used during initialisation of the 3-state filter
floatrngSum;// Sum of range measurements (m) - used during initialisation of the 3-state filter
uint8_tN_beacons;// Number of range beacons in use
floatmaxBcnPosD;// maximum position of all beacons in the down direction (m)
floatminBcnPosD;// minimum position of all beacons in the down direction (m)
floatbcnPosOffset;// Vertical position offset of the beacon constellation origin relative to the EKF origin (m)
boolusingMinHypothesis;// true when the min beacob constellatio offset hyopthesis is being used
floatbcnPosDownOffsetMax;// Vertical position offset of the beacon constellation origin relative to the EKF origin (m)
floatbcnPosOffsetMaxVar;// Variance of the bcnPosDownOffsetMax state (m)
floatOffsetMaxInnovFilt;// Filtered magnitude of the range innovations using bcnPosOffsetHigh
floatbcnPosDownOffsetMin;// Vertical position offset of the beacon constellation origin relative to the EKF origin (m)
floatbcnPosOffsetMinVar;// Variance of the bcnPosDownOffsetMin state (m)
floatOffsetMinInnovFilt;// Filtered magnitude of the range innovations using bcnPosOffsetLow
// Range Beacon Fusion Debug Reporting
uint8_trngBcnFuseDataReportIndex;// index of range beacon fusion data last reported
struct{
floatrng;// measured range to beacon (m)
floatinnov;// range innovation (m)
floatinnovVar;// innovation variance (m^2)
floattestRatio;// innovation consistency test ratio
Vector3fbeaconPosNED;// beacon NED position
}rngBcnFusionReport[10];
// height source selection logic
uint8_tactiveHgtSource;// integer defining active height source
// Movement detector
booltakeOffDetected;// true when takeoff for optical flow navigation has been detected
floatrngAtStartOfFlight;// range finder measurement at start of flight
uint32_ttimeAtArming_ms;// time in msec that the vehicle armed
// baro ground effect
boolexpectGndEffectTakeoff;// external state from ArduCopter - takeoff expected
uint32_ttakeoffExpectedSet_ms;// system time at which expectGndEffectTakeoff was set
boolexpectGndEffectTouchdown;// external state from ArduCopter - touchdown expected
uint32_ttouchdownExpectedSet_ms;// system time at which expectGndEffectTouchdown was set
floatmeaHgtAtTakeOff;// height measured at commencement of takeoff
// control of post takeoff magentic field and heading resets
boolfinalInflightYawInit;// true when the final post takeoff initialisation of yaw angle has been performed
boolfinalInflightMagInit;// true when the final post takeoff initialisation of magnetic field states been performed
boolmagStateResetRequest;// true if magnetic field states need to be reset using the magneteomter measurements
boolmagYawResetRequest;// true if the vehicle yaw and magnetic field states need to be reset using the magnetometer measurements
boolgpsYawResetRequest;// true if the vehicle yaw needs to be reset to the GPS course
floatposDownAtLastMagReset;// vertical position last time the mag states were reset (m)
floatyawInnovAtLastMagReset;// magnetic yaw innovation last time the yaw and mag field states were reset (rad)
QuaternionquatAtLastMagReset;// quaternion states last time the mag states were reset
// flags indicating severe numerical errors in innovation variance calculation for different fusion operations
struct{
boolbad_xmag:1;
boolbad_ymag:1;
boolbad_zmag:1;
boolbad_airspeed:1;
boolbad_sideslip:1;
boolbad_nvel:1;
boolbad_evel:1;
boolbad_dvel:1;
boolbad_npos:1;
boolbad_epos:1;
boolbad_dpos:1;
boolbad_yaw:1;
boolbad_decl:1;
boolbad_xflow:1;
boolbad_yflow:1;
boolbad_rngbcn:1;
}faultStatus;
// flags indicating which GPS quality checks are failing
struct{
boolbad_sAcc:1;
boolbad_hAcc:1;
boolbad_yaw:1;
boolbad_sats:1;
boolbad_VZ:1;
boolbad_horiz_drift:1;
boolbad_hdop:1;
boolbad_vert_vel:1;
boolbad_fix:1;
boolbad_horiz_vel:1;
}gpsCheckStatus;
// states held by magnetomter fusion across time steps
// magnetometer X,Y,Z measurements are fused across three time steps
// to level computational load as this is an expensive operation
struct{
ftypeq0;
ftypeq1;
ftypeq2;
ftypeq3;
ftypemagN;
ftypemagE;
ftypemagD;
ftypemagXbias;
ftypemagYbias;
ftypemagZbias;
uint8_tobsIndex;
Matrix3fDCM;
Vector3fMagPred;
ftypeR_MAG;
Vector9SH_MAG;
}mag_state;
// string representing last reason for prearm failure
