Skip to content

Commit 06e6823

Browse files
snktshrmasnktshrma
committed
AP_OpticalFlow: Use flow rates and add ground distance field in MAVLink driver
1 parent 297ec3f commit 06e6823

18 files changed

+116
-29
lines changed

‎libraries/AP_AHRS/AP_AHRS.cpp

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -2399,13 +2399,13 @@ bool AP_AHRS::get_filter_status(nav_filter_status &status) const
23992399
}
24002400

24012401
// write optical flow data to EKF
2402-
void AP_AHRS::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride)
2402+
void AP_AHRS::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const float rawGroundDistance, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride)
24032403
{
24042404
#if HAL_NAVEKF2_AVAILABLE
24052405
EKF2.writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas, posOffset, heightOverride);
24062406
#endif
24072407
#if EK3_FEATURE_OPTFLOW_FUSION
2408-
EKF3.writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas, posOffset, heightOverride);
2408+
EKF3.writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, rawGroundDistance, msecFlowMeas, posOffset, heightOverride);
24092409
#endif
24102410
}
24112411

‎libraries/AP_AHRS/AP_AHRS.h

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -291,7 +291,7 @@ class AP_AHRS {
291291
bool get_vert_pos_rate_D(float &velocity) const;
292292

293293
// write optical flow measurements to EKF
294-
void writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, const float heightOverride);
294+
void writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const float rawGroundDistance, const uint32_t msecFlowMeas, const Vector3f &posOffset, const float heightOverride);
295295

296296
// retrieve latest corrected optical flow samples (used for calibration)
297297
bool getOptFlowSample(uint32_t& timeStamp_ms, Vector2f& flowRate, Vector2f& bodyRate, Vector2f& losPred) const;

‎libraries/AP_DAL/AP_DAL.cpp

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -341,14 +341,15 @@ bool AP_DAL::ekf_low_time_remaining(EKFType etype, uint8_t core)
341341
}
342342

343343
// log optical flow data
344-
void AP_DAL::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride)
344+
void AP_DAL::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const float rawGroundDistance, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride)
345345
{
346346
end_frame();
347347

348348
const log_ROFH old = _ROFH;
349349
_ROFH.rawFlowQuality = rawFlowQuality;
350350
_ROFH.rawFlowRates = rawFlowRates;
351351
_ROFH.rawGyroRates = rawGyroRates;
352+
_ROFH.rawGroundDistance = rawGroundDistance;
352353
_ROFH.msecFlowMeas = msecFlowMeas;
353354
_ROFH.posOffset = posOffset;
354355
_ROFH.heightOverride = heightOverride;
@@ -474,7 +475,7 @@ void AP_DAL::handle_message(const log_ROFH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3)
474475
_ROFH = msg;
475476
ekf2.writeOptFlowMeas(msg.rawFlowQuality, msg.rawFlowRates, msg.rawGyroRates, msg.msecFlowMeas, msg.posOffset, msg.heightOverride);
476477
#if EK3_FEATURE_OPTFLOW_FUSION
477-
ekf3.writeOptFlowMeas(msg.rawFlowQuality, msg.rawFlowRates, msg.rawGyroRates, msg.msecFlowMeas, msg.posOffset, msg.heightOverride);
478+
ekf3.writeOptFlowMeas(msg.rawFlowQuality, msg.rawFlowRates, msg.rawGyroRates, msg.rawGroundDistance, msg.msecFlowMeas, msg.posOffset, msg.heightOverride);
478479
#endif
479480
}
480481

‎libraries/AP_DAL/AP_DAL.h

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -213,7 +213,7 @@ class AP_DAL {
213213
}
214214

215215
// log optical flow data
216-
void writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride);
216+
void writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const float rawGroundDistance, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride);
217217

218218
// log external nav data
219219
void writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms);

