Merged master into mixer unit tests branch

2026-05-25 08:36:08 +08:00 · 2014-01-15 07:43:17 +01:00
parent 9612514a3f cd72f564ef
commit f3cd83e804
69 changed files with 2886 additions and 1044 deletions
@@ -75,14 +75,15 @@ then
 	#
 	# Load microSD params
 	#
-	#if ramtron start
-	#then
-	#	param select /ramtron/params
-	#	if [ -f /ramtron/params ]
-	#	then
-	#		param load /ramtron/params
-	#	fi
-	#else
+	if mtd start
+	then
+		param select /fs/mtd_params
+		if param load /fs/mtd_params
+		then
+		else
+			echo "FAILED LOADING PARAMS"
+		fi
+	else
 		param select /fs/microsd/params
 		if [ -f /fs/microsd/params ]
 		then
@@ -93,7 +94,7 @@ then
 				echo "Parameter file corrupt - ignoring"
 			fi
 		fi
-	#fi
+	fi
 	
 	#
 	# Start system state indicator
@@ -213,7 +214,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 +280,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
@@ -2,6 +2,7 @@
 #
 # PX4FMU startup script for test hackery.
 #
+uorb start

 if sercon
 then
@@ -9,4 +10,68 @@ then

 	# Try to get an USB console
 	nshterm /dev/ttyACM0 &
-fi
+fi
+
+#
+# Try to mount the microSD card.
+#
+echo "[init] looking for microSD..."
+if mount -t vfat /dev/mmcsd0 /fs/microsd
+then
+	echo "[init] card mounted at /fs/microsd"
+	# Start playing the startup tune
+	tone_alarm start
+else
+	echo "[init] no microSD card found"
+	# Play SOS
+	tone_alarm error
+fi
+
+#
+# Start a minimal system
+#
+
+if [ -f /etc/extras/px4io-v2_default.bin ]
+then
+	set io_file /etc/extras/px4io-v2_default.bin
+else
+	set io_file /etc/extras/px4io-v1_default.bin
+fi
+
+if px4io start
+then
+	echo "PX4IO OK"
+fi
+
+if px4io checkcrc $io_file
+then
+	echo "PX4IO CRC OK"
+else
+	echo "PX4IO CRC failure"
+	tone_alarm MBABGP
+	if px4io forceupdate 14662 $io_file
+	then
+		usleep 500000
+		if px4io start
+		then
+			echo "PX4IO restart OK"
+			tone_alarm MSPAA
+		else
+			echo "PX4IO restart failed"
+			tone_alarm MNGGG
+			sleep 5
+			reboot
+		fi
+	else
+		echo "PX4IO update failed"
+		tone_alarm MNGGG
+	fi
+fi
+
+#
+# The presence of this file suggests we're running a mount stress test
+#
+if [ -f /fs/microsd/mount_test_cmds.txt ]
+then
+	tests mount
+fi
@@ -5,23 +5,33 @@ class DokuWikiOutput(output.Output):
        result = ""
        for group in groups:
            result += "==== %s ====\n\n" % group.GetName()
+            result += "^ Name            ^ Description    ^ Min    ^ Max    ^ Default   ^ Comment    ^\n"
            for param in group.GetParams():
                code = param.GetFieldValue("code")
                name = param.GetFieldValue("short_desc")
-                if code != name:
-                    name = "%s (%s)" % (name, code)
-                result += "=== %s ===\n\n" % name
-                long_desc = param.GetFieldValue("long_desc")
-                if long_desc is not None:
-                    result += "%s\n\n" % long_desc
+                name = name.replace("\n", "")
+                result += "| %s   | %s  " % (code, name)
                min_val = param.GetFieldValue("min")
                if min_val is not None:
-                    result += "* Minimal value: %s\n" % min_val
+                    result += "| %s  " % min_val
+                else:
+                    result += "|"
                max_val = param.GetFieldValue("max")
                if max_val is not None:
-                    result += "* Maximal value: %s\n" % max_val
+                    result += "| %s  " % max_val
+                else:
+                    result += "|"
                def_val = param.GetFieldValue("default")
                if def_val is not None:
-                    result += "* Default value: %s\n" % def_val
-                result += "\n"
+                    result += "| %s  " % def_val
+                else:
+                    result += "|"
+                long_desc = param.GetFieldValue("long_desc")
+                if long_desc is not None:
+                    long_desc = long_desc.replace("\n", "")
+                    result += "| %s  " % long_desc
+                else:
+                    result += "|"
+                result += "|\n"
+            result += "\n"
        return result
@@ -0,0 +1,27 @@
+import output
+
+class DokuWikiOutput(output.Output):
+    def Generate(self, groups):
+        result = ""
+        for group in groups:
+            result += "==== %s ====\n\n" % group.GetName()
+            for param in group.GetParams():
+                code = param.GetFieldValue("code")
+                name = param.GetFieldValue("short_desc")
+                if code != name:
+                    name = "%s (%s)" % (name, code)
+                result += "=== %s ===\n\n" % name
+                long_desc = param.GetFieldValue("long_desc")
+                if long_desc is not None:
+                    result += "%s\n\n" % long_desc
+                min_val = param.GetFieldValue("min")
+                if min_val is not None:
+                    result += "* Minimal value: %s\n" % min_val
+                max_val = param.GetFieldValue("max")
+                if max_val is not None:
+                    result += "* Maximal value: %s\n" % max_val
+                def_val = param.GetFieldValue("default")
+                if def_val is not None:
+                    result += "* Default value: %s\n" % def_val
+                result += "\n"
+        return result
@@ -25,14 +25,10 @@ MODULES		+= drivers/bma180
 MODULES		+= drivers/mpu6000
 MODULES		+= drivers/hmc5883
 MODULES		+= drivers/ms5611
-MODULES		+= drivers/mb12xx
 MODULES		+= drivers/gps
 MODULES		+= drivers/hil
-MODULES		+= drivers/hott/hott_telemetry
-MODULES		+= drivers/hott/hott_sensors
 MODULES		+= drivers/blinkm
 MODULES		+= drivers/rgbled
-MODULES		+= drivers/mkblctrl
 MODULES		+= drivers/roboclaw
 MODULES		+= drivers/airspeed
 MODULES		+= drivers/ets_airspeed
@@ -42,8 +38,7 @@ MODULES		+= modules/sensors
 #
 # System commands
 #
-MODULES		+= systemcmds/eeprom
-MODULES		+= systemcmds/ramtron
+MODULES		+= systemcmds/mtd
 MODULES		+= systemcmds/bl_update
 MODULES		+= systemcmds/boardinfo
 MODULES		+= systemcmds/i2c
@@ -71,22 +66,11 @@ MODULES		+= modules/gpio_led
 #
 # Estimation modules (EKF/ SO3 / other filters)
 #
-#MODULES		+= modules/attitude_estimator_ekf
 MODULES		+= modules/att_pos_estimator_ekf
-#MODULES		+= modules/position_estimator_inav
-MODULES		+= examples/flow_position_estimator
-MODULES		+= modules/attitude_estimator_so3

 #
 # Vehicle Control
 #
-#MODULES		+= modules/segway # XXX Needs GCC 4.7 fix
-#MODULES		+= modules/fw_pos_control_l1
-#MODULES		+= modules/fw_att_control
-#MODULES		+= modules/multirotor_att_control
-#MODULES		+= modules/multirotor_pos_control
-#MODULES		+= examples/flow_position_control
-#MODULES		+= examples/flow_speed_control
 MODULES 		+= modules/fixedwing_backside

 #
@@ -21,7 +21,7 @@ MODULES		+= drivers/px4fmu
 MODULES		+= drivers/boards/px4fmu-v1
 MODULES		+= drivers/ardrone_interface
 MODULES		+= drivers/l3gd20
-MODULES		+= drivers/bma180
+#MODULES		+= drivers/bma180
 MODULES		+= drivers/mpu6000
 MODULES		+= drivers/hmc5883
 MODULES		+= drivers/ms5611
@@ -33,17 +33,16 @@ MODULES		+= drivers/hott/hott_sensors
 MODULES		+= drivers/blinkm
 MODULES		+= drivers/rgbled
 MODULES		+= drivers/mkblctrl
-MODULES		+= drivers/roboclaw
 MODULES		+= drivers/airspeed
 MODULES		+= drivers/ets_airspeed
 MODULES		+= drivers/meas_airspeed
+MODULES		+= drivers/frsky_telemetry
 MODULES		+= modules/sensors

 #
 # System commands
 #
-MODULES		+= systemcmds/eeprom
-MODULES		+= systemcmds/ramtron
+MODULES		+= systemcmds/mtd
 MODULES		+= systemcmds/bl_update
 MODULES		+= systemcmds/boardinfo
 MODULES		+= systemcmds/i2c
@@ -75,18 +74,17 @@ MODULES		+= modules/attitude_estimator_ekf
 MODULES		+= modules/attitude_estimator_so3
 MODULES		+= modules/att_pos_estimator_ekf
 MODULES		+= modules/position_estimator_inav
-MODULES		+= examples/flow_position_estimator
+#MODULES		+= examples/flow_position_estimator

 #
 # Vehicle Control
 #
-#MODULES		+= modules/segway # XXX Needs GCC 4.7 fix
 MODULES		+= modules/fw_pos_control_l1
 MODULES		+= modules/fw_att_control
 MODULES		+= modules/multirotor_att_control
 MODULES		+= modules/multirotor_pos_control
-MODULES		+= examples/flow_position_control
-MODULES		+= examples/flow_speed_control
+#MODULES		+= examples/flow_position_control
+#MODULES		+= examples/flow_speed_control

 #
 # Logging
