From 570858d87c016841610ef71bfe2a9184c1f319b9 Mon Sep 17 00:00:00 2001
From: Simon Wilks <sjwilks@gmail.com>
Date: Tue, 23 Apr 2013 22:41:44 +0200
Subject: [PATCH 01/17] General cleanup based on the pull request from
 muggenhor

---
 apps/hott_telemetry/hott_telemetry_main.c | 117 +++++++++-------------
 apps/hott_telemetry/messages.c            |  17 ++--
 apps/hott_telemetry/messages.h            |  20 ++--
 3 files changed, 68 insertions(+), 86 deletions(-)

diff --git a/apps/hott_telemetry/hott_telemetry_main.c b/apps/hott_telemetry/hott_telemetry_main.c
index 31c9247aa5..3e147d8292 100644
--- a/apps/hott_telemetry/hott_telemetry_main.c
+++ b/apps/hott_telemetry/hott_telemetry_main.c
@@ -53,6 +53,7 @@
 #include <termios.h>
 #include <sys/ioctl.h>
 #include <unistd.h>
+#include <systemlib/err.h>
 #include <systemlib/systemlib.h>
 
 #include "messages.h"
@@ -66,56 +67,44 @@
 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 char *daemon_name = "hott_telemetry";
-static char *commandline_usage = "usage: hott_telemetry start|status|stop [-d <device>]";
+static const char daemon_name[] = "hott_telemetry";
+static const char commandline_usage[] = "usage: hott_telemetry start|status|stop [-d <device>]";
 
-
-/* A little console messaging experiment - console helper macro */
-#define FATAL_MSG(_msg)		fprintf(stderr, "[%s] %s\n", daemon_name, _msg); exit(1);
-#define ERROR_MSG(_msg)		fprintf(stderr, "[%s] %s\n", daemon_name, _msg);
-#define INFO_MSG(_msg)		printf("[%s] %s\n", daemon_name, _msg);
 /**
  * Deamon management function.
  */
 __EXPORT int hott_telemetry_main(int argc, char *argv[]);
 
 /**
- * Mainloop of deamon.
+ * Mainloop of daemon.
  */
 int hott_telemetry_thread_main(int argc, char *argv[]);
 
-static int read_data(int uart, int *id);
-static int send_data(int uart, uint8_t *buffer, int size);
+static int recv_req_id(int uart, uint8_t *id);
+static int send_data(int uart, uint8_t *buffer, size_t size);
 
-static int open_uart(const char *device, struct termios *uart_config_original)
+static int
+open_uart(const char *device, struct termios *uart_config_original)
 {
 	/* baud rate */
-	int speed = B19200;
-	int uart;
+	static const speed_t speed = B19200;
 
 	/* open uart */
-	uart = open(device, O_RDWR | O_NOCTTY);
+	const int uart = open(device, O_RDWR | O_NOCTTY);
 
 	if (uart < 0) {
-		char msg[80];
-		sprintf(msg, "Error opening port: %s\n", device);
-		FATAL_MSG(msg);
+		err(1, "Error opening port: %s", device);
 	}
-
-	/* Try to set baud rate */
-	struct termios uart_config;
+	
+	/* Back up the original uart configuration to restore it after exit */	
 	int termios_state;
-
-	/* Back up the original uart configuration to restore it after exit */
-	char msg[80];
-
 	if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
-		sprintf(msg, "Error getting baudrate / termios config for %s: %d\n", device, termios_state);
 		close(uart);
-		FATAL_MSG(msg);
+		err(1, "Error getting baudrate / termios config for %s: %d", device, termios_state);
 	}
 
 	/* 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) */
@@ -123,16 +112,14 @@ static int open_uart(const char *device, struct termios *uart_config_original)
 
 	/* Set baud rate */
 	if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
-		sprintf(msg, "Error setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n",
-			device, termios_state);
 		close(uart);
-		FATAL_MSG(msg);
+		err(1, "Error setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)",
+			 device, termios_state);
 	}
 
 	if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
-		sprintf(msg, "Error setting baudrate / termios config for %s (tcsetattr)\n", device);
 		close(uart);
-		FATAL_MSG(msg);
+		err(1, "Error setting baudrate / termios config for %s (tcsetattr)", device);
 	}
 
 	/* Activate single wire mode */
@@ -141,18 +128,19 @@ static int open_uart(const char *device, struct termios *uart_config_original)
 	return uart;
 }
 
-int read_data(int uart, int *id)
+int
+recv_req_id(int uart, uint8_t *id)
 {
-	const int timeout = 1000;
+	static const int timeout_ms = 1000;  // TODO make it a define
 	struct pollfd fds[] = { { .fd = uart, .events = POLLIN } };
 
-	char mode;
+	uint8_t mode;
 
-	if (poll(fds, 1, timeout) > 0) {
+	if (poll(fds, 1, timeout_ms) > 0) {
 		/* Get the mode: binary or text  */
-		read(uart, &mode, 1);
+		read(uart, &mode, sizeof(mode));
 		/* Read the device ID being polled */
-		read(uart, id, 1);
+		read(uart, id, sizeof(*id));
 
 		/* if we have a binary mode request */
 		if (mode != BINARY_MODE_REQUEST_ID) {
@@ -160,20 +148,21 @@ int read_data(int uart, int *id)
 		}
 
 	} else {
-		ERROR_MSG("UART timeout on TX/RX port");
+		warnx("UART timeout on TX/RX port");
 		return ERROR;
 	}
 
 	return OK;
 }
 
-int send_data(int uart, uint8_t *buffer, int size)
+int
+send_data(int uart, uint8_t *buffer, size_t size)
 {
 	usleep(POST_READ_DELAY_IN_USECS);
 
 	uint16_t checksum = 0;
 
-	for (int i = 0; i < size; i++) {
+	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;
@@ -182,7 +171,7 @@ int send_data(int uart, uint8_t *buffer, int size)
 			checksum += buffer[i];
 		}
 
-		write(uart, &buffer[i], 1);
+		write(uart, &buffer[i], sizeof(buffer[i]));
 
 		/* Sleep before sending the next byte. */
 		usleep(POST_WRITE_DELAY_IN_USECS);
@@ -196,13 +185,14 @@ int send_data(int uart, uint8_t *buffer, int size)
 	return OK;
 }
 
-int hott_telemetry_thread_main(int argc, char *argv[])
+int
+hott_telemetry_thread_main(int argc, char *argv[])
 {
-	INFO_MSG("starting");
+	warnx("starting");
 
 	thread_running = true;
 
-	char *device = "/dev/ttyS1";		/**< Default telemetry port: USART2 */
+	const char *device = "/dev/ttyS1";		/**< Default telemetry port: USART2 */
 
 	/* read commandline arguments */
 	for (int i = 0; i < argc && argv[i]; i++) {
@@ -212,31 +202,28 @@ int hott_telemetry_thread_main(int argc, char *argv[])
 
 			} else {
 				thread_running = false;
-				ERROR_MSG("missing parameter to -d");
-				ERROR_MSG(commandline_usage);
-				exit(1);
+				errx(1, "missing parameter to -d\n%s", commandline_usage);
 			}
 		}
 	}
 
 	/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
 	struct termios uart_config_original;
-	int uart = open_uart(device, &uart_config_original);
+	const int uart = open_uart(device, &uart_config_original);
 
 	if (uart < 0) {
-		ERROR_MSG("Failed opening HoTT UART, exiting.");
+		errx(1, "Failed opening HoTT UART, exiting.");
 		thread_running = false;
-		exit(ERROR);
 	}
 
 	messages_init();
 
 	uint8_t buffer[MESSAGE_BUFFER_SIZE];
-	int size = 0;
-	int id = 0;
+	size_t size = 0;
+	uint8_t id = 0;
 
 	while (!thread_should_exit) {
-		if (read_data(uart, &id) == OK) {
+		if (recv_req_id(uart, &id) == OK) {
 			switch (id) {
 			case ELECTRIC_AIR_MODULE:
 				build_eam_response(buffer, &size);
@@ -250,7 +237,7 @@ int hott_telemetry_thread_main(int argc, char *argv[])
 		}
 	}
 
-	INFO_MSG("exiting");
+	warnx("exiting");
 
 	close(uart);
 
@@ -262,23 +249,22 @@ int hott_telemetry_thread_main(int argc, char *argv[])
 /**
  * Process command line arguments and tart the daemon.
  */
-int hott_telemetry_main(int argc, char *argv[])
+int
+hott_telemetry_main(int argc, char *argv[])
 {
 	if (argc < 1) {
-		ERROR_MSG("missing command");
-		ERROR_MSG(commandline_usage);
-		exit(1);
+		errx(1, "missing command\n%s", commandline_usage);
 	}
 
 	if (!strcmp(argv[1], "start")) {
 
 		if (thread_running) {
-			INFO_MSG("deamon already running");
+			warnx("deamon already running");
 			exit(0);
 		}
 
 		thread_should_exit = false;
-		deamon_task = task_spawn("hott_telemetry",
+		deamon_task = task_spawn(daemon_name,
 					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_MAX - 40,
 					 2048,
@@ -294,19 +280,14 @@ int hott_telemetry_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "status")) {
 		if (thread_running) {
-			INFO_MSG("daemon is running");
+			warnx("daemon is running");
 
 		} else {
-			INFO_MSG("daemon not started");
+			warnx("daemon not started");
 		}
 
 		exit(0);
 	}
 
-	ERROR_MSG("unrecognized command");
-	ERROR_MSG(commandline_usage);
-	exit(1);
+	errx(1, "unrecognized command\n%s", commandline_usage);
 }
-
-
-
diff --git a/apps/hott_telemetry/messages.c b/apps/hott_telemetry/messages.c
index 8bfb997737..c1988cd054 100644
--- a/apps/hott_telemetry/messages.c
+++ b/apps/hott_telemetry/messages.c
@@ -48,27 +48,26 @@
 static int battery_sub = -1;
 static int sensor_sub = -1;
 
-void messages_init(void)
+void 
+messages_init(void)
 {
 	battery_sub = orb_subscribe(ORB_ID(battery_status));
 	sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
 }
 
-void build_eam_response(uint8_t *buffer, int *size)
+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));
+	struct sensor_combined_s raw = { 0 };
 	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));
