optical flow sensor pipeline overhaul

- all sources of optical flow publish sensor_optical_flow
 - sensor_optical_flow is aggregated by the sensors module, aligned with integrated gyro, and published as vehicle_optical_flow

Co-authored-by: alexklimaj <alex@arkelectron.com>

msg/CMakeLists.txt

@@ -65,26 +65,25 @@ set(msg_files
 	differential_pressure.msg
 	distance_sensor.msg
 	ekf2_timestamps.msg
-	estimator_gps_status.msg
 	esc_report.msg
 	esc_status.msg
+	estimator_aid_source_1d.msg
+	estimator_aid_source_2d.msg
+	estimator_aid_source_3d.msg
 	estimator_baro_bias.msg
 	estimator_event_flags.msg
+	estimator_gps_status.msg
 	estimator_innovations.msg
-	estimator_optical_flow_vel.msg
 	estimator_selector_status.msg
 	estimator_sensor_bias.msg
 	estimator_states.msg
 	estimator_status.msg
 	estimator_status_flags.msg
-	estimator_aid_source_1d.msg
-	estimator_aid_source_2d.msg
-	estimator_aid_source_3d.msg
 	event.msg
-	follow_target_status.msg
+	failure_detector_status.msg
 	follow_target.msg
 	follow_target_estimator.msg
-	failure_detector_status.msg
+	follow_target_status.msg
 	generator_status.msg
 	geofence_result.msg
 	gimbal_device_attitude_status.msg
@@ -99,8 +98,8 @@ set(msg_files
 	heater_status.msg
 	home_position.msg
 	hover_thrust_estimate.msg
-	internal_combustion_engine_status.msg
 	input_rc.msg
+	internal_combustion_engine_status.msg
 	iridiumsbd_status.msg
 	irlock_report.msg
 	landing_gear.msg
@@ -123,17 +122,16 @@ set(msg_files
 	obstacle_distance.msg
 	offboard_control_mode.msg
 	onboard_computer_status.msg
-	optical_flow.msg
 	orbit_status.msg
 	parameter_update.msg
 	ping.msg
-	pps_capture.msg
 	position_controller_landing_status.msg
 	position_controller_status.msg
 	position_setpoint.msg
 	position_setpoint_triplet.msg
 	power_button_state.msg
 	power_monitor.msg
+	pps_capture.msg
 	pwm_input.msg
 	px4io_status.msg
 	radio_status.msg
@@ -148,17 +146,18 @@ set(msg_files
 	sensor_baro.msg
 	sensor_combined.msg
 	sensor_correction.msg
-	sensor_gps.msg
 	sensor_gnss_relative.msg
+	sensor_gps.msg
 	sensor_gyro.msg
 	sensor_gyro_fft.msg
 	sensor_gyro_fifo.msg
 	sensor_hygrometer.msg
 	sensor_mag.msg
+	sensor_optical_flow.msg
 	sensor_preflight_mag.msg
 	sensor_selection.msg
-	sensors_status_imu.msg
 	sensors_status.msg
+	sensors_status_imu.msg
 	system_power.msg
 	takeoff_status.msg
 	task_stack_info.msg
@@ -176,8 +175,8 @@ set(msg_files
 	uavcan_parameter_value.msg
 	ulog_stream.msg
 	ulog_stream_ack.msg
-	uwb_grid.msg
 	uwb_distance.msg
+	uwb_grid.msg
 	vehicle_acceleration.msg
 	vehicle_air_data.msg
 	vehicle_angular_acceleration.msg
@@ -198,6 +197,8 @@ set(msg_files
 	vehicle_local_position_setpoint.msg
 	vehicle_magnetometer.msg
 	vehicle_odometry.msg
+	vehicle_optical_flow.msg
+	vehicle_optical_flow_vel.msg
 	vehicle_rates_setpoint.msg
 	vehicle_roi.msg
 	vehicle_status.msg

msg/estimator_optical_flow_vel.msg

@@ -1,8 +0,0 @@
-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/optical_flow.msg

@@ -1,29 +0,0 @@
-# Optical flow in XYZ body frame in SI units.
-# http://en.wikipedia.org/wiki/International_System_of_Units
-
-uint64 timestamp		# time since system start (microseconds)
-
-uint8 sensor_id			# id of the sensor emitting the flow value
-float32 pixel_flow_x_integral	# accumulated optical flow in radians where a positive value is produced by a RH rotation about the X body axis
-float32 pixel_flow_y_integral	# accumulated optical flow in radians where a positive value is produced by a RH rotation about the Y body axis
-float32 gyro_x_rate_integral	# accumulated gyro value in radians where a positive value is produced by a RH rotation about the X body axis. Set to NaN if flow sensor does not have 3-axis gyro data.
-float32 gyro_y_rate_integral	# accumulated gyro value in radians where a positive value is produced by a RH rotation about the Y body axis. Set to NaN if flow sensor does not have 3-axis gyro data.
-float32 gyro_z_rate_integral	# accumulated gyro value in radians where a positive value is produced by a RH rotation about the Z body axis. Set to NaN if flow sensor does not have 3-axis gyro data.
-float32 ground_distance_m	# Altitude / distance to ground in meters
-uint32 integration_timespan	# accumulation timespan in microseconds
-uint32 time_since_last_sonar_update	# time since last sonar update in microseconds
-uint16 frame_count_since_last_readout	# number of accumulated frames in timespan
-int16 gyro_temperature	# Temperature * 100 in centi-degrees Celsius
-uint8 quality	# Average of quality of accumulated frames, 0: bad quality, 255: maximum quality
-
-float32 max_flow_rate # Magnitude of maximum angular which the optical flow sensor can measure reliably
-float32 min_ground_distance # Minimum distance from ground at which the optical flow sensor operates reliably
-float32 max_ground_distance # Maximum distance from ground at which the optical flow sensor operates reliably
-
-
-uint8 MODE_UNKNOWN        = 0
-uint8 MODE_BRIGHT         = 1
-uint8 MODE_LOWLIGHT       = 2
-uint8 MODE_SUPER_LOWLIGHT = 3
-
-uint8 mode

msg/sensor_optical_flow.msg

@@ -0,0 +1,30 @@
+uint64 timestamp               # time since system start (microseconds)
+uint64 timestamp_sample
+
+uint32 device_id               # unique device ID for the sensor that does not change between power cycles
+
+float32[2] pixel_flow          # (radians) optical flow in radians where a positive value is produced by a RH rotation about the body axis
+
+float32[3] delta_angle         # (radians) accumulated gyro radians where a positive value is produced by a RH rotation about the body axis. Set to NaN if flow sensor does not have 3-axis gyro data.
+bool delta_angle_available
+
+float32 distance_m             # (meters) Distance to the center of the flow field
+bool distance_available
+
+uint32 integration_timespan_us # (microseconds) accumulation timespan in microseconds
+
+uint8 quality                  # quality, 0: bad quality, 255: maximum quality
+
+uint32 error_count
+
+float32 max_flow_rate          # (radians/s) Magnitude of maximum angular which the optical flow sensor can measure reliably
+
+float32 min_ground_distance    # (meters) Minimum distance from ground at which the optical flow sensor operates reliably
+float32 max_ground_distance    # (meters) Maximum distance from ground at which the optical flow sensor operates reliably
+
+uint8 MODE_UNKNOWN        = 0
+uint8 MODE_BRIGHT         = 1
+uint8 MODE_LOWLIGHT       = 2
+uint8 MODE_SUPER_LOWLIGHT = 3
+
+uint8 mode

msg/tools/urtps_bridge_topics.yaml

@@ -40,7 +40,7 @@ rtps:
     receive: true
   - msg:     offboard_control_mode
     receive: true
-  - msg:     optical_flow
+  - msg:     sensor_optical_flow
     receive: true
   - msg:     position_setpoint
     receive: true

msg/vehicle_optical_flow.msg

@@ -0,0 +1,21 @@
+# Optical flow in XYZ body frame in SI units.
+
+uint64 timestamp               # time since system start (microseconds)
+uint64 timestamp_sample
+
+uint32 device_id               # unique device ID for the sensor that does not change between power cycles
+
+float32[2] pixel_flow          # (radians) accumulated optical flow in radians where a positive value is produced by a RH rotation about the body axis
+
+float32[3] delta_angle         # (radians) accumulated gyro radians where a positive value is produced by a RH rotation about the body axis. (NAN if unavailable)
+
+float32 distance_m             # (meters) Distance to the center of the flow field (NAN if unavailable)
+
+uint32 integration_timespan_us # (microseconds) accumulation timespan in microseconds
+
+uint8 quality                  # Average of quality of accumulated frames, 0: bad quality, 255: maximum quality
+
+float32 max_flow_rate          # (radians/s) Magnitude of maximum angular which the optical flow sensor can measure reliably
+
+float32 min_ground_distance    # (meters) Minimum distance from ground at which the optical flow sensor operates reliably
+float32 max_ground_distance    # (meters) Maximum distance from ground at which the optical flow sensor operates reliably

msg/vehicle_optical_flow_vel.msg

@@ -0,0 +1,13 @@
+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                   # 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)
+
+# TOPICS estimator_optical_flow_vel vehicle_optical_flow_vel