‎libraries/AP_DAL/LogStructure.h

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -320,6 +320,7 @@ struct log_RVOH {
320320
struct log_ROFH {
321321
Vector2f rawFlowRates;
322322
Vector2f rawGyroRates;
323+
float rawGroundDistance;
323324
uint32_t msecFlowMeas;
324325
Vector3f posOffset;
325326
float heightOverride;
@@ -440,7 +441,7 @@ struct log_RBOH {
440441
{ LOG_RVOH_MSG, RLOG_SIZE(RVOH), \
441442
"RVOH", "fffIBB", "OX,OY,OZ,Del,H,Ena", "------", "------" }, \
442443
{ LOG_ROFH_MSG, RLOG_SIZE(ROFH), \
443-
"ROFH", "ffffIffffB", "FX,FY,GX,GY,Tms,PX,PY,PZ,HgtOvr,Qual", "----------", "----------" }, \
444+
"ROFH", "fffffIffffB", "FX,FY,GX,GY,DG,Tms,PX,PY,PZ,HgtOvr,Qual", "-----------", "-----------" }, \
444445
{ LOG_REPH_MSG, RLOG_SIZE(REPH), \
445446
"REPH", "fffffffffIIH", "PX,PY,PZ,Q1,Q2,Q3,Q4,PEr,AEr,TS,RT,D", "------------", "------------" }, \
446447
{ LOG_RSLL_MSG, RLOG_SIZE(RSLL), \

‎libraries/AP_Logger/LogStructure.h

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -333,6 +333,7 @@ struct PACKED log_Optflow {
333333
float flow_y;
334334
float body_x;
335335
float body_y;
336+
float ground_distance;
336337
};
337338

338339
struct PACKED log_POWR {
@@ -825,6 +826,7 @@ struct PACKED log_VER {
825826
// @Field: flowY: Sensor flow rate,Y-axis
826827
// @Field: bodyX: derived rotational velocity, X-axis
827828
// @Field: bodyY: derived rotational velocity, Y-axis
829+
// @Field: groundDistance: altitude, Z-axis
828830

829831
// @LoggerMessage: PARM
830832
// @Description: parameter value
@@ -1234,7 +1236,7 @@ LOG_STRUCTURE_FROM_FENCE \
12341236
"MAV", "QBHHHBHH", "TimeUS,chan,txp,rxp,rxdp,flags,ss,tf", "s#----s-", "F-000-C-" }, \
12351237
LOG_STRUCTURE_FROM_VISUALODOM \
12361238
{ LOG_OPTFLOW_MSG, sizeof(log_Optflow), \
1237-
"OF", "QBffff", "TimeUS,Qual,flowX,flowY,bodyX,bodyY", "s-EEEE", "F-0000" , true }, \
1239+
"OF", "QBfffff", "TimeUS,Qual,flowX,flowY,bodyX,bodyY,groundDistance", "s-EEEEE", "F-00000" , true }, \
12381240
{ LOG_WHEELENCODER_MSG, sizeof(log_WheelEncoder), \
12391241
"WENC", "Qfbfb", "TimeUS,Dist0,Qual0,Dist1,Qual1", "sm-m-", "F0-0-" , true }, \
12401242
{ LOG_ADSB_MSG, sizeof(log_ADSB), \

‎libraries/AP_NavEKF2/AP_NavEKF2.cpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1216,7 +1216,7 @@ bool NavEKF2::use_compass(void) const
12161216
// heightOverride is the fixed height of the sensor above ground in m, when on rover vehicles. 0 if not used
12171217
void NavEKF2::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride)
12181218
{
1219-
AP::dal().writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas, posOffset, heightOverride);
1219+
AP::dal().writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, 0.0f, msecFlowMeas, posOffset, heightOverride);
12201220

12211221
if (core) {
12221222
for (uint8_t i=0; i<num_cores; i++) {

‎libraries/AP_NavEKF3/AP_NavEKF3.cpp

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -1556,13 +1556,13 @@ bool NavEKF3::configuredToUseGPSForPosXY(void) const
15561556
// posOffset is the XYZ flow sensor position in the body frame in m
15571557
// heightOverride is the fixed height of the sensor above ground in m, when on rover vehicles. 0 if not used
15581558
#if EK3_FEATURE_OPTFLOW_FUSION
1559-
void NavEKF3::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride)
1559+
void NavEKF3::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const float rawGroundDistance, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride)
15601560
{
1561-
dal.writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas, posOffset, heightOverride);
1561+
dal.writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, rawGroundDistance, msecFlowMeas, posOffset, heightOverride);
15621562

15631563
if (core) {
15641564
for (uint8_t i=0; i<num_cores; i++) {
1565-
core[i].writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas, posOffset, heightOverride);
1565+
core[i].writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, rawGroundDistance, msecFlowMeas, posOffset, heightOverride);
15661566
}
15671567
}
15681568
}

