Kicked out mix_and_link, deleted old MPU driver, disabled (still needed for reference) old HMC and MS5611 drivers. Removed driver init from up_nsh.c. Reworked fixedwing_control to be closer to up-to-date api, still more clean up needed. Fixed a bug that limited the motor thrust for multirotor control

6 files changed, 17 insertions, 610 deletions

diff --git a/nuttx/configs/px4fmu/include/drv_mpu6000.h b/nuttx/configs/px4fmu/include/drv_mpu6000.h
deleted file mode 100644
index 0a5a48b70..000000000
--- a/nuttx/configs/px4fmu/include/drv_mpu6000.h
+++ /dev/null
@@ -1,92 +0,0 @@
-/*
- *   Copyright (C) 2012 Lorenz Meier. 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 of the author or the names of 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.
- */
-
-/*
- * Driver for the Invense MPU-6000 gyroscope
- */
-
-/* IMPORTANT NOTES:
- *
- * SPI max. clock frequency: 10 Mhz
- * CS has to be high before transfer,
- * go low right before transfer and
- * go high again right after transfer
- *
- */
-
-#include <sys/ioctl.h>
-
-#define _MPU6000BASE	0x7600
-#define MPU6000C(_x)	_IOC(_MPU6000BASE, _x)
-
-/* 
- * Sets the sensor internal sampling rate, and if a buffer
- * has been configured, the rate at which entries will be
- * added to the buffer.
- */
-#define MPU6000_SETRATE		MPU6000C(1)
-
-#define MPU6000_RATE_95HZ_LP_12_5HZ		((0<<7) | (0<<6) | (0<<5) | (0<<4))
-
-/*
- * Sets the sensor internal range.
- */
-#define MPU6000_SETRANGE		MPU6000C(2)
-
-#define MPU6000_RANGE_250DPS			(0<<4)
-
-#define MPU6000_RATE_95HZ			((0<<6) | (0<<4))
-
-
-
-/*
- * Sets the address of a shared MPU6000_buffer
- * structure that is maintained by the driver.
- *
- * If zero is passed as the address, disables
- * the buffer updating.
- */
-#define MPU6000_SETBUFFER	MPU6000C(3)
-
-struct MPU6000_buffer {
-	uint32_t	size;		/* number of entries in the samples[] array */
-	uint32_t	next;		/* the next entry that will be populated */
-	struct {
-		uint16_t	x;
-		uint16_t	y;
-		uint16_t	z;
-		uint16_t	roll;
-		uint16_t	pitch;
-		uint16_t	yaw;
-	} samples[];
-};
-
-extern int	mpu6000_attach(struct spi_dev_s *spi, int spi_id);
diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig
index a99b5f6ad..834b18165 100644
--- a/nuttx/configs/px4fmu/nsh/appconfig
+++ b/nuttx/configs/px4fmu/nsh/appconfig
@@ -71,12 +71,10 @@ CONFIGURED_APPS += gps
 CONFIGURED_APPS += commander
 #CONFIGURED_APPS += sdlog
 CONFIGURED_APPS += sensors
-CONFIGURED_APPS += ardrone_control
 CONFIGURED_APPS += ardrone_interface
 CONFIGURED_APPS += multirotor_att_control
 CONFIGURED_APPS += px4/attitude_estimator_bm
 CONFIGURED_APPS += fixedwing_control
-CONFIGURED_APPS += mix_and_link
 CONFIGURED_APPS += position_estimator
 CONFIGURED_APPS += attitude_estimator_ekf
 
diff --git a/nuttx/configs/px4fmu/src/Makefile b/nuttx/configs/px4fmu/src/Makefile
index 2e3138aaa..5a60b7d19 100644
--- a/nuttx/configs/px4fmu/src/Makefile
+++ b/nuttx/configs/px4fmu/src/Makefile
@@ -42,9 +42,9 @@ AOBJS		= $(ASRCS:.S=$(OBJEXT))
 
 CSRCS		= up_boot.c up_leds.c up_spi.c up_hrt.c \
 		  drv_gpio.c drv_bma180.c drv_l3gd20.c \