+	struct battery_status_s battery = { 0 };
 	orb_copy(ORB_ID(battery_status), battery_sub, &battery);
 
-	struct eam_module_msg msg;
+	struct eam_module_msg msg = { 0 };
 	*size = sizeof(msg);
-	memset(&msg, 0, *size);
 
 	msg.start = START_BYTE;
 	msg.eam_sensor_id = ELECTRIC_AIR_MODULE;
@@ -84,4 +83,4 @@ void build_eam_response(uint8_t *buffer, int *size)
 	msg.stop = STOP_BYTE;
 
 	memcpy(buffer, &msg, *size);
-}
\ No newline at end of file
+}
diff --git a/apps/hott_telemetry/messages.h b/apps/hott_telemetry/messages.h
index 44001e04f0..9ca205f818 100644
--- a/apps/hott_telemetry/messages.h
+++ b/apps/hott_telemetry/messages.h
@@ -43,11 +43,6 @@
 
 #include <stdlib.h>
 
-/* The buffer size used to store messages. This must be at least as big as the number of
- * fields in the largest message struct.
- */
-#define MESSAGE_BUFFER_SIZE 50
-
 /* 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 constucting
  * the message after the read which takes some milliseconds.
@@ -71,12 +66,12 @@
 /* The Electric Air Module message. */
 struct eam_module_msg {
 	uint8_t start;			/**< Start byte   				*/
-	uint8_t eam_sensor_id;		/**< EAM sensor ID 				*/
+	uint8_t eam_sensor_id;	/**< EAM sensor					*/
 	uint8_t warning;
-	uint8_t sensor_id;		/**< Sensor ID, why different? 			*/
+	uint8_t sensor_id;		/**< Sensor ID, why different?	*/
 	uint8_t alarm_inverse1;
 	uint8_t alarm_inverse2;
-	uint8_t cell1_L;		/**< Lipo cell voltages. Not supported. 	*/
+	uint8_t cell1_L;		/**< Lipo cell voltages. Not supported.	*/
 	uint8_t cell2_L;
 	uint8_t cell3_L;
 	uint8_t cell4_L;
@@ -117,7 +112,14 @@ struct eam_module_msg {
 	uint8_t checksum;		/**< Lower 8-bits of all bytes summed. 			*/
 };
 
+/** 
+ * The maximum buffer size required to store a HoTT message.
+ */
+#define MESSAGE_BUFFER_SIZE sizeof(union { 	\
+    struct eam_module_msg eam; 				\
+})
+
 void messages_init(void);
-void build_eam_response(uint8_t *buffer, int *size);
+void build_eam_response(uint8_t *buffer, size_t *size);
 
 #endif /* MESSAGES_H_ */

From 443c3f67d3cdfbbf9a2d16a9c126171ed316051c Mon Sep 17 00:00:00 2001
From: Simon Wilks <sjwilks@gmail.com>
Date: Thu, 25 Apr 2013 08:36:14 +0200
Subject: [PATCH 02/17] Reformatting.

---
 apps/hott_telemetry/messages.c | 11 ++++++++++-
 1 file changed, 10 insertions(+), 1 deletion(-)

diff --git a/apps/hott_telemetry/messages.c b/apps/hott_telemetry/messages.c
index c1988cd054..0b863c6895 100644
--- a/apps/hott_telemetry/messages.c
+++ b/apps/hott_telemetry/messages.c
@@ -45,6 +45,9 @@
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/sensor_combined.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 sensor_sub = -1;
 
@@ -72,14 +75,20 @@ build_eam_response(uint8_t *buffer, size_t *size)
 	msg.start = START_BYTE;
 	msg.eam_sensor_id = ELECTRIC_AIR_MODULE;
 	msg.sensor_id = EAM_SENSOR_ID;
+	
 	msg.temperature1 = (uint8_t)(raw.baro_temp_celcius + 20);
-	msg.temperature2 = TEMP_ZERO_CELSIUS;
+	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;
 
+	// TODO: flight time
+	// TODO: climb rate
+	
+
 	msg.stop = STOP_BYTE;
 
 	memcpy(buffer, &msg, *size);

From b666bbb55c83979d220d26e70a0aaa6f8e908095 Mon Sep 17 00:00:00 2001
From: Simon Wilks <sjwilks@gmail.com>
Date: Thu, 25 Apr 2013 20:18:06 +0200
Subject: [PATCH 03/17] First steps to supporting GPS

---
 apps/hott_telemetry/messages.h | 99 ++++++++++++++++++++++++++++------
 1 file changed, 82 insertions(+), 17 deletions(-)

diff --git a/apps/hott_telemetry/messages.h b/apps/hott_telemetry/messages.h
index 9ca205f818..8b0935e06d 100644
--- a/apps/hott_telemetry/messages.h
+++ b/apps/hott_telemetry/messages.h
@@ -65,13 +65,13 @@
 
 /* The Electric Air Module message. */
 struct eam_module_msg {
-	uint8_t start;			/**< Start byte   				*/
-	uint8_t eam_sensor_id;	/**< EAM sensor					*/
+	uint8_t start;				/**< Start byte   				*/
+	uint8_t eam_sensor_id;		/**< EAM sensor					*/
 	uint8_t warning;
-	uint8_t sensor_id;		/**< Sensor ID, why different?	*/
+	uint8_t sensor_id;			/**< Sensor ID, why different?	*/
 	uint8_t alarm_inverse1;
 	uint8_t alarm_inverse2;
-	uint8_t cell1_L;		/**< Lipo cell voltages. Not supported.	*/
+	uint8_t cell1_L;			/**< Lipo cell voltages. Not supported.	*/
 	uint8_t cell2_L;
 	uint8_t cell3_L;
 	uint8_t cell4_L;
@@ -89,27 +89,27 @@ struct eam_module_msg {
 	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 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_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_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_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_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_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 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 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. 			*/
+	uint8_t stop;				/**< Stop byte */
+	uint8_t checksum;			/**< Lower 8-bits of all bytes summed. */
 };
 
 /** 
@@ -119,7 +119,72 @@ struct eam_module_msg {
     struct eam_module_msg eam; 				\
 })
 
+/* 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 gps_sensor_id;		/**< EAM sensor */
+	uint8_t warning;			/**< Byte 3: 0…= warning beeps */
+	uint8_t sensor_id;			/**< Sensor ID, why different?	*/
+	uint8_t alarm_inverse1;		/**< Byte 5: 01 inverse status */
+	uint8_t alarm_inverse2;		/**< Byte 6: 00 inverse status status 1 = no GPS Signal */
+	uint8_t flight_direction;	/**< Byte 7: 119 = Flightdir./dir. 1 = 2°; 0° (North), 9 0° (East), 180° (South), 270° (West) */
+	uint8_t gps_speed_L;		/**< Byte 8: 8 = /GPS speed low byte 8km/h */
+	uint8_t gps_speed_H;		/**< Byte 9: 0 = /GPS speed high byte */
+	  
+	uint8_t latitude_ns;		/**< Byte 10: 000 = N = 48°39’988 */
+	uint8_t latitude_min_L;		/**< Byte 11: 231 0xE7 = 0x12E7 = 4839 */
+	uint8_t latitude_min_H;		/**< Byte 12: 018 18 = 0x12 */
+	uint8_t latitude_sec_L;		/**< Byte 13: 171 220 = 0xDC = 0x03DC =0988 */
+	uint8_t latitude_sec_H;		/**< Byte 14: 016 3 = 0x03 */
+	 
+	uint8_t longitude_ew;		/**< Byte 15: 000  = E= 9° 25’9360 */
+	uint8_t longitude_in_L;		/**< Byte 16: 150 157 = 0x9D = 0x039D = 0925 */
+	uint8_t longitude_min_H;	/**< Byte 17: 003 3 = 0x03 */
+	uint8_t longitude_sec_L		/**< Byte 18: 056 144 = 0x90 0x2490 = 9360*/
+	uint8_t longitude_sec_H;	/**< Byte 19: 004 36 = 0x24 */
+	  
+	uint8_t distance_L			/**< Byte 20: 027 123 = /distance low byte 6 = 6 m */
+	uint8_t distance_H;			/**< Byte 21: 036 35 = /distance high byte */
+	uint8_t altitude_L			/**< Byte 22: 243 244 = /Altitude low byte 500 = 0m */
+	uint8_t altitude_H;			/**< Byte 23: 001 1 = /Altitude high byte */
+	uint8_t resolution_L		/**< Byte 24: 48 = Low Byte m/s resolution 0.01m 48 = 30000 = 0.00m/s (1=0.01m/s) */
+	uint8_t resolution_H;		/**< Byte 25: 117 = High Byte m/s resolution 0.01m */
+	uint8_t unknown1;			/**< Byte 26: 120 = 0m/3s */
+	uint8_t gps_num_sat;		/**< Byte 27: GPS.Satellites (number of satelites) (1 byte) */
+	uint8_t gps_fix_char;		/**< Byte 28: GPS.FixChar. (GPS fix character. display, if DGPS, 2D oder 3D) (1 byte) */
+	uint8_t home_direction;		/**< Byte 29: HomeDirection (direction from starting point to Model position) (1 byte) */
+	uint8_t angle_x_direction;	/**< Byte 30: angle x-direction (1 byte) */
+	uint8_t angle_y_direction;	/**< Byte 31: angle y-direction (1 byte) */
+	uint8_t angle_z_direction;	/**< Byte 32: angle z-direction (1 byte) */
+	uint8_t gyro_x_L			/**< Byte 33: gyro x low byte (2 bytes) */
+	uint8_t gyro_x_H;			/**< Byte 34: gyro x high byte */
+	uint8_t gyro_y_L			/**< Byte 35: gyro y low byte (2 bytes) */
+	uint8_t gyro_y_H;			/**< Byte 36: gyro y high byte */
+	uint8_t gyro_z_L			/**< Byte 37: gyro z low byte (2 bytes) */
+	uint8_t gyro_z_H;			/**< Byte 38: gyro z high byte */
+	uint8_t vibration;			/**< Byte 39: vibration (1 bytes) */
+	uint8_t ascii4;				/**< Byte 40: 00 ASCII Free Character [4] */
+	uint8_t ascii5;				/**< Byte 41: 00 ASCII Free Character [5] */
+	uint8_t gps_fix;			/**< Byte 42: 00 ASCII Free Character [6], we use it for GPS FIX */
+	uint8_t version;			/**< Byte 43: 00 version number */
+	uint8_t stop;				/**< Byte 44: 0x7D Ende byte */
+	uint8_t checksum;			/**< Byte 45: Parity Byte */
+};
+
+/** 
+ * The maximum buffer size required to store a HoTT message.
+ */
+#define GPS_MESSAGE_BUFFER_SIZE sizeof(union { 	\
+    struct gps_module_msg gps; 				\
+})
+
 void messages_init(void);
 void build_eam_response(uint8_t *buffer, size_t *size);
