Mavlink: get rid of some warnings, initialize channel

4 files changed, 32 insertions, 41 deletions

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index b5bf9ece0..21ba51b21 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -36,15 +36,18 @@
  * MAVLink 1.0 protocol implementation.
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
  */
 
 #include <nuttx/config.h>
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
+#include <stdbool.h>
 #include <unistd.h>
 #include <fcntl.h>
 #include <errno.h>
+#include <assert.h>
 #include <math.h>
 #include <poll.h>
 #include <termios.h>
@@ -66,27 +69,15 @@
 #include <systemlib/perf_counter.h>
 #include <systemlib/systemlib.h>
 #include <geo/geo.h>
+#include <dataman/dataman.h>
 #include <mathlib/mathlib.h>
 #include <mavlink/mavlink_log.h>
 #include <sys/types.h>
 #include <sys/stat.h>
 #include <commander/px4_custom_mode.h>
 
-
 #include "mavlink_bridge_header.h"
-#include <uORB/uORB.h>
 #include "math.h" /* isinf / isnan checks */
-#include <assert.h>
-#include <stdio.h>
-#include <fcntl.h>
-#include <unistd.h>
-#include <stdbool.h>
-#include <string.h>
-#include <errno.h>
-#include <systemlib/param/param.h>
-#include <systemlib/err.h>
-#include <sys/stat.h>
-#include <dataman/dataman.h>
 #include "mavlink_main.h"
 #include "mavlink_orb_listener.h"
 #include "mavlink_receiver.h"
@@ -144,8 +135,11 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
 		#endif
 	}
 
-	size_t desired = (size_t)(sizeof(uint8_t) * length);
-	int ret = write(uart, ch, desired);
+	warnx("uart: %d", uart);
+	warnx("channel: %d", channel);
+
+	ssize_t desired = (sizeof(uint8_t) * length);
+	ssize_t ret = write(uart, ch, desired);
 
 	if (ret != desired)
 		warn("write err");
@@ -160,12 +154,11 @@ namespace mavlink
 	Mavlink	*g_mavlink;
 }
 
-Mavlink::Mavlink() : 
-
+Mavlink::Mavlink() :
+	_mavlink_fd(-1),
 	_task_should_exit(false),
 	thread_running(false),
 	_mavlink_task(-1),
-	_mavlink_fd(-1),
 	_mavlink_incoming_fd(-1),
 
 /* performance counters */
@@ -276,8 +269,6 @@ Mavlink::parameters_update()
 int
 Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg)
 {
-	static unsigned int total_counter = 0;
-
 	switch (cmd) {
 	case (int)MAVLINK_IOC_SEND_TEXT_INFO:
 	case (int)MAVLINK_IOC_SEND_TEXT_CRITICAL:
@@ -430,7 +421,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
 }
 
 int
-Mavlink::set_hil_on_off(bool hil_enabled)
+Mavlink::set_hil_enabled(bool hil_enabled)
 {
 	int ret = OK;
 
@@ -1398,7 +1389,7 @@ Mavlink::mavlink_missionlib_send_message(mavlink_message_t *msg)
 {
 	uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg);
 
-	mavlink_send_uart_bytes(chan, missionlib_msg_buf, len);
+	mavlink_send_uart_bytes(_chan, missionlib_msg_buf, len);
 }
 
 
@@ -1441,9 +1432,8 @@ Mavlink::task_main(int argc, char *argv[])
 	fflush(stdout);
 
 	/* initialize logging device */
-	// YYY
-
-	_mavlink_fd = 0;//open(MAVLINK_LOG_DEVICE, 0);
+	// TODO
+	_mavlink_fd = -1;//open(MAVLINK_LOG_DEVICE, 0);
 
 	//mavlink_log_info(_mavlink_fd, "[mavlink] started");
 
@@ -1451,8 +1441,9 @@ Mavlink::task_main(int argc, char *argv[])
 	// mavlink_logbuffer_init(&lb, 10);
 
 	int ch;
-	char *device_name = "/dev/ttyS1";
+	const char *device_name = "/dev/ttyS1";
 	_baudrate = 57600;
+	_chan = MAVLINK_COMM_0;
 
 	/* work around some stupidity in task_create's argv handling */
 	argc -= 2;
@@ -1587,7 +1578,7 @@ Mavlink::task_main(int argc, char *argv[])
 	/* arm counter to go off immediately */
 	unsigned lowspeed_counter = 10;
 
-		/* wakeup source(s) */
+	/* wakeup source(s) */
 	struct pollfd fds[1];
 
 	/* Setup of loop */
@@ -1623,16 +1614,17 @@ Mavlink::task_main(int argc, char *argv[])
 			get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
 
 			/* send heartbeat */
-			mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_base_mode, mavlink_custom_mode, mavlink_state);
+			warnx("send heartbeat, chan: %d", _chan);
+			mavlink_msg_heartbeat_send(_chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_base_mode, mavlink_custom_mode, mavlink_state);
 
 			/* switch HIL mode if required */
 			if (v_status.hil_state == HIL_STATE_ON)
