diff --git a/ROMFS/px4fmu_common/init.d/4900_crazyflie b/ROMFS/px4fmu_common/init.d/4900_crazyflie
index 3d0d037f13ab415f7690a3a5b7b63ba7f0ef2984..153c6d2134cc5a1e5f9764d40f5b4e23fdc4998f 100644
--- a/ROMFS/px4fmu_common/init.d/4900_crazyflie
+++ b/ROMFS/px4fmu_common/init.d/4900_crazyflie
@@ -7,48 +7,8 @@
 # @maintainer Tim Dyer <dyer.ti@gmail.com>
 #
 
-# uorb start
+sh /etc/init.d/4001_quad_x
 
-# Params
 
-# System status LEDs
-
-# waypoint storage
-
-# Sensors
-mpu9250 -R 12 start
-ak8963 start
-lps25h start
-sensors start
-
-commander start
-
-# Output mode
-crazyflie start
-
-# MAVLINK
-# mavlink start -r 1200 -d /dev/ttyS***
-
-# MAVTYPE
-param set MAV_TYPE 2
-
-exit 1
-
-set OUTPUT_DEV /dev/pwm_output0
-set MIXER_FILE /etc/mixers/quad_x.main.mix
-if mixer load $OUTPUT_DEV $MIXER_FILE
-then
-	echo "[i] Mixer: $MIXER_FILE on $OUTPUT_DEV"
-else
-	echo "[i] Error loading mixer: $MIXER_FILE"
-	echo "ERROR: Could not load mixer: $MIXER_FILE" >> $LOG_FILE
-	tone_alarm $TUNE_ERR
-fi
-
-sh /etc/init.d/rc.mc_apps
-
-# Navigator
-navigator start
-
-# Boot complete
-mavlink boot_complete
\ No newline at end of file
+set OUTPUT_MODE crazyflie
+set USE_IO no
diff --git a/cmake/configs/nuttx_crazyflie_default.cmake b/cmake/configs/nuttx_crazyflie_default.cmake
index ca6da271711d1806e5a61b5ae935d785ab677644..a31b27aa47f935b07433d4cfd3e5fbf020c52d85 100644
--- a/cmake/configs/nuttx_crazyflie_default.cmake
+++ b/cmake/configs/nuttx_crazyflie_default.cmake
@@ -11,8 +11,8 @@ set(config_module_list
 	drivers/led
 	drivers/boards/crazyflie
 	drivers/crazyflie
-	drivers/mpu9250
-	drivers/ak8963
+	#drivers/mpu9250
+	#drivers/ak8963
 	drivers/lps25h
 	drivers/gps
 	# drivers/pwm_out_sim
@@ -39,8 +39,10 @@ set(config_module_list
 	# General system control
 	#
 	modules/commander
+	modules/load_mon
 	modules/navigator
 	modules/mavlink
+	#modules/gpio_led
 	modules/land_detector
 
 	modules/dummy
@@ -51,18 +53,18 @@ set(config_module_list
 	# Too high RAM usage due to static allocations
 	# modules/attitude_estimator_ekf
 	modules/attitude_estimator_q
-	modules/ekf_att_pos_estimator
 	modules/position_estimator_inav
+	modules/local_position_estimator
 
 	#
 	# Vehicle Control
 	#
 	# modules/segway # XXX Needs GCC 4.7 fix
-	modules/fw_pos_control_l1
-	modules/fw_att_control
+	# modules/fw_pos_control_l1
+	# modules/fw_att_control
 	modules/mc_att_control
 	modules/mc_pos_control
-	modules/vtol_att_control
+	# modules/vtol_att_control
 
 	#
 	# Logging
@@ -76,7 +78,6 @@ set(config_module_list
 	modules/param
 	modules/systemlib
 	modules/systemlib/mixer
-	modules/controllib
 	modules/uORB
 	modules/dataman
 
@@ -84,8 +85,10 @@ set(config_module_list
 	# Libraries
 	#
 	#lib/mathlib/CMSIS
+	lib/controllib
 	lib/mathlib
 	lib/mathlib/math/filter
+	lib/rc
 	lib/ecl
 	lib/external_lgpl
 	lib/geo
@@ -94,10 +97,12 @@ set(config_module_list
 	lib/launchdetection
 	lib/terrain_estimation
 	lib/runway_takeoff
+	lib/tailsitter_recovery
+	lib/DriverFramework/framework
 	platforms/nuttx
 
 	# had to add for cmake, not sure why wasn't in original config
-	platforms/common 
+	platforms/common
 	platforms/nuttx/px4_layer
 
 	#
@@ -140,13 +145,16 @@ set(config_extra_builtin_cmds
 	)
 
 set(config_extra_libs
-	${CMAKE_SOURCE_DIR}/src/lib/mathlib/CMSIS/libarm_cortexM4lf_math.a
 	)
 
 add_custom_target(sercon)
 set_target_properties(sercon PROPERTIES
-	MAIN "sercon" STACK "2048")
+	PRIORITY "SCHED_PRIORITY_DEFAULT"
+	MAIN "sercon"
+	STACK_MAIN "2048")
 
 add_custom_target(serdis)
 set_target_properties(serdis PROPERTIES
-	MAIN "serdis" STACK "2048")
+	PRIORITY "SCHED_PRIORITY_DEFAULT"
+	MAIN "serdis"
+	STACK_MAIN "2048")
diff --git a/nuttx-configs/crazyflie/include/board.h b/nuttx-configs/crazyflie/include/board.h
index 9857eb03a2400ad802d8512b827d1f06c6f77721..05a32dcad77f4443f8d52977b2844814f78843dd 100644
--- a/nuttx-configs/crazyflie/include/board.h
+++ b/nuttx-configs/crazyflie/include/board.h
@@ -162,12 +162,16 @@
  * UARTs.
  *
  */
+ /* E_TX2 / E_RX2 */
 #define GPIO_USART2_RX	GPIO_USART2_RX_1
 #define GPIO_USART2_TX	GPIO_USART2_TX_1
 
+/* E_TX1 / E_RX1 */
 #define GPIO_USART3_RX	GPIO_USART3_RX_2
 #define GPIO_USART3_TX	GPIO_USART3_TX_2
 
+/* NRF51 via syslink */
+// TODO: Flow control?
 #define GPIO_USART6_RX	GPIO_USART6_RX_1
 #define GPIO_USART6_TX	GPIO_USART6_TX_1
 
diff --git a/nuttx-configs/crazyflie/nsh/defconfig b/nuttx-configs/crazyflie/nsh/defconfig
index dae2963998dfb3cdb07d50545a77a59239747596..7b5b196317b36915a65b6c1d9702272dd2283601 100644
--- a/nuttx-configs/crazyflie/nsh/defconfig
+++ b/nuttx-configs/crazyflie/nsh/defconfig
@@ -433,7 +433,27 @@ CONFIG_WATCHDOG=y
 # CONFIG_INPUT is not set
 # CONFIG_LCD is not set
 # CONFIG_MMCSD is not set
-# CONFIG_MTD is not set
+CONFIG_MTD=y
+
+#
+# MTD Configuration
+#
+CONFIG_MTD_PARTITION=y
+CONFIG_MTD_BYTE_WRITE=y
+
+#
+# MTD Device Drivers
+#
+# CONFIG_RAMMTD is not set
+CONFIG_MTD_AT24XX=y
+# 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_RAMTRON_FUJITSU is not set
+# CONFIG_MTD_SST25 is not set
+# CONFIG_MTD_SST39FV is not set
+# CONFIG_MTD_W25 is not set
 CONFIG_PIPES=y
 # CONFIG_PM is not set
 # CONFIG_POWER is not set
@@ -527,7 +547,7 @@ CONFIG_CDCACM_RXBUFSIZE=300
 CONFIG_CDCACM_TXBUFSIZE=1000
 CONFIG_CDCACM_VENDORID=0x26ac
 CONFIG_CDCACM_PRODUCTID=0x0016
-CONFIG_CDCACM_VENDORSTR="3D Robotics"
+CONFIG_CDCACM_VENDORSTR="Bitcraze AB"
 CONFIG_CDCACM_PRODUCTSTR="PX4 Crazyflie v2.0"
 # CONFIG_USBMSC is not set
 # CONFIG_USBHOST is not set
diff --git a/src/drivers/boards/crazyflie/CMakeLists.txt b/src/drivers/boards/crazyflie/CMakeLists.txt
index c5d9913bb09b03517e76e17fa4c9dd8d9c27c954..d74e3d6bc44d9c9b478ec77d5c7029e104847cf8 100644
--- a/src/drivers/boards/crazyflie/CMakeLists.txt
+++ b/src/drivers/boards/crazyflie/CMakeLists.txt
@@ -37,6 +37,7 @@ px4_add_module(
 		crazyflie_usb.c
 		crazyflie_led.c
 		crazyflie_timer_config.c
+		crazyflie_i2c.cpp
 	DEPENDS
 		platforms__common
 	)
diff --git a/src/drivers/boards/crazyflie/board_config.h b/src/drivers/boards/crazyflie/board_config.h
index 948956cddacaf1b626029ad1842396ce1d953701..dfc59d456e136601e051a904ddb3ae79f397a8e9 100644
--- a/src/drivers/boards/crazyflie/board_config.h
+++ b/src/drivers/boards/crazyflie/board_config.h
@@ -83,6 +83,8 @@ __BEGIN_DECLS
 #define PX4_I2C_BUS_ONBOARD_HZ      400000
 #define PX4_I2C_BUS_EXPANSION_HZ      400000
 
+#define PX4_I2C_BUS_MTD	PX4_I2C_BUS_EXPANSION
+
 
 
 /* Devices on the onboard bus.
@@ -127,6 +129,13 @@ __BEGIN_DECLS
 #define GPIO_TIM2_CH1OUT	GPIO_TIM2_CH1OUT_2
 #define GPIO_TIM4_CH4OUT	GPIO_TIM4_CH4OUT_1
 
+#define GPIO_TIM2_CH2IN		GPIO_TIM2_CH2IN_1
+#define GPIO_TIM2_CH4IN		GPIO_TIM2_CH4IN_2
+#define GPIO_TIM2_CH1IN		GPIO_TIM2_CH1IN_2
+#define GPIO_TIM4_CH4IN		GPIO_TIM4_CH4IN_1
+
+
+
 /* High-resolution timer */
 #define HRT_TIMER		8	/* use timer8 for the HRT */
 #define HRT_TIMER_CHANNEL	1	/* use capture/compare channel */
@@ -176,6 +185,20 @@ extern void stm32_usbinitialize(void);
 int nsh_archinitialize(void);
 #endif
 
+
+
+
+/****************************************************************************
+ * Name: board_i2c_initialize
+ *
+ * Description:
+ *   Called to set I2C bus frequncies.
+ *
+ ****************************************************************************/
+
+int board_i2c_initialize(void);
+
+
 #endif /* __ASSEMBLY__ */
 
 __END_DECLS
diff --git a/src/drivers/boards/crazyflie/crazyflie_i2c.cpp b/src/drivers/boards/crazyflie/crazyflie_i2c.cpp
index ad8ff063b63cf0a081f2539f17d514dc8c727c61..932f48a8b02e32b7b6de3c3a9228e386d1e4c693 100644
--- a/src/drivers/boards/crazyflie/crazyflie_i2c.cpp
+++ b/src/drivers/boards/crazyflie/crazyflie_i2c.cpp
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
  *   Copyright (c) 2016 PX4 Development Team. All rights reserved.
- *         Author: David Sidrane <david_s5@nscdg.com>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
diff --git a/src/drivers/boards/crazyflie/crazyflie_init.c b/src/drivers/boards/crazyflie/crazyflie_init.c
index fb999f6b1b0cd4264567e21db58a042247d18407..966862f6b68719ff84c4a23564752aa0be37e871 100644
--- a/src/drivers/boards/crazyflie/crazyflie_init.c
+++ b/src/drivers/boards/crazyflie/crazyflie_init.c
@@ -145,6 +145,7 @@ __EXPORT int matherr(struct exception *e)
 
 __EXPORT int nsh_archinitialize(void)
 {
+	int result;
 
 	/* configure the high-resolution time/callout interface */
 	hrt_init();
@@ -175,10 +176,10 @@ __EXPORT int nsh_archinitialize(void)
 
 	result = board_i2c_initialize();
 
-//	if (result != OK) {
+	if (result != OK) {
 //		up_ledon(LED_AMBER);
-//		return -ENODEV;
-//	}
+		return -ENODEV;
+	}
 
 
 
diff --git a/src/drivers/boards/crazyflie/crazyflie_timer_config.c b/src/drivers/boards/crazyflie/crazyflie_timer_config.c
index 4617a23a994d47eb2491bfb824abd6cd96babd2f..936896297c3acb96a4a869af8d333d0966f19ca7 100644
--- a/src/drivers/boards/crazyflie/crazyflie_timer_config.c
+++ b/src/drivers/boards/crazyflie/crazyflie_timer_config.c
@@ -42,11 +42,11 @@
 #include <stdint.h>
 
 #include <stm32.h>
-#include <stm32_gpio_out.h>
+#include <stm32_gpio.h>
 #include <stm32_tim.h>
 
-#include <drivers/stm32/drv_io_timer.h>
 #include <drivers/drv_pwm_output.h>
+#include <drivers/stm32/drv_io_timer.h>
 
 #include "board_config.h"
 
@@ -55,7 +55,7 @@ __EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = {
 		.base = STM32_TIM2_BASE,
 		.clock_register = STM32_RCC_APB1ENR,
 		.clock_bit = RCC_APB1ENR_TIM2EN,
-		.clock_freq = STM32_APB1_TIM2_CLKIN
+		.clock_freq = STM32_APB1_TIM2_CLKIN,
 		.first_channel_index = 0,
 		.last_channel_index = 2,
 		.handler = io_timer_handler0,
@@ -65,7 +65,7 @@ __EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = {
 		.base = STM32_TIM4_BASE,
 		.clock_register = STM32_RCC_APB1ENR,
 		.clock_bit = RCC_APB1ENR_TIM4EN,
-		.clock_freq = STM32_APB1_TIM4_CLKIN
+		.clock_freq = STM32_APB1_TIM4_CLKIN,
 		.first_channel_index = 3,
 		.last_channel_index = 3,
 		.handler = io_timer_handler1,
diff --git a/src/drivers/crazyflie/crazyflie.cpp b/src/drivers/crazyflie/crazyflie.cpp
index e9c9ef87f63448e143f75ee26c6c5089763848ef..e0ba77a436f176097731ff7864d7d53ec261c383 100644
--- a/src/drivers/crazyflie/crazyflie.cpp
+++ b/src/drivers/crazyflie/crazyflie.cpp
@@ -72,10 +72,6 @@
 #include <drivers/drv_rc_input.h>
 
 #include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/actuator_controls_0.h>
-#include <uORB/topics/actuator_controls_1.h>
-#include <uORB/topics/actuator_controls_2.h>
-#include <uORB/topics/actuator_controls_3.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/parameter_update.h>
@@ -1078,10 +1074,18 @@ crazyflie_main(int argc, char *argv[])
 		errx(0, "Crazyflie driver stopped");
 	}
 
-	if (crazyflie_start() != OK) {
-		errx(1, "failed to start the Crazyflie driver");
+	if (!strcmp(verb, "start")) {
+		if (crazyflie_start() == OK) {
+			errx(0, "Crazyflie driver started");
+		}
+		else {
+			errx(1, "failed to start the Crazyflie driver");
+		}
+
 	}
 
+	// TODO: Ensure driver is running
+
 	if (!strcmp(verb, "test")) {
 		test();
 	}
diff --git a/src/systemcmds/mtd/mtd.c b/src/systemcmds/mtd/mtd.c
index 30658de117c147bef80069e3d3f21bb374db766f..5afd6eb9104c48d54a5ba40d13951e7e9872a484 100644
--- a/src/systemcmds/mtd/mtd.c
+++ b/src/systemcmds/mtd/mtd.c
@@ -80,10 +80,15 @@ int mtd_main(int argc, char *argv[])
 static void	ramtron_attach(void);
 #else
 
-#ifndef PX4_I2C_BUS_ONBOARD
-#  error PX4_I2C_BUS_ONBOARD not defined, cannot locate onboard EEPROM
+#ifndef PX4_I2C_BUS_MTD
+#  ifdef PX4_I2C_BUS_ONBOARD
+#    define PX4_I2C_BUS_MTD PX4_I2C_BUS_ONBOARD
+#  else
+#    error PX4_I2C_BUS_MTD and PX4_I2C_BUS_ONBOARD not defined, cannot locate onboard EEPROM
+#  endif
 #endif
 
+
 static void	at24xxx_attach(void);
 #endif
 static void	mtd_start(char *partition_names[], unsigned n_partitions);
@@ -225,7 +230,7 @@ static void
 at24xxx_attach(void)
 {
 	/* find the right I2C */
-	struct i2c_dev_s *i2c = px4_i2cbus_initialize(PX4_I2C_BUS_ONBOARD);
+	struct i2c_dev_s *i2c = px4_i2cbus_initialize(PX4_I2C_BUS_MTD);
 	/* this resets the I2C bus, set correct bus speed again */
 	I2C_SETFREQUENCY(i2c, 400000);