-		  drv_led.c drv_hmc5833l.c drv_ms5611.c drv_eeprom.c \
+		  drv_led.c drv_eeprom.c \
 		  drv_tone_alarm.c up_pwm_servo.c up_usbdev.c \
-		  up_cpuload.c drv_mpu6000.c
+		  up_cpuload.c
 
 ifeq ($(CONFIG_NSH_ARCHINIT),y)
 CSRCS		+= up_nsh.c
diff --git a/nuttx/configs/px4fmu/src/drv_mpu6000.c b/nuttx/configs/px4fmu/src/drv_mpu6000.c
deleted file mode 100644
index aded70318..000000000
--- a/nuttx/configs/px4fmu/src/drv_mpu6000.c
+++ /dev/null
@@ -1,433 +0,0 @@
-/*
- *   Copyright (C) 2012 Lorenz Meier. 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 of the author or the names of 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.
- */
-
-/*
- * Driver for the ST mpu6000 MEMS gyroscope
- */
-
-#include <nuttx/config.h>
-
-#include <stdint.h>
-#include <stdbool.h>
-#include <debug.h>
-#include <errno.h>
-#include <unistd.h>
-#include <stdio.h>
-
-#include <nuttx/spi.h>
-#include <arch/board/board.h>
-#include <nuttx/arch.h>
-
-#include "chip.h"
-#include "stm32_internal.h"
-#include "px4fmu-internal.h"
-
-#include <arch/board/drv_mpu6000.h>
-
-#define DIR_READ					(0x80)
-#define DIR_WRITE					(0<<7)
-#define ADDR_INCREMENT				(1<<6)
-
-#define WHO_I_AM					0xD4
-
-// MPU 6000 registers
-#define MPUREG_WHOAMI 0x75 //
-#define MPUREG_SMPLRT_DIV 0x19 //
-#define MPUREG_CONFIG 0x1A //
-#define MPUREG_GYRO_CONFIG 0x1B
-#define MPUREG_ACCEL_CONFIG 0x1C
-#define MPUREG_FIFO_EN 0x23
-#define MPUREG_INT_PIN_CFG 0x37
-#define MPUREG_INT_ENABLE 0x38
-#define MPUREG_INT_STATUS 0x3A
-#define MPUREG_ACCEL_XOUT_H 0x3B //
-#define MPUREG_ACCEL_XOUT_L 0x3C //
-#define MPUREG_ACCEL_YOUT_H 0x3D //
-#define MPUREG_ACCEL_YOUT_L 0x3E //
-#define MPUREG_ACCEL_ZOUT_H 0x3F //
-#define MPUREG_ACCEL_ZOUT_L 0x40 //
-#define MPUREG_TEMP_OUT_H 0x41//
-#define MPUREG_TEMP_OUT_L 0x42//
-#define MPUREG_GYRO_XOUT_H 0x43 //
-#define MPUREG_GYRO_XOUT_L 0x44 //
-#define MPUREG_GYRO_YOUT_H 0x45 //
-#define MPUREG_GYRO_YOUT_L 0x46 //
-#define MPUREG_GYRO_ZOUT_H 0x47 //
-#define MPUREG_GYRO_ZOUT_L 0x48 //
-#define MPUREG_USER_CTRL 0x6A //
-#define MPUREG_PWR_MGMT_1 0x6B //
-#define MPUREG_PWR_MGMT_2 0x6C //
-#define MPUREG_FIFO_COUNTH 0x72
-#define MPUREG_FIFO_COUNTL 0x73
-#define MPUREG_FIFO_R_W 0x74
-#define MPUREG_PRODUCT_ID 			0x0C	// Product ID Register
-
-
-// Configuration bits MPU 3000 and MPU 6000 (not revised)?
-#define BIT_SLEEP 0x40
-#define BIT_H_RESET 0x80
-#define BITS_CLKSEL 0x07
-#define MPU_CLK_SEL_PLLGYROX 0x01
-#define MPU_CLK_SEL_PLLGYROZ 0x03
-#define MPU_EXT_SYNC_GYROX 0x02
-#define BITS_FS_250DPS              0x00
-#define BITS_FS_500DPS              0x08
-#define BITS_FS_1000DPS             0x10
-#define BITS_FS_2000DPS             0x18
-#define BITS_FS_MASK                0x18
-#define BITS_DLPF_CFG_256HZ_NOLPF2  0x00
-#define BITS_DLPF_CFG_188HZ         0x01
-#define BITS_DLPF_CFG_98HZ          0x02
-#define BITS_DLPF_CFG_42HZ          0x03
-#define BITS_DLPF_CFG_20HZ          0x04
-#define BITS_DLPF_CFG_10HZ          0x05
-#define BITS_DLPF_CFG_5HZ           0x06
-#define BITS_DLPF_CFG_2100HZ_NOLPF  0x07
-#define BITS_DLPF_CFG_MASK          0x07
-#define BIT_INT_ANYRD_2CLEAR      0x10
-#define BIT_RAW_RDY_EN        0x01
-#define BIT_I2C_IF_DIS              0x10
-#define BIT_INT_STATUS_DATA   0x01
-											// Product ID Description for MPU6000
-											// high 4 bits 	low 4 bits
-											// Product Name	Product Revision
-#define MPU6000ES_REV_C4 			0x14 	// 0001			0100
-#define MPU6000ES_REV_C5 			0x15 	// 0001			0101
-#define MPU6000ES_REV_D6 			0x16	// 0001			0110
-#define MPU6000ES_REV_D7 			0x17	// 0001			0111
-#define MPU6000ES_REV_D8 			0x18	// 0001			1000	
-#define MPU6000_REV_C4 				0x54	// 0101			0100 
-#define MPU6000_REV_C5 				0x55	// 0101			0101
-#define MPU6000_REV_D6 				0x56	// 0101			0110	
-#define MPU6000_REV_D7 				0x57	// 0101			0111
-#define MPU6000_REV_D8 				0x58	// 0101			1000
-#define MPU6000_REV_D9 				0x59	// 0101			1001
-#define MPU6000_REV_D10 			0x5A	// 0101			1010
-
-static FAR struct mpu6000_dev_s	mpu6000_dev;
-
-static ssize_t mpu6000_read(struct file *filp, FAR char *buffer, size_t buflen);
-static int mpu6000_ioctl(struct file *filp, int cmd, unsigned long arg);
-
-static const struct file_operations mpu6000_fops = {
-	.open  = 0,
-	.close = 0,
-	.read  = mpu6000_read,
-	.write = 0,
-	.seek  = 0,
-	.ioctl = mpu6000_ioctl,
-#ifndef CONFIG_DISABLE_POLL
-	.poll  = 0
-#endif
-};
-
-struct mpu6000_dev_s
-{
-	struct spi_dev_s	*spi;
-	int			spi_id;
-	uint8_t			rate;
-	struct mpu6000_buffer	*buffer;
-};
-
-static void	mpu6000_write_reg(uint8_t address, uint8_t data);
-static uint8_t	mpu6000_read_reg(uint8_t address);
-
-static void
-mpu6000_write_reg(uint8_t address, uint8_t data)
-{
-	uint8_t cmd[2] = { address | DIR_WRITE, data };
-
-	SPI_SELECT(mpu6000_dev.spi, mpu6000_dev.spi_id, true);
-	SPI_SNDBLOCK(mpu6000_dev.spi, &cmd, sizeof(cmd));
-	SPI_SELECT(mpu6000_dev.spi, mpu6000_dev.spi_id, false);
-}
-
-static uint8_t
-mpu6000_read_reg(uint8_t address)
-{
-	uint8_t	cmd[2] = {address | DIR_READ, 0};
-	uint8_t data[2];
-
-	SPI_SELECT(mpu6000_dev.spi, mpu6000_dev.spi_id, true);
-	SPI_EXCHANGE(mpu6000_dev.spi, cmd, data, sizeof(cmd));
-	SPI_SELECT(mpu6000_dev.spi, mpu6000_dev.spi_id, false);
-
-	return data[1];
-}
-
-static int16_t
-mpu6000_read_int16(uint8_t address)
-{
-	uint8_t	cmd[3] = {address | DIR_READ, 0, 0};
-	uint8_t data[3];
-
-	SPI_SELECT(mpu6000_dev.spi, mpu6000_dev.spi_id, true);
-	SPI_EXCHANGE(mpu6000_dev.spi, cmd, data, sizeof(cmd));
-	SPI_SELECT(mpu6000_dev.spi, mpu6000_dev.spi_id, false);
-
-	return (((int16_t)data[1])<<8) | data[2];
-}
-
-static int
-mpu6000_set_range(uint8_t range)
-{
-//	/* mask out illegal bit positions */
-//	uint8_t write_range = range & REG4_RANGE_MASK;
-//	/* immediately return if user supplied invalid value */
-//	if (write_range != range) return EINVAL;
-//	/* set remaining bits to a sane value */
-//	write_range |= REG4_BDU;
-//	/* write to device */
-//	write_reg(ADDR_CTRL_REG4, write_range);
-//	/* return 0 if register value is now written value, 1 if unchanged */
-//	return !(read_reg(ADDR_CTRL_REG4) == write_range);
-}
-
-static int
-mpu6000_set_rate(uint8_t rate)
-{
-//	/* mask out illegal bit positions */
-//	uint8_t write_rate = rate & REG1_RATE_LP_MASK;
-//	/* immediately return if user supplied invalid value */
-//	if (write_rate != rate) return EINVAL;
-//	/* set remaining bits to a sane value */
-//	write_rate |= REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE;
-//	/* write to device */
-//	write_reg(ADDR_CTRL_REG1, write_rate);
-//	/* return 0 if register value is now written value, 1 if unchanged */
-//	return !(read_reg(ADDR_CTRL_REG1) == write_rate);
-}
-
-static int
-mpu6000_read_fifo(int16_t *data)
-{
-//	struct {					/* status register and data as read back from the device */
-//		uint8_t		cmd;
-//		uint8_t		temp;
-//		uint8_t		status;
-//		int16_t		x;
-//		int16_t		y;
-//		int16_t		z;
-//	} __attribute__((packed))	report;
-//
-//	report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT;
-//
-//	/* exchange the report structure with the device */
-//	SPI_LOCK(mpu6000_dev.spi, true);
-//
-//	SPI_SELECT(mpu6000_dev.spi, mpu6000_dev.spi_id, true);
-//
-//	read_reg(ADDR_WHO_AM_I);
-//
-//	SPI_EXCHANGE(mpu6000_dev.spi, &report, &report, sizeof(report));
-//	SPI_SELECT(mpu6000_dev.spi, mpu6000_dev.spi_id, false);
-//
-//	SPI_LOCK(mpu6000_dev.spi, false);
-//
-//
-//
-	//
-
-	// Device has MSB first at lower address (big endian)
-
-
-	struct {					/* status register and data as read back from the device */
-		// uint8_t		cmd;
-		// uint8_t		int_status;
-		int16_t		xacc;
-		int16_t		yacc;
-		int16_t		zacc;
-		int16_t		temp;
-		int16_t		rollspeed;
-		int16_t		pitchspeed;
-		int16_t		yawspeed;
-	} report;
-
-	uint8_t cmd[sizeof(report)];
-	cmd[0] = MPUREG_ACCEL_XOUT_H | DIR_READ; // was addr_incr
-
-	SPI_LOCK(mpu6000_dev.spi, true);
-	SPI_SELECT(mpu6000_dev.spi, PX4_SPIDEV_MPU, true);
-	report.xacc = mpu6000_read_int16(MPUREG_ACCEL_XOUT_H);
-	report.yacc = mpu6000_read_int16(MPUREG_ACCEL_YOUT_H);
-	report.zacc = mpu6000_read_int16(MPUREG_ACCEL_ZOUT_H);
-	report.rollspeed = mpu6000_read_int16(MPUREG_GYRO_XOUT_H);
-	report.pitchspeed = mpu6000_read_int16(MPUREG_GYRO_YOUT_H);
-	report.yawspeed = mpu6000_read_int16(MPUREG_GYRO_ZOUT_H);
-	SPI_SELECT(mpu6000_dev.spi, PX4_SPIDEV_MPU, false);
-	SPI_LOCK(mpu6000_dev.spi, false);
-
-	data[0] = report.xacc;
-	data[1] = report.yacc;
-	data[2] = report.zacc;
-	data[3] = report.rollspeed;
-	data[4] = report.pitchspeed;
-	data[5] = report.yawspeed;
-
-	return 1;//(report.int_status & 0x01);
-}
-
-static ssize_t
-mpu6000_read(struct file *filp, char *buffer, size_t buflen)
-{
-	/* if the buffer is large enough, and data are available, return success */
-	if (buflen >= 12) {
-		if (mpu6000_read_fifo((int16_t *)buffer))
-			return 12;
-
-		/* no data */
-		return 0;
-	}
-
-	/* buffer too small */
-	errno = ENOSPC;
-	return ERROR;
-}
-
-static int
-mpu6000_ioctl(struct file *filp, int cmd, unsigned long arg)
-{
-	int	result = ERROR;
-
-	switch (cmd) {
-        case MPU6000_SETRATE:
-            if ((arg & 0x00/* XXX REG MASK MISSING */) == arg) {
-            	SPI_LOCK(mpu6000_dev.spi, true);
-                mpu6000_set_rate(arg);
-                SPI_LOCK(mpu6000_dev.spi, false);
-                result = 0;
-                mpu6000_dev.rate = arg;
-            }
-            break;
-
-        case MPU6000_SETRANGE:
-            if ((arg & 0x00/* XXX REG MASK MISSING */) == arg) {
-            	SPI_LOCK(mpu6000_dev.spi, true);
-                mpu6000_set_range(arg);
-                SPI_LOCK(mpu6000_dev.spi, false);
-                result = 0;
-            }
-            break;
-
-        case MPU6000_SETBUFFER:
-            mpu6000_dev.buffer = (struct mpu6000_buffer *)arg;
-            result = 0;
-            break;
-	}
-
-	if (result)
-		errno = EINVAL;
-	return result;
-}
-
-int
-mpu6000_attach(struct spi_dev_s *spi, int spi_id)
-{
-	int	result = ERROR;
-
-	mpu6000_dev.spi = spi;
-	mpu6000_dev.spi_id = spi_id;
-
-	SPI_LOCK(mpu6000_dev.spi, true);
-
-	// Set sensor-specific SPI mode
-	SPI_SETFREQUENCY(mpu6000_dev.spi, 10000000); // 500 KHz
-	SPI_SETBITS(mpu6000_dev.spi, 8);
-	// Either mode 1 or mode 3
-	SPI_SETMODE(mpu6000_dev.spi, SPIDEV_MODE3);
-
-	// Chip reset
-	mpu6000_write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET);
-	up_udelay(10000);
-	// Wake up device and select GyroZ clock (better performance)
-	mpu6000_write_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ);
-	up_udelay(1000);
-	// Disable I2C bus (recommended on datasheet)
-	mpu6000_write_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS);
-	up_udelay(1000);
-	// SAMPLE RATE
-	mpu6000_write_reg(MPUREG_SMPLRT_DIV,0x04);     // Sample rate = 200Hz    Fsample= 1Khz/(4+1) = 200Hz
-	usleep(1000);
-	// FS & DLPF   FS=2000¼/s, DLPF = 98Hz (low pass filter)
-	mpu6000_write_reg(MPUREG_CONFIG, BITS_DLPF_CFG_98HZ);
-	usleep(1000);
-	mpu6000_write_reg(MPUREG_GYRO_CONFIG,BITS_FS_2000DPS);  // Gyro scale 2000¼/s
-	usleep(1000);
-
-	uint8_t _product_id = mpu6000_read_reg(MPUREG_PRODUCT_ID);
-	printf("MPU-6000 product id: %d\n", (int)_product_id);
-
-	if ((_product_id == MPU6000ES_REV_C4) || (_product_id == MPU6000ES_REV_C5) ||
-		(_product_id == MPU6000_REV_C4)   || (_product_id == MPU6000_REV_C5)){
-		// Accel scale 8g (4096 LSB/g)
-		// Rev C has different scaling than rev D
-		mpu6000_write_reg(MPUREG_ACCEL_CONFIG,1<<3);
-	} else {
-		// Accel scale 8g (4096 LSB/g)
-		mpu6000_write_reg(MPUREG_ACCEL_CONFIG,2<<3);
-	}
-	usleep(1000);
-
-	// INT CFG => Interrupt on Data Ready
-	mpu6000_write_reg(MPUREG_INT_ENABLE,BIT_RAW_RDY_EN);         // INT: Raw data ready
-	usleep(1000);
-	mpu6000_write_reg(MPUREG_INT_PIN_CFG,BIT_INT_ANYRD_2CLEAR);  // INT: Clear on any read
-	usleep(1000);
-	// Oscillator set
-	// write_reg(MPUREG_PWR_MGMT_1,MPU_CLK_SEL_PLLGYROZ);
-	usleep(1000);
-
-	/* revert back to normal bus mode */
-	SPI_SETFREQUENCY(mpu6000_dev.spi, 10000000);
-	SPI_SETBITS(mpu6000_dev.spi, 8);
-	SPI_SETMODE(mpu6000_dev.spi, SPIDEV_MODE3);
-
-	/* verify that the device is attached and functioning */
-	if ((_product_id == MPU6000ES_REV_C4) || (_product_id == MPU6000ES_REV_C5) ||
-		(_product_id == MPU6000_REV_C4)   || (_product_id == MPU6000_REV_C5) ||
-		(_product_id == MPU6000_REV_D7)	  || (_product_id == MPU6000_REV_D8) ||
-		(_product_id == MPU6000_REV_D9)   || (_product_id == MPU6000_REV_D10)){
-
-		/* make ourselves available */
-		register_driver("/dev/mpu6000", &mpu6000_fops, 0666, NULL);
-
-		result = OK;
-	} else {
-
-		errno = EIO;
-	}
-
-	SPI_LOCK(mpu6000_dev.spi, false);
-
-	SPI_LOCK(mpu6000_dev.spi, false);
-
-	return result;
-}
diff --git a/nuttx/configs/px4fmu/src/up_cpuload.c b/nuttx/configs/px4fmu/src/up_cpuload.c
index 750ec4852..bf867ae8b 100644
--- a/nuttx/configs/px4fmu/src/up_cpuload.c
+++ b/nuttx/configs/px4fmu/src/up_cpuload.c
@@ -45,6 +45,7 @@
 #include <debug.h>
 
 #include <sys/time.h>
