manual merge of origin/master into fw_control

4 files changed, 30 insertions, 0 deletions

diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c
index 991bbfbab..b393620e2 100644
--- a/apps/mavlink/mavlink.c
+++ b/apps/mavlink/mavlink.c
@@ -547,6 +547,9 @@ int mavlink_thread_main(int argc, char *argv[])
 	/* print welcome text */
 	warnx("MAVLink v1.0 serial interface starting...");
 
+	/* inform about mode */
+	warnx((mavlink_link_mode == MAVLINK_INTERFACE_MODE_ONBOARD) ? "ONBOARD MODE" : "DOWNLINK MODE");
+
 	/* Flush stdout in case MAVLink is about to take it over */
 	fflush(stdout);
 
diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c
index 45fe1ccb5..e2f28dabd 100644
--- a/apps/mavlink/mavlink_receiver.c
+++ b/apps/mavlink/mavlink_receiver.c
@@ -206,6 +206,10 @@ handle_message(mavlink_message_t *msg)
 		vicon_position.y = pos.y;
 		vicon_position.z = pos.z;
 
+		vicon_position.roll = pos.roll;
+		vicon_position.pitch = pos.pitch;
+		vicon_position.yaw = pos.yaw;
+
 		if (vicon_position_pub <= 0) {
 			vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position);
 		} else {
@@ -356,6 +360,11 @@ handle_message(mavlink_message_t *msg)
 			struct manual_control_setpoint_s mc;
 			static orb_advert_t mc_pub = 0;
 
+			int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
+
+			/* get a copy first, to prevent altering values that are not sent by the mavlink command */
+			orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &mc);
+
 			mc.timestamp = rc_hil.timestamp;
 			mc.roll = man.x / 1000.0f;
 			mc.pitch = man.y / 1000.0f;
diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c
index 3ac229d73..4567a08f8 100644
--- a/apps/mavlink/orb_listener.c
+++ b/apps/mavlink/orb_listener.c
@@ -113,6 +113,7 @@ static void	l_actuator_armed(struct listener *l);
 static void	l_manual_control_setpoint(struct listener *l);
 static void	l_vehicle_attitude_controls(struct listener *l);
 static void	l_debug_key_value(struct listener *l);
+static void	l_optical_flow(struct listener *l);
 
 struct listener listeners[] = {
 	{l_sensor_combined,		&mavlink_subs.sensor_sub,	0},
@@ -134,6 +135,7 @@ struct listener listeners[] = {
 	{l_manual_control_setpoint,	&mavlink_subs.man_control_sp_sub, 0},
 	{l_vehicle_attitude_controls,	&mavlink_subs.actuators_sub,	0},
 	{l_debug_key_value,		&mavlink_subs.debug_key_value,	0},
+	{l_optical_flow,		&mavlink_subs.optical_flow,	0},
 };
 
 static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]);
@@ -586,6 +588,17 @@ l_debug_key_value(struct listener *l)
 					   debug.value);
 }
 
+void
+l_optical_flow(struct listener *l)
+{
+	struct optical_flow_s flow;
+
+	orb_copy(ORB_ID(optical_flow), mavlink_subs.optical_flow, &flow);
+
+	mavlink_msg_optical_flow_send(MAVLINK_COMM_0, flow.timestamp, flow.sensor_id, flow.flow_raw_x, flow.flow_raw_y,
+		flow.flow_comp_x_m, flow.flow_comp_y_m, flow.quality, flow.ground_distance_m);
+}
+
 static void *
 uorb_receive_thread(void *arg)
 {
@@ -710,6 +723,10 @@ uorb_receive_start(void)
 	mavlink_subs.debug_key_value = orb_subscribe(ORB_ID(debug_key_value));
 	orb_set_interval(mavlink_subs.debug_key_value, 100);	/* 10Hz updates */
 
+	/* --- FLOW SENSOR --- */
+	mavlink_subs.optical_flow = orb_subscribe(ORB_ID(optical_flow));
+	orb_set_interval(mavlink_subs.optical_flow, 200); 	/* 5Hz updates */
+
 	/* start the listener loop */
 	pthread_attr_t uorb_attr;
 	pthread_attr_init(&uorb_attr);
diff --git a/apps/mavlink/orb_topics.h b/apps/mavlink/orb_topics.h
index 6860702d2..1b371e5cd 100644
--- a/apps/mavlink/orb_topics.h
+++ b/apps/mavlink/orb_topics.h
@@ -77,6 +77,7 @@ struct mavlink_subscriptions {
 	int spg_sub;
 	int debug_key_value;
 	int input_rc_sub;
+	int optical_flow;
 };
 
 extern struct mavlink_subscriptions mavlink_subs;