‎libraries/AP_NavEKF3/AP_NavEKF3.h

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -195,7 +195,7 @@ class NavEKF3 {
195195
// msecFlowMeas is the scheduler time in msec when the optical flow data was received from the sensor.
196196
// posOffset is the XYZ flow sensor position in the body frame in m
197197
// heightOverride is the fixed height of the sensor above ground in m, when on rover vehicles. 0 if not used
198-
void writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride);
198+
void writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const float rawGroundDistance, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride);
199199

200200
// retrieve latest corrected optical flow samples (used for calibration)
201201
bool getOptFlowSample(uint32_t& timeStamp_ms, Vector2f& flowRate, Vector2f& bodyRate, Vector2f& losPred) const;

‎libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -171,7 +171,7 @@ void NavEKF3_core::writeWheelOdom(float delAng, float delTime, uint32_t timeStam
171171
#if EK3_FEATURE_OPTFLOW_FUSION
172172
// write the raw optical flow measurements
173173
// this needs to be called externally.
174-
void NavEKF3_core::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride)
174+
void NavEKF3_core::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const float rawGroundDistance, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride)
175175
{
176176
// limit update rate to maximum allowed by sensor buffers
177177
if ((imuSampleTime_ms - flowMeaTime_ms) < frontend->sensorIntervalMin_ms) {
@@ -201,7 +201,6 @@ void NavEKF3_core::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f
201201
// don't use data with a low quality indicator or extreme rates (helps catch corrupt sensor data)
202202
if ((rawFlowQuality > 0) && rawFlowRates.length() < 4.2f && rawGyroRates.length() < 4.2f) {
203203
// correct flow sensor body rates for bias and write
204-
of_elements ofDataNew {};
205204
ofDataNew.bodyRadXYZ.x = rawGyroRates.x - flowGyroBias.x;
206205
ofDataNew.bodyRadXYZ.y = rawGyroRates.y - flowGyroBias.y;
207206
// the sensor interface doesn't provide a z axis rate so use the rate from the nav sensor instead
@@ -220,6 +219,8 @@ void NavEKF3_core::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f
220219
ofDataNew.flowRadXY = - rawFlowRates.toftype(); // raw (non motion compensated) optical flow angular rate about the X axis (rad/sec)
221220
// write the flow sensor position in body frame
222221
ofDataNew.body_offset = posOffset.toftype();
222+
// write the flow sensor's ground distance
223+
ofDataNew.ground_distance = rawGroundDistance;
223224
// write the flow sensor height override
224225
ofDataNew.heightOverride = heightOverride;
225226
// write flow rate measurements corrected for body rates

‎libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp

Lines changed: 60 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -39,7 +39,7 @@ void NavEKF3_core::SelectFlowFusion()
3939
// Check if the optical flow data is still valid
4040
flowDataValid = ((imuSampleTime_ms - flowValidMeaTime_ms) < 1000);
4141
// 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
42-
gndOffsetValid = ((imuSampleTime_ms - gndHgtValidTime_ms) < 5000) || (activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER);
42+
gndOffsetValid = ((imuSampleTime_ms - gndHgtValidTime_ms) < 5000) || (activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER) || (ofDataDelayed.ground_distance > 0);
4343
// Perform tilt check
4444
bool tiltOK = (prevTnb.c.z > frontend->DCM33FlowMin);
4545
// Constrain measurements to zero if takeoff is not detected and the height above ground
@@ -52,7 +52,7 @@ void NavEKF3_core::SelectFlowFusion()
5252
}
5353

5454
// if have valid flow or range measurements, fuse data into a 1-state EKF to estimate terrain height
55-
if (((flowDataToFuse && (frontend->_flowUse == FLOW_USE_TERRAIN)) || rangeDataToFuse) && tiltOK) {
55+
if (((flowDataToFuse && (frontend->_flowUse == FLOW_USE_TERRAIN)) || rangeDataToFuse || (ofDataDelayed.ground_distance > 0)) && tiltOK) {
5656
// Estimate the terrain offset (runs a one state EKF)
5757
EstimateTerrainOffset(ofDataDelayed);
5858
}
@@ -86,7 +86,10 @@ void NavEKF3_core::EstimateTerrainOffset(const of_elements &ofDataDelayed)
8686
|| velHorizSq < 25.0f
8787
|| (MAX(ofDataDelayed.flowRadXY[0],ofDataDelayed.flowRadXY[1]) > frontend->_maxFlowRate));
8888

89-
if ((!rangeDataToFuse && cantFuseFlowData) || (activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER)) {
89+
// check if ground distance data is reliable and skip update
90+
bool fuseGroundDist = ofDataDelayed.ground_distance > 0;
91+
92+
if ((!rangeDataToFuse && cantFuseFlowData) || (!fuseGroundDist && cantFuseFlowData) || (activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER)) {
9093
// skip update
9194
inhibitGndState = true;
9295
} else {
@@ -105,8 +108,59 @@ void NavEKF3_core::EstimateTerrainOffset(const of_elements &ofDataDelayed)
105108
Popt += Pincrement;
106109
timeAtLastAuxEKF_ms = imuSampleTime_ms;
107110

108-
// fuse range finder data
109-
if (rangeDataToFuse) {
111+
// fuse range finder or ground distance data
112+
if (fuseGroundDist) {
113+
// reset terrain state if ground distance data not fused for 5 seconds
114+
if (imuSampleTime_ms - gndHgtValidTime_ms > 5000) {
115+
terrainState = MAX(ofDataDelayed.ground_distance * prevTnb.c.z, rngOnGnd) + stateStruct.position.z;
116+
}
117+
118+
// predict range
119+
ftype predRngMeas = MAX((terrainState - stateStruct.position[2]),rngOnGnd) / prevTnb.c.z;
120+
// Copy required states to local variable names
121+
ftype q0 = stateStruct.quat[0]; // quaternion at optical flow measurement time
122+
ftype q1 = stateStruct.quat[1]; // quaternion at optical flow measurement time
123+
ftype q2 = stateStruct.quat[2]; // quaternion at optical flow measurement time
124+
ftype q3 = stateStruct.quat[3]; // quaternion at optical flow measurement time
125+
126+
// Set range finder measurement noise variance. TODO make this a function of range and tilt to allow for sensor, alignment and AHRS errors
127+
ftype R_RNG = frontend->_rngNoise.get();
128+
129+
// calculate Kalman gain
130+
ftype SK_RNG = sq(q0) - sq(q1) - sq(q2) + sq(q3);
131+
ftype K_RNG = Popt/(SK_RNG*(R_RNG + Popt/sq(SK_RNG)));
132+
133+
// Calculate the innovation variance for data logging
134+
varInnovRng = (R_RNG + Popt/sq(SK_RNG));
135+
136+
// constrain terrain height to be below the vehicle
137+
terrainState = MAX(terrainState, stateStruct.position[2] + rngOnGnd);
138+
139+
// Calculate the measurement innovation
140+
innovRng = predRngMeas - ofDataDelayed.ground_distance;
141+
142+
// calculate the innovation consistency test ratio
143+
auxRngTestRatio = sq(innovRng) / (sq(MAX(0.01f * (ftype)frontend->_rngInnovGate, 1.0f)) * varInnovRng);
144+
145+
// Check the innovation test ratio and don't fuse if too large
146+
if (auxRngTestRatio < 1.0f) {
147+
// correct the state
148+
terrainState -= K_RNG * innovRng;
149+
150+
// constrain the state
151+
terrainState = MAX(terrainState, stateStruct.position[2] + rngOnGnd);
152+
153+
// correct the covariance
154+
Popt = Popt - sq(Popt)/(SK_RNG*(R_RNG + Popt/sq(SK_RNG))*(sq(q0) - sq(q1) - sq(q2) + sq(q3)));
155+
156+
// prevent the state variance from becoming negative
157+
Popt = MAX(Popt,0.0f);
158+
159+
// record the time we last updated the terrain offset state
160+
gndHgtValidTime_ms = imuSampleTime_ms;
161+
}
162+
163+
} else if (rangeDataToFuse) {
110164
// reset terrain state if rangefinder data not fused for 5 seconds
111165
if (imuSampleTime_ms - gndHgtValidTime_ms > 5000) {
112166
terrainState = MAX(rangeDataDelayed.rng * prevTnb.c.z, rngOnGnd) + stateStruct.position.z;
@@ -121,7 +175,7 @@ void NavEKF3_core::EstimateTerrainOffset(const of_elements &ofDataDelayed)
121175
ftype q3 = stateStruct.quat[3]; // quaternion at optical flow measurement time
122176

123177
// Set range finder measurement noise variance. TODO make this a function of range and tilt to allow for sensor, alignment and AHRS errors
124-
ftype R_RNG = frontend->_rngNoise;
178+
ftype R_RNG = frontend->_rngNoise.get();
125179

126180
// calculate Kalman gain
127181
ftype SK_RNG = sq(q0) - sq(q1) - sq(q2) + sq(q3);

‎libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp

Lines changed: 13 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -394,6 +394,11 @@ void NavEKF3_core::detectFlight()
394394
if ((rangeDataNew.rng - rngAtStartOfFlight) < -0.5f) {
395395
inFlight = true;
396396
}
397+
398+
// If OF ground distance has decreased since arming, then we definitely are diving
399+
if ((ofDataNew.ground_distance - grndDistAtStartOfFlight) < -0.5f) {
400+
inFlight = true;
401+
}
397402
#else
398403
// If height has increased since exiting on-ground, then we definitely are flying
399404
if ((stateStruct.position.z - posDownAtTakeoff) < -1.5f) {
@@ -404,6 +409,11 @@ void NavEKF3_core::detectFlight()
404409
if ((rangeDataNew.rng - rngAtStartOfFlight) > 0.5f) {
405410
inFlight = true;
406411
}
412+
413+
// If OF ground distance has increased since exiting on-ground, then we definitely are flying
414+
if ((ofDataNew.ground_distance - grndDistAtStartOfFlight) > 0.5f) {
415+
inFlight = true;
416+
}
407417
#endif
408418

409419
// If more than 5 seconds since likely_flying was set
@@ -421,6 +431,8 @@ void NavEKF3_core::detectFlight()
421431
posDownAtTakeoff = stateStruct.position.z;
422432
// store the range finder measurement which will be used as a reference to detect when we have taken off
423433
rngAtStartOfFlight = rangeDataNew.rng;
434+
// store the mav optical flow ground distance measurement which will be used to detect when we have taken off
435+
grndDistAtStartOfFlight = ofDataNew.ground_distance;
424436
// if the magnetic field states have been set, then continue to update the vertical position
425437
// quaternion and yaw innovation snapshots to use as a reference when we start to fly.
426438
if (magStateInitComplete) {
@@ -473,7 +485,7 @@ void NavEKF3_core::detectOptFlowTakeoff(void)
473485
getGyroBias(gyroBias);
474486
angRateVec = ins.get_gyro(gyro_index_active) - gyroBias;
475487

476-
takeOffDetected = (takeOffDetected || (angRateVec.length() > 0.1f) || (rangeDataNew.rng > (rngAtStartOfFlight + 0.1f)));
488+
takeOffDetected = (takeOffDetected || (angRateVec.length() > 0.1f) || (rangeDataNew.rng > (rngAtStartOfFlight + 0.1f)) || ofDataNew.ground_distance > (grndDistAtStartOfFlight + 0.1f));
477489
} else if (onGround) {
478490
// we are confidently on the ground so set the takeoff detected status to false
479491
takeOffDetected = false;

0 commit comments

Comments
 (0)