+#include <sched.h>
 
 #include <arch/board/board.h>
 #include <arch/board/up_hrt.h>
diff --git a/nuttx/configs/px4fmu/src/up_nsh.c b/nuttx/configs/px4fmu/src/up_nsh.c
index d185bf7f1..25f3c2795 100644
--- a/nuttx/configs/px4fmu/src/up_nsh.c
+++ b/nuttx/configs/px4fmu/src/up_nsh.c
@@ -1,5 +1,5 @@
 /****************************************************************************
- * config/stm3210e_eval/src/up_nsh.c
+ * config/px4fmu/src/up_nsh.c
  * arch/arm/src/board/up_nsh.c
  *
  *   Copyright (C) 2009, 2011 Gregory Nutt. All rights reserved.
@@ -63,11 +63,8 @@
 #include <arch/board/board.h>
 #include <arch/board/drv_bma180.h>
 #include <arch/board/drv_l3gd20.h>
-#include <arch/board/drv_hmc5883l.h>
-#include <arch/board/drv_mpu6000.h>
-#include <arch/board/drv_ms5611.h>
-#include <arch/board/drv_eeprom.h>
 #include <arch/board/drv_led.h>
+#include <arch/board/drv_eeprom.h>
 
 /****************************************************************************
  * Pre-Processor Definitions
@@ -96,20 +93,6 @@
  ****************************************************************************/
 
 /****************************************************************************
- * Name: multiport_setup
- *
- * Description:
- *   Perform setup of the PX4FMU's multi function ports
- *
- ****************************************************************************/
-int multiport_setup(void)
-{
-  int result = OK;
-
-  return result;
-}
-
-/****************************************************************************
  * Public Functions
  ****************************************************************************/
 
