From 212022fc00b3ee72fc6a6f97ae03a27dae31beda Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 2 Sep 2014 23:23:23 +0200 Subject: [PATCH] flightgear: restrict distance sensor to sane values --- src/comm/QGCFlightGearLink.cc | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/comm/QGCFlightGearLink.cc b/src/comm/QGCFlightGearLink.cc index 692dddc31..9bc8a7aa2 100644 --- a/src/comm/QGCFlightGearLink.cc +++ b/src/comm/QGCFlightGearLink.cc @@ -432,7 +432,11 @@ void QGCFlightGearLink::readBytes() // qDebug() << "sensorHilGpsChanged " << lat << lon << alt << vel; // Send Optical Flow message. For now we set the flow quality to 0 and just write the ground_distance field - float distanceMeasurement = (float)(1.0/cosPhi * 1.0/cosThe * alt_agl); //asuming planar ground + float distanceMeasurement = -1.0; // -1 means invalid value + if (fabsf(roll) < 0.87 && fabsf(pitch) < 0.87) // return a valid value only for decent angles + { + distanceMeasurement = fabsf((float)(1.0/cosPhi * 1.0/cosThe * alt_agl)); // assuming planar ground + } emit sensorHilOpticalFlowChanged(QGC::groundTimeUsecs(), 0, 0, 0.0f, 0.0f, 0.0f, distanceMeasurement); } else { -- 2.22.0