OpticalFlow: add optical flow velocity logging

This is important to align the flow with the IMU data and verify that
the compensation is properly done

msg/CMakeLists.txt

@@ -56,6 +56,7 @@ set(msg_files
 	esc_report.msg
 	esc_status.msg
 	estimator_innovations.msg
+	estimator_optical_flow_vel.msg
 	estimator_sensor_bias.msg
 	estimator_states.msg
 	estimator_status.msg

msg/estimator_optical_flow_vel.msg

@@ -0,0 +1,8 @@
+uint64 timestamp			# time since system start (microseconds)
+uint64 timestamp_sample			# the timestamp of the raw data (microseconds)
+
+float32[2] vel_body			# velocity obtained from gyro-compensated and distance-scaled optical flow raw measurements in body frame(m/s)
+float32[2] vel_ne			# same as vel_body but in local frame (m/s)
+float32[2] flow_uncompensated_integral	# integrated optical flow measurement (rad)
+float32[2] flow_compensated_integral	# integrated optical flow measurement compensated for angular motion (rad)
+float32[3] gyro_rate_integral		# gyro measurement integrated to flow rate and synchronized with flow measurements (rad)

msg/tools/uorb_rtps_message_ids.yaml

@@ -285,6 +285,8 @@ rtps:
     id: 136
   - msg: navigator_mission_item
     id: 137
+  - msg: estimator_optical_flow_vel
+    id: 138
   ########## multi topics: begin ##########
   - msg: actuator_controls_0
     id: 150

src/lib/ecl

@@ -1 +1 @@
-Subproject commit f666ebb99552eed796e82a20bb60e813d45eee03
+Subproject commit dd3ffc419238bfc7d9b7e3be92a178c2cf7756f1

src/modules/ekf2/EKF2.cpp

@@ -159,6 +159,7 @@ EKF2::EKF2(bool replay_mode):
 	_estimator_innovation_test_ratios_pub.advertise();
 	_estimator_innovation_variances_pub.advertise();
 	_estimator_innovations_pub.advertise();
+	_estimator_optical_flow_vel_pub.advertise();
 	_estimator_sensor_bias_pub.advertise();
 	_estimator_states_pub.advertise();
 	_estimator_status_pub.advertise();
@@ -489,6 +490,7 @@ void EKF2::Run()
 		}
 
 		if (_optical_flow_sub.updated()) {
+			_new_optical_flow_data_received = true;
 			optical_flow_s optical_flow;
 
 			if (_optical_flow_sub.copy(&optical_flow)) {
@@ -1042,6 +1044,11 @@ void EKF2::Run()
 
 			publish_yaw_estimator_status(now);
 
+			if (_new_optical_flow_data_received) {
+				publish_estimator_optical_flow_vel(now);
+				_new_optical_flow_data_received = false;
+			}
+
 			if (!_mag_decl_saved && (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY)) {
 				_mag_decl_saved = update_mag_decl(_param_ekf2_mag_decl);
 			}
@@ -1333,6 +1340,21 @@ void EKF2::publish_wind_estimate(const hrt_abstime &timestamp)
 	}
 }
 
+void EKF2::publish_estimator_optical_flow_vel(const hrt_abstime &timestamp)
+{
+	estimator_optical_flow_vel_s flow_vel{};
+	flow_vel.timestamp_sample = timestamp;
+
+	_ekf.getFlowVelBody().copyTo(flow_vel.vel_body);
+	_ekf.getFlowVelNE().copyTo(flow_vel.vel_ne);
+	_ekf.getFlowUncompensated().copyTo(flow_vel.flow_uncompensated_integral);
+	_ekf.getFlowCompensated().copyTo(flow_vel.flow_compensated_integral);
+	_ekf.getFlowGyro().copyTo(flow_vel.gyro_rate_integral);
+	flow_vel.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
+
+	_estimator_optical_flow_vel_pub.publish(flow_vel);
+}
+
 float EKF2::filter_altitude_ellipsoid(float amsl_hgt)
 {
 	float height_diff = static_cast<float>(_gps_alttitude_ellipsoid) * 1e-3f - amsl_hgt;

src/modules/ekf2/EKF2.hpp

@@ -81,6 +81,7 @@
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/wind_estimate.h>
 #include <uORB/topics/yaw_estimator_status.h>
+#include <uORB/topics/estimator_optical_flow_vel.h>
 
 #include "Utility/PreFlightChecker.hpp"
 
@@ -125,6 +126,7 @@ private:
 	void publish_odometry(const hrt_abstime &timestamp, const imuSample &imu, const vehicle_local_position_s &lpos);
 	void publish_wind_estimate(const hrt_abstime &timestamp);
 	void publish_yaw_estimator_status(const hrt_abstime &timestamp);
+	void publish_estimator_optical_flow_vel(const hrt_abstime &timestamp);
 
 	/*
 	 * Calculate filtered WGS84 height from estimated AMSL height
@@ -169,6 +171,8 @@ private:
 	bool new_ev_data_received = false;
 	vehicle_odometry_s _ev_odom{};
 
+	bool _new_optical_flow_data_received{false};
+
 	uORB::Subscription _airdata_sub{ORB_ID(vehicle_air_data)};
 	uORB::Subscription _airspeed_sub{ORB_ID(airspeed)};
 	uORB::Subscription _ev_odom_sub{ORB_ID(vehicle_visual_odometry)};
@@ -201,6 +205,7 @@ private:
 	uORB::Publication<estimator_innovations_s>		_estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)};
 	uORB::Publication<estimator_innovations_s>		_estimator_innovation_variances_pub{ORB_ID(estimator_innovation_variances)};
 	uORB::Publication<estimator_innovations_s>		_estimator_innovations_pub{ORB_ID(estimator_innovations)};
+	uORB::Publication<estimator_optical_flow_vel_s>		_estimator_optical_flow_vel_pub{ORB_ID(estimator_optical_flow_vel)};
 	uORB::Publication<estimator_sensor_bias_s>		_estimator_sensor_bias_pub{ORB_ID(estimator_sensor_bias)};
 	uORB::Publication<estimator_states_s>			_estimator_states_pub{ORB_ID(estimator_states)};
 	uORB::PublicationData<estimator_status_s>		_estimator_status_pub{ORB_ID(estimator_status)};

src/modules/logger/logged_topics.cpp

@@ -60,6 +60,7 @@ void LoggedTopics::add_default_topics()
 	add_topic("estimator_innovation_test_ratios", 200);
 	add_topic("estimator_innovation_variances", 200);
 	add_topic("estimator_innovations", 200);
+	add_topic("estimator_optical_flow_vel");
 	add_topic("estimator_sensor_bias", 1000);
 	add_topic("estimator_states", 1000);
 	add_topic("estimator_status", 200);