Merge remote-tracking branch 'upstream/master' into navigator_new

Conflicts:
	makefiles/config_px4fmu-v1_backside.mk
	src/modules/commander/commander.cpp
	src/modules/sdlog2/sdlog2.c

55 files changed, 1955 insertions, 1123 deletions

diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 90f2e2b17..d66c2c54c 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -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
diff --git a/Tools/px4params/dokuwikiout.py b/Tools/px4params/dokuwikiout.py
index 33f76b415..4d40a6201 100644
--- a/Tools/px4params/dokuwikiout.py
+++ b/Tools/px4params/dokuwikiout.py
@@ -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
diff --git a/Tools/px4params/dokuwikiout_listings.py b/Tools/px4params/dokuwikiout_listings.py
new file mode 100644
index 000000000..33f76b415
--- /dev/null
+++ b/Tools/px4params/dokuwikiout_listings.py
@@ -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
diff --git a/Tools/tests-host/Makefile b/Tools/tests-host/Makefile
index 97410ff47..7ab1454f0 100644
--- a/Tools/tests-host/Makefile
+++ b/Tools/tests-host/Makefile
@@ -10,11 +10,13 @@ LIBS=-lm
 #_DEPS = test.h
 #DEPS = $(patsubst %,$(IDIR)/%,$(_DEPS))
 
-_OBJ = mixer_test.o test_mixer.o mixer_simple.o mixer_multirotor.o mixer.o mixer_group.o mixer_load.o
+_OBJ = mixer_test.o test_mixer.o mixer_simple.o mixer_multirotor.o \
+	mixer.o mixer_group.o mixer_load.o test_conv.o pwm_limit.o hrt.o
 OBJ = $(patsubst %,$(ODIR)/%,$(_OBJ))
 
 #$(DEPS)
 $(ODIR)/%.o: %.cpp
+	mkdir -p obj
 	$(CC) -c -o $@ $< $(CFLAGS)
 
 $(ODIR)/%.o: ../../src/systemcmds/tests/%.cpp
@@ -26,6 +28,12 @@ $(ODIR)/%.o: ../../src/modules/systemlib/%.cpp
 $(ODIR)/%.o: ../../src/modules/systemlib/mixer/%.cpp
 	$(CC) -c -o $@ $< $(CFLAGS)
 
+$(ODIR)/%.o: ../../src/modules/systemlib/pwm_limit/%.cpp
+	$(CC) -c -o $@ $< $(CFLAGS)
+
+$(ODIR)/%.o: ../../src/modules/systemlib/pwm_limit/%.c
+	$(CC) -c -o $@ $< $(CFLAGS)
+
 $(ODIR)/%.o: ../../src/modules/systemlib/mixer/%.c
 	$(CC) -c -o $@ $< $(CFLAGS)
 
diff --git a/Tools/tests-host/hrt.cpp b/Tools/tests-host/hrt.cpp
new file mode 100644
index 000000000..01b5958b7
--- /dev/null
+++ b/Tools/tests-host/hrt.cpp
@@ -0,0 +1,16 @@
+#include <sys/time.h>
+#include <inttypes.h>
+#include <drivers/drv_hrt.h>
+#include <stdio.h>
+
+hrt_abstime hrt_absolute_time() {
+    struct timeval te; 
+    gettimeofday(&te, NULL); // get current time
+    hrt_abstime us = static_cast<uint64_t>(te.tv_sec) * 1e6 + te.tv_usec; // caculate us
+    return us;
+}
+
+hrt_abstime hrt_elapsed_time(const volatile hrt_abstime *then) {
+	// not thread safe
+    return hrt_absolute_time() - *then;
+}
diff --git a/Tools/tests-host/mixer_test.cpp b/Tools/tests-host/mixer_test.cpp
index 042322aad..e311617f9 100644
--- a/Tools/tests-host/mixer_test.cpp
+++ b/Tools/tests-host/mixer_test.cpp
@@ -9,4 +9,6 @@ int main(int argc, char *argv[]) {
 				 "../../ROMFS/px4fmu_common/mixers/FMU_quad_w.mix"};
 
 	test_mixer(3, args);
+
+	test_conv(1, args);
 }
\ No newline at end of file
diff --git a/Tools/tests-host/queue.h b/Tools/tests-host/queue.h
new file mode 100644
index 000000000..0fdb170db
--- /dev/null
+++ b/Tools/tests-host/queue.h
@@ -0,0 +1,133 @@
+/************************************************************************
+ * include/queue.h
+ *
+ *   Copyright (C) 2007-2009 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * 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 NuttX 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.
+ *
+ ************************************************************************/
+
+#ifndef __INCLUDE_QUEUE_H
+#define __INCLUDE_QUEUE_H
+
+#ifndef FAR
+#define FAR
+#endif
+
+/************************************************************************
+ * Included Files
+ ************************************************************************/
+
+#include <sys/types.h>
+
+/************************************************************************
+ * Pre-processor Definitions
+ ************************************************************************/
+
+#define sq_init(q) do { (q)->head = NULL; (q)->tail = NULL; } while (0)
+#define dq_init(q) do { (q)->head = NULL; (q)->tail = NULL; } while (0)
+
+#define sq_next(p) ((p)->flink)
+#define dq_next(p) ((p)->flink)
+#define dq_prev(p) ((p)->blink)
+
+#define sq_empty(q) ((q)->head == NULL)
+#define dq_empty(q) ((q)->head == NULL)
+
+#define sq_peek(q)  ((q)->head)
+#define dq_peek(q)  ((q)->head)
+
+/************************************************************************
+ * Global Type Declarations
+ ************************************************************************/
+
+struct sq_entry_s
+{
+  FAR struct sq_entry_s *flink;
+};
+typedef struct sq_entry_s sq_entry_t;
+
+struct dq_entry_s
+{
+  FAR struct dq_entry_s *flink;
+  FAR struct dq_entry_s *blink;
+};
+typedef struct dq_entry_s dq_entry_t;
+
+struct sq_queue_s
+{
+  FAR sq_entry_t *head;
+  FAR sq_entry_t *tail;
+};
+typedef struct sq_queue_s  sq_queue_t;
+
+struct dq_queue_s
+{
+  FAR dq_entry_t *head;
+  FAR dq_entry_t *tail;
+};
+typedef struct dq_queue_s dq_queue_t;
+
+/************************************************************************
+ * Global Function Prototypes
+ ************************************************************************/
+
+#ifdef __cplusplus
+#define EXTERN extern "C"
+extern "C" {
+#else
+#define EXTERN extern
+#endif
+
+EXTERN void  sq_addfirst(FAR sq_entry_t *node, sq_queue_t *queue);
+EXTERN void  dq_addfirst(FAR dq_entry_t *node, dq_queue_t *queue);
+EXTERN void  sq_addlast(FAR sq_entry_t *node, sq_queue_t *queue);
+EXTERN void  dq_addlast(FAR dq_entry_t *node, dq_queue_t *queue);
+EXTERN void  sq_addafter(FAR sq_entry_t *prev, FAR sq_entry_t *node,
+                         sq_queue_t *queue);
+EXTERN void  dq_addafter(FAR dq_entry_t *prev, FAR dq_entry_t *node,
+                         dq_queue_t *queue);
+EXTERN void  dq_addbefore(FAR dq_entry_t *next, FAR dq_entry_t *node,
+                          dq_queue_t *queue);
+
+EXTERN FAR sq_entry_t *sq_remafter(FAR sq_entry_t *node, sq_queue_t *queue);
+EXTERN void            sq_rem(FAR sq_entry_t *node, sq_queue_t *queue);
+EXTERN void            dq_rem(FAR dq_entry_t *node, dq_queue_t *queue);
+EXTERN FAR sq_entry_t *sq_remlast(sq_queue_t *queue);
+EXTERN FAR dq_entry_t *dq_remlast(dq_queue_t *queue);
+EXTERN FAR sq_entry_t *sq_remfirst(sq_queue_t *queue);
+EXTERN FAR dq_entry_t *dq_remfirst(dq_queue_t *queue);
+
+#undef EXTERN
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __INCLUDE_QUEUE_H_ */
+
diff --git a/makefiles/config_px4fmu-v1_backside.mk b/makefiles/config_px4fmu-v1_backside.mk
deleted file mode 100644
index 6d2a9f7bd..000000000
--- a/makefiles/config_px4fmu-v1_backside.mk
+++ /dev/null
@@ -1,148 +0,0 @@
-#
-# Makefile for the px4fmu_default configuration
-#
-
-#
-# Use the configuration's ROMFS.
-#
-ROMFS_ROOT	 = $(PX4_BASE)/ROMFS/px4fmu_common
-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/px4io
-MODULES		+= drivers/px4fmu
-MODULES		+= drivers/boards/px4fmu-v1
-MODULES		+= drivers/ardrone_interface
-MODULES		+= drivers/l3gd20
-MODULES		+= drivers/bma180
-MODULES		+= drivers/mpu6000
-MODULES		+= drivers/hmc5883
-MODULES		+= drivers/ms5611
-MODULES		+= drivers/gps
-MODULES		+= drivers/hil
-MODULES		+= drivers/blinkm
-MODULES		+= drivers/rgbled
-MODULES		+= drivers/roboclaw
-MODULES		+= drivers/airspeed
-MODULES		+= drivers/ets_airspeed
-MODULES		+= drivers/meas_airspeed
-MODULES		+= modules/sensors
-
-#
-# System commands
-#
-MODULES		+= systemcmds/eeprom
-MODULES		+= systemcmds/ramtron
-MODULES		+= systemcmds/bl_update
-MODULES		+= systemcmds/boardinfo
-MODULES		+= systemcmds/i2c
-MODULES		+= systemcmds/mixer
-MODULES		+= systemcmds/param
-MODULES		+= systemcmds/perf
-MODULES		+= systemcmds/preflight_check
-MODULES		+= systemcmds/pwm
-MODULES		+= systemcmds/esc_calib
-MODULES		+= systemcmds/reboot
-MODULES		+= systemcmds/top
-MODULES		+= systemcmds/tests
-MODULES		+= systemcmds/config
-MODULES		+= systemcmds/nshterm
-
-#
-# General system control
-#
-MODULES		+= modules/commander
-MODULES		+= modules/navigator
-MODULES		+= modules/mavlink
-MODULES		+= modules/mavlink_onboard
-MODULES		+= modules/gpio_led
-
-#
-# Estimation modules (EKF/ SO3 / other filters)
-#
-MODULES		+= modules/att_pos_estimator_ekf
-
-#
-# Vehicle Control
-#
-MODULES 		+= modules/fixedwing_backside
-
-#
-# Logging
-#
-MODULES		+= modules/sdlog2
-
-#
-# Unit tests
-#
-#MODULES 	+= modules/unit_test
-#MODULES 	+= modules/commander/commander_tests
-
-#
-# Library modules
-#
-MODULES		+= modules/systemlib
-MODULES		+= modules/systemlib/mixer
-MODULES		+= modules/controllib
-MODULES		+= modules/uORB
-MODULES		+= modules/dataman
-
-#
-# Libraries
-#
-LIBRARIES	+= lib/mathlib/CMSIS
-MODULES		+= lib/mathlib
-MODULES		+= lib/mathlib/math/filter
-MODULES		+= lib/ecl
-MODULES		+= lib/external_lgpl
-MODULES		+= lib/geo
-MODULES		+= lib/conversion
-MODULES		+= lib/launchdetection
-
-#
-# Demo apps
-#
-#MODULES		+= examples/math_demo
-# Tutorial code from
-# https://pixhawk.ethz.ch/px4/dev/hello_sky
-#MODULES		+= examples/px4_simple_app
-
-# Tutorial code from
-# https://pixhawk.ethz.ch/px4/dev/daemon
-#MODULES		+= examples/px4_daemon_app
-
-# Tutorial code from
-# https://pixhawk.ethz.ch/px4/dev/debug_values
-#MODULES		+= examples/px4_mavlink_debug
-
-# Tutorial code from
-# https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control
-#MODULES			+= examples/fixedwing_control
-
-# Hardware test
-#MODULES			+= examples/hwtest
-
-#
-# 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                ) \
-	$(call _B, sysinfo,                ,                          2048,  sysinfo_main               )
diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk
index 573b23221..afe5d2dc6 100644
--- a/makefiles/config_px4fmu-v1_default.mk
+++ b/makefiles/config_px4fmu-v1_default.mk
@@ -42,8 +42,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
diff --git a/makefiles/config_px4fmu-v1_test.mk b/makefiles/config_px4fmu-v1_test.mk
deleted file mode 100644
index 41e8b95ff..000000000
--- a/makefiles/config_px4fmu-v1_test.mk
+++ /dev/null
@@ -1,48 +0,0 @@
-#
-# 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
-
-#
-# 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                )
diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk
index a2dc897c9..23c2dc4ef 100644
--- a/makefiles/config_px4fmu-v2_default.mk
+++ b/makefiles/config_px4fmu-v2_default.mk
@@ -46,7 +46,6 @@ MODULES		+= modules/sensors
 #
 # System commands
 #
-MODULES		+= systemcmds/ramtron
 MODULES		+= systemcmds/bl_update
 MODULES		+= systemcmds/boardinfo
 MODULES		+= systemcmds/mixer
@@ -60,6 +59,7 @@ MODULES		+= systemcmds/top
 MODULES		+= systemcmds/tests
 MODULES		+= systemcmds/config
 MODULES		+= systemcmds/nshterm
+MODULES		+= systemcmds/mtd
 
 #
 # General system control
diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk
index 6faef7e0a..f54a4d825 100644
--- a/makefiles/config_px4fmu-v2_test.mk
+++ b/makefiles/config_px4fmu-v2_test.mk
@@ -22,6 +22,7 @@ MODULES		+= systemcmds/perf
 MODULES		+= systemcmds/reboot
 MODULES		+= systemcmds/tests
 MODULES		+= systemcmds/nshterm
+MODULES		+= systemcmds/mtd
 
 #
 # Library modules
diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig
index e60120b49..1dc96b3c3 100644
--- a/nuttx-configs/px4fmu-v1/nsh/defconfig
+++ b/nuttx-configs/px4fmu-v1/nsh/defconfig
@@ -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
@@ -483,6 +483,25 @@ CONFIG_USART1_SERIAL_CONSOLE=y
 # 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
 #
 CONFIG_USART1_RXBUFSIZE=512
diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig
index c0ed11a62..2a734c27e 100644
--- a/nuttx-configs/px4fmu-v2/nsh/defconfig
+++ b/nuttx-configs/px4fmu-v2/nsh/defconfig
@@ -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
diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h
index 4972e6914..7cfca7656 100644
--- a/src/drivers/boards/px4fmu-v2/board_config.h
+++ b/src/drivers/boards/px4fmu-v2/board_config.h
@@ -75,7 +75,7 @@ __BEGIN_DECLS
 /* PX4FMU GPIOs ***********************************************************************************/
 /* LEDs */
 
-#define GPIO_LED1		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN12)
+#define GPIO_LED1		(GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN12)
 
 /* External interrupts */
 #define GPIO_EXTI_GYRO_DRDY	(GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN0)
@@ -87,7 +87,7 @@ __BEGIN_DECLS
 #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_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)
@@ -98,7 +98,7 @@ __BEGIN_DECLS
 #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_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)
diff --git a/src/drivers/boards/px4io-v1/board_config.h b/src/drivers/boards/px4io-v1/board_config.h
index c3f39addf..1be4877ba 100644
--- a/src/drivers/boards/px4io-v1/board_config.h
+++ b/src/drivers/boards/px4io-v1/board_config.h
@@ -58,11 +58,11 @@
 /* PX4IO GPIOs **********************************************************************/
 /* LEDs */
 
-#define GPIO_LED1       (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\
+#define GPIO_LED1       (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|\
                          GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN14)