@@ -0,0 +1,49 @@
+#
+# Makefile for the px4fmu_default configuration
+#
+
+#
+# Use the configuration's ROMFS.
+#
+ROMFS_ROOT	 = $(PX4_BASE)/ROMFS/px4fmu_test
+ROMFS_OPTIONAL_FILES = $(PX4_BASE)/Images/px4io-v1_default.bin
+
+#
+# Board support modules
+#
+MODULES		+= drivers/device
+MODULES		+= drivers/stm32
+MODULES		+= drivers/stm32/adc
+MODULES		+= drivers/stm32/tone_alarm
+MODULES		+= drivers/led
+MODULES		+= drivers/boards/px4fmu-v1
+MODULES		+= drivers/px4io
+MODULES		+= systemcmds/perf
+MODULES		+= systemcmds/reboot
+MODULES		+= systemcmds/tests
+MODULES		+= systemcmds/nshterm
+MODULES		+= systemcmds/mtd
+
+#
+# Library modules
+#
+MODULES		+= modules/systemlib
+MODULES		+= modules/systemlib/mixer
+MODULES		+= modules/uORB
+
+#
+# Transitional support - add commands from the NuttX export archive.
+#
+# In general, these should move to modules over time.
+#
+# Each entry here is <command>.<priority>.<stacksize>.<entrypoint> but we use a helper macro
+# to make the table a bit more readable.
+#
+define _B
+	$(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4)
+endef
+
+#                  command                 priority                   stack  entrypoint
+BUILTIN_COMMANDS := \
+	$(call _B, sercon,                 ,                          2048,  sercon_main                ) \
+	$(call _B, serdis,                 ,                          2048,  serdis_main                )
@@ -36,6 +36,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,
@@ -45,7 +46,6 @@ MODULES		+= modules/sensors
 #
 # System commands
 #
-MODULES		+= systemcmds/ramtron
 MODULES		+= systemcmds/bl_update
 MODULES		+= systemcmds/boardinfo
 MODULES		+= systemcmds/mixer
@@ -59,6 +59,7 @@ MODULES		+= systemcmds/top
 MODULES		+= systemcmds/tests
 MODULES		+= systemcmds/config
 MODULES		+= systemcmds/nshterm
+MODULES		+= systemcmds/mtd

 #
 # General system control
@@ -6,21 +6,29 @@
 # Use the configuration's ROMFS.
 #
 ROMFS_ROOT	 = $(PX4_BASE)/ROMFS/px4fmu_test
+ROMFS_OPTIONAL_FILES = $(PX4_BASE)/Images/px4io-v2_default.bin

 #
 # Board support modules
 #
 MODULES		+= drivers/device
 MODULES		+= drivers/stm32
+MODULES		+= drivers/stm32/adc
+MODULES		+= drivers/stm32/tone_alarm
 MODULES		+= drivers/led
 MODULES		+= drivers/boards/px4fmu-v2
+MODULES		+= drivers/px4io
 MODULES		+= systemcmds/perf
 MODULES		+= systemcmds/reboot
+MODULES		+= systemcmds/tests
+MODULES		+= systemcmds/nshterm
+MODULES		+= systemcmds/mtd

 #
 # Library modules
 #
 MODULES		+= modules/systemlib
+MODULES		+= modules/systemlib/mixer
 MODULES		+= modules/uORB

 #
@@ -138,8 +138,7 @@ ARCHCWARNINGS		 = $(ARCHWARNINGS) \
 			   -Wold-style-declaration \
 			   -Wmissing-parameter-type \
 			   -Wmissing-prototypes \
-			   -Wnested-externs \
-			   -Wunsuffixed-float-constants
+			   -Wnested-externs

 # C++-specific warnings
 #
@@ -460,7 +460,7 @@ CONFIG_MMCSD_NSLOTS=1
 CONFIG_MMCSD_SPI=y
 CONFIG_MMCSD_SPICLOCK=24000000
 # CONFIG_MMCSD_SDIO is not set
-# CONFIG_MTD is not set
+CONFIG_MTD=y
 CONFIG_PIPES=y
 # CONFIG_PM is not set
 # CONFIG_POWER is not set
@@ -482,6 +482,25 @@ CONFIG_USART1_SERIAL_CONSOLE=y
 # CONFIG_USART6_SERIAL_CONSOLE is not set
 # CONFIG_NO_SERIAL_CONSOLE is not set

+#
+# MTD Configuration
+#
+CONFIG_MTD_PARTITION=y
+CONFIG_MTD_BYTE_WRITE=y
+
+#
+# MTD Device Drivers
+#
+# CONFIG_RAMMTD is not set
+# CONFIG_MTD_AT24XX is not set
+# CONFIG_MTD_AT45DB is not set
+# CONFIG_MTD_M25P is not set
+# CONFIG_MTD_SMART is not set
+# CONFIG_MTD_RAMTRON is not set
+# CONFIG_MTD_SST25 is not set
+# CONFIG_MTD_SST39FV is not set
+# CONFIG_MTD_W25 is not set
+
 #
 # USART1 Configuration
 #
@@ -566,7 +585,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"
@@ -295,16 +295,16 @@ 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
 CONFIG_USART3_RXDMA=y
 # CONFIG_UART4_RS485 is not set
 CONFIG_UART4_RXDMA=y
-# CONFIG_UART5_RXDMA is not set
+CONFIG_UART5_RXDMA=y
 # 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
@@ -500,8 +500,8 @@ CONFIG_MTD=y
 #
 # MTD Configuration
 #
-# CONFIG_MTD_PARTITION is not set
-# CONFIG_MTD_BYTE_WRITE is not set
+CONFIG_MTD_PARTITION=y
+CONFIG_MTD_BYTE_WRITE=y

 #
 # MTD Device Drivers
@@ -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"
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-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
@@ -34,7 +34,7 @@
 /**
 * @file board_config.h
 *
- * PX4FMU internal definitions
+ * PX4FMUv1 internal definitions
 */

 #pragma once
@@ -60,6 +60,7 @@ __BEGIN_DECLS

 /* PX4IO connection configuration */
 #define PX4IO_SERIAL_DEVICE	"/dev/ttyS2"
+#define UDID_START		0x1FFF7A10

 //#ifdef CONFIG_STM32_SPI2
 //#  error "SPI2 is not supported on this board"
@@ -180,7 +181,7 @@ __BEGIN_DECLS
 #define HRT_TIMER		1	/* use timer1 for the HRT */
 #define HRT_TIMER_CHANNEL	1	/* use capture/compare channel */
 #define HRT_PPM_CHANNEL		3	/* use capture/compare channel 3 */
-#define GPIO_PPM_IN		(GPIO_ALT|GPIO_AF1|GPIO_SPEED_50MHz|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN10)
+#define GPIO_PPM_IN		(GPIO_ALT|GPIO_AF1|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN10)

 /****************************************************************************************************
 * Public Types
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (C) 2012 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
@@ -34,7 +34,7 @@
 /**
 * @file board_config.h
 *
- * PX4FMU internal definitions
+ * PX4FMUv2 internal definitions
 */

 #pragma once
@@ -52,6 +52,8 @@ __BEGIN_DECLS
 /* these headers are not C++ safe */
 #include <stm32.h>
 #include <arch/board/board.h>
+
+#define UDID_START		0x1FFF7A10
 
 /****************************************************************************************************
 * Definitions
@@ -82,21 +84,21 @@ __BEGIN_DECLS
 #define GPIO_EXTI_MPU_DRDY	(GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15)

 /* Data ready pins off */
-#define GPIO_GYRO_DRDY_OFF                (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN0)
-#define GPIO_MAG_DRDY_OFF                (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN1)
-#define GPIO_ACCEL_DRDY_OFF                (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN4)
-#define GPIO_EXTI_MPU_DRDY	(GPIO_INPUT|GPIO_PULLDOWN|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15)
+#define GPIO_GYRO_DRDY_OFF	(GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN0)
+#define GPIO_MAG_DRDY_OFF	(GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN1)
+#define GPIO_ACCEL_DRDY_OFF	(GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN4)
+#define GPIO_EXTI_MPU_DRDY_OFF	(GPIO_INPUT|GPIO_PULLDOWN|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15)

 /* SPI1 off */
-#define GPIO_SPI1_SCK_OFF        (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN5)
-#define GPIO_SPI1_MISO_OFF        (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN6)
-#define GPIO_SPI1_MOSI_OFF        (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN7)
+#define GPIO_SPI1_SCK_OFF	(GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN5)
+#define GPIO_SPI1_MISO_OFF	(GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN6)
+#define GPIO_SPI1_MOSI_OFF	(GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN7)

 /* SPI1 chip selects off */
-#define GPIO_SPI_CS_GYRO_OFF                (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN13)
-#define GPIO_SPI_CS_ACCEL_MAG_OFF        (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN15)
-#define GPIO_SPI_CS_BARO_OFF                (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTD|GPIO_PIN7)
-#define GPIO_SPI_CS_MPU		(GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN2)
+#define GPIO_SPI_CS_GYRO_OFF		(GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN13)
+#define GPIO_SPI_CS_ACCEL_MAG_OFF	(GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN15)
+#define GPIO_SPI_CS_BARO_OFF		(GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTD|GPIO_PIN7)
+#define GPIO_SPI_CS_MPU_OFF		(GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN2)

 /* SPI chip selects */
 #define GPIO_SPI_CS_GYRO	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)
@@ -177,7 +179,7 @@ __BEGIN_DECLS
 *
 * PA9  OTG_FS_VBUS VBUS sensing (also connected to the green LED)
 */
-#define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9)
+#define GPIO_OTGFS_VBUS		(GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9)

 /* High-resolution timer */
 #define HRT_TIMER		8	/* use timer8 for the HRT */
@@ -282,7 +282,7 @@ __EXPORT int nsh_archinitialize(void)
 	SPI_SELECT(spi1, PX4_SPIDEV_MPU, false);
 	up_udelay(20);

-	message("[boot] Successfully initialized SPI port 1\n");
+	message("[boot] Initialized SPI port 1 (SENSORS)\n");

 	/* Get the SPI port for the FRAM */

@@ -294,20 +294,23 @@ __EXPORT int nsh_archinitialize(void)
 		return -ENODEV;
 	}

-	/* Default SPI2 to 37.5 MHz (F4 max) and de-assert the known chip selects. */
-	SPI_SETFREQUENCY(spi2, 375000000);
+	/* Default SPI2 to 37.5 MHz (40 MHz rounded to nearest valid divider, F4 max)
+	 * and de-assert the known chip selects. */
+
+	// XXX start with 10.4 MHz in FRAM usage and go up to 37.5 once validated
+	SPI_SETFREQUENCY(spi2, 12 * 1000 * 1000);
 	SPI_SETBITS(spi2, 8);
 	SPI_SETMODE(spi2, SPIDEV_MODE3);
 	SPI_SELECT(spi2, SPIDEV_FLASH, false);