+void build_gps_response(uint8_t *buffer, size_t *size);
 
 #endif /* MESSAGES_H_ */

From 0af9b60db7c9de7b59aa8378980e4dc55a10a051 Mon Sep 17 00:00:00 2001
From: Simon Wilks <sjwilks@gmail.com>
Date: Fri, 26 Apr 2013 22:28:31 +0200
Subject: [PATCH 04/17] More changes.

---
 apps/hott_telemetry/messages.c | 26 ++++++++++++++++++++++++--
 apps/hott_telemetry/messages.h |  4 ++--
 2 files changed, 26 insertions(+), 4 deletions(-)

diff --git a/apps/hott_telemetry/messages.c b/apps/hott_telemetry/messages.c
index 0b863c6895..db76fa5ace 100644
--- a/apps/hott_telemetry/messages.c
+++ b/apps/hott_telemetry/messages.c
@@ -73,8 +73,8 @@ build_eam_response(uint8_t *buffer, size_t *size)
 	*size = sizeof(msg);
 
 	msg.start = START_BYTE;
-	msg.eam_sensor_id = ELECTRIC_AIR_MODULE;
-	msg.sensor_id = EAM_SENSOR_ID;
+	msg.eam_sensor_id = EAM_SENSOR_ID;
+	msg.sensor_id = EAM_SENSOR_TEXT_ID;
 	
 	msg.temperature1 = (uint8_t)(raw.baro_temp_celcius + 20);
 	msg.temperature2 = msg.temperature1 - BOARD_TEMP_OFFSET_DEG;
@@ -89,6 +89,28 @@ build_eam_response(uint8_t *buffer, size_t *size)
 	// TODO: climb rate
 	
 
+	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 = { 0 };
+	orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
+
+
+	struct gps_module_msg msg = { 0 };
+	*size = sizeof(msg);
+
+	msg.start = START_BYTE;
+	msg.eam_sensor_id = GPS_SENSOR_ID;
+	msg.sensor_id = GPS_SENSOR_TEXT_ID;
+	
+	// TODO(sjwilks): Complete.
+
 	msg.stop = STOP_BYTE;
 
 	memcpy(buffer, &msg, *size);
diff --git a/apps/hott_telemetry/messages.h b/apps/hott_telemetry/messages.h
index 8b0935e06d..8ba324f960 100644
--- a/apps/hott_telemetry/messages.h
+++ b/apps/hott_telemetry/messages.h
@@ -60,8 +60,8 @@
 #define TEMP_ZERO_CELSIUS	0x14
 
 /* Electric Air Module (EAM) constants. */
