From 30218b0de709a0bf38f4b5c0cc104caf66f4e343 Mon Sep 17 00:00:00 2001
From: TSC21 <n.marques21@hotmail.com>
Date: Wed, 20 May 2015 00:15:27 +0100
Subject: [PATCH] distance_sensor: added mavlink_receiver handler for msg

---
 src/modules/mavlink/mavlink_receiver.cpp | 36 +++++++++++++++++++++++-
 src/modules/mavlink/mavlink_receiver.h   |  3 ++
 2 files changed, 38 insertions(+), 1 deletion(-)

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 3c4fa76a0c..b84d381c6f 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -134,7 +134,8 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_att_sp{},
 	_rates_sp{},
 	_time_offset_avg_alpha(0.6),
-	_time_offset(0)
+	_time_offset(0),
+	_distance_sensor_pub(-1)
 {
 
 }
@@ -211,6 +212,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_timesync(msg);
 		break;
 
+	case MAVLINK_MSG_ID_DISTANCE_SENSOR:
+		handle_message_distance_sensor(msg);
+		break;
+
 	default:
 		break;
 	}
@@ -495,6 +500,35 @@ MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg)
 	}
 }
 
+void
+MavlinkReceiver::handle_message_distance_sensor(mavlink_message_t *msg)
+{
+	/* distance sensor */
+	mavlink_distance_sensor_t dist_sensor;
+	mavlink_msg_distance_sensor_decode(msg, &dist_sensor);
+
+	struct distance_sensor_s d;
+	memset(&d, 0, sizeof(d));
+
+	d.time_boot_ms = dist_sensor.time_boot_ms;
+	d.min_distance = dist_sensor.min_distance;
+	d.max_distance = dist_sensor.max_distance;
+	d.current_distance = dist_sensor.current_distance;
+	d.type = dist_sensor.type;
+	d.id = dist_sensor.id;
+	d.orientation = dist_sensor.orientation;
+	d.covariance = dist_sensor.covariance;
+
+	/// TODO Add sensor rotation according to MAV_SENSOR_ORIENTATION enum
+
+	if (_distance_sensor_pub < 0) {
+		_distance_sensor_pub = orb_advertise(ORB_ID(distance_sensor), &d);
+
+	} else {
+		orb_publish(ORB_ID(distance_sensor), _distance_sensor_pub, &d);
+	}
+}
+
 void
 MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg)
 {
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index 887d2f88ed..7b592ab51a 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -74,6 +74,7 @@
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/vehicle_force_setpoint.h>
 #include <uORB/topics/time_offset.h>
+#include <uORB/topics/distance_sensor.h>
 
 #include "mavlink_ftp.h"
 
@@ -135,6 +136,7 @@ private:
 	void handle_message_hil_sensor(mavlink_message_t *msg);
 	void handle_message_hil_gps(mavlink_message_t *msg);
 	void handle_message_hil_state_quaternion(mavlink_message_t *msg);
+	void handle_message_distance_sensor(mavlink_message_t *msg);
 
 	void *receive_thread(void *arg);
 
@@ -191,6 +193,7 @@ private:
 	struct vehicle_rates_setpoint_s _rates_sp;
 	double _time_offset_avg_alpha;
 	uint64_t _time_offset;
+	orb_advert_t _distance_sensor_pub;
 
 	/* do not allow copying this class */
 	MavlinkReceiver(const MavlinkReceiver &);