-	message("[boot] Successfully initialized SPI port 2\n");
+	message("[boot] Initialized SPI port 2 (RAMTRON FRAM)\n");

 	#ifdef CONFIG_MMCSD
 	/* First, get an instance of the SDIO interface */

 	sdio = sdio_initialize(CONFIG_NSH_MMCSDSLOTNO);
 	if (!sdio) {
-		message("nsh_archinitialize: Failed to initialize SDIO slot %d\n",
+		message("[boot] Failed to initialize SDIO slot %d\n",
 			CONFIG_NSH_MMCSDSLOTNO);
 		return -ENODEV;
 	}
@@ -315,7 +318,7 @@ __EXPORT int nsh_archinitialize(void)
 	/* Now bind the SDIO interface to the MMC/SD driver */
 	int ret = mmcsd_slotinitialize(CONFIG_NSH_MMCSDMINOR, sdio);
 	if (ret != OK) {
-		message("nsh_archinitialize: Failed to bind SDIO to the MMC/SD driver: %d\n", ret);
+		message("[boot] Failed to bind SDIO to the MMC/SD driver: %d\n", ret);
 		return ret;
 	}

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-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
@@ -92,4 +92,4 @@
 #define HRT_TIMER		1	/* use timer1 for the HRT */
 #define HRT_TIMER_CHANNEL	2	/* use capture/compare channel 2 */
 #define HRT_PPM_CHANNEL		1	/* use capture/compare channel 1 */
-#define GPIO_PPM_IN		GPIO_TIM1_CH1IN
+#define GPIO_PPM_IN		(GPIO_ALT|GPIO_CNF_INPULLUP|GPIO_PORTE|GPIO_PIN9)
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (C) 2012 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
@@ -32,7 +32,7 @@
 ****************************************************************************/

 /**
- * @file px4iov2_internal.h
+ * @file board_config.h
 *
 * PX4IOV2 internal definitions
 */
@@ -122,7 +122,7 @@
 #define HRT_TIMER		1	/* use timer1 for the HRT */
 #define HRT_TIMER_CHANNEL	2	/* use capture/compare channel 2 */
 #define HRT_PPM_CHANNEL		1	/* use capture/compare channel 1 */
-#define GPIO_PPM_IN		GPIO_TIM1_CH1IN
+#define GPIO_PPM_IN		(GPIO_ALT|GPIO_CNF_INPULLUP|GPIO_PORTE|GPIO_PIN9)

 /* LED definitions ******************************************************************/
 /* PX4 has two LEDs that we will encode as: */
@@ -60,7 +60,7 @@
 /**
 * Maximum number of R/C input channels in the system. S.Bus has up to 18 channels.
 */
-#define RC_INPUT_MAX_CHANNELS	20
+#define RC_INPUT_MAX_CHANNELS	18

 /**
 * Input signal type, value is a control position from zero to 100
@@ -0,0 +1,289 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
+ *   Author: Stefan Rado <px4@sradonia.net>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file frsky_data.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>
+#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)
+
+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.
+ */
+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));
+}
+
+/**
+ * Sends a 0x5E start/stop byte.
+ */
+static void frsky_send_startstop(int uart)
+{
+	static const uint8_t c = 0x5E;
+	write(uart, &c, sizeof(c));
+}
+
+/**
+ * Sends one byte, performing byte-stuffing if necessary.
+ */
+static void frsky_send_byte(int uart, uint8_t value)
+{
+	const uint8_t x5E[] = { 0x5D, 0x3E };
+	const uint8_t x5D[] = { 0x5D, 0x3D };
+
+	switch (value) {
+	case 0x5E:
+		write(uart, x5E, sizeof(x5E));
+		break;
+
+	case 0x5D:
+		write(uart, x5D, sizeof(x5D));
+		break;
+
+	default:
+		write(uart, &value, sizeof(value));
+		break;
+	}
+}
+
+/**
+ * Sends one data id/value pair.
+ */
+static void frsky_send_data(int uart, uint8_t id, int16_t data)
+{
+	/* Cast data to unsigned, because signed shift might behave incorrectly */
+	uint16_t udata = data;
+
+	frsky_send_startstop(uart);
+
+	frsky_send_byte(uart, id);
+	frsky_send_byte(uart, udata);      /* LSB */
+	frsky_send_byte(uart, udata >> 8); /* MSB */
+}
+
+/**
+ * Sends frame 1 (every 200ms):
+ *   acceleration values, barometer altitude, temperature, battery voltage & current
+ */
+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 */
+	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);
+}
+
+/**
+ * Formats the decimal latitude/longitude to the required degrees/minutes/seconds.
+ */
+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;
+
+	return (dms_deg * 100.0f) + dms_min + (dms_sec / 100.0f);
+}
+
+/**
+ * Sends frame 2 (every 1000ms):
+ *   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 */
+	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 */
+	float course = 0, lat = 0, lon = 0, speed = 0, alt = 0;
+	char lat_ns = 0, lon_ew = 0;
+	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';
+		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, frac(course) * 1000.0f);
+
+	frsky_send_data(uart, FRSKY_ID_GPS_LAT_BP, lat);
+	frsky_send_data(uart, FRSKY_ID_GPS_LAT_AP, frac(lat) * 10000.0f);
+	frsky_send_data(uart, FRSKY_ID_GPS_LAT_NS, lat_ns);
+
+	frsky_send_data(uart, FRSKY_ID_GPS_LONG_BP, lon);
+	frsky_send_data(uart, FRSKY_ID_GPS_LONG_AP, frac(lon) * 10000.0f);
+	frsky_send_data(uart, FRSKY_ID_GPS_LONG_EW, lon_ew);
+
+	frsky_send_data(uart, FRSKY_ID_GPS_SPEED_BP, speed);
+	frsky_send_data(uart, FRSKY_ID_GPS_SPEED_AP, frac(speed) * 100.0f);
+
+	frsky_send_data(uart, FRSKY_ID_GPS_ALT_BP, alt);
+	frsky_send_data(uart, FRSKY_ID_GPS_ALT_AP, frac(alt) * 100.0f);
+
+	frsky_send_data(uart, FRSKY_ID_FUEL,
+			roundf(vehicle_status.battery_remaining * 100.0f));
+
+	frsky_send_data(uart, FRSKY_ID_GPS_SEC, sec);
+
+	frsky_send_startstop(uart);
+}
+
+/**
+ * Sends frame 3 (every 5000ms):
+ *   GPS date & time
+ */
+void frsky_send_frame3(int uart)
+{
+	/* 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 */
+	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);
+}
@@ -0,0 +1,51 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
+ *   Author: Stefan Rado <px4@sradonia.net>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file frsky_data.h
+ * @author Stefan Rado <px4@sradonia.net>
+ *
+ * FrSky telemetry implementation.
+ *
+ */
+#ifndef _FRSKY_DATA_H
+#define _FRSKY_DATA_H
+
+// 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 */
@@ -0,0 +1,266 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
+ *   Author: Stefan Rado <px4@sradonia.net>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file frsky_telemetry.c
+ * @author Stefan Rado <px4@sradonia.net>
+ *
+ * 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;
+}
@@ -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
@@ -32,8 +32,10 @@
 ############################################################################

 #
-# EEPROM file system driver
+# FrSky telemetry application.
 #

-MODULE_COMMAND	 = eeprom
-SRCS		 = 24xxxx_mtd.c eeprom.c
+MODULE_COMMAND	= frsky_telemetry
+
+SRCS		= frsky_data.c \
+              frsky_telemetry.c
@@ -85,7 +85,7 @@ static const int ERROR = -1;
 class GPS : public device::CDev
 {
 public:
-	GPS(const char *uart_path);
+	GPS(const char *uart_path, bool fake_gps);
 	virtual ~GPS();

 	virtual int			init();
@@ -112,6 +112,7 @@ private:
 	struct vehicle_gps_position_s 	_report;					///< uORB topic for gps position
 	orb_advert_t			_report_pub;					///< uORB pub for gps position
 	float				_rate;						///< position update rate
+	bool				_fake_gps;					///< fake gps output


 	/**
@@ -156,7 +157,7 @@ GPS	*g_dev;
 }


-GPS::GPS(const char *uart_path) :
+GPS::GPS(const char *uart_path, bool fake_gps) :
 	CDev("gps", GPS_DEVICE_PATH),
 	_task_should_exit(false),
 	_healthy(false),
@@ -164,7 +165,8 @@ GPS::GPS(const char *uart_path) :
 	_mode(GPS_DRIVER_MODE_UBX),
 	_Helper(nullptr),
 	_report_pub(-1),
-	_rate(0.0f)
+	_rate(0.0f),
+	_fake_gps(fake_gps)
 {
 	/* store port name */
 	strncpy(_port, uart_path, sizeof(_port));
@@ -264,98 +266,133 @@ GPS::task_main()
 	/* loop handling received serial bytes and also configuring in between */
 	while (!_task_should_exit) {

-		if (_Helper != nullptr) {
-			delete(_Helper);
-			/* set to zero to ensure parser is not used while not instantiated */
-			_Helper = nullptr;
-		}
+		if (_fake_gps) {

-		switch (_mode) {
-		case GPS_DRIVER_MODE_UBX:
-			_Helper = new UBX(_serial_fd, &_report);
-			break;
+			_report.timestamp_position = hrt_absolute_time();
+			_report.lat = (int32_t)47.378301e7f;
+			_report.lon = (int32_t)8.538777e7f;
+			_report.alt = (int32_t)400e3f;
+			_report.timestamp_variance = hrt_absolute_time();
+			_report.s_variance_m_s = 10.0f;
+			_report.p_variance_m = 10.0f;
+			_report.c_variance_rad = 0.1f;
+			_report.fix_type = 3;
+			_report.eph_m = 10.0f;
+			_report.epv_m = 10.0f;
+			_report.timestamp_velocity = hrt_absolute_time();
+			_report.vel_n_m_s = 0.0f;
+			_report.vel_e_m_s = 0.0f;
+			_report.vel_d_m_s = 0.0f;
+			_report.vel_m_s = sqrtf(_report.vel_n_m_s * _report.vel_n_m_s + _report.vel_e_m_s * _report.vel_e_m_s + _report.vel_d_m_s * _report.vel_d_m_s);
+			_report.cog_rad = 0.0f;
+			_report.vel_ned_valid = true;

-		case GPS_DRIVER_MODE_MTK:
-			_Helper = new MTK(_serial_fd, &_report);
-			break;
+			//no time and satellite information simulated

-		default:
-			break;
-		}
+			if (_report_pub > 0) {
+				orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);

-		unlock();
-
-		if (_Helper->configure(_baudrate) == 0) {
-			unlock();
-
-			// GPS is obviously detected successfully, reset statistics
-			_Helper->reset_update_rates();
-
-			while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) {
-//				lock();
-				/* opportunistic publishing - else invalid data would end up on the bus */
-				if (_report_pub > 0) {
-					orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
-
-				} else {
-					_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
-				}
-
-				last_rate_count++;
-
-				/* measure update rate every 5 seconds */
-				if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) {
-					_rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f);
-					last_rate_measurement = hrt_absolute_time();
-					last_rate_count = 0;
-					_Helper->store_update_rates();
-					_Helper->reset_update_rates();
-				}
-
-				if (!_healthy) {
-					char *mode_str = "unknown";
-
-					switch (_mode) {
-					case GPS_DRIVER_MODE_UBX:
-						mode_str = "UBX";
-						break;
-
-					case GPS_DRIVER_MODE_MTK:
-						mode_str = "MTK";
-						break;
-
-					default:
-						break;
-					}
-
-					warnx("module found: %s", mode_str);
-					_healthy = true;
-				}
+			} else {
+				_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
 			}

-			if (_healthy) {
-				warnx("module lost");
-				_healthy = false;
-				_rate = 0.0f;
+			usleep(2e5);
+
+		} else {
+
+			if (_Helper != nullptr) {
+				delete(_Helper);
+				/* set to zero to ensure parser is not used while not instantiated */
+				_Helper = nullptr;
+			}
+
+			switch (_mode) {
+			case GPS_DRIVER_MODE_UBX:
+				_Helper = new UBX(_serial_fd, &_report);
+				break;
+
+			case GPS_DRIVER_MODE_MTK:
+				_Helper = new MTK(_serial_fd, &_report);
+				break;
+
+			default:
+				break;
+			}
+
+			unlock();
+
+			if (_Helper->configure(_baudrate) == 0) {
+				unlock();
+
+				// GPS is obviously detected successfully, reset statistics
+				_Helper->reset_update_rates();
+
+				while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) {
+	//				lock();
+					/* opportunistic publishing - else invalid data would end up on the bus */
+					if (_report_pub > 0) {
+						orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
+
+					} else {
+						_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
+					}
+
+					last_rate_count++;
+
+					/* measure update rate every 5 seconds */
+					if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) {
+						_rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f);
+						last_rate_measurement = hrt_absolute_time();
+						last_rate_count = 0;
+						_Helper->store_update_rates();
+						_Helper->reset_update_rates();
+					}
+
+					if (!_healthy) {
+						char *mode_str = "unknown";
+
+						switch (_mode) {
+						case GPS_DRIVER_MODE_UBX:
+							mode_str = "UBX";
+							break;
+
+						case GPS_DRIVER_MODE_MTK:
+							mode_str = "MTK";
+							break;
+
+						default:
+							break;
+						}
+
+						warnx("module found: %s", mode_str);
+						_healthy = true;
+					}
+				}
+
+				if (_healthy) {
+					warnx("module lost");
+					_healthy = false;
+					_rate = 0.0f;
+				}
+
+				lock();
 			}

 			lock();
