From 5f18ce506d1a8395e1565db941b7af64a5936f31 Mon Sep 17 00:00:00 2001
From: Stefan Rado <px4@sradonia.net>
Date: Sun, 24 Nov 2013 13:39:02 +0100
Subject: Add FrSky telemetry application. This daemon emulates an FrSky sensor
 hub by periodically sending data packets to an attached FrSky receiver.

---
 makefiles/config_px4fmu-v1_default.mk         |   1 +
 makefiles/config_px4fmu-v2_default.mk         |   1 +
 src/drivers/frsky_telemetry/frsky_data.c      | 223 +++++++++++++++++++++
 src/drivers/frsky_telemetry/frsky_data.h      |  84 ++++++++
 src/drivers/frsky_telemetry/frsky_telemetry.c | 269 ++++++++++++++++++++++++++
 src/drivers/frsky_telemetry/module.mk         |  41 ++++
 6 files changed, 619 insertions(+)
 create mode 100644 src/drivers/frsky_telemetry/frsky_data.c
 create mode 100644 src/drivers/frsky_telemetry/frsky_data.h
 create mode 100644 src/drivers/frsky_telemetry/frsky_telemetry.c
 create mode 100644 src/drivers/frsky_telemetry/module.mk

diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk
index 306827086..7e53de0ae 100644
--- a/makefiles/config_px4fmu-v1_default.mk
+++ b/makefiles/config_px4fmu-v1_default.mk
@@ -37,6 +37,7 @@ MODULES		+= drivers/roboclaw
 MODULES		+= drivers/airspeed
 MODULES		+= drivers/ets_airspeed
 MODULES		+= drivers/meas_airspeed
+MODULES		+= drivers/frsky_telemetry
 MODULES		+= modules/sensors
 
 #
diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk
index 761fb8d9d..c5190375f 100644
--- a/makefiles/config_px4fmu-v2_default.mk
+++ b/makefiles/config_px4fmu-v2_default.mk
@@ -35,6 +35,7 @@ MODULES		+= drivers/roboclaw
 MODULES		+= drivers/airspeed
 MODULES		+= drivers/ets_airspeed
 MODULES		+= drivers/meas_airspeed
+MODULES		+= drivers/frsky_telemetry
 MODULES		+= modules/sensors
 
 # Needs to be burned to the ground and re-written; for now,
diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c
new file mode 100644
index 000000000..8a57070b1
--- /dev/null
+++ b/src/drivers/frsky_telemetry/frsky_data.c
@@ -0,0 +1,223 @@
+/****************************************************************************
+ *
+ *   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 frsky_data.c
+ * @author Stefan Rado <px4@sradonia.net>
+ *
+ * FrSky telemetry implementation.
+ *
+ */
+
+#include "frsky_data.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>
+
+
+static int battery_sub = -1;
+static int sensor_sub = -1;
+static int global_position_sub = -1;
+
+
+/**
+ * Initializes the uORB subscriptions.
+ */
+void frsky_init()
+{
+	battery_sub = orb_subscribe(ORB_ID(battery_status));
+	global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
+	sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
+}
+
+/**
+ * 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, uint16_t data)
+{
+	frsky_send_startstop(uart);
+
+	frsky_send_byte(uart, id);
+	frsky_send_byte(uart, data);      /* Low */
+	frsky_send_byte(uart, data >> 8); /* High */
+}
+
+/**
+ * Sends frame 1 (every 200ms):
+ *   acceleration values, altitude (vario), temperatures, current & voltages, RPM
+ */
+void frsky_send_frame1(int uart)
+{
+	/* 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);
+
+	/* send formatted frame */
+	// TODO
+	frsky_send_data(uart, FRSKY_ID_ACCEL_X, raw.accelerometer_m_s2[0] * 100);
+	frsky_send_data(uart, FRSKY_ID_ACCEL_Y, raw.accelerometer_m_s2[1] * 100);
+	frsky_send_data(uart, FRSKY_ID_ACCEL_Z, raw.accelerometer_m_s2[2] * 100);
+
+	frsky_send_data(uart, FRSKY_ID_BARO_ALT_BP, raw.baro_alt_meter);
+	frsky_send_data(uart, FRSKY_ID_BARO_ALT_AP, (raw.baro_alt_meter - (int)raw.baro_alt_meter) * 1000.0f);
+
+	frsky_send_data(uart, FRSKY_ID_TEMP1, raw.baro_temp_celcius);
+	frsky_send_data(uart, FRSKY_ID_TEMP2, 0);
+
+	frsky_send_data(uart, FRSKY_ID_VOLTS, 0); /* cell voltage. 4 bits cell number, 12 bits voltage in 0.2V steps, scale 0-4.2V */
+	frsky_send_data(uart, FRSKY_ID_CURRENT, battery.current_a);
+
+	frsky_send_data(uart, FRSKY_ID_VOLTS_BP, battery.voltage_v);
+	frsky_send_data(uart, FRSKY_ID_VOLTS_AP, (battery.voltage_v - (int)battery.voltage_v) * 1000.0f);
+
+	frsky_send_data(uart, FRSKY_ID_RPM, 0);
+
+	frsky_send_startstop(uart);
+}
+
+/**
+ * Sends frame 2 (every 1000ms):
+ *   course, latitude, longitude, speed, altitude (GPS), fuel level
+ */
+void frsky_send_frame2(int uart)
+{
+ 	/* get a local copy of the battery data */
+	struct vehicle_global_position_s global_pos;
+	memset(&global_pos, 0, sizeof(global_pos));
+	orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos);
+
+	/* send formatted frame */
+	// TODO
+	float course = 0, lat = 0, lon = 0, speed = 0, alt = 0, sec = 0;
+	if (global_pos.valid)
+	{
+		course = (global_pos.yaw + M_PI_F) / M_PI_F * 180.0f;
+		// TODO: latitude, longitude
+		speed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy);
+		alt = global_pos.alt;
+	}
+
+	frsky_send_data(uart, FRSKY_ID_GPS_COURS_BP, course);
+	frsky_send_data(uart, FRSKY_ID_GPS_COURS_AP, (course - (int)course) * 100.0f);
+
+	frsky_send_data(uart, FRSKY_ID_GPS_LAT_BP, 0);
+	frsky_send_data(uart, FRSKY_ID_GPS_LAT_AP, 0);
+	frsky_send_data(uart, FRSKY_ID_GPS_LAT_NS, 0);
+
+	frsky_send_data(uart, FRSKY_ID_GPS_LONG_BP, 0);
+	frsky_send_data(uart, FRSKY_ID_GPS_LONG_AP, 0);
+	frsky_send_data(uart, FRSKY_ID_GPS_LONG_EW, 0);
+
+	frsky_send_data(uart, FRSKY_ID_GPS_SPEED_BP, speed);
+	frsky_send_data(uart, FRSKY_ID_GPS_SPEED_AP, (speed - (int)speed) * 100.0f);
+
+	frsky_send_data(uart, FRSKY_ID_GPS_ALT_BP, alt);
+	frsky_send_data(uart, FRSKY_ID_GPS_ALT_AP, (alt - (int)alt) * 100.0f);
+
+	frsky_send_data(uart, FRSKY_ID_FUEL, 0);
+
+	frsky_send_data(uart, FRSKY_ID_GPS_SEC, 0);
+
+	frsky_send_startstop(uart);
+}
+
+/**
+ * Sends frame 3 (every 5000ms):
+ *   date, time
+ */
+void frsky_send_frame3(int uart)
+{
+ 	/* get a local copy of the battery data */
+	struct vehicle_global_position_s global_pos;
+	memset(&global_pos, 0, sizeof(global_pos));
+	orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos);
+
+	/* send formatted frame */
+	// TODO
+	frsky_send_data(uart, FRSKY_ID_GPS_DAY_MONTH, 0);
+	frsky_send_data(uart, FRSKY_ID_GPS_YEAR, 0);
+	frsky_send_data(uart, FRSKY_ID_GPS_HOUR_MIN, 0);
+	frsky_send_data(uart, FRSKY_ID_GPS_SEC, 0);
+
+	frsky_send_startstop(uart);
+}
diff --git a/src/drivers/frsky_telemetry/frsky_data.h b/src/drivers/frsky_telemetry/frsky_data.h
new file mode 100644
index 000000000..e0c39a42b
--- /dev/null
+++ b/src/drivers/frsky_telemetry/frsky_data.h
@@ -0,0 +1,84 @@
+/****************************************************************************
+ *
+ *   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 frsky_data.h
+ * @author Stefan Rado <px4@sradonia.net>
+ *
+ * FrSky telemetry implementation.
+ *
+ */
+#ifndef _FRSKY_DATA_H
+#define _FRSKY_DATA_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
+
+// Public functions
+void frsky_init(void);
+void frsky_send_frame1(int uart);
+void frsky_send_frame2(int uart);
+void frsky_send_frame3(int uart);
+
+#endif /* _FRSKY_TELEMETRY_H */
diff --git a/src/drivers/frsky_telemetry/frsky_telemetry.c b/src/drivers/frsky_telemetry/frsky_telemetry.c
new file mode 100644
index 000000000..5f3837b6e
--- /dev/null
+++ b/src/drivers/frsky_telemetry/frsky_telemetry.c
@@ -0,0 +1,269 @@
+/****************************************************************************
+ *
+ *   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 frsky_telemetry.c
+ * @author Stefan Rado <px4@sradonia.net>
+ *
+ * FrSky telemetry implementation.
+ *
+ * This daemon emulates an FrSky sensor hub by periodically sending data
+ * packets to an attached FrSky receiver.
+ *
+ */
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <stdbool.h>
+#include <string.h>
+#include <sys/types.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <systemlib/err.h>
+#include <systemlib/systemlib.h>
+#include <termios.h>
+
+#include "frsky_data.h"
+
+
+/* thread state */
+static volatile bool thread_should_exit = false;
+static volatile bool thread_running = false;
+static int frsky_task;
+
+/* functions */
+static int frsky_open_uart(const char *uart_name, struct termios *uart_config_original);
+static void usage(void);
+static int frsky_telemetry_thread_main(int argc, char *argv[]);
+__EXPORT int frsky_telemetry_main(int argc, char *argv[]);
+
+
+/**
+ * Opens the UART device and sets all required serial parameters.
+ */
+static int
+frsky_open_uart(const char *uart_name, struct termios *uart_config_original)
+{
+	/* Open UART */
+	const int uart = open(uart_name, O_WRONLY | O_NOCTTY);
+
+	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("ERROR get termios config %s: %d\n", uart_name, termios_state);
+		close(uart);
+		return -1;
+	}
+
+	/* Fill the struct for the new configuration */
+	struct termios uart_config;
+	tcgetattr(uart, &uart_config);
+
+	/* Disable output post-processing */
+	uart_config.c_oflag &= ~OPOST;
+
+	/* Set baud rate */
+	static const speed_t speed = B9600;
+
+	if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
+		warnx("ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
+		close(uart);
+		return -1;
+	}
+
+	if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
+		warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name);
+		close(uart);
+		return -1;
+	}
+
+	return uart;
+}
+
+/**
+ * Print command usage information
+ */
+static void
+usage()
+{
+	fprintf(stderr, "usage: frsky_telemetry start [-d <devicename>]\n"
+		"       frsky_telemetry stop\n"
+		"       frsky_telemetry status\n");
+	exit(1);
+}
+
+/**
+ * The daemon thread.
+ */
+static int
+frsky_telemetry_thread_main(int argc, char *argv[])
+{
+	/* Default values for arguments */
+	char *device_name = "/dev/ttyS1"; /* USART2 */
+
+	/* 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;
+		}
+	}
+
+	/* Print welcome text */
+	warnx("FrSky telemetry interface starting...");
+
+	/* Open UART */
+	struct termios uart_config_original;
+	const int uart = frsky_open_uart(device_name, &uart_config_original);
+
+	if (uart < 0)
+		err(1, "could not open %s", device_name);
+
+	/* Subscribe to topics */
+	frsky_init();
+
+	thread_running = true;
+
+	/* Main thread loop */
+	unsigned int iteration = 0;
+	while (!thread_should_exit) {
+
+		/* Sleep 200 ms */
+		usleep(200000);
+
+		/* Send frame 1 (every 200ms): acceleration values, altitude (vario), temperatures, current & voltages, RPM */
+		frsky_send_frame1(uart);
+
+		/* Send frame 2 (every 1000ms): course, latitude, longitude, speed, altitude (GPS), fuel level */
+		if (iteration % 5 == 0)
+		{
+			frsky_send_frame2(uart);
+		}
+
+		/* Send frame 3 (every 5000ms): date, time */
+		if (iteration % 25 == 0)
+		{
+			frsky_send_frame3(uart);
+
+			iteration = 0;
+		}
+
+		iteration++;
+	}
+
+	/* 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 = task_spawn_cmd("frsky_telemetry",
+					  SCHED_DEFAULT,
+					  SCHED_PRIORITY_DEFAULT,
+					  2048,
+					  frsky_telemetry_thread_main,
+					  (const char **)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(200000);
+			warnx(".");
+		}
+
+		warnx("terminated.");
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (thread_running) {
+			errx(0, "running");
+
+		} else {
+			errx(1, "not running");
+		}
+	}
+
+	warnx("unrecognized command");
+	usage();
+	/* not getting here */
+	return 0;
+}
diff --git a/src/drivers/frsky_telemetry/module.mk b/src/drivers/frsky_telemetry/module.mk
new file mode 100644
index 000000000..808966691
--- /dev/null
+++ b/src/drivers/frsky_telemetry/module.mk
@@ -0,0 +1,41 @@
+############################################################################
+#
+#   Copyright (c) 2012, 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.
+#
+############################################################################
+
+#
+# FrSky telemetry application.
+#
+
+MODULE_COMMAND	= frsky_telemetry
+
+SRCS		= frsky_data.c \
+              frsky_telemetry.c
-- 
cgit v1.2.3


