Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-03 04:03:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: integrate Source for alt

Browse Source
This commit is contained in:
Randy Mackay 2020-07-04 12:27:49 +09:00
parent c21d58ebea
commit 8931e50166
10 changed files with 60 additions and 66 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -189,13 +189,7 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = {
// Height measurement parameters
// @Param: ALT_SOURCE
// @DisplayName: Primary altitude sensor source
// @Description: Primary height sensor used by the EKF. If a sensor other than Baro is selected and becomes unavailable, then the Baro sensor will be used as a fallback. NOTE: the EK3_RNG_USE_HGT parameter can be used to switch to range-finder when close to the ground in conjunction with EK3_ALT_SOURCE = 0 or 2 (Baro or GPS). NOTE: Setting EK3_ALT_SOURCE = 4 uses external nav system data only if data is also being used for horizontal position
// @Values: 0:Use Baro, 1:Use Range Finder, 2:Use GPS, 3:Use Range Beacon, 4:Use External Nav
// @User: Advanced
// @RebootRequired: True
AP_GROUPINFO("ALT_SOURCE", 9, NavEKF3, _altSource, 0),
// 9 was ALT_SOURCE
// @Param: ALT_M_NSE
// @DisplayName: Altitude measurement noise (m)

1
libraries/AP_NavEKF3/AP_NavEKF3.h
View File

@ -460,7 +460,6 @@ private:
AP_Int8 _flowDelay_ms; // effective average delay of optical flow measurements rel to IMU (msec)
AP_Int16 _rngInnovGate; // Percentage number of standard deviations applied to range finder innovation consistency check
AP_Float _maxFlowRate; // Maximum flow rate magnitude that will be accepted by the filter
AP_Int8 _altSource; // Primary alt source. 0 = Baro, 1 = range finder, 2 = GPS, 3 = range beacons, 4 = external nav
AP_Float _rngNoise; // Range finder noise : m
AP_Int8 _gpsCheck; // Bitmask controlling which preflight GPS checks are bypassed
AP_Int8 _imuMask; // Bitmask of IMUs to instantiate EKF3 for

2
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -600,7 +600,7 @@ void NavEKF3_core::updateFilterStatus(void)
bool filterHealthy = healthy() && tiltAlignComplete && (yawAlignComplete || (!use_compass() && (PV_AidingMode != AID_ABSOLUTE)));
// If GPS height usage is specified, height is considered to be inaccurate until the GPS passes all checks
bool hgtNotAccurate = (frontend->_altSource == 2) && !validOrigin;
bool hgtNotAccurate = (frontend->_sources.getPosZSource() == AP_NavEKF_Source::SourceZ::GPS) && !validOrigin;
// set individual flags
filterStatus.flags.attitude = !stateStruct.quat.is_nan() && filterHealthy; // attitude valid (we need a better check)

4
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -765,11 +765,11 @@ void NavEKF3_core::correctEkfOriginHeight()
// calculate the variance of our a-priori estimate of the ekf origin height
float deltaTime = constrain_float(0.001f * (imuDataDelayed.time_ms - lastOriginHgtTime_ms), 0.0f, 1.0f);
if (activeHgtSource == HGT_SOURCE_BARO) {
if (activeHgtSource == AP_NavEKF_Source::SourceZ::BARO) {
// Use the baro drift rate
const float baroDriftRate = 0.05f;
ekfOriginHgtVar += sq(baroDriftRate * deltaTime);
} else if (activeHgtSource == HGT_SOURCE_RNG) {
} else if (activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER) {
// use the worse case expected terrain gradient and vehicle horizontal speed
const float maxTerrGrad = 0.25f;
ekfOriginHgtVar += sq(maxTerrGrad * norm(stateStruct.velocity.x , stateStruct.velocity.y) * deltaTime);

4
libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp
View File

@ -32,7 +32,7 @@ void NavEKF3_core::SelectFlowFusion()
// Check if the optical flow data is still valid
flowDataValid = ((imuSampleTime_ms - flowValidMeaTime_ms) < 1000);
// check is the terrain offset estimate is still valid - if we are using range finder as the main height reference, the ground is assumed to be at 0
gndOffsetValid = ((imuSampleTime_ms - gndHgtValidTime_ms) < 5000) || (activeHgtSource == HGT_SOURCE_RNG);
gndOffsetValid = ((imuSampleTime_ms - gndHgtValidTime_ms) < 5000) || (activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER);
// Perform tilt check
bool tiltOK = (prevTnb.c.z > frontend->DCM33FlowMin);
// Constrain measurements to zero if takeoff is not detected and the height above ground
@ -80,7 +80,7 @@ void NavEKF3_core::EstimateTerrainOffset()
|| velHorizSq < 25.0f
|| (MAX(ofDataDelayed.flowRadXY[0],ofDataDelayed.flowRadXY[1]) > frontend->_maxFlowRate));
if ((!rangeDataToFuse && cantFuseFlowData) || (activeHgtSource == HGT_SOURCE_RNG)) {
if ((!rangeDataToFuse && cantFuseFlowData) || (activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER)) {
// skip update
inhibitGndState = true;
} else {

4
libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
View File

@ -128,7 +128,7 @@ bool NavEKF3_core::getHeightControlLimit(float &height) const
}
height = MAX(float(_rng->max_distance_cm_orient(ROTATION_PITCH_270)) * 0.007f - 1.0f, 1.0f);
// If we are are not using the range finder as the height reference, then compensate for the difference between terrain and EKF origin
if (frontend->_altSource != 1) {
if (frontend->_sources.getPosZSource() != AP_NavEKF_Source::SourceZ::RANGEFINDER) {
height -= terrainState;
}
return true;
@ -635,7 +635,7 @@ void NavEKF3_core::send_status_report(mavlink_channel_t chan) const
// height estimation or optical flow operation. This prevents false alarms at the GCS if a
// range finder is fitted for other applications
float temp;
if (((frontend->_useRngSwHgt > 0) && activeHgtSource == HGT_SOURCE_RNG) || (PV_AidingMode == AID_RELATIVE && flowDataValid)) {
if (((frontend->_useRngSwHgt > 0) && activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER) || (PV_AidingMode == AID_RELATIVE && flowDataValid)) {
temp = sqrtf(auxRngTestRatio);
} else {
temp = 0.0f;

80
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -247,7 +247,7 @@ void NavEKF3_core::ResetHeight(void)
// Check that GPS vertical velocity data is available and can be used
if (inFlight && !gpsNotAvailable && (frontend->_sources.getVelZSource() == AP_NavEKF_Source::SourceZ::GPS) && !frontend->inhibitGpsVertVelUse) {
stateStruct.velocity.z = gpsDataNew.vel.z;
} else if (inFlight && useExtNavVel && (activeHgtSource == HGT_SOURCE_EXTNAV)) {
} else if (inFlight && useExtNavVel && (activeHgtSource == AP_NavEKF_Source::SourceZ::EXTNAV)) {
stateStruct.velocity.z = extNavVelDelayed.vel.z;
} else if (onGround) {
stateStruct.velocity.z = 0.0f;
@ -275,7 +275,7 @@ void NavEKF3_core::ResetHeight(void)
// Return true if the height datum reset has been performed
bool NavEKF3_core::resetHeightDatum(void)
{
if (activeHgtSource == HGT_SOURCE_RNG || !onGround) {
if (activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER || !onGround) {
// only allow resets when on the ground.
// If using using rangefinder for height then never perform a
// reset of the height datum
@ -489,7 +489,7 @@ void NavEKF3_core::SelectVelPosFusion()
ResetPositionNE(gpsDataDelayed.pos.x, gpsDataDelayed.pos.y);
// If we are also using GPS as the height reference, reset the height
if (activeHgtSource == HGT_SOURCE_GPS) {
if (activeHgtSource == AP_NavEKF_Source::SourceZ::GPS) {
ResetPositionD(-hgtMea);
}
}
@ -499,7 +499,7 @@ void NavEKF3_core::SelectVelPosFusion()
// mark a source reset as consumed
pos_source_reset = false;
ResetPositionNE(extNavDataDelayed.pos.x, extNavDataDelayed.pos.y);
if (activeHgtSource == HGT_SOURCE_EXTNAV) {
if (activeHgtSource == AP_NavEKF_Source::SourceZ::EXTNAV) {
ResetPositionD(-hgtMea);
}
}
@ -635,7 +635,7 @@ void NavEKF3_core::FuseVelPosNED()
// if vertical GPS velocity data and an independent height source is being used, check to see if the GPS vertical velocity and altimeter
// innovations have the same sign and are outside limits. If so, then it is likely aliasing is affecting
// the accelerometers and we should disable the GPS and barometer innovation consistency checks.
if (useGpsVertVel && fuseVelData && (frontend->_altSource != 2)) {
if (useGpsVertVel && fuseVelData && (frontend->_sources.getPosZSource() != AP_NavEKF_Source::SourceZ::GPS)) {
// calculate innovations for height and vertical GPS vel measurements
const float hgtErr = stateStruct.position.z - velPosObs[5];
const float velDErr = stateStruct.velocity.z - velPosObs[2];
@ -810,7 +810,7 @@ void NavEKF3_core::FuseVelPosNED()
const float gndMaxBaroErr = 4.0f;
const float gndBaroInnovFloor = -0.5f;
if(expectGndEffectTouchdown && activeHgtSource == HGT_SOURCE_BARO) {
if (expectGndEffectTouchdown && activeHgtSource == AP_NavEKF_Source::SourceZ::BARO) {
// when a touchdown is expected, floor the barometer innovation at gndBaroInnovFloor
// constrain the correction between 0 and gndBaroInnovFloor+gndMaxBaroErr
// this function looks like this:
@ -970,13 +970,13 @@ void NavEKF3_core::selectHeightForFusion()
bool rangeFinderDataIsFresh = (imuSampleTime_ms - rngValidMeaTime_ms < 500);
const bool extNavDataIsFresh = (imuSampleTime_ms - extNavMeasTime_ms < 500);
// select height source
if (extNavUsedForPos && (frontend->_altSource == 4)) {
if (extNavUsedForPos && (frontend->_sources.getPosZSource() == AP_NavEKF_Source::SourceZ::EXTNAV)) {
// always use external navigation as the height source if using for position.
activeHgtSource = HGT_SOURCE_EXTNAV;
} else if ((frontend->_altSource == 1) && _rng && rangeFinderDataIsFresh) {
activeHgtSource = AP_NavEKF_Source::SourceZ::EXTNAV;
} else if ((frontend->_sources.getPosZSource() == AP_NavEKF_Source::SourceZ::RANGEFINDER) && _rng && rangeFinderDataIsFresh) {
// user has specified the range finder as a primary height source
activeHgtSource = HGT_SOURCE_RNG;
} else if ((frontend->_useRngSwHgt > 0) && ((frontend->_altSource == 0) || (frontend->_altSource == 2)) && _rng && rangeFinderDataIsFresh) {
activeHgtSource = AP_NavEKF_Source::SourceZ::RANGEFINDER;
} else if ((frontend->_useRngSwHgt > 0) && ((frontend->_sources.getPosZSource() == AP_NavEKF_Source::SourceZ::BARO) || (frontend->_sources.getPosZSource() == AP_NavEKF_Source::SourceZ::GPS)) && _rng && rangeFinderDataIsFresh) {
// determine if we are above or below the height switch region
float rangeMaxUse = 1e-4f * (float)_rng->max_distance_cm_orient(ROTATION_PITCH_270) * (float)frontend->_useRngSwHgt;
bool aboveUpperSwHgt = (terrainState - stateStruct.position.z) > rangeMaxUse;
@ -1003,40 +1003,40 @@ void NavEKF3_core::selectHeightForFusion()
* hysteresis to avoid rapid switching. Using range finder for height requires a consistent terrain height
* which cannot be assumed if the vehicle is moving horizontally.
*/
if ((aboveUpperSwHgt || dontTrustTerrain) && (activeHgtSource == HGT_SOURCE_RNG)) {
if ((aboveUpperSwHgt || dontTrustTerrain) && (activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER)) {
// cannot trust terrain or range finder so stop using range finder height
if (frontend->_altSource == 0) {
activeHgtSource = HGT_SOURCE_BARO;
} else if (frontend->_altSource == 2) {
activeHgtSource = HGT_SOURCE_GPS;
if (frontend->_sources.getPosZSource() == AP_NavEKF_Source::SourceZ::BARO) {
activeHgtSource = AP_NavEKF_Source::SourceZ::BARO;
} else if (frontend->_sources.getPosZSource() == AP_NavEKF_Source::SourceZ::GPS) {
activeHgtSource = AP_NavEKF_Source::SourceZ::GPS;
}
} else if (belowLowerSwHgt && trustTerrain && (prevTnb.c.z >= 0.7f)) {
// reliable terrain and range finder so start using range finder height
activeHgtSource = HGT_SOURCE_RNG;
activeHgtSource = AP_NavEKF_Source::SourceZ::RANGEFINDER;
}
} else if (frontend->_altSource == 0) {
activeHgtSource = HGT_SOURCE_BARO;
} else if ((frontend->_altSource == 2) && ((imuSampleTime_ms - lastTimeGpsReceived_ms) < 500) && validOrigin && gpsAccuracyGood) {
activeHgtSource = HGT_SOURCE_GPS;
} else if ((frontend->_altSource == 3) && validOrigin && rngBcnGoodToAlign) {
activeHgtSource = HGT_SOURCE_BCN;
} else if ((frontend->_altSource == 4) && extNavDataIsFresh) {
activeHgtSource = HGT_SOURCE_EXTNAV;
} else if (frontend->_sources.getPosZSource() == AP_NavEKF_Source::SourceZ::BARO) {
activeHgtSource = AP_NavEKF_Source::SourceZ::BARO;
} else if ((frontend->_sources.getPosZSource() == AP_NavEKF_Source::SourceZ::GPS) && ((imuSampleTime_ms - lastTimeGpsReceived_ms) < 500) && validOrigin && gpsAccuracyGood) {
activeHgtSource = AP_NavEKF_Source::SourceZ::GPS;
} else if ((frontend->_sources.getPosZSource() == AP_NavEKF_Source::SourceZ::BEACON) && validOrigin && rngBcnGoodToAlign) {
activeHgtSource = AP_NavEKF_Source::SourceZ::BEACON;
} else if ((frontend->_sources.getPosZSource() == AP_NavEKF_Source::SourceZ::EXTNAV) && extNavDataIsFresh) {
activeHgtSource = AP_NavEKF_Source::SourceZ::EXTNAV;
}
// Use Baro alt as a fallback if we lose range finder, GPS, external nav or Beacon
bool lostRngHgt = ((activeHgtSource == HGT_SOURCE_RNG) && !rangeFinderDataIsFresh);
bool lostGpsHgt = ((activeHgtSource == HGT_SOURCE_GPS) && ((imuSampleTime_ms - lastTimeGpsReceived_ms) > 2000));
bool lostExtNavHgt = ((activeHgtSource == HGT_SOURCE_EXTNAV) && !extNavDataIsFresh);
bool lostRngBcnHgt = ((activeHgtSource == HGT_SOURCE_BCN) && ((imuSampleTime_ms - rngBcnDataDelayed.time_ms) > 2000));
bool lostRngHgt = ((activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER) && !rangeFinderDataIsFresh);
bool lostGpsHgt = ((activeHgtSource == AP_NavEKF_Source::SourceZ::GPS) && ((imuSampleTime_ms - lastTimeGpsReceived_ms) > 2000));
bool lostExtNavHgt = ((activeHgtSource == AP_NavEKF_Source::SourceZ::EXTNAV) && !extNavDataIsFresh);
bool lostRngBcnHgt = ((activeHgtSource == AP_NavEKF_Source::SourceZ::BEACON) && ((imuSampleTime_ms - rngBcnDataDelayed.time_ms) > 2000));
if (lostRngHgt || lostGpsHgt || lostExtNavHgt || lostRngBcnHgt) {
activeHgtSource = HGT_SOURCE_BARO;
activeHgtSource = AP_NavEKF_Source::SourceZ::BARO;
}
// if there is new baro data to fuse, calculate filtered baro data required by other processes
if (baroDataToFuse) {
// calculate offset to baro data that enables us to switch to Baro height use during operation
if (activeHgtSource != HGT_SOURCE_BARO) {
if (activeHgtSource != AP_NavEKF_Source::SourceZ::BARO) {
calcFiltBaroOffset();
}
// filtered baro data used to provide a reference for takeoff
@ -1053,19 +1053,19 @@ void NavEKF3_core::selectHeightForFusion()
// combined local NED position height and origin height remains consistent with the GPS altitude
// This also enables the GPS height to be used as a backup height source
if (gpsDataToFuse &&
(((frontend->_originHgtMode & (1 << 0)) && (activeHgtSource == HGT_SOURCE_BARO)) ||
((frontend->_originHgtMode & (1 << 1)) && (activeHgtSource == HGT_SOURCE_RNG)))
(((frontend->_originHgtMode & (1 << 0)) && (activeHgtSource == AP_NavEKF_Source::SourceZ::BARO)) ||
((frontend->_originHgtMode & (1 << 1)) && (activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER)))
) {
correctEkfOriginHeight();
}
// Select the height measurement source
if (extNavDataToFuse && (activeHgtSource == HGT_SOURCE_EXTNAV)) {
if (extNavDataToFuse && (activeHgtSource == AP_NavEKF_Source::SourceZ::EXTNAV)) {
hgtMea = -extNavDataDelayed.pos.z;
velPosObs[5] = -hgtMea;
posDownObsNoise = sq(constrain_float(extNavDataDelayed.posErr, 0.1f, 10.0f));
fuseHgtData = true;
} else if (rangeDataToFuse && (activeHgtSource == HGT_SOURCE_RNG)) {
} else if (rangeDataToFuse && (activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER)) {
// using range finder data
// correct for tilt using a flat earth model
if (prevTnb.c.z >= 0.7) {
@ -1084,7 +1084,7 @@ void NavEKF3_core::selectHeightForFusion()
// disable fusion if tilted too far
fuseHgtData = false;
}
} else if (gpsDataToFuse && (activeHgtSource == HGT_SOURCE_GPS)) {
} else if (gpsDataToFuse && (activeHgtSource == AP_NavEKF_Source::SourceZ::GPS)) {
// using GPS data
hgtMea = gpsDataDelayed.hgt;
velPosObs[5] = -hgtMea;
@ -1096,7 +1096,7 @@ void NavEKF3_core::selectHeightForFusion()
} else {
posDownObsNoise = sq(constrain_float(1.5f * frontend->_gpsHorizPosNoise, 0.1f, 10.0f));
}
} else if (baroDataToFuse && (activeHgtSource == HGT_SOURCE_BARO)) {
} else if (baroDataToFuse && (activeHgtSource == AP_NavEKF_Source::SourceZ::BARO)) {
// using Baro data
hgtMea = baroDataDelayed.hgt - baroHgtOffset;
// enable fusion
@ -1117,6 +1117,12 @@ void NavEKF3_core::selectHeightForFusion()
fuseHgtData = false;
}
// detect changes in source and reset height
if ((activeHgtSource != prevHgtSource) && fuseHgtData) {
prevHgtSource = activeHgtSource;
ResetPositionD(-hgtMea);
}
// If we haven't fused 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_ms = ((useGpsVertVel || useExtNavVel) && !velTimeout) ? frontend->hgtRetryTimeMode0_ms : frontend->hgtRetryTimeMode12_ms;

10
libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp
View File

@ -54,7 +54,7 @@ void NavEKF3_core::FuseRngBcn()
// health is set bad until test passed
rngBcnHealth = false;
if (activeHgtSource != HGT_SOURCE_BCN) {
if (activeHgtSource != AP_NavEKF_Source::SourceZ::BEACON) {
// calculate the vertical offset from EKF datum to beacon datum
CalcRangeBeaconPosDownOffset(R_BCN, stateStruct.position, false);
} else {
@ -96,7 +96,7 @@ void NavEKF3_core::FuseRngBcn()
// are at the same height as the flight vehicle when calculating the observation derivatives
// and Kalman gains
// TODO - less hacky way of achieving this, preferably using an alternative derivation
if (activeHgtSource != HGT_SOURCE_BCN) {
if (activeHgtSource != AP_NavEKF_Source::SourceZ::BEACON) {
t2 = 0.0f;
}
H_BCN[9] = -t2*t9;
@ -158,7 +158,7 @@ void NavEKF3_core::FuseRngBcn()
}
// only allow the range observations to modify the vertical states if we are using it as a height reference
if (activeHgtSource == HGT_SOURCE_BCN) {
if (activeHgtSource == AP_NavEKF_Source::SourceZ::BEACON) {
Kfusion[6] = -t26*(P[6][7]*t4*t9+P[6][8]*t3*t9+P[6][9]*t2*t9);
Kfusion[9] = -t26*(t10+P[9][7]*t4*t9+P[9][8]*t3*t9);
} else {
@ -324,7 +324,7 @@ void NavEKF3_core::FuseRngBcnStatic()
}
if (rngBcnAlignmentCompleted) {
if (activeHgtSource != HGT_SOURCE_BCN) {
if (activeHgtSource != AP_NavEKF_Source::SourceZ::BEACON) {
// We are using a different height reference for the main EKF so need to estimate a vertical
// position offset to be applied to the beacon system that minimises the range innovations
// The position estimate should be stable after 100 iterations so we use a simple dual
@ -341,7 +341,7 @@ void NavEKF3_core::FuseRngBcnStatic()
}
} else {
if (activeHgtSource != HGT_SOURCE_BCN) {
if (activeHgtSource != AP_NavEKF_Source::SourceZ::BEACON) {
// The position estimate is not yet stable so we cannot run the 1-state EKF to estimate
// beacon system vertical position offset. Instead we initialise the dual hypothesis offset states
// using the beacon vertical position, vertical position estimate relative to the beacon origin

3
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -317,7 +317,8 @@ void NavEKF3_core::InitialiseVariables()
delAngBiasLearned = false;
memset(&filterStatus, 0, sizeof(filterStatus));
gpsInhibit = false;
activeHgtSource = 0;
activeHgtSource = AP_NavEKF_Source::SourceZ::BARO;
prevHgtSource = activeHgtSource;
memset(&rngMeasIndex, 0, sizeof(rngMeasIndex));
memset(&storedRngMeasTime_ms, 0, sizeof(storedRngMeasTime_ms));
memset(&storedRngMeas, 0, sizeof(storedRngMeas));

10
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -46,13 +46,6 @@
#define MASK_GPS_VERT_SPD (1<<6)
#define MASK_GPS_HORIZ_SPD (1<<7)
// active height source
#define HGT_SOURCE_BARO 0
#define HGT_SOURCE_RNG 1
#define HGT_SOURCE_GPS 2
#define HGT_SOURCE_BCN 3
#define HGT_SOURCE_EXTNAV 4
#define earthRate 0.000072921f // earth rotation rate (rad/sec)
// maximum allowed gyro bias (rad/sec)
@ -1306,7 +1299,8 @@ private:
} rngBcnFusionReport[4];
// height source selection logic
uint8_t activeHgtSource; // integer defining active height source
AP_NavEKF_Source::SourceZ activeHgtSource; // active height source
AP_NavEKF_Source::SourceZ prevHgtSource; // previous height source used to detect changes in source
// Movement detector
bool takeOffDetected; // true when takeoff for optical flow navigation has been detected