organize all telemetry drivers in subdirectory

2026-10-20 05:33:33 +08:00 · 2018-02-09 22:40:34 +01:00
parent 681e351f62
commit c459a04b9b
37 changed files with 62 additions and 42 deletions
@@ -0,0 +1,37 @@
+############################################################################
+#
+#   Copyright (c) 2017 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.
+#
+############################################################################
+
+add_subdirectory(bst)
+add_subdirectory(frsky_telemetry)
+add_subdirectory(hott)
+add_subdirectory(iridiumsbd)
@@ -0,0 +1,43 @@
+############################################################################
+#
+#   Copyright (c) 2015 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__bst
+	MAIN bst
+	STACK_MAIN 1200
+	COMPILE_FLAGS
+	SRCS
+		bst.cpp
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix :
@@ -0,0 +1,416 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file bst.cpp
+ *
+ * Black Sheep Telemetry driver
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#include <px4_config.h>
+#include <px4_getopt.h>
+#include <px4_workqueue.h>
+#include <drivers/device/i2c.h>
+#include <systemlib/err.h>
+#include <string.h>
+#include <stdlib.h>
+#include <math.h>
+#include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/battery_status.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <mathlib/math/Quaternion.hpp>
+
+using namespace matrix;
+
+#define BST_DEVICE_PATH "/dev/bst0"
+
+static const char commandline_usage[] = "usage: bst start|status|stop";
+
+extern "C" __EXPORT int bst_main(int argc, char *argv[]);
+
+#define BST_ADDR		0x76
+
+namespace px4
+{
+namespace bst
+{
+
+#pragma pack(push, 1)
+
+template <typename T>
+struct BSTPacket {
+	uint8_t length;
+	uint8_t type;
+	T payload;
+	uint8_t crc;
+};
+
+struct BSTDeviceInfoRequest {
+	uint8_t cmd;
+};
+
+struct BSTDeviceInfoReply {
+	uint32_t hw_id;
+	uint16_t fw_id;
+	uint8_t dev_name_len;
+	char dev_name[32];
+};
+
+struct BSTGPSPosition {
+	int32_t lat;
+	int32_t lon;
+	uint16_t gs;
+	uint16_t heading;
+	uint16_t alt;
+	uint8_t sats;
+};
+
+struct BSTAttitude {
+	int16_t pitch;
+	int16_t roll;
+	int16_t yaw;
+};
+
+struct BSTBattery {
+	uint16_t voltage;
+	uint16_t current;
+	uint8_t capacity[3];
+};
+
+#pragma pack(pop)
+
+class BST : public device::I2C
+{
+public:
+	BST(int bus);
+
+	virtual ~BST();
+
+	virtual int		init();
+
+	virtual int		probe();
+
+	virtual int		info() { return 0; }
+
+	virtual int		ioctl(device::file_t *filp, int cmd, unsigned long arg) { return 0; }
+
+	work_s *work_ptr() { return &_work; }
+
+	void stop();
+
+	static void		start_trampoline(void *arg);
+
+private:
+	work_s			_work = {};
+	bool			_should_run = false;
+	unsigned		_interval = 100;
+	int				_gps_sub;
+	int				_attitude_sub;
+	int				_battery_sub;
+
+	void			start();
+
+	static void		cycle_trampoline(void *arg);
+
+	void			cycle();
+
+	template <typename T>
+	void			send_packet(BSTPacket<T> &packet)
+	{
+		packet.length = sizeof(packet) - 1;	// Length
+		packet.crc = crc8(reinterpret_cast<uint8_t *>(&packet.type), sizeof(packet) - 2);
+
+		transfer(reinterpret_cast<uint8_t *>(&packet), sizeof(packet), nullptr, 0);
+	}
+
+	template <typename T_SEND, typename T_RECV>
+	void				send_packet(BSTPacket<T_SEND> &packet_send, BSTPacket<T_RECV> &packet_recv)
+	{
+		packet_send.length = sizeof(packet_send) - 1;	// Length
+		packet_send.crc = crc8(reinterpret_cast<uint8_t *>(&packet_send.type), sizeof(packet_send) - 2);
+		transfer(reinterpret_cast<uint8_t *>(&packet_send), sizeof(packet_send), reinterpret_cast<uint8_t *>(&packet_recv),
+			 sizeof(packet_recv));
+	}
+
+	static uint8_t	crc8(uint8_t *data, size_t len);
+
+	//! Byte swap unsigned short
+	uint16_t swap_uint16(uint16_t val)
+	{
+		return (val << 8) | (val >> 8);
+	}
+
+	//! Byte swap short
+	int16_t swap_int16(int16_t val)
+	{
+		return (val << 8) | ((val >> 8) & 0xFF);
+	}
+
+	//! Byte swap unsigned int
+	uint32_t swap_uint32(uint32_t val)
+	{
+		val = ((val << 8) & 0xFF00FF00) | ((val >> 8) & 0xFF00FF);
+		return (val << 16) | (val >> 16);
+	}
+
+	//! Byte swap int
+	int32_t swap_int32(int32_t val)
+	{
+		val = ((val << 8) & 0xFF00FF00) | ((val >> 8) & 0xFF00FF);
+		return (val << 16) | ((val >> 16) & 0xFFFF);
+	}
+};
+
+static BST *g_bst = nullptr;
+
+BST::BST(int bus) :
+	I2C("bst", BST_DEVICE_PATH, bus, BST_ADDR
+#ifdef __PX4_NUTTX
+	    , 100000 /* maximum speed supported */
+#endif
+	   )
+{
+}
+
+BST::~BST()
+{
+	_should_run = false;
+
+	work_cancel(LPWORK, &_work);
+}
+
+int BST::probe()
+{
+	int retries_prev = _retries;
+	_retries = 3;
+
+	BSTPacket<BSTDeviceInfoRequest> dev_info_req = {};
+	dev_info_req.type = 0x0A;
+	dev_info_req.payload.cmd = 0x04;
+	BSTPacket<BSTDeviceInfoReply> dev_info_reply = {};
+	send_packet(dev_info_req, dev_info_reply);
+
+	if (dev_info_reply.type != 0x05) {
+		warnx("no devices found");
+		return -EIO;
+	}
+
+	uint8_t *reply_raw = reinterpret_cast<uint8_t *>(&dev_info_reply);
+	uint8_t crc_calc = crc8(reinterpret_cast<uint8_t *>(&dev_info_reply.type), dev_info_reply.length - 1);
+	uint8_t crc_recv = reply_raw[dev_info_reply.length];
+
+	if (crc_recv != crc_calc) {
+		warnx("CRC error: got %02x, should be %02x", (int)crc_recv, (int)crc_calc);
+		return -EIO;
+	}
+
+	dev_info_reply.payload.dev_name[dev_info_reply.payload.dev_name_len] = '\0';
+	warnx("device info: hardware ID: 0x%08X, firmware ID: 0x%04X, device name: %s",
+	      (int)swap_uint32(dev_info_reply.payload.hw_id), (int)swap_uint16(dev_info_reply.payload.fw_id),
+	      dev_info_reply.payload.dev_name);
+
+	_retries = retries_prev;
+
+	return OK;
+}
+
+int BST::init()
+{
+	int ret;
+	ret = I2C::init();
+
+	if (ret != OK) {
+		return ret;
+	}
+
+	work_queue(LPWORK, &_work, BST::start_trampoline, g_bst, 0);
+
+	return OK;
+}
+
+void BST::start_trampoline(void *arg)
+{
+	reinterpret_cast<BST *>(arg)->start();
+}
+
+void BST::start()
+{
+	_should_run = true;
+
+	_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+	_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
+	_battery_sub = orb_subscribe(ORB_ID(battery_status));
+
+	set_device_address(0x00);	// General call address
+
+	work_queue(LPWORK, &_work, BST::cycle_trampoline, this, 0);
+}
+
+void BST::cycle_trampoline(void *arg)
+{
+	reinterpret_cast<BST *>(arg)->cycle();
+}
+
+void BST::cycle()
+{
+	if (_should_run) {
+		bool updated = false;
+
+		orb_check(_attitude_sub, &updated);
+
+		if (updated) {
+			vehicle_attitude_s att;
+			orb_copy(ORB_ID(vehicle_attitude), _attitude_sub, &att);
+			Quatf q(att.q);
+			Eulerf euler(q);
+			BSTPacket<BSTAttitude> bst_att = {};
+			bst_att.type = 0x1E;
+			bst_att.payload.roll = swap_int32(euler.phi() * 10000);
+			bst_att.payload.pitch = swap_int32(euler.theta() * 10000);
+			bst_att.payload.yaw = swap_int32(euler.psi() * 10000);
+			send_packet(bst_att);
+		}
+
+		updated = false;
+		orb_check(_battery_sub, &updated);
+
+		if (updated) {
+			battery_status_s batt;
+			orb_copy(ORB_ID(battery_status), _battery_sub, &batt);
+			BSTPacket<BSTBattery> bst_batt = {};
+			bst_batt.type = 0x08;
+			bst_batt.payload.voltage = swap_uint16(batt.voltage_v * 10.0f);
+			bst_batt.payload.current = swap_uint16(batt.current_a * 10.0f);
+			uint32_t discharged = batt.discharged_mah;
+			bst_batt.payload.capacity[0] = static_cast<uint8_t>(discharged >> 16);
+			bst_batt.payload.capacity[1] = static_cast<uint8_t>(discharged >> 8);
+			bst_batt.payload.capacity[2] = static_cast<uint8_t>(discharged);
+			send_packet(bst_batt);
+		}
+
+		updated = false;
+		orb_check(_gps_sub, &updated);
+
+		if (updated) {
+			vehicle_gps_position_s gps;
+			orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &gps);
+
+			if (gps.fix_type >= 3 && gps.eph < 50.0f) {
+				BSTPacket<BSTGPSPosition> bst_gps = {};
+				bst_gps.type = 0x02;
+				bst_gps.payload.lat = swap_int32(gps.lat);
+				bst_gps.payload.lon = swap_int32(gps.lon);
+				bst_gps.payload.alt = swap_int16(gps.alt / 1000 + 1000);
+				bst_gps.payload.gs = swap_int16(gps.vel_m_s * 360.0f);
+				bst_gps.payload.heading = swap_int16(gps.cog_rad * 18000.0f / M_PI_F);
+				bst_gps.payload.sats = gps.satellites_used;
+				send_packet(bst_gps);
+			}
+		}
+
+		work_queue(LPWORK, &_work, BST::cycle_trampoline, this, _interval);
+	}
+}
+
+uint8_t BST::crc8(uint8_t *data, size_t len)
+{
+	uint8_t crc = 0x00;
+
+	while (len--) {
+		crc ^= *data++;
+
+		for (int i = 0; i < 8; i++) {
+			crc = crc & 0x80 ? (crc << 1) ^ 0xD5 : crc << 1;
+		}
+	}
+
+	return crc;
+}
+
+}
+}
+
+using namespace px4::bst;
+
+int bst_main(int argc, char *argv[])
+{
+	if (argc < 2) {
+		errx(1, "missing command\n%s", commandline_usage);
+	}
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (g_bst) {
+			warnx("already running");
+			exit(0);
+		}
+
+		g_bst = new BST(PX4_I2C_BUS_EXPANSION);
+
+		if (g_bst != nullptr && OK != g_bst->init()) {
+			delete g_bst;
+			g_bst = nullptr;
+		}
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		if (!g_bst) {
+			warnx("not running");
+			exit(0);
+		}
+
+		delete g_bst;
+		g_bst = nullptr;
+		warnx("stopped");
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (g_bst) {
+			warnx("is running");
+
+		} else {
+			warnx("is not running");
+		}
+
+		exit(0);
+	}
+
+	PX4_WARN("unrecognized command\n%s", commandline_usage);
+	return 1;
+}
@@ -0,0 +1,45 @@
+############################################################################
+#
+#   Copyright (c) 2015 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__frsky_telemetry
+	MAIN frsky_telemetry
+	STACK_MAIN 1200
+	COMPILE_FLAGS
+	SRCS
+		frsky_data.c
+		sPort_data.c
+		frsky_telemetry.c
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix :
@@ -0,0 +1,81 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2017 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file common.h
+ *
+ * common declarations
+ *
+ */
+#pragma once
+
+#include <stdint.h>
+
+/* FrSky sensor hub data IDs */
+#define FRSKY_ID_GPS_ALT_BP     0x01
+#define FRSKY_ID_TEMP1          0x02
+#define FRSKY_ID_RPM            0x03
+#define FRSKY_ID_FUEL           0x04
+#define FRSKY_ID_TEMP2          0x05
+#define FRSKY_ID_VOLTS          0x06
+#define FRSKY_ID_GPS_ALT_AP     0x09
+#define FRSKY_ID_BARO_ALT_BP    0x10
+#define FRSKY_ID_GPS_SPEED_BP   0x11
+#define FRSKY_ID_GPS_LONG_BP    0x12
+#define FRSKY_ID_GPS_LAT_BP     0x13
+#define FRSKY_ID_GPS_COURS_BP   0x14
+#define FRSKY_ID_GPS_DAY_MONTH  0x15
+#define FRSKY_ID_GPS_YEAR       0x16
+#define FRSKY_ID_GPS_HOUR_MIN   0x17
+#define FRSKY_ID_GPS_SEC        0x18
+#define FRSKY_ID_GPS_SPEED_AP   0x19
+#define FRSKY_ID_GPS_LONG_AP    0x1A
+#define FRSKY_ID_GPS_LAT_AP     0x1B
+#define FRSKY_ID_GPS_COURS_AP   0x1C
+#define FRSKY_ID_BARO_ALT_AP    0x21
+#define FRSKY_ID_GPS_LONG_EW    0x22
+#define FRSKY_ID_GPS_LAT_NS     0x23
+#define FRSKY_ID_ACCEL_X        0x24
+#define FRSKY_ID_ACCEL_Y        0x25
+#define FRSKY_ID_ACCEL_Z        0x26
+#define FRSKY_ID_CURRENT        0x28
+#define FRSKY_ID_VARIO          0x30
+#define FRSKY_ID_VFAS           0x39
+#define FRSKY_ID_VOLTS_BP       0x3A
+#define FRSKY_ID_VOLTS_AP       0x3B
+
+
+/**
+ * Map the PX4 flight mode (vehicle_status_s::nav_state) to the telemetry flight mode
+ */
+uint16_t get_telemetry_flight_mode(int px4_flight_mode);
@@ -0,0 +1,390 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2017 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file frsky_data.c
+ *
+ * @author Stefan Rado <px4@sradonia.net>
+ *
+ * FrSky telemetry implementation.
+ *
+ */
+
+#include "frsky_data.h"
+#include "common.h"
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <stdbool.h>
+#include <arch/math.h>
+#include <geo/geo.h>
+#include <stdbool.h>
+
+#include <uORB/topics/battery_status.h>
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/vehicle_status.h>
+
+#include <drivers/drv_hrt.h>
+
+#define frac(f) (f - (int)f)
+
+struct frsky_subscription_data_s {
+	struct battery_status_s battery_status;
+	struct vehicle_global_position_s global_pos;
+	struct sensor_combined_s sensor_combined;
+	struct vehicle_gps_position_s vehicle_gps_position;
+	uint8_t current_flight_mode; // == vehicle_status.nav_state
+
+	int battery_status_sub;
+	int vehicle_global_position_sub;
+	int sensor_sub;
+	int vehicle_gps_position_sub;
+	int vehicle_status_sub;
+};
+
+static struct frsky_subscription_data_s *subscription_data = NULL;
+
+/**
+ * Initializes the uORB subscriptions.
+ */
+bool frsky_init()
+{
+	subscription_data = (struct frsky_subscription_data_s *)calloc(1, sizeof(struct frsky_subscription_data_s));
+
+	if (!subscription_data) {
+		return false;
+	}
+
+	subscription_data->battery_status_sub = orb_subscribe(ORB_ID(battery_status));
+	subscription_data->vehicle_global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
+	subscription_data->sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
+	subscription_data->vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
+	subscription_data->vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
+	return true;
+}
+
+void frsky_deinit()
+{
+	if (subscription_data) {
+		orb_unsubscribe(subscription_data->battery_status_sub);
+		orb_unsubscribe(subscription_data->vehicle_global_position_sub);
+		orb_unsubscribe(subscription_data->sensor_sub);
+		orb_unsubscribe(subscription_data->vehicle_gps_position_sub);
+		orb_unsubscribe(subscription_data->vehicle_status_sub);
+		free(subscription_data);
+		subscription_data = NULL;
+	}
+}
+
+/**
+ * Sends a 0x5E start/stop byte.
+ */
+static void frsky_send_startstop(int uart)
+{
+	static const uint8_t c = 0x5E;
+	write(uart, &c, sizeof(c));
+}
+
+/**
+ * Sends one byte, performing byte-stuffing if necessary.
+ */
+static void frsky_send_byte(int uart, uint8_t value)
+{
+	const uint8_t x5E[] = { 0x5D, 0x3E };
+	const uint8_t x5D[] = { 0x5D, 0x3D };
+
+	switch (value) {
+	case 0x5E:
+		write(uart, x5E, sizeof(x5E));
+		break;
+
+	case 0x5D:
+		write(uart, x5D, sizeof(x5D));
+		break;
+
+	default:
+		write(uart, &value, sizeof(value));
+		break;
+	}
+}
+
+/**
+ * Sends one data id/value pair.
+ */
+static void frsky_send_data(int uart, uint8_t id, int16_t data)
+{
+	/* Cast data to unsigned, because signed shift might behave incorrectly */
+	uint16_t udata = data;
+
+	frsky_send_startstop(uart);
+
+	frsky_send_byte(uart, id);
+	frsky_send_byte(uart, udata);      /* LSB */
+	frsky_send_byte(uart, udata >> 8); /* MSB */
+}
+
+void frsky_update_topics()
+{
+	struct frsky_subscription_data_s *subs = subscription_data;
+	bool updated;
+
+	/* get a local copy of the current sensor values */
+	orb_check(subs->sensor_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(sensor_combined), subs->sensor_sub, &subs->sensor_combined);
+	}
+
+	/* get a local copy of the vehicle status */
+	orb_check(subs->vehicle_status_sub, &updated);
+
+	if (updated) {
+		struct vehicle_status_s vehicle_status;
+		orb_copy(ORB_ID(vehicle_status), subs->vehicle_status_sub, &vehicle_status);
+		subs->current_flight_mode = vehicle_status.nav_state;
+	}
+
+	/* get a local copy of the battery data */
+	orb_check(subs->battery_status_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(battery_status), subs->battery_status_sub, &subs->battery_status);
+	}
+
+	/* get a local copy of the global position data */
+	orb_check(subs->vehicle_global_position_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(vehicle_global_position), subs->vehicle_global_position_sub, &subs->global_pos);
+	}
+
+	/* get a local copy of the raw GPS data */
+	orb_check(subs->vehicle_gps_position_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(vehicle_gps_position), subs->vehicle_gps_position_sub, &subs->vehicle_gps_position);
+	}
+}
+
+/**
+ * Sends frame 1 (every 200ms):
+ *   acceleration values, barometer altitude, temperature, battery voltage & current
+ */
+void frsky_send_frame1(int uart)
+{
+	struct frsky_subscription_data_s *subs = subscription_data;
+
+	/* send formatted frame */
+	frsky_send_data(uart, FRSKY_ID_ACCEL_X, roundf(subs->sensor_combined.accelerometer_m_s2[0] * 1000.0f));
+	frsky_send_data(uart, FRSKY_ID_ACCEL_Y, roundf(subs->sensor_combined.accelerometer_m_s2[1] * 1000.0f));
+	frsky_send_data(uart, FRSKY_ID_ACCEL_Z, roundf(subs->sensor_combined.accelerometer_m_s2[2] * 1000.0f));
+
+	frsky_send_data(uart, FRSKY_ID_BARO_ALT_BP,
+			subs->sensor_combined.baro_alt_meter);
+	frsky_send_data(uart, FRSKY_ID_BARO_ALT_AP,
+			roundf(frac(subs->sensor_combined.baro_alt_meter) * 100.0f));
+
+	frsky_send_data(uart, FRSKY_ID_VFAS,
+			roundf(subs->battery_status.voltage_v * 10.0f));
+	frsky_send_data(uart, FRSKY_ID_CURRENT,
+			(subs->battery_status.current_a < 0) ? 0 : roundf(subs->battery_status.current_a * 10.0f));
+
+	int16_t telem_flight_mode = get_telemetry_flight_mode(subs->current_flight_mode);
+	frsky_send_data(uart, FRSKY_ID_TEMP1, telem_flight_mode); // send flight mode as TEMP1. This matches with OpenTX & APM
+
+	frsky_send_data(uart, FRSKY_ID_TEMP2, subs->vehicle_gps_position.satellites_used * 10 +
+			subs->vehicle_gps_position.fix_type);
+
+	frsky_send_startstop(uart);
+}
+
+/**
+ * Formats the decimal latitude/longitude to the required degrees/minutes.
+ */
+static float frsky_format_gps(float dec)
+{
+	float dm_deg = (int) dec;
+	return (dm_deg * 100.0f) + (dec - dm_deg) * 60;
+}
+
+/**
+ * Sends frame 2 (every 1000ms):
+ *   GPS course, latitude, longitude, ground speed, GPS altitude, remaining battery level
+ */
+void frsky_send_frame2(int uart)
+{
+	struct vehicle_global_position_s *global_pos = &subscription_data->global_pos;
+	struct battery_status_s *battery_status = &subscription_data->battery_status;
+	struct vehicle_gps_position_s *gps = &subscription_data->vehicle_gps_position;
+	/* send formatted frame */
+	float course = 0, lat = 0, lon = 0, speed = 0, alt = 0;
+	char lat_ns = 0, lon_ew = 0;
+	int sec = 0;
+
+	if (global_pos->timestamp != 0 && hrt_absolute_time() < global_pos->timestamp + 20000) {
+		course = global_pos->yaw / M_PI_F * 180.0f;
+
+		if (course < 0.f) { // course is in range [0, 360], 0=north, CW
+			course += 360.f;
+		}
+
+		lat    = frsky_format_gps(fabsf(global_pos->lat));
+		lat_ns = (global_pos->lat < 0) ? 'S' : 'N';
+		lon    = frsky_format_gps(fabsf(global_pos->lon));
+		lon_ew = (global_pos->lon < 0) ? 'W' : 'E';
+		speed  = sqrtf(global_pos->vel_n * global_pos->vel_n + global_pos->vel_e * global_pos->vel_e)
+			 * 25.0f / 46.0f;
+		alt    = global_pos->alt;
+	}
+
+	if (gps->timestamp != 0 && hrt_absolute_time() < gps->timestamp + 20000) {
+		time_t time_gps = gps->time_utc_usec / 1000000ULL;
+		struct tm *tm_gps = gmtime(&time_gps);
+
+		sec    = tm_gps->tm_sec;
+	}
+
+	frsky_send_data(uart, FRSKY_ID_GPS_COURS_BP, course);
+	frsky_send_data(uart, FRSKY_ID_GPS_COURS_AP, frac(course) * 1000.0f);
+
+	frsky_send_data(uart, FRSKY_ID_GPS_LAT_BP, lat);
+	frsky_send_data(uart, FRSKY_ID_GPS_LAT_AP, frac(lat) * 10000.0f);
+	frsky_send_data(uart, FRSKY_ID_GPS_LAT_NS, lat_ns);
+
+	frsky_send_data(uart, FRSKY_ID_GPS_LONG_BP, lon);
+	frsky_send_data(uart, FRSKY_ID_GPS_LONG_AP, frac(lon) * 10000.0f);
+	frsky_send_data(uart, FRSKY_ID_GPS_LONG_EW, lon_ew);
+
+	frsky_send_data(uart, FRSKY_ID_GPS_SPEED_BP, speed);
+	frsky_send_data(uart, FRSKY_ID_GPS_SPEED_AP, frac(speed) * 100.0f);
+
+	frsky_send_data(uart, FRSKY_ID_GPS_ALT_BP, alt);
+	frsky_send_data(uart, FRSKY_ID_GPS_ALT_AP, frac(alt) * 100.0f);
+
+	frsky_send_data(uart, FRSKY_ID_FUEL,
+			roundf(battery_status->remaining * 100.0f));
+
+	frsky_send_data(uart, FRSKY_ID_GPS_SEC, sec);
+
+	frsky_send_startstop(uart);
+}
+
+/**
+ * Sends frame 3 (every 5000ms):
+ *   GPS date & time
+ */
+void frsky_send_frame3(int uart)
+{
+	/* send formatted frame */
+	time_t time_gps = subscription_data->vehicle_gps_position.time_utc_usec / 1000000ULL;
+	struct tm *tm_gps = gmtime(&time_gps);
+	uint16_t hour_min = (tm_gps->tm_min << 8) | (tm_gps->tm_hour & 0xff);
+	frsky_send_data(uart, FRSKY_ID_GPS_DAY_MONTH, tm_gps->tm_mday);
+	frsky_send_data(uart, FRSKY_ID_GPS_YEAR, tm_gps->tm_year);
+	frsky_send_data(uart, FRSKY_ID_GPS_HOUR_MIN, hour_min);
+	frsky_send_data(uart, FRSKY_ID_GPS_SEC, tm_gps->tm_sec);
+
+	frsky_send_startstop(uart);
+}
+
+/* parse 11 byte frames */
+bool frsky_parse_host(uint8_t *sbuf, int nbytes, struct adc_linkquality *v)
+{
+	bool data_ready = false;
+	static int dcount = 0;
+	static uint8_t type = 0;
+	static uint8_t data[11];
+	static enum {
+		HEADER = 0,
+		TYPE,
+		DATA,
+		TRAILER
+	} state = HEADER;
+
+	for (int i = 0; i < nbytes; i++) {
+		switch (state) {
+		case HEADER:
+			if (sbuf[i] == 0x7E) {
+				state = TYPE;
+			}
+
+			break;
+
+		case TYPE:
+			if (sbuf[i] != 0x7E) {
+				state = DATA;
+				type = sbuf[i];
+				dcount = 0;
+			}
+
+			break;
+
+		case DATA:
+
+			/* read 8 data bytes */
+			if (dcount < 7) {
+				data[dcount++] = sbuf[i];
+
+			} else {
+				/* received all data bytes */
+				state = TRAILER;
+			}
+
+			break;
+
+		case TRAILER:
+			state = HEADER;
+
+			if (sbuf[i] != 0x7E) {
+//				warnx("host packet error: %x", sbuf[i]);
+
+			} else {
+				data_ready = true;
+
+				if (type == 0xFE) {
+					/* this is an adc_linkquality packet */
+					v->ad1 = data[0];
+					v->ad2 = data[1];
+					v->linkq = data[2];
+				}
+			}
+
+			break;
+		}
+	}
+
+	return data_ready;
+}
@@ -0,0 +1,62 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
+ *   Author: Stefan Rado <px4@sradonia.net>
+ *
+ * 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file frsky_data.h
+ * @author Stefan Rado <px4@sradonia.net>
+ *
+ * FrSky telemetry implementation.
+ *
+ */
+#ifndef _FRSKY_DATA_H
+#define _FRSKY_DATA_H
+
+#include <stdbool.h>
+
+// Public functions
+bool frsky_init(void);
+void frsky_deinit(void);
+void frsky_update_topics(void);
+void frsky_send_frame1(int uart);
+void frsky_send_frame2(int uart);
+void frsky_send_frame3(int uart);
+
+struct adc_linkquality {
+	uint8_t ad1;
+	uint8_t ad2;
+	uint8_t linkq;
+};
+bool frsky_parse_host(uint8_t *sbuf, int nbytes, struct adc_linkquality *v);
+
+#endif /* _FRSKY_TELEMETRY_H */
@@ -0,0 +1,707 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
+ *   Author: Stefan Rado <px4@sradonia.net>
+ *
+ * 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file frsky_telemetry.c
+ * @author Stefan Rado <px4@sradonia.net>
+ * @author Mark Whitehorn <kd0aij@github.com>
+ *
+ * FrSky D8 mode and SmartPort (D16 mode) telemetry implementation.
+ *
+ * This daemon emulates the FrSky Sensor Hub for D8 mode.
+ * For X series receivers (D16 mode) it emulates SmartPort sensors by responding to polling
+ * packets received from an attached FrSky X series receiver.
+ *
+ */
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <stdbool.h>
+#include <string.h>
+#include <sys/types.h>
+#include <poll.h>
+#include <fcntl.h>
+#include <unistd.h>
+
+#include <px4_tasks.h>
+#include <px4_module.h>
+#include <systemlib/err.h>
+#include <termios.h>
+#include <drivers/drv_hrt.h>
+#include <uORB/topics/sensor_baro.h>
+#include <math.h>	// NAN
+
+#include "sPort_data.h"
+#include "frsky_data.h"
+#include "common.h"
+
+
+/* thread state */
+static volatile bool thread_should_exit = false;
+static volatile bool thread_running = false;
+static int frsky_task;
+typedef enum { SCANNING, SPORT, DTYPE } frsky_state_t;
+static frsky_state_t frsky_state = SCANNING;
+
+/* functions */
+static int sPort_open_uart(const char *uart_name, struct termios *uart_config, struct termios *uart_config_original);
+static int set_uart_speed(int uart, struct termios *uart_config, speed_t speed);
+static void usage(void);
+static int frsky_telemetry_thread_main(int argc, char *argv[]);
+__EXPORT int frsky_telemetry_main(int argc, char *argv[]);
+
+
+uint16_t get_telemetry_flight_mode(int px4_flight_mode)
+{
+	// map the flight modes (see https://github.com/ilihack/LuaPilot_Taranis_Telemetry/blob/master/SCRIPTS/TELEMETRY/LuaPil.lua#L790)
+	switch (px4_flight_mode) {
+	case 0: return 18; // manual
+
+	case 1: return 23; // alt control
+
+	case 2: return 22; // pos control
+
+	case 3: return 27; // mission
+
+	case 4: return 26; // loiter
+
+	case 5:
+	case 6:
+	case 7: return 28; // rtl
+
+	case 10: return 19; // acro
+
+	case 14: return 24; // offboard
+
+	case 15: return 20; // stabilized
+
+	case 16: return 21; // rattitude
+
+	case 17: return 25; // takeoff
+
+	case 8:
+	case 9:
+	case 18: return 29; // land
+
+	case 19: return 30; // follow target
+	}
+
+	return -1;
+}
+
+/**
+ * Opens the UART device and sets all required serial parameters.
+ */
+static int sPort_open_uart(const char *uart_name, struct termios *uart_config, struct termios *uart_config_original)
+{
+	/* Open UART */
+	const int uart = open(uart_name, O_RDWR | O_NOCTTY | O_NONBLOCK);
+
+	if (uart < 0) {
+		err(1, "Error opening port: %s", uart_name);
+	}
+
+	/* Back up the original UART configuration to restore it after exit */
+	int termios_state;
+
+	if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
+		warnx("ERR: tcgetattr%s: %d\n", uart_name, termios_state);
+		close(uart);
+		return -1;
+	}
+
+	/* Fill the struct for the new configuration */
+	tcgetattr(uart, uart_config);
+
+	/* Disable output post-processing */
+	uart_config->c_oflag &= ~OPOST;
+
+	/* Set baud rate */
+	const speed_t speed = B9600;
+
+	if (cfsetispeed(uart_config, speed) < 0 || cfsetospeed(uart_config, speed) < 0) {
+		warnx("ERR: %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
+		close(uart);
+		return -1;
+	}
+
+	if ((termios_state = tcsetattr(uart, TCSANOW, uart_config)) < 0) {
+		warnx("ERR: %s (tcsetattr)\n", uart_name);
+		close(uart);
+		return -1;
+	}
+
+	return uart;
+}
+
+static int set_uart_speed(int uart, struct termios *uart_config, speed_t speed)
+{
+
+	if (cfsetispeed(uart_config, speed) < 0) {
+		return -1;
+	}
+
+	if (tcsetattr(uart, TCSANOW, uart_config) < 0) {
+		return -1;
+	}
+
+	return uart;
+}
+
+/**
+ * Print command usage information
+ */
+static void usage()
+{
+	PRINT_MODULE_DESCRIPTION("FrSky Telemetry support. Auto-detects D or S.PORT protocol.");
+
+	PRINT_MODULE_USAGE_NAME("frsky_telemetry", "communication");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_PARAM_STRING('d', "/dev/ttyS6", "<file:dev>", "Select Serial Device", true);
+	PRINT_MODULE_USAGE_COMMAND("stop");
+	PRINT_MODULE_USAGE_COMMAND("status");
+	exit(1);
+}
+
+/**
+ * The daemon thread.
+ */
+static int frsky_telemetry_thread_main(int argc, char *argv[])
+{
+	/* Default values for arguments */
+	const char *device_name = "/dev/ttyS6"; /* USART8 */
+
+	/* Work around some stupidity in task_create's argv handling */
+	argc -= 2;
+	argv += 2;
+
+	int ch;
+
+	while ((ch = getopt(argc, argv, "d:")) != EOF) {
+		switch (ch) {
+		case 'd':
+			device_name = optarg;
+			break;
+
+		default:
+			usage();
+			break;
+		}
+	}
+
+	/* Open UART assuming D type telemetry */
+	struct termios uart_config_original;
+	struct termios uart_config;
+	const int uart = sPort_open_uart(device_name, &uart_config, &uart_config_original);
+
+	if (uart < 0) {
+		warnx("could not open %s", device_name);
+		err(1, "could not open %s", device_name);
+	}
+
+	/* poll descriptor */
+	struct pollfd fds[1];
+	fds[0].fd = uart;
+	fds[0].events = POLLIN;
+
+	thread_running = true;
+
+	/* Main thread loop */
+	char sbuf[20];
+	frsky_state = SCANNING;
+	frsky_state_t baudRate = DTYPE;
+
+	while (!thread_should_exit && frsky_state == SCANNING) {
+		/* 2 byte polling frames indicate SmartPort telemetry
+		 * 11 byte packets indicate D type telemetry
+		 */
+		int status = poll(fds, sizeof(fds) / sizeof(fds[0]), 3000);
+
+		if (status > 0) {
+			/* traffic on the port, D type is 11 bytes per frame, SmartPort is only 2
+			 * Wait long enough for 11 bytes at 9600 baud
+			 */
+			usleep(12000);
+			int nbytes = read(uart, &sbuf[0], sizeof(sbuf));
+			PX4_DEBUG("frsky input: %d bytes: %x %x, speed: %d", nbytes, sbuf[0], sbuf[1], baudRate);
+
+			// look for valid header byte
+			if (nbytes > 10) {
+				if (baudRate == DTYPE) {
+					// see if we got a valid D-type hostframe
+					struct adc_linkquality host_frame;
+
+					if (frsky_parse_host((uint8_t *)&sbuf[0], nbytes, &host_frame)) {
+						frsky_state = baudRate;
+						break;
+					}
+
+				} else {
+					// check for alternating S.port start bytes
+					int index = 0;
+
+					while (index < 2 && sbuf[index] != 0x7E) { index++; }
+
+					if (index < 2) {
+
+						int success = 1;
+
+						for (int i = index + 2; i < nbytes; i += 2) {
+							if (sbuf[i] != 0x7E) { success = 0; break; }
+						}
+
+						if (success) {
+							frsky_state = baudRate;
+							break;
+						}
+					}
+				}
+
+			}
+
+			// alternate between S.port and D-type baud rates
+			if (baudRate == SPORT) {
+				PX4_DEBUG("setting baud rate to %d", 9600);
+				set_uart_speed(uart, &uart_config, B9600);
+				baudRate = DTYPE;
+
+			} else {
+				PX4_DEBUG("setting baud rate to %d", 57600);
+				set_uart_speed(uart, &uart_config, B57600);
+				baudRate = SPORT;
+
+			}
+
+			// wait a second
+			usleep(1000000);
+			// flush buffer
+			read(uart, &sbuf[0], sizeof(sbuf));
+
+		}
+	}
+
+	if (frsky_state == SPORT) {
+		/* Subscribe to topics */
+		if (!sPort_init()) {
+			err(1, "could not allocate memory");
+		}
+
+		PX4_INFO("sending FrSky SmartPort telemetry");
+
+		struct sensor_baro_s *sensor_baro = malloc(sizeof(struct sensor_baro_s));
+
+		if (sensor_baro == NULL) {
+			err(1, "could not allocate memory");
+		}
+
+		float filtered_alt = NAN;
+		float last_baro_alt = 0.f;
+		int sensor_sub = orb_subscribe(ORB_ID(sensor_baro));
+
+		uint32_t lastBATV_ms = 0;
+		uint32_t lastCUR_ms = 0;
+		uint32_t lastALT_ms = 0;
+		uint32_t lastSPD_ms = 0;
+		uint32_t lastFUEL_ms = 0;
+		uint32_t lastVSPD_ms = 0;
+		uint32_t lastGPS_ms = 0;
+		uint32_t lastNAV_STATE_ms = 0;
+		uint32_t lastGPS_FIX_ms = 0;
+
+		/* send S.port telemetry */
+		while (!thread_should_exit) {
+
+			/* wait for poll frame starting with value 0x7E
+			* note that only the bus master is supposed to put a 0x7E on the bus.
+			* slaves use byte stuffing to send 0x7E and 0x7D.
+			* we expect a poll frame every 12msec
+			*/
+			int status = poll(fds, sizeof(fds) / sizeof(fds[0]), 50);
+
+			if (status < 1) { continue; }
+
+			// read 1 byte
+			int newBytes = read(uart, &sbuf[0], 1);
+
+			if (newBytes < 1 || sbuf[0] != 0x7E) { continue; }
+
+			/* wait for ID byte */
+			status = poll(fds, sizeof(fds) / sizeof(fds[0]), 50);
+
+			if (status < 1) { continue; }
+
+			uint32_t now_ms = hrt_absolute_time() / 1000;
+
+			newBytes = read(uart, &sbuf[1], 1);
+
+			/* get a local copy of the current sensor values
+			 * in order to apply a lowpass filter to baro pressure.
+			 */
+			bool sensor_updated;
+			orb_check(sensor_sub, &sensor_updated);
+
+			if (sensor_updated) {
+				orb_copy(ORB_ID(sensor_baro), sensor_sub, sensor_baro);
+
+				if (isnan(filtered_alt)) {
+					filtered_alt = sensor_baro->altitude;
+
+				} else {
+					filtered_alt = .05f * sensor_baro->altitude + .95f * filtered_alt;
+				}
+			}
+
+			// allow a minimum of 500usec before reply
+			usleep(500);
+
+			sPort_update_topics();
+
+
+			switch (sbuf[1]) {
+
+			case SMARTPORT_POLL_1:
+
+				/* report BATV at 1Hz */
+				if (now_ms - lastBATV_ms > 1000) {
+					lastBATV_ms = now_ms;
+					/* send battery voltage */
+					sPort_send_BATV(uart);
+				}
+
+				break;
+
+
+			case SMARTPORT_POLL_2:
+
+				/* report battery current at 5Hz */
+				if (now_ms - lastCUR_ms > 200) {
+					lastCUR_ms = now_ms;
+					/* send battery current */
+					sPort_send_CUR(uart);
+				}
+
+				break;
+
+
+			case SMARTPORT_POLL_3:
+
+				/* report altitude at 5Hz */
+				if (now_ms - lastALT_ms > 200) {
+					lastALT_ms = now_ms;
+					/* send altitude */
+					sPort_send_ALT(uart);
+				}
+
+				break;
+
+
+			case SMARTPORT_POLL_4:
+
+				/* report speed at 5Hz */
+				if (now_ms - lastSPD_ms > 200) {
+					lastSPD_ms = now_ms;
+					/* send speed */
+					sPort_send_SPD(uart);
+				}
+
+				break;
+
+			case SMARTPORT_POLL_5:
+
+				/* report fuel at 1Hz */
+				if (now_ms - lastFUEL_ms > 1000) {
+					lastFUEL_ms = now_ms;
+					/* send fuel */
+					sPort_send_FUEL(uart);
+				}
+
+				break;
+
+			case SMARTPORT_POLL_6:
+
+				/* report vertical speed at 10Hz */
+				if (now_ms - lastVSPD_ms > 100) {
+					/* estimate vertical speed using first difference and delta t */
+					uint32_t dt = now_ms - lastVSPD_ms;
+					float speed  = (filtered_alt - last_baro_alt) / (1e-3f * (float)dt);
+
+					/* save current alt and timestamp */
+					last_baro_alt = filtered_alt;
+					lastVSPD_ms = now_ms;
+
+					sPort_send_VSPD(uart, speed);
+				}
+
+				break;
+
+			case SMARTPORT_POLL_7:
+
+				/* report GPS data elements at 5*5Hz */
+				if (now_ms - lastGPS_ms > 100) {
+					static int elementCount = 0;
+
+					switch (elementCount) {
+
+					case 0:
+						sPort_send_GPS_LON(uart);
+						elementCount++;
+						break;
+
+					case 1:
+						sPort_send_GPS_LAT(uart);
+						elementCount++;
+						break;
+
+					case 2:
+						sPort_send_GPS_CRS(uart);
+						elementCount++;
+						break;
+
+					case 3:
+						sPort_send_GPS_ALT(uart);
+						elementCount++;
+						break;
+
+					case 4:
+						sPort_send_GPS_SPD(uart);
+						elementCount++;
+						break;
+
+					case 5:
+						sPort_send_GPS_TIME(uart);
+						elementCount = 0;
+						break;
+					}
+
+				}
+
+			/* FALLTHROUGH */
+
+			case SMARTPORT_POLL_8:
+
+				/* report nav_state as DIY_NAVSTATE 2Hz */
+				if (now_ms - lastNAV_STATE_ms > 500) {
+					lastNAV_STATE_ms = now_ms;
+					/* send T1 */
+					sPort_send_NAV_STATE(uart);
+				}
+
+				/* report satcount and fix as DIY_GPSFIX at 2Hz */
+				else if (now_ms - lastGPS_FIX_ms > 500) {
+					lastGPS_FIX_ms = now_ms;
+					/* send T2 */
+					sPort_send_GPS_FIX(uart);
+				}
+
+				break;
+
+			case SMARTPORT_SENSOR_ID_SP2UR: {
+					static int elementCount = 0;
+
+					switch (elementCount++ % 2) {
+					case 0:
+						sPort_send_flight_mode(uart);
+						break;
+
+					default:
+						sPort_send_GPS_info(uart);
+						break;
+					}
+				}
+
+				break;
+			}
+		}
+
+		PX4_DEBUG("freeing sPort memory");
+		sPort_deinit();
+		free(sensor_baro);
+
+		/* either no traffic on the port (0=>timeout), or D type packet */
+
+	} else if (frsky_state == DTYPE) {
+		/* detected D type telemetry: reconfigure UART */
+		PX4_INFO("sending FrSky D type telemetry");
+		int status = set_uart_speed(uart, &uart_config, B9600);
+
+		if (status < 0) {
+			PX4_DEBUG("error setting speed for %s, quitting", device_name);
+			/* Reset the UART flags to original state */
+			tcsetattr(uart, TCSANOW, &uart_config_original);
+			close(uart);
+
+			thread_running = false;
+			return 0;
+		}
+
+		int iteration = 0;
+
+		/* Subscribe to topics */
+		if (!frsky_init()) {
+			err(1, "could not allocate memory");
+		}
+
+		struct adc_linkquality host_frame;
+
+		/* send D8 mode telemetry */
+		while (!thread_should_exit) {
+
+			/* Sleep 100 ms */
+			usleep(100000);
+
+			/* parse incoming data */
+			int nbytes = read(uart, &sbuf[0], sizeof(sbuf));
+			bool new_input = frsky_parse_host((uint8_t *)&sbuf[0], nbytes, &host_frame);
+
+			/* the RSSI value could be useful */
+			if (new_input) {
+				PX4_DEBUG("host frame: ad1:%u, ad2: %u, rssi: %u",
+					  host_frame.ad1, host_frame.ad2, host_frame.linkq);
+			}
+
+			frsky_update_topics();
+
+			/* Send frame 1 (every 200ms): acceleration values, altitude (vario), temperatures, current & voltages, RPM */
+			if (iteration % 2 == 0) {
+				frsky_send_frame1(uart);
+			}
+
+			/* Send frame 2 (every 1000ms): course, latitude, longitude, speed, altitude (GPS), fuel level */
+			if (iteration % 10 == 0) {
+				frsky_send_frame2(uart);
+			}
+
+			/* Send frame 3 (every 5000ms): date, time */
+			if (iteration % 50 == 0) {
+				frsky_send_frame3(uart);
+
+				iteration = 0;
+			}
+
+			iteration++;
+		}
+
+//		/* TODO: flush the input buffer if in full duplex mode */
+//		read(uart, &sbuf[0], sizeof(sbuf));
+		PX4_DEBUG("freeing frsky memory");
+		frsky_deinit();
+
+	}
+
+	/* Reset the UART flags to original state */
+	tcsetattr(uart, TCSANOW, &uart_config_original);
+	close(uart);
+
+	thread_running = false;
+	return 0;
+}
+
+/**
+ * The main command function.
+ * Processes command line arguments and starts the daemon.
+ */
+int frsky_telemetry_main(int argc, char *argv[])
+{
+	if (argc < 2) {
+		warnx("missing command");
+		usage();
+	}
+
+	if (!strcmp(argv[1], "start")) {
+
+		/* this is not an error */
+		if (thread_running) {
+			errx(0, "frsky_telemetry already running");
+		}
+
+		thread_should_exit = false;
+		frsky_task = px4_task_spawn_cmd("frsky_telemetry",
+						SCHED_DEFAULT,
+						SCHED_PRIORITY_DEFAULT + 4,
+						1268,
+						frsky_telemetry_thread_main,
+						(char *const *)argv);
+
+		while (!thread_running) {
+			usleep(200);
+		}
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+
+		/* this is not an error */
+		if (!thread_running) {
+			errx(0, "frsky_telemetry already stopped");
+		}
+
+		thread_should_exit = true;
+
+		while (thread_running) {
+			usleep(1000000);
+			warnx(".");
+		}
+
+		warnx("terminated.");
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (thread_running) {
+			switch (frsky_state) {
+			case SCANNING:
+				errx(0, "running: SCANNING");
+				break;
+
+			case SPORT:
+				errx(0, "running: SPORT");
+				break;
+
+			case DTYPE:
+				errx(0, "running: DTYPE");
+				break;
+
+			}
+
+		} else {
+			errx(1, "not running");
+		}
+	}
+
+	warnx("unrecognized command");
+	usage();
+	/* not getting here */
+	return 0;
+}
@@ -0,0 +1,399 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
+ *   Author: Stefan Rado <px4@sradonia.net>
+ *
+ * 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file sPort_data.c
+ * @author Stefan Rado <px4@sradonia.net>
+ * @author Mark Whitehorn <kd0aij@github.com>
+ *
+ * FrSky SmartPort telemetry implementation.
+ *
+ */
+
+#include "sPort_data.h"
+#include "common.h"
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <arch/math.h>
+#include <geo/geo.h>
+
+#include <uORB/topics/battery_status.h>
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_gps_position.h>
+
+
+#include <drivers/drv_hrt.h>
+
+#define frac(f) (f - (int)f)
+
+struct s_port_subscription_data_s {
+	int sensor_sub;
+	int global_position_sub;
+	int local_position_sub;
+	int battery_status_sub;
+	int vehicle_status_sub;
+	int gps_position_sub;
+
+	struct sensor_combined_s sensor_combined;
+	struct vehicle_global_position_s global_pos;
+	struct vehicle_local_position_s local_pos;
+	struct battery_status_s battery_status;
+	struct vehicle_status_s vehicle_status;
+	struct vehicle_gps_position_s gps_position;
+};
+
+static struct s_port_subscription_data_s *s_port_subscription_data = NULL;
+
+
+/**
+ * Initializes the uORB subscriptions.
+ */
+bool sPort_init()
+{
+	s_port_subscription_data = (struct s_port_subscription_data_s *)calloc(1, sizeof(struct s_port_subscription_data_s));
+
+	if (s_port_subscription_data == NULL) {
+		return false;
+	}
+
+
+	s_port_subscription_data->sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
+	s_port_subscription_data->global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
+	s_port_subscription_data->local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position));
+	s_port_subscription_data->battery_status_sub = orb_subscribe(ORB_ID(battery_status));
+	s_port_subscription_data->vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
+	s_port_subscription_data->gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
+
+	return true;
+}
+
+void sPort_deinit()
+{
+	if (s_port_subscription_data) {
+		orb_unsubscribe(s_port_subscription_data->sensor_sub);
+		orb_unsubscribe(s_port_subscription_data->global_position_sub);
+		orb_unsubscribe(s_port_subscription_data->local_position_sub);
+		orb_unsubscribe(s_port_subscription_data->battery_status_sub);
+		orb_unsubscribe(s_port_subscription_data->vehicle_status_sub);
+		orb_unsubscribe(s_port_subscription_data->gps_position_sub);
+		free(s_port_subscription_data);
+		s_port_subscription_data = NULL;
+	}
+}
+
+void sPort_update_topics()
+{
+	struct s_port_subscription_data_s *subs = s_port_subscription_data;
+
+	bool updated;
+	/* get a local copy of the current sensor values */
+	orb_check(subs->sensor_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(sensor_combined), subs->sensor_sub, &subs->sensor_combined);
+	}
+
+	/* get a local copy of the battery data */
+	orb_check(subs->battery_status_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(battery_status), subs->battery_status_sub, &subs->battery_status);
+	}
+
+	/* get a local copy of the global position data */
+	orb_check(subs->global_position_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(vehicle_global_position), subs->global_position_sub, &subs->global_pos);
+	}
+
+	/* get a local copy of the local position data */
+	orb_check(subs->local_position_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(vehicle_local_position), subs->local_position_sub, &subs->local_pos);
+	}
+
+	/* get a local copy of the vehicle status data */
+	orb_check(subs->vehicle_status_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(vehicle_status), subs->vehicle_status_sub, &subs->vehicle_status);
+	}
+
+	/* get a local copy of the gps position data */
+	orb_check(subs->gps_position_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(vehicle_gps_position), subs->gps_position_sub, &subs->gps_position);
+	}
+
+}
+
+
+static void update_crc(uint16_t *crc, unsigned char b)
+{
+	*crc += b;
+	*crc += *crc >> 8;
+	*crc &= 0xFF;
+}
+
+/**
+ * Sends one byte, performing byte-stuffing if necessary.
+ */
+static void sPort_send_byte(int uart, uint8_t value)
+{
+	const uint8_t x7E[] = { 0x7D, 0x5E };
+	const uint8_t x7D[] = { 0x7D, 0x5D };
+
+	switch (value) {
+	case 0x7E:
+		write(uart, x7E, sizeof(x7E));
+		break;
+
+	case 0x7D:
+		write(uart, x7D, sizeof(x7D));
+		break;
+
+	default:
+		write(uart, &value, sizeof(value));
+		break;
+	}
+}
+
+/**
+ * Sends one data id/value pair.
+ */
+void sPort_send_data(int uart, uint16_t id, uint32_t data)
+{
+	union {
+		uint32_t word;
+		uint8_t byte[4];
+	} buf;
+
+	/* send start byte */
+	const uint8_t c = 0x10;
+	write(uart, &c, 1);
+
+	/* init crc */
+	uint16_t crc = c;
+
+	buf.word = id;
+
+	for (int i = 0; i < 2; i++) {
+		update_crc(&crc, buf.byte[i]);
+		sPort_send_byte(uart, buf.byte[i]);      /* LSB first */
+	}
+
+	buf.word = data;
+
+	for (int i = 0; i < 4; i++) {
+		update_crc(&crc, buf.byte[i]);
+		sPort_send_byte(uart, buf.byte[i]);      /* LSB first */
+	}
+
+	sPort_send_byte(uart, 0xFF - crc);
+}
+
+
+// scaling correct with OpenTX 2.1.7
+void sPort_send_BATV(int uart)
+{
+	/* send battery voltage as VFAS */
+	uint32_t voltage = (int)(100 * s_port_subscription_data->battery_status.voltage_v);
+	sPort_send_data(uart, SMARTPORT_ID_VFAS, voltage);
+}
+
+// verified scaling
+void sPort_send_CUR(int uart)
+{
+	/* send data */
+	uint32_t current = (int)(10 * s_port_subscription_data->battery_status.current_a);
+	sPort_send_data(uart, SMARTPORT_ID_CURR, current);
+}
+
+// verified scaling for "custom" altitude option
+// OpenTX uses the initial reading as field elevation and displays
+// the difference (altitude - field)
+void sPort_send_ALT(int uart)
+{
+	/* send data */
+	uint32_t alt = (int)(100 * s_port_subscription_data->sensor_combined.baro_alt_meter);
+	sPort_send_data(uart, SMARTPORT_ID_ALT, alt);
+}
+
+// verified scaling for "calculated" option
+void sPort_send_SPD(int uart)
+{
+	struct vehicle_global_position_s *global_pos = &s_port_subscription_data->global_pos;
+	/* send data for A2 */
+	float speed  = sqrtf(global_pos->vel_n * global_pos->vel_n + global_pos->vel_e * global_pos->vel_e);
+	uint32_t ispeed = (int)(10 * speed);
+	sPort_send_data(uart, SMARTPORT_ID_GPS_SPD, ispeed);
+}
+
+// TODO: verify scaling
+void sPort_send_VSPD(int uart, float speed)
+{
+	/* send data for VARIO vertical speed: int16 cm/sec */
+	int32_t ispeed = (int)(100 * speed);
+	sPort_send_data(uart, SMARTPORT_ID_VARIO, ispeed);
+}
+
+// verified scaling
+void sPort_send_FUEL(int uart)
+{
+	/* send data */
+	uint32_t fuel = (int)(100 * s_port_subscription_data->battery_status.remaining);
+	sPort_send_data(uart, SMARTPORT_ID_FUEL, fuel);
+}
+
+void sPort_send_GPS_LON(int uart)
+{
+	/* send longitude */
+	/* convert to 30 bit signed magnitude degrees*6E5 with MSb = 1 and bit 30=sign */
+	/* precision is approximately 0.1m */
+	uint32_t iLon =  6E-2 * fabs(s_port_subscription_data->gps_position.lon);
+
+	iLon |= (1 << 31);
+
+	if (s_port_subscription_data->gps_position.lon < 0) { iLon |= (1 << 30); }
+
+	sPort_send_data(uart, SMARTPORT_ID_GPS_LON_LAT, iLon);
+}
+
+void sPort_send_GPS_LAT(int uart)
+{
+	/* send latitude */
+	/* convert to 30 bit signed magnitude degrees*6E5 with MSb = 0 and bit 30=sign */
+	uint32_t iLat = 6E-2 * fabs(s_port_subscription_data->gps_position.lat);
+
+	if (s_port_subscription_data->gps_position.lat < 0) { iLat |= (1 << 30); }
+
+	sPort_send_data(uart, SMARTPORT_ID_GPS_LON_LAT, iLat);
+}
+
+void sPort_send_GPS_ALT(int uart)
+{
+	/* send altitude */
+	uint32_t iAlt = s_port_subscription_data->gps_position.alt / 10;
+	sPort_send_data(uart, SMARTPORT_ID_GPS_ALT, iAlt);
+}
+
+void sPort_send_GPS_CRS(int uart)
+{
+	/* send course */
+
+	/* convert to 30 bit signed magnitude degrees*6E5 with MSb = 1 and bit 30=sign */
+	int32_t iYaw = s_port_subscription_data->local_pos.yaw * 18000.0f / M_PI_F;
+
+	if (iYaw < 0) { iYaw += 36000; }
+
+	sPort_send_data(uart, SMARTPORT_ID_GPS_CRS, iYaw);
+}
+
+void sPort_send_GPS_TIME(int uart)
+{
+	static int date = 0;
+
+	/* send formatted frame */
+	time_t time_gps = s_port_subscription_data->gps_position.time_utc_usec / 1000000ULL;
+	struct tm *tm_gps = gmtime(&time_gps);
+
+	if (date) {
+
+		sPort_send_data(uart, SMARTPORT_ID_GPS_TIME,
+				(uint32_t) 0xff | (tm_gps->tm_mday << 8) | ((tm_gps->tm_mon + 1) << 16) | ((tm_gps->tm_year - 100) << 24));
+		date = 0;
+
+	} else {
+
+		sPort_send_data(uart, SMARTPORT_ID_GPS_TIME,
+				(uint32_t) 0x00 | (tm_gps->tm_sec << 8) | (tm_gps->tm_min  << 16) | (tm_gps->tm_hour << 24));
+		date = 1;
+
+	}
+}
+
+void sPort_send_GPS_SPD(int uart)
+{
+	struct vehicle_global_position_s *global_pos = &s_port_subscription_data->global_pos;
+	/* send 100 * knots */
+	float speed  = sqrtf(global_pos->vel_n * global_pos->vel_n + global_pos->vel_e * global_pos->vel_e);
+	uint32_t ispeed = (int)(1944 * speed);
+	sPort_send_data(uart, SMARTPORT_ID_GPS_SPD, ispeed);
+}
+
+/*
+ * Sends nav_state + 128
+ */
+void sPort_send_NAV_STATE(int uart)
+{
+	uint32_t navstate = (int)(128 + s_port_subscription_data->vehicle_status.nav_state);
+
+	/* send data */
+	sPort_send_data(uart, SMARTPORT_ID_DIY_NAVSTATE, navstate);
+}
+
+// verified scaling
+// sends number of sats and type of gps fix
+void sPort_send_GPS_FIX(int uart)
+{
+	/* send data */
+	uint32_t satcount = (int)(s_port_subscription_data->gps_position.satellites_used);
+	uint32_t fixtype = (int)(s_port_subscription_data->gps_position.fix_type);
+	uint32_t t2 = satcount * 10 + fixtype;
+	sPort_send_data(uart, SMARTPORT_ID_DIY_GPSFIX, t2);
+}
+
+
+void sPort_send_flight_mode(int uart)
+{
+	struct s_port_subscription_data_s *subs = s_port_subscription_data;
+	int16_t telem_flight_mode = get_telemetry_flight_mode(subs->vehicle_status.nav_state);
+
+	sPort_send_data(uart, FRSKY_ID_TEMP1, telem_flight_mode); // send flight mode as TEMP1. This matches with OpenTX & APM
+
+}
+
+void sPort_send_GPS_info(int uart)
+{
+	struct s_port_subscription_data_s *subs = s_port_subscription_data;
+	sPort_send_data(uart, FRSKY_ID_TEMP2, subs->gps_position.satellites_used * 10 + subs->gps_position.fix_type);
+}
@@ -0,0 +1,110 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
+ *   Author: Stefan Rado <px4@sradonia.net>
+ *
+ * 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file sPort_data.h
+ * @author Mark Whitehorn <kd0aij@github.com>
+ *
+ * FrSky SmartPort telemetry implementation.
+ *
+ */
+#ifndef _SPORT_DATA_H
+#define _SPORT_DATA_H
+
+#include <sys/types.h>
+#include <stdbool.h>
+
+/* FrSky SmartPort polling IDs captured from X4R */
+#define SMARTPORT_POLL_1    0x1B
+#define SMARTPORT_POLL_2    0x34
+#define SMARTPORT_POLL_3    0x95
+#define SMARTPORT_POLL_4    0x16
+#define SMARTPORT_POLL_5    0xB7
+#define SMARTPORT_POLL_6    0x00
+#define SMARTPORT_POLL_7    0x83
+#define SMARTPORT_POLL_8    0xBA
+#define SMARTPORT_SENSOR_ID_SP2UR     0xC6 // Sensor ID  6
+
+/* FrSky SmartPort sensor IDs. See more here: https://github.com/opentx/opentx/blob/2.2/radio/src/telemetry/frsky.h */
+#define SMARTPORT_ID_RSSI          0xf101
+#define SMARTPORT_ID_RXA1          0xf102	// supplied by RX
+#define SMARTPORT_ID_RXA2          0xf103	// supplied by RX
+#define SMARTPORT_ID_BATV          0xf104
+#define SMARTPORT_ID_SWR           0xf105   // Standing Wave Ratio
+#define SMARTPORT_ID_T1            0x0400
+#define SMARTPORT_ID_T2            0x0410
+#define SMARTPORT_ID_RPM           0x0500
+#define SMARTPORT_ID_FUEL          0x0600
+#define SMARTPORT_ID_ALT           0x0100
+#define SMARTPORT_ID_VARIO         0x0110   //VSPEED
+#define SMARTPORT_ID_ACCX          0x0700
+#define SMARTPORT_ID_ACCY          0x0710
+#define SMARTPORT_ID_ACCZ          0x0720
+#define SMARTPORT_ID_CURR          0x0200
+#define SMARTPORT_ID_VFAS          0x0210  //Volt per Cell
+#define SMARTPORT_ID_CELLS         0x0300
+#define SMARTPORT_ID_GPS_LON_LAT   0x0800
+#define SMARTPORT_ID_GPS_ALT       0x0820
+#define SMARTPORT_ID_GPS_SPD       0x0830
+#define SMARTPORT_ID_GPS_CRS       0x0840
+#define SMARTPORT_ID_GPS_TIME      0x0850
+#define SMARTPORT_ID_DIY_FIRST     0x5000
+#define SMARTPORT_ID_DIY_LAST      0x50ff  //We have 256 possible ID's for custom values :)
+#define SMARTPORT_ID_DIY_NAVSTATE  0x5000
+#define SMARTPORT_ID_DIY_GPSFIX    0x5001
+
+// Public functions
+bool sPort_init(void);
+void sPort_deinit(void);
+void sPort_update_topics(void);
+void sPort_send_data(int uart, uint16_t id, uint32_t data);
+void sPort_send_BATV(int uart);
+void sPort_send_CUR(int uart);
+void sPort_send_ALT(int uart);
+void sPort_send_SPD(int uart);
+void sPort_send_VSPD(int uart, float speed);
+void sPort_send_FUEL(int uart);
+void sPort_send_GPS_LON(int uart);
+void sPort_send_GPS_LAT(int uart);
+void sPort_send_GPS_ALT(int uart);
+void sPort_send_GPS_SPD(int uart);
+void sPort_send_GPS_CRS(int uart);
+void sPort_send_GPS_TIME(int uart);
+void sPort_send_flight_mode(int uart);
+void sPort_send_GPS_info(int uart);
+
+void sPort_send_NAV_STATE(int uart);
+void sPort_send_GPS_FIX(int uart);
+
+#endif /* _SPORT_TELEMETRY_H */
@@ -0,0 +1,45 @@
+############################################################################
+#
+#   Copyright (c) 2015 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.
+#
+############################################################################
+list(APPEND MODULE_CFLAGS -Wframe-larger-than=1100)
+px4_add_module(
+	MODULE drivers__hott
+	COMPILE_FLAGS
+	SRCS
+		messages.cpp
+		comms.cpp
+	DEPENDS
+		platforms__common
+	)
+
+add_subdirectory(hott_sensors)
+add_subdirectory(hott_telemetry)
@@ -0,0 +1,96 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file comms.c
+ * @author Simon Wilks <sjwilks@gmail.com>
+ *
+ */
+
+#include "comms.h"
+
+#include <fcntl.h>
+#include <stdio.h>
+#include <sys/ioctl.h>
+#include <systemlib/err.h>
+#include <termios.h>
+
+int
+open_uart(const char *device)
+{
+	/* baud rate */
+	static const speed_t speed = B19200;
+
+	/* open uart */
+	const int uart = open(device, O_RDWR | O_NOCTTY);
+
+	if (uart < 0) {
+		PX4_ERR("opening %s", device);
+		return 1;
+	}
+
+	/* Back up the original uart configuration to restore it after exit */
+	int termios_state;
+	struct termios uart_config_original;
+
+	if ((termios_state = tcgetattr(uart, &uart_config_original)) < 0) {
+		close(uart);
+		PX4_ERR("%s: %d", device, termios_state);
+		return 1;
+	}
+
+	/* Fill the struct for the new configuration */
+	struct termios uart_config;
+	tcgetattr(uart, &uart_config);
+
+	/* Clear ONLCR flag (which appends a CR for every LF) */
+	uart_config.c_oflag &= ~ONLCR;
+
+	/* Set baud rate */
+	if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
+		close(uart);
+		PX4_ERR("%s: %d (cfsetispeed, cfsetospeed)", device, termios_state);
+		return 1;
+	}
+
+	if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
+		close(uart);
+		PX4_ERR("%s (tcsetattr)", device);
+		return 1;
+	}
+
+	/* Activate single wire mode */
+	ioctl(uart, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED);
+
+	return uart;
+}
@@ -0,0 +1,46 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file comms.h
+ * @author Simon Wilks <sjwilks@gmail.com>
+ *
+ */
+
+
+#ifndef COMMS_H_
+#define COMMS_H
+
+__EXPORT int open_uart(const char *device);
+
+#endif /* COMMS_H_ */
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (c) 2015 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__hott__hott_sensors
+	MAIN hott_sensors
+	COMPILE_FLAGS
+	SRCS
+		hott_sensors.cpp
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix :
@@ -0,0 +1,241 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Author: Simon Wilks <sjwilks@gmail.com>
+ *
+ * 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file hott_sensors.c
+ * @author Simon Wilks <sjwilks@gmail.com>
+ *
+ * Graupner HoTT sensor driver implementation.
+ *
+ * Poll any sensors connected to the PX4 via the telemetry wire.
+ */
+
+#include <fcntl.h>
+#include <px4_config.h>
+#include <px4_defines.h>
+#include <px4_tasks.h>
+#include <poll.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <sys/ioctl.h>
+#include <unistd.h>
+#include <systemlib/err.h>
+#include <systemlib/systemlib.h>
+
+#include "../comms.h"
+#include "../messages.h"
+
+#define DEFAULT_UART "/dev/ttyS0";		/**< USART1 */
+
+static int thread_should_exit = false;		/**< Deamon exit flag */
+static int thread_running = false;		/**< Deamon status flag */
+static int deamon_task;				/**< Handle of deamon task / thread */
+static const char daemon_name[] = "hott_sensors";
+static const char commandline_usage[] = "usage: hott_sensors start|status|stop [-d <device>]";
+
+/**
+ * Deamon management function.
+ */
+extern "C" __EXPORT int hott_sensors_main(int argc, char *argv[]);
+
+/**
+ * Mainloop of daemon.
+ */
+int hott_sensors_thread_main(int argc, char *argv[]);
+
+static int recv_data(int uart, uint8_t *buffer, size_t *size, uint8_t *id);
+static int send_poll(int uart, uint8_t *buffer, size_t size);
+
+int
+send_poll(int uart, uint8_t *buffer, size_t size)
+{
+	for (size_t i = 0; i < size; i++) {
+		write(uart, &buffer[i], sizeof(buffer[i]));
+
+		/* Sleep before sending the next byte. */
+		usleep(POST_WRITE_DELAY_IN_USECS);
+	}
+
+	/* A hack the reads out what was written so the next read from the receiver doesn't get it. */
+	/* TODO: Fix this!! */
+	uint8_t dummy[size];
+	read(uart, &dummy, size);
+
+	return OK;
+}
+
+int
+recv_data(int uart, uint8_t *buffer, size_t *size, uint8_t *id)
+{
+	static const int timeout_ms = 1000;
+
+	struct pollfd fds;
+	fds.fd = uart;
+	fds.events = POLLIN;
+
+	// XXX should this poll be inside the while loop???
+	if (poll(&fds, 1, timeout_ms) > 0) {
+		int i = 0;
+		bool stop_byte_read = false;
+
+		while (true)  {
+			read(uart, &buffer[i], sizeof(buffer[i]));
+
+			if (stop_byte_read) {
+				// XXX process checksum
+				*size = ++i;
+				return OK;
+			}
+
+			// XXX can some other field not have the STOP BYTE value?
+			if (buffer[i] == STOP_BYTE) {
+				*id = buffer[1];
+				stop_byte_read = true;
+			}
+
+			i++;
+		}
+	}
+
+	return PX4_ERROR;
+}
+
+int
+hott_sensors_thread_main(int argc, char *argv[])
+{
+	warnx("starting");
+
+	thread_running = true;
+
+	const char *device = DEFAULT_UART;
+
+	/* read commandline arguments */
+	for (int i = 0; i < argc && argv[i]; i++) {
+		if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { //device set
+			if (argc > i + 1) {
+				device = argv[i + 1];
+
+			} else {
+				thread_running = false;
+				errx(1, "missing parameter to -d\n%s", commandline_usage);
+			}
+		}
+	}
+
+	/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
+	const int uart = open_uart(device);
+
+	if (uart < 0) {
+		errx(1, "Open fail, exiting.");
+		thread_running = false;
+	}
+
+	init_pub_messages();
+
+	uint8_t buffer[MAX_MESSAGE_BUFFER_SIZE];
+	size_t size = 0;
+	uint8_t id = 0;
+
+	while (!thread_should_exit) {
+		// Currently we only support a General Air Module sensor.
+		build_gam_request(&buffer[0], &size);
+		send_poll(uart, buffer, size);
+
+		// The sensor will need a little time before it starts sending.
+		usleep(5000);
+
+		recv_data(uart, &buffer[0], &size, &id);
+
+		// Determine which module sent it and process accordingly.
+		if (id == GAM_SENSOR_ID) {
+			publish_gam_message(buffer);
+
+		} else {
+			warnx("Unknown sensor ID: %d", id);
+		}
+	}
+
+	warnx("exiting");
+	close(uart);
+	thread_running = false;
+
+	return 0;
+}
+
+/**
+ * Process command line arguments and start the daemon.
+ */
+int
+hott_sensors_main(int argc, char *argv[])
+{
+	if (argc < 2) {
+		errx(1, "missing command\n%s", commandline_usage);
+	}
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (thread_running) {
+			warnx("already running");
+			exit(0);
+		}
+
+		thread_should_exit = false;
+		deamon_task = px4_task_spawn_cmd(daemon_name,
+						 SCHED_DEFAULT,
+						 SCHED_PRIORITY_DEFAULT,
+						 1024,
+						 hott_sensors_thread_main,
+						 (argv) ? (char *const *)&argv[2] : (char *const *)NULL);
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		thread_should_exit = true;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (thread_running) {
+			warnx("is running");
+
+		} else {
+			warnx("not started");
+		}
+
+		exit(0);
+	}
+
+	errx(1, "unrecognized command\n%s", commandline_usage);
+}
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (c) 2015 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__hott__hott_telemetry
+	MAIN hott_telemetry
+	COMPILE_FLAGS
+	SRCS
+		hott_telemetry.cpp
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix :
@@ -0,0 +1,376 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Author: Simon Wilks <sjwilks@gmail.com>
+ *
+ * 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file hott_telemetry_main.c
+ * @author Simon Wilks <sjwilks@gmail.com>
+ *
+ * Graupner HoTT Telemetry implementation.
+ *
+ * The HoTT receiver polls each device at a regular interval at which point
+ * a data packet can be returned if necessary.
+ *
+ */
+
+#include <fcntl.h>
+#include <px4_config.h>
+#include <px4_defines.h>
+#include <px4_tasks.h>
+#include <poll.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <sys/ioctl.h>
+#include <unistd.h>
+#include <systemlib/err.h>
+#include <systemlib/perf_counter.h>
+#include <systemlib/systemlib.h>
+
+#include "../comms.h"
+#include "../messages.h"
+
+#define DEFAULT_UART "/dev/ttyS6";		/**< Serial4 */
+
+static int thread_should_exit = false;		/**< Deamon exit flag */
+static int thread_running = false;		/**< Deamon status flag */
+static int deamon_task;				/**< Handle of deamon task / thread */
+static const char daemon_name[] = "hott_telemetry";
+static const char commandline_usage[] =
+	"usage: hott_telemetry start|status|stop [-d <device>] [-t <timeout ms>] [-r <read delay us>] [-w <write delay us>]";
+
+static uint8_t read_log[16];
+
+static int timeout_ms = POLL_TIMEOUT_IN_MSECS;
+static int read_delay_us = POST_READ_DELAY_IN_USECS;
+static int write_delay_us = POST_WRITE_DELAY_IN_USECS;
+
+perf_counter_t	reqs_count;
+perf_counter_t	connect_count;
+perf_counter_t	recon_port;
+perf_counter_t	bin_reply;
+perf_counter_t	txt_reply;
+perf_counter_t	bad_reply;
+perf_counter_t	dead_reply;
+
+/**
+ * Deamon management function.
+ */
+extern "C" __EXPORT int hott_telemetry_main(int argc, char *argv[]);
+
+/**
+ * Mainloop of daemon.
+ */
+int hott_telemetry_thread_main(int argc, char *argv[]);
+
+static int recv_req_id(int uart, uint8_t *id);
+static int send_data(int uart, uint8_t *buffer, size_t size);
+
+int
+recv_req_id(int uart, uint8_t *id)
+{
+	uint8_t mode;
+
+	struct pollfd fds;
+	fds.fd = uart;
+	fds.events = POLLIN;
+
+	if (poll(&fds, 1, timeout_ms) > 0) {
+		/* Get the mode: binary or text  */
+		read(uart, &mode, sizeof(mode));
+
+		perf_count(reqs_count);
+
+		// Debug log
+		for (int x = 15; x > 0; x--) {
+			read_log[x] = read_log[x - 1];
+		}
+
+		read_log[0] = mode;
+
+		/* if we have a binary mode request */
+		if (mode != BINARY_MODE_REQUEST_ID) {
+			return PX4_ERROR;
+		}
+
+		/* Read the device ID being polled */
+		read(uart, id, sizeof(*id));
+
+	} else {
+		warnx("UART timeout on TX/RX port");
+		return PX4_ERROR;
+	}
+
+	return PX4_OK;
+}
+
+int
+send_data(int uart, uint8_t *buffer, size_t size)
+{
+	usleep(read_delay_us);
+
+	uint16_t checksum = 0;
+
+	for (size_t i = 0; i < size; i++) {
+		if (i == size - 1) {
+			/* Set the checksum: the first uint8_t is taken as the checksum. */
+			buffer[i] = checksum & 0xff;
+
+		} else {
+			checksum += buffer[i];
+		}
+
+		write(uart, &buffer[i], sizeof(buffer[i]));
+
+		/* Sleep before sending the next byte. */
+		usleep(write_delay_us);
+	}
+
+	/* A hack the reads out what was written so the next read from the receiver doesn't get it. */
+	/* TODO: Fix this!! */
+	uint8_t dummy[size];
+	read(uart, &dummy, size);
+
+	return PX4_OK;
+}
+
+int
+hott_telemetry_thread_main(int argc, char *argv[])
+{
+	warnx("starting");
+
+	connect_count = perf_alloc(PC_COUNT,	"reconnects       ");
+	recon_port = perf_alloc(PC_COUNT,		"reopen port      ");
+	reqs_count = perf_alloc(PC_COUNT,		"requests        ");
+	bin_reply = perf_alloc(PC_COUNT,		"bin replies     ");
+	txt_reply = perf_alloc(PC_COUNT,		"text replies    ");
+	bad_reply = perf_alloc(PC_COUNT,		"unknown replies ");
+	dead_reply = perf_alloc(PC_COUNT,		"dead replies    ");
+
+	thread_running = true;
+
+	const char *device = DEFAULT_UART;
+
+	/* read commandline arguments */
+	for (int i = 0; i < argc && argv[i]; i++) {
+		if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { //device set
+			if (argc > i + 1) {
+				device = argv[i + 1];
+
+			} else {
+				thread_running = false;
+				errx(1, "missing parameter to -d\n%s", commandline_usage);
+			}
+		}
+	}
+
+	/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
+	int uart = open_uart(device);
+
+	if (uart < 0) {
+		errx(1, "Failed opening HoTT UART, exiting.");
+		thread_running = false;
+	}
+
+	init_sub_messages();
+
+	uint8_t buffer[MAX_MESSAGE_BUFFER_SIZE];
+	size_t size = 0;
+	uint8_t id = 0;
+
+	bool connected = true;
+	int recon = 0;
+
+	while (!thread_should_exit) {
+		// Listen for and serve poll from the receiver.
+		if (recv_req_id(uart, &id) == PX4_OK) {
+			if (!connected) {
+				connected = true;
+			}
+
+			switch (id) {
+			case EAM_SENSOR_ID:
+				build_eam_response(buffer, &size);
+				perf_count(bin_reply);
+				break;
+
+			case GAM_SENSOR_ID:
+				build_gam_response(buffer, &size);
+				perf_count(bin_reply);
+				break;
+
+			case GPS_SENSOR_ID:
+				build_gps_response(buffer, &size);
+				perf_count(bin_reply);
+				break;
+
+			case BINARY_MODE_REQUEST_ID:
+				perf_count(dead_reply);
+				break;
+
+			default:
+				perf_count(bad_reply);
+				continue;	// Not a module we support.
+			}
+
+			send_data(uart, buffer, size);
+
+		} else {
+			if (connected) {
+				connected = false;
+
+			} else {
+				recon++;
+			}
+
+			if (recon > 100) {
+				perf_count(recon_port);
+				close(uart);
+				uart = open_uart(device);
+				perf_reset(reqs_count);
+				perf_reset(bin_reply);
+				perf_reset(txt_reply);
+				perf_reset(dead_reply);
+				perf_reset(bad_reply);
+			}
+		}
+	}
+
+	warnx("exiting");
+
+	close(uart);
+
+	thread_running = false;
+
+	perf_free(connect_count);
+	perf_free(recon_port);
+	perf_free(reqs_count);
+	perf_free(bin_reply);
+	perf_free(txt_reply);
+	perf_free(bad_reply);
+	perf_free(dead_reply);
+
+	return 0;
+}
+
+/**
+ * Process command line arguments and start the daemon.
+ */
+int
+hott_telemetry_main(int argc, char *argv[])
+{
+	if (argc < 2) {
+		errx(1, "missing command\n%s", commandline_usage);
+	}
+
+	/* read commandline arguments */
+	for (int i = 0; i < argc && argv[i]; i++) {
+		if (strcmp(argv[i], "-t") == 0 || strcmp(argv[i], "--timeout") == 0) { //device set
+			if (argc > i + 1) {
+				timeout_ms = atoi(argv[i + 1]);
+
+			} else {
+				errx(1, "missing parameter to -t\n%s", commandline_usage);
+			}
+		}
+
+		if (strcmp(argv[i], "-r") == 0 || strcmp(argv[i], "--read-delay") == 0) { //device set
+			if (argc > i + 1) {
+				read_delay_us = atoi(argv[i + 1]);
+
+			} else {
+				errx(1, "missing parameter to -r\n%s", commandline_usage);
+			}
+		}
+
+		if (strcmp(argv[i], "-w") == 0 || strcmp(argv[i], "--write-delay") == 0) { //device set
+			if (argc > i + 1) {
+				write_delay_us = atoi(argv[i + 1]);
+
+			} else {
+				errx(1, "missing parameter to -w\n%s", commandline_usage);
+			}
+		}
+	}
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (thread_running) {
+			warnx("already running");
+			exit(0);
+		}
+
+		thread_should_exit = false;
+		deamon_task = px4_task_spawn_cmd(daemon_name,
+						 SCHED_DEFAULT,
+						 SCHED_PRIORITY_DEFAULT,
+						 1500,
+						 hott_telemetry_thread_main,
+						 (argv) ? (char *const *)&argv[2] : (char *const *)NULL);
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		thread_should_exit = true;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (thread_running) {
+			warnx("is running");
+
+
+			for (int x = 15; x >= 0; x--) {
+				printf("%2x ", read_log[x]);
+			}
+
+			printf("\npoll timeout     : %i ms\n", timeout_ms);
+			printf("post write delay : %i us\n", write_delay_us);
+			printf("post read delay  : %i us\n", read_delay_us);
+			perf_print_counter(recon_port);
+			perf_print_counter(connect_count);
+			perf_print_counter(reqs_count);
+			perf_print_counter(bin_reply);
+			perf_print_counter(txt_reply);
+			perf_print_counter(bad_reply);
+			perf_print_counter(dead_reply);
+
+		} else {
+			warnx("not started");
+		}
+
+		exit(0);
+	}
+
+	errx(1, "unrecognized command\n%s", commandline_usage);
+}
@@ -0,0 +1,336 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: @author Simon Wilks <sjwilks@gmail.com>
+ *
+ * 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file messages.c
+ *
+ */
+
+#include "messages.h"
+
+#include <math.h>
+#include <stdio.h>
+#include <string.h>
+#include <geo/geo.h>
+#include <unistd.h>
+#include <uORB/topics/airspeed.h>
+#include <uORB/topics/battery_status.h>
+#include <uORB/topics/esc_status.h>
+#include <uORB/topics/home_position.h>
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/vehicle_gps_position.h>
+
+#include <drivers/drv_hrt.h>
+
+/* The board is very roughly 5 deg warmer than the surrounding air */
+#define BOARD_TEMP_OFFSET_DEG 5
+
+static int _battery_sub = -1;
+static int _gps_sub = -1;
+static int _home_sub = -1;
+static int _sensor_sub = -1;
+static int _airspeed_sub = -1;
+static int _esc_sub = -1;
+
+static orb_advert_t _esc_pub = nullptr;
+
+static bool _home_position_set = false;
+static double _home_lat = 0.0d;
+static double _home_lon = 0.0d;
+
+void
+init_sub_messages(void)
+{
+	_battery_sub = orb_subscribe(ORB_ID(battery_status));
+	_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
+	_home_sub = orb_subscribe(ORB_ID(home_position));
+	_sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
+	_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
+	_esc_sub = orb_subscribe(ORB_ID(esc_status));
+}
+
+void
+init_pub_messages(void)
+{
+}
+
+void
+build_gam_request(uint8_t *buffer, size_t *size)
+{
+	struct gam_module_poll_msg msg;
+	*size = sizeof(msg);
+	memset(&msg, 0, *size);
+
+	msg.mode = BINARY_MODE_REQUEST_ID;
+	msg.id = GAM_SENSOR_ID;
+
+	memcpy(buffer, &msg, *size);
+}
+
+void
+publish_gam_message(const uint8_t *buffer)
+{
+	struct gam_module_msg msg;
+	size_t size = sizeof(msg);
+	memset(&msg, 0, size);
+	memcpy(&msg, buffer, size);
+	struct esc_status_s esc;
+	memset(&esc, 0, sizeof(esc));
+
+	// Publish it.
+	esc.timestamp = hrt_absolute_time();
+	esc.esc_count = 1;
+	esc.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_PPM;
+
+	esc.esc[0].esc_vendor = esc_status_s::ESC_VENDOR_GRAUPNER_HOTT;
+	esc.esc[0].esc_rpm = (uint16_t)((msg.rpm_H << 8) | (msg.rpm_L & 0xff)) * 10;
+	esc.esc[0].esc_temperature = static_cast<float>(msg.temperature1) - 20.0F;
+	esc.esc[0].esc_voltage = static_cast<float>((msg.main_voltage_H << 8) | (msg.main_voltage_L & 0xff)) * 0.1F;
+	esc.esc[0].esc_current = static_cast<float>((msg.current_H << 8) | (msg.current_L & 0xff)) * 0.1F;
+
+	/* announce the esc if needed, just publish else */
+	if (_esc_pub != nullptr) {
+		orb_publish(ORB_ID(esc_status), _esc_pub, &esc);
+
+	} else {
+		_esc_pub = orb_advertise(ORB_ID(esc_status), &esc);
+	}
+}
+
+void
+build_eam_response(uint8_t *buffer, size_t *size)
+{
+	/* get a local copy of the current sensor values */
+	struct sensor_combined_s raw;
+	memset(&raw, 0, sizeof(raw));
+	orb_copy(ORB_ID(sensor_combined), _sensor_sub, &raw);
+
+	/* get a local copy of the battery data */
+	struct battery_status_s battery;
+	memset(&battery, 0, sizeof(battery));
+	orb_copy(ORB_ID(battery_status), _battery_sub, &battery);
+
+	struct eam_module_msg msg;
+	*size = sizeof(msg);
+	memset(&msg, 0, *size);
+
+	msg.start = START_BYTE;
+	msg.eam_sensor_id = EAM_SENSOR_ID;
+	msg.sensor_text_id = EAM_SENSOR_TEXT_ID;
+
+	msg.temperature1 = (uint8_t)(raw.baro_temp_celcius + 20);
+	msg.temperature2 = msg.temperature1 - BOARD_TEMP_OFFSET_DEG;
+
+	msg.main_voltage_L = (uint8_t)(battery.voltage_v * 10);
+
+	uint16_t alt = (uint16_t)(raw.baro_alt_meter + 500);
+	msg.altitude_L = (uint8_t)alt & 0xff;
+	msg.altitude_H = (uint8_t)(alt >> 8) & 0xff;
+
+	/* get a local copy of the airspeed data */
+	struct airspeed_s airspeed;
+	memset(&airspeed, 0, sizeof(airspeed));
+	orb_copy(ORB_ID(airspeed), _airspeed_sub, &airspeed);
+
+	uint16_t speed = (uint16_t)(airspeed.indicated_airspeed_m_s * 3.6f);
+	msg.speed_L = (uint8_t)speed & 0xff;
+	msg.speed_H = (uint8_t)(speed >> 8) & 0xff;
+
+	msg.stop = STOP_BYTE;
+	memcpy(buffer, &msg, *size);
+}
+
+void
+build_gam_response(uint8_t *buffer, size_t *size)
+{
+	/* get a local copy of the ESC Status values */
+	struct esc_status_s esc;
+	memset(&esc, 0, sizeof(esc));
+	orb_copy(ORB_ID(esc_status), _esc_sub, &esc);
+
+	struct gam_module_msg msg;
+	*size = sizeof(msg);
+	memset(&msg, 0, *size);
+
+	msg.start = START_BYTE;
+	msg.gam_sensor_id = GAM_SENSOR_ID;
+	msg.sensor_text_id = GAM_SENSOR_TEXT_ID;
+
+	msg.temperature1 = (uint8_t)(esc.esc[0].esc_temperature + 20.0F);
+	msg.temperature2 = 20;  // 0 deg. C.
+
+	const uint16_t voltage = (uint16_t)(esc.esc[0].esc_voltage * 10.0F);
+	msg.main_voltage_L = (uint8_t)voltage & 0xff;
+	msg.main_voltage_H = (uint8_t)(voltage >> 8) & 0xff;
+
+	const uint16_t current = (uint16_t)(esc.esc[0].esc_current * 10.0F);
+	msg.current_L = (uint8_t)current & 0xff;
+	msg.current_H = (uint8_t)(current >> 8) & 0xff;
+
+	const uint16_t rpm = (uint16_t)(esc.esc[0].esc_rpm * 0.1f);
+	msg.rpm_L = (uint8_t)rpm & 0xff;
+	msg.rpm_H = (uint8_t)(rpm >> 8) & 0xff;
+
+	msg.stop = STOP_BYTE;
+	memcpy(buffer, &msg, *size);
+}
+
+void
+build_gps_response(uint8_t *buffer, size_t *size)
+{
+	/* get a local copy of the current sensor values */
+	struct sensor_combined_s raw;
+	memset(&raw, 0, sizeof(raw));
+	orb_copy(ORB_ID(sensor_combined), _sensor_sub, &raw);
+
+	/* get a local copy of the battery data */
+	struct vehicle_gps_position_s gps;
+	memset(&gps, 0, sizeof(gps));
+	orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &gps);
+
+	struct gps_module_msg msg;
+	*size = sizeof(msg);
+	memset(&msg, 0, *size);
+
+	msg.start = START_BYTE;
+	msg.sensor_id = GPS_SENSOR_ID;
+	msg.sensor_text_id = GPS_SENSOR_TEXT_ID;
+
+	msg.gps_num_sat = gps.satellites_used;
+
+	/* The GPS fix type: 0 = none, 2 = 2D, 3 = 3D */
+	msg.gps_fix_char = (uint8_t)(gps.fix_type + 48);
+	msg.gps_fix = (uint8_t)(gps.fix_type + 48);
+
+	/* No point collecting more data if we don't have a 3D fix yet */
+	if (gps.fix_type > 2) {
+		/* Current flight direction */
+		msg.flight_direction = (uint8_t)(gps.cog_rad * M_RAD_TO_DEG_F);
+
+		/* GPS speed */
+		uint16_t speed = (uint16_t)(gps.vel_m_s * 3.6f);
+		msg.gps_speed_L = (uint8_t)speed & 0xff;
+		msg.gps_speed_H = (uint8_t)(speed >> 8) & 0xff;
+
+		/* Get latitude in degrees, minutes and seconds */
+		double lat = ((double)(gps.lat)) * 1e-7d;
+
+		/* Set the N or S specifier */
+		msg.latitude_ns = 0;
+
+		if (lat < 0) {
+			msg.latitude_ns = 1;
+			lat = abs(lat);
+		}
+
+		int deg;
+		int min;
+		int sec;
+		convert_to_degrees_minutes_seconds(lat, &deg, &min, &sec);
+
+		uint16_t lat_min = (uint16_t)(deg * 100 + min);
+		msg.latitude_min_L = (uint8_t)lat_min & 0xff;
+		msg.latitude_min_H = (uint8_t)(lat_min >> 8) & 0xff;
+		uint16_t lat_sec = (uint16_t)(sec);
+		msg.latitude_sec_L = (uint8_t)lat_sec & 0xff;
+		msg.latitude_sec_H = (uint8_t)(lat_sec >> 8) & 0xff;
+
+		/* Get longitude in degrees, minutes and seconds */
+		double lon = ((double)(gps.lon)) * 1e-7d;
+
+		/* Set the E or W specifier */
+		msg.longitude_ew = 0;
+
+		if (lon < 0) {
+			msg.longitude_ew = 1;
+			lon = abs(lon);
+		}
+
+		convert_to_degrees_minutes_seconds(lon, &deg, &min, &sec);
+
+		uint16_t lon_min = (uint16_t)(deg * 100 + min);
+		msg.longitude_min_L = (uint8_t)lon_min & 0xff;
+		msg.longitude_min_H = (uint8_t)(lon_min >> 8) & 0xff;
+		uint16_t lon_sec = (uint16_t)(sec);
+		msg.longitude_sec_L = (uint8_t)lon_sec & 0xff;
+		msg.longitude_sec_H = (uint8_t)(lon_sec >> 8) & 0xff;
+
+		/* Altitude */
+		uint16_t alt = (uint16_t)(gps.alt * 1e-3f + 500.0f);
+		msg.altitude_L = (uint8_t)alt & 0xff;
+		msg.altitude_H = (uint8_t)(alt >> 8) & 0xff;
+
+		/* Get any (and probably only ever one) _home_sub position report */
+		bool updated;
+		orb_check(_home_sub, &updated);
+
+		if (updated) {
+			/* get a local copy of the home position data */
+			struct home_position_s home;
+			memset(&home, 0, sizeof(home));
+			orb_copy(ORB_ID(home_position), _home_sub, &home);
+
+			_home_lat = home.lat;
+			_home_lon = home.lon;
+			_home_position_set = true;
+		}
+
+		/* Distance from home */
+		if (_home_position_set) {
+			uint16_t dist = (uint16_t)get_distance_to_next_waypoint(_home_lat, _home_lon, lat, lon);
+
+			msg.distance_L = (uint8_t)dist & 0xff;
+			msg.distance_H = (uint8_t)(dist >> 8) & 0xff;
+
+			/* Direction back to home */
+			uint16_t bearing = (uint16_t)(get_bearing_to_next_waypoint(_home_lat, _home_lon, lat, lon) * M_RAD_TO_DEG_F);
+			msg.home_direction = (uint8_t)bearing >> 1;
+		}
+	}
+
+	msg.stop = STOP_BYTE;
+	memcpy(buffer, &msg, *size);
+}
+
+void
+convert_to_degrees_minutes_seconds(double val, int *deg, int *min, int *sec)
+{
+	*deg = (int)val;
+
+	double delta = val - *deg;
+	const double min_d = delta * 60.0d;
+	*min = (int)min_d;
+	delta = min_d - *min;
+	*sec = (int)(delta * 10000.0d);
+}
@@ -0,0 +1,253 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Author: Simon Wilks <sjwilks@gmail.com>
+ *
+ * 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file messages.h
+ * @author Simon Wilks <sjwilks@gmail.com>
+ *
+ * Graupner HoTT Telemetry message generation.
+ *
+ */
+#ifndef MESSAGES_H_
+#define MESSAGES_H
+
+#include <stdlib.h>
+#include <stdint.h>
+
+#define POLL_TIMEOUT_IN_MSECS		3500
+
+/* The HoTT receiver demands a minimum 5ms period of silence after delivering its request.
+ * Note that the value specified here is lower than 5000 (5ms) as time is lost constructing
+ * the message after the read which takes some milliseconds.
+ */
+#define POST_READ_DELAY_IN_USECS	4000
+
+/* A pause of 3ms is required between each uint8_t sent back to the HoTT receiver. Much lower
+ * values can be used in practise though.
+ */
+#define POST_WRITE_DELAY_IN_USECS	3000
+
+// Protocol constants.
+#define BINARY_MODE_REQUEST_ID	0x80
+#define START_BYTE		0x7c
+#define STOP_BYTE		0x7d
+#define TEMP_ZERO_CELSIUS	0x14
+
+/* The GAM Module poll message. */
+struct gam_module_poll_msg {
+	uint8_t mode;
+	uint8_t id;
+};
+
+/* Electric Air Module (EAM) constants. */
+#define EAM_SENSOR_ID			0x8e
+#define EAM_SENSOR_TEXT_ID		0xe0
+
+/* The Electric Air Module message. */
+struct eam_module_msg {
+	uint8_t start;			/**< Start byte */
+	uint8_t eam_sensor_id;		/**< EAM sensor	*/
+	uint8_t warning;
+	uint8_t sensor_text_id;
+	uint8_t alarm_inverse1;
+	uint8_t alarm_inverse2;
+	uint8_t cell1_L;		/**< Lipo cell voltages. Not supported.	*/
+	uint8_t cell2_L;
+	uint8_t cell3_L;
+	uint8_t cell4_L;
+	uint8_t cell5_L;
+	uint8_t cell6_L;
+	uint8_t cell7_L;
+	uint8_t cell1_H;
+	uint8_t cell2_H;
+	uint8_t cell3_H;
+	uint8_t cell4_H;
+	uint8_t cell5_H;
+	uint8_t cell6_H;
+	uint8_t cell7_H;
+	uint8_t batt1_voltage_L;	/**< Battery 1 voltage, lower 8-bits in steps of 0.02V 	*/
+	uint8_t batt1_voltage_H;
+	uint8_t batt2_voltage_L;	/**< Battery 2 voltage, lower 8-bits in steps of 0.02V 	*/
+	uint8_t batt2_voltage_H;
+	uint8_t temperature1;		/**< Temperature sensor 1. 20 = 0 degrees */
+	uint8_t temperature2;
+	uint8_t altitude_L;		/**< Attitude (meters) lower 8-bits. 500 = 0 meters */
+	uint8_t altitude_H;
+	uint8_t current_L;		/**< Current (mAh) lower 8-bits in steps of 0.1V */
+	uint8_t current_H;
+	uint8_t main_voltage_L;		/**< Main power voltage lower 8-bits in steps of 0.1V */
+	uint8_t main_voltage_H;
+	uint8_t battery_capacity_L;	/**< Used battery capacity in steps of 10mAh */
+	uint8_t battery_capacity_H;
+	uint8_t climbrate_L;		/**< Climb rate in 0.01m/s. 0m/s = 30000 */
+	uint8_t climbrate_H;
+	uint8_t climbrate_3s;		/**< Climb rate in m/3sec. 0m/3sec = 120 */
+	uint8_t rpm_L;			/**< RPM Lower 8-bits In steps of 10 U/min */
+	uint8_t rpm_H;
+	uint8_t electric_min;		/**< Flight time in minutes. */
+	uint8_t electric_sec;		/**< Flight time in seconds. */
+	uint8_t speed_L;		/**< Airspeed in km/h in steps of 1 km/h */
+	uint8_t speed_H;
+	uint8_t stop;			/**< Stop byte */
+	uint8_t checksum;		/**< Lower 8-bits of all bytes summed. */
+};
+
+
+/* General Air Module (GAM) constants. */
+#define GAM_SENSOR_ID			0x8d
+#define GAM_SENSOR_TEXT_ID		0xd0
+
+struct gam_module_msg {
+	uint8_t start;			/**< Start byte	*/
+	uint8_t gam_sensor_id;		/**< GAM sensor id */
+	uint8_t warning_beeps;
+	uint8_t sensor_text_id;
+	uint8_t alarm_invers1;
+	uint8_t alarm_invers2;
+	uint8_t cell1;			/**< Lipo cell voltages. Not supported.	*/
+	uint8_t cell2;
+	uint8_t cell3;
+	uint8_t cell4;
+	uint8_t cell5;
+	uint8_t cell6;
+	uint8_t batt1_L;		/**< Battery 1 voltage LSB value. 0.1V steps. 50 = 5.5V	*/
+	uint8_t batt1_H;
+	uint8_t batt2_L;		/**< Battery 2 voltage LSB value. 0.1V steps. 50 = 5.5V	*/
+	uint8_t batt2_H;
+	uint8_t temperature1;		/**< Temperature 1. offset of 20. a value of 20 = 0°C */
+	uint8_t temperature2;		/**< Temperature 2. offset of 20. a value of 20 = 0°C */
+	uint8_t fuel_procent;		/**< Fuel capacity in %. Values 0 - 100 */
+	/**< Graphical display ranges: 0 25% 50% 75% 100% */
+	uint8_t fuel_ml_L;		/**< Fuel in ml scale. Full = 65535 */
+	uint8_t fuel_ml_H;
+	uint8_t rpm_L;			/**< RPM in 10 RPM steps. 300 = 3000rpm */
+	uint8_t rpm_H;
+	uint8_t altitude_L;		/**< Altitude in meters. offset of 500, 500 = 0m */
+	uint8_t altitude_H;
+	uint8_t climbrate_L;		/**< Climb rate in 0.01m/s. Value of 30000 = 0.00 m/s */
+	uint8_t climbrate_H;
+	uint8_t climbrate3s;		/**< Climb rate in m/3sec. Value of 120 = 0m/3sec */
+	uint8_t current_L;		/**< Current in 0.1A steps */
+	uint8_t current_H;
+	uint8_t main_voltage_L;		/**< Main power voltage using 0.1V steps */
+	uint8_t main_voltage_H;
+	uint8_t batt_cap_L;		/**< Used battery capacity in 10mAh steps */
+	uint8_t batt_cap_H;
+	uint8_t speed_L;		/**< Speed in km/h */
+	uint8_t speed_H;
+	uint8_t min_cell_volt;		/**< Minimum cell voltage in 2mV steps. 124 = 2,48V */
+	uint8_t min_cell_volt_num;	/**< Number of the cell with the lowest voltage */
+	uint8_t rpm2_L;			/**< RPM in 10 RPM steps. 300 = 3000rpm */
+	uint8_t rpm2_H;
+	uint8_t general_error_number;	/**< Voice error == 12. TODO: more docu */
+	uint8_t pressure;		/**< Pressure up to 16bar. 0,1bar scale. 20 = 2bar */
+	uint8_t version;
+	uint8_t stop;			/**< Stop byte */
+	uint8_t checksum;		/**< Lower 8-bits of all bytes summed */
+};
+
+/* GPS sensor constants. */
+#define GPS_SENSOR_ID		0x8a
+#define GPS_SENSOR_TEXT_ID	0xa0
+
+/**
+ * The GPS sensor message
+ * Struct based on: https://code.google.com/p/diy-hott-gps/downloads
+ */
+struct gps_module_msg {
+	uint8_t start;			/**< Start byte */
+	uint8_t sensor_id;		/**< GPS sensor ID*/
+	uint8_t warning;		/**< 0…= warning beeps */
+	uint8_t sensor_text_id;		/**< GPS Sensor text mode ID */
+	uint8_t alarm_inverse1;		/**< 01 inverse status */
+	uint8_t alarm_inverse2;		/**< 00 inverse status status 1 = no GPS Signal */
+	uint8_t flight_direction;	/**< 119 = Flightdir./dir. 1 = 2°; 0° (North), 9 0° (East), 180° (South), 270° (West) */
+	uint8_t gps_speed_L;		/**< 8 = /GPS speed low byte 8km/h */
+	uint8_t gps_speed_H;		/**< 0 = /GPS speed high byte */
+
+	uint8_t latitude_ns;		/**< 000 = N = 48°39’988 */
+	uint8_t latitude_min_L;		/**< 231 0xE7 = 0x12E7 = 4839 */
+	uint8_t latitude_min_H;		/**< 018 18 = 0x12 */
+	uint8_t latitude_sec_L;		/**< 171 220 = 0xDC = 0x03DC =0988 */
+	uint8_t latitude_sec_H;		/**< 016 3 = 0x03 */
+
+	uint8_t longitude_ew;		/**< 000  = E= 9° 25’9360 */
+	uint8_t longitude_min_L;	/**< 150 157 = 0x9D = 0x039D = 0925 */
+	uint8_t longitude_min_H;	/**< 003 3 = 0x03 */
+	uint8_t longitude_sec_L;	/**< 056 144 = 0x90 0x2490 = 9360 */
+	uint8_t longitude_sec_H;	/**< 004 36 = 0x24 */
+
+	uint8_t distance_L;		/**< 027 123 = /distance low byte 6 = 6 m */
+	uint8_t distance_H;		/**< 036 35 = /distance high byte */
+	uint8_t altitude_L;		/**< 243 244 = /Altitude low byte 500 = 0m */
+	uint8_t altitude_H;		/**< 001 1 = /Altitude high byte */
+	uint8_t resolution_L;		/**< 48 = Low Byte m/s resolution 0.01m 48 = 30000 = 0.00m/s (1=0.01m/s) */
+	uint8_t resolution_H;		/**< 117 = High Byte m/s resolution 0.01m */
+	uint8_t unknown1;		/**< 120 = 0m/3s */
+	uint8_t gps_num_sat;		/**< GPS.Satellites (number of satellites) (1 byte) */
+	uint8_t gps_fix_char;		/**< GPS.FixChar. (GPS fix character. display, if DGPS, 2D oder 3D) (1 byte) */
+	uint8_t home_direction;		/**< HomeDirection (direction from starting point to Model position) (1 byte) */
+	uint8_t angle_x_direction;	/**< angle x-direction (1 byte) */
+	uint8_t angle_y_direction;	/**< angle y-direction (1 byte) */
+	uint8_t angle_z_direction;	/**< angle z-direction (1 byte) */
+	uint8_t gyro_x_L;		/**< gyro x low byte (2 bytes) */
+	uint8_t gyro_x_H;		/**< gyro x high byte */
+	uint8_t gyro_y_L;		/**< gyro y low byte (2 bytes) */
+	uint8_t gyro_y_H;		/**< gyro y high byte */
+	uint8_t gyro_z_L;		/**< gyro z low byte (2 bytes) */
+	uint8_t gyro_z_H;		/**< gyro z high byte */
+	uint8_t vibration;		/**< vibration (1 bytes) */
+	uint8_t ascii4;			/**< 00 ASCII Free Character [4] */
+	uint8_t ascii5;			/**< 00 ASCII Free Character [5] */
+	uint8_t gps_fix;		/**< 00 ASCII Free Character [6], we use it for GPS FIX */
+	uint8_t version;
+	uint8_t stop;			/**< Stop byte */
+	uint8_t checksum;		/**< Lower 8-bits of all bytes summed */
+};
+
+// The maximum size of a message.
+#define MAX_MESSAGE_BUFFER_SIZE 45
+
+__EXPORT void init_sub_messages(void);
+__EXPORT void init_pub_messages(void);
+__EXPORT void build_gam_request(uint8_t *buffer, size_t *size);
+__EXPORT void publish_gam_message(const uint8_t *buffer);
+__EXPORT void build_eam_response(uint8_t *buffer, size_t *size);
+__EXPORT void build_gam_response(uint8_t *buffer, size_t *size);
+__EXPORT void build_gps_response(uint8_t *buffer, size_t *size);
+__EXPORT float _get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
+__EXPORT void convert_to_degrees_minutes_seconds(double lat, int *deg, int *min, int *sec);
+
+
+#endif /* MESSAGES_H_ */
@@ -0,0 +1,44 @@
+############################################################################
+#
+#   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
+	SRCS
+		IridiumSBD.cpp
+		iridiumsbd_params.c
+	DEPENDS
+		platforms__common
+	)
@@ -0,0 +1,848 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2016-2018 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 <px4_tasks.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 <systemlib/err.h>
+#include <systemlib/systemlib.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) {
+		strncpy(instance->test_command, argv[2], sizeof(instance->test_command));
+		instance->test_command[sizeof(instance->test_command) - 1] = 0;
+
+	} 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;
+
+		}
+
+		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;
+	}
+
+	PX4_DEBUG("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) {
+			PX4_DEBUG("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, "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 && ((int64_t)(hrt_absolute_time() - last_read_time) > param_read_interval_s * 1000000)) {
+		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) {
+		PX4_DEBUG("UPDATE SIGNAL QUALITY: ERROR");
+
+		new_state = SATCOM_STATE_STANDBY;
+		return;
+	}
+
+	if (strncmp((const char *)rx_command_buf, "+CSQ:", 5)) {
+		PX4_DEBUG("UPDATE SIGNAL QUALITY: WRONG ANSWER:");
+		PX4_DEBUG("%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();
+
+	PX4_DEBUG("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) {
+		PX4_DEBUG("SBD SESSION: ERROR");
+		PX4_DEBUG("SBD SESSION: RESULT %d", res);
+
+		new_state = SATCOM_STATE_STANDBY;
+		return;
+	}
+
+	if (strncmp((const char *)rx_command_buf, "+SBDIX:", 7)) {
+		PX4_DEBUG("SBD SESSION: WRONG ANSWER:");
+		PX4_DEBUG("%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);
+
+	PX4_DEBUG("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:
+		PX4_DEBUG("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:
+		PX4_DEBUG("SBD SESSION: MO SUCCESS, MT FAIL");
+
+		tx_session_pending = false;
+		break;
+
+	case 32:
+		PX4_DEBUG("SBD SESSION: NO NETWORK SIGNAL");
+
+		signal_quality = 0;
+
+		break;
+
+	default:
+		PX4_DEBUG("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)
+{
+	PX4_DEBUG("WRITE: LEN %d, TX WRITTEN: %d", buflen, tx_buf_write_idx);
+
+	if ((ssize_t)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)
+{
+	PX4_DEBUG("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 */
+			return CDev::ioctl(filp, cmd, arg);
+		}
+	}
+}
+
+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()) {
+		PX4_DEBUG("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) {
+		PX4_DEBUG("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);
+
+	PX4_DEBUG("SEND SBD: CHECKSUM %d %d", checksum[0], checksum[1]);
+
+	if (read_at_command() != SATCOM_RESULT_OK) {
+		PX4_DEBUG("SEND SBD: ERROR WHILE WRITING DATA TO MODEM!");
+
+		pthread_mutex_unlock(&tx_buf_mutex);
+		return;
+	}
+
+	if (rx_command_buf[0] != '0') {
+		PX4_DEBUG("SEND SBD: ERROR WHILE WRITING DATA TO MODEM! (%d)", rx_command_buf[0] - '0');
+
+		pthread_mutex_unlock(&tx_buf_mutex);
+		return;
+	}
+
+	PX4_DEBUG("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()) {
+		PX4_DEBUG("READ SBD: MODEM NOT READY!");
+
+		return;
+	}
+
+	write_at("AT+SBDRB");
+
+	if (read_at_msg() != SATCOM_RESULT_OK) {
+		PX4_DEBUG("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) {
+		PX4_DEBUG("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])) {
+		PX4_DEBUG("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;
+
+	PX4_DEBUG("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') {
+		PX4_DEBUG("CLEAR MO BUFFER: ERROR");
+
+		return false;
+	}
+
+	return true;
+}
+
+void IridiumSBD::start_csq(void)
+{
+	PX4_DEBUG("UPDATING SIGNAL QUALITY");
+
+	if (!is_modem_ready()) {
+		PX4_DEBUG("UPDATE SIGNAL QUALITY: MODEM NOT READY!");
+
+		return;
+	}
+
+	write_at("AT+CSQ");
+	new_state = SATCOM_STATE_CSQ;
+}
+
+void IridiumSBD::start_sbd_session(void)
+{
+	PX4_DEBUG("STARTING SBD SESSION");
+
+	if (!is_modem_ready()) {
+		PX4_DEBUG("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)
+{
+	PX4_DEBUG("opening iridium sbd modem UART: %s", uart_name);
+
+	uart_fd = ::open(uart_name, O_RDWR | O_BINARY);
+
+	if (uart_fd < 0) {
+		PX4_DEBUG("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);
+
+	PX4_DEBUG("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)
+{
+	PX4_DEBUG("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;
+
+						PX4_DEBUG("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]");
+
+	return PX4_ERROR;
+}
@@ -0,0 +1,282 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2016-2018 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;
+
+	int32_t 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();
+
+	/*
+	 * 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);
+};
@@ -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);