From 50cbd1949971241a0b6108708b2b3fefe0c498e3 Mon Sep 17 00:00:00 2001
From: Stefan Rado <px4@sradonia.net>
Date: Mon, 30 Dec 2013 20:27:04 +0100
Subject: Fixes to FrSky telemetry data formats.

---
 src/drivers/frsky_telemetry/frsky_data.c | 137 +++++++++++++++++++++++--------
 src/drivers/frsky_telemetry/frsky_data.h |  33 --------
 2 files changed, 105 insertions(+), 65 deletions(-)

diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c
index 8a57070b1..3a0c4b4b3 100644
--- a/src/drivers/frsky_telemetry/frsky_data.c
+++ b/src/drivers/frsky_telemetry/frsky_data.c
@@ -51,11 +51,52 @@
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_status.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
+
+
+#define frac(f) (f - (int)f)
+
+float frsky_format_gps(float dec);
 
 
 static int battery_sub = -1;
 static int sensor_sub = -1;
 static int global_position_sub = -1;
+static int vehicle_status_sub = -1;
 
 
 /**
@@ -66,6 +107,7 @@ void frsky_init()
 	battery_sub = orb_subscribe(ORB_ID(battery_status));
 	global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
 	sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
+	vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
 }
 
 /**
@@ -107,13 +149,16 @@ frsky_send_byte(int uart, uint8_t value)
  * Sends one data id/value pair.
  */
 static void
-frsky_send_data(int uart, uint8_t id, uint16_t data)
+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, data);      /* Low */
-	frsky_send_byte(uart, data >> 8); /* High */
+	frsky_send_byte(uart, udata);      /* LSB */
+	frsky_send_byte(uart, udata >> 8); /* MSB */
 }
 
 /**
@@ -133,70 +178,96 @@ void frsky_send_frame1(int uart)
 	orb_copy(ORB_ID(battery_status), battery_sub, &battery);
 
 	/* send formatted frame */
-	// TODO
-	frsky_send_data(uart, FRSKY_ID_ACCEL_X, raw.accelerometer_m_s2[0] * 100);
-	frsky_send_data(uart, FRSKY_ID_ACCEL_Y, raw.accelerometer_m_s2[1] * 100);
-	frsky_send_data(uart, FRSKY_ID_ACCEL_Z, raw.accelerometer_m_s2[2] * 100);
+	frsky_send_data(uart, FRSKY_ID_ACCEL_X, raw.accelerometer_m_s2[0] * 1000.0f);
+	frsky_send_data(uart, FRSKY_ID_ACCEL_Y, raw.accelerometer_m_s2[1] * 1000.0f);
+	frsky_send_data(uart, FRSKY_ID_ACCEL_Z, raw.accelerometer_m_s2[2] * 1000.0f);
 
 	frsky_send_data(uart, FRSKY_ID_BARO_ALT_BP, raw.baro_alt_meter);
-	frsky_send_data(uart, FRSKY_ID_BARO_ALT_AP, (raw.baro_alt_meter - (int)raw.baro_alt_meter) * 1000.0f);
+	frsky_send_data(uart, FRSKY_ID_BARO_ALT_AP, frac(raw.baro_alt_meter) * 1000.0f);
 
-	frsky_send_data(uart, FRSKY_ID_TEMP1, raw.baro_temp_celcius);
+	frsky_send_data(uart, FRSKY_ID_TEMP1, raw.baro_temp_celcius * 10.0f);
 	frsky_send_data(uart, FRSKY_ID_TEMP2, 0);
 
 	frsky_send_data(uart, FRSKY_ID_VOLTS, 0); /* cell voltage. 4 bits cell number, 12 bits voltage in 0.2V steps, scale 0-4.2V */
 	frsky_send_data(uart, FRSKY_ID_CURRENT, battery.current_a);
 
-	frsky_send_data(uart, FRSKY_ID_VOLTS_BP, battery.voltage_v);
-	frsky_send_data(uart, FRSKY_ID_VOLTS_AP, (battery.voltage_v - (int)battery.voltage_v) * 1000.0f);
+	float voltage = battery.voltage_v * 11.0f / 21.0f;
+	frsky_send_data(uart, FRSKY_ID_VOLTS_BP, voltage);
+	frsky_send_data(uart, FRSKY_ID_VOLTS_AP, frac(voltage) * 10.0f);
 
 	frsky_send_data(uart, FRSKY_ID_RPM, 0);
 
 	frsky_send_startstop(uart);
 }
 
+/**
+ * Formats the decimal latitude/longitude to the required degrees/minutes/seconds.
+ */
+float frsky_format_gps(float dec)
+{
+	float dms_deg = (int)dec;
+    float dec_deg = dec - dms_deg;
+    float dms_min = (int)(dec_deg * 60);
+    float dec_min = (dec_deg * 60) - dms_min;
+    float dms_sec = dec_min * 60;
+
+    return (dms_deg * 100.0f) + dms_min + (dms_sec / 100.0f);
+}
+
 /**
  * Sends frame 2 (every 1000ms):
  *   course, latitude, longitude, speed, altitude (GPS), fuel level
  */
 void frsky_send_frame2(int uart)
 {
- 	/* get a local copy of the battery data */
+ 	/* get a local copy of the global position data */
 	struct vehicle_global_position_s global_pos;
 	memset(&global_pos, 0, sizeof(global_pos));
 	orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos);
 
+ 	/* get a local copy of the vehicle status data */
+	struct vehicle_status_s vehicle_status;
+	memset(&vehicle_status, 0, sizeof(vehicle_status));
+	orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status);
+
 	/* send formatted frame */
-	// TODO
 	float course = 0, lat = 0, lon = 0, speed = 0, alt = 0, sec = 0;