-		}

-		lock();
+			/* select next mode */
+			switch (_mode) {
+			case GPS_DRIVER_MODE_UBX:
+				_mode = GPS_DRIVER_MODE_MTK;
+				break;

-		/* select next mode */
-		switch (_mode) {
-		case GPS_DRIVER_MODE_UBX:
-			_mode = GPS_DRIVER_MODE_MTK;
-			break;
+			case GPS_DRIVER_MODE_MTK:
+				_mode = GPS_DRIVER_MODE_UBX;
+				break;

-		case GPS_DRIVER_MODE_MTK:
-			_mode = GPS_DRIVER_MODE_UBX;
-			break;
-
-		default:
-			break;
+			default:
+				break;
+			}
 		}

 	}
@@ -417,7 +454,7 @@ namespace gps

 GPS	*g_dev;

-void	start(const char *uart_path);
+void	start(const char *uart_path, bool fake_gps);
 void	stop();
 void	test();
 void	reset();
@@ -427,7 +464,7 @@ void	info();
 * Start the driver.
 */
 void
-start(const char *uart_path)
+start(const char *uart_path, bool fake_gps)
 {
 	int fd;

@@ -435,7 +472,7 @@ start(const char *uart_path)
 		errx(1, "already started");

 	/* create the driver */
-	g_dev = new GPS(uart_path);
+	g_dev = new GPS(uart_path, fake_gps);

 	if (g_dev == nullptr)
 		goto fail;
@@ -527,6 +564,7 @@ gps_main(int argc, char *argv[])

 	/* set to default */
 	char *device_name = GPS_DEFAULT_UART_PORT;
+	bool fake_gps = false;

 	/*
 	 * Start/load the driver.
@@ -542,7 +580,13 @@ gps_main(int argc, char *argv[])
 			}
 		}

-		gps::start(device_name);
+		/* Detect fake gps option */
+		for (int i = 2; i < argc; i++) {
+			if (!strcmp(argv[i], "-f"))
+				fake_gps = true;
+		}
+
+		gps::start(device_name, fake_gps);
 	}

 	if (!strcmp(argv[1], "stop"))
@@ -567,5 +611,5 @@ gps_main(int argc, char *argv[])
 		gps::info();

 out:
-	errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n]");
+	errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n][-f]");
 }
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-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
@@ -193,9 +193,10 @@ HIL::~HIL()
 		} while (_task != -1);
 	}

-	/* clean up the alternate device node */
-	if (_primary_pwm_device)
-		unregister_driver(PWM_OUTPUT_DEVICE_PATH);
+	// XXX already claimed with CDEV
+	// /* clean up the alternate device node */
+	// if (_primary_pwm_device)
+	// 	unregister_driver(PWM_OUTPUT_DEVICE_PATH);

 	g_hil = nullptr;
 }
@@ -211,7 +211,7 @@ hott_sensors_main(int argc, char *argv[])
 		thread_should_exit = false;
 		deamon_task = task_spawn_cmd(daemon_name,
 					     SCHED_DEFAULT,
-					     SCHED_PRIORITY_MAX - 40,
+					     SCHED_PRIORITY_DEFAULT,
 					     1024,
 					     hott_sensors_thread_main,
 					     (argv) ? (const char **)&argv[2] : (const char **)NULL);
@@ -237,7 +237,7 @@ hott_telemetry_main(int argc, char *argv[])
 		thread_should_exit = false;
 		deamon_task = task_spawn_cmd(daemon_name,
 					 SCHED_DEFAULT,
-					 SCHED_PRIORITY_MAX - 40,
+					 SCHED_PRIORITY_DEFAULT,
 					 2048,
 					 hott_telemetry_thread_main,
 					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
@@ -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;

@@ -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);
@@ -65,6 +65,7 @@
 #include <systemlib/err.h>
 #include <systemlib/mixer/mixer.h>
 #include <systemlib/pwm_limit/pwm_limit.h>
+#include <systemlib/board_serial.h>
 #include <drivers/drv_mixer.h>
 #include <drivers/drv_rc_input.h>

@@ -224,10 +225,10 @@ PX4FMU::PX4FMU() :
 	_armed(false),
 	_pwm_on(false),
 	_mixers(nullptr),
-	_failsafe_pwm( {0}),
-	       _disarmed_pwm( {0}),
-	       _num_failsafe_set(0),
-	       _num_disarmed_set(0)
+	_failsafe_pwm({0}),
+	      _disarmed_pwm({0}),
+	      _num_failsafe_set(0),
+	      _num_disarmed_set(0)
 {
 	for (unsigned i = 0; i < _max_actuators; i++) {
 		_min_pwm[i] = PWM_DEFAULT_MIN;
@@ -575,7 +576,7 @@ PX4FMU::task_main()
 						if (i >= outputs.noutputs ||
 						    !isfinite(outputs.output[i]) ||
 						    outputs.output[i] < -1.0f ||
-						outputs.output[i] > 1.0f) {
+						    outputs.output[i] > 1.0f) {
 							/*
 							 * Value is NaN, INF or out of band - set to the minimum value.
 							 * This will be clearly visible on the servo status and will limit the risk of accidentally
@@ -933,7 +934,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 			break;
 		}

-		/* FALLTHROUGH */
+	/* FALLTHROUGH */
 	case PWM_SERVO_SET(3):
 	case PWM_SERVO_SET(2):
 		if (_mode < MODE_4PWM) {
@@ -941,7 +942,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 			break;
 		}

-		/* FALLTHROUGH */
+	/* FALLTHROUGH */
 	case PWM_SERVO_SET(1):
 	case PWM_SERVO_SET(0):
 		if (arg <= 2100) {
@@ -960,7 +961,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 			break;
 		}

-		/* FALLTHROUGH */
+	/* FALLTHROUGH */
 	case PWM_SERVO_GET(3):
 	case PWM_SERVO_GET(2):
 		if (_mode < MODE_4PWM) {
@@ -968,7 +969,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 			break;
 		}

-		/* FALLTHROUGH */
+	/* FALLTHROUGH */
 	case PWM_SERVO_GET(1):
 	case PWM_SERVO_GET(0):
 		*(servo_position_t *)arg = up_pwm_servo_get(cmd - PWM_SERVO_GET(0));
@@ -1107,10 +1108,12 @@ PX4FMU::sensor_reset(int ms)
 	stm32_configgpio(GPIO_SPI_CS_GYRO_OFF);
 	stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG_OFF);
 	stm32_configgpio(GPIO_SPI_CS_BARO_OFF);
+	stm32_configgpio(GPIO_SPI_CS_MPU_OFF);

 	stm32_gpiowrite(GPIO_SPI_CS_GYRO_OFF, 0);
 	stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG_OFF, 0);
 	stm32_gpiowrite(GPIO_SPI_CS_BARO_OFF, 0);
+	stm32_gpiowrite(GPIO_SPI_CS_MPU_OFF, 0);

 	stm32_configgpio(GPIO_SPI1_SCK_OFF);
 	stm32_configgpio(GPIO_SPI1_MISO_OFF);
@@ -1123,10 +1126,12 @@ PX4FMU::sensor_reset(int ms)
 	stm32_configgpio(GPIO_GYRO_DRDY_OFF);
 	stm32_configgpio(GPIO_MAG_DRDY_OFF);
 	stm32_configgpio(GPIO_ACCEL_DRDY_OFF);
+	stm32_configgpio(GPIO_EXTI_MPU_DRDY_OFF);

 	stm32_gpiowrite(GPIO_GYRO_DRDY_OFF, 0);
 	stm32_gpiowrite(GPIO_MAG_DRDY_OFF, 0);
 	stm32_gpiowrite(GPIO_ACCEL_DRDY_OFF, 0);
+	stm32_gpiowrite(GPIO_EXTI_MPU_DRDY_OFF, 0);

 	/* set the sensor rail off */
 	stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN);
