Changed device::px4_device_handle_t to device::file_t

This change allowed the _posix.cpp file changes to be merged
back into the original files.

Signed-off-by: Mark Charlebois <charlebm@gmail.com>

src/modules/commander/module.mk

@@ -46,14 +46,8 @@ SRCS		 	= commander.cpp \
 			accelerometer_calibration.cpp \
 			rc_calibration.cpp \
 			airspeed_calibration.cpp \
-			PreflightCheck.cpp
-
-ifdef ($(PX4_TARGET_OS),nuttx)
-SRCS += 
+			PreflightCheck.cpp \
 			state_machine_helper.cpp 
-else
-SRCS +=			state_machine_helper_posix.cpp 
-endif
 
 MODULE_STACKSIZE = 5000
 

src/modules/commander/state_machine_helper.cpp

@@ -39,6 +39,7 @@
  * @author Julian Oes <julian@oes.ch>
  */
 
+#include <px4_posix.h>
 #include <stdio.h>
 #include <unistd.h>
 #include <stdint.h>
@@ -129,10 +130,12 @@ arming_state_transition(struct vehicle_status_s *status,		///< current vehicle s
 			prearm_ret = prearm_check(status, mavlink_fd);
 		}
 
+#ifdef __PX4_NUTTX
 		/*
 		 * Perform an atomic state update
 		 */
 		irqstate_t flags = irqsave();
+#endif
 
 		/* enforce lockdown in HIL */
 		if (status->hil_state == vehicle_status_s::HIL_STATE_ON) {
@@ -236,8 +239,10 @@ arming_state_transition(struct vehicle_status_s *status,		///< current vehicle s
 			status->arming_state = new_arming_state;
 		}
 
+#ifdef __PX4_NUTTX
 		/* end of atomic state update */
 		irqrestore(flags);
+#endif
 	}
 
 	if (ret == TRANSITION_DENIED) {
@@ -337,6 +342,121 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
 	return ret;
 }
 
+#ifdef PX4_NUTTX
+static transition_result_t disable_publication(const int mavlink_fd)
+{
+	transition_result_t ret;
+
+	/* Disable publication of all attached sensors */
+	/* list directory */
+	DIR		*d;
+	d = opendir("/dev");
+
+	if (d) {
+		struct dirent	*direntry;
+		char devname[24];
+
+		while ((direntry = readdir(d)) != NULL) {
+
+			/* skip serial ports */
+			if (!strncmp("tty", direntry->d_name, 3)) {
+				continue;
+			}
+
+			/* skip mtd devices */
+			if (!strncmp("mtd", direntry->d_name, 3)) {
+				continue;
+			}
+
+			/* skip ram devices */
+			if (!strncmp("ram", direntry->d_name, 3)) {
+				continue;
+			}
+
+			/* skip MMC devices */
+			if (!strncmp("mmc", direntry->d_name, 3)) {
+				continue;
+			}
+
+			/* skip mavlink */
+			if (!strcmp("mavlink", direntry->d_name)) {
+				continue;
+			}
+
+			/* skip console */
+			if (!strcmp("console", direntry->d_name)) {
+				continue;
+			}
+
+			/* skip null */
+			if (!strcmp("null", direntry->d_name)) {
+				continue;
+			}
+
+			snprintf(devname, sizeof(devname), "/dev/%s", direntry->d_name);
+
+			int sensfd = ::open(devname, 0);
+
+			if (sensfd < 0) {
+				warn("failed opening device %s", devname);
+				continue;
+			}
+
+			int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 1);
+			close(sensfd);
+
+			printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR");
+		}
+		closedir(d);
+		ret = TRANSITION_CHANGED;
+		mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
+
+
+	} else {
+		/* failed opening dir */
+		mavlink_log_info(mavlink_fd, "FAILED LISTING DEVICE ROOT DIRECTORY");
+		ret = TRANSITION_DENIED;
+	}
+	return ret;
+}
+
+#else
+
+static transition_result_t disable_publication(const int mavlink_fd)
+{
+	transition_result_t ret;
+	const char *devname;
+	unsigned int handle = 0;
+	for(;;) {
+		devname = px4_get_device_names(&handle);
+		if (devname == NULL)
+			break;
+
+		/* skip mavlink */
+		if (!strcmp("/dev/mavlink", devname)) {
+			continue;
+		}
+
+
+		int sensfd = px4_open(devname, 0);
+
+		if (sensfd < 0) {
+			warn("failed opening device %s", devname);
+			continue;
+		}
+
+		int block_ret = px4_ioctl(sensfd, DEVIOCSPUBBLOCK, 1);
+		px4_close(sensfd);
+
+			printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR");
+	}
+	ret = TRANSITION_CHANGED;
+	mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
+
+	return ret;
+}
+#endif
+
 /**
  * Transition from one hil state to another
  */
