Merge pull request #334 from PX4/pr-ekf_minor_flow_fix

ekf: fix optical flow bugs

EKF/control.cpp

@@ -300,6 +300,7 @@ void Ekf::controlOpticalFlowFusion()
 				// set the flag and reset the fusion timeout
 				_control_status.flags.opt_flow = true;
 				_time_last_of_fuse = _time_last_imu;
+				ECL_INFO("EKF Starting Optical Flow Use");
 
 				// if we are not using GPS then the velocity and position states and covariances need to be set
 				if (!_control_status.flags.gps) {
@@ -365,26 +366,12 @@ void Ekf::controlOpticalFlowFusion()
 
 		}
 
-		// handle the case when we are relying on optical flow fusion and lose it
-		if (_control_status.flags.opt_flow && !_control_status.flags.gps && !_control_status.flags.ev_pos) {
-			// We are relying on flow aiding to constrain attitude drift so after 5s without aiding we need to do something
-			if ((_time_last_imu - _time_last_of_fuse > 5e6)) {
-				// Switch to the non-aiding mode, zero the velocity states
-				// and set the synthetic position to the current estimate
-				_control_status.flags.opt_flow = false;
-				_last_known_posNE(0) = _state.pos(0);
-				_last_known_posNE(1) = _state.pos(1);
-				_state.vel.setZero();
-
-			}
-		}
-
 		// Accumulate autopilot gyro data across the same time interval as the flow sensor
 		_imu_del_ang_of += _imu_sample_delayed.delta_ang - _state.gyro_bias;
 		_delta_time_of += _imu_sample_delayed.delta_ang_dt;
 
-		// fuse the data
-		if (_control_status.flags.opt_flow) {
+		// fuse the data if the terrain/distance to bottom is valid
+		if (_control_status.flags.opt_flow && get_terrain_valid()) {
 			// Update optical flow bias estimates
 			calcOptFlowBias();
 
@@ -395,6 +382,21 @@ void Ekf::controlOpticalFlowFusion()
 
 		}
 	}
+
+	// handle the case when we are relying on optical flow fusion and lose it
+	if (_control_status.flags.opt_flow && !_control_status.flags.gps && !_control_status.flags.ev_pos) {
+		// We are relying on flow aiding to constrain attitude drift so after 5s without aiding we need to do something
+		if ((_time_last_imu - _time_last_of_fuse > 5e6)) {
+			// Switch to the non-aiding mode, zero the velocity states
+			// and set the synthetic position to the current estimate
+			_control_status.flags.opt_flow = false;
+			_last_known_posNE(0) = _state.pos(0);
+			_last_known_posNE(1) = _state.pos(1);
+			_state.vel.setZero();
+			ECL_WARN("EKF Stopping Optical Flow Use");
+		}
+	}
+
 }
 
 void Ekf::controlGpsFusion()

EKF/estimator_interface.cpp

@@ -278,11 +278,17 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl
 		// check if enough integration time and fail if integration time is less than 50%
 		// of min arrival interval because too much data is being lost
 		float delta_time = 1e-6f * (float)flow->dt;
-		bool delta_time_good = (delta_time >= 5e-7f * (float)_min_obs_interval_us);
+		float delta_time_min = 5e-7f * (float)_min_obs_interval_us;
+		bool delta_time_good = delta_time >= delta_time_min;
+		if (!delta_time_good) {
+			// protect against overflow casued by division with very small delta_time
+			delta_time = delta_time_min;
+		}
+
 
 		// check magnitude is within sensor limits
 		float flow_rate_magnitude;
-		bool flow_magnitude_good = false;
+		bool flow_magnitude_good = true;
 
 		if (delta_time_good) {
 			flow_rate_magnitude = flow->flowdata.norm() / delta_time;
@@ -292,7 +298,9 @@ 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 || !_control_status.flags.in_air)) {
+		// Always use data when on ground to allow for bad quality due to unfocussed sensors and operator handling
+		// If flow quality fails checks on ground, assume zero flow rate after body rate compensation
+		if ((delta_time_good && flow_quality_good && flow_magnitude_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;
@@ -307,8 +315,8 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl
 
 			} 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);
+				optflow_sample_new.flowRadXY(0) = - flow->gyrodata(0);
+				optflow_sample_new.flowRadXY(1) = - flow->gyrodata(1);
 
 			}
 
@@ -316,7 +324,7 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl
 			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);
 			// convert integration interval to seconds
-			optflow_sample_new.dt = 1e-6f * (float)flow->dt;
+			optflow_sample_new.dt = delta_time;
 			_time_last_optflow = time_usec;
 			// push to buffer
 			_flow_buffer.push(optflow_sample_new);

EKF/terrain_estimator.cpp

@@ -159,7 +159,7 @@ void Ekf::fuseHagl()
 bool Ekf::get_terrain_valid()
 {
 	if (_terrain_initialised && _range_data_continuous && !_rng_stuck &&
-		  !_innov_check_fail_status.flags.reject_hagl) {
+		  (_time_last_imu - _time_last_hagl_fuse < 5E6)) {
 		return true;
 
 	} else {