Iridium driver and support

Mavlink module implement HIGH_LATENCY (Iridium)

ROMFS/px4fmu_common/init.d/rcS

@@ -600,6 +600,11 @@ then
 		then
 			mavlink start -d $MAVLINK_COMPANION_DEVICE -b 57600 -r 1000
 		fi
+		if param compare SYS_COMPANION 419200
+		then
+			iridiumsbd start -d /dev/ttyS2
+			mavlink start -d /dev/iridium -b 19200 -m iridium -r 10
+		fi
 		if param compare SYS_COMPANION 1921600
 		then
 			mavlink start -d $MAVLINK_COMPANION_DEVICE -b 921600 -r 20000

cmake/configs/nuttx_px4fmu-v2_default.cmake

@@ -48,6 +48,7 @@ set(config_module_list
 	drivers/bst
 	#drivers/snapdragon_rc_pwm
 	drivers/lis3mdl
+	drivers/iridiumsbd
 
 	#
 	# System commands

cmake/configs/nuttx_px4fmu-v3_default.cmake

@@ -23,6 +23,7 @@ set(config_module_list
 	drivers/hott
 	drivers/hott/hott_sensors
 	drivers/hott/hott_telemetry
+	drivers/iridiumsbd
 	drivers/l3gd20
 	drivers/led
 	drivers/lis3mdl

cmake/configs/nuttx_px4fmu-v4_default.cmake

@@ -50,6 +50,7 @@ set(config_module_list
 	drivers/bma180
 	drivers/bmi160
 	drivers/tap_esc
+	drivers/iridiumsbd
 
 	#
 	# System commands

msg/telemetry_status.msg

@@ -3,6 +3,7 @@ uint8 TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO = 1
 uint8 TELEMETRY_STATUS_RADIO_TYPE_UBIQUITY_BULLET = 2
 uint8 TELEMETRY_STATUS_RADIO_TYPE_WIRE = 3
 uint8 TELEMETRY_STATUS_RADIO_TYPE_USB = 4
+uint8 TELEMETRY_STATUS_RADIO_TYPE_IRIDIUM = 5
 
 uint64 heartbeat_time			# Time of last received heartbeat from remote system
 uint64 telem_time			# Time of last received telemetry status packet, 0 for none

src/drivers/drv_iridiumsbd.h

@@ -0,0 +1,8 @@
+#pragma once
+
+#include <stdint.h>
+#include <sys/ioctl.h>
+
+#include "board_config.h"
+
+#define IRIDIUMSBD_DEVICE_PATH	"/dev/iridium"

src/drivers/iridiumsbd/CMakeLists.txt

@@ -0,0 +1,45 @@
+############################################################################
+#
+#   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+px4_add_module(
+	MODULE drivers__iridiumsbd
+	MAIN iridiumsbd
+	STACK 1024
+	STACK_MAIN 1024
+	COMPILE_FLAGS
+		-Os
+	SRCS
+		IridiumSBD.cpp
+		iridiumsbd_params.c
+	DEPENDS
+		platforms__common
+	)

src/drivers/iridiumsbd/IridiumSBD.cpp

@@ -0,0 +1,863 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "IridiumSBD.h"
+
+#include <errno.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <termios.h>
+#include <pthread.h>
+
+#include <nuttx/config.h>
+#include <systemlib/err.h>
+#include <systemlib/systemlib.h>
+#include <systemlib/scheduling_priorities.h>
+#include <systemlib/param/param.h>
+
+#include "drivers/drv_iridiumsbd.h"
+
+IridiumSBD *IridiumSBD::instance;
+int IridiumSBD::task_handle;
+
+IridiumSBD::IridiumSBD()
+	: CDev("iridiumsbd", IRIDIUMSBD_DEVICE_PATH)
+{
+}
+
+int IridiumSBD::start(int argc, char *argv[])
+{
+	PX4_INFO("starting");
+
+	if (IridiumSBD::instance != nullptr) {
+		PX4_WARN("already started");
+		return PX4_ERROR;
+	}
+
+	IridiumSBD::instance = new IridiumSBD();
+
+	IridiumSBD::task_handle = px4_task_spawn_cmd("iridiumsbd", SCHED_DEFAULT,
+				  SCHED_PRIORITY_SLOW_DRIVER, 1024, (main_t)&IridiumSBD::main_loop_helper, argv);
+
+	return OK;
+}
+
+int IridiumSBD::stop()
+{
+	if (IridiumSBD::instance == nullptr) {
+		PX4_WARN("not started");
+		return PX4_ERROR;
+	}
+
+	PX4_WARN("stopping...");
+
+	IridiumSBD::instance->task_should_exit = true;
+
+	// give it enough time to stop
+	//param_timeout_s = 10;
+
+	// TODO
+	for (int i = 0; (i < 10 + 1) && (IridiumSBD::task_handle != -1); i++) {
+		sleep(1);
+	}
+
+	// well, kill it anyway, though this may crash
+	if (IridiumSBD::task_handle != -1) {
+		PX4_WARN("killing task forcefully");
+
+		::close(IridiumSBD::instance->uart_fd);
+		task_delete(IridiumSBD::task_handle);
+		IridiumSBD::task_handle = -1;
+		delete IridiumSBD::instance;
+		IridiumSBD::instance = nullptr;
+	}
+
+	return OK;
+}
+
+void IridiumSBD::status()
+{
+	if (IridiumSBD::instance == nullptr) {
+		PX4_WARN("not started");
+		return;
+	}
+
+	PX4_INFO("started");
+	PX4_INFO("state: %s", satcom_state_string[instance->state]);
+
+	PX4_INFO("TX buf written: %d", instance->tx_buf_write_idx);
+	PX4_INFO("Signal quality: %d", instance->signal_quality);
+}
+
+void IridiumSBD::test(int argc, char *argv[])
+{
+	if (instance == nullptr) {
+		PX4_WARN("not started");
+		return;
+	}
+
+	if (instance->state != SATCOM_STATE_STANDBY || instance->test_pending) {
+		PX4_WARN("MODEM BUSY!");
+		return;
+	}
+
+	if (argc > 2) {
+		strcpy(instance->test_command, argv[2]);
+
+	} else {
+		instance->test_command[0] = 0;
+	}
+
+	instance->schedule_test();
+}
+
+void IridiumSBD::main_loop_helper(int argc, char *argv[])
+{
+	// start the main loop and stay in it
+	IridiumSBD::instance->main_loop(argc, argv);
+
+	// tear down everything after the main loop exits
+	::close(IridiumSBD::instance->uart_fd);
+	IridiumSBD::task_handle = -1;
+	delete IridiumSBD::instance;
+	IridiumSBD::instance = nullptr;
+
+	PX4_WARN("stopped");
+}
+
+void IridiumSBD::main_loop(int argc, char *argv[])
+{
+	CDev::init();
+
+	pthread_mutex_init(&tx_buf_mutex, NULL);
+
+	int arg_i = 3;
+	int arg_uart_name = 0;
+
+	while (arg_i < argc) {
+		if (!strcmp(argv[arg_i], "-d")) {
+			arg_i++;
+			arg_uart_name = arg_i;
+
+		} else if (!strcmp(argv[arg_i], "-v")) {
+			PX4_WARN("verbose mode ON");
+			verbose = true;
+		}
+
+		arg_i++;
+	}
+
+	if (arg_uart_name == 0) {
+		PX4_WARN("no iridium sbd modem UART port provided!");
+		task_should_exit = true;
+		return;
+	}
+
+	if (open_uart(argv[arg_uart_name]) != SATCOM_UART_OK) {
+		PX4_WARN("failed to open UART port!");
+		task_should_exit = true;
+		return;
+	}
+
+	// disable flow control
+	write_at("AT&K0");
+
+	if (read_at_command() != SATCOM_RESULT_OK) {
+		PX4_WARN("modem not responding");
+		return;
+	}
+
+	// disable command echo
+	write_at("ATE0");
+
+	if (read_at_command() != SATCOM_RESULT_OK) {
+		PX4_WARN("modem not responding");
+		return;
+	}
+
+	param_t param_pointer;
+
+	param_pointer = param_find("ISBD_READINT");
+	param_get(param_pointer, &param_read_interval_s);
+
+	if (param_read_interval_s == -1) {
+		param_read_interval_s = 10;
+	}
+
+	if (verbose) { PX4_INFO("read interval %d", param_read_interval_s); }
+
+	while (!task_should_exit) {
+		switch (state) {
+		case SATCOM_STATE_STANDBY:
+			standby_loop();
+			break;
+
+		case SATCOM_STATE_CSQ:
+			csq_loop();
+			break;
+
+		case SATCOM_STATE_SBDSESSION:
+			sbdsession_loop();
+			break;
+
+		case SATCOM_STATE_TEST:
+			test_loop();
+			break;
+		}
+
+		if (new_state != state) {
+			if (verbose) { PX4_INFO("SWITCHING STATE FROM %s TO %s", satcom_state_string[state], satcom_state_string[new_state]); }
+
+			state = new_state;
+
+		} else {
+			usleep(100000);	// 100ms
+		}
+	}
+}
+
+void IridiumSBD::standby_loop(void)
+{
+	// TODO probably remove this later
+	if (test_pending) {
+		test_pending = false;
+
+		if (!strcmp(test_command, "s")) {
+			write(0, "kreczmer", 8);
+
+		} else if (!strcmp(test_command, "ss")) {
+			write(0, "kreczmerkreczmerkreczmerkreczmerkreczmerkreczmerkreczmerkreczmerkreczmerkreczmerkreczmerkreczmer!!!!!!", 100);
+
+		} else if (!strcmp(test_command, "read")) {
+			rx_session_pending = true;
+
+		} else {
+			test_timer = hrt_absolute_time();
+			start_test();
+			return;
+		}
+	}
+
+	// check for incoming SBDRING, handled inside read_at_command()
+	read_at_command();
+
+	if (param_read_interval_s != 0 && (hrt_absolute_time() - last_read_time) / 1000000 > param_read_interval_s) {
+		rx_session_pending = true;
+	}
+
+	// write the MO buffer when the message stacking time expires
+	if ((tx_buf_write_idx > 0) && (hrt_absolute_time() - last_write_time > SATCOM_TX_STACKING_TIME)) {
+		write_tx_buf();
+	}
+
+	// do not start an SBD session if there is still data in the MT buffer, or it will be lost
+	if ((tx_session_pending || rx_session_pending) && !rx_read_pending) {
+		if (hrt_absolute_time() - last_signal_check < SATCOM_SIGNAL_REFRESH_DELAY && signal_quality > 0) {
+			// clear the MO buffer if we only want to read a message
+			if (rx_session_pending && !tx_session_pending) {
+				if (clear_mo_buffer()) {
+					start_sbd_session();
+				}
+
+			} else {
+				start_sbd_session();
+			}
+
+		} else {
+			start_csq();
+		}
+	}
+
+	// only read the MT buffer if the higher layer (mavlink app) read the previous message
+	if (rx_read_pending && (rx_msg_read_idx == rx_msg_end_idx)) {
+		read_rx_buf();
+	}
+}
+
+void IridiumSBD::csq_loop(void)
+{
+	int res = read_at_command();
+
+	if (res == SATCOM_RESULT_NA) {
+		return;
+	}
+
+	if (res != SATCOM_RESULT_OK) {
+		if (verbose) { PX4_INFO("UPDATE SIGNAL QUALITY: ERROR"); }
+
+		new_state = SATCOM_STATE_STANDBY;
+		return;
+	}
+
+	if (strncmp((const char *)rx_command_buf, "+CSQ:", 5)) {
+		if (verbose) { PX4_INFO("UPDATE SIGNAL QUALITY: WRONG ANSWER:"); }
+
+		if (verbose) { PX4_INFO("%s", rx_command_buf); }
+
+		new_state = SATCOM_STATE_STANDBY;
+		return;
+	}
+
+	signal_quality = rx_command_buf[5] - 48;
+	//signal_check_pending = false;
+	last_signal_check = hrt_absolute_time();
+
+	if (verbose) { PX4_INFO("SIGNAL QUALITY: %d", signal_quality); }
+
+	new_state = SATCOM_STATE_STANDBY;
+
+	// publish telemetry status for logger
+	struct telemetry_status_s tstatus = {};
+
+	tstatus.timestamp = hrt_absolute_time();
+	tstatus.telem_time = tstatus.timestamp;
+	tstatus.type = telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_IRIDIUM;
+	tstatus.rssi = signal_quality;
+	tstatus.txbuf = tx_buf_write_idx;
+
+	if (telemetry_status_pub == nullptr) {
+		int multi_instance;
+		telemetry_status_pub = orb_advertise_multi(ORB_ID(telemetry_status), &tstatus, &multi_instance, ORB_PRIO_LOW);
+
+	} else {
+		orb_publish(ORB_ID(telemetry_status), telemetry_status_pub, &tstatus);
+	}
+}
+
+void IridiumSBD::sbdsession_loop(void)
+{
+	int res = read_at_command();
+
+	if (res == SATCOM_RESULT_NA) {
+		return;
+	}
+
+	if (res != SATCOM_RESULT_OK) {
+		if (verbose) { PX4_INFO("SBD SESSION: ERROR"); }
+
+		if (verbose) { PX4_INFO("SBD SESSION: RESULT %d", res); }
+
+		new_state = SATCOM_STATE_STANDBY;
+		return;
+	}
+
+	if (strncmp((const char *)rx_command_buf, "+SBDIX:", 7)) {
+		if (verbose) { PX4_INFO("SBD SESSION: WRONG ANSWER:"); }
+
+		if (verbose) { PX4_INFO("%s", rx_command_buf); }
+
+		new_state = SATCOM_STATE_STANDBY;
+		return;
+	}
+
+	int mo_status, mt_status, mt_len, mt_queued;
+	const char *p = (const char *)rx_command_buf + 7;
+	char **rx_buf_parse = (char **)&p;
+
+	mo_status = strtol(*rx_buf_parse, rx_buf_parse, 10);
+	(*rx_buf_parse)++;
+	strtol(*rx_buf_parse, rx_buf_parse, 10); // MOMSN, ignore it
+	(*rx_buf_parse)++;
+	mt_status = strtol(*rx_buf_parse, rx_buf_parse, 10);
+	(*rx_buf_parse)++;
+	strtol(*rx_buf_parse, rx_buf_parse, 10); // MTMSN, ignore it
+	(*rx_buf_parse)++;
+	mt_len = strtol(*rx_buf_parse, rx_buf_parse, 10);
+	(*rx_buf_parse)++;
+	mt_queued = strtol(*rx_buf_parse, rx_buf_parse, 10);
+
+	if (verbose) { PX4_INFO("MO ST: %d, MT ST: %d, MT LEN: %d, MT QUEUED: %d", mo_status, mt_status, mt_len, mt_queued); }
+
+	switch (mo_status) {
+	case 0:
+	case 2:
+	case 3:
+	case 4:
+		if (verbose) { PX4_INFO("SBD SESSION: SUCCESS"); }
+
+		ring_pending = false;
+		rx_session_pending = false;
+		tx_session_pending = false;
+		last_read_time = hrt_absolute_time();
+
+		if (mt_len > 0) {
+			rx_read_pending = true;
+		}
+
+		break;
+
+	case 1:
+		if (verbose) { PX4_INFO("SBD SESSION: MO SUCCESS, MT FAIL"); }
+
+		tx_session_pending = false;
+		break;
+
+	case 32:
+		if (verbose) { PX4_INFO("SBD SESSION: NO NETWORK SIGNAL"); }
+
+		signal_quality = 0;
+
+		break;
+
+	default:
+		if (verbose) { PX4_INFO("SBD SESSION: FAILED (%d)", mo_status); }
+	}
+
+	new_state = SATCOM_STATE_STANDBY;
+}
+
+void IridiumSBD::test_loop(void)
+{
+	int res = read_at_command();
+
+	if (res != SATCOM_RESULT_NA) {
+		PX4_INFO("TEST RESULT: %d, LENGTH %d\nDATA:\n%s", res, rx_command_len, rx_command_buf);
+		PX4_INFO("TEST DONE, TOOK %lld MS", (hrt_absolute_time() - test_timer) / 1000);
+		new_state = SATCOM_STATE_STANDBY;
+	}
+}
+
+ssize_t IridiumSBD::write(struct file *filp, const char *buffer, size_t buflen)
+{
+	if (verbose) { PX4_INFO("WRITE: LEN %d, TX WRITTEN: %d", buflen, tx_buf_write_idx); }
+
+	if (buflen > SATCOM_TX_BUF_LEN - tx_buf_write_idx) {
+		return PX4_ERROR;
+	}
+
+	pthread_mutex_lock(&tx_buf_mutex);
+
+	memcpy(tx_buf + tx_buf_write_idx, buffer, buflen);
+
+	tx_buf_write_idx += buflen;
+	last_write_time = hrt_absolute_time();
+
+	pthread_mutex_unlock(&tx_buf_mutex);
+
+	return buflen;
+}
+
+ssize_t IridiumSBD::read(struct file *filp, char *buffer, size_t buflen)
+{
+	if (verbose) { PX4_INFO("READ: LEN %d, RX: %d RX END: %d", buflen, rx_msg_read_idx, rx_msg_end_idx); }
+
+	if (rx_msg_read_idx < rx_msg_end_idx) {
+		size_t bytes_to_copy = rx_msg_end_idx - rx_msg_read_idx;
+
+		if (bytes_to_copy > buflen) {
+			bytes_to_copy = buflen;
+		}
+
+		memcpy(buffer, &rx_msg_buf[rx_msg_read_idx], bytes_to_copy);
+
+		rx_msg_read_idx += bytes_to_copy;
+
+		return bytes_to_copy;
+
+	} else {
+		return -EAGAIN;
+	}
+}
+
+int IridiumSBD::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	switch (cmd) {
+	case FIONREAD: {
+			int count = rx_msg_end_idx - rx_msg_read_idx;
+			*(int *)arg = count;
+
+			return OK;
+		}
+
+	case FIONWRITE: {
+			int count = SATCOM_TX_BUF_LEN - tx_buf_write_idx;
+			*(int *)arg = count;
+
+			return OK;
+		}
+
+	default: {
+
+			/* see if the parent class can make any use of it */
+#ifdef __PX4_NUTTX
+			return CDev::ioctl(filp, cmd, arg);
+#else
+			return VDev::ioctl(filp, cmd, arg);
+#endif
+		}
+	}
+}
+
+pollevent_t IridiumSBD::poll_state(struct file *filp)
+{
+	pollevent_t pollstate = 0;
+
+	if (rx_msg_read_idx < rx_msg_end_idx) {
+		pollstate |= POLLIN;
+	}
+
+	if (SATCOM_TX_BUF_LEN - tx_buf_write_idx > 0) {
+		pollstate |= POLLOUT;
+	}
+
+	return pollstate;
+}
+
+void IridiumSBD::write_tx_buf()
+{
+	if (!is_modem_ready()) {
+		if (verbose) { PX4_INFO("SEND SBD: MODEM NOT READY!"); }
+
+		return;
+	}
+
+	pthread_mutex_lock(&tx_buf_mutex);
+
+	char command[13];
+	sprintf(command, "AT+SBDWB=%d", tx_buf_write_idx);
+	write_at(command);
+
+	if (read_at_command() != SATCOM_RESULT_READY) {
+		if (verbose) { PX4_INFO("SEND SBD: MODEM NOT RESPONDING!"); }
+
+		return;
+	}
+
+	int sum = 0;
+
+	int written = 0;
+
+	while (written != tx_buf_write_idx) {
+		written += ::write(uart_fd, tx_buf + written, tx_buf_write_idx - written);
+	}
+
+	for (int i = 0; i < tx_buf_write_idx; i++) {
+		sum += tx_buf[i];
+	}
+
+	uint8_t checksum[2] = {(uint8_t)(sum / 256), (uint8_t)(sum & 255)};
+	::write(uart_fd, checksum, 2);
+
+	if (verbose) { PX4_INFO("SEND SBD: CHECKSUM %d %d", checksum[0], checksum[1]); }
+
+	if (read_at_command() != SATCOM_RESULT_OK) {
+		if (verbose) { PX4_INFO("SEND SBD: ERROR WHILE WRITING DATA TO MODEM!"); }
+
+		pthread_mutex_unlock(&tx_buf_mutex);
+		return;
+	}
+
+	if (rx_command_buf[0] != '0') {
+		if (verbose) { PX4_INFO("SEND SBD: ERROR WHILE WRITING DATA TO MODEM! (%d)", rx_command_buf[0] - '0'); }
+
+		pthread_mutex_unlock(&tx_buf_mutex);
+		return;
+	}
+
+	if (verbose) { PX4_INFO("SEND SBD: DATA WRITTEN TO MODEM"); }
+
+	tx_buf_write_idx = 0;
+
+	pthread_mutex_unlock(&tx_buf_mutex);
+
+	tx_session_pending = true;
+}
+
+void IridiumSBD::read_rx_buf(void)
+{
+	if (!is_modem_ready()) {
+		if (verbose) { PX4_INFO("READ SBD: MODEM NOT READY!"); }
+
+		return;
+	}
+
+	write_at("AT+SBDRB");
+
+	if (read_at_msg() != SATCOM_RESULT_OK) {
+		if (verbose) { PX4_INFO("READ SBD: MODEM NOT RESPONDING!"); }
+
+		return;
+	}
+
+	int data_len = (rx_msg_buf[0] << 8) + rx_msg_buf[1];
+
+	// rx_buf contains 2 byte length, data, 2 byte checksum and /r/n delimiter
+	if (data_len != rx_msg_end_idx - 6) {
+		if (verbose) { PX4_INFO("READ SBD: WRONG DATA LENGTH"); }
+
+		return;
+	}
+
+	int checksum = 0;
+
+	for (int i = 2; i < data_len + 2; i++) {
+		checksum += rx_msg_buf[i];
+	}
+
+	if ((checksum / 256 != rx_msg_buf[rx_msg_end_idx - 4]) || ((checksum & 255) != rx_msg_buf[rx_msg_end_idx - 3])) {
+		if (verbose) { PX4_INFO("READ SBD: WRONG DATA CHECKSUM"); }
+
+		return;
+	}
+
+	rx_msg_read_idx = 2;	// ignore the length
+	rx_msg_end_idx -= 4;	// ignore the checksum and delimiter
+	rx_read_pending = false;
+
+	if (verbose) { PX4_INFO("READ SBD: SUCCESS, LEN: %d", data_len); }
+}
+
+bool IridiumSBD::clear_mo_buffer()
+{
+	write_at("AT+SBDD0");
+
+	if (read_at_command() != SATCOM_RESULT_OK || rx_command_buf[0] != '0') {
+		if (verbose) { PX4_INFO("CLEAR MO BUFFER: ERROR"); }
+
+		return false;
+	}
+
+	return true;
+}
+
+void IridiumSBD::start_csq(void)
+{
+	if (verbose) { PX4_INFO("UPDATING SIGNAL QUALITY"); }
+
+	if (!is_modem_ready()) {
+		if (verbose) { PX4_INFO("UPDATE SIGNAL QUALITY: MODEM NOT READY!"); }
+
+		return;
+	}
+
+	write_at("AT+CSQ");
+	new_state = SATCOM_STATE_CSQ;
+}
+
+void IridiumSBD::start_sbd_session(void)
+{
+	if (verbose) { PX4_INFO("STARTING SBD SESSION"); }
+
+	if (!is_modem_ready()) {
+		if (verbose) { PX4_INFO("SBD SESSION: MODEM NOT READY!"); }
+
+		return;
+	}
+
+	if (ring_pending) {
+		write_at("AT+SBDIXA");
+
+	} else {
+		write_at("AT+SBDIX");
+	}
+
+	new_state = SATCOM_STATE_SBDSESSION;
+}
+
+void IridiumSBD::start_test(void)
+{
+	int res = read_at_command();
+
+	if (res != SATCOM_RESULT_NA) {
+		PX4_WARN("SOMETHING WAS IN BUFFER");
+		printf("TEST RESULT: %d, LENGTH %d\nDATA:\n%s\nRAW DATA:\n", res, rx_command_len, rx_command_buf);
+
+		for (int i = 0; i < rx_command_len; i++) {
+			printf("%d ", rx_command_buf[i]);
+		}
+
+		printf("\n");
+	}
+
+	if (!is_modem_ready()) {
+		PX4_WARN("MODEM NOT READY!");
+		return;
+	}
+
+	if (strlen(test_command) != 0) {
+		PX4_INFO("TEST %s", test_command);
+		write_at(test_command);
+		new_state = SATCOM_STATE_TEST;
+
+	} else {
+		PX4_INFO("TEST DONE");
+	}
+}
+
+satcom_uart_status IridiumSBD::open_uart(char *uart_name)
+{
+	if (verbose) { PX4_INFO("opening iridium sbd modem UART: %s", uart_name); }
+
+	uart_fd = ::open(uart_name, O_RDWR | O_BINARY);
+
+	if (uart_fd < 0) {
+		if (verbose) { PX4_WARN("UART open failed!"); }
+
+		return SATCOM_UART_OPEN_FAIL;
+	}
+
+	// set the UART speed to 19200
+	struct termios uart_config;
+	tcgetattr(uart_fd, &uart_config);
+	cfsetspeed(&uart_config, 19200);
+	tcsetattr(uart_fd, TCSANOW, &uart_config);
+
+	if (verbose) { PX4_INFO("UART opened"); }
+
+	return SATCOM_UART_OK;
+}
+
+bool IridiumSBD::is_modem_ready(void)
+{
+	write_at("AT");
+
+	if (read_at_command() == SATCOM_RESULT_OK) {
+		return true;
+
+	} else {
+		return false;
+	}
+}
+
+void IridiumSBD::write_at(const char *command)
+{
+	if (verbose) { PX4_INFO("WRITING AT COMMAND: %s", command); }
+
+	::write(uart_fd, command, strlen(command));
+	::write(uart_fd, "\r", 1);
+}
+
+satcom_result_code IridiumSBD::read_at_command()
+{
+	return read_at(rx_command_buf, &rx_command_len);
+}
+
+satcom_result_code IridiumSBD::read_at_msg()
+{
+	return read_at(rx_msg_buf, &rx_msg_end_idx);
+}
+
+satcom_result_code IridiumSBD::read_at(uint8_t *rx_buf, int *rx_len)
+{
+	struct pollfd fds[1];
+	fds[0].fd = uart_fd;
+	fds[0].events = POLLIN;
+
+	uint8_t buf = 0;
+	int last_rn_idx = 0;
+	int rx_buf_pos = 0;
+	*rx_len = 0;
+
+	while (1) {
+		if (::poll(&fds[0], 1, 100) > 0) {
+			if (::read(uart_fd, &buf, 1) > 0) {
+				if (rx_buf_pos == 0 && (buf == '\r' || buf == '\n')) {
+					// ignore the leading \r\n
+					continue;
+				}
+
+				rx_buf[rx_buf_pos++] = buf;
+
+				if (rx_buf[rx_buf_pos - 1] == '\n' && rx_buf[rx_buf_pos - 2] == '\r') {
+					// found the \r\n delimiter
+					if (rx_buf_pos == last_rn_idx + 2) {
+						//if (verbose) { PX4_INFO("second in a row, ignore it");}
+						; // second in a row, ignore it
+
+					} else if (!strncmp((const char *)&rx_buf[last_rn_idx], "OK\r\n", 4)) {
+						rx_buf[*rx_len] = 0; 	// null terminator after the information response for printing purposes
+						return SATCOM_RESULT_OK;
+
+					} else if (!strncmp((const char *)&rx_buf[last_rn_idx], "ERROR\r\n", 7)) {
+						return SATCOM_RESULT_ERROR;
+
+					} else if (!strncmp((const char *)&rx_buf[last_rn_idx], "SBDRING\r\n", 9)) {
+						ring_pending = true;
+						rx_session_pending = true;
+
+						if (verbose) { PX4_INFO("GET SBDRING");}
+
+						return SATCOM_RESULT_SBDRING;
+
+					} else if (!strncmp((const char *)&rx_buf[last_rn_idx], "READY\r\n", 7)) {
+						return SATCOM_RESULT_READY;
+
+					} else if (!strncmp((const char *)&rx_buf[last_rn_idx], "HARDWARE FAILURE", 16)) {
+						PX4_WARN("HARDWARE FAILURE!");
+						return SATCOM_RESULT_HWFAIL;
+
+					} else {
+						*rx_len = rx_buf_pos;	// that was the information response, result code incoming
+					}
+
+					last_rn_idx = rx_buf_pos;
+				}
+			}
+
+		} else {
+			break;
+		}
+	}
+
+	return SATCOM_RESULT_NA;
+}
+
+void IridiumSBD::schedule_test(void)
+{
+	test_pending = true;
+}
+
+int iridiumsbd_main(int argc, char *argv[])
+{
+	if (!strcmp(argv[1], "start")) {
+		return IridiumSBD::start(argc, argv);
+
+	} else if (!strcmp(argv[1], "stop")) {
+		return IridiumSBD::stop();
+
+	} else if (!strcmp(argv[1], "status")) {
+		IridiumSBD::status();
+		return OK;
+
+	} else if (!strcmp(argv[1], "test")) {
+		IridiumSBD::test(argc, argv);
+		return OK;
+	}
+
+	PX4_INFO("usage: iridiumsbd {start|stop|status|test} [-d uart_device | -v]");
+
+	return PX4_ERROR;
+}

src/drivers/iridiumsbd/IridiumSBD.h

@@ -0,0 +1,283 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <stdlib.h>
+#include <stdbool.h>
+
+#include <drivers/device/device.h>
+#include <drivers/drv_hrt.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/telemetry_status.h>
+
+typedef enum {
+	SATCOM_OK = 0,
+	SATCOM_NO_MSG = -1,
+	SATCOM_ERROR = -255,
+} satcom_status;
+
+typedef enum {
+	SATCOM_UART_OK = 0,
+	SATCOM_UART_OPEN_FAIL = -1,
+} satcom_uart_status;
+
+typedef enum {
+	SATCOM_READ_OK = 0,
+	SATCOM_READ_TIMEOUT = -1,
+	SATCOM_READ_PARSING_FAIL = -2,
+} satcom_read_status;
+
+typedef enum {
+	SATCOM_RESULT_OK,
+	SATCOM_RESULT_ERROR,
+	SATCOM_RESULT_SBDRING,
+	SATCOM_RESULT_READY,
+	SATCOM_RESULT_HWFAIL,
+	SATCOM_RESULT_NA,
+} satcom_result_code;
+
+//typedef struct
+//{
+//	uint8_t	info;
+//	uint8_t	result_code;
+//} satcom_at_msg;
+
+typedef enum {
+	SATCOM_STATE_STANDBY,
+	SATCOM_STATE_CSQ,
+	SATCOM_STATE_SBDSESSION,
+	SATCOM_STATE_TEST,
+} satcom_state;
+
+const char *satcom_state_string[4] = {"STANDBY", "SIGNAL CHECK", "SBD SESSION", "TEST"};
+
+extern "C" __EXPORT int iridiumsbd_main(int argc, char *argv[]);
+
+#define SATCOM_TX_BUF_LEN				50		// TX buffer size - maximum for a SBD MO message is 340, but billed per 50
+#define SATCOM_RX_MSG_BUF_LEN			300		// RX buffer size for MT messages
+#define SATCOM_RX_COMMAND_BUF_LEN		50		// RX buffer size for other commands
+#define SATCOM_TX_STACKING_TIME			3000000	// time to wait for additional mavlink messages, TODO make this a param
+#define SATCOM_SIGNAL_REFRESH_DELAY		5000000 // update signal quality every 5s
+
+class IridiumSBD : public device::CDev
+{
+public:
+	static IridiumSBD *instance;
+	static int task_handle;
+	bool task_should_exit = false;
+	int uart_fd = -1;
+
+	int param_read_interval_s;
+
+	hrt_abstime last_signal_check = 0;
+	uint8_t signal_quality = 0;
+
+	orb_advert_t telemetry_status_pub = nullptr;
+
+	bool test_pending = false;
+	char test_command[32];
+	hrt_abstime test_timer = 0;
+
+	uint8_t rx_command_buf[SATCOM_RX_COMMAND_BUF_LEN] = {0};
+	int rx_command_len = 0;
+
+	uint8_t rx_msg_buf[SATCOM_RX_MSG_BUF_LEN] = {0};
+	int rx_msg_end_idx = 0;
+	int rx_msg_read_idx = 0;
+
+	uint8_t tx_buf[SATCOM_TX_BUF_LEN] = {0};
+	int tx_buf_write_idx = 0;
+
+	bool ring_pending = false;
+	bool rx_session_pending = false;
+	bool rx_read_pending = false;
+	bool tx_session_pending = false;
+
+	hrt_abstime last_write_time = 0;
+	hrt_abstime last_read_time = 0;
+
+	satcom_state state = SATCOM_STATE_STANDBY;
+	satcom_state new_state = SATCOM_STATE_STANDBY;
+
+	pthread_mutex_t tx_buf_mutex = pthread_mutex_t();
+	bool verbose = false;
+
+	/*
+	 * Constructor
+	 */
+	IridiumSBD();
+
+	/*
+	 * Start the driver
+	 */
+	static int start(int argc, char *argv[]);
+
+	/*
+	 * Stop the driver
+	 */
+	static int stop();
+
+	/*
+	 * Display driver status
+	 */
+	static void status();
+
+	/*
+	 * Run a basic driver test
+	 */
+	static void test(int argc, char *argv[]);
+
+	/*
+	 * Entry point of the task, has to be a static function
+	 */
+	static void main_loop_helper(int argc, char *argv[]);
+
+	/*
+	 * Main driver loop
+	 */
+	void main_loop(int argc, char *argv[]);
+
+	/*
+	 * Use to send mavlink messages directly
+	 */
+	ssize_t write(struct file *filp, const char *buffer, size_t buflen);
+
+	/*
+	 * Use to read received mavlink messages directly
+	 */
+	ssize_t read(struct file *filp, char *buffer, size_t buflen);
+
+	/*
+	 * Passes everything to CDev
+	 */
+	int ioctl(struct file *filp, int cmd, unsigned long arg);
+
+	/*
+	 * Get the poll state
+	 */
+	pollevent_t poll_state(struct file *filp);
+
+	/*
+	 * Open and configure the given UART port
+	 */
+	satcom_uart_status open_uart(char *uart_name);
+
+	/*
+	 *
+	 */
+	void write_tx_buf();
+
+	/*
+	 *
+	 */
+	void read_rx_buf();
+
+	/*
+	 *
+	 */
+	bool clear_mo_buffer();
+
+	/*
+	 * Perform a SBD session, sending the message from the MO buffer (if previously written)
+	 * and retrieving a MT message from the Iridium system (if there is one waiting)
+	 * This will also update the registration needed for SBD RING
+	 */
+	int sbd_session(void);
+
+	/*
+	 * Get the network signal strength
+	 */
+	void start_csq(void);
+
+	/*
+	 *
+	 */
+	satcom_result_code read_at_command();
+
+	/*
+	 *
+	 */
+	satcom_result_code read_at_msg();
+
+	/*
+	 *
+	 */
+	satcom_result_code read_at(uint8_t *rx_buf, int *rx_len);
+
+	/*
+	 *
+	 */
+	void schedule_test(void);
+
+	/*
+	 *
+	 */
+	void standby_loop(void);
+
+	/*
+	 *
+	 */
+	void csq_loop(void);
+
+	/*
+	 *
+	 */
+	void sbdsession_loop(void);
+
+	/*
+	 *
+	 */
+	void test_loop(void);
+
+	/*
+	 * TEST
+	 */
+	void start_test(void);
+
+	/*
+	 *
+	 */
+	void start_sbd_session(void);
+
+	/*
+	 * Checks if the modem responds to the "AT" command
+	 */
+	bool is_modem_ready(void);
+
+	/*
+	 * Send a AT command to the modem
+	 */
+	void write_at(const char *command);
+};

src/drivers/iridiumsbd/iridiumsbd_params.c

@@ -0,0 +1,11 @@
+#include <systemlib/param/param.h>
+
+/**
+ * Satellite radio read interval
+ *
+ * @unit s
+ * @min 0
+ * @max 300
+ * @group Iridium SBD
+ */
+PARAM_DEFINE_INT32(ISBD_READINT, 10);

src/modules/mavlink/mavlink_main.cpp

@@ -1818,6 +1818,10 @@ Mavlink::task_main(int argc, char *argv[])
 
 			} else if (strcmp(myoptarg, "config") == 0) {
 				_mode = MAVLINK_MODE_CONFIG;
+
+			} else if (strcmp(myoptarg, "iridium") == 0) {
+				_mode = MAVLINK_MODE_IRIDIUM;
+				_rstatus.type = telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_IRIDIUM;
 			}
 
 			break;