-#define GPIO_LED2       (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\
+#define GPIO_LED2       (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|\
                          GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN15)
-#define GPIO_LED3       (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\
+#define GPIO_LED3       (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|\
                          GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
 
 /* Safety switch button *************************************************************/
diff --git a/src/drivers/boards/px4io-v2/board_config.h b/src/drivers/boards/px4io-v2/board_config.h
index 8da555211..8152ea4d4 100644
--- a/src/drivers/boards/px4io-v2/board_config.h
+++ b/src/drivers/boards/px4io-v2/board_config.h
@@ -74,9 +74,9 @@
 
 /* LEDS  **********************************************************************/
 
-#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14)
-#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15)
-#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13)
+#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14)
+#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15)
+#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13)
 
 /* Safety switch button *******************************************************/
 
diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index fc500a9ec..6b72d560f 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -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) {
+
+			_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;
+
+			//no time and satellite information simulated
+
+			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);
+			}
 
-		switch (_mode) {
-		case GPS_DRIVER_MODE_UBX:
-			_Helper = new UBX(_serial_fd, &_report);
-			break;
+			usleep(2e5);
 
-		case GPS_DRIVER_MODE_MTK:
-			_Helper = new MTK(_serial_fd, &_report);
-			break;
+		} else {
 
-		default:
-			break;
-		}
+			if (_Helper != nullptr) {
+				delete(_Helper);
+				/* set to zero to ensure parser is not used while not instantiated */
+				_Helper = nullptr;
+			}
 
-		unlock();
+			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;
+			}
 
-		if (_Helper->configure(_baudrate) == 0) {
 			unlock();
 
-			// GPS is obviously detected successfully, reset statistics
-			_Helper->reset_update_rates();
+			if (_Helper->configure(_baudrate) == 0) {
+				unlock();
 
-			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);
+				// GPS is obviously detected successfully, reset statistics
+				_Helper->reset_update_rates();
 
-				} else {
-					_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
-				}
+				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++;
+					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();
-				}
+					/* 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";
+					if (!_healthy) {
+						char *mode_str = "unknown";
 
-					switch (_mode) {
-					case GPS_DRIVER_MODE_UBX:
-						mode_str = "UBX";
-						break;
+						switch (_mode) {
+						case GPS_DRIVER_MODE_UBX:
+							mode_str = "UBX";
+							break;
 
-					case GPS_DRIVER_MODE_MTK:
-						mode_str = "MTK";
-						break;
+						case GPS_DRIVER_MODE_MTK:
+							mode_str = "MTK";
+							break;
 
-					default:
-						break;
+						default:
+							break;
+						}
+
+						warnx("module found: %s", mode_str);
+						_healthy = true;
 					}
+				}
 
-					warnx("module found: %s", mode_str);
-					_healthy = true;
+				if (_healthy) {
+					warnx("module lost");
+					_healthy = false;
+					_rate = 0.0f;
 				}
-			}
 
-			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]");
 }
diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp
index c1d73dd87..0a047f38f 100644
--- a/src/drivers/hil/hil.cpp
+++ b/src/drivers/hil/hil.cpp
@@ -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;
 }
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index b28d318d7..4b1b0ed0b 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -1108,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);
@@ -1124,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);
@@ -1160,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
 }
diff --git a/src/drivers/px4fmu/module.mk b/src/drivers/px4fmu/module.mk
index d918abd57..05bc7a5b3 100644
--- a/src/drivers/px4fmu/module.mk
+++ b/src/drivers/px4fmu/module.mk
@@ -3,5 +3,4 @@
 #
 
 MODULE_COMMAND	 = fmu
-SRCS		 = fmu.cpp \
-			../../modules/systemlib/pwm_limit/pwm_limit.c
+SRCS		 = fmu.cpp
diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp
index 727c86e02..4f58891ed 100644
--- a/src/drivers/rgbled/rgbled.cpp
+++ b/src/drivers/rgbled/rgbled.cpp
@@ -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>");
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 1e5318121..dbfca38b1 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -106,14 +106,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));
@@ -888,14 +880,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);
 			}
 		}
 
@@ -948,46 +938,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;
-
-				if (armed.armed) {
-					arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed);
+			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;
 
-				} else {
-					arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed);
-				}
+			if (armed.armed) {
+				arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed);
 
-				status_changed = true;
+			} else {
+				arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed);
 			}
 
-			critical_voltage_counter++;
-
-		} else {
-
-			low_voltage_counter = 0;
-			critical_voltage_counter = 0;
+			status_changed = true;
 		}
 
 		/* End battery voltage check */
diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp
index 565b4b66a..21a1c4c2c 100644
--- a/src/modules/commander/commander_helper.cpp
+++ b/src/modules/commander/commander_helper.cpp
@@ -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;
diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h
index e9514446c..d0393f45a 100644
--- a/src/modules/commander/commander_helper.h
+++ b/src/modules/commander/commander_helper.h
@@ -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_ */
diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c
index 691d3efcb..d3155f7bf 100644
--- a/src/modules/commander/commander_params.c
+++ b/src/modules/commander/commander_params.c
@@ -48,6 +48,7 @@
 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);
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
index 9f46b5170..7954d75c2 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
@@ -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);
 
 
diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c
index 4d3c9dd2c..4f8091716 100644
--- a/src/modules/mavlink/mavlink.c
+++ b/src/modules/mavlink/mavlink.c
@@ -680,8 +680,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,
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 7083ce5a2..e16cadd29 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -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>
  *