-#define ELECTRIC_AIR_MODULE	0x8e
-#define EAM_SENSOR_ID		0xe0
+#define EAM_SENSOR_ID			0x8e
+#define EAM_SENSOR_TEXT_ID		0xe0
 
 /* The Electric Air Module message. */
 struct eam_module_msg {

From 5ebd78345cf815689117af73bc7909145ba71e4c Mon Sep 17 00:00:00 2001
From: Simon Wilks <sjwilks@gmail.com>
Date: Wed, 1 May 2013 21:30:11 +0200
Subject: [PATCH 05/17] Add some GPS data to telemetry message.

---
 apps/hott_telemetry/hott_telemetry_main.c |  6 ++++-
 apps/hott_telemetry/messages.c            | 23 +++++++++++++++++--
 apps/hott_telemetry/messages.h            | 28 ++++++++++++-----------
 3 files changed, 41 insertions(+), 16 deletions(-)

diff --git a/apps/hott_telemetry/hott_telemetry_main.c b/apps/hott_telemetry/hott_telemetry_main.c
index 3e147d8292..33b721962b 100644
--- a/apps/hott_telemetry/hott_telemetry_main.c
+++ b/apps/hott_telemetry/hott_telemetry_main.c
@@ -225,7 +225,11 @@ hott_telemetry_thread_main(int argc, char *argv[])
 	while (!thread_should_exit) {
 		if (recv_req_id(uart, &id) == OK) {
 			switch (id) {
-			case ELECTRIC_AIR_MODULE:
+			case EAM_SENSOR_ID:
+				build_eam_response(buffer, &size);
+				break;
+
+			case GPS_SENSOR_ID:
 				build_eam_response(buffer, &size);
 				break;
 
diff --git a/apps/hott_telemetry/messages.c b/apps/hott_telemetry/messages.c
index db76fa5ace..2e1a5c064e 100644
--- a/apps/hott_telemetry/messages.c
+++ b/apps/hott_telemetry/messages.c
@@ -44,17 +44,20 @@
 #include <unistd.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/vehicle_gps_position.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 sensor_sub = -1;
 
 void 
 messages_init(void)
 {
 	battery_sub = orb_subscribe(ORB_ID(battery_status));
+	gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
 	sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
 }
 
@@ -101,14 +104,30 @@ build_gps_response(uint8_t *buffer, size_t *size)
 	struct sensor_combined_s raw = { 0 };
 	orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
 
+ 	/* get a local copy of the battery data */
+	struct vehicle_gps_position_s gps = { 0 };
+	orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps);
 
 	struct gps_module_msg msg = { 0 };
 	*size = sizeof(msg);
 
 	msg.start = START_BYTE;
-	msg.eam_sensor_id = GPS_SENSOR_ID;
-	msg.sensor_id = GPS_SENSOR_TEXT_ID;
+	msg.sensor_id = GPS_SENSOR_ID;
+	msg.sensor_text_id = GPS_SENSOR_TEXT_ID;
+
+	uint16_t speed = (uint16_t)(gps.vel_m_s * 3.6);
+	msg.gps_speed_L = (uint8_t)speed & 0xff;
+	msg.gps_speed_H = (uint8_t)(speed >> 8) & 0xff;
 	
+	uint16_t alt = (uint16_t)(gps.alt);
+	msg.altitude_L = (uint8_t)alt & 0xff;
+	msg.altitude_H = (uint8_t)(alt >> 8) & 0xff;
+
+	msg.gps_num_sat = gps.satellites_visible;
+
+	/* Display the fix type: 0 = none, 2 = 2D, 3 = 3D */
+	msg.gps_fix = (uint8_t)(gps.fix_type + 48);
+
 	// TODO(sjwilks): Complete.
 
 	msg.stop = STOP_BYTE;
diff --git a/apps/hott_telemetry/messages.h b/apps/hott_telemetry/messages.h
index 8ba324f960..c83672c08f 100644
--- a/apps/hott_telemetry/messages.h
+++ b/apps/hott_telemetry/messages.h
@@ -123,13 +123,15 @@ struct eam_module_msg {
 #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 = { 
+/** 
+ * 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 gps_sensor_id;		/**< EAM sensor */
+	uint8_t sensor_id;			/**< GPS sensor ID*/
 	uint8_t warning;			/**< Byte 3: 0…= warning beeps */
-	uint8_t sensor_id;			/**< Sensor ID, why different?	*/
+	uint8_t sensor_text_id;		/**< GPS Sensor text mode ID */
 	uint8_t alarm_inverse1;		/**< Byte 5: 01 inverse status */
 	uint8_t alarm_inverse2;		/**< Byte 6: 00 inverse status status 1 = no GPS Signal */
 	uint8_t flight_direction;	/**< Byte 7: 119 = Flightdir./dir. 1 = 2°; 0° (North), 9 0° (East), 180° (South), 270° (West) */
@@ -143,16 +145,16 @@ struct gps_module_msg = {
 	uint8_t latitude_sec_H;		/**< Byte 14: 016 3 = 0x03 */
 	 
 	uint8_t longitude_ew;		/**< Byte 15: 000  = E= 9° 25’9360 */
-	uint8_t longitude_in_L;		/**< Byte 16: 150 157 = 0x9D = 0x039D = 0925 */
+	uint8_t longitude_min_L;		/**< Byte 16: 150 157 = 0x9D = 0x039D = 0925 */
 	uint8_t longitude_min_H;	/**< Byte 17: 003 3 = 0x03 */
-	uint8_t longitude_sec_L		/**< Byte 18: 056 144 = 0x90 0x2490 = 9360*/
+	uint8_t longitude_sec_L;	/**< Byte 18: 056 144 = 0x90 0x2490 = 9360*/
 	uint8_t longitude_sec_H;	/**< Byte 19: 004 36 = 0x24 */
 	  
-	uint8_t distance_L			/**< Byte 20: 027 123 = /distance low byte 6 = 6 m */
+	uint8_t distance_L;			/**< Byte 20: 027 123 = /distance low byte 6 = 6 m */
 	uint8_t distance_H;			/**< Byte 21: 036 35 = /distance high byte */
-	uint8_t altitude_L			/**< Byte 22: 243 244 = /Altitude low byte 500 = 0m */
+	uint8_t altitude_L;			/**< Byte 22: 243 244 = /Altitude low byte 500 = 0m */
 	uint8_t altitude_H;			/**< Byte 23: 001 1 = /Altitude high byte */
-	uint8_t resolution_L		/**< Byte 24: 48 = Low Byte m/s resolution 0.01m 48 = 30000 = 0.00m/s (1=0.01m/s) */
+	uint8_t resolution_L;		/**< Byte 24: 48 = Low Byte m/s resolution 0.01m 48 = 30000 = 0.00m/s (1=0.01m/s) */
 	uint8_t resolution_H;		/**< Byte 25: 117 = High Byte m/s resolution 0.01m */
 	uint8_t unknown1;			/**< Byte 26: 120 = 0m/3s */
 	uint8_t gps_num_sat;		/**< Byte 27: GPS.Satellites (number of satelites) (1 byte) */
@@ -161,11 +163,11 @@ struct gps_module_msg = {
 	uint8_t angle_x_direction;	/**< Byte 30: angle x-direction (1 byte) */
 	uint8_t angle_y_direction;	/**< Byte 31: angle y-direction (1 byte) */
 	uint8_t angle_z_direction;	/**< Byte 32: angle z-direction (1 byte) */
-	uint8_t gyro_x_L			/**< Byte 33: gyro x low byte (2 bytes) */
+	uint8_t gyro_x_L;			/**< Byte 33: gyro x low byte (2 bytes) */
 	uint8_t gyro_x_H;			/**< Byte 34: gyro x high byte */
-	uint8_t gyro_y_L			/**< Byte 35: gyro y low byte (2 bytes) */
+	uint8_t gyro_y_L;			/**< Byte 35: gyro y low byte (2 bytes) */
 	uint8_t gyro_y_H;			/**< Byte 36: gyro y high byte */
-	uint8_t gyro_z_L			/**< Byte 37: gyro z low byte (2 bytes) */
+	uint8_t gyro_z_L;			/**< Byte 37: gyro z low byte (2 bytes) */
 	uint8_t gyro_z_H;			/**< Byte 38: gyro z high byte */
 	uint8_t vibration;			/**< Byte 39: vibration (1 bytes) */
 	uint8_t ascii4;				/**< Byte 40: 00 ASCII Free Character [4] */

From 150eabbf3986bd120908506b97ed7d9e9eed1038 Mon Sep 17 00:00:00 2001
From: Simon Wilks <sjwilks@gmail.com>
Date: Tue, 14 May 2013 23:53:21 +0200
Subject: [PATCH 06/17] GPS support - mostly working.

---
 apps/hott_telemetry/hott_telemetry_main.c |  2 +-
 apps/hott_telemetry/messages.c            | 87 +++++++++++++++++++++--
 apps/hott_telemetry/messages.h            |  4 +-
 3 files changed, 87 insertions(+), 6 deletions(-)

diff --git a/apps/hott_telemetry/hott_telemetry_main.c b/apps/hott_telemetry/hott_telemetry_main.c
index 33b721962b..f419c95ded 100644
--- a/apps/hott_telemetry/hott_telemetry_main.c
+++ b/apps/hott_telemetry/hott_telemetry_main.c
@@ -230,7 +230,7 @@ hott_telemetry_thread_main(int argc, char *argv[])
 				break;
 
 			case GPS_SENSOR_ID:
-				build_eam_response(buffer, &size);
+				build_gps_response(buffer, &size);
 				break;
 
 			default:
diff --git a/apps/hott_telemetry/messages.c b/apps/hott_telemetry/messages.c
index 2e1a5c064e..feec8998c5 100644
--- a/apps/hott_telemetry/messages.c
+++ b/apps/hott_telemetry/messages.c
@@ -39,10 +39,12 @@
 
 #include "messages.h"
 
+#include <math.h>
 #include <string.h>
 #include <systemlib/systemlib.h>
 #include <unistd.h>
 #include <uORB/topics/battery_status.h>
+#include <uORB/topics/home_position.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/vehicle_gps_position.h>
 
@@ -51,6 +53,7 @@
 
 static int battery_sub = -1;
 static int gps_sub = -1;
+static int home_sub = -1;
 static int sensor_sub = -1;
 
 void 
@@ -58,6 +61,7 @@ messages_init(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));
 }
 
@@ -115,22 +119,97 @@ build_gps_response(uint8_t *buffer, size_t *size)
 	msg.sensor_id = GPS_SENSOR_ID;
 	msg.sensor_text_id = GPS_SENSOR_TEXT_ID;
 
+	/* 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.6);
 	msg.gps_speed_L = (uint8_t)speed & 0xff;
 	msg.gps_speed_H = (uint8_t)(speed >> 8) & 0xff;
 	
-	uint16_t alt = (uint16_t)(gps.alt);
+	/* Get latitude in degrees, minutes and seconds */
+	double lat = ((double)(gps.lat)) * 1e-7d;
+
+	msg.latitude_ns = 0;
+	if (lat < 0) {
+		msg.latitude_ns = 1;
+		lat = -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;
+
+	msg.longitude_ew = 0;
+	if (lon < 0) {
+		msg.longitude_ew = 1;
+		lon = -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-3 + 500.0f);
 	msg.altitude_L = (uint8_t)alt & 0xff;
 	msg.altitude_H = (uint8_t)(alt >> 8) & 0xff;
 
+	/* Distance from home */
+	bool updated;
+	orb_check(home_sub, &updated);
+	if (updated) {
+	    /* get a local copy of the home position data */
+		struct home_position_s home = { 0 };
+		orb_copy(ORB_ID(home_position), home_sub, &home);
+
+	    uint16_t dist = (uint16_t)get_distance_to_next_waypoint(
+	    	(double)home.lat*1e-7, (double)home.lon*1e-7, lat, lon);
+	    warnx("dist %d home.lat %3.6f home.lon %3.6f lat %3.6f lon %3.6f ", 
+	    	  dist, (double)home.lat*1e-7d, (double)home.lon*1e-7d, 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(
+	    	(double)home.lat*1e-7, (double)home.lon*1e-7, lat, lon) * M_RAD_TO_DEG_F;
+		msg.home_direction = (uint8_t)bearing >> 1;
+    }
+
 	msg.gps_num_sat = gps.satellites_visible;
 
-	/* Display the fix type: 0 = none, 2 = 2D, 3 = 3D */
+	/* 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);
 
-	// TODO(sjwilks): Complete.
-
 	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;
+    *min = (int)(delta * 60.0);
+    *sec = (int)(delta * 3600.0);
+}
diff --git a/apps/hott_telemetry/messages.h b/apps/hott_telemetry/messages.h
index c83672c08f..4ccc1b3114 100644
--- a/apps/hott_telemetry/messages.h
+++ b/apps/hott_telemetry/messages.h
@@ -145,7 +145,7 @@ struct gps_module_msg {
 	uint8_t latitude_sec_H;		/**< Byte 14: 016 3 = 0x03 */
 	 
 	uint8_t longitude_ew;		/**< Byte 15: 000  = E= 9° 25’9360 */
-	uint8_t longitude_min_L;		/**< Byte 16: 150 157 = 0x9D = 0x039D = 0925 */
+	uint8_t longitude_min_L;	/**< Byte 16: 150 157 = 0x9D = 0x039D = 0925 */
 	uint8_t longitude_min_H;	/**< Byte 17: 003 3 = 0x03 */
 	uint8_t longitude_sec_L;	/**< Byte 18: 056 144 = 0x90 0x2490 = 9360*/
 	uint8_t longitude_sec_H;	/**< Byte 19: 004 36 = 0x24 */
@@ -188,5 +188,7 @@ struct gps_module_msg {
 void messages_init(void);
 void build_eam_response(uint8_t *buffer, size_t *size);
 void build_gps_response(uint8_t *buffer, size_t *size);
+void convert_to_degrees_minutes_seconds(double lat, int *deg, int *min, int *sec);
+
 
 #endif /* MESSAGES_H_ */

From 2876bc72f979b4596fbe97cfae71c0d04d4753b2 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 28 May 2013 17:46:24 +0200
Subject: [PATCH 07/17] Slightly reworked IO internal failsafe, added command
 to activate it (px4io failsafe), does not parse commandline arguments yet

---
 src/drivers/px4io/px4io.cpp           | 52 +++++++++++++++++++++++----
 src/modules/px4iofirmware/mixer.cpp   | 29 +++++++++++----
 src/modules/px4iofirmware/protocol.h  |  4 ++-
 src/modules/px4iofirmware/registers.c |  3 +-
 4 files changed, 73 insertions(+), 15 deletions(-)

diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 8b2fae12b8..f25d471aa0 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -106,7 +106,7 @@ public:
 	* @param rate    The rate in Hz actuator outpus are sent to IO.
 	*      Min 10 Hz, max 400 Hz
 	*/
-	int      set_update_rate(int rate);
+	int      		set_update_rate(int rate);
 
 	/**
 	* Set the battery current scaling and bias
@@ -114,7 +114,15 @@ public:
 	* @param amp_per_volt
 	* @param amp_bias
 	*/
-	void      set_battery_current_scaling(float amp_per_volt, float amp_bias);
+	void      		set_battery_current_scaling(float amp_per_volt, float amp_bias);
+
+	/**
+	 * Push failsafe values to IO.
+	 *
+	 * @param vals Failsafe control inputs: in us PPM (900 for zero, 1500 for centered, 2100 for full)
+	 * @param len Number of channels, could up to 8
+	 */
+	int			set_failsafe_values(const uint16_t *vals, unsigned len);
 
 	/**
 	* Print the current status of IO
@@ -326,11 +334,11 @@ PX4IO::PX4IO() :
 	_to_actuators_effective(0),
 	_to_outputs(0),
 	_to_battery(0),
+	_primary_pwm_device(false),
 	_battery_amp_per_volt(90.0f/5.0f), // this matches the 3DR current sensor
 	_battery_amp_bias(0),
 	_battery_mamphour_total(0),
-	_battery_last_timestamp(0),
-	_primary_pwm_device(false)
+	_battery_last_timestamp(0)
 {
 	/* we need this potentially before it could be set in task_main */
 	g_dev = this;
@@ -689,6 +697,21 @@ PX4IO::io_set_control_state()
 	return io_reg_set(PX4IO_PAGE_CONTROLS, 0, regs, _max_controls);
 }
 
+int
+PX4IO::set_failsafe_values(const uint16_t *vals, unsigned len)
+{
+	uint16_t 		regs[_max_actuators];
+
+	unsigned max = (len < _max_actuators) ? len : _max_actuators;
+
+	/* set failsafe values */
+	for (unsigned i = 0; i < max; i++)
+		regs[i] = FLOAT_TO_REG(vals[i]);
+
+	/* copy values to registers in IO */
+	return io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, regs, max);
+}
+
 int
 PX4IO::io_set_arming_state()
 {
@@ -1250,7 +1273,7 @@ PX4IO::print_status()
 	printf("%u bytes free\n",
 		io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM));
 	uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS);
-	printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s\n",
+	printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s\n",
 		flags,
 		((flags & PX4IO_P_STATUS_FLAGS_ARMED)    ? " ARMED" : ""),
 		((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""),
@@ -1262,7 +1285,8 @@ PX4IO::print_status()
 		((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM)  ? " RAW_PPM" : ""),
 		((flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ? " MIXER_OK" : " MIXER_FAIL"),
 		((flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC) ? " ARM_SYNC" : " ARM_NO_SYNC"),
-		((flags & PX4IO_P_STATUS_FLAGS_INIT_OK)  ? " INIT_OK" : " INIT_FAIL"));
+		((flags & PX4IO_P_STATUS_FLAGS_INIT_OK)  ? " INIT_OK" : " INIT_FAIL"),
+		((flags & PX4IO_P_STATUS_FLAGS_FAILSAFE)  ? " FAILSAFE" : ""));
 	uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS);
 	printf("alarms 0x%04x%s%s%s%s%s%s%s\n",
 		alarms,
@@ -1718,6 +1742,20 @@ px4io_main(int argc, char *argv[])
 		exit(0);
 	}
 
+	if (!strcmp(argv[1], "failsafe")) {
+
+		/* XXX parse arguments here */
+
+		if (g_dev != nullptr) {
+			/* XXX testing values */
+			uint16_t failsafe[4] = {1500, 1500, 1200, 1200};
+			g_dev->set_failsafe_values(failsafe, sizeof(failsafe) / sizeof(failsafe[0]));
+		} else {
+			errx(1, "not loaded");
+		}
+		exit(0);
+	}
+
 	if (!strcmp(argv[1], "recovery")) {
 
 		if (g_dev != nullptr) {
@@ -1845,5 +1883,5 @@ px4io_main(int argc, char *argv[])
 		monitor();
 
 	out:
-	errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit', 'current' or 'update'");
+	errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit', 'current', 'failsafe' or 'update'");
 }
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index 1234e2eea5..448d2355fc 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -102,13 +102,16 @@ mixer_tick(void)
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK;
 	}
 
+	/* default to failsafe mixing */
 	source = MIX_FAILSAFE;
 
 	/*
 	 * Decide which set of controls we're using.
 	 */
-	if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) ||
-		!(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
+
+	/* do not mix if mixer is invalid or if RAW_PWM mode is on and FMU is good */
+	if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) &&
+	        !(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
 
 		/* don't actually mix anything - we already have raw PWM values or
 		 not a valid mixer. */
@@ -117,6 +120,7 @@ mixer_tick(void)
 	} else {
 
 		if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
+		     (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) &&
 		     (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
 
 			/* mix from FMU controls */
@@ -132,15 +136,29 @@ mixer_tick(void)
 		}
 	}
 
+	/*
+	 * Set failsafe status flag depending on mixing source
+	 */
+	if (source == MIX_FAILSAFE) {
+		r_status_flags |= PX4IO_P_STATUS_FLAGS_FAILSAFE;
+	} else {
+		r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FAILSAFE);
+	}
+
 	/*
 	 * Run the mixers.
 	 */
 	if (source == MIX_FAILSAFE) {
 
 		/* copy failsafe values to the servo outputs */
-		for (unsigned i = 0; i < IO_SERVO_COUNT; i++)
+		for (unsigned i = 0; i < IO_SERVO_COUNT; i++) {
 			r_page_servos[i] = r_page_servo_failsafe[i];
 
+			/* safe actuators for FMU feedback */
+			r_page_actuators[i] = (r_page_servos[i] - 1500) / 600.0f;
+		}
+
+
 	} else if (source != MIX_NONE) {
 
 		float	outputs[IO_SERVO_COUNT];
@@ -156,7 +174,7 @@ mixer_tick(void)
 			r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
 
 			/* scale to servo output */
-			r_page_servos[i] = (outputs[i] * 500.0f) + 1500;
+			r_page_servos[i] = (outputs[i] * 600.0f) + 1500;
 
 		}
 		for (unsigned i = mixed; i < IO_SERVO_COUNT; i++)
@@ -175,7 +193,7 @@ mixer_tick(void)
 	bool should_arm = (
 	    /* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
 	 	/* IO is armed */  (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) &&
-		/* there is valid input */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) &&
+		/* there is valid input via direct PWM or mixer */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) &&
 		/* IO initialised without error */  (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) &&
 		/* FMU is available or FMU is not available but override is an option */
 		((r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) || (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && (r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ))
@@ -225,7 +243,6 @@ mixer_callback(uintptr_t handle,
 
 	case MIX_FAILSAFE:
 	case MIX_NONE:
-		/* XXX we could allow for configuration of per-output failsafe values */
 		return -1;
 	}
 
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index e575bd8412..674f9dddd6 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -85,7 +85,7 @@
 #define PX4IO_P_CONFIG_ACTUATOR_COUNT		5	/* hardcoded max actuator output count */
 #define PX4IO_P_CONFIG_RC_INPUT_COUNT		6	/* hardcoded max R/C input count supported */
 #define PX4IO_P_CONFIG_ADC_INPUT_COUNT		7	/* hardcoded max ADC inputs */
-#define PX4IO_P_CONFIG_RELAY_COUNT		8	/* harcoded # of relay outputs */
+#define PX4IO_P_CONFIG_RELAY_COUNT		8	/* hardcoded # of relay outputs */
 
 /* dynamic status page */
 #define PX4IO_PAGE_STATUS		1
@@ -104,6 +104,7 @@
 #define PX4IO_P_STATUS_FLAGS_MIXER_OK		(1 << 8) /* mixer is OK */
 #define PX4IO_P_STATUS_FLAGS_ARM_SYNC		(1 << 9) /* the arming state between IO and FMU is in sync */
 #define PX4IO_P_STATUS_FLAGS_INIT_OK		(1 << 10) /* initialisation of the IO completed without error */
+#define PX4IO_P_STATUS_FLAGS_FAILSAFE		(1 << 11) /* failsafe is active */
 
 #define PX4IO_P_STATUS_ALARMS			3	 /* alarm flags - alarms latch, write 1 to a bit to clear it */
 #define PX4IO_P_STATUS_ALARMS_VBATT_LOW		(1 << 0) /* VBatt is very close to regulator dropout */
@@ -148,6 +149,7 @@
 #define PX4IO_P_SETUP_ARMING_IO_ARM_OK		(1 << 0) /* OK to arm the IO side */
 #define PX4IO_P_SETUP_ARMING_FMU_ARMED		(1 << 1) /* FMU is already armed */
 #define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK	(1 << 2) /* OK to switch to manual override via override RC channel */
+#define PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM	(1 << 3) /* use custom failsafe values, not 0 values of mixer */
 #define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK	(1 << 4) /* OK to try in-air restart */
 
 #define PX4IO_P_SETUP_PWM_RATES			2	/* bitmask, 0 = low rate, 1 = high rate */
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 9698a0f2fb..3abf56a18d 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -41,6 +41,7 @@
 
 #include <stdbool.h>
 #include <stdlib.h>
+#include <string.h>
 
 #include <drivers/drv_hrt.h>
 
@@ -179,7 +180,7 @@ uint16_t		r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE
  *
  * Failsafe servo PWM values
  */
-uint16_t		r_page_servo_failsafe[IO_SERVO_COUNT];
+uint16_t		r_page_servo_failsafe[IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, 900, 900, 900 };
 
 void
 registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)

