mavlink: -r (datarate) parameter implemented, minor fixes

5 files changed, 65 insertions, 30 deletions

diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb
index 3d8be089e..558be4275 100644
--- a/ROMFS/px4fmu_common/init.d/rc.usb
+++ b/ROMFS/px4fmu_common/init.d/rc.usb
@@ -5,7 +5,7 @@
 
 echo "Starting MAVLink on this USB console"
 
-mavlink start -b 230400 -d /dev/ttyACM0
+mavlink start -r 10000 -d /dev/ttyACM0
 
 # Exit shell to make it available to MAVLink
 exit 
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 76f021e33..c3065b6fc 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -390,14 +390,14 @@ then
 		if [ $TTYS1_BUSY == yes ]
 		then
 			# Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else
-			mavlink start -d /dev/ttyS0
+			mavlink start -r 1000 -d /dev/ttyS0
 			usleep 5000
 			
 			# Exit from nsh to free port for mavlink
 			set EXIT_ON_END yes
 		else
 			# Start MAVLink on default port: ttyS1
-			mavlink start
+			mavlink start -r 1000
 			usleep 5000
 		fi
 	fi
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 8a026742c..974fd27bf 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -87,6 +87,7 @@
 #endif
 static const int ERROR = -1;
 
+#define MAX_DATA_RATE	10000	// max data rate in bytes/s
 #define MAIN_LOOP_DELAY 10000	// 100 Hz
 
 static Mavlink* _head = nullptr;
@@ -449,7 +450,6 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
 	}
 
 	/* open uart */
-	warnx("UART is %s, baudrate is %d\n", uart_name, baud);
 	_uart = open(uart_name, O_RDWR | O_NOCTTY);
 
 	/* Try to set baud rate */
@@ -1407,6 +1407,7 @@ Mavlink::task_main(int argc, char *argv[])
 
 	int ch;
 	_baudrate = 57600;
+	_datarate = 0;
 	_channel = MAVLINK_COMM_0;
 
 	_mode = MODE_OFFBOARD;
@@ -1415,7 +1416,7 @@ Mavlink::task_main(int argc, char *argv[])
 	argc -= 2;
 	argv += 2;
 
-	while ((ch = getopt(argc, argv, "b:d:eov")) != EOF) {
+	while ((ch = getopt(argc, argv, "b:r:d:m:v")) != EOF) {
 		switch (ch) {
 		case 'b':
 			_baudrate = strtoul(optarg, NULL, 10);
@@ -1425,6 +1426,14 @@ Mavlink::task_main(int argc, char *argv[])
 
 			break;
 
+		case 'r':
+			_datarate = strtoul(optarg, NULL, 10);
+
+			if (_datarate < 10 || _datarate > MAX_DATA_RATE)
+				errx(1, "invalid data rate '%s'", optarg);
+
+			break;
+
 		case 'd':
 			device_name = optarg;
 			break;
@@ -1433,8 +1442,19 @@ Mavlink::task_main(int argc, char *argv[])
 //			mavlink_link_termination_allowed = true;
 //			break;
 
-		case 'o':
-			_mode = MODE_ONBOARD;
+		case 'm':
+			if (strcmp(optarg, "offboard") == 0) {
+				_mode = MODE_OFFBOARD;
+
+			} else if (strcmp(optarg, "onboard") == 0) {
+				_mode = MODE_ONBOARD;
+
+			} else if (strcmp(optarg, "hil") == 0) {
+				_mode = MODE_HIL;
+
+			} else if (strcmp(optarg, "custom") == 0) {
+				_mode = MODE_CUSTOM;
+			}
 			break;
 
 		case 'v':
@@ -1447,51 +1467,61 @@ Mavlink::task_main(int argc, char *argv[])
 		}
 	}
 
-	if (Mavlink::instance_exists(device_name, this)) {
-		errx(1, "mavlink instance for %s already running", device_name);
+	if (_datarate == 0) {
+		/* convert bits to bytes and use 1/2 of bandwidth by default */
+		_datarate = _baudrate / 20;
 	}
 
-	struct termios uart_config_original;
+	if (_datarate > MAX_DATA_RATE) {
+		_datarate = MAX_DATA_RATE;
+	}
 
-	bool usb_uart;
+	if (Mavlink::instance_exists(device_name, this)) {
+		errx(1, "mavlink instance for %s already running", device_name);
+	}
 
 	/* inform about mode */
 	switch (_mode) {
-		case MODE_TX_HEARTBEAT_ONLY:
-			warnx("MODE_TX_HEARTBEAT_ONLY");
+		case MODE_CUSTOM:
+			warnx("mode: CUSTOM");
 			break;
 		case MODE_OFFBOARD:
-			warnx("MODE_OFFBOARD");
+			warnx("mode: OFFBOARD");
 			break;
 		case MODE_ONBOARD:
-			warnx("MODE_ONBOARD");
+			warnx("mode: ONBOARD");
 			break;
 		case MODE_HIL:
-			warnx("MODE_HIL");
+			warnx("mode: HIL");
 			break;
 		default:
-			warnx("Error: Unknown mode");
+			warnx("ERROR: Unknown mode");
 			break;
 	}
 
 	switch(_mode) {
 		case MODE_OFFBOARD:
 		case MODE_HIL:
+		case MODE_CUSTOM:
 			_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 */
+	warnx("data rate: %d bytes/s", _datarate);
+	warnx("port: %s, baudrate: %d", device_name, _baudrate);
+
+	/* flush stdout in case MAVLink is about to take it over */
 	fflush(stdout);
 
+	struct termios uart_config_original;
+	bool usb_uart;
+
 	/* default values for arguments */
 	_uart = mavlink_open_uart(_baudrate, device_name, &uart_config_original, &usb_uart);
 
@@ -1529,17 +1559,14 @@ Mavlink::task_main(int argc, char *argv[])
 	warnx("started");
 	mavlink_log_info(_mavlink_fd, "[mavlink] started");
 
-	/* add default streams depending on mode, intervals depend on baud rate */
-	float rate_mult = _baudrate / 57600.0f;
-	if (rate_mult > 4.0f) {
-		rate_mult = 4.0f;
-	}
+	/* add default streams depending on mode and intervals depending on datarate */
+	float rate_mult = _datarate / 1000.0f;
 
 	add_stream("HEARTBEAT", 1.0f);
 
 	switch(_mode) {
 	case MODE_OFFBOARD:
-		add_stream("SYS_STATUS", 1.0f * rate_mult);
+		add_stream("SYS_STATUS", 1.0f);
 		add_stream("GPS_GLOBAL_ORIGIN", 0.5f * rate_mult);
 		add_stream("HIGHRES_IMU", 1.0f * rate_mult);
 		add_stream("ATTITUDE", 10.0f * rate_mult);
@@ -1550,13 +1577,14 @@ Mavlink::task_main(int argc, char *argv[])
 		break;
 
 	case MODE_HIL:
-		add_stream("SYS_STATUS", 1.0f * rate_mult);
+		add_stream("SYS_STATUS", 1.0f);
 		add_stream("GPS_GLOBAL_ORIGIN", 0.5f * rate_mult);
 		add_stream("HIGHRES_IMU", 1.0f * rate_mult);
 		add_stream("ATTITUDE", 10.0f * rate_mult);
 		add_stream("GPS_RAW_INT", 1.0f * rate_mult);
 		add_stream("GLOBAL_POSITION_INT", 5.0f * rate_mult);
 		add_stream("LOCAL_POSITION_NED", 5.0f * rate_mult);
+		add_stream("HIL_CONTROLS", 20.0f * rate_mult);
 		break;
 
 	default:
@@ -1703,7 +1731,12 @@ Mavlink::status()
 
 static void usage()
 {
-	errx(1, "usage: mavlink {start|stop-all} [-d device] [-b baudrate] [-o] [-v]");
+	errx(1, "usage: mavlink {start|stop-all} [-d device] [-b baudrate] [-r datarate] [-m mode] [-v]\n\n"
+			"Supported modes (-m):\n"
+			"\toffboard\tSend standard telemetry data to ground station (default)\n"
+			"\tonboard\tOnboard comminication mode, e.g. to connect PX4FLOW\n"
+			"\thil\tHardware In the Loop mode, send telemetry and HIL_CONTROLS\n"
+			"\tcustom\tCustom configuration, don't send anything by default, streams can be enabled by 'mavlink stream' command\n");
 }
 
 int mavlink_main(int argc, char *argv[])
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 506b4317a..afbf85787 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -166,7 +166,7 @@ public:
 	const char *device_name;
 
 	enum MAVLINK_MODE {
-		MODE_TX_HEARTBEAT_ONLY=0,
+		MODE_CUSTOM=0,
 		MODE_OFFBOARD,
 		MODE_ONBOARD,
 		MODE_HIL
@@ -245,6 +245,7 @@ private:
 	bool _verbose;
 	int _uart;
 	int _baudrate;
+	int _datarate;
 
 	/**
 	 * If the queue index is not at 0, the queue sending
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index b828420e6..a3546e954 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -250,7 +250,8 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		if (mavlink_system.sysid < 4) {
 
 			/* switch to a receiving link mode */
-			_mavlink->set_mode(Mavlink::MODE_TX_HEARTBEAT_ONLY);
+			//TODO why do we need this?
+			//_mavlink->set_mode(Mavlink::MODE_TX_HEARTBEAT_ONLY);
 
 			/*
 			 * rate control mode - defined by MAVLink