Butchered MAVLink C++ app to compile and link - there is no hope it will work out of the box 8)

src/modules/mavlink/mavlink_bridge_header.h

@@ -42,6 +42,8 @@
 #ifndef MAVLINK_BRIDGE_HEADER_H
 #define MAVLINK_BRIDGE_HEADER_H
 
+__BEGIN_DECLS
+
 #define MAVLINK_USE_CONVENIENCE_FUNCTIONS
 
 /* use efficient approach, see mavlink_helpers.h */
@@ -72,11 +74,13 @@ extern mavlink_system_t mavlink_system;
  * @param chan MAVLink channel to use, usually MAVLINK_COMM_0 = UART0
  * @param ch Character to send
  */
-extern void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int length);
+void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int length);
 
 extern mavlink_status_t *mavlink_get_channel_status(uint8_t chan);
 extern mavlink_message_t *mavlink_get_channel_buffer(uint8_t chan);
 
 #include <v1.0/common/mavlink.h>
 
+__END_DECLS
+
 #endif /* MAVLINK_BRIDGE_HEADER_H */

src/modules/mavlink/mavlink_main.cpp

@@ -97,6 +97,8 @@
 #endif
 static const int ERROR = -1;
 
+static Mavlink* _head = nullptr;
+
 /**
  * mavlink app start / stop handling function
  *
@@ -203,10 +205,10 @@ void Mavlink::set_mode(enum MAVLINK_MODE mode)
 int Mavlink::instance_count()
 {
 	/* note: a local buffer count will help if this ever is called often */
-	Mavlink* inst = _head;
+	Mavlink* inst = ::_head;
 	unsigned inst_index = 0;
