Update optical flow interface

EKF/common.h

@@ -73,13 +73,6 @@ struct gps_message {
 	float pdop;		///< position dilution of precision
 };
 
-struct flow_message {
-	uint8_t quality;	///< Quality of Flow data
-	Vector2f flowdata;	///< Optical flow rates about the X and Y body axes (rad/sec)
-	Vector3f gyrodata;	///< Gyro rates about the XYZ body axes (rad/sec)
-	uint32_t dt;		///< integration time of flow samples (microseconds)
-};
-
 struct outputSample {
 	Quatf  quat_nominal;	///< nominal quaternion describing vehicle attitude
 	Vector3f    vel;	///< NED velocity estimate in earth frame (m/sec)
@@ -137,8 +130,8 @@ struct airspeedSample {
 
 struct flowSample {
 	uint8_t  quality;	///< quality indicator between 0 and 255
-	Vector2f flowRadXY;	///< measured delta angle of the image about the X and Y body axes (rad), RH rotation is positive
+	Vector2f flow_xy_rad;	///< measured delta angle of the image about the X and Y body axes (rad), RH rotation is positive
-	Vector3f gyroXYZ;	///< measured delta angle of the inertial frame about the body axes obtained from rate gyro measurements (rad), RH rotation is positive
+	Vector3f gyro_xyz;	///< measured delta angle of the inertial frame about the body axes obtained from rate gyro measurements (rad), RH rotation is positive
 	float    dt;		///< amount of integration time (sec)
 	uint64_t time_us;	///< timestamp of the integration period leading edge (uSec)
 };

EKF/control.cpp

@@ -133,8 +133,9 @@ void Ekf::controlFusionModes()
 	}
 
 	// We don't fuse flow data immediately because we have to wait for the mid integration point to fall behind the fusion time horizon.
-	// This means we stop looking for new data until the old data has been fused.
+	// This means we stop looking for new data until the old data has been fused, unless we are not fusing optical flow,
-	if (!_flow_data_ready) {
+	// in this case we need to empty the buffer
+	if (!_flow_data_ready || !_control_status.flags.opt_flow) {
 		_flow_data_ready = _flow_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_flow_sample_delayed)
 				   && (_R_to_earth(2, 2) > _params.range_cos_max_tilt);
 	}
@@ -525,11 +526,11 @@ void Ekf::controlOpticalFlowFusion()
 
 			if (!flow_quality_good && !_control_status.flags.in_air) {
 				// when on the ground with poor flow quality, assume zero ground relative velocity and LOS rate
-				_flowRadXYcomp.zero();
+				_flow_compensated_XY_rad.zero();
 			} else {
 				// compensate for body motion to give a LOS rate
-				_flowRadXYcomp(0) = _flow_sample_delayed.flowRadXY(0) - _flow_sample_delayed.gyroXYZ(0);
+				_flow_compensated_XY_rad(0) = _flow_sample_delayed.flow_xy_rad(0) - _flow_sample_delayed.gyro_xyz(0);
-				_flowRadXYcomp(1) = _flow_sample_delayed.flowRadXY(1) - _flow_sample_delayed.gyroXYZ(1);
+				_flow_compensated_XY_rad(1) = _flow_sample_delayed.flow_xy_rad(1) - _flow_sample_delayed.gyro_xyz(1);
 			}
 		} else {
 			// don't use this flow data and wait for the next data to arrive

EKF/ekf.h

@@ -429,7 +429,7 @@ private:
 	uint64_t _time_bad_motion_us{0};	///< last system time that on-ground motion exceeded limits (uSec)
 	uint64_t _time_good_motion_us{0};	///< last system time that on-ground motion was within limits (uSec)
 	bool _inhibit_flow_use{false};	///< true when use of optical flow and range finder is being inhibited
-	Vector2f _flowRadXYcomp;	///< measured delta angle of the image about the X and Y body axes after removal of body rotation (rad), RH rotation is positive
+	Vector2f _flow_compensated_XY_rad;	///< measured delta angle of the image about the X and Y body axes after removal of body rotation (rad), RH rotation is positive
 
 	// output predictor states
 	Vector3f _delta_angle_corr;	///< delta angle correction vector (rad)

EKF/ekf_helper.cpp

@@ -73,8 +73,8 @@ bool Ekf::resetVelocity()
 			// we should have reliable OF measurements so
 			// calculate X and Y body relative velocities from OF measurements
 			Vector3f vel_optflow_body;
-			vel_optflow_body(0) = - range * _flowRadXYcomp(1) / _flow_sample_delayed.dt;
+			vel_optflow_body(0) = - range * _flow_compensated_XY_rad(1) / _flow_sample_delayed.dt;
-			vel_optflow_body(1) =   range * _flowRadXYcomp(0) / _flow_sample_delayed.dt;
+			vel_optflow_body(1) =   range * _flow_compensated_XY_rad(0) / _flow_sample_delayed.dt;
 			vel_optflow_body(2) = 0.0f;
 
 			// rotate from body to earth frame

EKF/estimator_interface.cpp

@@ -336,8 +336,7 @@ void EstimatorInterface::setRangeData(const rangeSample& range_sample)
 	}
 }
 
