Merge branch 'navigator_new' into navigator_new_vector

31 files changed, 1029 insertions, 908 deletions

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 146a77d23..970d0b1b6 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 215293529..e01afc64e 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
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/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/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/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/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
index 686656597..b3fceceb5 100644
--- a/src/systemcmds/mtd/module.mk
+++ b/src/systemcmds/mtd/module.mk
@@ -3,4 +3,4 @@
 #
 
 MODULE_COMMAND	 = mtd
-SRCS		 = mtd.c
+SRCS		 = mtd.c 24xxxx_mtd.c
diff --git a/src/systemcmds/mtd/mtd.c b/src/systemcmds/mtd/mtd.c
index baef9dccc..a2a0c109c 100644
--- a/src/systemcmds/mtd/mtd.c
+++ b/src/systemcmds/mtd/mtd.c
@@ -62,26 +62,43 @@
 #include "systemlib/param/param.h"
 #include "systemlib/err.h"
 
+#include <board_config.h>
+
 __EXPORT int mtd_main(int argc, char *argv[]);
 
-#ifndef CONFIG_MTD_RAMTRON
+#ifndef CONFIG_MTD
 
 /* create a fake command with decent warnx to not confuse users */
 int mtd_main(int argc, char *argv[])
 {
-	errx(1, "RAMTRON not enabled, skipping.");
+	errx(1, "MTD not enabled, skipping.");
 }
 
 #else
 
-static void	mtd_attach(void);
+#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"};
@@ -93,9 +110,8 @@ int mtd_main(int argc, char *argv[])
 		if (!strcmp(argv[1], "start")) {
 
 			/* start mapping according to user request */
-			if (argc > 3) {
+			if (argc >= 3) {
 				mtd_start(argv + 2, argc - 2);
-
 			} else {
 				mtd_start(partition_names_default, n_partitions_default);
 			}
@@ -104,28 +120,49 @@ int mtd_main(int argc, char *argv[])
 		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) {
-				errx(1, "usage: mtd erase <PARTITION_PATH..>");
+			if (argc >= 3) {
+				mtd_erase(argv + 2, argc - 2);
+			} else {
+				mtd_erase(partition_names_default, n_partitions_default);
 			}
-			mtd_erase(argv + 2, argc - 2);
                 }
 	}
 
-	errx(1, "expected a command, try 'start', 'erase' or 'test'");
+	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
-mtd_attach(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, 40 * 1000 * 1000);
+	SPI_SETFREQUENCY(spi, 10 * 1000 * 1000);
 	SPI_SETBITS(spi, 8);
 	SPI_SETMODE(spi, SPIDEV_MODE3);
 	SPI_SELECT(spi, SPIDEV_FLASH, false);
@@ -133,7 +170,7 @@ mtd_attach(void)
 	if (spi == NULL)
 		errx(1, "failed to locate spi bus");
 
-	/* start the MTD driver, attempt 5 times */
+	/* start the RAMTRON driver, attempt 5 times */
 	for (int i = 0; i < 5; i++) {
 		mtd_dev = ramtron_initialize(spi);
 
@@ -151,8 +188,44 @@ mtd_attach(void)
 	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)
@@ -162,42 +235,25 @@ mtd_start(char *partition_names[], unsigned n_partitions)
 	if (started)
 		errx(1, "mtd already mounted");
 
-	if (!attached)
-		mtd_attach();
+	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;
 
-	/* Get the geometry of the FLASH device */
-
-	FAR struct mtd_geometry_s geo;
-
-	ret = mtd_dev->ioctl(mtd_dev, MTDIOC_GEOMETRY, (unsigned long)((uintptr_t)&geo));
-
-	if (ret < 0) {
-		warnx("ERROR: mtd->ioctl failed: %d", ret);
+	ret = mtd_get_geometry(&blocksize, &erasesize, &neraseblocks, &blkpererase, &nblocks, &partsize, n_partitions);
+	if (ret)
 		exit(3);
-	}
-
-	warnx("Flash Geometry:");
-	warnx("  blocksize:      %lu", (unsigned long)geo.blocksize);
-	warnx("  erasesize:      %lu", (unsigned long)geo.erasesize);
-	warnx("  neraseblocks:   %lu", (unsigned long)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).
-	 */
-
-	unsigned blkpererase = geo.erasesize / geo.blocksize;
-	unsigned nblocks     = (geo.neraseblocks / n_partitions) * blkpererase;
-	unsigned partsize    = nblocks * geo.blocksize;
-
-	warnx("  No. partitions: %u", n_partitions);
-	warnx("  Partition size: %lu Blocks (%lu bytes)", (unsigned long)nblocks, (unsigned long)partsize);
 
 	/* Now create MTD FLASH partitions */
 
@@ -244,18 +300,98 @@ mtd_start(char *partition_names[], unsigned n_partitions)
 		}
 	}
 
+	n_partitions_current = n_partitions;
+
 	started = true;
 	exit(0);
 }
 
-static void
+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);
 }
 
-static void
+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];
@@ -267,7 +403,7 @@ mtd_erase(char *partition_names[], unsigned n_partitions)
 		if (fd == -1) {
 			errx(1, "Failed to open partition");
 		}
-		while (write(fd, &v, sizeof(v)) == sizeof(v)) {
+		while (write(fd, v, sizeof(v)) == sizeof(v)) {
 			count += sizeof(v);
 		}
 		printf("Erased %lu bytes\n", (unsigned long)count);
@@ -276,4 +412,82 @@ mtd_erase(char *partition_names[], unsigned n_partitions)
 	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/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 1509ac064..acf28c35b 100644
--- a/src/systemcmds/tests/module.mk
+++ b/src/systemcmds/tests/module.mk
@@ -30,4 +30,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 83d09dd5e..96be1e8df 100644
--- a/src/systemcmds/tests/test_file.c
+++ b/src/systemcmds/tests/test_file.c
@@ -54,7 +54,7 @@
 
 #include "tests.h"
 
-int check_user_abort(int fd);
+static int check_user_abort(int fd);
 
 int check_user_abort(int fd) {
 	/* check if user wants to abort */
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 8223de712..b86e4d13f 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[]);
 extern int	test_mathlib(int argc, char *argv[]);
 
 __END_DECLS
diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c
index d0a8635ed..73827b7cf 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},
 	{"mathlib",		test_mathlib,	0},
 	{"help",		test_help,	OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST},
 	{NULL,			NULL, 		0}