@@ -704,6 +704,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));
@@ -729,6 +730,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 */
@@ -755,6 +757,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)
@@ -763,9 +766,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 = 20;
-	/* 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];
@@ -890,6 +893,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.
@@ -984,8 +993,6 @@ int sdlog2_thread_main(int argc, char *argv[])
 				log_msg.body.log_STAT.main_state = (uint8_t) buf.control_mode.main_state;
 				log_msg.body.log_STAT.navigation_state = (uint8_t) buf.control_mode.nav_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;
@@ -1251,6 +1258,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 */
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index 3afaaa2ad..d80027cbf 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -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;
@@ -247,6 +245,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 {
@@ -280,7 +287,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"),
@@ -290,6 +297,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(GPSP, "BLLffBfbfff", "AltRel,Lat,Lon,Alt,Yaw,NavCmd,LoitR,LoitDir,AccR,TimeIn,PitMin"),
 	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"),
diff --git a/src/modules/sensors/module.mk b/src/modules/sensors/module.mk
index ebbc580e1..aa538fd6b 100644
--- a/src/modules/sensors/module.mk
+++ b/src/modules/sensors/module.mk
@@ -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
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index 763723554..bbc84ef93 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -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);
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 9baf1a6af..c78b9b361 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -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 = hrt_absolute_time();
+			}
+			_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);
+
+				} 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);
diff --git a/src/modules/systemlib/bson/tinybson.c b/src/modules/systemlib/bson/tinybson.c
index 8aca6a25d..49403c98b 100644
--- a/src/modules/systemlib/bson/tinybson.c
+++ b/src/modules/systemlib/bson/tinybson.c
@@ -407,6 +407,9 @@ bson_encoder_fini(bson_encoder_t encoder)
 		memcpy(encoder->buf, &len, sizeof(len));
 	}
 
+	/* sync file */
+	fsync(encoder->fd);
+
 	return 0;
 }
 
diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk
index 8c6c300d6..3953b757d 100644
--- a/src/modules/systemlib/module.mk
+++ b/src/modules/systemlib/module.mk
@@ -51,5 +51,6 @@ SRCS		 = err.c \
 		   mavlink_log.c \
 		   rc_check.c \
 		   otp.c \
-		   board_serial.c
+		   board_serial.c \
+		   pwm_limit/pwm_limit.c
 
diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c
index 398657dd7..2d773fd25 100644
--- a/src/modules/systemlib/param/param.c
+++ b/src/modules/systemlib/param/param.c
@@ -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
@@ -61,7 +61,7 @@
 #include "uORB/uORB.h"
 #include "uORB/topics/parameter_update.h"
 
-#if 1
+#if 0
 # define debug(fmt, args...)		do { warnx(fmt, ##args); } while(0)
 #else
 # define debug(fmt, args...)		do { } while(0)
@@ -512,6 +512,28 @@ param_save_default(void)
 	int fd;
 
 	const char *filename = param_get_default_file();
+
+	/* write parameters to temp file */
+	fd = open(filename, O_WRONLY | O_CREAT);
+
+	if (fd < 0) {
+		warn("failed to open param file: %s", filename);
+		return ERROR;
+	}
+
+	if (res == OK) {
+		res = param_export(fd, false);
+
+	        if (res != OK) {
+	            warnx("failed to write parameters to file: %s", filename);
+	        }
+	}
+
+	close(fd);
+
+	return res;
+
+#if 0
 	const char *filename_tmp = malloc(strlen(filename) + 5);
 	sprintf(filename_tmp, "%s.tmp", filename);
 
@@ -565,6 +587,7 @@ param_save_default(void)
 	free(filename_tmp);
 
 	return res;
+#endif
 }
 
 /**
@@ -573,9 +596,9 @@ param_save_default(void)
 int
 param_load_default(void)
 {
-	int fd = open(param_get_default_file(), O_RDONLY);
+	int fd_load = open(param_get_default_file(), O_RDONLY);
 
-	if (fd < 0) {
+	if (fd_load < 0) {
 		/* no parameter file is OK, otherwise this is an error */
 		if (errno != ENOENT) {
 			warn("open '%s' for reading failed", param_get_default_file());
@@ -584,8 +607,8 @@ param_load_default(void)
 		return 1;
 	}
 