@@ -221,7 +204,7 @@ int nsh_archinitialize(void)
 	  up_udelay(1000);
   }
 
-  if (gyro_fail) message("[boot] FAILED to attach L3GD20 gyro\r\n");
+  if (!gyro_fail) message("[boot] Found L3GD20 gyro\n");
 
   int acc_attempts = 0;
   int acc_fail = 0;
@@ -234,13 +217,13 @@ int nsh_archinitialize(void)
 	  up_udelay(1000);
   }
 
-  if (acc_fail) message("[boot] FAILED to attach BMA180 accelerometer\r\n");
+  if (!acc_fail) message("[boot] Found BMA180 accelerometer\n");
 
   /* initialize I2C2 bus */
 
   i2c2 = up_i2cinitialize(2);
   if (!i2c2) {
-	  message("[boot] FAILED to initialize I2C bus 2\r\n");
+	  message("[boot] FAILED to initialize I2C bus 2\n");
 	  up_ledon(LED_AMBER);
 	  return -ENODEV;
   }
@@ -251,7 +234,7 @@ int nsh_archinitialize(void)
 
   i2c3 = up_i2cinitialize(3);
   if (!i2c3) {
-	  message("[boot] FAILED to initialize I2C bus 3\r\n");
+	  message("[boot] FAILED to initialize I2C bus 3\n");
 	  up_ledon(LED_AMBER);
 	  return -ENODEV;
   }