@@ -1935,14 +1939,18 @@ Mavlink::task_main(int argc, char *argv[])
 
 	/* add default streams depending on mode */
 
-	/* HEARTBEAT is constant rate stream, rate never adjusted */
+	if (_mode != MAVLINK_MODE_IRIDIUM) {
-	configure_stream("HEARTBEAT", 1.0f);
 
-	/* STATUSTEXT stream is like normal stream but gets messages from logbuffer instead of uORB */
+		/* HEARTBEAT is constant rate stream, rate never adjusted */
-	configure_stream("STATUSTEXT", 20.0f);
+		configure_stream("HEARTBEAT", 1.0f);
 
-	/* COMMAND_LONG stream: use high rate to avoid commands skipping */
+		/* STATUSTEXT stream is like normal stream but gets messages from logbuffer instead of uORB */
-	configure_stream("COMMAND_LONG", 100.0f);
+		configure_stream("STATUSTEXT", 20.0f);
+
+		/* COMMAND_LONG stream: use high rate to avoid commands skipping */
+		configure_stream("COMMAND_LONG", 100.0f);
+
+	}
 
 	/* PARAM_VALUE stream */
 	_parameters_manager = (MavlinkParametersManager *) MavlinkParametersManager::new_instance(this);
@@ -2078,6 +2086,11 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("MISSION_ITEM", 50.0f);
 		configure_stream("ACTUATOR_CONTROL_TARGET0", 30.0f);
 		configure_stream("MANUAL_CONTROL", 5.0f);
+		break;
+
+	case MAVLINK_MODE_IRIDIUM:
+		configure_stream("HIGH_LATENCY", 0.1f);
+		break;
 
 	default:
 		break;
@@ -2441,7 +2454,7 @@ Mavlink::start(int argc, char *argv[])
 	px4_task_spawn_cmd(buf,
 			   SCHED_DEFAULT,
 			   SCHED_PRIORITY_DEFAULT,
-			   2400,
+			   2500,
 			   (px4_main_t)&Mavlink::start_helper,
 			   (char *const *)argv);
 

src/modules/mavlink/mavlink_main.h

@@ -163,7 +163,8 @@ public:
 		MAVLINK_MODE_ONBOARD,
 		MAVLINK_MODE_OSD,
 		MAVLINK_MODE_MAGIC,
-		MAVLINK_MODE_CONFIG
+		MAVLINK_MODE_CONFIG,
+		MAVLINK_MODE_IRIDIUM
 	};
 
 	enum BROADCAST_MODE {
@@ -191,6 +192,9 @@ public:
 		case MAVLINK_MODE_CONFIG:
 			return "Config";
 
+		case MAVLINK_MODE_IRIDIUM:
+			return "Iridium";
+
 		default:
 			return "Unknown";
 		}
@@ -232,14 +236,6 @@ public:
 
 	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(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet,
-			uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode);
-
 	/**
 	 * Enable / disable Hardware in the Loop simulation mode.
 	 *

src/modules/mavlink/mavlink_messages.cpp

@@ -3592,6 +3592,210 @@ protected:
 	}
 };
 
+class MavlinkStreamHighLatency : public MavlinkStream
+{
+public:
+	const char *get_name() const
+	{
+		return MavlinkStreamHighLatency::get_name_static();
+	}
+
+	static const char *get_name_static()
+	{
+		return "HIGH_LATENCY";
+	}
+
+	static uint16_t get_id_static()
+	{
+		return MAVLINK_MSG_ID_HIGH_LATENCY;
+	}
+
+	uint16_t get_id()
+	{
+		return get_id_static();
+	}
+
+	static MavlinkStream *new_instance(Mavlink *mavlink)
+	{
+		return new MavlinkStreamHighLatency(mavlink);
+	}
+
+	unsigned get_size()
+	{
+		return MAVLINK_MSG_ID_HIGH_LATENCY_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+	}
+
+private:
+	MavlinkOrbSubscription *_actuator_sub;
+	uint64_t _actuator_time;
+
+	MavlinkOrbSubscription *_airspeed_sub;
+	uint64_t _airspeed_time;
+
+	MavlinkOrbSubscription *_attitude_sp_sub;
+	uint64_t _attitude_sp_time;
+
+	MavlinkOrbSubscription *_attitude_sub;
+	uint64_t _attitude_time;
+
+	MavlinkOrbSubscription *_battery_sub;
+	uint64_t _battery_time;
+
+	MavlinkOrbSubscription *_fw_pos_ctrl_status_sub;
+	uint64_t _fw_pos_ctrl_status_time;
+
+	MavlinkOrbSubscription *_global_pos_sub;
+	uint64_t _global_pos_time;
+
+	MavlinkOrbSubscription *_gps_sub;
+	uint64_t _gps_time;
+
+	MavlinkOrbSubscription *_home_sub;
+	uint64_t _home_time;
+
+	MavlinkOrbSubscription *_landed_sub;
+	uint64_t _landed_time;
+
+	MavlinkOrbSubscription *_mission_result_sub;
+	uint64_t _mission_result_time;
+
+	MavlinkOrbSubscription *_sensor_sub;
+	uint64_t _sensor_time;
+
+	MavlinkOrbSubscription *_status_sub;
+	uint64_t _status_time;
+
+	MavlinkOrbSubscription *_tecs_status_sub;
+	uint64_t _tecs_time;
+
+	MavlinkOrbSubscription *_wind_sub;
+	uint64_t _wind_time;
+
+	/* do not allow top copying this class */
+	MavlinkStreamHighLatency(MavlinkStreamHighLatency &);
+	MavlinkStreamHighLatency& operator = (const MavlinkStreamHighLatency &);
+
+protected:
+	explicit MavlinkStreamHighLatency(Mavlink *mavlink) : MavlinkStream(mavlink),
+		_actuator_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_0))),
+		_actuator_time(0),
+		_airspeed_sub(_mavlink->add_orb_subscription(ORB_ID(airspeed))),
+		_airspeed_time(0),
+		_attitude_sp_sub(_mavlink->add_orb_subscription(ORB_ID(fw_pos_ctrl_status))),
+		_attitude_sp_time(0),
+		_attitude_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude))),
+		_attitude_time(0),
+		_battery_sub(_mavlink->add_orb_subscription(ORB_ID(battery_status))),
+		_battery_time(0),
+		_fw_pos_ctrl_status_sub(_mavlink->add_orb_subscription(ORB_ID(fw_pos_ctrl_status))),
+		_fw_pos_ctrl_status_time(0),
+		_global_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position))),
+		_global_pos_time(0),
+		_gps_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position))),
+		_gps_time(0),
+		_home_sub(_mavlink->add_orb_subscription(ORB_ID(home_position))),
+		_home_time(0),
+		_landed_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_land_detected))),
+		_landed_time(0),
+		_mission_result_sub(_mavlink->add_orb_subscription(ORB_ID(mission_result))),
+		_mission_result_time(0),
+		_sensor_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined))),
+		_sensor_time(0),
+		_status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))),
+		_status_time(0),
+		_tecs_status_sub(_mavlink->add_orb_subscription(ORB_ID(tecs_status))),
+		_tecs_time(0)
+	{}
+
+	void send(const hrt_abstime t)
+	{
+		struct actuator_controls_s actuator = {};
+		struct airspeed_s airspeed = {};
+		struct battery_status_s battery = {};
+		struct fw_pos_ctrl_status_s fw_pos_ctrl_status = {};
+		struct home_position_s home = {};
+		struct mission_result_s mission_result = {};
+		struct sensor_combined_s sensor = {};
+		struct tecs_status_s tecs_status = {};
+		struct vehicle_attitude_s attitude = {};
+		struct vehicle_attitude_setpoint_s attitude_sp = {};
+		struct vehicle_global_position_s global_pos = {};
+		struct vehicle_gps_position_s gps = {};
+		struct vehicle_land_detected_s land_detected = {};
+		struct vehicle_status_s status = {};
+
+		bool updated = _status_sub->update(&_status_time, &status);
+		updated |= _actuator_sub->update(&_actuator_time, &actuator);
+		updated |= _airspeed_sub->update(&_airspeed_time, &airspeed);
+		updated |= _attitude_sp_sub->update(&_attitude_sp_time, &attitude_sp);
+		updated |= _attitude_sub->update(&_attitude_time, &attitude);
+		updated |= _battery_sub->update(&_battery_time, &battery);
+		updated |= _fw_pos_ctrl_status_sub->update(&_fw_pos_ctrl_status_time, &fw_pos_ctrl_status);
+		updated |= _global_pos_sub->update(&_global_pos_time, &global_pos);
+		updated |= _gps_sub->update(&_gps_time, &gps);
+		updated |= _home_sub->update(&_home_time, &home);
+		updated |= _landed_sub->update(&_landed_time, &land_detected);
+		updated |= _mission_result_sub->update(&_mission_result_time, &mission_result);
+		updated |= _sensor_sub->update(&_sensor_time, &sensor);
+		updated |= _tecs_status_sub->update(&_tecs_time, &tecs_status);
+
+		if (updated) {
+			mavlink_high_latency_t msg = {};
+
+			//timespec tv;
+			//px4_clock_gettime(CLOCK_REALTIME, &tv);
+			//msg.time_usec = (uint64_t)tv.tv_sec * 1000000 + tv.tv_nsec / 1000;
+
+			msg.base_mode = 0;
+			msg.custom_mode = 0;
+			uint8_t sys_status;
+			get_mavlink_mode_state(&status, &sys_status, &msg.base_mode, &msg.custom_mode);
+
+			matrix::Eulerf euler = matrix::Quatf(attitude.q);
+			msg.roll = math::degrees(euler.phi()) * 100;
+			msg.pitch = math::degrees(euler.theta()) * 100;
+			msg.heading = math::degrees(_wrap_2pi(euler.psi())) * 100;
+
+			//msg.roll_sp = math::degrees(attitude_sp.roll_body) * 100;
+			//msg.pitch_sp = math::degrees(attitude_sp.pitch_body) * 100;
+			msg.heading_sp = math::degrees(_wrap_2pi(attitude_sp.yaw_body)) * 100;
+
+			if (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
+				msg.throttle = actuator.control[actuator_controls_s::INDEX_THROTTLE] * 100;
+			} else {
+				msg.throttle = 0;
+			}
+
+			msg.latitude = global_pos.lat * 1e7;
+			msg.longitude = global_pos.lon * 1e7;
+
+			//msg.altitude_home = (_home_time > 0) ? (global_pos.alt - home.alt) : NAN;
+			msg.altitude_amsl = (_global_pos_time > 0) ? global_pos.alt : NAN;
+
+			msg.altitude_sp = (_tecs_time > 0) ? (tecs_status.altitudeSp - home.alt) : NAN;
+
+			msg.airspeed = airspeed.indicated_airspeed_m_s * 100.0f;
+			msg.groundspeed = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e) * 100.0f;
+			msg.climb_rate = -global_pos.vel_d;
+
+			msg.gps_nsat = gps.satellites_used;
+			msg.gps_fix_type = gps.fix_type;
+
+			msg.landed_state = land_detected.landed ? MAV_LANDED_STATE_ON_GROUND : MAV_LANDED_STATE_IN_AIR;
+
+			msg.battery_remaining = (battery.connected) ? battery.remaining * 100.0f : -1;
+
+			msg.temperature = sensor.baro_temp_celcius;
+			msg.temperature_air = airspeed.air_temperature_celsius;
+
+			msg.wp_num = mission_result.seq_current;
+			msg.wp_distance = fw_pos_ctrl_status.wp_dist;
+
+			mavlink_msg_high_latency_send_struct(_mavlink->get_channel(), &msg);
+		}
+	}
+};
+
 const StreamListItem *streams_list[] = {
 	new StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static, &MavlinkStreamHeartbeat::get_id_static),
 	new StreamListItem(&MavlinkStreamStatustext::new_instance, &MavlinkStreamStatustext::get_name_static, &MavlinkStreamStatustext::get_id_static),