-	int result = param_load(fd);
-	close(fd);
+	int result = param_load(fd_load);
+	close(fd_load);
 
 	if (result != 0) {
 		warn("error reading parameters from '%s'", param_get_default_file());
diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.c b/src/modules/systemlib/pwm_limit/pwm_limit.c
index cac3dc82a..190b315f1 100644
--- a/src/modules/systemlib/pwm_limit/pwm_limit.c
+++ b/src/modules/systemlib/pwm_limit/pwm_limit.c
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
  *   Author: Julian Oes <joes@student.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
@@ -44,38 +44,53 @@
 #include <math.h>
 #include <stdbool.h>
 #include <drivers/drv_hrt.h>
+#include <stdio.h>
 
 void pwm_limit_init(pwm_limit_t *limit)
 {
-	limit->state = LIMIT_STATE_OFF;
+	limit->state = PWM_LIMIT_STATE_INIT;
 	limit->time_armed = 0;
 	return;
 }
 
-void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, float *output, uint16_t *effective_pwm, pwm_limit_t *limit)
+void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, const float *output, uint16_t *effective_pwm, pwm_limit_t *limit)
 {
+
 	/* first evaluate state changes */
 	switch (limit->state) {
-		case LIMIT_STATE_OFF:
-			if (armed)
-				limit->state = LIMIT_STATE_RAMP;
-			limit->time_armed = hrt_absolute_time();
+		case PWM_LIMIT_STATE_INIT:
+
+			if (armed) {
+
+				/* set arming time for the first call */
+				if (limit->time_armed == 0) {
+					limit->time_armed = hrt_absolute_time();
+				}
+
+				if (hrt_elapsed_time(&limit->time_armed) >= INIT_TIME_US) {
+					limit->state = PWM_LIMIT_STATE_OFF;
+				}
+			}
 			break;
-		case LIMIT_STATE_INIT:
-			if (!armed)
-				limit->state = LIMIT_STATE_OFF;
-			else if (hrt_absolute_time() - limit->time_armed >= INIT_TIME_US)
-				limit->state = LIMIT_STATE_RAMP;
+		case PWM_LIMIT_STATE_OFF:
+			if (armed) {
+				limit->state = PWM_LIMIT_STATE_RAMP;
+
+				/* reset arming time, used for ramp timing */
+				limit->time_armed = hrt_absolute_time();
+			}
 			break;
-		case LIMIT_STATE_RAMP:
-			if (!armed)
-				limit->state = LIMIT_STATE_OFF;
-			else if (hrt_absolute_time() - limit->time_armed >= INIT_TIME_US + RAMP_TIME_US)
-				limit->state = LIMIT_STATE_ON;
+		case PWM_LIMIT_STATE_RAMP:
+			if (!armed) {
+				limit->state = PWM_LIMIT_STATE_OFF;
+			} else if (hrt_elapsed_time(&limit->time_armed) >= RAMP_TIME_US) {
+				limit->state = PWM_LIMIT_STATE_ON;
+			}
 			break;
-		case LIMIT_STATE_ON:
-			if (!armed)
-				limit->state = LIMIT_STATE_OFF;
+		case PWM_LIMIT_STATE_ON:
+			if (!armed) {
+				limit->state = PWM_LIMIT_STATE_OFF;
+			}
 			break;
 		default:
 			break;
@@ -86,44 +101,47 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_
 
 	/* then set effective_pwm based on state */
 	switch (limit->state) {
-		case LIMIT_STATE_OFF:
-		case LIMIT_STATE_INIT:
+		case PWM_LIMIT_STATE_OFF:
+		case PWM_LIMIT_STATE_INIT:
 			for (unsigned i=0; i<num_channels; i++) {
 				effective_pwm[i] = disarmed_pwm[i];
-				output[i] = 0.0f;
 			}
 			break;
-		case LIMIT_STATE_RAMP:
+		case PWM_LIMIT_STATE_RAMP:
+			{
+				hrt_abstime diff = hrt_elapsed_time(&limit->time_armed);
 
-			progress = (hrt_absolute_time() - INIT_TIME_US - limit->time_armed)*10000 / RAMP_TIME_US;
-			for (unsigned i=0; i<num_channels; i++) {
-                
-				uint16_t ramp_min_pwm;
-                
-				/* if a disarmed pwm value was set, blend between disarmed and min */
-				if (disarmed_pwm[i] > 0) {
-
-					/* safeguard against overflows */
-					uint16_t disarmed = disarmed_pwm[i];
-					if (disarmed > min_pwm[i])
-						disarmed = min_pwm[i];
-
-					uint16_t disarmed_min_diff = min_pwm[i] - disarmed;
-					ramp_min_pwm = disarmed + (disarmed_min_diff * progress) / 10000;
-				} else {
-                    
-					/* no disarmed pwm value set, choose min pwm */
-					ramp_min_pwm = min_pwm[i];
-				}
+				progress = diff * 10000 / RAMP_TIME_US;
+
+				for (unsigned i=0; i<num_channels; i++) {
+	                
+					uint16_t ramp_min_pwm;
+	                
+					/* if a disarmed pwm value was set, blend between disarmed and min */
+					if (disarmed_pwm[i] > 0) {
 
-				effective_pwm[i] = output[i] * (max_pwm[i] - ramp_min_pwm)/2 + (max_pwm[i] + ramp_min_pwm)/2;
-				output[i] = (float)progress/10000.0f * output[i];
+						/* safeguard against overflows */
+						unsigned disarmed = disarmed_pwm[i];
+						if (disarmed > min_pwm[i]) {
+							disarmed = min_pwm[i];
+						}
+
+						unsigned disarmed_min_diff = min_pwm[i] - disarmed;
+						ramp_min_pwm = disarmed + (disarmed_min_diff * progress) / 10000;
+
+					} else {
+	                    
+						/* no disarmed pwm value set, choose min pwm */
+						ramp_min_pwm = min_pwm[i];
+					}
+
+					effective_pwm[i] = output[i] * (max_pwm[i] - ramp_min_pwm)/2 + (max_pwm[i] + ramp_min_pwm)/2;
+				}
 			}
 			break;
-		case LIMIT_STATE_ON:
+		case PWM_LIMIT_STATE_ON:
 			for (unsigned i=0; i<num_channels; i++) {
 				effective_pwm[i] = output[i] * (max_pwm[i] - min_pwm[i])/2 + (max_pwm[i] + min_pwm[i])/2;
-				/* effective_output stays the same */
 			}
 			break;
 		default:
diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.h b/src/modules/systemlib/pwm_limit/pwm_limit.h
index 9974770be..6a667ac6f 100644
--- a/src/modules/systemlib/pwm_limit/pwm_limit.h
+++ b/src/modules/systemlib/pwm_limit/pwm_limit.h
@@ -46,6 +46,8 @@
 #include <stdint.h>
 #include <stdbool.h>
 
+__BEGIN_DECLS
+
 /*
  * time for the ESCs to initialize
  * (this is not actually needed if PWM is sent right after boot)
@@ -56,21 +58,21 @@
  */
 #define RAMP_TIME_US 2500000
 
+enum pwm_limit_state {
+	PWM_LIMIT_STATE_OFF = 0,
+	PWM_LIMIT_STATE_INIT,
+	PWM_LIMIT_STATE_RAMP,
+	PWM_LIMIT_STATE_ON
+};
+
 typedef struct {
-	enum {
-		LIMIT_STATE_OFF = 0,
-		LIMIT_STATE_INIT,
-		LIMIT_STATE_RAMP,
-		LIMIT_STATE_ON
-	} state;
+	enum pwm_limit_state state;
 	uint64_t time_armed;
 } pwm_limit_t;
 
-__BEGIN_DECLS
-
 __EXPORT void pwm_limit_init(pwm_limit_t *limit);
 
-__EXPORT void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, float *output, uint16_t *effective_pwm, pwm_limit_t *limit);
+__EXPORT void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, const float *output, uint16_t *effective_pwm, pwm_limit_t *limit);
 
 __END_DECLS
 
diff --git a/src/modules/uORB/topics/battery_status.h b/src/modules/uORB/topics/battery_status.h
index c40d0d4e5..d473dff3f 100644
--- a/src/modules/uORB/topics/battery_status.h
+++ b/src/modules/uORB/topics/battery_status.h
@@ -53,9 +53,10 @@
  */
 struct battery_status_s {
 	uint64_t	timestamp;		/**< microseconds since system boot, needed to integrate */
-	float   	voltage_v;		/**< Battery voltage in volts, filtered           	 */
-	float		current_a;		/**< Battery current in amperes, filtered, -1 if unknown */
-	float		discharged_mah;		/**< Discharged amount in mAh, filtered, -1 if unknown	 */
+	float   	voltage_v;		/**< Battery voltage in volts, 0 if unknown           	 */
+	float   	voltage_filtered_v;		/**< Battery voltage in volts, filtered, 0 if unknown  	 */
+	float		current_a;		/**< Battery current in amperes, -1 if unknown */
+	float		discharged_mah;		/**< Discharged amount in mAh, -1 if unknown	 */
 };
 
 /**
@@ -65,4 +66,4 @@ struct battery_status_s {
 /* register this as object request broker structure */
 ORB_DECLARE(battery_status);
 
-#endif
\ No newline at end of file
+#endif
diff --git a/src/systemcmds/eeprom/eeprom.c b/src/systemcmds/eeprom/eeprom.c
deleted file mode 100644
index 2aed80e01..000000000
--- a/src/systemcmds/eeprom/eeprom.c
+++ /dev/null
@@ -1,265 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
- *
- * 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 eeprom.c
- *
- * EEPROM service and utility app.
- */
-
-#include <nuttx/config.h>
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <stdbool.h>
-#include <unistd.h>
-#include <fcntl.h>
-#include <sys/mount.h>
-#include <sys/ioctl.h>
-#include <sys/stat.h>
-
-#include <nuttx/i2c.h>
-#include <nuttx/mtd.h>
-#include <nuttx/fs/nxffs.h>
-#include <nuttx/fs/ioctl.h>
-
-#include <board_config.h>
-
-#include "systemlib/systemlib.h"
-#include "systemlib/param/param.h"
-#include "systemlib/err.h"
-
-#ifndef PX4_I2C_BUS_ONBOARD
-#  error PX4_I2C_BUS_ONBOARD not defined, cannot locate onboard EEPROM
-#endif
-
-__EXPORT int eeprom_main(int argc, char *argv[]);
-
-static void	eeprom_attach(void);
-static void	eeprom_start(void);
-static void	eeprom_erase(void);
-static void	eeprom_ioctl(unsigned operation);
-static void	eeprom_save(const char *name);
-static void	eeprom_load(const char *name);
-static void	eeprom_test(void);
-
-static bool attached = false;
-static bool started = false;
-static struct mtd_dev_s *eeprom_mtd;
-
-int eeprom_main(int argc, char *argv[])
-{
-	if (argc >= 2) {
-		if (!strcmp(argv[1], "start"))
-			eeprom_start();
-
-		if (!strcmp(argv[1], "save_param"))
-			eeprom_save(argv[2]);
-
-		if (!strcmp(argv[1], "load_param"))
-			eeprom_load(argv[2]);
-
-		if (!strcmp(argv[1], "erase"))
-			eeprom_erase();
-
-		if (!strcmp(argv[1], "test"))
-			eeprom_test();
-
-		if (0) {	/* these actually require a file on the filesystem... */
-
-			if (!strcmp(argv[1], "reformat"))
-				eeprom_ioctl(FIOC_REFORMAT);
-
-			if (!strcmp(argv[1], "repack"))
-				eeprom_ioctl(FIOC_OPTIMIZE);
-		}
-	}
-
-	errx(1, "expected a command, try 'start'\n\t'save_param /eeprom/parameters'\n\t'load_param /eeprom/parameters'\n\t'erase'\n");
-}
-
-
-static void
-eeprom_attach(void)
-{
-	/* find the right I2C */
-	struct i2c_dev_s *i2c = up_i2cinitialize(PX4_I2C_BUS_ONBOARD);
-	/* this resets the I2C bus, set correct bus speed again */
-	I2C_SETFREQUENCY(i2c, 400000);
-
-	if (i2c == NULL)
-		errx(1, "failed to locate I2C bus");
-
-	/* start the MTD driver, attempt 5 times */
-	for (int i = 0; i < 5; i++) {
-		eeprom_mtd = at24c_initialize(i2c);
-		if (eeprom_mtd) {
-			/* abort on first valid result */
-			if (i > 0) {
-				warnx("warning: EEPROM needed %d attempts to attach", i+1);
-			}
-			break;
-		}
-	}
-
-	/* if last attempt is still unsuccessful, abort */
-	if (eeprom_mtd == NULL)
-		errx(1, "failed to initialize EEPROM driver");
-
-	attached = true;
-}
-
-static void
-eeprom_start(void)
-{
-	int ret;
-
-	if (started)
-		errx(1, "EEPROM already mounted");
-
-	if (!attached)
-		eeprom_attach();
-
-	/* start NXFFS */
-	ret = nxffs_initialize(eeprom_mtd);
-
-	if (ret < 0)
-		errx(1, "failed to initialize NXFFS - erase EEPROM to reformat");
-
-	/* mount the EEPROM */
-	ret = mount(NULL, "/eeprom", "nxffs", 0, NULL);
-
-	if (ret < 0)
-		errx(1, "failed to mount /eeprom - erase EEPROM to reformat");
-
-	started = true;
-	warnx("mounted EEPROM at /eeprom");
-	exit(0);
-}
-
-extern int at24c_nuke(void);
-
-static void
-eeprom_erase(void)
-{
-	if (!attached)
-		eeprom_attach();
-
-	if (at24c_nuke())
-		errx(1, "erase failed");
-
-	errx(0, "erase done, reboot now");
-}
-
-static void
-eeprom_ioctl(unsigned operation)
-{
-	int fd;
-
-	fd = open("/eeprom/.", 0);
-
-	if (fd < 0)
-		err(1, "open /eeprom");
-
-	if (ioctl(fd, operation, 0) < 0)
-		err(1, "ioctl");
-
-	exit(0);
-}
-
-static void
-eeprom_save(const char *name)
-{
-	if (!started)
-		errx(1, "must be started first");
-
-	if (!name)
-		err(1, "missing argument for device name, try '/eeprom/parameters'");
-
-	warnx("WARNING: 'eeprom save_param' deprecated - use 'param save' instead");
-
-	/* delete the file in case it exists */
-	unlink(name);
-
-	/* create the file */
-	int fd = open(name, O_WRONLY | O_CREAT | O_EXCL);
-
-	if (fd < 0)
-		err(1, "opening '%s' failed", name);
-
-	int result = param_export(fd, false);
-	close(fd);
-
-	if (result < 0) {
-		unlink(name);
-		errx(1, "error exporting to '%s'", name);
-	}
-
-	exit(0);
-}
-
-static void
-eeprom_load(const char *name)
-{
-	if (!started)
-		errx(1, "must be started first");
-
-	if (!name)
-		err(1, "missing argument for device name, try '/eeprom/parameters'");
-
-	warnx("WARNING: 'eeprom load_param' deprecated - use 'param load' instead");
-
-	int fd = open(name, O_RDONLY);
-
-	if (fd < 0)
-		err(1, "open '%s'", name);
-
-	int result = param_load(fd);
-	close(fd);
-
-	if (result < 0)
-		errx(1, "error importing from '%s'", name);
-
-	exit(0);
-}
-
-extern void at24c_test(void);
-
-static void
-eeprom_test(void)
-{
-	at24c_test();
-	exit(0);
-}
diff --git a/src/systemcmds/eeprom/module.mk b/src/systemcmds/eeprom/module.mk
deleted file mode 100644
index 07f3945be..000000000
--- a/src/systemcmds/eeprom/module.mk
+++ /dev/null
@@ -1,39 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# EEPROM file system driver
-#
-
-MODULE_COMMAND	 = eeprom
-SRCS		 = 24xxxx_mtd.c eeprom.c
diff --git a/src/systemcmds/eeprom/24xxxx_mtd.c b/src/systemcmds/mtd/24xxxx_mtd.c
index e34be44e3..e34be44e3 100644
--- a/src/systemcmds/eeprom/24xxxx_mtd.c
+++ b/src/systemcmds/mtd/24xxxx_mtd.c
diff --git a/src/systemcmds/mtd/module.mk b/src/systemcmds/mtd/module.mk
new file mode 100644
index 000000000..b3fceceb5
--- /dev/null
+++ b/src/systemcmds/mtd/module.mk
@@ -0,0 +1,6 @@
+#
+# RAMTRON file system driver
+#
+
+MODULE_COMMAND	 = mtd
+SRCS		 = mtd.c 24xxxx_mtd.c
diff --git a/src/systemcmds/mtd/mtd.c b/src/systemcmds/mtd/mtd.c
new file mode 100644
index 000000000..a2a0c109c
--- /dev/null
+++ b/src/systemcmds/mtd/mtd.c
@@ -0,0 +1,493 @@
+/****************************************************************************
+ *
+ *   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
+ * 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 mtd.c
+ *
+ * mtd service and utility app.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ */
+
+#include <nuttx/config.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdbool.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <sys/mount.h>
+#include <sys/ioctl.h>
+#include <sys/stat.h>
+
+#include <nuttx/spi.h>
+#include <nuttx/mtd.h>
+#include <nuttx/fs/nxffs.h>
+#include <nuttx/fs/ioctl.h>
+
+#include <arch/board/board.h>
+
+#include "systemlib/systemlib.h"
+#include "systemlib/param/param.h"
+#include "systemlib/err.h"
+
+#include <board_config.h>
+
+__EXPORT int mtd_main(int argc, char *argv[]);
+
+#ifndef CONFIG_MTD
+
+/* create a fake command with decent warnx to not confuse users */
+int mtd_main(int argc, char *argv[])
+{
+	errx(1, "MTD not enabled, skipping.");
+}
+
+#else
+
+#ifdef CONFIG_MTD_RAMTRON
+static void	ramtron_attach(void);
+#else
+
+#ifndef PX4_I2C_BUS_ONBOARD
+#  error PX4_I2C_BUS_ONBOARD not defined, cannot locate onboard EEPROM
+#endif
+
+static void	at24xxx_attach(void);
+#endif
+static void	mtd_start(char *partition_names[], unsigned n_partitions);
+static void	mtd_test(void);
+static void	mtd_erase(char *partition_names[], unsigned n_partitions);
+static void	mtd_readtest(char *partition_names[], unsigned n_partitions);
+static void	mtd_rwtest(char *partition_names[], unsigned n_partitions);
+static void	mtd_print_info();
+static int	mtd_get_geometry(unsigned long *blocksize, unsigned long *erasesize, unsigned long *neraseblocks, 
+	unsigned *blkpererase, unsigned *nblocks, unsigned *partsize, unsigned n_partitions);
+
+static bool attached = false;
+static bool started = false;
+static struct mtd_dev_s *mtd_dev;
+static unsigned n_partitions_current = 0;
+
+/* note, these will be equally sized */
+static char *partition_names_default[] = {"/fs/mtd_params", "/fs/mtd_waypoints"};
+static const int n_partitions_default = sizeof(partition_names_default) / sizeof(partition_names_default[0]);
+
+int mtd_main(int argc, char *argv[])
+{
+	if (argc >= 2) {
+		if (!strcmp(argv[1], "start")) {
+
+			/* start mapping according to user request */
+			if (argc >= 3) {
+				mtd_start(argv + 2, argc - 2);
+			} else {
+				mtd_start(partition_names_default, n_partitions_default);
+			}
+		}
+
+		if (!strcmp(argv[1], "test"))
+			mtd_test();
+
+		if (!strcmp(argv[1], "readtest")) {
+			if (argc >= 3) {
+				mtd_readtest(argv + 2, argc - 2);
+			} else {
+				mtd_readtest(partition_names_default, n_partitions_default);
+			}
+                }
+
+		if (!strcmp(argv[1], "rwtest")) {
+			if (argc >= 3) {
+				mtd_rwtest(argv + 2, argc - 2);
+			} else {
+				mtd_rwtest(partition_names_default, n_partitions_default);
+			}
+                }
+
+		if (!strcmp(argv[1], "status"))
+			mtd_status();
+
+		if (!strcmp(argv[1], "erase")) {
+			if (argc >= 3) {
+				mtd_erase(argv + 2, argc - 2);
+			} else {
+				mtd_erase(partition_names_default, n_partitions_default);
+			}
+                }
+	}
+
+	errx(1, "expected a command, try 'start', 'erase', 'status', 'readtest', 'rwtest' or 'test'");
+}
+
+struct mtd_dev_s *ramtron_initialize(FAR struct spi_dev_s *dev);
+struct mtd_dev_s *mtd_partition(FAR struct mtd_dev_s *mtd,
+                                    off_t firstblock, off_t nblocks);
+
+#ifdef CONFIG_MTD_RAMTRON
+static void
+ramtron_attach(void)
+{
+	/* find the right spi */
+	struct spi_dev_s *spi = up_spiinitialize(2);
+	/* this resets the spi bus, set correct bus speed again */
+	SPI_SETFREQUENCY(spi, 10 * 1000 * 1000);
+	SPI_SETBITS(spi, 8);
+	SPI_SETMODE(spi, SPIDEV_MODE3);
+	SPI_SELECT(spi, SPIDEV_FLASH, false);
+
+	if (spi == NULL)
+		errx(1, "failed to locate spi bus");
+
+	/* start the RAMTRON driver, attempt 5 times */
+	for (int i = 0; i < 5; i++) {
+		mtd_dev = ramtron_initialize(spi);
+
+		if (mtd_dev) {
+			/* abort on first valid result */
+			if (i > 0) {
+				warnx("warning: mtd needed %d attempts to attach", i + 1);
+			}
+
+			break;
+		}
+	}
+
+	/* if last attempt is still unsuccessful, abort */
+	if (mtd_dev == NULL)
+		errx(1, "failed to initialize mtd driver");
+
+	int ret = mtd_dev->ioctl(mtd_dev, MTDIOC_SETSPEED, (unsigned long)10*1000*1000);
+        if (ret != OK)
+            warnx(1, "failed to set bus speed");
+
+	attached = true;
+}
+#else
+
+static void
+at24xxx_attach(void)
+{
+	/* find the right I2C */
+	struct i2c_dev_s *i2c = up_i2cinitialize(PX4_I2C_BUS_ONBOARD);
+	/* this resets the I2C bus, set correct bus speed again */
+	I2C_SETFREQUENCY(i2c, 400000);
+
+	if (i2c == NULL)
+		errx(1, "failed to locate I2C bus");
+
+	/* start the MTD driver, attempt 5 times */
+	for (int i = 0; i < 5; i++) {
+		mtd_dev = at24c_initialize(i2c);
+		if (mtd_dev) {
+			/* abort on first valid result */
+			if (i > 0) {
+				warnx("warning: EEPROM needed %d attempts to attach", i+1);
+			}
+			break;
+		}
+	}
+
+	/* if last attempt is still unsuccessful, abort */
+	if (mtd_dev == NULL)
+		errx(1, "failed to initialize EEPROM driver");
+
+	attached = true;
+}
+#endif
+
+static void
+mtd_start(char *partition_names[], unsigned n_partitions)
+{
+	int ret;
+
+	if (started)
+		errx(1, "mtd already mounted");
+
+	if (!attached) {
+		#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+		at24xxx_attach();
+		#else
+		ramtron_attach();
+		#endif
+	}
+
+	if (!mtd_dev) {
+		warnx("ERROR: Failed to create RAMTRON FRAM MTD instance");
+		exit(1);
+	}
+
+	unsigned long blocksize, erasesize, neraseblocks;
+	unsigned blkpererase, nblocks, partsize;
+
+	ret = mtd_get_geometry(&blocksize, &erasesize, &neraseblocks, &blkpererase, &nblocks, &partsize, n_partitions);
+	if (ret)
+		exit(3);
+
+	/* Now create MTD FLASH partitions */
+
+	warnx("Creating partitions");
+	FAR struct mtd_dev_s *part[n_partitions];
+	char blockname[32];
+
+	unsigned offset;
+	unsigned i;
+
+	for (offset = 0, i = 0; i < n_partitions; offset += nblocks, i++) {
+
+		warnx("  Partition %d. Block offset=%lu, size=%lu",
+		      i, (unsigned long)offset, (unsigned long)nblocks);
+
+		/* Create the partition */
+
+		part[i] = mtd_partition(mtd_dev, offset, nblocks);
+
+		if (!part[i]) {
+			warnx("ERROR: mtd_partition failed. offset=%lu nblocks=%lu",
+			      (unsigned long)offset, (unsigned long)nblocks);
+			exit(4);
+		}
+
+		/* Initialize to provide an FTL block driver on the MTD FLASH interface */
+
+		snprintf(blockname, sizeof(blockname), "/dev/mtdblock%d", i);
+
+		ret = ftl_initialize(i, part[i]);
+
+		if (ret < 0) {
+			warnx("ERROR: ftl_initialize %s failed: %d", blockname, ret);
+			exit(5);
+		}
+
+		/* Now create a character device on the block device */
+
+		ret = bchdev_register(blockname, partition_names[i], false);
+
+		if (ret < 0) {
+			warnx("ERROR: bchdev_register %s failed: %d", partition_names[i], ret);
+			exit(6);
+		}
+	}
+
+	n_partitions_current = n_partitions;
+
+	started = true;
+	exit(0);
+}
+
+int mtd_get_geometry(unsigned long *blocksize, unsigned long *erasesize, unsigned long *neraseblocks, 
+	unsigned *blkpererase, unsigned *nblocks, unsigned *partsize, unsigned n_partitions)
+{
+		/* Get the geometry of the FLASH device */
+
+	FAR struct mtd_geometry_s geo;
+
+	int ret = mtd_dev->ioctl(mtd_dev, MTDIOC_GEOMETRY, (unsigned long)((uintptr_t)&geo));
+
+	if (ret < 0) {
+		warnx("ERROR: mtd->ioctl failed: %d", ret);
+		return ret;
+	}
+
+	*blocksize = geo.blocksize;
+	*erasesize = geo.blocksize;
+	*neraseblocks = geo.neraseblocks;
+
+	/* Determine the size of each partition.  Make each partition an even
+	 * multiple of the erase block size (perhaps not using some space at the
+	 * end of the FLASH).
+	 */
+
+	*blkpererase = geo.erasesize / geo.blocksize;
+	*nblocks     = (geo.neraseblocks / n_partitions) * *blkpererase;
+	*partsize    = *nblocks * geo.blocksize;
+
+	return ret;
+}
+
+/*
+  get partition size in bytes
+ */
+static ssize_t mtd_get_partition_size(void)
+{
+	unsigned long blocksize, erasesize, neraseblocks;
+	unsigned blkpererase, nblocks, partsize = 0;
+
+	int ret = mtd_get_geometry(&blocksize, &erasesize, &neraseblocks, &blkpererase, &nblocks, &partsize, n_partitions_current);
+	if (ret != OK) {
+		errx(1, "Failed to get geometry");
+	}
+	return partsize;
+}
+
+void mtd_print_info()
+{
+	if (!attached)
+		exit(1);
+
+	unsigned long blocksize, erasesize, neraseblocks;
+	unsigned blkpererase, nblocks, partsize;
+
+	int ret = mtd_get_geometry(&blocksize, &erasesize, &neraseblocks, &blkpererase, &nblocks, &partsize, n_partitions_current);
+	if (ret)
+		exit(3);
+
+	warnx("Flash Geometry:");
+
+	printf("  blocksize:      %lu\n", blocksize);
+	printf("  erasesize:      %lu\n", erasesize);
+	printf("  neraseblocks:   %lu\n", neraseblocks);
+	printf("  No. partitions: %u\n", n_partitions_current);
+	printf("  Partition size: %u Blocks (%u bytes)\n", nblocks, partsize);
+	printf("  TOTAL SIZE: %u KiB\n", neraseblocks * erasesize / 1024);
+
+}
+
+void
+mtd_test(void)
+{
+	warnx("This test routine does not test anything yet!");
+	exit(1);
+}
+
+void
+mtd_status(void)
+{
+	if (!attached)
+		errx(1, "MTD driver not started");
+
+	mtd_print_info();
+	exit(0);
+}
+
+void
+mtd_erase(char *partition_names[], unsigned n_partitions)
+{
+	uint8_t v[64];
+	memset(v, 0xFF, sizeof(v));
+	for (uint8_t i = 0; i < n_partitions; i++) {
+		uint32_t count = 0;
+		printf("Erasing %s\n", partition_names[i]);
+		int fd = open(partition_names[i], O_WRONLY);
+		if (fd == -1) {
+			errx(1, "Failed to open partition");
+		}
+		while (write(fd, v, sizeof(v)) == sizeof(v)) {
+			count += sizeof(v);
+		}
+		printf("Erased %lu bytes\n", (unsigned long)count);
+		close(fd);
+	}
+	exit(0);
+}
+
+/*
+  readtest is useful during startup to validate the device is
+  responding on the bus. It relies on the driver returning an error on
+  bad reads (the ramtron driver does return an error)
+ */
+void
+mtd_readtest(char *partition_names[], unsigned n_partitions)
+{
+	ssize_t expected_size = mtd_get_partition_size();
+
+	uint8_t v[128];
+	for (uint8_t i = 0; i < n_partitions; i++) {
+		uint32_t count = 0;
+		printf("reading %s expecting %u bytes\n", partition_names[i], expected_size);
+		int fd = open(partition_names[i], O_RDONLY);
+		if (fd == -1) {
+			errx(1, "Failed to open partition");
+		}
+		while (read(fd, v, sizeof(v)) == sizeof(v)) {
+			count += sizeof(v);
+		}
+		if (count != expected_size) {
+			errx(1,"Failed to read partition - got %u/%u bytes", count, expected_size);
+		}
+		close(fd);
+	}
+	printf("readtest OK\n");
+	exit(0);
+}
+
+/*
+  rwtest is useful during startup to validate the device is
+  responding on the bus for both reads and writes. It reads data in
+  blocks and writes the data back, then reads it again, failing if the
+  data isn't the same
+ */
+void
+mtd_rwtest(char *partition_names[], unsigned n_partitions)
+{
+	ssize_t expected_size = mtd_get_partition_size();
+
+	uint8_t v[128], v2[128];
+	for (uint8_t i = 0; i < n_partitions; i++) {
+		uint32_t count = 0;
+                off_t offset = 0;
+		printf("rwtest %s testing %u bytes\n", partition_names[i], expected_size);
+		int fd = open(partition_names[i], O_RDWR);
+		if (fd == -1) {
+			errx(1, "Failed to open partition");
+		}
+		while (read(fd, v, sizeof(v)) == sizeof(v)) {
+			count += sizeof(v);
+                        if (lseek(fd, offset, SEEK_SET) != offset) {
+                            errx(1, "seek failed");                            
+                        }
+                        if (write(fd, v, sizeof(v)) != sizeof(v)) {
+                            errx(1, "write failed");
+                        }
+                        if (lseek(fd, offset, SEEK_SET) != offset) {
+                            errx(1, "seek failed");                            
+                        }
+                        if (read(fd, v2, sizeof(v2)) != sizeof(v2)) {
+                            errx(1, "read failed");
+                        }
+                        if (memcmp(v, v2, sizeof(v2)) != 0) {
+                            errx(1, "memcmp failed");
+                        }
+                        offset += sizeof(v);
+		}
+		if (count != expected_size) {
+			errx(1,"Failed to read partition - got %u/%u bytes", count, expected_size);
+		}
+		close(fd);
+	}
+	printf("rwtest OK\n");
+	exit(0);
+}
+
+#endif
diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c
index 65f291f40..580fdc62f 100644
--- a/src/systemcmds/param/param.c
+++ b/src/systemcmds/param/param.c
@@ -72,7 +72,12 @@ param_main(int argc, char *argv[])
 			if (argc >= 3) {
 				do_save(argv[2]);
 			} else {
-				do_save(param_get_default_file());
+				if (param_save_default()) {
+					warnx("Param export failed.");
+					exit(1);
+				} else {
+					exit(0);
+				}
 			}
 		}
 
@@ -133,11 +138,8 @@ param_main(int argc, char *argv[])
 static void
 do_save(const char* param_file_name)
 {
-	/* delete the parameter file in case it exists */
-	unlink(param_file_name);
-
 	/* create the file */
-	int fd = open(param_file_name, O_WRONLY | O_CREAT | O_EXCL);
+	int fd = open(param_file_name, O_WRONLY | O_CREAT);
 
 	if (fd < 0)
 		err(1, "opening '%s' failed", param_file_name);
@@ -146,7 +148,7 @@ do_save(const char* param_file_name)
 	close(fd);
 
 	if (result < 0) {
-		unlink(param_file_name);
+		(void)unlink(param_file_name);
 		errx(1, "error exporting to '%s'", param_file_name);
 	}
 
@@ -203,11 +205,38 @@ do_show_print(void *arg, param_t param)
 	int32_t i;
 	float f;
 	const char *search_string = (const char*)arg;
+	const char *p_name = (const char*)param_name(param);
 
 	/* print nothing if search string is invalid and not matching */
-	if (!(arg == NULL || (!strcmp(search_string, param_name(param))))) {
-		/* param not found */
-		return;
+	if (!(arg == NULL)) {
+
+		/* start search */
+		char *ss = search_string;
+		char *pp = p_name;
+		bool mismatch = false;
+
+		/* XXX this comparison is only ok for trailing wildcards */
+		while (*ss != '\0' && *pp != '\0') {
+
+			if (*ss == *pp) {
+				ss++;
+				pp++;
+			} else if (*ss == '*') {
+				if (*(ss + 1) != '\0') {
+					warnx("* symbol only allowed at end of search string.");
+					exit(1);
+				}
+
+				pp++;
+			} else {
+				/* param not found */
+				return;
+			}
+		}
+
+		/* the search string must have been consumed */
+		if (!(*ss == '\0' || *ss == '*'))
+			return;
 	}
 
 	printf("%c %s: ",
diff --git a/src/systemcmds/ramtron/module.mk b/src/systemcmds/ramtron/module.mk
deleted file mode 100644
index e4eb1d143..000000000
--- a/src/systemcmds/ramtron/module.mk
+++ /dev/null
@@ -1,6 +0,0 @@
-#
-# RAMTRON file system driver
-#
-
-MODULE_COMMAND	 = ramtron
-SRCS		 = ramtron.c
diff --git a/src/systemcmds/ramtron/ramtron.c b/src/systemcmds/ramtron/ramtron.c
deleted file mode 100644
index 03c713987..000000000
--- a/src/systemcmds/ramtron/ramtron.c
+++ /dev/null
@@ -1,279 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
- *
- * 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 ramtron.c
- *
- * ramtron service and utility app.
- */
-
-#include <nuttx/config.h>
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <stdbool.h>
-#include <unistd.h>
-#include <fcntl.h>
-#include <sys/mount.h>
-#include <sys/ioctl.h>
-#include <sys/stat.h>
-
-#include <nuttx/spi.h>
-#include <nuttx/mtd.h>
-#include <nuttx/fs/nxffs.h>
-#include <nuttx/fs/ioctl.h>
-
-#include <arch/board/board.h>
-
-#include "systemlib/systemlib.h"
-#include "systemlib/param/param.h"
-#include "systemlib/err.h"
-
-__EXPORT int ramtron_main(int argc, char *argv[]);
-
-#ifndef CONFIG_MTD_RAMTRON
-
-/* create a fake command with decent message to not confuse users */
-int ramtron_main(int argc, char *argv[])
-{
-	errx(1, "RAMTRON not enabled, skipping.");
-}
-#else
-
-static void	ramtron_attach(void);
-static void	ramtron_start(void);
-static void	ramtron_erase(void);
-static void	ramtron_ioctl(unsigned operation);
-static void	ramtron_save(const char *name);
-static void	ramtron_load(const char *name);
-static void	ramtron_test(void);
-
-static bool attached = false;
-static bool started = false;
-static struct mtd_dev_s *ramtron_mtd;
-
-int ramtron_main(int argc, char *argv[])
-{
-	if (argc >= 2) {
-		if (!strcmp(argv[1], "start"))
-			ramtron_start();
-
-		if (!strcmp(argv[1], "save_param"))
-			ramtron_save(argv[2]);
-
-		if (!strcmp(argv[1], "load_param"))
-			ramtron_load(argv[2]);
-
-		if (!strcmp(argv[1], "erase"))
-			ramtron_erase();
-
-		if (!strcmp(argv[1], "test"))
-			ramtron_test();
-
-		if (0) {	/* these actually require a file on the filesystem... */
-
-			if (!strcmp(argv[1], "reformat"))
-				ramtron_ioctl(FIOC_REFORMAT);
-
-			if (!strcmp(argv[1], "repack"))
-				ramtron_ioctl(FIOC_OPTIMIZE);
-		}
-	}
-
-	errx(1, "expected a command, try 'start'\n\t'save_param /ramtron/parameters'\n\t'load_param /ramtron/parameters'\n\t'erase'\n");
-}
-
-struct mtd_dev_s *ramtron_initialize(FAR struct spi_dev_s *dev);
-
-
-static void
-ramtron_attach(void)
-{
-	/* find the right spi */
-	struct spi_dev_s *spi = up_spiinitialize(2);
-	/* this resets the spi bus, set correct bus speed again */
-    // xxx set in ramtron driver, leave this out
-//	SPI_SETFREQUENCY(spi, 4000000);
-	SPI_SETFREQUENCY(spi, 375000000);
-	SPI_SETBITS(spi, 8);
-	SPI_SETMODE(spi, SPIDEV_MODE3);
-	SPI_SELECT(spi, SPIDEV_FLASH, false);
-
-	if (spi == NULL)
-		errx(1, "failed to locate spi bus");
-
-	/* start the MTD driver, attempt 5 times */
-	for (int i = 0; i < 5; i++) {
-		ramtron_mtd = ramtron_initialize(spi);
-		if (ramtron_mtd) {
-			/* abort on first valid result */
-			if (i > 0) {
-				warnx("warning: ramtron needed %d attempts to attach", i+1);
-			}
-			break;
-		}
-	}
-
-	/* if last attempt is still unsuccessful, abort */
-	if (ramtron_mtd == NULL)
-		errx(1, "failed to initialize ramtron driver");
-
-	attached = true;
-}
-
-static void
-ramtron_start(void)
-{
-	int ret;
-
-	if (started)
-		errx(1, "ramtron already mounted");
-
-	if (!attached)
-		ramtron_attach();
-
-	/* start NXFFS */
-	ret = nxffs_initialize(ramtron_mtd);
-
-	if (ret < 0)
-		errx(1, "failed to initialize NXFFS - erase ramtron to reformat");
-
-	/* mount the ramtron */
-	ret = mount(NULL, "/ramtron", "nxffs", 0, NULL);
-
-	if (ret < 0)
-		errx(1, "failed to mount /ramtron - erase ramtron to reformat");
-
-	started = true;
-	warnx("mounted ramtron at /ramtron");
-	exit(0);
-}
-
-//extern int at24c_nuke(void);
-
-static void
-ramtron_erase(void)
-{
-	if (!attached)
-		ramtron_attach();
-
-//	if (at24c_nuke())
-		errx(1, "erase failed");
-
-	errx(0, "erase done, reboot now");
-}
-
-static void
-ramtron_ioctl(unsigned operation)
-{
-	int fd;
-
-	fd = open("/ramtron/.", 0);
-
-	if (fd < 0)
-		err(1, "open /ramtron");
-
-	if (ioctl(fd, operation, 0) < 0)
-		err(1, "ioctl");
-
-	exit(0);
-}
-
-static void
-ramtron_save(const char *name)
-{
-	if (!started)
-		errx(1, "must be started first");
-
-	if (!name)
-		err(1, "missing argument for device name, try '/ramtron/parameters'");
-
-	warnx("WARNING: 'ramtron save_param' deprecated - use 'param save' instead");
-
-	/* delete the file in case it exists */
-	unlink(name);
-
-	/* create the file */
-	int fd = open(name, O_WRONLY | O_CREAT | O_EXCL);
-
-	if (fd < 0)
-		err(1, "opening '%s' failed", name);
-
-	int result = param_export(fd, false);
-	close(fd);
-
-	if (result < 0) {
-		unlink(name);
-		errx(1, "error exporting to '%s'", name);
-	}
-
-	exit(0);
-}
-
-static void
-ramtron_load(const char *name)
-{
-	if (!started)
-		errx(1, "must be started first");
-
-	if (!name)
-		err(1, "missing argument for device name, try '/ramtron/parameters'");
-
-	warnx("WARNING: 'ramtron load_param' deprecated - use 'param load' instead");
-
-	int fd = open(name, O_RDONLY);
-
-	if (fd < 0)
-		err(1, "open '%s'", name);
-
-	int result = param_load(fd);
-	close(fd);
-
-	if (result < 0)
-		errx(1, "error importing from '%s'", name);
-
-	exit(0);
-}
-
-//extern void at24c_test(void);
-
-static void
-ramtron_test(void)
-{
-//	at24c_test();
-	exit(0);
-}
-
-#endif
diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk
index 28e214a6c..beb9ad13d 100644
--- a/src/systemcmds/tests/module.mk
+++ b/src/systemcmds/tests/module.mk
@@ -29,4 +29,6 @@ SRCS			 = test_adc.c \
 			   test_param.c \
 			   test_ppm_loopback.c \
 			   test_rc.c \
-			   test_mount.c
+			   test_conv.cpp \
+			   test_mount.c \
+			   test_mtd.c
diff --git a/src/systemcmds/tests/test_conv.cpp b/src/systemcmds/tests/test_conv.cpp
new file mode 100644
index 000000000..50dece816
--- /dev/null
+++ b/src/systemcmds/tests/test_conv.cpp
@@ -0,0 +1,76 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file test_conv.cpp
+ * Tests conversions used across the system.
+ *
+ */
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+
+#include <stdio.h>
+#include <poll.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+
+#include "tests.h"
+
+#include <math.h>
+#include <float.h>
+
+#include <systemlib/err.h>
+#include <unit_test/unit_test.h>
+#include <px4iofirmware/protocol.h>
+
+int test_conv(int argc, char *argv[])
+{
+	warnx("Testing system conversions");
+
+	for (int i = -10000; i <= 10000; i+=1) {
+		float f = i/10000.0f;
+		float fres = REG_TO_FLOAT(FLOAT_TO_REG(f));
+		if (fabsf(f - fres) > 0.0001f) {
+			warnx("conversion fail: input: %8.4f, intermediate: %d, result: %8.4f", f, REG_TO_SIGNED(FLOAT_TO_REG(f)), fres);
+			return 1;
+		}
+	}
+
+	warnx("All conversions clean");
+
+	return 0;
+}
diff --git a/src/systemcmds/tests/test_file.c b/src/systemcmds/tests/test_file.c
index 7206b87d6..96be1e8df 100644
--- a/src/systemcmds/tests/test_file.c
+++ b/src/systemcmds/tests/test_file.c
@@ -54,9 +54,9 @@
 
 #include "tests.h"
 
-int check_user_abort();
+static int check_user_abort(int fd);
 
-int check_user_abort() {
+int check_user_abort(int fd) {
 	/* check if user wants to abort */
 	char c;
 
@@ -77,6 +77,8 @@ int check_user_abort() {
 		case 'q':
 		{
 			warnx("Test aborted.");
+			fsync(fd);
+			close(fd);
 			return OK;
 			/* not reached */
 			}
@@ -141,7 +143,7 @@ test_file(int argc, char *argv[])
 
 				fsync(fd);
 
-				if (!check_user_abort())
+				if (!check_user_abort(fd))
 					return OK;
 
 			}
@@ -175,7 +177,7 @@ test_file(int argc, char *argv[])
 					return 1;
 				}
 
-				if (!check_user_abort())
+				if (!check_user_abort(fd))
 					return OK;
 
 			}
@@ -199,7 +201,7 @@ test_file(int argc, char *argv[])
 					return 1;
 				}
 
-				if (!check_user_abort())
+				if (!check_user_abort(fd))
 					return OK;
 
 			}
@@ -232,7 +234,7 @@ test_file(int argc, char *argv[])
 						break;
 					}
 
-					if (!check_user_abort())
+					if (!check_user_abort(fd))
 						return OK;
 				}
 
