EKF: Allow optical flow use when on-ground with poor data quality

EKF/estimator_interface.cpp

@@ -303,7 +303,7 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl
 		// check quality metric
 		bool flow_quality_good = (flow->quality >= _params.flow_qual_min);
 
-		if (delta_time_good && flow_magnitude_good && flow_quality_good) {
+		if (delta_time_good && flow_magnitude_good && (flow_quality_good || !_control_status.flags.in_air)) {
 			flowSample optflow_sample_new;
 			// calculate the system time-stamp for the mid point of the integration period
 			optflow_sample_new.time_us = time_usec - _params.flow_delay_ms * 1000 - flow->dt / 2;
@@ -311,8 +311,16 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl
 			optflow_sample_new.quality = flow->quality;
 			// NOTE: the EKF uses the reverse sign convention to the flow sensor. EKF assumes positive LOS rate is produced by a RH rotation of the image about the sensor axis.
 			// copy the optical and gyro measured delta angles
-			optflow_sample_new.flowRadXY = - flow->flowdata;
 			optflow_sample_new.gyroXYZ = - flow->gyrodata;
+			if (flow_quality_good) {
+				optflow_sample_new.flowRadXY = - flow->flowdata;
+
+			} else {
+				// when on the ground with poor flow quality, assume zero ground relative velocity
+				optflow_sample_new.flowRadXY(0) = + flow->gyrodata(0);
+				optflow_sample_new.flowRadXY(1) = + flow->gyrodata(1);
+
+			}
 			// compensate for body motion to give a LOS rate
 			optflow_sample_new.flowRadXYcomp(0) = optflow_sample_new.flowRadXY(0) - optflow_sample_new.gyroXYZ(0);
 			optflow_sample_new.flowRadXYcomp(1) = optflow_sample_new.flowRadXY(1) - optflow_sample_new.gyroXYZ(1);