@@ -3638,5 +3842,6 @@ const StreamListItem *streams_list[] = {
 	new StreamListItem(&MavlinkStreamCollision::new_instance, &MavlinkStreamCollision::get_name_static, &MavlinkStreamCollision::get_id_static),
 	new StreamListItem(&MavlinkStreamWind::new_instance, &MavlinkStreamWind::get_name_static, &MavlinkStreamWind::get_id_static),
 	new StreamListItem(&MavlinkStreamMountOrientation::new_instance, &MavlinkStreamMountOrientation::get_name_static, &MavlinkStreamMountOrientation::get_id_static),
+	new StreamListItem(&MavlinkStreamHighLatency::new_instance, &MavlinkStreamHighLatency::get_name_static, &MavlinkStreamWind::get_id_static),
 	nullptr
 };

src/modules/systemlib/system_params.c

@@ -121,6 +121,7 @@ PARAM_DEFINE_INT32(SYS_MC_EST_GROUP, 2);
  * @value 319200 Normal Telemetry (19200 baud, 8N1)
  * @value 338400 Normal Telemetry (38400 baud, 8N1)
  * @value 357600 Normal Telemetry (57600 baud, 8N1)
+ * @value 419200 Iridium Telemetry (19200 baud, 8N1)
  * @value 1921600 ESP8266 (921600 baud, 8N1)
  *
  * @min 0