From f6570172da02bba79b5e050c6e02b86dc96551c9 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 29 May 2013 17:07:26 +0200
Subject: [PATCH 08/17] Set default failsafe value to 0 of mixer

---
 src/drivers/px4io/px4io.cpp           | 29 +++++++++++++++------
 src/modules/px4iofirmware/mixer.cpp   | 37 +++++++++++++++++++++++++++
 src/modules/px4iofirmware/registers.c |  5 +++-
 3 files changed, 62 insertions(+), 9 deletions(-)

diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index f25d471aa0..873d7eb82a 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -704,12 +704,8 @@ PX4IO::set_failsafe_values(const uint16_t *vals, unsigned len)
 
 	unsigned max = (len < _max_actuators) ? len : _max_actuators;
 
-	/* set failsafe values */
-	for (unsigned i = 0; i < max; i++)
-		regs[i] = FLOAT_TO_REG(vals[i]);
-
 	/* copy values to registers in IO */
-	return io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, regs, max);
+	return io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, vals, max);
 }
 
 int
@@ -1744,11 +1740,28 @@ px4io_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "failsafe")) {
 
-		/* XXX parse arguments here */
+		if (argc < 3) {
+			errx(1, "failsafe command needs at least one channel value (ppm)");
+		}
 
 		if (g_dev != nullptr) {
-			/* XXX testing values */
-			uint16_t failsafe[4] = {1500, 1500, 1200, 1200};
+
+			/* set values for first 8 channels, fill unassigned channels with 1500. */
+			uint16_t failsafe[8];
+
+			for (int i = 0; i < sizeof(failsafe) / sizeof(failsafe[0]); i++)
+			{
+				/* set channel to commanline argument or to 900 for non-provided channels */
+				if (argc > i + 2) {
+					failsafe[i] = atoi(argv[i+2]);
+					if (failsafe[i] < 800 || failsafe[i] > 2200) {
+						errx(1, "value out of range of 800 < value < 2200. Aborting.");
+					}
+				} else {
+					failsafe[i] = 1500;
+				}
+			}
+
 			g_dev->set_failsafe_values(failsafe, sizeof(failsafe) / sizeof(failsafe[0]));
 		} else {
 			errx(1, "not loaded");
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index 448d2355fc..0b8ed6dc59 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -85,6 +85,9 @@ static int	mixer_callback(uintptr_t handle,
 
 static MixerGroup mixer_group(mixer_callback, 0);
 
+/* Set the failsafe values of all mixed channels (based on zero throttle, controls centered) */
+static void mixer_set_failsafe();
+
 void
 mixer_tick(void)
 {
@@ -243,6 +246,7 @@ mixer_callback(uintptr_t handle,
 
 	case MIX_FAILSAFE:
 	case MIX_NONE:
+		control = 0.0f;
 		return -1;
 	}
 
@@ -320,8 +324,41 @@ mixer_handle_text(const void *buffer, size_t length)
 				memcpy(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid);
 
 			mixer_text_length = resid;
+
+			/* update failsafe values */
+			mixer_set_failsafe();
 		}
 
 		break;
 	}
 }
+
+static void
+mixer_set_failsafe()
+{
+	/* 
+	 * Check if a custom failsafe value has been written,
+	 * else use the opportunity to set decent defaults.
+	 */
+	if (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)
+		return;
+
+	float	outputs[IO_SERVO_COUNT];
+	unsigned mixed;
+
+	/* mix */
+	mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT);
+
+	/* scale to PWM and update the servo outputs as required */
+	for (unsigned i = 0; i < mixed; i++) {
+
+		/* scale to servo output */
+		r_page_servo_failsafe[i] = (outputs[i] * 600.0f) + 1500;
+
+	}
+
+	/* disable the rest of the outputs */
+	for (unsigned i = mixed; i < IO_SERVO_COUNT; i++)
+		r_page_servo_failsafe[i] = 0;
+
+}
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 3abf56a18d..49a7233528 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -231,11 +231,14 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
 	case PX4IO_PAGE_FAILSAFE_PWM:
 
 		/* copy channel data */