@@ -259,91 +242,41 @@ int nsh_archinitialize(void)
   /* set I2C3 speed */
   I2C_SETFREQUENCY(i2c3, 400000);
 
-  int mag_attempts = 0;
-  int mag_fail = 0;
-
-  while (mag_attempts < 5)
-  {
-	  mag_fail = hmc5883l_attach(i2c2);
-	  mag_attempts++;
-	  if (mag_fail == 0) break;
-	  up_udelay(1000);
-  }
-
-  if (mag_fail) message("[boot] FAILED to attach HMC5883L magnetometer\r\n");
-
-  int baro_attempts = 0;
-  int baro_fail = 0;
-  while (baro_attempts < 5)
-  {
-	  baro_fail = ms5611_attach(i2c2);
-	  baro_attempts++;
-	  if (baro_fail == 0) break;
-	  up_udelay(1000);
-  }
-
-  if (baro_fail) message("[boot] FAILED to attach MS5611 baro at addr #1 or #2 (0x76 or 0x77)\r\n");
-
   /* try to attach, don't fail if device is not responding */
   (void)eeprom_attach(i2c3, FMU_BASEBOARD_EEPROM_ADDRESS,
 		  FMU_BASEBOARD_EEPROM_TOTAL_SIZE_BYTES,
 		  FMU_BASEBOARD_EEPROM_PAGE_SIZE_BYTES,
 		  FMU_BASEBOARD_EEPROM_PAGE_WRITE_TIME_US, "/dev/baseboard_eeprom", 1);
 
-#if 0
-  int eeprom_attempts = 0;
-  int eeprom_fail;
-  while (eeprom_attempts < 5)
-  {
-	  /* try to attach, fail if device does not respond */
-	  eeprom_fail = eeprom_attach(i2c2, FMU_ONBOARD_EEPROM_ADDRESS,
-			  FMU_ONBOARD_EEPROM_TOTAL_SIZE_BYTES,
-			  FMU_ONBOARD_EEPROM_PAGE_SIZE_BYTES,
-			  FMU_ONBOARD_EEPROM_PAGE_WRITE_TIME_US, "/dev/eeprom", 1);
-	  eeprom_attempts++;
-	  if (eeprom_fail == OK) break;
-	  up_udelay(1000);
-  }
-
-  if (eeprom_fail) message("[boot] FAILED to attach FMU EEPROM\r\n");
-
-#endif
-
-  /* Report back sensor status */
-  if (gyro_fail || mag_fail || baro_fail/* || eeprom_fail*/)
-  {
-	  up_ledon(LED_AMBER);
-  }
-
 #if defined(CONFIG_STM32_SPI3)
   /* Get the SPI port */
 
-  message("[boot] Initializing SPI port 3\r\n");
+  message("[boot] Initializing SPI port 3\n");
   spi3 = up_spiinitialize(3);
   if (!spi3)
     {
-      message("[boot] FAILED to initialize SPI port 3\r\n");
+      message("[boot] FAILED to initialize SPI port 3\n");
       up_ledon(LED_AMBER);
       return -ENODEV;
     }
-  message("[boot] Successfully initialized SPI port 3\r\n");
+  message("[boot] Successfully initialized SPI port 3\n");
 
   /* Now bind the SPI interface to the MMCSD driver */
   result = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi3);
   if (result != OK)
   {
-	  message("[boot] FAILED to bind SPI port 3 to the MMCSD driver\r\n");
+	  message("[boot] FAILED to bind SPI port 3 to the MMCSD driver\n");
 	  up_ledon(LED_AMBER);
 	  return -ENODEV;
   }
-  message("[boot] Successfully bound SPI port 3 to the MMCSD driver\r\n");
+  message("[boot] Successfully bound SPI port 3 to the MMCSD driver\n");
 #endif /* SPI3 */
 
   /* initialize I2C1 bus */
 
   i2c1 = up_i2cinitialize(1);
   if (!i2c1) {
-    message("[boot] FAILED to initialize I2C bus 1\r\n");
+    message("[boot] FAILED to initialize I2C bus 1\n");
     up_ledon(LED_AMBER);
     return -ENODEV;
   }
@@ -355,7 +288,7 @@ int nsh_archinitialize(void)
 
   /* Get board information if available */
 
-    /* Initialize the user GPIOs */
+  /* Initialize the user GPIOs */
   px4fmu_gpio_init();
 
 #ifdef CONFIG_ADC
@@ -367,7 +300,7 @@ int nsh_archinitialize(void)
     if (adc_state != OK)
     {
       /* Give up */
-      message("[boot] FAILED adc_devinit: %d\r\n", adc_state);
+      message("[boot] FAILED adc_devinit: %d\n", adc_state);
       return -ENODEV;
     }
   }