+	char lat_ns = 0, lon_ew = 0;
 	if (global_pos.valid)
 	{
+		time_t time_gps = global_pos.time_gps_usec / 1000000;
+		struct tm *tm_gps = gmtime(&time_gps);
+
 		course = (global_pos.yaw + M_PI_F) / M_PI_F * 180.0f;
-		// TODO: latitude, longitude
-		speed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy);
-		alt = global_pos.alt;
+		lat    = frsky_format_gps(abs(global_pos.lat) / 10000000.0f);
+        lat_ns = (global_pos.lat < 0) ? 'S' : 'N';
+		lon    = frsky_format_gps(abs(global_pos.lon) / 10000000.0f);
+		lon_ew = (global_pos.lon < 0) ? 'W' : 'E';
+		speed  = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy) * 25.0f / 46.0f;
+		alt    = global_pos.alt;
+		sec    = tm_gps->tm_sec;
 	}
 
 	frsky_send_data(uart, FRSKY_ID_GPS_COURS_BP, course);
-	frsky_send_data(uart, FRSKY_ID_GPS_COURS_AP, (course - (int)course) * 100.0f);
+	frsky_send_data(uart, FRSKY_ID_GPS_COURS_AP, frac(course) * 1000.0f);
 
-	frsky_send_data(uart, FRSKY_ID_GPS_LAT_BP, 0);
-	frsky_send_data(uart, FRSKY_ID_GPS_LAT_AP, 0);
-	frsky_send_data(uart, FRSKY_ID_GPS_LAT_NS, 0);
+	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, 0);
-	frsky_send_data(uart, FRSKY_ID_GPS_LONG_AP, 0);
-	frsky_send_data(uart, FRSKY_ID_GPS_LONG_EW, 0);
+	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, (speed - (int)speed) * 100.0f);
+	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, (alt - (int)alt) * 100.0f);
+	frsky_send_data(uart, FRSKY_ID_GPS_ALT_AP, frac(alt) * 100.0f);
 
-	frsky_send_data(uart, FRSKY_ID_FUEL, 0);
+	frsky_send_data(uart, FRSKY_ID_FUEL, vehicle_status.battery_remaining);
 
-	frsky_send_data(uart, FRSKY_ID_GPS_SEC, 0);
+	frsky_send_data(uart, FRSKY_ID_GPS_SEC, sec);
 
 	frsky_send_startstop(uart);
 }
@@ -213,11 +284,13 @@ void frsky_send_frame3(int uart)
 	orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos);
 
 	/* send formatted frame */
-	// TODO
-	frsky_send_data(uart, FRSKY_ID_GPS_DAY_MONTH, 0);
-	frsky_send_data(uart, FRSKY_ID_GPS_YEAR, 0);
-	frsky_send_data(uart, FRSKY_ID_GPS_HOUR_MIN, 0);
-	frsky_send_data(uart, FRSKY_ID_GPS_SEC, 0);
+	time_t time_gps = global_pos.time_gps_usec / 1000000;
+	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);
 }
diff --git a/src/drivers/frsky_telemetry/frsky_data.h b/src/drivers/frsky_telemetry/frsky_data.h
index e0c39a42b..cea93ef2b 100644
--- a/src/drivers/frsky_telemetry/frsky_data.h
+++ b/src/drivers/frsky_telemetry/frsky_data.h
@@ -42,39 +42,6 @@
 #ifndef _FRSKY_DATA_H
 #define _FRSKY_DATA_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
-
 // Public functions
 void frsky_init(void);
 void frsky_send_frame1(int uart);
-- 
cgit v1.2.3


From 5f44be31ad77618d0a7514d129f41666a956a52d Mon Sep 17 00:00:00 2001
From: Stefan Rado <px4@sradonia.net>
Date: Thu, 2 Jan 2014 02:07:49 +0100
Subject: Update copyright info for 2014. Happy New Year everyone!

---
 src/drivers/frsky_telemetry/frsky_data.c      | 4 ++--
 src/drivers/frsky_telemetry/frsky_data.h      | 4 ++--
 src/drivers/frsky_telemetry/frsky_telemetry.c | 4 ++--
 src/drivers/frsky_telemetry/module.mk         | 2 +-
 4 files changed, 7 insertions(+), 7 deletions(-)

diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c
index 3a0c4b4b3..9a7b6beef 100644
--- a/src/drivers/frsky_telemetry/frsky_data.c
+++ b/src/drivers/frsky_telemetry/frsky_data.c
@@ -1,7 +1,7 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
- *   Author: Simon Wilks <sjwilks@gmail.com>
+ *   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
diff --git a/src/drivers/frsky_telemetry/frsky_data.h b/src/drivers/frsky_telemetry/frsky_data.h
index cea93ef2b..a7d9eee53 100644
--- a/src/drivers/frsky_telemetry/frsky_data.h
+++ b/src/drivers/frsky_telemetry/frsky_data.h
@@ -1,7 +1,7 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
- *   Author: Simon Wilks <sjwilks@gmail.com>
+ *   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
diff --git a/src/drivers/frsky_telemetry/frsky_telemetry.c b/src/drivers/frsky_telemetry/frsky_telemetry.c
index 5f3837b6e..c4e29fe51 100644
--- a/src/drivers/frsky_telemetry/frsky_telemetry.c
+++ b/src/drivers/frsky_telemetry/frsky_telemetry.c
@@ -1,7 +1,7 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
- *   Author: Simon Wilks <sjwilks@gmail.com>
+ *   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
diff --git a/src/drivers/frsky_telemetry/module.mk b/src/drivers/frsky_telemetry/module.mk
index 808966691..1632c74f7 100644
--- a/src/drivers/frsky_telemetry/module.mk
+++ b/src/drivers/frsky_telemetry/module.mk
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2013-2014 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
-- 
cgit v1.2.3


From 8fd909f51911b9ede93b19188ed360231f75de1c Mon Sep 17 00:00:00 2001
From: Stefan Rado <px4@sradonia.net>
Date: Thu, 2 Jan 2014 02:08:44 +0100
Subject: Directly write to the voltage field for better precision.

---
 src/drivers/frsky_telemetry/frsky_data.c | 4 +---
 1 file changed, 1 insertion(+), 3 deletions(-)

diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c
index 9a7b6beef..edbdbf366 100644
--- a/src/drivers/frsky_telemetry/frsky_data.c
+++ b/src/drivers/frsky_telemetry/frsky_data.c
@@ -191,9 +191,7 @@ void frsky_send_frame1(int uart)
 	frsky_send_data(uart, FRSKY_ID_VOLTS, 0); /* cell voltage. 4 bits cell number, 12 bits voltage in 0.2V steps, scale 0-4.2V */
 	frsky_send_data(uart, FRSKY_ID_CURRENT, battery.current_a);
 
-	float voltage = battery.voltage_v * 11.0f / 21.0f;
-	frsky_send_data(uart, FRSKY_ID_VOLTS_BP, voltage);
-	frsky_send_data(uart, FRSKY_ID_VOLTS_AP, frac(voltage) * 10.0f);
+	frsky_send_data(uart, FRSKY_ID_VFAS, battery.voltage_v * 10.0f);
 
 	frsky_send_data(uart, FRSKY_ID_RPM, 0);
 
-- 
cgit v1.2.3


From 1e7e65717a4522de59957d20be2b06ccc7b5a71d Mon Sep 17 00:00:00 2001
From: Stefan Rado <px4@sradonia.net>
Date: Thu, 2 Jan 2014 02:11:52 +0100
Subject: Only send data packets we really support.

---
 src/drivers/frsky_telemetry/frsky_data.c | 7 +------
 1 file changed, 1 insertion(+), 6 deletions(-)

diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c
index edbdbf366..9e6d57fac 100644
--- a/src/drivers/frsky_telemetry/frsky_data.c
+++ b/src/drivers/frsky_telemetry/frsky_data.c
@@ -186,14 +186,9 @@ void frsky_send_frame1(int uart)
 	frsky_send_data(uart, FRSKY_ID_BARO_ALT_AP, frac(raw.baro_alt_meter) * 1000.0f);
 
 	frsky_send_data(uart, FRSKY_ID_TEMP1, raw.baro_temp_celcius * 10.0f);
-	frsky_send_data(uart, FRSKY_ID_TEMP2, 0);
-
-	frsky_send_data(uart, FRSKY_ID_VOLTS, 0); /* cell voltage. 4 bits cell number, 12 bits voltage in 0.2V steps, scale 0-4.2V */
-	frsky_send_data(uart, FRSKY_ID_CURRENT, battery.current_a);
 
 	frsky_send_data(uart, FRSKY_ID_VFAS, battery.voltage_v * 10.0f);
-
-	frsky_send_data(uart, FRSKY_ID_RPM, 0);
+	frsky_send_data(uart, FRSKY_ID_CURRENT, battery.current_a);
 
 	frsky_send_startstop(uart);
 }
-- 
cgit v1.2.3


From 4508972121196d8892520e56e6b374a59281e50a Mon Sep 17 00:00:00 2001
From: Stefan Rado <px4@sradonia.net>
Date: Thu, 2 Jan 2014 22:34:08 +0100
Subject: Further data format and code style fixes.

---
 src/drivers/frsky_telemetry/frsky_data.c      | 96 +++++++++++++--------------
 src/drivers/frsky_telemetry/frsky_telemetry.c | 19 +++---
 2 files changed, 56 insertions(+), 59 deletions(-)

diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c
index 9e6d57fac..63b2d2d29 100644
--- a/src/drivers/frsky_telemetry/frsky_data.c
+++ b/src/drivers/frsky_telemetry/frsky_data.c
@@ -53,8 +53,7 @@
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_status.h>
 
-
-// FrSky sensor hub data IDs
+/* FrSky sensor hub data IDs */
 #define FRSKY_ID_GPS_ALT_BP     0x01
 #define FRSKY_ID_TEMP1          0x02
 #define FRSKY_ID_RPM            0x03
@@ -87,18 +86,13 @@
 #define FRSKY_ID_VOLTS_BP       0x3A
 #define FRSKY_ID_VOLTS_AP       0x3B
 
-
 #define frac(f) (f - (int)f)
 
-float frsky_format_gps(float dec);
-
-
 static int battery_sub = -1;
 static int sensor_sub = -1;
 static int global_position_sub = -1;
 static int vehicle_status_sub = -1;
 
-
 /**
  * Initializes the uORB subscriptions.
  */
@@ -113,8 +107,7 @@ void frsky_init()
 /**
  * Sends a 0x5E start/stop byte.
  */
-static void
-frsky_send_startstop(int uart)
+static void frsky_send_startstop(int uart)
 {
 	static const uint8_t c = 0x5E;
 	write(uart, &c, sizeof(c));
@@ -123,14 +116,12 @@ frsky_send_startstop(int uart)
 /**
  * Sends one byte, performing byte-stuffing if necessary.
  */
-static void
-frsky_send_byte(int uart, uint8_t value)
+static void frsky_send_byte(int uart, uint8_t value)
 {
-	const uint8_t x5E[] = {0x5D, 0x3E};
-	const uint8_t x5D[] = {0x5D, 0x3D};
+	const uint8_t x5E[] = { 0x5D, 0x3E };
+	const uint8_t x5D[] = { 0x5D, 0x3D };
 
-	switch (value)
-	{
+	switch (value) {
 	case 0x5E:
 		write(uart, x5E, sizeof(x5E));
 		break;
@@ -148,10 +139,9 @@ frsky_send_byte(int uart, uint8_t value)
 /**
  * Sends one data id/value pair.
  */
-static void
-frsky_send_data(int uart, uint8_t id, int16_t data)
+static void frsky_send_data(int uart, uint8_t id, int16_t data)
 {
-	// Cast data to unsigned, because signed shift might behave incorrectly
+	/* Cast data to unsigned, because signed shift might behave incorrectly */
 	uint16_t udata = data;
 
 	frsky_send_startstop(uart);
@@ -163,7 +153,7 @@ frsky_send_data(int uart, uint8_t id, int16_t data)
 
 /**
  * Sends frame 1 (every 200ms):
- *   acceleration values, altitude (vario), temperatures, current & voltages, RPM
+ *   acceleration values, barometer altitude, temperature, battery voltage & current
  */
 void frsky_send_frame1(int uart)
 {
@@ -178,17 +168,25 @@ void frsky_send_frame1(int uart)
 	orb_copy(ORB_ID(battery_status), battery_sub, &battery);
 
 	/* send formatted frame */
-	frsky_send_data(uart, FRSKY_ID_ACCEL_X, raw.accelerometer_m_s2[0] * 1000.0f);
-	frsky_send_data(uart, FRSKY_ID_ACCEL_Y, raw.accelerometer_m_s2[1] * 1000.0f);
-	frsky_send_data(uart, FRSKY_ID_ACCEL_Z, raw.accelerometer_m_s2[2] * 1000.0f);
-
-	frsky_send_data(uart, FRSKY_ID_BARO_ALT_BP, raw.baro_alt_meter);
-	frsky_send_data(uart, FRSKY_ID_BARO_ALT_AP, frac(raw.baro_alt_meter) * 1000.0f);
-
-	frsky_send_data(uart, FRSKY_ID_TEMP1, raw.baro_temp_celcius * 10.0f);
-
-	frsky_send_data(uart, FRSKY_ID_VFAS, battery.voltage_v * 10.0f);
-	frsky_send_data(uart, FRSKY_ID_CURRENT, battery.current_a);
+	frsky_send_data(uart, FRSKY_ID_ACCEL_X,
+			roundf(raw.accelerometer_m_s2[0] * 1000.0f));
+	frsky_send_data(uart, FRSKY_ID_ACCEL_Y,
+			roundf(raw.accelerometer_m_s2[1] * 1000.0f));
+	frsky_send_data(uart, FRSKY_ID_ACCEL_Z,
+			roundf(raw.accelerometer_m_s2[2] * 1000.0f));
+
+	frsky_send_data(uart, FRSKY_ID_BARO_ALT_BP,
+			raw.baro_alt_meter);
+	frsky_send_data(uart, FRSKY_ID_BARO_ALT_AP,
+			roundf(frac(raw.baro_alt_meter) * 100.0f));
+
+	frsky_send_data(uart, FRSKY_ID_TEMP1,
+			roundf(raw.baro_temp_celcius));
+
+	frsky_send_data(uart, FRSKY_ID_VFAS,
+			roundf(battery.voltage_v * 10.0f));
+	frsky_send_data(uart, FRSKY_ID_CURRENT,
+			(battery.current_a < 0) ? 0 : roundf(battery.current_a * 10.0f));
 
 	frsky_send_startstop(uart);
 }
@@ -196,47 +194,48 @@ void frsky_send_frame1(int uart)
 /**
  * Formats the decimal latitude/longitude to the required degrees/minutes/seconds.
  */
-float frsky_format_gps(float dec)
+static float frsky_format_gps(float dec)
 {
-	float dms_deg = (int)dec;
-    float dec_deg = dec - dms_deg;
-    float dms_min = (int)(dec_deg * 60);
-    float dec_min = (dec_deg * 60) - dms_min;
-    float dms_sec = dec_min * 60;
+	float dms_deg = (int) dec;
+	float dec_deg = dec - dms_deg;
+	float dms_min = (int) (dec_deg * 60);
+	float dec_min = (dec_deg * 60) - dms_min;
+	float dms_sec = dec_min * 60;
 
-    return (dms_deg * 100.0f) + dms_min + (dms_sec / 100.0f);
+	return (dms_deg * 100.0f) + dms_min + (dms_sec / 100.0f);
 }
 
 /**
  * Sends frame 2 (every 1000ms):
- *   course, latitude, longitude, speed, altitude (GPS), fuel level
+ *   GPS course, latitude, longitude, ground speed, GPS altitude, remaining battery level
  */
 void frsky_send_frame2(int uart)
 {
- 	/* get a local copy of the global position data */
+	/* get a local copy of the global position data */
 	struct vehicle_global_position_s global_pos;
 	memset(&global_pos, 0, sizeof(global_pos));
 	orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos);
 
- 	/* get a local copy of the vehicle status data */
+	/* get a local copy of the vehicle status data */
 	struct vehicle_status_s vehicle_status;
 	memset(&vehicle_status, 0, sizeof(vehicle_status));
 	orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status);
 
 	/* send formatted frame */
-	float course = 0, lat = 0, lon = 0, speed = 0, alt = 0, sec = 0;
+	float course = 0, lat = 0, lon = 0, speed = 0, alt = 0;
 	char lat_ns = 0, lon_ew = 0;
-	if (global_pos.valid)
-	{
+	int sec = 0;
+	if (global_pos.valid) {
 		time_t time_gps = global_pos.time_gps_usec / 1000000;
 		struct tm *tm_gps = gmtime(&time_gps);
 
 		course = (global_pos.yaw + M_PI_F) / M_PI_F * 180.0f;
 		lat    = frsky_format_gps(abs(global_pos.lat) / 10000000.0f);
-        lat_ns = (global_pos.lat < 0) ? 'S' : 'N';
+		lat_ns = (global_pos.lat < 0) ? 'S' : 'N';
 		lon    = frsky_format_gps(abs(global_pos.lon) / 10000000.0f);
 		lon_ew = (global_pos.lon < 0) ? 'W' : 'E';
-		speed  = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy) * 25.0f / 46.0f;
+		speed  = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy)
+				* 25.0f / 46.0f;
 		alt    = global_pos.alt;
 		sec    = tm_gps->tm_sec;
 	}
@@ -258,7 +257,8 @@ void frsky_send_frame2(int uart)
 	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, vehicle_status.battery_remaining);
+	frsky_send_data(uart, FRSKY_ID_FUEL,
+			roundf(vehicle_status.battery_remaining * 100.0f));
 
 	frsky_send_data(uart, FRSKY_ID_GPS_SEC, sec);
 
@@ -267,11 +267,11 @@ void frsky_send_frame2(int uart)
 
 /**
  * Sends frame 3 (every 5000ms):
- *   date, time
+ *   GPS date & time
  */
 void frsky_send_frame3(int uart)
 {
- 	/* get a local copy of the battery data */
+	/* get a local copy of the battery data */
 	struct vehicle_global_position_s global_pos;
 	memset(&global_pos, 0, sizeof(global_pos));
 	orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos);
diff --git a/src/drivers/frsky_telemetry/frsky_telemetry.c b/src/drivers/frsky_telemetry/frsky_telemetry.c
index c4e29fe51..7b08ca69e 100644
--- a/src/drivers/frsky_telemetry/frsky_telemetry.c
+++ b/src/drivers/frsky_telemetry/frsky_telemetry.c
@@ -72,8 +72,7 @@ __EXPORT int frsky_telemetry_main(int argc, char *argv[]);
 /**
  * Opens the UART device and sets all required serial parameters.
  */
-static int
-frsky_open_uart(const char *uart_name, struct termios *uart_config_original)
+static int frsky_open_uart(const char *uart_name, struct termios *uart_config_original)
 {
 	/* Open UART */
 	const int uart = open(uart_name, O_WRONLY | O_NOCTTY);
@@ -118,20 +117,19 @@ frsky_open_uart(const char *uart_name, struct termios *uart_config_original)
 /**
  * Print command usage information
  */
-static void
-usage()
+static void usage()
 {
-	fprintf(stderr, "usage: frsky_telemetry start [-d <devicename>]\n"
-		"       frsky_telemetry stop\n"
-		"       frsky_telemetry status\n");
+	fprintf(stderr,
+			"usage: frsky_telemetry start [-d <devicename>]\n"
+			"       frsky_telemetry stop\n"
+			"       frsky_telemetry status\n");
 	exit(1);
 }
 
 /**
  * The daemon thread.
  */
-static int
-frsky_telemetry_thread_main(int argc, char *argv[])
+static int frsky_telemetry_thread_main(int argc, char *argv[])
 {
 	/* Default values for arguments */
 	char *device_name = "/dev/ttyS1"; /* USART2 */
@@ -207,8 +205,7 @@ frsky_telemetry_thread_main(int argc, char *argv[])
  * The main command function.
  * Processes command line arguments and starts the daemon.
  */
-int
-frsky_telemetry_main(int argc, char *argv[])
+int frsky_telemetry_main(int argc, char *argv[])
 {
 	if (argc < 2) {
 		warnx("missing command");
-- 
cgit v1.2.3


From 76477b205775ddcbce6147d93985ddcdb10a3d52 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Mon, 6 Jan 2014 09:25:39 +0100
Subject: Added support for Octo Cox

---
 ROMFS/px4fmu_common/init.d/rcS | 11 ++++++++++-
 1 file changed, 10 insertions(+), 1 deletion(-)

diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index f122921c5..66cb3f237 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -213,7 +213,9 @@ then
 	#  7000	..	 7999		Hexa +
 	#  8000	..	 8999		Octo X
 	#  9000	..	 9999		Octo +
-	# 10000	..	19999		Wide arm / H frame
+	# 10000	..	10999		Wide arm / H frame
+	# 11000	..	11999		Hexa Cox
+	# 12000	..	12999		Octo Cox
 	
 	if param compare SYS_AUTOSTART 4008 8
 	then
@@ -277,6 +279,13 @@ then
 		sh /etc/init.d/rc.octo
 		set MODE custom
 	fi
+
+	if param compare SYS_AUTOSTART 12001
+	then
+		set MIXER /etc/mixers/FMU_octo_cox.mix
+		sh /etc/init.d/rc.octo
+		set MODE custom
+	fi
 	
 	if param compare SYS_AUTOSTART 10015 15
 	then
-- 
cgit v1.2.3


From 1a13e66aab3cd88b5447448b577fc44165ab01bc Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Tue, 7 Jan 2014 15:59:18 +0800
Subject: px4iofirmware: make forceupdate more reliable

this schedules a reboot rather than rebooting immediately, which means
the FMU gets an ACK for its reboot operation, preventing it from
timing out waiting for the ACK. That makes the timing of the reboot
more consistent, which makes it more reliable for forceupdate
---
 src/modules/px4iofirmware/px4io.c     | 21 +++++++++++++++++++++
 src/modules/px4iofirmware/px4io.h     |  4 ++++
 src/modules/px4iofirmware/registers.c | 15 +++++----------
 3 files changed, 30 insertions(+), 10 deletions(-)

diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c
index 745bd5705..0b8c4a6a8 100644
--- a/src/modules/px4iofirmware/px4io.c
+++ b/src/modules/px4iofirmware/px4io.c
@@ -125,6 +125,25 @@ heartbeat_blink(void)
 	LED_BLUE(heartbeat = !heartbeat);
 }
 
+static uint64_t reboot_time;
+
+/**
+   schedule a reboot in time_delta_usec microseconds
+ */
+void schedule_reboot(uint32_t time_delta_usec)
+{
+    reboot_time = hrt_absolute_time() + time_delta_usec;
+}
+
+/**
+   check for a scheduled reboot
+ */
+static void check_reboot(void)
+{
+    if (reboot_time != 0 && hrt_absolute_time() > reboot_time) {
+        up_systemreset();
+    }
+}
 
 static void
 calculate_fw_crc(void)
@@ -249,6 +268,8 @@ user_start(int argc, char *argv[])
                     heartbeat_blink();
                 }
 
+                check_reboot();
+
 #if 0
 		/* check for debug activity */
 		show_debug_messages();
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index dea04a663..a0daa97ea 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -220,3 +220,7 @@ extern volatile uint8_t debug_level;
 
 /** send a debug message to the console */
 extern void	isr_debug(uint8_t level, const char *fmt, ...);
+
+/** schedule a reboot */
+extern void schedule_reboot(uint32_t time_delta_usec);
+
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 0358725db..ad4473073 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -518,16 +518,11 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			// check the magic value
 			if (value != PX4IO_REBOOT_BL_MAGIC)
 				break;
-                        
-                        // note that we don't set BL_WAIT_MAGIC in
-                        // BKP_DR1 as that is not necessary given the
-                        // timing of the forceupdate command. The
-                        // bootloader on px4io waits for enough time
-                        // anyway, and this method works with older
-                        // bootloader versions (tested with both
-                        // revision 3 and revision 4).
-
-			up_systemreset();
+
+                        // we schedule a reboot rather than rebooting
+                        // immediately to allow the IO board to ACK
+                        // the reboot command
+                        schedule_reboot(100000);
 			break;
 
 		case PX4IO_P_SETUP_DSM:
-- 
cgit v1.2.3


From c4fc730acad12b74f51d9ba7d3ff267e3e1a1ab3 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Tue, 7 Jan 2014 11:13:51 +0800
Subject: FMUv2: make all UARTs use 512 byte buffer, 2048 for CDCACM output

this is important when using UARTs for things like secondary GPS
modules, which may produce large enough transfers that 128 bytes is
not enough.

The 2048 buffer for CDCACM transmit makes mavlink log and parameter
transfer faster
---
 nuttx-configs/px4fmu-v2/nsh/defconfig | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig
index 110bcb363..e20411bca 100644
--- a/nuttx-configs/px4fmu-v2/nsh/defconfig
+++ b/nuttx-configs/px4fmu-v2/nsh/defconfig
@@ -582,8 +582,8 @@ CONFIG_USART3_OFLOWCONTROL=y
 #
 # UART4 Configuration
 #
-CONFIG_UART4_RXBUFSIZE=128
-CONFIG_UART4_TXBUFSIZE=128
+CONFIG_UART4_RXBUFSIZE=512
+CONFIG_UART4_TXBUFSIZE=512
 CONFIG_UART4_BAUD=57600
 CONFIG_UART4_BITS=8
 CONFIG_UART4_PARITY=0
@@ -594,8 +594,8 @@ CONFIG_UART4_2STOP=0
 #
 # USART6 Configuration
 #
-CONFIG_USART6_RXBUFSIZE=256
-CONFIG_USART6_TXBUFSIZE=256
+CONFIG_USART6_RXBUFSIZE=512
+CONFIG_USART6_TXBUFSIZE=512
 CONFIG_USART6_BAUD=57600
 CONFIG_USART6_BITS=8
 CONFIG_USART6_PARITY=0
@@ -662,7 +662,7 @@ CONFIG_CDCACM_NWRREQS=4
 CONFIG_CDCACM_NRDREQS=4
 CONFIG_CDCACM_BULKIN_REQLEN=96
 CONFIG_CDCACM_RXBUFSIZE=512
-CONFIG_CDCACM_TXBUFSIZE=512
+CONFIG_CDCACM_TXBUFSIZE=2048
 CONFIG_CDCACM_VENDORID=0x26ac
 CONFIG_CDCACM_PRODUCTID=0x0011
 CONFIG_CDCACM_VENDORSTR="3D Robotics"
-- 
cgit v1.2.3


From d4d2571161530848f2a4ac153f2529ab50ec4fcc Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Tue, 7 Jan 2014 11:25:09 +0800
Subject: FMUv2: enable RXDMA on 2nd GPS port and debug console

This should reduce the chance of lost data on these ports due to high
interrupt latency
---
 nuttx-configs/px4fmu-v2/nsh/defconfig | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig
index e20411bca..8a282693f 100644
--- a/nuttx-configs/px4fmu-v2/nsh/defconfig
+++ b/nuttx-configs/px4fmu-v2/nsh/defconfig
@@ -295,7 +295,7 @@ CONFIG_STM32_USART=y
 # U[S]ART Configuration
 #
 # CONFIG_USART1_RS485 is not set
-# CONFIG_USART1_RXDMA is not set
+CONFIG_USART1_RXDMA=y
 # CONFIG_USART2_RS485 is not set
 CONFIG_USART2_RXDMA=y
 # CONFIG_USART3_RS485 is not set
@@ -304,7 +304,7 @@ CONFIG_USART3_RXDMA=y
 CONFIG_UART4_RXDMA=y
 # CONFIG_UART5_RXDMA is not set
 # CONFIG_USART6_RS485 is not set
-# CONFIG_USART6_RXDMA is not set
+CONFIG_USART6_RXDMA=y
 # CONFIG_UART7_RS485 is not set
 # CONFIG_UART7_RXDMA is not set
 # CONFIG_UART8_RS485 is not set
-- 
cgit v1.2.3


From 1f564a95ee89278b3e4eea6c2d4ded378b71542c Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Tue, 7 Jan 2014 11:37:40 +0800
Subject: meas_airspeed: avoid trivial dependency on math lib

including the math lib adds a huge amount to flash usage
---
 src/drivers/meas_airspeed/meas_airspeed.cpp | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp
index 3cd6d6720..a95c4576b 100644
--- a/src/drivers/meas_airspeed/meas_airspeed.cpp
+++ b/src/drivers/meas_airspeed/meas_airspeed.cpp
@@ -198,7 +198,9 @@ MEASAirspeed::collect()
 //	uint16_t diff_press_pa = abs(dp_raw - (16384 / 2.0f));
 	const float P_min = -1.0f;
 	const float P_max = 1.0f;
-	float diff_press_pa = math::max(0.0f, fabsf( ( ((float)dp_raw - 0.1f*16383.0f) * (P_max-P_min)/(0.8f*16383.0f) + P_min) * 6894.8f)   - _diff_pres_offset);
+	float diff_press_pa = fabsf( ( ((float)dp_raw - 0.1f*16383.0f) * (P_max-P_min)/(0.8f*16383.0f) + P_min) * 6894.8f) - _diff_pres_offset;
+        if (diff_press_pa < 0.0f)
+            diff_press_pa = 0.0f;
 
 	struct differential_pressure_s report;
 
-- 
cgit v1.2.3


From 3be1a5182db7bd3802b77e7c03fc14f00ca218c3 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Tue, 31 Dec 2013 22:39:51 +1100
Subject: FMUv1: use larger CDCACM buffer size for faster log transfer on FMUv1

---
 nuttx-configs/px4fmu-v1/nsh/defconfig | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig
index e43b9c18e..e60120b49 100644
--- a/nuttx-configs/px4fmu-v1/nsh/defconfig
+++ b/nuttx-configs/px4fmu-v1/nsh/defconfig
@@ -566,7 +566,7 @@ CONFIG_CDCACM_NWRREQS=4
 CONFIG_CDCACM_NRDREQS=4
 CONFIG_CDCACM_BULKIN_REQLEN=96
 CONFIG_CDCACM_RXBUFSIZE=512
-CONFIG_CDCACM_TXBUFSIZE=512
+CONFIG_CDCACM_TXBUFSIZE=2048
 CONFIG_CDCACM_VENDORID=0x26ac
 CONFIG_CDCACM_PRODUCTID=0x0010
 CONFIG_CDCACM_VENDORSTR="3D Robotics"
-- 
cgit v1.2.3


From d6088efd34e5482d302e3a253fdd3a170d88490a Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Tue, 31 Dec 2013 10:32:15 +1100
Subject: ms5611: report P and T in ms5611 info

---
 src/drivers/ms5611/ms5611.cpp | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp
index 87788824a..6326cf7fc 100644
--- a/src/drivers/ms5611/ms5611.cpp
+++ b/src/drivers/ms5611/ms5611.cpp
@@ -124,6 +124,8 @@ protected:
 	int32_t			_TEMP;
 	int64_t			_OFF;
 	int64_t			_SENS;
+	float			_P;
+	float			_T;
 
 	/* altitude conversion calibration */
 	unsigned		_msl_pressure;	/* in kPa */
@@ -623,6 +625,8 @@ MS5611::collect()
 
 		/* pressure calculation, result in Pa */
 		int32_t P = (((raw * _SENS) >> 21) - _OFF) >> 15;
+		_P = P * 0.01f;
+		_T = _TEMP * 0.01f;
 
 		/* generate a new report */
 		report.temperature = _TEMP / 100.0f;
@@ -695,6 +699,8 @@ MS5611::print_info()
 	printf("TEMP:           %d\n", _TEMP);
 	printf("SENS:           %lld\n", _SENS);
 	printf("OFF:            %lld\n", _OFF);
+	printf("P:              %.3f\n", _P);
+	printf("T:              %.3f\n", _T);
 	printf("MSL pressure:   %10.4f\n", (double)(_msl_pressure / 100.f));
 
 	printf("factory_setup             %u\n", _prom.factory_setup);
-- 
cgit v1.2.3


From 94b539dfddc5a2e293f51058ee5bf0d6ffc78406 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Wed, 18 Sep 2013 10:30:09 +1000
Subject: px4io: enable power on Spektrum connector on init

---
 src/modules/px4iofirmware/dsm.c | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c
index 4d306d6d0..60eda2319 100644
--- a/src/modules/px4iofirmware/dsm.c
+++ b/src/modules/px4iofirmware/dsm.c
@@ -203,6 +203,12 @@ dsm_guess_format(bool reset)
 int
 dsm_init(const char *device)
 {
+
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
+	// enable power on DSM connector
+	POWER_SPEKTRUM(true);
+#endif
+
 	if (dsm_fd < 0)
 		dsm_fd = open(device, O_RDONLY | O_NONBLOCK);
 
-- 
cgit v1.2.3