-		while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
+		while ((offset < IO_SERVO_COUNT) && (num_values > 0)) {
 
 			/* XXX range-check value? */
 			r_page_servo_failsafe[offset] = *values;
 
+			/* flag the failsafe values as custom */
+			r_setup_arming |= PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM;
+
 			offset++;
 			num_values--;
 			values++;

From 5f2571dd01c90ba1209d263c52d818225644ce07 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 29 May 2013 18:29:41 +0200
Subject: [PATCH 09/17] Set unknown channels to zero, since centering them is a
 slightly dangerous guess

---
 src/drivers/px4io/px4io.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 873d7eb82a..0a31831f09 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -1758,7 +1758,8 @@ px4io_main(int argc, char *argv[])
 						errx(1, "value out of range of 800 < value < 2200. Aborting.");
 					}
 				} else {
-					failsafe[i] = 1500;
+					/* a zero value will result in stopping to output any pulse */
+					failsafe[i] = 0;
 				}
 			}
 

From abb024c724435937c1a0ae707dccfa28a48ef559 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 29 May 2013 18:32:23 +0200
Subject: [PATCH 10/17] More safety added by disabling pulses

---
 src/modules/px4iofirmware/registers.c | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 49a7233528..df7d6dcd35 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -179,8 +179,10 @@ uint16_t		r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE
  * PAGE 105
  *
  * Failsafe servo PWM values
+ * 
+ * Disable pulses as default.
  */
-uint16_t		r_page_servo_failsafe[IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, 900, 900, 900 };
+uint16_t		r_page_servo_failsafe[IO_SERVO_COUNT] = { 0 };
 
 void
 registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)

From 1addb9f6c56f68f600acd4bdd609ef14697bda44 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Sat, 1 Jun 2013 20:42:43 +0400
Subject: [PATCH 11/17] Fixed bug in UBX::configure_message_rate()

---
 src/drivers/gps/ubx.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp
index b3093b0f6f..f2e7ca67dc 100644
--- a/src/drivers/gps/ubx.cpp
+++ b/src/drivers/gps/ubx.cpp
@@ -739,7 +739,7 @@ UBX::configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate)
     msg.msg_class	= msg_class;
     msg.msg_id		= msg_id;
     msg.rate		= rate;
-    send_message(CFG, UBX_MESSAGE_CFG_MSG, &msg, sizeof(msg));
+    send_message(UBX_CLASS_CFG, UBX_MESSAGE_CFG_MSG, &msg, sizeof(msg));
 }
 
 void

From f435025d26f49d1d9069282aa72c7e1cb9201773 Mon Sep 17 00:00:00 2001
From: Simon Wilks <sjwilks@gmail.com>
Date: Tue, 4 Jun 2013 00:10:58 +0200
Subject: [PATCH 12/17] Completed main implementation and debugging