@@ -359,77 +479,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub,
 			if (current_status->arming_state == vehicle_status_s::ARMING_STATE_INIT
 			    || current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY
 			    || current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) {
-
-				/* Disable publication of all attached sensors */
-				/* list directory */
-				DIR		*d;
-				d = opendir("/dev");
-
-				if (d) {
-					struct dirent	*direntry;
-					char devname[24];
-
-					while ((direntry = readdir(d)) != NULL) {
-
-						/* skip serial ports */
-						if (!strncmp("tty", direntry->d_name, 3)) {
-							continue;
-						}
-
-						/* skip mtd devices */
-						if (!strncmp("mtd", direntry->d_name, 3)) {
-							continue;
-						}
-
-						/* skip ram devices */
-						if (!strncmp("ram", direntry->d_name, 3)) {
-							continue;
-						}
-
-						/* skip MMC devices */
-						if (!strncmp("mmc", direntry->d_name, 3)) {
-							continue;
-						}
-
-						/* skip mavlink */
-						if (!strcmp("mavlink", direntry->d_name)) {
-							continue;
-						}
-
-						/* skip console */
-						if (!strcmp("console", direntry->d_name)) {
-							continue;
-						}
-
-						/* skip null */
-						if (!strcmp("null", direntry->d_name)) {
-							continue;
-						}
-
-						snprintf(devname, sizeof(devname), "/dev/%s", direntry->d_name);
-
-						int sensfd = ::open(devname, 0);
-
-						if (sensfd < 0) {
-							warn("failed opening device %s", devname);
-							continue;
-						}
-
-						int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 1);
-						close(sensfd);
-
-						printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR");
-					}
-					closedir(d);
-					ret = TRANSITION_CHANGED;
-					mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
-
-
-				} else {
-					/* failed opening dir */
-					mavlink_log_info(mavlink_fd, "FAILED LISTING DEVICE ROOT DIRECTORY");
-					ret = TRANSITION_DENIED;
-				}
+				ret = disable_publication(mavlink_fd);
 			} else {
 				mavlink_log_critical(mavlink_fd, "Not switching to HIL when armed");
 				ret = TRANSITION_DENIED;

src/modules/mavlink/mavlink_main.cpp

@@ -40,6 +40,7 @@
  */
 
 #include <px4_config.h>
+#include <px4_getopt.h>
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
@@ -50,7 +51,9 @@
 #include <assert.h>
 #include <math.h>
 #include <poll.h>
+#ifndef __PX4_QURT
 #include <termios.h>
+#endif
 #include <time.h>
 #include <math.h> /* isinf / isnan checks */
 
@@ -68,7 +71,7 @@
 #include <systemlib/systemlib.h>
 #include <geo/geo.h>
 #include <dataman/dataman.h>
-#include <mathlib/mathlib.h>
+//#include <mathlib/mathlib.h>
 #include <mavlink/mavlink_log.h>
 
 #include <uORB/topics/parameter_update.h>
@@ -96,8 +99,10 @@ static const int ERROR = -1;
 
 static Mavlink *_mavlink_instances = nullptr;
 
+#ifdef __PX4_NUTTX
 /* TODO: if this is a class member it crashes */
 static struct file_operations fops;
+#endif
 
 static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS;
 static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS;
@@ -114,6 +119,9 @@ extern mavlink_system_t mavlink_system;
 static void usage(void);
 
 Mavlink::Mavlink() :
+#ifndef __PX4_NUTTX
+	VDev("mavlink-log", MAVLINK_LOG_DEVICE),
+#endif
 	_device_name(DEFAULT_DEVICE_NAME),
 	_task_should_exit(false),
 	next(nullptr),
@@ -140,7 +148,9 @@ Mavlink::Mavlink() :
 	_forwarding_on(false),
 	_passing_on(false),
 	_ftp_on(false),
+#ifndef __PX4_QURT
 	_uart_fd(-1),
+#endif
 	_baudrate(57600),
 	_datarate(1000),
 	_datarate_events(500),
@@ -175,7 +185,9 @@ Mavlink::Mavlink() :
 	_loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")),
 	_txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe"))
 {
+#ifdef __PX4_NUTTX
 	fops.ioctl = (int (*)(file *, int, long unsigned int))&mavlink_dev_ioctl;
+#endif
 
 	_instance_id = Mavlink::instance_count();
 
@@ -216,7 +228,8 @@ Mavlink::Mavlink() :
 #endif
 
 	default:
-		errx(1, "instance ID is out of range");
+		warnx("instance ID is out of range");
+		px4_task_exit(1);
 		break;
 	}
 
@@ -398,6 +411,7 @@ Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self)
 	}
 }
 