-				set_hil_on_off(true);
+				set_hil_enabled(true);
 			else if (v_status.hil_state == HIL_STATE_OFF)
-				set_hil_on_off(false);
+				set_hil_enabled(false);
 
 			/* send status (values already copied in the section above) */
-			mavlink_msg_sys_status_send(chan,
+			mavlink_msg_sys_status_send(_chan,
 						    v_status.onboard_control_sensors_present,
 						    v_status.onboard_control_sensors_enabled,
 						    v_status.onboard_control_sensors_health,
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index bf8c63d38..8c9430829 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -182,7 +182,7 @@ public:
 	void		set_mode(enum MAVLINK_MODE);
 	enum MAVLINK_MODE		get_mode() { return _mode; }
 
-	bool		hil_enabled() { return _mavlink_hil_enabled; };
+	bool		get_hil_enabled() { return _mavlink_hil_enabled; };
 
 	/**
 	 * Handle waypoint related messages.
@@ -206,7 +206,7 @@ public:
 	 *			requested change could not be made or was
 	 *			redundant.
 	 */
-	int		set_hil_on_off(bool hil_enabled);
+	int		set_hil_enabled(bool hil_enabled);
 
 	struct mavlink_subscriptions {
 		int sensor_sub;
@@ -242,7 +242,7 @@ public:
 	struct mavlink_subscriptions subs;
 
 	struct mavlink_subscriptions* get_subs() { return &subs; }
-	mavlink_channel_t get_chan() { return chan; }
+	mavlink_channel_t get_chan() { return _chan; }
 
 	/** Global position */
 	struct vehicle_global_position_s global_pos;
@@ -289,7 +289,7 @@ private:
 	MAVLINK_MODE _mode;
 
 	uint8_t mavlink_wpm_comp_id;
-	mavlink_channel_t chan;
+	mavlink_channel_t _chan;
 
 //  XXX probably should be in a header... 
 // extern pthread_t receive_start(int uart);
diff --git a/src/modules/mavlink/mavlink_orb_listener.cpp b/src/modules/mavlink/mavlink_orb_listener.cpp
index ffff1838c..6d64569de 100644
--- a/src/modules/mavlink/mavlink_orb_listener.cpp
+++ b/src/modules/mavlink/mavlink_orb_listener.cpp
@@ -276,9 +276,9 @@ MavlinkOrbListener::l_vehicle_status(const struct listener *l)
 
 	/* enable or disable HIL */
 	if (l->listener->v_status.hil_state == HIL_STATE_ON)
-		l->mavlink->set_hil_on_off(true);
+		l->mavlink->set_hil_enabled(true);
 	else if (l->listener->v_status.hil_state == HIL_STATE_OFF)
-		l->mavlink->set_hil_on_off(false);
+		l->mavlink->set_hil_enabled(false);
 
 	/* translate the current syste state to mavlink state and mode */
 	uint8_t mavlink_state = 0;
@@ -464,7 +464,7 @@ 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->mavlink->hil_enabled() && l->listener->armed.armed && ids[l->arg] == ORB_ID(actuator_outputs_0)) {
+		if (l->mavlink->get_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;
@@ -683,7 +683,6 @@ MavlinkOrbListener::uorb_receive_thread(void *arg)
 
 	struct listener* next = _listeners;
 	unsigned i = 0;
-
 	while (next != nullptr) {
 
 		fds[i].fd = *next->subp;
@@ -708,7 +707,7 @@ MavlinkOrbListener::uorb_receive_thread(void *arg)
 
 		} else {
 
-			unsigned i = 0;
+			i = 0;
 			struct listener* cb = _listeners;
 			while (cb != nullptr) {
 
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 3752bde10..94d97fc12 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -346,7 +346,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 	 * COMMAND_LONG message or a SET_MODE message
 	 */
 
-	if (_mavlink->hil_enabled()) {
+	if (_mavlink->get_hil_enabled()) {
 
 		uint64_t timestamp = hrt_absolute_time();