From 068dd7676329473f3c648986ae5b1b136f5470e9 Mon Sep 17 00:00:00 2001
From: v01d <phreakuencies@gmail.com>
Date: Sun, 20 Sep 2015 18:17:19 -0300
Subject: [PATCH] simulator: add publishing of HIL_OPTICAL_FLOW as uORB topic

---
 src/modules/simulator/simulator.h           |  3 ++
 src/modules/simulator/simulator_mavlink.cpp | 45 ++++++++++++++++++++-
 2 files changed, 47 insertions(+), 1 deletion(-)

diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h
index af4b8a746c..5d8eedac2d 100644
--- a/src/modules/simulator/simulator.h
+++ b/src/modules/simulator/simulator.h
@@ -51,6 +51,7 @@
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_rc_input.h>
 #include <uORB/uORB.h>
+#include <uORB/topics/optical_flow.h>
 #include <v1.0/mavlink_types.h>
 #include <v1.0/common/mavlink.h>
 namespace simulator {
@@ -255,11 +256,13 @@ private:
 	orb_advert_t _baro_pub;
 	orb_advert_t _gyro_pub;
 	orb_advert_t _mag_pub;
+	orb_advert_t _flow_pub;
 
 	bool _initialized;
 
 	// class methods
 	int publish_sensor_topics(mavlink_hil_sensor_t *imu);
+	int publish_flow_topic(mavlink_hil_optical_flow_t *flow);
 
 #ifndef __PX4_QURT
 	// uORB publisher handlers
diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp
index ddbc79db76..a7c4d02f5e 100644
--- a/src/modules/simulator/simulator_mavlink.cpp
+++ b/src/modules/simulator/simulator_mavlink.cpp
@@ -229,6 +229,14 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish) {
 		update_sensors(&imu);
 		break;
 
+	case MAVLINK_MSG_ID_HIL_OPTICAL_FLOW:
+		mavlink_hil_optical_flow_t flow;
+		mavlink_msg_hil_optical_flow_decode(msg, &flow);
+		if (publish) {
+			publish_flow_topic(&flow);
+		}
+		break;
+
 	case MAVLINK_MSG_ID_HIL_GPS:
 		mavlink_hil_gps_t gps_sim;
 		mavlink_msg_hil_gps_decode(msg, &gps_sim);
@@ -708,5 +716,40 @@ int Simulator::publish_sensor_topics(mavlink_hil_sensor_t *imu) {
 		}
 	}
 
-        return OK;
+				return OK;
+}
+
+int Simulator::publish_flow_topic(mavlink_hil_optical_flow_t* flow_mavlink)
+{
+  uint64_t timestamp = hrt_absolute_time();
+
+  /* gyro */
+	{
+		struct optical_flow_s flow;
+		memset(&flow, 0, sizeof(flow));
+
+		flow.sensor_id = flow_mavlink->sensor_id;
+		flow.timestamp = timestamp;
+		flow.time_since_last_sonar_update = 0;
+		flow.frame_count_since_last_readout = 0; // ?
+		flow.integration_timespan = flow_mavlink->integration_time_us;
+
+		flow.ground_distance_m = flow_mavlink->distance;
+		flow.gyro_temperature = flow_mavlink->temperature;
+		flow.gyro_x_rate_integral = flow_mavlink->integrated_xgyro;
+		flow.gyro_y_rate_integral = flow_mavlink->integrated_ygyro;
+		flow.gyro_z_rate_integral = flow_mavlink->integrated_zgyro;
+		flow.pixel_flow_x_integral = flow_mavlink->integrated_x;
+		flow.pixel_flow_x_integral = flow_mavlink->integrated_y;
+		flow.quality = flow_mavlink->quality;
+
+		if (_flow_pub == nullptr) {
+		_flow_pub = orb_advertise(ORB_ID(optical_flow), &flow);
+
+		} else {
+		orb_publish(ORB_ID(optical_flow), _flow_pub, &flow);
+		}
+	}
+
+	return OK;
 }