+#ifndef __PX4_QURT
 int
 Mavlink::get_uart_fd(unsigned index)
 {
@@ -415,6 +429,7 @@ Mavlink::get_uart_fd()
 {
 	return _uart_fd;
 }
+#endif // __PX4_QURT
 
 int
 Mavlink::get_instance_id()
@@ -433,7 +448,11 @@ Mavlink::get_channel()
  ****************************************************************************/
 
 int
-Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg)
+#ifdef __PX4_NUTTX
+Mavlink::mavlink_dev_ioctl(struct file *filp, int cmd, unsigned long arg)
+#else
+Mavlink::ioctl(device::file_t *filp, int cmd, unsigned long arg)
+#endif
 {
 	switch (cmd) {
 	case (int)MAVLINK_IOC_SEND_TEXT_INFO:
@@ -549,6 +568,7 @@ int Mavlink::get_component_id()
 	return mavlink_system.compid;
 }
 
+#ifndef __PX4_QURT
 int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb)
 {
 	/* process baud rate */
@@ -604,7 +624,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
 	}
 
 	/* open uart */
-	_uart_fd = open(uart_name, O_RDWR | O_NOCTTY);
+	_uart_fd = ::open(uart_name, O_RDWR | O_NOCTTY);
 
 	if (_uart_fd < 0) {
 		return _uart_fd;
@@ -619,7 +639,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
 	/* Back up the original uart configuration to restore it after exit */
 	if ((termios_state = tcgetattr(_uart_fd, uart_config_original)) < 0) {
 		warnx("ERR GET CONF %s: %d\n", uart_name, termios_state);
-		close(_uart_fd);
+		::close(_uart_fd);
 		return -1;
 	}
 
@@ -635,7 +655,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
 		/* Set baud rate */
 		if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
 			warnx("ERR SET BAUD %s: %d\n", uart_name, termios_state);
-			close(_uart_fd);
+			::close(_uart_fd);
 			return -1;
 		}
 
@@ -643,7 +663,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
 
 	if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &uart_config)) < 0) {
 		warnx("ERR SET CONF %s\n", uart_name);
-		close(_uart_fd);
+		::close(_uart_fd);
 		return -1;
 	}
 
@@ -653,7 +673,11 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
 		 * which is not an issue, but requires a separate call so we can fail silently.
 		 */
 		(void)tcgetattr(_uart_fd, &uart_config);
+#ifdef CRTS_IFLOW
 		uart_config.c_cflag |= CRTS_IFLOW;
+#else
+		uart_config.c_cflag |= CRTSCTS;
+#endif
 		(void)tcsetattr(_uart_fd, TCSANOW, &uart_config);
 
 		/* setup output flow control */
@@ -696,6 +720,8 @@ Mavlink::enable_flow_control(bool enabled)
 	return ret;
 }
 
+#endif
+
 int
 Mavlink::set_hil_enabled(bool hil_enabled)
 {
@@ -727,7 +753,13 @@ Mavlink::get_free_tx_buf()
 	 * flow control if it continues to be full
 	 */
 	int buf_free = 0;
+
+#ifndef __PX4_QURT
+
+// No FIONWRITE on Linux
+#if !defined(__PX4_LINUX)
 	(void) ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free);
+#endif
 
 	if (get_flow_control_enabled() && buf_free < FLOW_CONTROL_DISABLE_THRESHOLD) {
 		/* Disable hardware flow control:
@@ -741,6 +773,7 @@ Mavlink::get_free_tx_buf()
 			enable_flow_control(false);
 		}
 	}
+#endif
 
 	return buf_free;
 }
@@ -795,8 +828,9 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID
 	buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF);
 	buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8);
 
+#ifndef __PX4_QURT
 	/* send message to UART */
-	ssize_t ret = write(_uart_fd, buf, packet_len);
+	ssize_t ret = ::write(_uart_fd, buf, packet_len);
 
 	if (ret != (int) packet_len) {
 		count_txerr();
@@ -806,6 +840,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID
 		_last_write_success_time = _last_write_try_time;
 		count_txbytes(packet_len);
 	}
+#endif
 
 	pthread_mutex_unlock(&_send_mutex);
 }
@@ -845,8 +880,9 @@ Mavlink::resend_message(mavlink_message_t *msg)
 	buf[MAVLINK_NUM_HEADER_BYTES + msg->len] = (uint8_t)(msg->checksum & 0xFF);
 	buf[MAVLINK_NUM_HEADER_BYTES + msg->len + 1] = (uint8_t)(msg->checksum >> 8);
 
+#ifndef __PX4_QURT
 	/* send message to UART */
-	ssize_t ret = write(_uart_fd, buf, packet_len);
+	ssize_t ret = ::write(_uart_fd, buf, packet_len);
 
 	if (ret != (int) packet_len) {
 		count_txerr();
@@ -856,6 +892,7 @@ Mavlink::resend_message(mavlink_message_t *msg)
 		_last_write_success_time = _last_write_try_time;
 		count_txbytes(packet_len);
 	}
+#endif
 
 	pthread_mutex_unlock(&_send_mutex);
 }
@@ -1195,38 +1232,42 @@ Mavlink::task_main(int argc, char *argv[])
 	_datarate = 0;
 	_mode = MAVLINK_MODE_NORMAL;
 
+#ifdef __PX4_NUTTX
 	/* work around some stupidity in task_create's argv handling */
 	argc -= 2;
 	argv += 2;
+#endif
 
 	/* don't exit from getopt loop to leave getopt global variables in consistent state,
 	 * set error flag instead */
 	bool err_flag = false;
+	int myoptind=1;
+	const char *myoptarg = NULL;
 
-	while ((ch = getopt(argc, argv, "b:r:d:m:fpvwx")) != EOF) {
+	while ((ch = px4_getopt(argc, argv, "b:r:d:m:fpvwx", &myoptind, &myoptarg)) != EOF) {
 		switch (ch) {
 		case 'b':
-			_baudrate = strtoul(optarg, NULL, 10);
+			_baudrate = strtoul(myoptarg, NULL, 10);
 
 			if (_baudrate < 9600 || _baudrate > 921600) {
-				warnx("invalid baud rate '%s'", optarg);
+				warnx("invalid baud rate '%s'", myoptarg);
 				err_flag = true;
 			}
 
 			break;
 
 		case 'r':
-			_datarate = strtoul(optarg, NULL, 10);
+			_datarate = strtoul(myoptarg, NULL, 10);
 
 			if (_datarate < 10 || _datarate > MAX_DATA_RATE) {
-				warnx("invalid data rate '%s'", optarg);
+				warnx("invalid data rate '%s'", myoptarg);
 				err_flag = true;
 			}
 
 			break;
 
 		case 'd':
-			_device_name = optarg;
+			_device_name = myoptarg;
 			break;
 
 //		case 'e':
@@ -1234,13 +1275,13 @@ Mavlink::task_main(int argc, char *argv[])
 //			break;
 
 		case 'm':
-			if (strcmp(optarg, "custom") == 0) {
+			if (strcmp(myoptarg, "custom") == 0) {
 				_mode = MAVLINK_MODE_CUSTOM;
 
-			} else if (strcmp(optarg, "camera") == 0) {
+			} else if (strcmp(myoptarg, "camera") == 0) {
 				// left in here for compatibility
 				_mode = MAVLINK_MODE_ONBOARD;
-			} else if (strcmp(optarg, "onboard") == 0) {
+			} else if (strcmp(myoptarg, "onboard") == 0) {
 				_mode = MAVLINK_MODE_ONBOARD;
 			}
 
@@ -1296,6 +1337,7 @@ Mavlink::task_main(int argc, char *argv[])
 	/* flush stdout in case MAVLink is about to take it over */
 	fflush(stdout);
 
+#ifndef __PX4_QURT
 	struct termios uart_config_original;
 
 	/* default values for arguments */
@@ -1305,6 +1347,7 @@ Mavlink::task_main(int argc, char *argv[])
 		warn("could not open %s", _device_name);
 		return ERROR;
 	}
+#endif
 
 	/* initialize send mutex */
 	pthread_mutex_init(&_send_mutex, NULL);
@@ -1319,7 +1362,8 @@ Mavlink::task_main(int argc, char *argv[])
 		 * marker ring buffer approach.
 		 */
 		if (OK != message_buffer_init(2 * sizeof(mavlink_message_t) + 1)) {
-			errx(1, "msg buf:");
+			warnx("msg buf:");
+			return 1;
 		}
 
 		/* initialize message buffer mutex */
@@ -1327,10 +1371,14 @@ Mavlink::task_main(int argc, char *argv[])
 	}
 
 	/* create the device node that's used for sending text log messages, etc. */
+#ifdef __PX4_NUTTX
 	register_driver(MAVLINK_LOG_DEVICE, &fops, 0666, NULL);
+#else
+	register_driver(MAVLINK_LOG_DEVICE, NULL);
+#endif
 
 	/* initialize logging device */
-	_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+	_mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0);
 
 	/* Initialize system properties */
 	mavlink_update_system();
@@ -1556,14 +1604,16 @@ Mavlink::task_main(int argc, char *argv[])
 	/* wait for threads to complete */
 	pthread_join(_receive_thread, NULL);
 
+#ifndef __PX4_QURT
 	/* reset the UART flags to original state */
 	tcsetattr(_uart_fd, TCSANOW, &uart_config_original);
 
 	/* close UART */
-	close(_uart_fd);
+	::close(_uart_fd);
+#endif
 
 	/* close mavlink logging device */
-	close(_mavlink_fd);
+	px4_close(_mavlink_fd);
 
 	if (_passing_on || _ftp_on) {
 		message_buffer_destroy();
@@ -1618,11 +1668,11 @@ Mavlink::start(int argc, char *argv[])
 	// between the starting task and the spawned
 	// task - start_helper() only returns
 	// when the started task exits.
-	task_spawn_cmd(buf,
+	px4_task_spawn_cmd(buf,
 		       SCHED_DEFAULT,
 		       SCHED_PRIORITY_DEFAULT,
 		       2400,
-		       (main_t)&Mavlink::start_helper,
+		       (px4_main_t)&Mavlink::start_helper,
 		       (char * const *)argv);
 
 	// Ensure that this shell command
@@ -1653,7 +1703,7 @@ Mavlink::display_status()
 {
 
 	if (_rstatus.heartbeat_time > 0) {
-		printf("\tGCS heartbeat:\t%llu us ago\n", hrt_elapsed_time(&_rstatus.heartbeat_time));
+		printf("\tGCS heartbeat:\t%llu us ago\n", (unsigned long long)hrt_elapsed_time(&_rstatus.heartbeat_time));
 	}
 
 	printf("\tmavlink chan: #%u\n", _channel);
@@ -1743,11 +1793,13 @@ Mavlink::stream_command(int argc, char *argv[])
 
 			// If the link is not running we should complain, but not fall over
 			// because this is so easy to get wrong and not fatal. Warning is sufficient.
-			errx(0, "mavlink for device %s is not running", device_name);
+			warnx("mavlink for device %s is not running", device_name);
+			return 0;
 		}
 
 	} else {
-		errx(1, "usage: mavlink stream [-d device] -s stream -r rate");
+		warnx("usage: mavlink stream [-d device] -s stream -r rate");
+		return 1;
 	}
 
 	return OK;
@@ -1762,7 +1814,7 @@ int mavlink_main(int argc, char *argv[])
 {
 	if (argc < 2) {
 		usage();
-		exit(1);
+		return 1;
 	}
 
 	if (!strcmp(argv[1], "start")) {
@@ -1771,7 +1823,7 @@ int mavlink_main(int argc, char *argv[])
 	} else if (!strcmp(argv[1], "stop")) {
 		warnx("mavlink stop is deprecated, use stop-all instead");
 		usage();
-		exit(1);
+		return 1;
 
 	} else if (!strcmp(argv[1], "stop-all")) {
 		return Mavlink::destroy_all_instances();
@@ -1784,7 +1836,7 @@ int mavlink_main(int argc, char *argv[])
 
 	} else {
 		usage();
-		exit(1);
+		return 1;
 	}
 
 	return 0;

src/modules/mavlink/mavlink_main.h

@@ -421,7 +421,7 @@ private:
 #ifdef __PX4_NUTTX
 	static int	mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg);
 #else
-	virtual int	ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg);
+	virtual int	ioctl(device::file_t *filp, int cmd, unsigned long arg);
 #endif
 
 	/**

src/modules/mavlink/mavlink_main_posix.cpp

@@ -437,7 +437,7 @@ Mavlink::get_channel()
  ****************************************************************************/
 
 int
-Mavlink::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg)
+Mavlink::ioctl(device::file_t *filp, int cmd, unsigned long arg)
 {
 	switch (cmd) {
 	case (int)MAVLINK_IOC_SEND_TEXT_INFO:

src/modules/mavlink/module.mk

@@ -35,14 +35,8 @@
 # MAVLink protocol to uORB interface process
 #
 
-MODULE_COMMAND	 = mavlink
-ifeq ($(PX4_TARGET_OS),nuttx)
-SRCS		 += 	mavlink_main_nuttx.cpp 
-else
-SRCS		 += 	mavlink_main_posix.cpp 
-endif
-
 SRCS 		 +=	mavlink.c \
+		  	mavlink_main.cpp \
 			mavlink_mission.cpp \
 			mavlink_parameters.cpp \
 			mavlink_orb_subscription.cpp \

src/modules/uORB/MuORB.cpp

@@ -121,16 +121,16 @@ public:
 	ORBDevNode(const struct orb_metadata *meta, const char *name, const char *path, int priority);
 	~ORBDevNode();
 
-	virtual int		open(device::px4_dev_handle_t *handlep);
-	virtual int		close(device::px4_dev_handle_t *handlep);
-	virtual ssize_t		read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen);
-	virtual ssize_t		write(device::px4_dev_handle_t *handlep, const char *buffer, size_t buflen);
-	virtual int		ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg);
+	virtual int		open(device::file_t *filp);
+	virtual int		close(device::file_t *filp);
+	virtual ssize_t		read(device::file_t *filp, char *buffer, size_t buflen);
+	virtual ssize_t		write(device::file_t *filp, const char *buffer, size_t buflen);
+	virtual int		ioctl(device::file_t *filp, int cmd, unsigned long arg);
 
 	static ssize_t		publish(const orb_metadata *meta, orb_advert_t handle, const void *data);
 
 protected:
-	virtual pollevent_t	poll_state(device::px4_dev_handle_t *handlep);
+	virtual pollevent_t	poll_state(device::file_t *filp);
 	virtual void		poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events);
 
 private:
@@ -150,7 +150,7 @@ private:
 	pid_t			_publisher;	/**< if nonzero, current publisher */
 	const int		_priority;	/**< priority of topic */
 
-	SubscriberData		*handlep_to_sd(device::px4_dev_handle_t *handlep);
+	SubscriberData		*filp_to_sd(device::file_t *filp);
 
 	/**
 	 * Perform a deferred update for a rate-limited subscriber.
@@ -173,16 +173,13 @@ private:
 	bool			appears_updated(SubscriberData *sd);
 };
 
-ORBDevNode::SubscriberData	*ORBDevNode::handlep_to_sd(device::px4_dev_handle_t *handlep)
+ORBDevNode::SubscriberData	*ORBDevNode::filp_to_sd(device::file_t *filp)
 {
-	PX4_DEBUG("ORBDevNode::handlep_to_sd %p\n", handlep);
 	ORBDevNode::SubscriberData *sd;
-	if (handlep) {
-		sd = (ORBDevNode::SubscriberData *)(handlep->priv);
-		PX4_DEBUG("   sd = %p \n", sd);
+	if (filp) {
+		sd = (ORBDevNode::SubscriberData *)(filp->priv);
 	}
 	else {
-		PX4_DEBUG("*** ERROR ORBDevNode::handlep_to_sd(0)\n");
 		sd = 0;
 	}
 	return sd;
@@ -209,12 +206,12 @@ ORBDevNode::~ORBDevNode()
 }
 
 int
-ORBDevNode::open(device::px4_dev_handle_t *handlep)
+ORBDevNode::open(device::file_t *filp)
 {
 	int ret;
 
 	/* is this a publisher? */
-	if (handlep->flags == PX4_F_WRONLY) {
+	if (filp->flags == PX4_F_WRONLY) {
 
 		/* become the publisher if we can */
 		lock();
@@ -231,7 +228,7 @@ ORBDevNode::open(device::px4_dev_handle_t *handlep)
 
 		/* now complete the open */
 		if (ret == PX4_OK) {
-			ret = VDev::open(handlep);
+			ret = VDev::open(filp);
 
 			/* open failed - not the publisher anymore */
 			if (ret != PX4_OK)
@@ -242,7 +239,7 @@ ORBDevNode::open(device::px4_dev_handle_t *handlep)
 	}
 
 	/* is this a new subscriber? */
-	if (handlep->flags == PX4_F_RDONLY) {
+	if (filp->flags == PX4_F_RDONLY) {
 
 		/* allocate subscriber data */
 		SubscriberData *sd = new SubscriberData;
@@ -258,16 +255,16 @@ ORBDevNode::open(device::px4_dev_handle_t *handlep)
 		/* set priority */
 		sd->priority = _priority;
 
-		handlep->priv = (void *)sd;
+		filp->priv = (void *)sd;
 
-		ret = VDev::open(handlep);
+		ret = VDev::open(filp);
 
 		if (ret != PX4_OK) {
-			PX4_DEBUG("ERROR: VDev::open failed\n");
+			warnx("ERROR: VDev::open failed\n");
 			delete sd;
 		}
 
-		PX4_DEBUG("ORBDevNode::Open: fd = %d flags = %d, priv = %p cdev = %p\n", handlep->fd, handlep->flags, handlep->priv, handlep->cdev);
+		//warnx("ORBDevNode::Open: fd = %d flags = %d, priv = %p cdev = %p\n", filp->fd, filp->flags, filp->priv, filp->cdev);
 		return ret;
 	}
 
@@ -276,31 +273,30 @@ ORBDevNode::open(device::px4_dev_handle_t *handlep)
 }
 
 int
-ORBDevNode::close(device::px4_dev_handle_t *handlep)
+ORBDevNode::close(device::file_t *filp)
 {
-	PX4_DEBUG("ORBDevNode::close fd = %d\n", handlep->fd);
+	//warnx("ORBDevNode::close fd = %d", filp->fd);
 	/* is this the publisher closing? */
 	if (getpid() == _publisher) {
 		_publisher = 0;
 
 	} else {
-		SubscriberData *sd = handlep_to_sd(handlep);
+		SubscriberData *sd = filp_to_sd(filp);
 
 		if (sd != nullptr) {
-			PX4_DEBUG("  delete handlep->priv %p", handlep->priv);
 			hrt_cancel(&sd->update_call);
 			delete sd;
 		}
 	}
 
-	return VDev::close(handlep);
+	return VDev::close(filp);
 }
 
 ssize_t
-ORBDevNode::read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen)
+ORBDevNode::read(device::file_t *filp, char *buffer, size_t buflen)
 {
-	PX4_DEBUG("ORBDevNode::read fd = %d\n", handlep->fd);
-	SubscriberData *sd = (SubscriberData *)handlep_to_sd(handlep);
+	//warnx("ORBDevNode::read fd = %d\n", filp->fd);
+	SubscriberData *sd = (SubscriberData *)filp_to_sd(filp);
 
 	/* if the object has not been written yet, return zero */
 	if (_data == nullptr)
@@ -338,9 +334,9 @@ ORBDevNode::read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen)
 }
 
 ssize_t
-ORBDevNode::write(device::px4_dev_handle_t *handlep, const char *buffer, size_t buflen)
+ORBDevNode::write(device::file_t *filp, const char *buffer, size_t buflen)
 {
-	PX4_DEBUG("ORBDevNode::write handlep = %p (null is normal)\n", handlep);
+	//warnx("ORBDevNode::write filp = %p (null is normal)", filp);
 	/*
 	 * Writes are legal from interrupt context as long as the
 	 * object has already been initialised from thread context.
@@ -348,7 +344,7 @@ ORBDevNode::write(device::px4_dev_handle_t *handlep, const char *buffer, size_t
 	 * Writes outside interrupt context will allocate the object
 	 * if it has not yet been allocated.
 	 *
-	 * Note that handlep will usually be NULL.
+	 * Note that filp will usually be NULL.
 	 */
 	if (nullptr == _data) {
 		lock();
@@ -385,10 +381,10 @@ ORBDevNode::write(device::px4_dev_handle_t *handlep, const char *buffer, size_t
 }
 
 int
-ORBDevNode::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg)
+ORBDevNode::ioctl(device::file_t *filp, int cmd, unsigned long arg)
 {
-	PX4_DEBUG("ORBDevNode::ioctl fd = %d cmd = %d\n", handlep->fd, cmd);
-	SubscriberData *sd = handlep_to_sd(handlep);
+	//warnx("ORBDevNode::ioctl fd = %d cmd = %d", filp->fd, cmd);
+	SubscriberData *sd = filp_to_sd(filp);
 
 	switch (cmd) {
 	case ORBIOCLASTUPDATE:
@@ -413,17 +409,17 @@ ORBDevNode::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg)
 
 	default:
 		/* give it to the superclass */
-		return VDev::ioctl(handlep, cmd, arg);
+		return VDev::ioctl(filp, cmd, arg);
 	}
 }
 
 ssize_t
 ORBDevNode::publish(const orb_metadata *meta, orb_advert_t handle, const void *data)
 {
-	PX4_DEBUG("ORBDevNode::publish meta = %p\n", meta);
+	//warnx("ORBDevNode::publish meta = %p", meta);
 
 	if (handle < 0) {
-		PX4_DEBUG("ORBDevNode::publish called with invalid handle\n", meta);
+		warnx("ORBDevNode::publish called with invalid handle");
 		errno = EINVAL;
 		return ERROR;
 	}
@@ -452,10 +448,10 @@ ORBDevNode::publish(const orb_metadata *meta, orb_advert_t handle, const void *d
 }
 
 pollevent_t
-ORBDevNode::poll_state(device::px4_dev_handle_t *handlep)
+ORBDevNode::poll_state(device::file_t *filp)
 {
-	PX4_DEBUG("ORBDevNode::poll_state fd = %d\n", handlep->fd);
-	SubscriberData *sd = handlep_to_sd(handlep);
+	//warnx("ORBDevNode::poll_state fd = %d", filp->fd);
+	SubscriberData *sd = filp_to_sd(filp);
 
 	/*
 	 * If the topic appears updated to the subscriber, say so.
@@ -469,8 +465,8 @@ ORBDevNode::poll_state(device::px4_dev_handle_t *handlep)
 void
 ORBDevNode::poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events)
 {
-	PX4_DEBUG("ORBDevNode::poll_notify_one fds = %p fds->priv = %p\n", fds, fds->priv);
-	SubscriberData *sd = handlep_to_sd((device::px4_dev_handle_t *)fds->priv);
+	//warnx("ORBDevNode::poll_notify_one fds = %p fds->priv = %p", fds, fds->priv);
+	SubscriberData *sd = filp_to_sd((device::file_t *)fds->priv);
 
 	/*
 	 * If the topic looks updated to the subscriber, go ahead and notify them.
@@ -482,7 +478,7 @@ ORBDevNode::poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events)
 bool
 ORBDevNode::appears_updated(SubscriberData *sd)
 {
-	PX4_DEBUG("ORBDevNode::appears_updated sd = %p\n", sd);
+	//warnx("ORBDevNode::appears_updated sd = %p", sd);
 	/* assume it doesn't look updated */
 	bool ret = false;
 
@@ -583,7 +579,7 @@ public:
 	ORBDevMaster(Flavor f);
 	~ORBDevMaster();
 
-	virtual int		ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg);
+	virtual int		ioctl(device::file_t *filp, int cmd, unsigned long arg);
 private:
 	Flavor			_flavor;
 };
@@ -603,7 +599,7 @@ ORBDevMaster::~ORBDevMaster()
 }
 
 int
-ORBDevMaster::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg)
+ORBDevMaster::ioctl(device::file_t *filp, int cmd, unsigned long arg)
 {
 	int ret;
 
@@ -696,7 +692,7 @@ ORBDevMaster::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long ar
 
 	default:
 		/* give it to the superclass */
-		return VDev::ioctl(handlep, cmd, arg);
+		return VDev::ioctl(filp, cmd, arg);
 	}
 }
 
@@ -1242,7 +1238,7 @@ node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool adve
 orb_advert_t
 orb_advertise(const struct orb_metadata *meta, const void *data)
 {
-	PX4_DEBUG("orb_advertise meta = %p\n", meta);
+	//warnx("orb_advertise meta = %p", meta);
 	return orb_advertise_multi(meta, data, nullptr, ORB_PRIO_DEFAULT);
 }
 
@@ -1252,29 +1248,27 @@ orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *inst
 	int result, fd;
 	orb_advert_t advertiser;
 
-	PX4_DEBUG("orb_advertise_multi meta = %p\n", meta);
+	//warnx("orb_advertise_multi meta = %p\n", meta);
 
 	/* open the node as an advertiser */
 	fd = node_open(PUBSUB, meta, data, true, instance, priority);
 	if (fd == ERROR) {
-		PX4_DEBUG("  node_open as advertiser failed.\n");
+		warnx("node_open as advertiser failed.");
 		return ERROR;
 	}
 
-	PX4_DEBUG("  node_open as advertiser passed. fd = %d, instance = %p %d\n", fd, instance, instance != 0 ? *instance : 0);
-
 	/* get the advertiser handle and close the node */
 	result = px4_ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser);
 	px4_close(fd);
 	if (result == ERROR) {
-		PX4_DEBUG("  px4_ioctl ORBIOCGADVERTISER  failed. fd = %d\n", fd);
+		warnx("px4_ioctl ORBIOCGADVERTISER  failed. fd = %d", fd);
 		return ERROR;
 	}
 
 	/* the advertiser must perform an initial publish to initialise the object */
 	result = orb_publish(meta, advertiser, data);
 	if (result == ERROR) {
-		PX4_DEBUG("  orb_publish failed\n");
+		warnx("orb_publish failed");
 		return ERROR;
 	}