---
 .../hott_telemetry/hott_telemetry_main.c      |   3 +
 src/drivers/hott_telemetry/messages.c         | 194 ++++++++++--------
 src/drivers/hott_telemetry/messages.h         |   1 +
 .../att_pos_estimator_ekf/KalmanNav.cpp       |   4 +-
 src/modules/systemlib/geo/geo.c               |   8 +-
 src/modules/systemlib/geo/geo.h               |  16 ++
 6 files changed, 133 insertions(+), 93 deletions(-)

diff --git a/src/drivers/hott_telemetry/hott_telemetry_main.c b/src/drivers/hott_telemetry/hott_telemetry_main.c
index 827cb862ad..4318244f81 100644
--- a/src/drivers/hott_telemetry/hott_telemetry_main.c
+++ b/src/drivers/hott_telemetry/hott_telemetry_main.c
@@ -138,6 +138,7 @@ recv_req_id(int uart, uint8_t *id)
 
 		/* if we have a binary mode request */
 		if (mode != BINARY_MODE_REQUEST_ID) {
+			warnx("Non binary request ID detected: %d", mode);
 			return ERROR;
 		}
 
@@ -232,6 +233,8 @@ hott_telemetry_thread_main(int argc, char *argv[])
 			}
 
 			send_data(uart, buffer, size);
+		} else {
+			warnx("NOK");
 		}
 	}
 
diff --git a/src/drivers/hott_telemetry/messages.c b/src/drivers/hott_telemetry/messages.c
index feec8998c5..1d746506e0 100644
--- a/src/drivers/hott_telemetry/messages.c
+++ b/src/drivers/hott_telemetry/messages.c
@@ -40,8 +40,9 @@
 #include "messages.h"
 
 #include <math.h>
+#include <stdio.h>
 #include <string.h>
-#include <systemlib/systemlib.h>
+#include <systemlib/geo/geo.h>
 #include <unistd.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/home_position.h>
@@ -51,11 +52,17 @@
 /* The board is very roughly 5 deg warmer than the surrounding air */
 #define BOARD_TEMP_OFFSET_DEG 5
 
+#define ALT_OFFSET 500.0f
+
 static int battery_sub = -1;
 static int gps_sub = -1;
 static int home_sub = -1;
 static int sensor_sub = -1;
 
+static bool home_position_set = false;
+static double home_lat = 0.0d;
+static double home_lon = 0.0d;
+
 void 
 messages_init(void)
 {
@@ -69,15 +76,18 @@ void
 build_eam_response(uint8_t *buffer, size_t *size)
 {
 	/* get a local copy of the current sensor values */
-	struct sensor_combined_s raw = { 0 };
+	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 = { 0 };
+	struct battery_status_s battery;
+	memset(&battery, 0, sizeof(battery));
 	orb_copy(ORB_ID(battery_status), battery_sub, &battery);
 
-	struct eam_module_msg msg = { 0 };
+	struct eam_module_msg msg;
 	*size = sizeof(msg);
+	memset(&msg, 0, *size);
 
 	msg.start = START_BYTE;
 	msg.eam_sensor_id = EAM_SENSOR_ID;
@@ -92,12 +102,7 @@ build_eam_response(uint8_t *buffer, size_t *size)
 	msg.altitude_L = (uint8_t)alt & 0xff;
 	msg.altitude_H = (uint8_t)(alt >> 8) & 0xff;
 
-	// TODO: flight time
-	// TODO: climb rate
-	
-
 	msg.stop = STOP_BYTE;
-
 	memcpy(buffer, &msg, *size);
 }
 
@@ -105,102 +110,113 @@ void
 build_gps_response(uint8_t *buffer, size_t *size)
 {
 	/* get a local copy of the current sensor values */
-	struct sensor_combined_s raw = { 0 };
+	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 = { 0 };
+	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 = { 0 };
 	*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;
 
-	/* 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.6);
-	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;
-
-	msg.latitude_ns = 0;
-	if (lat < 0) {
-		msg.latitude_ns = 1;
-		lat = -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;
-
-	msg.longitude_ew = 0;
-	if (lon < 0) {
-		msg.longitude_ew = 1;
-		lon = -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-3 + 500.0f);
-	msg.altitude_L = (uint8_t)alt & 0xff;
-	msg.altitude_H = (uint8_t)(alt >> 8) & 0xff;
-
-	/* Distance from home */
-	bool updated;
-	orb_check(home_sub, &updated);
-	if (updated) {
-	    /* get a local copy of the home position data */
-		struct home_position_s home = { 0 };
-		orb_copy(ORB_ID(home_position), home_sub, &home);
-
-	    uint16_t dist = (uint16_t)get_distance_to_next_waypoint(
-	    	(double)home.lat*1e-7, (double)home.lon*1e-7, lat, lon);
-	    warnx("dist %d home.lat %3.6f home.lon %3.6f lat %3.6f lon %3.6f ", 
-	    	  dist, (double)home.lat*1e-7d, (double)home.lon*1e-7d, 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(
-	    	(double)home.lat*1e-7, (double)home.lon*1e-7, lat, lon) * M_RAD_TO_DEG_F;
-		msg.home_direction = (uint8_t)bearing >> 1;
-    }
-
 	msg.gps_num_sat = gps.satellites_visible;
 
 	/* 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);
 
-	msg.stop = STOP_BYTE;
+	/* 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.6);
+		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;
+
+		/* Prepend 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;
+
+		/* Prepend 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-3 + ALT_OFFSET	);
+		msg.altitude_L = (uint8_t)alt & 0xff;
+		msg.altitude_H = (uint8_t)(alt >> 8) & 0xff;
+
+		/* Get any (and probably only ever one) home_sub postion 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 = ((double)(home.lat))*1e-7d;
+			home_lon = ((double)(home.lon))*1e-7d;
+			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);
 }
 
@@ -210,6 +226,8 @@ convert_to_degrees_minutes_seconds(double val, int *deg, int *min, int *sec)
     *deg = (int)val;
 
     double delta = val - *deg;
-    *min = (int)(delta * 60.0);
-    *sec = (int)(delta * 3600.0);
+    const double min_d = delta * 60.0d;
+    *min = (int)min_d;
+    delta = min_d - *min;
+    *sec = (int)(delta * 10000.0d);
 }
diff --git a/src/drivers/hott_telemetry/messages.h b/src/drivers/hott_telemetry/messages.h
index 5f7dd5c145..e6d5cc7239 100644
--- a/src/drivers/hott_telemetry/messages.h
+++ b/src/drivers/hott_telemetry/messages.h
@@ -189,6 +189,7 @@ struct gps_module_msg {
 void messages_init(void);
 void build_eam_response(uint8_t *buffer, size_t *size);
 void build_gps_response(uint8_t *buffer, size_t *size);
+float _get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
 void convert_to_degrees_minutes_seconds(double lat, int *deg, int *min, int *sec);
 
 
diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
index 4ef150da1e..c3836bdfab 100644
--- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
+++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
@@ -683,7 +683,8 @@ int KalmanNav::correctPos()
 	// fault detetcion
 	float beta = y.dot(S.inverse() * y);
 
-	if (beta > _faultPos.get()) {
+	static int counter = 0;
+	if (beta > _faultPos.get() && (counter % 10 == 0)) {
 		printf("fault in gps: beta = %8.4f\n", (double)beta);
 		printf("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n",
 		       double(y(0) / sqrtf(RPos(0, 0))),
@@ -693,6 +694,7 @@ int KalmanNav::correctPos()
 		       double(y(4) / sqrtf(RPos(4, 4))),
 		       double(y(5) / sqrtf(RPos(5, 5))));
 	}
+	counter++;
 
 	return ret_ok;
 }
diff --git a/src/modules/systemlib/geo/geo.c b/src/modules/systemlib/geo/geo.c
index 6746e8ff36..3ac4bd168c 100644
--- a/src/modules/systemlib/geo/geo.c
+++ b/src/modules/systemlib/geo/geo.c
@@ -46,7 +46,7 @@
 
 #include <systemlib/geo/geo.h>
 #include <nuttx/config.h>
-#include <unistd.h>
+//#include <unistd.h>
 #include <pthread.h>
 #include <stdio.h>
 #include <math.h>
@@ -185,12 +185,12 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou
 	double d_lat = lat_next_rad - lat_now_rad;
 	double d_lon = lon_next_rad - lon_now_rad;
 
-	double a = sin(d_lat / 2.0d) * sin(d_lat / 2.0) + sin(d_lon / 2.0d) * sin(d_lon / 2.0d) * cos(lat_now_rad) * cos(lat_next_rad);
+	double a = sin(d_lat / 2.0d) * sin(d_lat / 2.0d) + sin(d_lon / 2.0d) * sin(d_lon / 2.0d) * cos(lat_now_rad) * cos(lat_next_rad);
 	double c = 2.0d * atan2(sqrt(a), sqrt(1.0d - a));
 
 	const double radius_earth = 6371000.0d;
-
-	return radius_earth * c;
+	printf("DIST: %.4f\n", radius_earth * c);
+	return radius_earth * c; 
 }
 
 __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)
diff --git a/src/modules/systemlib/geo/geo.h b/src/modules/systemlib/geo/geo.h
index 0c0b5c533e..84097b49f7 100644
--- a/src/modules/systemlib/geo/geo.h
+++ b/src/modules/systemlib/geo/geo.h
@@ -82,8 +82,24 @@ __EXPORT void map_projection_project(double lat, double lon, float *x, float *y)
  */
 __EXPORT void map_projection_reproject(float x, float y, double *lat, double *lon);
 
+/**
+ * Returns the distance to the next waypoint in meters.
+ *
+ * @param lat_now current position in degrees (47.1234567°, not 471234567°)
+ * @param lon_now current position in degrees (8.1234567°, not 81234567°)
+ * @param lat_next next waypoint position in degrees (47.1234567°, not 471234567°)
+ * @param lon_next next waypoint position in degrees (8.1234567°, not 81234567°)
+ */
 __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
 
+/**
+ * Returns the bearing to the next waypoint in radians.
+ *
+ * @param lat_now current position in degrees (47.1234567°, not 471234567°)
+ * @param lon_now current position in degrees (8.1234567°, not 81234567°)
+ * @param lat_next next waypoint position in degrees (47.1234567°, not 471234567°)
+ * @param lon_next next waypoint position in degrees (8.1234567°, not 81234567°)
+ */
 __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
 
 __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end);

From 30d17cf0ba9da7587a2087674c37cda5399ab851 Mon Sep 17 00:00:00 2001
From: Simon Wilks <sjwilks@gmail.com>
Date: Tue, 4 Jun 2013 00:18:23 +0200
Subject: [PATCH 13/17] Fix whitespace

---
 src/drivers/hott_telemetry/messages.c | 18 +++++++++---------
 1 file changed, 9 insertions(+), 9 deletions(-)

diff --git a/src/drivers/hott_telemetry/messages.c b/src/drivers/hott_telemetry/messages.c
index 1d746506e0..e10a22dc45 100644
--- a/src/drivers/hott_telemetry/messages.c
+++ b/src/drivers/hott_telemetry/messages.c
@@ -193,7 +193,7 @@ build_gps_response(uint8_t *buffer, size_t *size)
 		bool updated;
 		orb_check(home_sub, &updated);
 		if (updated) {
-		    /* get a local copy of the home position data */
+			/* 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);
@@ -205,7 +205,7 @@ build_gps_response(uint8_t *buffer, size_t *size)
 
 		/* Distance from home */
 		if (home_position_set) {
-		    uint16_t dist = (uint16_t)get_distance_to_next_waypoint(home_lat, home_lon, lat, lon);
+			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;
@@ -213,7 +213,7 @@ build_gps_response(uint8_t *buffer, size_t *size)
 			/* 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;
@@ -223,11 +223,11 @@ build_gps_response(uint8_t *buffer, size_t *size)
 void
 convert_to_degrees_minutes_seconds(double val, int *deg, int *min, int *sec)
 {
-    *deg = (int)val;
+	*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);
+	double delta = val - *deg;
+	const double min_d = delta * 60.0d;
+	*min = (int)min_d;
+	delta = min_d - *min;
+	*sec = (int)(delta * 10000.0d);
 }

From 9374e4b1f221d571d56686d7d92ecb6cabcca8b6 Mon Sep 17 00:00:00 2001
From: Simon Wilks <sjwilks@gmail.com>
Date: Tue, 4 Jun 2013 00:52:48 +0200
Subject: [PATCH 14/17] Formatting and comments

---
 src/drivers/hott_telemetry/messages.c | 8 +++-----
 1 file changed, 3 insertions(+), 5 deletions(-)

diff --git a/src/drivers/hott_telemetry/messages.c b/src/drivers/hott_telemetry/messages.c
index e10a22dc45..369070f8c4 100644
--- a/src/drivers/hott_telemetry/messages.c
+++ b/src/drivers/hott_telemetry/messages.c
@@ -52,8 +52,6 @@
 /* The board is very roughly 5 deg warmer than the surrounding air */
 #define BOARD_TEMP_OFFSET_DEG 5
 
-#define ALT_OFFSET 500.0f
-
 static int battery_sub = -1;
 static int gps_sub = -1;
 static int home_sub = -1;
@@ -146,7 +144,7 @@ build_gps_response(uint8_t *buffer, size_t *size)
 		/* Get latitude in degrees, minutes and seconds */
 		double lat = ((double)(gps.lat))*1e-7d;
 
-		/* Prepend N or S specifier */		
+		/* Set the N or S specifier */		
 		msg.latitude_ns = 0;
 		if (lat < 0) {
 			msg.latitude_ns = 1;
@@ -168,7 +166,7 @@ build_gps_response(uint8_t *buffer, size_t *size)
 		/* Get longitude in degrees, minutes and seconds */
 		double lon = ((double)(gps.lon))*1e-7d;
 
-		/* Prepend E or W specifier */
+		/* Set the E or W specifier */
 		msg.longitude_ew = 0;
 		if (lon < 0) {
 			msg.longitude_ew = 1;
@@ -185,7 +183,7 @@ build_gps_response(uint8_t *buffer, size_t *size)
 		msg.longitude_sec_H = (uint8_t)(lon_sec >> 8) & 0xff;
 		
 		/* Altitude */
-		uint16_t alt = (uint16_t)(gps.alt*1e-3 + ALT_OFFSET	);
+		uint16_t alt = (uint16_t)(gps.alt*1e-3 + 500.0f);
 		msg.altitude_L = (uint8_t)alt & 0xff;
 		msg.altitude_H = (uint8_t)(alt >> 8) & 0xff;
 

From 82c7e58122992aab2cf698951f9a33817cf1a050 Mon Sep 17 00:00:00 2001
From: Simon Wilks <sjwilks@gmail.com>
Date: Tue, 4 Jun 2013 01:03:16 +0200
Subject: [PATCH 15/17] Removed some debugging code

---
 src/modules/systemlib/geo/geo.c | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/src/modules/systemlib/geo/geo.c b/src/modules/systemlib/geo/geo.c
index 3ac4bd168c..6463e6489e 100644
--- a/src/modules/systemlib/geo/geo.c
+++ b/src/modules/systemlib/geo/geo.c
@@ -46,7 +46,7 @@
 
 #include <systemlib/geo/geo.h>
 #include <nuttx/config.h>
-//#include <unistd.h>
+#include <unistd.h>
 #include <pthread.h>
 #include <stdio.h>
 #include <math.h>
@@ -189,7 +189,6 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou
 	double c = 2.0d * atan2(sqrt(a), sqrt(1.0d - a));
 
 	const double radius_earth = 6371000.0d;
-	printf("DIST: %.4f\n", radius_earth * c);
 	return radius_earth * c; 
 }
 

From 45fe45fefa7cff5c9799037ee024671b4493ab85 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 4 Jun 2013 13:32:57 +0200
Subject: [PATCH 16/17] Better error handling for too large arguments

---
 src/drivers/px4io/px4io.cpp | 11 ++++++++---
 1 file changed, 8 insertions(+), 3 deletions(-)

diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 0a31831f09..0934e614b3 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -702,10 +702,12 @@ PX4IO::set_failsafe_values(const uint16_t *vals, unsigned len)
 {
 	uint16_t 		regs[_max_actuators];
 
-	unsigned max = (len < _max_actuators) ? len : _max_actuators;
+	if (len > _max_actuators)
+		/* fail with error */
+		return E2BIG;
 
 	/* copy values to registers in IO */
-	return io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, vals, max);
+	return io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, vals, len);
 }
 
 int
@@ -1763,7 +1765,10 @@ px4io_main(int argc, char *argv[])
 				}
 			}
 
-			g_dev->set_failsafe_values(failsafe, sizeof(failsafe) / sizeof(failsafe[0]));
+			int ret = g_dev->set_failsafe_values(failsafe, sizeof(failsafe) / sizeof(failsafe[0]));
+
+			if (ret != OK)
+				errx(ret, "failed setting failsafe values");
 		} else {
 			errx(1, "not loaded");
 		}

From de82295ab5307bca0fbd2266fdd1547386fa19a8 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 4 Jun 2013 14:13:02 +0200
Subject: [PATCH 17/17] HOTFIX: Allow PWM command to correctly set ARM_OK flag

---
 src/systemcmds/pwm/pwm.c | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c
index ff733df52f..e150b5a74b 100644
--- a/src/systemcmds/pwm/pwm.c
+++ b/src/systemcmds/pwm/pwm.c
@@ -185,12 +185,18 @@ pwm_main(int argc, char *argv[])
 		const char *arg = argv[0];
 		argv++;
 		if (!strcmp(arg, "arm")) {
+			/* tell IO that its ok to disable its safety with the switch */
+			ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0);
+			if (ret != OK)
+				err(1, "PWM_SERVO_SET_ARM_OK");
+			/* tell IO that the system is armed (it will output values if safety is off) */
 			ret = ioctl(fd, PWM_SERVO_ARM, 0);
 			if (ret != OK)
 				err(1, "PWM_SERVO_ARM");
 			continue;
 		}
 		if (!strcmp(arg, "disarm")) {
+			/* disarm, but do not revoke the SET_ARM_OK flag */
 			ret = ioctl(fd, PWM_SERVO_DISARM, 0);
 			if (ret != OK)
 				err(1, "PWM_SERVO_DISARM");