ekf2: optical flow control always use provided flow gyro (with bias applied)

msg/VehicleOpticalFlowVel.msg

@@ -10,4 +10,6 @@ float32[2] flow_compensated_integral   # integrated optical flow measurement com
 float32[3] gyro_rate                   # gyro measurement synchronized with flow measurements (rad/s)
 float32[3] gyro_rate_integral          # gyro measurement integrated to flow rate and synchronized with flow measurements (rad)
 
+float32[3] gyro_bias
+
 # TOPICS estimator_optical_flow_vel vehicle_optical_flow_vel

src/modules/ekf2/EKF/ekf.h

@@ -183,6 +183,7 @@ public:
 
 	const Vector3f getFlowGyro() const { return _flow_sample_delayed.gyro_xyz * (1.f / _flow_sample_delayed.dt); }
 	const Vector3f &getFlowGyroIntegral() const { return _flow_sample_delayed.gyro_xyz; }
+	const Vector3f &getFlowGyroBias() const { return _flow_gyro_bias; }
 #endif // CONFIG_EKF2_OPTICAL_FLOW
 
 #if defined(CONFIG_EKF2_AUXVEL)

src/modules/ekf2/EKF/optflow_fusion.cpp

@@ -219,13 +219,13 @@ bool Ekf::calcOptFlowBodyRateComp()
 
 			// 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;
-
-			// apply gyro bias
-			_flow_sample_delayed.gyro_xyz -= (_flow_gyro_bias * _flow_sample_delayed.dt);
-
-			is_body_rate_comp_available = true;
 		}
 
+		// apply gyro bias
+		_flow_sample_delayed.gyro_xyz -= (_flow_gyro_bias * _flow_sample_delayed.dt);
+
+		is_body_rate_comp_available = true;
+
 	} 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

src/modules/ekf2/EKF2.cpp

@@ -2001,6 +2001,8 @@ void EKF2::PublishOpticalFlowVel(const hrt_abstime &timestamp)
 		_ekf.getFlowGyro().copyTo(flow_vel.gyro_rate);
 		_ekf.getFlowGyroIntegral().copyTo(flow_vel.gyro_rate_integral);
 
+		_ekf.getFlowGyroBias().copyTo(flow_vel.gyro_bias);
+
 		flow_vel.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 
 		_estimator_optical_flow_vel_pub.publish(flow_vel);