Merge remote-tracking branch 'upstream/master' into offboard2_externalsetpointmessages

Conflicts:
	src/modules/navigator/navigator.h
	src/modules/navigator/navigator_main.cpp

1 files changed, 57 insertions, 3 deletions

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 3ac4d3b03..e8d783847 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -55,6 +55,7 @@
 #include <drivers/drv_gyro.h>
 #include <drivers/drv_mag.h>
 #include <drivers/drv_baro.h>
+#include <drivers/drv_range_finder.h>
 #include <time.h>
 #include <float.h>
 #include <unistd.h>
@@ -103,6 +104,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_battery_pub(-1),
 	_cmd_pub(-1),
 	_flow_pub(-1),
+	_range_pub(-1),
 	_offboard_control_sp_pub(-1),
 	_global_vel_sp_pub(-1),
 	_att_sp_pub(-1),
@@ -123,7 +125,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 {
 
 	// make sure the FTP server is started
-	(void)MavlinkFTP::getServer();
+	(void)MavlinkFTP::get_server();
 }
 
 MavlinkReceiver::~MavlinkReceiver()
@@ -182,8 +184,8 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_request_data_stream(msg);
 		break;
 
-	case MAVLINK_MSG_ID_ENCAPSULATED_DATA:
-		MavlinkFTP::getServer()->handle_message(_mavlink, msg);
+	case MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL:
+		MavlinkFTP::get_server()->handle_message(_mavlink, msg);
 		break;
 
 	default:
@@ -210,6 +212,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 			handle_message_hil_state_quaternion(msg);
 			break;
 
+		case MAVLINK_MSG_ID_HIL_OPTICAL_FLOW:
+			handle_message_hil_optical_flow(msg);
+			break;
+
 		default:
 			break;
 		}
@@ -374,6 +380,52 @@ MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg)
 }
 
 void
+MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg)
+{
+	/* optical flow */
+	mavlink_hil_optical_flow_t flow;
+	mavlink_msg_hil_optical_flow_decode(msg, &flow);
+
+	struct optical_flow_s f;
+	memset(&f, 0, sizeof(f));
+
+	f.timestamp = hrt_absolute_time();
+	f.flow_timestamp = flow.time_usec;
+	f.flow_raw_x = flow.flow_x;
+	f.flow_raw_y = flow.flow_y;
+	f.flow_comp_x_m = flow.flow_comp_m_x;
+	f.flow_comp_y_m = flow.flow_comp_m_y;
+	f.ground_distance_m = flow.ground_distance;
+	f.quality = flow.quality;
+	f.sensor_id = flow.sensor_id;
+
+	if (_flow_pub < 0) {
+		_flow_pub = orb_advertise(ORB_ID(optical_flow), &f);
+
+	} else {
+		orb_publish(ORB_ID(optical_flow), _flow_pub, &f);
+	}
+
+	/* Use distance value for range finder report */
+	struct range_finder_report r;
+	memset(&r, 0, sizeof(f));
+
+	r.timestamp = hrt_absolute_time();
+	r.error_count = 0;
+	r.type = RANGE_FINDER_TYPE_LASER;
+	r.distance = flow.ground_distance;
+	r.minimum_distance = 0.0f;
+	r.maximum_distance = 40.0f; // this is set to match the typical range of real sensors, could be made configurable
+	r.valid = (r.distance > r.minimum_distance) && (r.distance < r.maximum_distance);
+
+	if (_range_pub < 0) {
+		_range_pub = orb_advertise(ORB_ID(sensor_range_finder), &r);
+	} else {
+		orb_publish(ORB_ID(sensor_range_finder), _range_pub, &r);
+	}
+}
+
+void
 MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg)
 {
 	mavlink_set_mode_t new_mode;
@@ -772,6 +824,8 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
 		tstatus.remote_noise = rstatus.remnoise;
 		tstatus.rxerrors = rstatus.rxerrors;
 		tstatus.fixed = rstatus.fixed;
+		tstatus.system_id = msg->sysid;
+		tstatus.component_id = msg->compid;
 
 		if (_telemetry_status_pub < 0) {
 			_telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus);