Mavlink: Start multiple uart listeners, HIL working

1 files changed, 32 insertions, 9 deletions

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 0d9d06190..79aa6135b 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -309,6 +309,16 @@ int Mavlink::get_uart_fd(unsigned index)
 	return -1;
 }
 
+int Mavlink::get_uart_fd()
+{
+	return _uart;
+}
+
+int Mavlink::get_channel()
+{
+	return (int)_chan;
+}
+
 void
 Mavlink::parameters_update()
 {
@@ -945,7 +955,7 @@ void Mavlink::mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8
 	wpa.target_component = compid;
 	wpa.type = type;
 
-	mavlink_msg_mission_ack_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpa);
+	mavlink_msg_mission_ack_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpa);
 	mavlink_missionlib_send_message(&msg);
 
 	if (verbose) warnx("Sent waypoint ack (%u) to ID %u", wpa.type, wpa.target_system);
@@ -968,7 +978,7 @@ void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t seq)
 
 		wpc.seq = seq;
 
-		mavlink_msg_mission_current_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpc);
+		mavlink_msg_mission_current_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpc);
 		mavlink_missionlib_send_message(&msg);
 
 		if (verbose) warnx("Broadcasted new current waypoint %u", wpc.seq);
@@ -988,7 +998,7 @@ void Mavlink::mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uin
 	wpc.target_component = compid;
 	wpc.count = mission.count;
 
-	mavlink_msg_mission_count_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpc);
+	mavlink_msg_mission_count_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpc);
 	mavlink_missionlib_send_message(&msg);
 
 	if (verbose) warnx("Sent waypoint count (%u) to ID %u", wpc.count, wpc.target_system);
@@ -1018,7 +1028,7 @@ void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t
 		wp.target_system = sysid;
 		wp.target_component = compid;
 		wp.seq = seq;
-		mavlink_msg_mission_item_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wp);
+		mavlink_msg_mission_item_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wp);
 		mavlink_missionlib_send_message(&msg);
 
 		if (verbose) warnx("Sent waypoint %u to ID %u", wp.seq, wp.target_system);
@@ -1036,7 +1046,7 @@ void Mavlink::mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, u
 		wpr.target_system = sysid;
 		wpr.target_component = compid;
 		wpr.seq = seq;
-		mavlink_msg_mission_request_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpr);
+		mavlink_msg_mission_request_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpr);
 		mavlink_missionlib_send_message(&msg);
 
 		if (verbose) warnx("Sent waypoint request %u to ID %u", wpr.seq, wpr.target_system);
@@ -1061,7 +1071,7 @@ void Mavlink::mavlink_wpm_send_waypoint_reached(uint16_t seq)
 
 	wp_reached.seq = seq;
 
-	mavlink_msg_mission_item_reached_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wp_reached);
+	mavlink_msg_mission_item_reached_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wp_reached);
 	mavlink_missionlib_send_message(&msg);
 
 	if (verbose) warnx("Sent waypoint %u reached message", wp_reached.seq);
@@ -1318,7 +1328,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 			mavlink_mission_item_t wp;
 			mavlink_msg_mission_item_decode(msg, &wp);
 
-			if (wp.target_system == mavlink_system.sysid && wp.target_component == mavlink_wpm_comp_id) {
+			if (wp.target_system == mavlink_system.sysid && wp.target_component == _mavlink_wpm_comp_id) {
 
 				wpm->timestamp_lastaction = now;
 
@@ -1576,6 +1586,21 @@ Mavlink::task_main(int argc, char *argv[])
 			break;
 	}
 
+	switch(_mode) {
+		case MODE_OFFBOARD:
+		case MODE_HIL:
+			_mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER;
+			break;
+		case MODE_ONBOARD:
+			_mavlink_wpm_comp_id = MAV_COMP_ID_CAMERA;
+			break;
+		case MODE_TX_HEARTBEAT_ONLY:
+		default:
+			_mavlink_wpm_comp_id = MAV_COMP_ID_ALL;
+			warnx("Error: Unknown mode");
+			break;
+	}
+
 	/* Flush stdout in case MAVLink is about to take it over */
 	fflush(stdout);
 
@@ -1597,11 +1622,9 @@ Mavlink::task_main(int argc, char *argv[])
 	mavlink_update_system();
 
 	/* start the MAVLink receiver */
-//	MavlinkReceiver rcv(this);
 	receive_thread = MavlinkReceiver::receive_start(this);
 
 	/* start the ORB receiver */
-//	MavlinkOrbListener listener(this);
 	uorb_receive_thread = MavlinkOrbListener::uorb_receive_start(this);
 
 	/* initialize waypoint manager */