-	while (inst->_head != nullptr) {
+	while (inst->_next != nullptr) {
-		inst = inst->_head;
+		inst = inst->_next;
 		inst_index++;
 	}
 
@@ -216,22 +218,22 @@ int Mavlink::instance_count()
 Mavlink* Mavlink::new_instance(const char *port, unsigned baud_rate)
 {
 	Mavlink* inst = new Mavlink(port, baud_rate);
-	Mavlink* parent = _head;
+	Mavlink* parent = ::_head;
-	while (parent->_head != nullptr)
+	while (parent->_next != nullptr)
-		parent = parent->_head;
+		parent = parent->_next;
 
 	/* now parent points to a null pointer, fill it */
-	parent->_head = inst;
+	parent->_next = inst;
 
 	return inst;
 }
 
 Mavlink* Mavlink::get_instance(unsigned instance)
 {
-	Mavlink* inst = _head;
+	Mavlink* inst = ::_head;
 	unsigned inst_index = 0;
-	while (inst->_head != nullptr && inst_index < instance) {
+	while (inst->_next != nullptr && inst_index < instance) {
-		inst = inst->_head;
+		inst = inst->_next;
 		inst_index++;
 	}
 
@@ -428,9 +430,9 @@ Mavlink::set_hil_on_off(bool hil_enabled)
 	int ret = OK;
 
 	/* Enable HIL */
-	if (hil_enabled && !mavlink_hil_enabled) {
+	if (hil_enabled && !_mavlink_hil_enabled) {
 
-		mavlink_hil_enabled = true;
+		_mavlink_hil_enabled = true;
 
 		/* ramp up some HIL-related subscriptions */
 		unsigned hil_rate_interval;
@@ -456,8 +458,8 @@ Mavlink::set_hil_on_off(bool hil_enabled)
 		set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, hil_rate_interval);
 	}
 
-	if (!hil_enabled && mavlink_hil_enabled) {
+	if (!hil_enabled && _mavlink_hil_enabled) {
-		mavlink_hil_enabled = false;
+		_mavlink_hil_enabled = false;
 		orb_set_interval(subs.spa_sub, 200);
 
 	} else {
@@ -1426,12 +1428,6 @@ Mavlink::mavlink_missionlib_send_gcs_string(const char *string)
 	}
 }
 
-void
-Mavlink::task_main_trampoline(int argc, char *argv[])
-{
-	mavlink::g_mavlink->task_main(argc, argv);
-}
-
 int
 Mavlink::task_main(int argc, char *argv[])
 {
@@ -1481,43 +1477,43 @@ Mavlink::task_main(int argc, char *argv[])
 
 
 	/* initialize mavlink text message buffering */
-	mavlink_logbuffer_init(&lb, 10);
+	// mavlink_logbuffer_init(&lb, 10);
 
 	int ch;
 	char *device_name = "/dev/ttyS1";
 	baudrate = 57600;
 
-	/* work around some stupidity in task_create's argv handling */
+	// /* work around some stupidity in task_create's argv handling */
-	argc -= 2;
+	// argc -= 2;
-	argv += 2;
+	// argv += 2;
 
-	while ((ch = getopt(argc, argv, "b:d:eo")) != EOF) {
+	// while ((ch = getopt(argc, argv, "b:d:eo")) != EOF) {
-		switch (ch) {
+	// 	switch (ch) {
-		case 'b':
+	// 	case 'b':
-			baudrate = strtoul(optarg, NULL, 10);
+	// 		baudrate = strtoul(optarg, NULL, 10);
 
-			if (baudrate < 9600 || baudrate > 921600)
+	// 		if (baudrate < 9600 || baudrate > 921600)
-				errx(1, "invalid baud rate '%s'", optarg);
+	// 			errx(1, "invalid baud rate '%s'", optarg);
 
-			break;
+	// 		break;
 
-		case 'd':
+	// 	case 'd':
-			device_name = optarg;
+	// 		device_name = optarg;
-			break;
+	// 		break;
 
-		case 'e':
+	// 	case 'e':
-			mavlink_link_termination_allowed = true;
+	// 		mavlink_link_termination_allowed = true;
-			break;
+	// 		break;
 
-		case 'o':
+	// 	case 'o':
-			_mode = MODE_ONBOARD;
+	// 		_mode = MODE_ONBOARD;
-			break;
+	// 		break;
 
-		default:
+	// 	default:
-			usage();
+	// 		usage();
-			break;
+	// 		break;
-		}
+	// 	}
-	}
+	// }
 
 	struct termios uart_config_original;
 
@@ -1699,15 +1695,15 @@ Mavlink::task_main(int argc, char *argv[])
 		/* sleep 10 ms */
 		usleep(10000);
 
-		/* send one string at 10 Hz */
+		// /* send one string at 10 Hz */
-		if (!mavlink_logbuffer_is_empty(&lb)) {
+		// if (!mavlink_logbuffer_is_empty(&lb)) {
-			struct mavlink_logmessage msg;
+		// 	struct mavlink_logmessage msg;
-			int lb_ret = mavlink_logbuffer_read(&lb, &msg);
+		// 	int lb_ret = mavlink_logbuffer_read(&lb, &msg);
 
-			if (lb_ret == OK) {
+		// 	if (lb_ret == OK) {
-				mavlink_missionlib_send_gcs_string(msg.text);
+		// 		mavlink_missionlib_send_gcs_string(msg.text);
-			}
+		// 	}
-		}
+		// }
 
 		/* sleep 15 ms */
 		usleep(15000);
@@ -1742,27 +1738,32 @@ Mavlink::task_main(int argc, char *argv[])
 	_exit(0);
 }
 
-int
+int Mavlink::start_helper(int argc, char *argv[])
-Mavlink::start()
+{
+	// This is beyond evil.. and needs a lock to be safe
+	return Mavlink::get_instance(Mavlink::instance_count() - 1)->task_main(argc, argv);
+}
+
+int
+Mavlink::start(Mavlink* mavlink)
 {
-	ASSERT(_mavlink_task == -1);
 
 	/* start the task */
 	char buf[32];
 	sprintf(buf, "mavlink if%d", Mavlink::instance_count());
 
-	_mavlink_task = task_spawn_cmd(buf,
+	mavlink->_mavlink_task = task_spawn_cmd(buf,
 					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_MAX - 5,
 					 2048,
-					 (main_t)&Mavlink::task_main_trampoline,
+					 (main_t)&Mavlink::start_helper,
-					 nullptr);
+					 NULL);
 
 	// while (!this->is_running()) {
 	// 		usleep(200);
 	// }
 
-	if (_mavlink_task < 0) {
+	if (mavlink->_mavlink_task < 0) {
 		warn("task start failed");
 		return -errno;
 	}
@@ -1794,6 +1795,9 @@ int mavlink_main(int argc, char *argv[])
 		if (mavlink::g_mavlink == nullptr)
 			mavlink::g_mavlink = instance;
 
+		// Instantiate thread
+		Mavlink::start(instance);
+
 		// if (mavlink::g_mavlink != nullptr) {
 		// 	errx(1, "already running");
 		// }

src/modules/mavlink/mavlink_main.h

@@ -155,7 +155,7 @@ public:
 	 *
 	 * @return		OK on success.
 	 */
-	int		start();
+	static int		start(Mavlink* mavlink);
 
 	/**
 	 * Display the mavlink status.
@@ -170,7 +170,7 @@ public:
 
 	static int get_uart_fd(unsigned index);
 
-	int get_uart_fd() { return _mavlink_fd; }
+	int get_uart_fd() { return uart; }
 
 	enum MAVLINK_MODE {
 		MODE_TX_HEARTBEAT_ONLY=0,
@@ -180,7 +180,7 @@ public:
 	};
 
 	void		set_mode(enum MAVLINK_MODE);
-	enum MAVLINK_MODE		get_mode();
+	enum MAVLINK_MODE		get_mode() { return _mode; }
 
 	bool		hil_enabled() { return _mavlink_hil_enabled; };
 
@@ -189,11 +189,24 @@ public:
 	 */
 	void mavlink_wpm_message_handler(const mavlink_message_t *msg);
 
+	static int start_helper(int argc, char *argv[]);
+
 	/**
 	 * Handle parameter related messages.
 	 */
 	void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg);
 
+	void get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode);
+
+	/**
+	 * Enable / disable Hardware in the Loop simulation mode.
+	 *
+	 * @param hil_enabled	The new HIL enable/disable state.
+	 * @return		OK if the HIL state changed, ERROR if the
+	 *			requested change could not be made or was
+	 *			redundant.
+	 */
+	int		set_hil_on_off(bool hil_enabled);
 
 	struct mavlink_subscriptions {
 		int sensor_sub;
@@ -217,12 +230,13 @@ public:
 		int input_rc_sub;
 		int optical_flow;
 		int rates_setpoint_sub;
-		int home_sub;
+		int get_sub;
 		int airspeed_sub;
 		int navigation_capabilities_sub;
 		int control_mode_sub;
 		int rc_sub;
 		int status_sub;
+		int home_sub;
 	};
 
 	struct mavlink_subscriptions subs;
@@ -252,6 +266,7 @@ protected:
 	 */
 	struct file_operations	fops;
 	int		_mavlink_fd;
+	Mavlink*	_next;
 
 private:
 
@@ -264,7 +279,6 @@ private:
 
 	/* states */
 	bool		_mavlink_hil_enabled;		/**< Hardware in the loop mode */
-	static Mavlink*	_head;
 
 	int		_params_sub;
 
@@ -289,7 +303,6 @@ private:
 	mavlink_wpm_storage *wpm;
 
 	bool verbose;
-	bool mavlink_hil_enabled;
 	int uart;
 	int baudrate;
 	bool gcs_link;
@@ -308,16 +321,6 @@ private:
 	 */
 	void		parameters_update();
 
-	/**
-	 * Enable / disable Hardware in the Loop simulation mode.
-	 *
-	 * @param hil_enabled	The new HIL enable/disable state.
-	 * @return		OK if the HIL state changed, ERROR if the
-	 *			requested change could not be made or was
-	 *			redundant.
-	 */
-	int		set_hil_on_off(bool hil_enabled);
-
 	/**
 	 * Send all parameters at once.
 	 *
@@ -390,8 +393,6 @@ private:
 
 	int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval);
 
-	void get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode);
-
 	/**
 	 * Callback for param interface.
 	 */
@@ -399,11 +400,6 @@ private:
 
 	static int	mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg);
 
-	/**
-	 * Shim for calling task_main from task_create.
-	 */
-	void		task_main_trampoline(int argc, char *argv[]);
-
 	/**
 	 * Main mavlink task.
 	 */

src/modules/mavlink/mavlink_orb_listener.cpp

@@ -296,11 +296,11 @@ MavlinkOrbListener::l_vehicle_status(const struct listener *l)
 }
 
 void
-MavlinkOrbListener::l_rc__mavlink->get_chan()nels(const struct listener *l)
+MavlinkOrbListener::l_rc_channels(const struct listener *l)
 {
-	/* copy rc _mavlink->get_chan()nels into local buffer */
+	/* copy rc channels into local buffer */
-	orb_copy(ORB_ID(rc__mavlink->get_chan()nels), rc_sub, &rc);
+	orb_copy(ORB_ID(rc_channels), l->mavlink->get_subs()->rc_sub, &l->listener->rc);
-	// XXX Add RC _mavlink->get_chan()nels scaled message here
+	// XXX Add RC channels scaled message here
 }
 
 void
@@ -315,7 +315,7 @@ MavlinkOrbListener::l_input_rc(const struct listener *l)
 
 		for (unsigned i = 0; (i * port_width) < (l->listener->rc_raw.channel_count + port_width); i++) {
 			/* Channels are sent in MAVLink main loop at a fixed interval */
-			mavlink_msg_rc_channels_raw_send(_mavlink->get_chan(),
+			mavlink_msg_rc_channels_raw_send(l->mavlink->get_chan(),
 							 l->listener->rc_raw.timestamp / 1000,
 							 i,
 							 (l->listener->rc_raw.channel_count > (i * port_width) + 0) ? l->listener->rc_raw.values[(i * port_width) + 0] : UINT16_MAX,
@@ -335,7 +335,7 @@ void
 MavlinkOrbListener::l_global_position(const struct listener *l)
 {
 	/* copy global position data into local buffer */
-	orb_copy(ORB_ID(vehicle_global_position), _mavlink->get_subs()->global_pos_sub, l->listener->global_pos);
+	orb_copy(ORB_ID(vehicle_global_position), l->mavlink->get_subs()->global_pos_sub, &l->listener->global_pos);
 
 	mavlink_msg_global_position_int_send(l->mavlink->get_chan(),
 						 l->listener->global_pos.timestamp / 1000,
@@ -353,7 +353,7 @@ void
 MavlinkOrbListener::l_local_position(const struct listener *l)
 {
 	/* copy local position data into local buffer */
-	orb_copy(ORB_ID(vehicle_local_position), _mavlink->get_subs()->local_pos_sub, l->listener->local_pos);
+	orb_copy(ORB_ID(vehicle_local_position), l->mavlink->get_subs()->local_pos_sub, &l->listener->local_pos);
 
 	if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD)
 		mavlink_msg_local_position_ned_send(l->mavlink->get_chan(),
@@ -407,7 +407,7 @@ MavlinkOrbListener::l_attitude_setpoint(const struct listener *l)
 	struct vehicle_attitude_setpoint_s att_sp;
 
 	/* copy local position data into local buffer */
-	orb_copy(ORB_ID(vehicle_attitude_setpoint), _mavlink->get_subs()->spa_sub, &att_sp);
+	orb_copy(ORB_ID(vehicle_attitude_setpoint), l->mavlink->get_subs()->spa_sub, &att_sp);
 
 	if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD)
 		mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(l->mavlink->get_chan(),
@@ -464,13 +464,13 @@ MavlinkOrbListener::l_actuator_outputs(const struct listener *l)
 						  act_outputs.output[7]);
 
 		/* only send in HIL mode and only send first group for HIL */
-		if (l->listener->mavlink_hil_enabled && l->listener->armed.armed && ids[l->arg] == ORB_ID(actuator_outputs_0)) {
+		if (l->mavlink->hil_enabled() && l->listener->armed.armed && ids[l->arg] == ORB_ID(actuator_outputs_0)) {
 
 			/* translate the current syste state to mavlink state and mode */
 			uint8_t mavlink_state = 0;
 			uint8_t mavlink_base_mode = 0;
 			uint32_t mavlink_custom_mode = 0;
-			get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
+			l->mavlink->get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
 
 			/* HIL message as per MAVLink spec */
 
@@ -616,9 +616,9 @@ MavlinkOrbListener::l_optical_flow(const struct listener *l)
 void
 MavlinkOrbListener::l_home(const struct listener *l)
 {
-	orb_copy(ORB_ID(home_position), l->mavlink->get_subs()->home_sub, &home);
+	orb_copy(ORB_ID(home_position), l->mavlink->get_subs()->home_sub, &l->listener->home);
 
-	mavlink_msg_gps_global_origin_send(l->mavlink->get_chan(), (int32_t)(home.lat*1e7d), (int32_t)(home.lon*1e7d), (int32_t)(home.alt)*1e3f);
+	mavlink_msg_gps_global_origin_send(l->mavlink->get_chan(), (int32_t)(l->listener->home.lat*1e7d), (int32_t)(l->listener->home.lon*1e7d), (int32_t)(l->listener->home.alt)*1e3f);
 }
 
 void

src/modules/mavlink/mavlink_orb_listener.h

@@ -170,6 +170,7 @@ private:
 	struct actuator_controls_s actuators_0;
 	struct vehicle_attitude_s att;
 	struct airspeed_s airspeed;
+	struct home_position_s home;
 
 	int status_sub;
 	int rc_sub;

src/modules/mavlink/mavlink_receiver.cpp

@@ -106,7 +106,8 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	telemetry_status_pub(-1),
 	lat0(0),
 	lon0(0),
-	alt0(0)
+	alt0(0),
+	thread_should_exit(false)
 {
 
 }
@@ -818,7 +819,7 @@ MavlinkReceiver::receive_thread(void *arg)
 
 			/* if read failed, this loop won't execute */
 			for (ssize_t i = 0; i < nread; i++) {
-				if (mavlink_parse_char(chan, buf[i], &msg, &status)) {
+				if (mavlink_parse_char(_mavlink->get_chan(), buf[i], &msg, &status)) {
 					/* handle generic messages and commands */
 					handle_message(&msg);
 

src/modules/mavlink/mavlink_receiver.h

@@ -102,7 +102,7 @@ public:
 
 private:
 
-	bool		_task_should_exit;		/**< if true, sensor task should exit */
+	bool		thread_should_exit;		/**< if true, sensor task should exit */
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 

src/modules/mavlink/module.mk

@@ -39,7 +39,6 @@ MODULE_COMMAND	 = mavlink
 SRCS		 += mavlink_main.cpp \
 			mavlink.c \
 			mavlink_receiver.cpp \
-			mavlink_orb_listener.cpp \
+			mavlink_orb_listener.cpp
-			waypoints.cpp
 
 INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink