Mavlink: Start multiple uart listeners, HIL working

6 files changed, 68 insertions, 82 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 */
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index d5bbb746b..c667a41da 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -96,46 +96,27 @@ enum MAVLINK_WPM_CODES {
 };
 
 
-/* WAYPOINT MANAGER - MISSION LIB */
-
-#define MAVLINK_WPM_MAX_WP_COUNT 15
-#define MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE                                  ///< Enable double buffer and in-flight updates
-#ifndef MAVLINK_WPM_TEXT_FEEDBACK
-#define MAVLINK_WPM_TEXT_FEEDBACK 0                                                  ///< Report back status information as text
-#endif
-#define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 5000000         ///< Protocol communication timeout in useconds
-#define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000000           ///< When to send a new setpoint
+#define MAVLINK_WPM_MAX_WP_COUNT 255
+#define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication timeout in useconds
+#define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000000 ///< When to send a new setpoint
 #define MAVLINK_WPM_PROTOCOL_DELAY_DEFAULT 40000
 
 
 struct mavlink_wpm_storage {
-        mavlink_mission_item_t waypoints[MAVLINK_WPM_MAX_WP_COUNT];      ///< Currently active waypoints
-#ifdef MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE
-        mavlink_mission_item_t rcv_waypoints[MAVLINK_WPM_MAX_WP_COUNT];  ///< Receive buffer for next waypoints
-#endif
-        uint16_t size;
-        uint16_t max_size;
-        uint16_t rcv_size;
-        enum MAVLINK_WPM_STATES current_state;
-        int16_t current_wp_id;                                                        ///< Waypoint in current transmission
-        int16_t current_active_wp_id;                                        ///< Waypoint the system is currently heading towards
-        uint16_t current_count;
-        uint8_t current_partner_sysid;
-        uint8_t current_partner_compid;
-        uint64_t timestamp_lastaction;
-        uint64_t timestamp_last_send_setpoint;
-        uint64_t timestamp_firstinside_orbit;
-        uint64_t timestamp_lastoutside_orbit;
-        uint32_t timeout;
-        uint32_t delay_setpoint;
-        float accept_range_yaw;
-        float accept_range_distance;
-        bool yaw_reached;
-        bool pos_reached;
-        bool idle;
-        int current_dataman_id;
+	uint16_t size;
+	uint16_t max_size;
+	enum MAVLINK_WPM_STATES current_state;
+	int16_t current_wp_id;	///< Waypoint in current transmission
+	uint16_t current_count;
+	uint8_t current_partner_sysid;
+	uint8_t current_partner_compid;
+	uint64_t timestamp_lastaction;
+	uint64_t timestamp_last_send_setpoint;
+	uint32_t timeout;
+	int current_dataman_id;
 };
 
+
 class Mavlink
 {
 
@@ -174,7 +155,9 @@ public:
 
 	static int get_uart_fd(unsigned index);
 
-	int get_uart_fd() { return _uart; }
+	int get_uart_fd();
+
+	int get_channel();
 
 	const char *device_name;
 
@@ -294,7 +277,7 @@ private:
 	uint8_t missionlib_msg_buf[300]; //XXX MAGIC NUMBER
 	MAVLINK_MODE _mode;
 
-	uint8_t mavlink_wpm_comp_id;
+	uint8_t _mavlink_wpm_comp_id;
 	mavlink_channel_t _chan;
 
 //  XXX probably should be in a header... 
diff --git a/src/modules/mavlink/mavlink_orb_listener.cpp b/src/modules/mavlink/mavlink_orb_listener.cpp
index a2b71c931..fdc196371 100644
--- a/src/modules/mavlink/mavlink_orb_listener.cpp
+++ b/src/modules/mavlink/mavlink_orb_listener.cpp
@@ -55,14 +55,14 @@
 #include <stdlib.h>
 #include <poll.h>
 #include <lib/geo/geo.h>
+#include <systemlib/err.h>
+
 
 #include <mavlink/mavlink_log.h>
 
 #include "mavlink_orb_listener.h"
 #include "mavlink_main.h"
 
-	void		*uorb_receive_thread(void *arg);
-
 
 
 uint16_t cm_uint16_from_m_float(float m);
@@ -647,9 +647,9 @@ void *
 MavlinkOrbListener::uorb_receive_thread(void *arg)
 {
 	/* Set thread name */
-	char buf[32];
-	sprintf(buf, "mavlink rcv%d", Mavlink::instance_count());
-	prctl(PR_SET_NAME, buf, getpid());
+	char thread_name[18];
+	sprintf(thread_name, "mavlink_uorb_rcv_%d", _mavlink->get_channel());
+	prctl(PR_SET_NAME, thread_name, getpid());
 
 	/*
 	 * set up poll to block for new data,
@@ -822,7 +822,7 @@ MavlinkOrbListener::uorb_receive_start(Mavlink* mavlink)
 	pthread_attr_setstacksize(&uorb_attr, 2048);
 
 	pthread_t thread;
-	pthread_create(&thread, &uorb_attr, MavlinkOrbListener::uorb_start_helper, mavlink);
+	pthread_create(&thread, &uorb_attr, MavlinkOrbListener::uorb_start_helper, (void*)mavlink);
 
 	pthread_attr_destroy(&uorb_attr);
 	return thread;
diff --git a/src/modules/mavlink/mavlink_orb_listener.h b/src/modules/mavlink/mavlink_orb_listener.h
index 317a489d4..26a2e832f 100644
--- a/src/modules/mavlink/mavlink_orb_listener.h
+++ b/src/modules/mavlink/mavlink_orb_listener.h
@@ -87,20 +87,7 @@ public:
 	 */
 	~MavlinkOrbListener();
 
-	// *
-	// * Start the mavlink task.
-	//  *
-	//  * @return		OK on success.
-	 
-	// int		start();
-
-	/**
-	 * Display the mavlink status.
-	 */
-	void		status();
-
 	static pthread_t uorb_receive_start(Mavlink *mavlink);
-	void * uorb_receive_thread(void *arg);
 
 	struct listener {
 		void	(*callback)(const struct listener *l);
@@ -124,15 +111,7 @@ private:
 	unsigned	_n_listeners;
 	static const unsigned _max_listeners = 32;
 
-	/**
-	 * Shim for calling task_main from task_create.
-	 */
-	void		task_main_trampoline(int argc, char *argv[]);
-
-	/**
-	 * Main sensor collection task.
-	 */
-	void		task_main() __attribute__((noreturn));
+	void *uorb_receive_thread(void *arg);
 
 	static void		l_sensor_combined(const struct listener *l);
 	static void		l_vehicle_attitude(const struct listener *l);
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 44fff2416..371f945c4 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -798,7 +798,10 @@ MavlinkReceiver::receive_thread(void *arg)
 
 	mavlink_message_t msg;
 
-	prctl(PR_SET_NAME, "mavlink_uart_rcv", getpid());
+	/* Set thread name */
+	char thread_name[18];
+	sprintf(thread_name, "mavlink_uart_rcv_%d", _mavlink->get_channel());
+	prctl(PR_SET_NAME, thread_name, getpid());
 
 	struct pollfd fds[1];
 	fds[0].fd = uart_fd;
@@ -842,20 +845,19 @@ void MavlinkReceiver::print_status()
 
 void * MavlinkReceiver::start_helper(void *context)
 {
-	MavlinkReceiver *rcv = new MavlinkReceiver(((Mavlink *)context));
+	MavlinkReceiver *rcv = new MavlinkReceiver((Mavlink*)context);
 	return rcv->receive_thread(NULL);
 }
 
 pthread_t
-MavlinkReceiver::receive_start(Mavlink *mavlink)
+MavlinkReceiver::receive_start(Mavlink *parent)
 {
-
 	pthread_attr_t receiveloop_attr;
 	pthread_attr_init(&receiveloop_attr);
 
 	// set to non-blocking read
-	int flags = fcntl(mavlink->get_uart_fd(), F_GETFL, 0);
-	fcntl(mavlink->get_uart_fd(), F_SETFL, flags | O_NONBLOCK);
+	int flags = fcntl(parent->get_uart_fd(), F_GETFL, 0);
+	fcntl(parent->get_uart_fd(), F_SETFL, flags | O_NONBLOCK);
 
 	struct sched_param param;
 	(void)pthread_attr_getschedparam(&receiveloop_attr, &param);
@@ -865,7 +867,7 @@ MavlinkReceiver::receive_start(Mavlink *mavlink)
 	pthread_attr_setstacksize(&receiveloop_attr, 3000);
 
 	pthread_t thread;
-	pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, mavlink);
+	pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, (void*)parent);
 
 	pthread_attr_destroy(&receiveloop_attr);
 	return thread;
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index 6614e13f4..8e139e5e4 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -95,7 +95,7 @@ public:
 	 */
 	void		print_status();
 
-	static pthread_t receive_start(Mavlink* mavlink);
+	static pthread_t receive_start(Mavlink *parent);
 
 	static void * start_helper(void *context);
 
@@ -105,7 +105,6 @@ private:
 
 	Mavlink*	_mavlink;
 
-
 	void handle_message(mavlink_message_t *msg);
 	void *receive_thread(void *arg);