@@ -1159,6 +1164,13 @@ PX4FMU::sensor_reset(int ms)
 	stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
 	stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
 	stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
+
+	// // XXX bring up the EXTI pins again
+	// stm32_configgpio(GPIO_GYRO_DRDY);
+	// stm32_configgpio(GPIO_MAG_DRDY);
+	// stm32_configgpio(GPIO_ACCEL_DRDY);
+	// stm32_configgpio(GPIO_EXTI_MPU_DRDY);
+
 #endif
 #endif
 }
@@ -1591,6 +1603,15 @@ fmu_main(int argc, char *argv[])
 		errx(0, "FMU driver stopped");
 	}

+	if (!strcmp(verb, "id")) {
+		char id[12];
+		(void)get_board_serial(id);
+
+		errx(0, "Board serial:\n %02X%02X%02X%02X %02X%02X%02X%02X %02X%02X%02X%02X",
+		     (unsigned)id[0], (unsigned)id[1], (unsigned)id[2], (unsigned)id[3], (unsigned)id[4], (unsigned)id[5],
+		     (unsigned)id[6], (unsigned)id[7], (unsigned)id[8], (unsigned)id[9], (unsigned)id[10], (unsigned)id[11]);
+	}
+

 	if (fmu_start() != OK)
 		errx(1, "failed to start the FMU driver");
@@ -1647,6 +1668,7 @@ fmu_main(int argc, char *argv[])
 			sensor_reset(0);
 			warnx("resettet default time");
 		}
+
 		exit(0);
 	}

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-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
@@ -35,7 +35,7 @@
 * @file px4io.cpp
 * Driver for the PX4IO board.
 *
- * PX4IO is connected via I2C.
+ * PX4IO is connected via I2C or DMA enabled high-speed UART.
 */

 #include <nuttx/config.h>
@@ -1690,6 +1690,9 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries)

 		int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, (sizeof(px4io_mixdata) + 2) / 2);

+		if (ret)
+			return ret;
+
 		retries--;

 		log("mixer sent");
@@ -2419,6 +2422,69 @@ detect(int argc, char *argv[])
 	}
 }

+void
+checkcrc(int argc, char *argv[])
+{
+	bool keep_running = false;
+
+	if (g_dev == nullptr) {
+		/* allocate the interface */
+		device::Device *interface = get_interface();
+
+		/* create the driver - it will set g_dev */
+		(void)new PX4IO(interface);
+
+		if (g_dev == nullptr)
+			errx(1, "driver alloc failed");
+	} else {
+		/* its already running, don't kill the driver */
+		keep_running = true;
+	}
+
+	/*
+	  check IO CRC against CRC of a file
+	 */
+	if (argc < 2) {
+		printf("usage: px4io checkcrc filename\n");
+		exit(1);
+	}
+	int fd = open(argv[1], O_RDONLY);
+	if (fd == -1) {
+		printf("open of %s failed - %d\n", argv[1], errno);
+		exit(1);			
+	}
+	const uint32_t app_size_max = 0xf000;
+	uint32_t fw_crc = 0;
+	uint32_t nbytes = 0;
+	while (true) {
+		uint8_t buf[16];
+		int n = read(fd, buf, sizeof(buf));
+		if (n <= 0) break;
+		fw_crc = crc32part(buf, n, fw_crc);
+		nbytes += n;
+	}
+	close(fd);
+	while (nbytes < app_size_max) {
+		uint8_t b = 0xff;
+		fw_crc = crc32part(&b, 1, fw_crc);			
+		nbytes++;
+	}
+
+	int ret = g_dev->ioctl(nullptr, PX4IO_CHECK_CRC, fw_crc);
+
+	if (!keep_running) {
+		delete g_dev;
+		g_dev = nullptr;
+	}
+
+	if (ret != OK) {
+		printf("check CRC failed - %d\n", ret);
+		exit(1);			
+	}
+	printf("CRCs match\n");
+	exit(0);
+}
+
 void
 bind(int argc, char *argv[])
 {
@@ -2613,6 +2679,9 @@ px4io_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "detect"))
 		detect(argc - 1, argv + 1);

+	if (!strcmp(argv[1], "checkcrc"))
+		checkcrc(argc - 1, argv + 1);
+
 	if (!strcmp(argv[1], "update")) {

 		if (g_dev != nullptr) {
@@ -2798,49 +2867,6 @@ px4io_main(int argc, char *argv[])
 		exit(0);
 	}

-	if (!strcmp(argv[1], "checkcrc")) {
-		/*
-		  check IO CRC against CRC of a file
-		 */
-		if (argc <= 2) {
-			printf("usage: px4io checkcrc filename\n");
-			exit(1);
-		}
-		if (g_dev == nullptr) {
-			printf("px4io is not started\n");
-			exit(1);
-		}
-		int fd = open(argv[2], O_RDONLY);
-		if (fd == -1) {
-			printf("open of %s failed - %d\n", argv[2], errno);
-			exit(1);			
-		}
-		const uint32_t app_size_max = 0xf000;
-		uint32_t fw_crc = 0;
-		uint32_t nbytes = 0;
-		while (true) {
-			uint8_t buf[16];
-			int n = read(fd, buf, sizeof(buf));
-			if (n <= 0) break;
-			fw_crc = crc32part(buf, n, fw_crc);
-			nbytes += n;
-		}
-		close(fd);
-		while (nbytes < app_size_max) {
-			uint8_t b = 0xff;
-			fw_crc = crc32part(&b, 1, fw_crc);			
-			nbytes++;
-		}
-
-		int ret = g_dev->ioctl(nullptr, PX4IO_CHECK_CRC, fw_crc);
-		if (ret != OK) {
-			printf("check CRC failed - %d\n", ret);
-			exit(1);			
-		}
-		printf("CRCs match\n");
-		exit(0);
-	}
-
 	if (!strcmp(argv[1], "rx_dsm") ||
 	    !strcmp(argv[1], "rx_dsm_10bit") ||
 	    !strcmp(argv[1], "rx_dsm_11bit") ||
@@ -559,7 +559,7 @@ RGBLED::get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b)
 void
 rgbled_usage()
 {
-	warnx("missing command: try 'start', 'test', 'info', 'off', 'rgb 30 40 50'");
+	warnx("missing command: try 'start', 'test', 'info', 'off', 'stop', 'rgb 30 40 50'");
 	warnx("options:");
 	warnx("    -b i2cbus (%d)", PX4_I2C_BUS_LED);
 	warnx("    -a addr (0x%x)", ADDR);
@@ -643,7 +643,7 @@ rgbled_main(int argc, char *argv[])
 	if (g_rgbled == nullptr) {
 		warnx("not started");
 		rgbled_usage();
-		exit(0);
+		exit(1);
 	}

 	if (!strcmp(verb, "test")) {
@@ -669,7 +669,7 @@ rgbled_main(int argc, char *argv[])
 		exit(0);
 	}

-	if (!strcmp(verb, "off")) {
+	if (!strcmp(verb, "off") || !strcmp(verb, "stop")) {
 		fd = open(RGBLED_DEVICE_PATH, 0);

 		if (fd == -1) {
@@ -681,6 +681,12 @@ rgbled_main(int argc, char *argv[])
 		exit(ret);
 	}

+	if (!strcmp(verb, "stop")) {
+		delete g_rgbled;
+		g_rgbled = nullptr;
+		exit(0);
+	}
+
 	if (!strcmp(verb, "rgb")) {
 		if (argc < 5) {
 			errx(1, "Usage: rgbled rgb <red> <green> <blue>");
@@ -404,10 +404,18 @@ void TECS::_update_pitch(void)
 	// Apply max and min values for integrator state that will allow for no more than
 	// 5deg of saturation. This allows for some pitch variation due to gusts before the
 	// integrator is clipped. Otherwise the effectiveness of the integrator will be reduced in turbulence
+	// During climbout/takeoff, bias the demanded pitch angle so that zero speed error produces a pitch angle
+	// demand equal to the minimum value (which is )set by the mission plan during this mode). Otherwise the
+	// integrator has to catch up before the nose can be raised to reduce speed during climbout.
 	float gainInv = (_integ5_state * _timeConst * CONSTANTS_ONE_G);
 	float temp = SEB_error + SEBdot_error * _ptchDamp + SEBdot_dem * _timeConst;
+	if (_climbOutDem)
+	{
+		temp += _PITCHminf * gainInv;
+	}
 	_integ7_state = constrain(_integ7_state, (gainInv * (_PITCHminf - 0.0783f)) - temp, (gainInv * (_PITCHmaxf + 0.0783f)) - temp);

+
 	// Calculate pitch demand from specific energy balance signals
 	_pitch_dem_unc = (temp + _integ7_state) / gainInv;

@@ -107,14 +107,9 @@ static const int ERROR = -1;

 extern struct system_load_s system_load;

-#define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f
-#define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f
-
 /* Decouple update interval and hysteris counters, all depends on intervals */
 #define COMMANDER_MONITORING_INTERVAL 50000
 #define COMMANDER_MONITORING_LOOPSPERMSEC (1/(COMMANDER_MONITORING_INTERVAL/1000.0f))
-#define LOW_VOLTAGE_BATTERY_COUNTER_LIMIT (LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
-#define CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT (CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)

 #define MAVLINK_OPEN_INTERVAL 50000

@@ -666,8 +661,6 @@ int commander_thread_main(int argc, char *argv[])

 	/* Start monitoring loop */
 	unsigned counter = 0;
-	unsigned low_voltage_counter = 0;
-	unsigned critical_voltage_counter = 0;
 	unsigned stick_off_counter = 0;
 	unsigned stick_on_counter = 0;

@@ -745,7 +738,6 @@ int commander_thread_main(int argc, char *argv[])
 	int battery_sub = orb_subscribe(ORB_ID(battery_status));
 	struct battery_status_s battery;
 	memset(&battery, 0, sizeof(battery));
-	battery.voltage_v = 0.0f;

 	/* Subscribe to subsystem info topic */
 	int subsys_sub = orb_subscribe(ORB_ID(subsystem_info));
@@ -890,14 +882,12 @@ int commander_thread_main(int argc, char *argv[])

 		if (updated) {
 			orb_copy(ORB_ID(battery_status), battery_sub, &battery);
-
-			// warnx("bat v: %2.2f", battery.voltage_v);
-
-			/* only consider battery voltage if system has been running 2s and battery voltage is higher than 4V */
-			if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_v > 4.0f) {
-				status.battery_voltage = battery.voltage_v;
+			/* only consider battery voltage if system has been running 2s and battery voltage is valid */
+			if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) {
+				status.battery_voltage = battery.voltage_filtered_v;
+				status.battery_current = battery.current_a;
 				status.condition_battery_voltage_valid = true;
-				status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage);
+				status.battery_remaining = battery_remaining_estimate_voltage(battery.voltage_filtered_v, battery.discharged_mah);
 			}
 		}

@@ -950,46 +940,29 @@ int commander_thread_main(int argc, char *argv[])
 			//on_usb_power = (stat("/dev/ttyACM0", &statbuf) == 0);
 		}

-		// XXX remove later
-		//warnx("bat remaining: %2.2f", status.battery_remaining);
-
 		/* if battery voltage is getting lower, warn using buzzer, etc. */
 		if (status.condition_battery_voltage_valid && status.battery_remaining < 0.25f && !low_battery_voltage_actions_done) {
-			//TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS
-			if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) {
-				low_battery_voltage_actions_done = true;
-				mavlink_log_critical(mavlink_fd, "#audio: WARNING: LOW BATTERY");
-				status.battery_warning = VEHICLE_BATTERY_WARNING_LOW;
-				status_changed = true;
-				battery_tune_played = false;
-			}
-
-			low_voltage_counter++;
+			low_battery_voltage_actions_done = true;
+			mavlink_log_critical(mavlink_fd, "#audio: WARNING: LOW BATTERY");
+			status.battery_warning = VEHICLE_BATTERY_WARNING_LOW;
+			status_changed = true;
+			battery_tune_played = false;

 		} else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.1f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) {
 			/* critical battery voltage, this is rather an emergency, change state machine */
-			if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) {
-				critical_battery_voltage_actions_done = true;
-				mavlink_log_critical(mavlink_fd, "#audio: EMERGENCY: CRITICAL BATTERY");
-				status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
-				battery_tune_played = false;
+			critical_battery_voltage_actions_done = true;
+			mavlink_log_critical(mavlink_fd, "#audio: EMERGENCY: CRITICAL BATTERY");
+			status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
+			battery_tune_played = false;

-				if (armed.armed) {
-					arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED_ERROR, &armed);
+			if (armed.armed) {
+				arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED_ERROR, &armed);

-				} else {
-					arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY_ERROR, &armed);
-				}
-
-				status_changed = true;
+			} else {
+				arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY_ERROR, &armed);
 			}