@@ -275,7 +277,7 @@ test_file(int argc, char *argv[])
 							break;
 					}
 
-					if (!check_user_abort())
+					if (!check_user_abort(fd))
 						return OK;
 				}
 
diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp
index 4da86042d..2a47551ee 100644
--- a/src/systemcmds/tests/test_mixer.cpp
+++ b/src/systemcmds/tests/test_mixer.cpp
@@ -48,9 +48,13 @@
 #include <errno.h>
 #include <string.h>
 #include <time.h>
+#include <math.h>
 
 #include <systemlib/err.h>
 #include <systemlib/mixer/mixer.h>
+#include <systemlib/pwm_limit/pwm_limit.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_pwm_output.h>
 
 #include "tests.h"
 
@@ -59,6 +63,20 @@ static int	mixer_callback(uintptr_t handle,
 			       uint8_t control_index,
 			       float &control);
 
+const unsigned output_max = 8;
+static float actuator_controls[output_max];
+static bool should_arm = false;
+uint16_t r_page_servo_disarmed[output_max];
+uint16_t r_page_servo_control_min[output_max];
+uint16_t r_page_servo_control_max[output_max];
+uint16_t r_page_servos[output_max];
+uint16_t servo_predicted[output_max];
+
+/*
+ * PWM limit structure
+ */
+pwm_limit_t pwm_limit;
+
 int test_mixer(int argc, char *argv[])
 {
 	warnx("testing mixer");
@@ -164,6 +182,174 @@ int test_mixer(int argc, char *argv[])
 	if (mixer_group.count() != 8)
 		return 1;
 
+	/* execute the mixer */
+	
+	float	outputs[output_max];
+	unsigned mixed;
+	const int jmax = 5;
+
+	pwm_limit_init(&pwm_limit);
+	should_arm = true;
+
+	/* run through arming phase */
+	for (int i = 0; i < output_max; i++) {
+		actuator_controls[i] = 0.1f;
+		r_page_servo_disarmed[i] = PWM_LOWEST_MIN;
+		r_page_servo_control_min[i] = PWM_DEFAULT_MIN;
+		r_page_servo_control_max[i] = PWM_DEFAULT_MAX;
+	}
+
+	warnx("ARMING TEST: STARTING RAMP");
+	unsigned sleep_quantum_us = 10000;
+
+	hrt_abstime starttime = hrt_absolute_time();
+	unsigned sleepcount = 0;
+
+	while (hrt_elapsed_time(&starttime) < INIT_TIME_US + RAMP_TIME_US) {
+		
+		/* mix */
+		mixed = mixer_group.mix(&outputs[0], output_max);
+
+		pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
+
+		//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
+		for (int i = 0; i < mixed; i++)
+		{
+			/* check mixed outputs to be zero during init phase */
+			if (hrt_elapsed_time(&starttime) < INIT_TIME_US &&
+				r_page_servos[i] != r_page_servo_disarmed[i]) {
+				warnx("disarmed servo value mismatch");
+				return 1;
+			}
+
+			if (hrt_elapsed_time(&starttime) >= INIT_TIME_US &&
+				r_page_servos[i] + 1 <= r_page_servo_disarmed[i]) {
+				warnx("ramp servo value mismatch");
+				return 1;
+			}
+
+			//printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, outputs_unlimited[i], outputs[i], (int)r_page_servos[i]);
+		}
+		usleep(sleep_quantum_us);
+		sleepcount++;
+
+		if (sleepcount % 10 == 0) {
+			printf(".");
+			fflush(stdout);
+		}
+	}
+	printf("\n");
+
+	warnx("ARMING TEST: NORMAL OPERATION");
+
+	for (int j = -jmax; j <= jmax; j++) {
+
+		for (int i = 0; i < output_max; i++) {
+			actuator_controls[i] = j/10.0f + 0.1f * i;
+			r_page_servo_disarmed[i] = PWM_LOWEST_MIN;
+			r_page_servo_control_min[i] = PWM_DEFAULT_MIN;
+			r_page_servo_control_max[i] = PWM_DEFAULT_MAX;
+		}
+
+		/* mix */
+		mixed = mixer_group.mix(&outputs[0], output_max);
+
+		pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
+
+		warnx("mixed %d outputs (max %d)", mixed, output_max);
+		for (int i = 0; i < mixed; i++)
+		{
+			servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f;
+			if (fabsf(servo_predicted[i] - r_page_servos[i]) > 2) {
+				printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, outputs[i], servo_predicted[i], (int)r_page_servos[i]);
+				warnx("mixer violated predicted value");
+				return 1;
+			}
+		}
+	}
+
+	warnx("ARMING TEST: DISARMING");
+
+	starttime = hrt_absolute_time();
+	sleepcount = 0;
+	should_arm = false;
+
+	while (hrt_elapsed_time(&starttime) < 600000) {
+		
+		/* mix */
+		mixed = mixer_group.mix(&outputs[0], output_max);
+
+		pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
+
+		//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
+		for (int i = 0; i < mixed; i++)
+		{
+			/* check mixed outputs to be zero during init phase */
+			if (r_page_servos[i] != r_page_servo_disarmed[i]) {
+				warnx("disarmed servo value mismatch");
+				return 1;
+			}
+
+			//printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, outputs_unlimited[i], outputs[i], (int)r_page_servos[i]);
+		}
+		usleep(sleep_quantum_us);
+		sleepcount++;
+
+		if (sleepcount % 10 == 0) {
+			printf(".");
+			fflush(stdout);
+		}
+	}
+	printf("\n");
+
+	warnx("ARMING TEST: REARMING: STARTING RAMP");
+
+	starttime = hrt_absolute_time();
+	sleepcount = 0;
+	should_arm = true;
+
+	while (hrt_elapsed_time(&starttime) < 600000 + RAMP_TIME_US) {
+		
+		/* mix */
+		mixed = mixer_group.mix(&outputs[0], output_max);
+
+		pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
+
+		//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
+		for (int i = 0; i < mixed; i++)
+		{
+			/* predict value */
+			servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f;
+
+			/* check ramp */
+
+			if (hrt_elapsed_time(&starttime) < RAMP_TIME_US &&
+				(r_page_servos[i] + 1 <= r_page_servo_disarmed[i] ||
+					r_page_servos[i] > servo_predicted[i])) {
+				warnx("ramp servo value mismatch");
+				return 1;
+			}
+
+			/* check post ramp phase */
+			if (hrt_elapsed_time(&starttime) > RAMP_TIME_US &&
+				fabsf(servo_predicted[i] - r_page_servos[i]) > 2) {
+				printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, outputs[i], servo_predicted[i], (int)r_page_servos[i]);
+				warnx("mixer violated predicted value");
+				return 1;
+			}
+
+			//printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, outputs_unlimited[i], outputs[i], (int)r_page_servos[i]);
+		}
+		usleep(sleep_quantum_us);
+		sleepcount++;
+
+		if (sleepcount % 10 == 0) {
+			printf(".");
+			fflush(stdout);
+		}
+	}
+	printf("\n");
+
 	/* load multirotor at once test */
 	mixer_group.reset();
 
