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.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 */
-	configure_stream("HEARTBEAT", 1.0f);
+	if (_mode != MAVLINK_MODE_IRIDIUM) {
 
-	/* STATUSTEXT stream is like normal stream but gets messages from logbuffer instead of uORB */
-	configure_stream("STATUSTEXT", 20.0f);
+		/* HEARTBEAT is constant rate stream, rate never adjusted */
+		configure_stream("HEARTBEAT", 1.0f);
 
-	/* COMMAND_LONG stream: use high rate to avoid commands skipping */
-	configure_stream("COMMAND_LONG", 100.0f);
+		/* STATUSTEXT stream is like normal stream but gets messages from logbuffer instead of uORB */
+		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