-			critical_voltage_counter++;
-
-		} else {
-
-			low_voltage_counter = 0;
-			critical_voltage_counter = 0;
+			status_changed = true;
 		}

 		/* End battery voltage check */
@@ -44,6 +44,7 @@
 #include <stdint.h>
 #include <stdbool.h>
 #include <fcntl.h>
+#include <math.h>

 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
@@ -251,36 +252,47 @@ void rgbled_set_pattern(rgbled_pattern_t *pattern)
 		ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern);
 }

-float battery_remaining_estimate_voltage(float voltage)
+float battery_remaining_estimate_voltage(float voltage, float discharged)
 {
 	float ret = 0;
-	static param_t bat_volt_empty;
-	static param_t bat_volt_full;
-	static param_t bat_n_cells;
+	static param_t bat_v_empty_h;
+	static param_t bat_v_full_h;
+	static param_t bat_n_cells_h;
+	static param_t bat_capacity_h;
+	static float bat_v_empty = 3.2f;
+	static float bat_v_full = 4.0f;
+	static int bat_n_cells = 3;
+	static float bat_capacity = -1.0f;
 	static bool initialized = false;
 	static unsigned int counter = 0;
-	static float ncells = 3;
-	// XXX change cells to int (and param to INT32)

 	if (!initialized) {
-		bat_volt_empty = param_find("BAT_V_EMPTY");
-		bat_volt_full = param_find("BAT_V_FULL");
-		bat_n_cells = param_find("BAT_N_CELLS");
+		bat_v_empty_h = param_find("BAT_V_EMPTY");
+		bat_v_full_h = param_find("BAT_V_FULL");
+		bat_n_cells_h = param_find("BAT_N_CELLS");
+		bat_capacity_h = param_find("BAT_CAPACITY");
 		initialized = true;
 	}

-	static float chemistry_voltage_empty = 3.2f;
-	static float chemistry_voltage_full = 4.05f;
-
 	if (counter % 100 == 0) {
-		param_get(bat_volt_empty, &chemistry_voltage_empty);
-		param_get(bat_volt_full, &chemistry_voltage_full);
-		param_get(bat_n_cells, &ncells);
+		param_get(bat_v_empty_h, &bat_v_empty);
+		param_get(bat_v_full_h, &bat_v_full);
+		param_get(bat_n_cells_h, &bat_n_cells);
+		param_get(bat_capacity_h, &bat_capacity);
 	}

 	counter++;

-	ret = (voltage - ncells * chemistry_voltage_empty) / (ncells * (chemistry_voltage_full - chemistry_voltage_empty));
+	/* remaining charge estimate based on voltage */
+	float remaining_voltage = (voltage - bat_n_cells * bat_v_empty) / (bat_n_cells * (bat_v_full - bat_v_empty));
+
+	if (bat_capacity > 0.0f) {
+		/* if battery capacity is known, use discharged current for estimate, but don't show more than voltage estimate */
+		ret = fminf(remaining_voltage, 1.0f - discharged / bat_capacity);
+	} else {
+		/* else use voltage */
+		ret = remaining_voltage;
+	}

 	/* limit to sane values */
 	ret = (ret < 0.0f) ? 0.0f : ret;
@@ -75,12 +75,13 @@ void rgbled_set_mode(rgbled_mode_t mode);
 void rgbled_set_pattern(rgbled_pattern_t *pattern);

 /**
- * Provides a coarse estimate of remaining battery power.
+ * Estimate remaining battery charge.
 *
- * The estimate is very basic and based on decharging voltage curves.
+ * Use integral of current if battery capacity known (BAT_CAPACITY parameter set),
+ * else use simple estimate based on voltage.
 *
 * @return the estimated remaining capacity in 0..1
 */
-float battery_remaining_estimate_voltage(float voltage);
+float battery_remaining_estimate_voltage(float voltage, float discharged);

 #endif /* COMMANDER_HELPER_H_ */
@@ -50,6 +50,7 @@ PARAM_DEFINE_FLOAT(NAV_TAKEOFF_GAP, 3.0f);
 PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f);
 PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f);
 PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f);
-PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.2f);
-PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.05f);
-PARAM_DEFINE_FLOAT(BAT_N_CELLS, 3);
+PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f);
+PARAM_DEFINE_FLOAT(BAT_V_FULL, 3.9f);
+PARAM_DEFINE_INT32(BAT_N_CELLS, 3);
+PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f);
@@ -1,7 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *   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
@@ -49,21 +48,75 @@
 *
 */

+/**
+ * L1 period
+ *
+ * This is the L1 distance and defines the tracking
+ * point ahead of the aircraft its following.
+ * A value of 25 meters works for most aircraft. Shorten
+ * slowly during tuning until response is sharp without oscillation.
+ *
+ * @min 1.0
+ * @max 100.0
+ * @group L1 Control
+ */
 PARAM_DEFINE_FLOAT(FW_L1_PERIOD, 25.0f);

-
+/**
+ * L1 damping
+ *
+ * Damping factor for L1 control.
+ *
+ * @min 0.6
+ * @max 0.9
+ * @group L1 Control
+ */
 PARAM_DEFINE_FLOAT(FW_L1_DAMPING, 0.75f);

-
+/**
+ * Default Loiter Radius
+ *
+ * This radius is used when no other loiter radius is set.
+ *
+ * @min 10.0
+ * @max 100.0
+ * @group L1 Control
+ */
 PARAM_DEFINE_FLOAT(FW_LOITER_R, 50.0f);

-
+/**
+ * Cruise throttle
+ *
+ * This is the throttle setting required to achieve the desired cruise speed. Most airframes have a value of 0.5-0.7.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group L1 Control
+ */
 PARAM_DEFINE_FLOAT(FW_THR_CRUISE, 0.7f);

-
+/**
+ * Negative pitch limit
+ *
+ * The minimum negative pitch the controller will output.
+ *
+ * @unit degrees
+ * @min -60.0
+ * @max 0.0
+ * @group L1 Control
+ */
 PARAM_DEFINE_FLOAT(FW_P_LIM_MIN, -45.0f);

-
+/**
+ * Positive pitch limit
+ *
+ * The maximum positive pitch the controller will output.
+ *
+ * @unit degrees
+ * @min 0.0
+ * @max 60.0
+ * @group L1 Control
+ */
 PARAM_DEFINE_FLOAT(FW_P_LIM_MAX, 45.0f);


@@ -677,8 +677,8 @@ int mavlink_thread_main(int argc, char *argv[])
 						    v_status.onboard_control_sensors_health,
 						    v_status.load * 1000.0f,
 						    v_status.battery_voltage * 1000.0f,
-						    v_status.battery_current * 1000.0f,
-						    v_status.battery_remaining,
+						    v_status.battery_current * 100.0f,
+						    v_status.battery_remaining * 100.0f,
 						    v_status.drop_rate_comm,
 						    v_status.errors_comm,
 						    v_status.errors_count1,
@@ -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);

@@ -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();
@@ -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);
+
@@ -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:
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
 *   Author: Lorenz Meier <lm@inf.ethz.ch>
 *           Anton Babushkin <anton.babushkin@me.com>
 *
@@ -702,6 +702,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		struct airspeed_s airspeed;
 		struct esc_status_s esc;
 		struct vehicle_global_velocity_setpoint_s global_vel_sp;
+		struct battery_status_s battery;
 	} buf;

 	memset(&buf, 0, sizeof(buf));
@@ -726,6 +727,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		int airspeed_sub;
 		int esc_sub;
 		int global_vel_sp_sub;
+		int battery_sub;
 	} subs;

 	/* log message buffer: header + body */
@@ -752,6 +754,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			struct log_GPSP_s log_GPSP;
 			struct log_ESC_s log_ESC;
 			struct log_GVSP_s log_GVSP;