@@ -180,8 +366,12 @@ int test_mixer(int argc, char *argv[])
 	unsigned mc_loaded = loaded;
 	mixer_group.load_from_buf(&buf[0], mc_loaded);
 	warnx("complete buffer load: loaded %u mixers", mixer_group.count());
-	if (mixer_group.count() != 8)
+	if (mixer_group.count() != 5) {
+		warnx("FAIL: Quad W mixer load failed");
 		return 1;
+	}
+
+	warnx("SUCCESS: No errors in mixer test");
 }
 
 static int
@@ -193,7 +383,10 @@ mixer_callback(uintptr_t handle,
 	if (control_group != 0)
 		return -1;
 
-	control = 0.0f;
+	if (control_index > (sizeof(actuator_controls) / sizeof(actuator_controls[0])))
+		return -1;
+
+	control = actuator_controls[control_index];
 
 	return 0;
 }
diff --git a/src/systemcmds/tests/test_mtd.c b/src/systemcmds/tests/test_mtd.c
new file mode 100644
index 000000000..bac9efbdb
--- /dev/null
+++ b/src/systemcmds/tests/test_mtd.c
@@ -0,0 +1,229 @@
+/****************************************************************************
+ *
+ *   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 test_mtd.c
+ *
+ * Param storage / file test.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ */
+
+#include <sys/stat.h>
+#include <poll.h>
+#include <dirent.h>
+#include <stdio.h>
+#include <stddef.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <systemlib/err.h>
+#include <systemlib/perf_counter.h>
+#include <string.h>
+
+#include <drivers/drv_hrt.h>
+
+#include "tests.h"
+
+#define PARAM_FILE_NAME "/fs/mtd_params"
+
+static int check_user_abort(int fd);
+
+int check_user_abort(int fd) {
+	/* check if user wants to abort */
+	char c;
+
+	struct pollfd fds;
+	int ret;
+	fds.fd = 0; /* stdin */
+	fds.events = POLLIN;
+	ret = poll(&fds, 1, 0);
+
+	if (ret > 0) {
+
+		read(0, &c, 1);
+
+		switch (c) {
+		case 0x03: // ctrl-c
+		case 0x1b: // esc
+		case 'c':
+		case 'q':
+		{
+			warnx("Test aborted.");
+			fsync(fd);
+			close(fd);
+			return OK;
+			/* not reached */
+			}
+		}
+	}
+
+	return 1;
+}
+
+void print_fail()
+{
+	printf("<[T]: MTD: FAIL>\n");
+}
+
+void print_success()
+{
+	printf("<[T]: MTD: OK>\n");
+}
+
+int
+test_mtd(int argc, char *argv[])
+{
+	unsigned iterations= 0;
+
+	/* check if microSD card is mounted */
+	struct stat buffer;
+	if (stat(PARAM_FILE_NAME, &buffer)) {
+		warnx("file %s not found, aborting MTD test", PARAM_FILE_NAME);
+		print_fail();
+		return 1;
+	}
+
+	// XXX get real storage space here
+	unsigned file_size = 4096;
+
+	/* perform tests for a range of chunk sizes */
+	unsigned chunk_sizes[] = {256, 512, 4096};
+
+	for (unsigned c = 0; c < (sizeof(chunk_sizes) / sizeof(chunk_sizes[0])); c++) {
+
+		printf("\n====== FILE TEST: %u bytes chunks ======\n", chunk_sizes[c]);
+
+		uint8_t write_buf[chunk_sizes[c]] __attribute__((aligned(64)));
+
+		/* fill write buffer with known values */
+		for (int i = 0; i < sizeof(write_buf); i++) {
+			/* this will wrap, but we just need a known value with spacing */
+			write_buf[i] = i+11;
+		}
+
+		uint8_t read_buf[chunk_sizes[c]] __attribute__((aligned(64)));
+		hrt_abstime start, end;
+
+		int fd = open(PARAM_FILE_NAME, O_RDONLY);
+		int rret = read(fd, read_buf, chunk_sizes[c]);
+		close(fd);
+
+		fd = open(PARAM_FILE_NAME, O_WRONLY);
+
+		printf("printing 2 percent of the first chunk:\n");
+		for (int i = 0; i < sizeof(read_buf) / 50; i++) {
+			printf("%02X", read_buf[i]);
+		}
+		printf("\n");
+
+		iterations = file_size / chunk_sizes[c];
+
+		start = hrt_absolute_time();
+		for (unsigned i = 0; i < iterations; i++) {
+			int wret = write(fd, write_buf, chunk_sizes[c]);
+
+			if (wret != (int)chunk_sizes[c]) {
+				warn("WRITE ERROR!");
+				print_fail();
+				return 1;
+			}
+
+			fsync(fd);
+
+			if (!check_user_abort(fd))
+				return OK;
+
+		}
+		end = hrt_absolute_time();
+
+		close(fd);
+		fd = open(PARAM_FILE_NAME, O_RDONLY);
+
+		/* read back data for validation */
+		for (unsigned i = 0; i < iterations; i++) {
+			int rret = read(fd, read_buf, chunk_sizes[c]);
+
+			if (rret != chunk_sizes[c]) {
+				warnx("READ ERROR!");
+				print_fail();
+				return 1;
+			}
+			
+			/* compare value */
+			bool compare_ok = true;
+
+			for (int j = 0; j < chunk_sizes[c]; j++) {
+				if (read_buf[j] != write_buf[j]) {
+					warnx("COMPARISON ERROR: byte %d", j);
+					print_fail();
+					compare_ok = false;
+					break;
+				}
+			}
+
+			if (!compare_ok) {
+				warnx("ABORTING FURTHER COMPARISON DUE TO ERROR");
+				print_fail();
+				return 1;
+			}
+
+			if (!check_user_abort(fd))
+				return OK;
+
+		}
+
+
+		close(fd);
+
+	}
+
+	/* fill the file with 0xFF to make it look new again */
+	char ffbuf[64];
+	memset(ffbuf, 0xFF, sizeof(ffbuf));
+	int fd = open(PARAM_FILE_NAME, O_WRONLY);
+	for (int i = 0; i < file_size / sizeof(ffbuf); i++) {
+		int ret = write(fd, ffbuf, sizeof(ffbuf));
+
+		if (ret != sizeof(ffbuf)) {
+			warnx("ERROR! Could not fill file with 0xFF");
+			close(fd);
+			print_fail();
+			return 1;
+		}
+	}
+
+	(void)close(fd);
+	print_success();
+
+	return 0;
+}
diff --git a/src/systemcmds/tests/test_rc.c b/src/systemcmds/tests/test_rc.c
index 72619fc8b..6a602ecfc 100644
--- a/src/systemcmds/tests/test_rc.c
+++ b/src/systemcmds/tests/test_rc.c
@@ -32,8 +32,8 @@
  ****************************************************************************/
 
 /**
- * @file test_ppm_loopback.c
- * Tests the PWM outputs and PPM input
+ * @file test_rc.c
+ * Tests RC input.
  *
  */
 
diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h
index bec13bd30..223edc14a 100644
--- a/src/systemcmds/tests/tests.h
+++ b/src/systemcmds/tests/tests.h
@@ -109,7 +109,9 @@ extern int	test_bson(int argc, char *argv[]);
 extern int	test_file(int argc, char *argv[]);
 extern int	test_mixer(int argc, char *argv[]);
 extern int	test_rc(int argc, char *argv[]);
+extern int	test_conv(int argc, char *argv[]);
 extern int	test_mount(int argc, char *argv[]);
+extern int	test_mtd(int argc, char *argv[]);
 
 __END_DECLS
 
diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c
index 84535126f..77a4df618 100644
--- a/src/systemcmds/tests/tests_main.c
+++ b/src/systemcmds/tests/tests_main.c
@@ -106,7 +106,9 @@ const struct {
 	{"file",		test_file,	0},
 	{"mixer",		test_mixer,	OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"rc",			test_rc,	OPT_NOJIGTEST | OPT_NOALLTEST},
+	{"conv",		test_conv,	OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"mount",		test_mount,	OPT_NOJIGTEST | OPT_NOALLTEST},
+	{"mtd",			test_mtd,	0},
 	{"help",		test_help,	OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST},
 	{NULL,			NULL, 		0}
 };