-// TODO: Change pointer to constant reference
+void EstimatorInterface::setOpticalFlowData(const flowSample& flow)
-void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *flow)
 {
 	if (!_initialised || _flow_buffer_fail) {
 		return;
@@ -355,10 +354,10 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl
 	}
 
 	// limit data rate to prevent data being lost
-	if ((time_usec - _time_last_optflow) > _min_obs_interval_us) {
+	if ((flow.time_us - _time_last_optflow) > _min_obs_interval_us) {
 		// 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; // in seconds
+		float delta_time = flow.dt; // in seconds
 		const float delta_time_min = 0.5e-6f * (float)_min_obs_interval_us;
 		bool delta_time_good = delta_time >= delta_time_min;
 
@@ -368,7 +367,7 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl
 			// check magnitude is within sensor limits
 			// use this to prevent use of a saturated flow sensor
 			// when there are other aiding sources available
-			const float flow_rate_magnitude = flow->flowdata.norm() / delta_time;
+			const float flow_rate_magnitude = flow.flow_xy_rad.norm() / delta_time;
 			flow_magnitude_good = (flow_rate_magnitude <= _flow_max_rate);
 		} else {
 			// protect against overflow caused by division with very small delta_time
@@ -377,25 +376,22 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl
 
 		const bool relying_on_flow = !_control_status.flags.gps && !_control_status.flags.ev_pos && !_control_status.flags.ev_vel;
 
-		const bool flow_quality_good = (flow->quality >= _params.flow_qual_min);
+		const bool flow_quality_good = (flow.quality >= _params.flow_qual_min);
 
 		// Check data validity and write to buffers
 		// Invalid flow data is allowed when on ground and is handled as a special case in controlOpticalFlowFusion()
 		bool use_flow_data_to_navigate = delta_time_good && flow_quality_good && (flow_magnitude_good || relying_on_flow);
 		if (use_flow_data_to_navigate || (!_control_status.flags.in_air && relying_on_flow)) {
-			flowSample optflow_sample_new;
-			// calculate the system time-stamp for the trailing edge of the flow data integration period
-			optflow_sample_new.time_us = time_usec - _params.flow_delay_ms * 1000;
 
-			optflow_sample_new.quality = flow->quality;
+			_time_last_optflow = flow.time_us;
 
-			// NOTE: the EKF uses the reverse sign convention to the flow sensor. EKF assumes positive LOS rate
+			flowSample optflow_sample_new = flow;
-			// is produced by a RH rotation of the image about the sensor axis.
+
-			optflow_sample_new.gyroXYZ = - flow->gyrodata;
+			// compensate time-stamp for the trailing edge of the flow data integration period
-			optflow_sample_new.flowRadXY = -flow->flowdata;
+			optflow_sample_new.time_us -= _params.flow_delay_ms * 1000;
+			optflow_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;
 
 			optflow_sample_new.dt = delta_time;
-			_time_last_optflow = time_usec;
 
 			_flow_buffer.push(optflow_sample_new);
 		}

EKF/estimator_interface.h

@@ -185,8 +185,8 @@ public:
 
 	void setRangeData(const rangeSample& range_sample);
 
-	// if optical flow sensor gyro delta angles are not available, set gyroXYZ vector fields to NaN and the EKF will use its internal delta angle data instead
+	// if optical flow sensor gyro delta angles are not available, set gyro_xyz vector fields to NaN and the EKF will use its internal delta angle data instead
-	void setOpticalFlowData(uint64_t time_usec, flow_message *flow);
+	void setOpticalFlowData(const flowSample& flow);
 
 	// set external vision position and attitude data
 	void setExtVisionData(const extVisionSample& evdata);

EKF/optflow_fusion.cpp

@@ -73,8 +73,8 @@ void Ekf::fuseOptFlow()
 	const Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body;
 
 	// calculate the velocity of the sensor relative to the imu in body frame
-	// Note: _flow_sample_delayed.gyroXYZ is the negative of the body angular velocity, thus use minus sign
+	// Note: _flow_sample_delayed.gyro_xyz is the negative of the body angular velocity, thus use minus sign
-	const Vector3f vel_rel_imu_body = Vector3f(-_flow_sample_delayed.gyroXYZ / _flow_sample_delayed.dt) % pos_offset_body;
+	const Vector3f vel_rel_imu_body = Vector3f(-_flow_sample_delayed.gyro_xyz / _flow_sample_delayed.dt) % pos_offset_body;
 
 	// calculate the velocity of the sensor in the earth frame
 	const Vector3f vel_rel_earth = _state.vel + _R_to_earth * vel_rel_imu_body;
@@ -102,8 +102,8 @@ void Ekf::fuseOptFlow()
 	// correct for gyro bias errors in the data used to do the motion compensation
 	// Note the sign convention used: A positive LOS rate is a RH rotation of the scene about that axis.
 	Vector2f opt_flow_rate;
-	opt_flow_rate(0) = _flowRadXYcomp(0) / _flow_sample_delayed.dt + _flow_gyro_bias(0);
+	opt_flow_rate(0) = _flow_compensated_XY_rad(0) / _flow_sample_delayed.dt + _flow_gyro_bias(0);
-	opt_flow_rate(1) = _flowRadXYcomp(1) / _flow_sample_delayed.dt + _flow_gyro_bias(1);
+	opt_flow_rate(1) = _flow_compensated_XY_rad(1) / _flow_sample_delayed.dt + _flow_gyro_bias(1);
 
 	if (opt_flow_rate.norm() < _flow_max_rate) {
 		_flow_innov[0] =  vel_body(1) / range - opt_flow_rate(0); // flow around the X axis
@@ -522,7 +522,7 @@ bool Ekf::calcOptFlowBodyRateComp()
 		return false;
 	}
 
-	const bool use_flow_sensor_gyro =  ISFINITE(_flow_sample_delayed.gyroXYZ(0)) && ISFINITE(_flow_sample_delayed.gyroXYZ(1)) && ISFINITE(_flow_sample_delayed.gyroXYZ(2));
+	const bool use_flow_sensor_gyro =  ISFINITE(_flow_sample_delayed.gyro_xyz(0)) && ISFINITE(_flow_sample_delayed.gyro_xyz(1)) && ISFINITE(_flow_sample_delayed.gyro_xyz(2));
 
 	if (use_flow_sensor_gyro) {
 
@@ -532,7 +532,7 @@ bool Ekf::calcOptFlowBodyRateComp()
 
 			const Vector3f reference_body_rate(_imu_del_ang_of * (1.0f / _delta_time_of));
 
-			const Vector3f measured_body_rate(_flow_sample_delayed.gyroXYZ * (1.0f / _flow_sample_delayed.dt));
+			const Vector3f measured_body_rate(_flow_sample_delayed.gyro_xyz * (1.0f / _flow_sample_delayed.dt));
 
 			// calculate the bias estimate using  a combined LPF and spike filter
 			_flow_gyro_bias = _flow_gyro_bias * 0.99f + matrix::constrain(measured_body_rate - reference_body_rate, -0.1f, 0.1f) * 0.01f;
@@ -541,7 +541,7 @@ bool Ekf::calcOptFlowBodyRateComp()
 	} else {
 		// Use the EKF gyro data if optical flow sensor gyro data is not available
 		// for clarification of the sign see definition of flowSample and imuSample in common.h
-		_flow_sample_delayed.gyroXYZ = -_imu_del_ang_of;
+		_flow_sample_delayed.gyro_xyz = -_imu_del_ang_of;
 		_flow_gyro_bias.zero();
 	}
 

EKF/terrain_estimator.cpp

@@ -187,7 +187,7 @@ void Ekf::fuseFlowForTerrain()
 	// calculate optical LOS rates using optical flow rates that have had the body angular rate contribution removed
 	// correct for gyro bias errors in the data used to do the motion compensation
 	// Note the sign convention used: A positive LOS rate is a RH rotation of the scene about that axis.
-	const Vector2f opt_flow_rate = Vector2f{_flowRadXYcomp} / _flow_sample_delayed.dt + Vector2f{_flow_gyro_bias};
+	const Vector2f opt_flow_rate = Vector2f{_flow_compensated_XY_rad} / _flow_sample_delayed.dt + Vector2f{_flow_gyro_bias};
 
 	// get latest estimated orientation
 	const float q0 = _state.quat_nominal(0);
@@ -205,8 +205,8 @@ void Ekf::fuseFlowForTerrain()
 	const Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body;
 
 	// calculate the velocity of the sensor relative to the imu in body frame
-	// Note: _flow_sample_delayed.gyroXYZ is the negative of the body angular velocity, thus use minus sign
+	// Note: _flow_sample_delayed.gyro_xyz is the negative of the body angular velocity, thus use minus sign
-	const Vector3f vel_rel_imu_body = Vector3f(-_flow_sample_delayed.gyroXYZ / _flow_sample_delayed.dt) % pos_offset_body;
+	const Vector3f vel_rel_imu_body = Vector3f(-_flow_sample_delayed.gyro_xyz / _flow_sample_delayed.dt) % pos_offset_body;
 
 	// calculate the velocity of the sensor in the earth frame
 	const Vector3f vel_rel_earth = _state.vel + _R_to_earth * vel_rel_imu_body;

test/sensor_simulator/flow.cpp

@@ -16,21 +16,22 @@ Flow::~Flow()
 void Flow::send(uint64_t time)
 {
 	_flow_data.dt = time - _time_last_data_sent;
-	_ekf->setOpticalFlowData(time, &_flow_data);
+	_flow_data.time_us = time;
+	_ekf->setOpticalFlowData(_flow_data);
 }
 
-void Flow::setData(const flow_message& flow)
+void Flow::setData(const flowSample& flow)
 {
 	_flow_data = flow;
 
 }
 
-flow_message Flow::dataAtRest()
+flowSample Flow::dataAtRest()
 {
-	flow_message _flow_at_rest;
+	flowSample _flow_at_rest;
-	_flow_at_rest.dt = 20000;
+	_flow_at_rest.dt = 0.02f;
-	_flow_at_rest.flowdata = Vector2f{0.0f, 0.0f};
+	_flow_at_rest.flow_xy_rad = Vector2f{0.0f, 0.0f};
-	_flow_at_rest.gyrodata = Vector3f{0.0f, 0.0f, 0.0f};
+	_flow_at_rest.gyro_xyz = Vector3f{0.0f, 0.0f, 0.0f};
 	_flow_at_rest.quality = 255;
 	return _flow_at_rest;
 }

test/sensor_simulator/flow.h

@@ -50,11 +50,11 @@ public:
 	Flow(std::shared_ptr<Ekf> ekf);
 	~Flow();
 
-	void setData(const flow_message& flow);
+	void setData(const flowSample& flow);
-	flow_message dataAtRest();
+	flowSample dataAtRest();
 
 private:
-	flow_message _flow_data;
+	flowSample _flow_data;
 
 	void send(uint64_t time) override;
 

test/sensor_simulator/range_finder.h

@@ -51,7 +51,6 @@ public:
 	~RangeFinder();
 
 	void setData(float range_data, int8_t range_quality);
-	flow_message dataAtRest();
 
 private:
 	rangeSample _range_sample {};

test/test_EKF_fusionLogic.cpp

@@ -161,8 +161,7 @@ TEST_F(EkfFusionLogicTest, doFlowFusion)
 
 	// WHEN: Flow data is not send and we enable flow fusion
 	_sensor_simulator.stopFlow();
-	_sensor_simulator.runSeconds(3); // TODO: without this line tests fail
+	_sensor_simulator.runSeconds(1); // empty buffer
-	// probably there are still values in the buffer.
 	_ekf_wrapper.enableFlowFusion();
 	_sensor_simulator.runSeconds(3);