+			struct log_BATT_s log_BATT;
 		} body;
 	} log_msg = {
 		LOG_PACKET_HEADER_INIT(0)
@@ -760,9 +763,9 @@ int sdlog2_thread_main(int argc, char *argv[])
 	memset(&log_msg.body, 0, sizeof(log_msg.body));

 	/* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
-	/* number of subscriptions */
-	const ssize_t fdsc = 19;
-	/* sanity check variable and index */
+	/* number of messages */
+	const ssize_t fdsc = 25;
+	/* Sanity check variable and index */
 	ssize_t fdsc_count = 0;
 	/* file descriptors to wait for */
 	struct pollfd fds[fdsc];
@@ -881,6 +884,12 @@ int sdlog2_thread_main(int argc, char *argv[])
 	fds[fdsc_count].events = POLLIN;
 	fdsc_count++;

+	/* --- BATTERY --- */
+	subs.battery_sub = orb_subscribe(ORB_ID(battery_status));
+	fds[fdsc_count].fd = subs.battery_sub;
+	fds[fdsc_count].events = POLLIN;
+	fdsc_count++;
+
 	/* WARNING: If you get the error message below,
 	 * then the number of registered messages (fdsc)
 	 * differs from the number of messages in the above list.
@@ -971,8 +980,6 @@ int sdlog2_thread_main(int argc, char *argv[])
 				log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state;
 				log_msg.body.log_STAT.navigation_state = (uint8_t) buf_status.navigation_state;
 				log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state;
-				log_msg.body.log_STAT.battery_voltage = buf_status.battery_voltage;
-				log_msg.body.log_STAT.battery_current = buf_status.battery_current;
 				log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining;
 				log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning;
 				log_msg.body.log_STAT.landed = (uint8_t) buf_status.condition_landed;
@@ -1238,6 +1245,17 @@ int sdlog2_thread_main(int argc, char *argv[])
 				LOGBUFFER_WRITE_AND_COUNT(GVSP);
 			}

+			/* --- BATTERY --- */
+			if (fds[ifds++].revents & POLLIN) {
+				orb_copy(ORB_ID(battery_status), subs.battery_sub, &buf.battery);
+				log_msg.msg_type = LOG_BATT_MSG;
+				log_msg.body.log_BATT.voltage = buf.battery.voltage_v;
+				log_msg.body.log_BATT.voltage_filtered = buf.battery.voltage_filtered_v;
+				log_msg.body.log_BATT.current = buf.battery.current_a;
+				log_msg.body.log_BATT.discharged = buf.battery.discharged_mah;
+				LOGBUFFER_WRITE_AND_COUNT(BATT);
+			}
+
 			/* signal the other thread new data, but not yet unlock */
 			if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) {
 				/* only request write if several packets can be written at once */
@@ -148,8 +148,6 @@ struct log_STAT_s {
 	uint8_t main_state;
 	uint8_t navigation_state;
 	uint8_t arming_state;
-	float battery_voltage;
-	float battery_current;
 	float battery_remaining;
 	uint8_t battery_warning;
 	uint8_t landed;
@@ -248,6 +246,15 @@ struct log_GVSP_s {
 	float vz;
 };

+/* --- BATT - BATTERY --- */
+#define LOG_BATT_MSG 20
+struct log_BATT_s {
+	float voltage;
+	float voltage_filtered;
+	float current;
+	float discharged;
+};
+
 /* --- TIME - TIME STAMP --- */
 #define LOG_TIME_MSG 129
 struct log_TIME_s {
@@ -281,7 +288,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"),
 	LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"),
 	LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
-	LOG_FORMAT(STAT, "BBBfffBB", "MainState,NavState,ArmState,BatV,BatC,BatRem,BatWarn,Landed"),
+	LOG_FORMAT(STAT, "BBBfBB", "MainState,NavState,ArmState,BatRem,BatWarn,Landed"),
 	LOG_FORMAT(RC, "ffffffffB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count"),
 	LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
 	LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"),
@@ -291,6 +298,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(GPSP, "BLLfffbBffff", "AltRel,Lat,Lon,Alt,Yaw,LoiterR,LoiterDir,NavCmd,P1,P2,P3,P4"),
 	LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
 	LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"),
+	LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"),
 	/* system-level messages, ID >= 0x80 */
 	// FMT: don't write format of format message, it's useless
 	LOG_FORMAT(TIME, "Q", "StartTime"),
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2012-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
@@ -1,9 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
- *           @author Thomas Gubler <thomasgubler@student.ethz.ch>
- *           @author Julian Oes <joes@student.ethz.ch>
+ *   Copyright (c) 2012-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
@@ -38,6 +35,10 @@
 * @file sensor_params.c
 *
 * Parameters defined by the sensors task.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
 */

 #include <nuttx/config.h>
@@ -45,41 +46,98 @@
 #include <systemlib/param/param.h>

 /**
- * Gyro X offset FIXME
+ * Gyro X offset
 *
- * This is an X-axis offset for the gyro.
- * Adjust it according to the calibration data.
+ * This is an X-axis offset for the gyro. Adjust it according to the calibration data.
 *
 * @min -10.0
 * @max 10.0
- * @group Gyro Config
+ * @group Sensor Calibration
 */
 PARAM_DEFINE_FLOAT(SENS_GYRO_XOFF, 0.0f);

 /**
- * Gyro Y offset FIXME with dot.
+ * Gyro Y offset
 *
 * @min -10.0
 * @max 10.0
- * @group Gyro Config
+ * @group Sensor Calibration
 */
 PARAM_DEFINE_FLOAT(SENS_GYRO_YOFF, 0.0f);

 /**
- * Gyro Z offset FIXME
+ * Gyro Z offset
 *
 * @min -5.0
 * @max 5.0
- * @group Gyro Config
+ * @group Sensor Calibration
 */
 PARAM_DEFINE_FLOAT(SENS_GYRO_ZOFF, 0.0f);

+/**
+ * Gyro X scaling
+ *
+ * X-axis scaling.
+ *
+ * @min -1.5
+ * @max 1.5
+ * @group Sensor Calibration
+ */
 PARAM_DEFINE_FLOAT(SENS_GYRO_XSCALE, 1.0f);
+
+/**
+ * Gyro Y scaling
+ *
+ * Y-axis scaling.
+ *
+ * @min -1.5
+ * @max 1.5
+ * @group Sensor Calibration
+ */
 PARAM_DEFINE_FLOAT(SENS_GYRO_YSCALE, 1.0f);
+
+/**
+ * Gyro Z scaling
+ *
+ * Z-axis scaling.
+ *
+ * @min -1.5
+ * @max 1.5
+ * @group Sensor Calibration
+ */
 PARAM_DEFINE_FLOAT(SENS_GYRO_ZSCALE, 1.0f);

+/**
+ * Magnetometer X offset
+ *
+ * This is an X-axis offset for the magnetometer.
+ *
+ * @min -500.0
+ * @max 500.0
+ * @group Sensor Calibration
+ */
 PARAM_DEFINE_FLOAT(SENS_MAG_XOFF, 0.0f);
+
+/**
+ * Magnetometer Y offset
+ *
+ * This is an Y-axis offset for the magnetometer.
+ *
+ * @min -500.0
+ * @max 500.0
+ * @group Sensor Calibration
+ */
 PARAM_DEFINE_FLOAT(SENS_MAG_YOFF, 0.0f);
+
+/**
+ * Magnetometer Z offset
+ *
+ * This is an Z-axis offset for the magnetometer.
+ *
+ * @min -500.0
+ * @max 500.0
+ * @group Sensor Calibration
+ */
 PARAM_DEFINE_FLOAT(SENS_MAG_ZOFF, 0.0f);

 PARAM_DEFINE_FLOAT(SENS_MAG_XSCALE, 1.0f);
@@ -100,16 +158,114 @@ PARAM_DEFINE_INT32(SENS_DPRES_ANA, 0);
 PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0);
 PARAM_DEFINE_INT32(SENS_EXT_MAG_ROT, 0);

+/**
+ * RC Channel 1 Minimum
+ *
+ * Minimum value for RC channel 1
+ *
+ * @min 800.0
+ * @max 1500.0
+ * @group Radio Calibration
+ */
 PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f);
+
+/**
+ * RC Channel 1 Trim
+ *
+ * Mid point value (same as min for throttle)
+ *
+ * @min 800.0
+ * @max 2200.0
+ * @group Radio Calibration
+ */
 PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f);
+
+/**
+ * RC Channel 1 Maximum
+ *
+ * Maximum value for RC channel 1
+ *
+ * @min 1500.0
+ * @max 2200.0
+ * @group Radio Calibration
+ */
 PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f);
+
+/**
+ * RC Channel 1 Reverse
+ *
+ * Set to -1 to reverse channel.
+ *
+ * @min -1.0
+ * @max 1.0
+ * @group Radio Calibration
+ */
 PARAM_DEFINE_FLOAT(RC1_REV, 1.0f);
+
+/**
+ * RC Channel 1 dead zone
+ *
+ * The +- range of this value around the trim value will be considered as zero.
+ *
+ * @min 0.0
+ * @max 100.0
+ * @group Radio Calibration
+ */
 PARAM_DEFINE_FLOAT(RC1_DZ, 10.0f);

-PARAM_DEFINE_FLOAT(RC2_MIN, 1000);
-PARAM_DEFINE_FLOAT(RC2_TRIM, 1500);
-PARAM_DEFINE_FLOAT(RC2_MAX, 2000);
+/**
+ * RC Channel 2 Minimum
+ *
+ * Minimum value for RC channel 2
+ *
+ * @min 800.0
+ * @max 1500.0
+ * @group Radio Calibration
+ */
+PARAM_DEFINE_FLOAT(RC2_MIN, 1000.0f);
+
+/**
+ * RC Channel 2 Trim
+ *
+ * Mid point value (same as min for throttle)
+ *
+ * @min 800.0
+ * @max 2200.0
+ * @group Radio Calibration
+ */
+PARAM_DEFINE_FLOAT(RC2_TRIM, 1500.0f);
+
+/**
+ * RC Channel 2 Maximum
+ *
+ * Maximum value for RC channel 2
+ *
+ * @min 1500.0
+ * @max 2200.0
+ * @group Radio Calibration
+ */
+PARAM_DEFINE_FLOAT(RC2_MAX, 2000.0f);
+
+/**
+ * RC Channel 2 Reverse
+ *
+ * Set to -1 to reverse channel.
+ *
+ * @min -1.0
+ * @max 1.0
+ * @group Radio Calibration
+ */
 PARAM_DEFINE_FLOAT(RC2_REV, 1.0f);
+
+/**
+ * RC Channel 2 dead zone
+ *
+ * The +- range of this value around the trim value will be considered as zero.
+ *
+ * @min 0.0
+ * @max 100.0
+ * @group Radio Calibration
+ */
 PARAM_DEFINE_FLOAT(RC2_DZ, 10.0f);

 PARAM_DEFINE_FLOAT(RC3_MIN, 1000);
@@ -223,6 +379,7 @@ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f);
 /* FMU with PX4IOAR: (3.3f * 52.0f / 5.0f / 4095.0f) */
 PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.00459340659f);
 #endif
+PARAM_DEFINE_FLOAT(BAT_C_SCALING, 0.0124);	/* scaling for 3DR power brick */

 PARAM_DEFINE_INT32(RC_MAP_ROLL, 1);
 PARAM_DEFINE_INT32(RC_MAP_PITCH, 2);
@@ -114,6 +114,7 @@

 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 #define ADC_BATTERY_VOLTAGE_CHANNEL	10
+#define ADC_BATTERY_CURRENT_CHANNEL	-1
 #define ADC_AIRSPEED_VOLTAGE_CHANNEL	11
 #endif

@@ -124,10 +125,8 @@
 #define ADC_AIRSPEED_VOLTAGE_CHANNEL	15
 #endif

-#define BAT_VOL_INITIAL 0.f
-#define BAT_VOL_LOWPASS_1 0.99f
-#define BAT_VOL_LOWPASS_2 0.01f
-#define VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS 3.5f
+#define BATT_V_LOWPASS 0.001f
+#define BATT_V_IGNORE_THRESHOLD 3.5f

 /**
 * HACK - true temperature is much less than indicated temperature in baro,
@@ -215,6 +214,9 @@ private:
 	math::Matrix	_external_mag_rotation;		/**< rotation matrix for the orientation that an external mag is mounted */
 	bool		_mag_is_external;		/**< true if the active mag is on an external board */

+	uint64_t _battery_discharged;			/**< battery discharged current in mA*ms */
+	hrt_abstime _battery_current_timestamp;	/**< timestamp of last battery current reading */
+
 	struct {
 		float min[_rc_max_chan_count];
 		float trim[_rc_max_chan_count];
@@ -265,6 +267,7 @@ private:
 		float rc_fs_thr;

 		float battery_voltage_scaling;
+		float battery_current_scaling;

 	}		_parameters;			/**< local copies of interesting parameters */

@@ -314,6 +317,7 @@ private:
 		param_t rc_fs_thr;

 		param_t battery_voltage_scaling;
+		param_t battery_current_scaling;

 		param_t board_rotation;
 		param_t external_mag_rotation;
@@ -467,7 +471,9 @@ Sensors::Sensors() :

 	_board_rotation(3, 3),
 	_external_mag_rotation(3, 3),
-	_mag_is_external(false)
+	_mag_is_external(false),
+	_battery_discharged(0),
+	_battery_current_timestamp(0)
 {

 	/* basic r/c parameters */
@@ -560,6 +566,7 @@ Sensors::Sensors() :
 	_parameter_handles.diff_pres_analog_enabled = param_find("SENS_DPRES_ANA");

 	_parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING");
+	_parameter_handles.battery_current_scaling = param_find("BAT_C_SCALING");

 	/* rotations */
 	_parameter_handles.board_rotation = param_find("SENS_BOARD_ROT");
@@ -740,6 +747,11 @@ Sensors::parameters_update()
 		warnx("Failed updating voltage scaling param");
 	}

+	/* scaling of ADC ticks to battery current */
+	if (param_get(_parameter_handles.battery_current_scaling, &(_parameters.battery_current_scaling)) != OK) {
+		warnx("Failed updating current scaling param");
+	}
+
 	param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation));
 	param_get(_parameter_handles.external_mag_rotation, &(_parameters.external_mag_rotation));

@@ -1157,17 +1169,16 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 	if (!_publishing)
 		return;

+	hrt_abstime t = hrt_absolute_time();
 	/* rate limit to 100 Hz */
-	if (hrt_absolute_time() - _last_adc >= 10000) {
+	if (t - _last_adc >= 10000) {
 		/* make space for a maximum of eight channels */
 		struct adc_msg_s buf_adc[8];
 		/* read all channels available */
 		int ret = read(_fd_adc, &buf_adc, sizeof(buf_adc));

-		for (unsigned i = 0; i < sizeof(buf_adc) / sizeof(buf_adc[0]); i++) {
-
-			if (ret >= (int)sizeof(buf_adc[0])) {
-
+		if (ret >= (int)sizeof(buf_adc[0])) {
+			for (unsigned i = 0; i < sizeof(buf_adc) / sizeof(buf_adc[0]); i++) {
 				/* Save raw voltage values */
 				if (i < (sizeof(raw.adc_voltage_v)) / sizeof(raw.adc_voltage_v[0])) {
 					raw.adc_voltage_v[i] = buf_adc[i].am_data / (4096.0f / 3.3f);
@@ -1178,27 +1189,40 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 					/* Voltage in volts */
 					float voltage = (buf_adc[i].am_data * _parameters.battery_voltage_scaling);

-					if (voltage > VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS) {
-
+					if (voltage > BATT_V_IGNORE_THRESHOLD) {
+						_battery_status.voltage_v = voltage;
 						/* one-time initialization of low-pass value to avoid long init delays */
-						if (_battery_status.voltage_v < 3.0f) {
-							_battery_status.voltage_v = voltage;
+						if (_battery_status.voltage_filtered_v < BATT_V_IGNORE_THRESHOLD) {
+							_battery_status.voltage_filtered_v = voltage;
 						}

-						_battery_status.timestamp = hrt_absolute_time();
-						_battery_status.voltage_v = (BAT_VOL_LOWPASS_1 * (_battery_status.voltage_v + BAT_VOL_LOWPASS_2 * voltage));;
-						/* current and discharge are unknown */
-						_battery_status.current_a = -1.0f;
-						_battery_status.discharged_mah = -1.0f;
+						_battery_status.timestamp = t;
+						_battery_status.voltage_filtered_v += (voltage - _battery_status.voltage_filtered_v) * BATT_V_LOWPASS;

-						/* announce the battery voltage if needed, just publish else */
-						if (_battery_pub > 0) {
-							orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status);
+					} else {
+						/* mark status as invalid */
+						_battery_status.voltage_v = -1.0f;
+						_battery_status.voltage_filtered_v = -1.0f;
+					}

-						} else {
-							_battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status);
+				} else if (ADC_BATTERY_CURRENT_CHANNEL == buf_adc[i].am_channel) {
+					/* handle current only if voltage is valid */
+					if (_battery_status.voltage_v > 0.0f) {
+						float current = (buf_adc[i].am_data * _parameters.battery_current_scaling);
+						/* check measured current value */
+						if (current >= 0.0f) {
+							_battery_status.timestamp = t;
+							_battery_status.current_a = current;
+							if (_battery_current_timestamp != 0) {
+								/* initialize discharged value */
+								if (_battery_status.discharged_mah < 0.0f)
+									_battery_status.discharged_mah = 0.0f;
+								_battery_discharged += current * (t - _battery_current_timestamp);
+								_battery_status.discharged_mah = ((float) _battery_discharged) / 3600000.0f;
+							}
 						}
 					}
+					_battery_current_timestamp = t;

 				} else if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) {

@@ -1214,7 +1238,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw)

 						float diff_pres_pa = voltage * 1000.0f - _parameters.diff_pres_offset_pa; //for MPXV7002DP sensor

-						_diff_pres.timestamp = hrt_absolute_time();
+						_diff_pres.timestamp = t;
 						_diff_pres.differential_pressure_pa = diff_pres_pa;
 						_diff_pres.voltage = voltage;

@@ -1227,8 +1251,16 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 						}
 					}
 				}
+			}
+			_last_adc = t;
+			if (_battery_status.voltage_v > 0.0f) {
+				/* announce the battery status if needed, just publish else */
+				if (_battery_pub > 0) {
+					orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status);

-				_last_adc = hrt_absolute_time();
+				} else {
+					_battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status);
+				}
 			}
 		}
 	}
@@ -1516,7 +1548,10 @@ Sensors::task_main()
 	raw.adc_voltage_v[3] = 0.0f;

 	memset(&_battery_status, 0, sizeof(_battery_status));
-	_battery_status.voltage_v = BAT_VOL_INITIAL;
+	_battery_status.voltage_v = 0.0f;
+	_battery_status.voltage_filtered_v = 0.0f;
+	_battery_status.current_a = -1.0f;
+	_battery_status.discharged_mah = -1.0f;

 	/* get a set of initial values */
 	accel_poll(raw);
@@ -0,0 +1,60 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-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
+ * 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 board_serial.h
+ * Read off the board serial
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author David "Buzz" Bussenschutt <davidbuzz@gmail.com>
+ *
+ */
+
+#include "otp.h"
+#include "board_config.h"
+#include "board_serial.h"
+
+int get_board_serial(char *serialid)
+{
+	const volatile unsigned *udid_ptr = (const unsigned *)UDID_START;
+	union udid id;
+	val_read((unsigned *)&id, udid_ptr, sizeof(id));
+
+
+	/* Copy the serial from the chips non-write memory and swap endianess */
+	serialid[0] = id.data[3];   serialid[1] = id.data[2];  serialid[2] = id.data[1];  serialid[3] = id.data[0];
+	serialid[4] = id.data[7];   serialid[5] = id.data[6];  serialid[6] = id.data[5];  serialid[7] = id.data[4];
+	serialid[8] = id.data[11];   serialid[9] = id.data[10];  serialid[10] = id.data[9];  serialid[11] = id.data[8];
+
+	return 0;
+}
@@ -0,0 +1,49 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-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
+ * 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 board_serial.h
+ * Read off the board serial
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author David "Buzz" Bussenschutt <davidbuzz@gmail.com>
+ *
+ */
+
+#pragma once
+
+__BEGIN_DECLS
+
+__EXPORT int get_board_serial(char *serialid);
+
+__END_DECLS
@@ -407,6 +407,9 @@ bson_encoder_fini(bson_encoder_t encoder)
 		memcpy(encoder->buf, &len, sizeof(len));
 	}

+	/* sync file */
+	fsync(encoder->fd);
+
 	return 0;
 }

--- a/Show More
+++ b/Show More