Initial build for Crazyflie 2.0

Working crazyflie firmware build

* Console on USART3
* Could not disable building PX4IO firmware, currently commented out

Don't build PX4IO firmware if the board doesn't ask for it

Added crazyflie motor driver

Fixed wrong register

CLK_SEL is in PWR_MGMT_1

Initial I2C/SPI MPU9250 device

* Tested with I2C
* Need to add error checking
* Intermittent crash on stop call

Working ak8963 mag driver

Functional lps25h driver. Work in progress.

Works well enough to probe and allow sensors task to start.

Added serial port test module

HACK! Get sensors module working

Set crazyflie PWM range

Extend baudrate for Crazyflie's NRF radio

Added dummy tone alarm to allow for init

Added autostart script for Crazyflie

src/drivers/boards/aerocore/CMakeLists.txt

@@ -37,10 +37,10 @@ px4_add_module(
 	SRCS
 		../common/board_name.c
 		aerocore_init.c
-		aerocore_pwm_servo.c
+		aerocore_timer_config.c
 		aerocore_spi.c
 		aerocore_led.c
 	DEPENDS
 		platforms__common
 	)
-# vim: set noet ft=cmake fenc=utf-8 ff=unix : 
+# vim: set noet ft=cmake fenc=utf-8 ff=unix :

src/drivers/boards/crazyflie/CMakeLists.txt

@@ -0,0 +1,43 @@
+############################################################################
+#
+#   Copyright (c) 2015 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.
+#
+############################################################################
+px4_add_module(
+	MODULE drivers__boards__crazyflie
+	SRCS
+		crazyflie_init.c
+		crazyflie_usb.c
+		crazyflie_led.c
+		crazyflie_timer_config.c
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix :

src/drivers/boards/crazyflie/board_config.h

@@ -0,0 +1,181 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file board_config.h
+ *
+ * PX4-CRAZYFLIE internal definitions
+ */
+
+#pragma once
+
+/****************************************************************************************************
+ * Included Files
+ ****************************************************************************************************/
+
+#include <nuttx/config.h>
+#include <nuttx/compiler.h>
+#include <stdint.h>
+
+__BEGIN_DECLS
+
+/* these headers are not C++ safe */
+#include <stm32.h>
+#include <arch/board/board.h>
+
+#define UDID_START		0x1FFF7A10
+
+/****************************************************************************************************
+ * Definitions
+ ****************************************************************************************************/
+/* Configuration ************************************************************************************/
+
+/* PX4-STM32F4Discovery GPIOs ***********************************************************************************/
+/* LEDs */
+// LED1 green, LED2 orange, LED3 red, LED4 blue
+
+
+#define GPIO_LED1       (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|\
+			 GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN0)
+#define GPIO_LED2       (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|\
+			 GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN1)
+#define GPIO_LED3       (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|\
+			 GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN2)
+#define GPIO_LED4       (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|\
+			 GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN3)
+
+/*
+ * I2C busses
+ */
+#define PX4_I2C_BUS_ONBOARD	3
+#define PX4_I2C_BUS_EXPANSION	1
+
+#define PX4_I2C_BUS_ONBOARD_HZ      400000
+#define PX4_I2C_BUS_EXPANSION_HZ      400000
+
+
+
+/* Devices on the onboard bus.
+ *
+ * Note that these are unshifted addresses.
+ */
+#define PX4_I2C_OBDEV_MPU9250	0x69
+
+/* USB OTG FS
+ *
+ * PA9  OTG_FS_VBUS VBUS sensing
+ */
+#define GPIO_OTGFS_VBUS		(GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9)
+
+/*
+ * ADC channels
+ *
+ * These are the channel numbers of the ADCs of the microcontroller that can be used by the Px4 Firmware in the adc driver
+ */
+#define ADC_CHANNELS 0
+
+// ADC defines to be used in sensors.cpp to read from a particular channel
+// Crazyflie 2 performs battery sensing via the NRF module
+#define ADC_BATTERY_VOLTAGE_CHANNEL	((uint8_t)(-1))
+#define ADC_BATTERY_CURRENT_CHANNEL	((uint8_t)(-1))
+#define ADC_AIRSPEED_VOLTAGE_CHANNEL	((uint8_t)(-1))
+
+ /* PWM
+ *
+ * Four PWM motor outputs are configured.
+ *
+ * Pins:
+ *
+ * CH1 : PA1  : TIM2_CH2
+ * CH2 : PB11 : TIM2_CH4
+ * CH3 : PA15 : TIM2_CH1
+ * CH4 : PB9  : TIM4_CH4
+ */
+
+#define GPIO_TIM2_CH2OUT	GPIO_TIM2_CH2OUT_1
+#define GPIO_TIM2_CH4OUT	GPIO_TIM2_CH4OUT_2
+#define GPIO_TIM2_CH1OUT	GPIO_TIM2_CH1OUT_2
+#define GPIO_TIM4_CH4OUT	GPIO_TIM4_CH4OUT_1
+
+/* High-resolution timer */
+#define HRT_TIMER		8	/* use timer8 for the HRT */
+#define HRT_TIMER_CHANNEL	1	/* use capture/compare channel */
+
+/****************************************************************************************************
+ * Public Types
+ ****************************************************************************************************/
+
+/****************************************************************************************************
+ * Public data
+ ****************************************************************************************************/
+
+#ifndef __ASSEMBLY__
+
+/****************************************************************************************************
+ * Public Functions
+ ****************************************************************************************************/
+
+/****************************************************************************************************
+ * Name: stm32_spiinitialize
+ *
+ * Description:
+ *   Called to configure SPI chip select GPIO pins for the PX4FMU board.
+ *
+ ****************************************************************************************************/
+
+extern void stm32_spiinitialize(void);
+
+extern void stm32_usbinitialize(void);
+
+/****************************************************************************
+ * Name: nsh_archinitialize
+ *
+ * Description:
+ *   Perform architecture specific initialization for NSH.
+ *
+ *   CONFIG_NSH_ARCHINIT=y :
+ *     Called from the NSH library
+ *
+ *   CONFIG_BOARD_INITIALIZE=y, CONFIG_NSH_LIBRARY=y, &&
+ *   CONFIG_NSH_ARCHINIT=n :
+ *     Called from board_initialize().
+ *
+ ****************************************************************************/
+
+#ifdef CONFIG_NSH_LIBRARY
+int nsh_archinitialize(void);
+#endif
+
+#endif /* __ASSEMBLY__ */
+
+__END_DECLS

src/drivers/boards/crazyflie/crazyflie_i2c.cpp

@@ -0,0 +1,65 @@
+/****************************************************************************
+ *
+ *   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
+ * 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 crazyflie_i2c.c
+ *
+ * Crazyflie I2C configuration
+ */
+
+#include <px4_config.h>
+
+
+#include "board_config.h"
+#include <arch/board/board.h>
+#include <drivers/device/i2c.h>
+/****************************************************************************
+ * Name: board_i2c_initialize
+ *
+ * Description:
+ *   Called to set I2C bus frequncies.
+ *
+ ****************************************************************************/
+
+int board_i2c_initialize(void)
+{
+
+	int ret = device::I2C::set_bus_clock(PX4_I2C_BUS_ONBOARD, PX4_I2C_BUS_ONBOARD_HZ);
+
+	if (ret == OK) {
+		ret = device::I2C::set_bus_clock(PX4_I2C_BUS_EXPANSION, PX4_I2C_BUS_EXPANSION_HZ);
+	}
+
+	return ret;
+}

src/drivers/boards/crazyflie/crazyflie_init.c

@@ -0,0 +1,188 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 crazyflie_init.c
+ *
+ * Crazyflie specific early startup code.  This file implements the
+ * nsh_archinitialize() function that is called early by nsh during startup.
+ *
+ * Code here is run before the rcS script is invoked; it should start required
+ * subsystems and perform board-specific initialisation.
+ */
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <stdbool.h>
+#include <stdio.h>
+#include <debug.h>
+#include <errno.h>
+
+#include <nuttx/arch.h>
+#include <nuttx/spi.h>
+#include <nuttx/i2c.h>
+#include <nuttx/sdio.h>
+#include <nuttx/mmcsd.h>
+#include <nuttx/analog/adc.h>
+#include <nuttx/gran.h>
+
+#include <stm32.h>
+#include "board_config.h"
+#include <stm32_uart.h>
+
+#include <arch/board/board.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_led.h>
+
+#include <systemlib/cpuload.h>
+#include <systemlib/perf_counter.h>
+
+/****************************************************************************
+ * Pre-Processor Definitions
+ ****************************************************************************/
+
+/* Configuration ************************************************************/
+
+/* Debug ********************************************************************/
+
+#ifdef CONFIG_CPP_HAVE_VARARGS
+#  ifdef CONFIG_DEBUG
+#    define message(...) lowsyslog(__VA_ARGS__)
+#  else
+#    define message(...) printf(__VA_ARGS__)
+#  endif
+#else
+#  ifdef CONFIG_DEBUG
+#    define message lowsyslog
+#  else
+#    define message printf
+#  endif
+#endif
+
+/****************************************************************************
+ * Protected Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/************************************************************************************
+ * Name: stm32_boardinitialize
+ *
+ * Description:
+ *   All STM32 architectures must provide the following entry point.  This entry point
+ *   is called early in the intitialization -- after all memory has been configured
+ *   and mapped but before any devices have been initialized.
+ *
+ ************************************************************************************/
+
+__EXPORT void
+stm32_boardinitialize(void)
+{
+	/* configure LEDs */
+	up_ledinit();
+}
+
+/****************************************************************************
+ * Name: nsh_archinitialize
+ *
+ * Description:
+ *   Perform architecture specific initialization
+ *
+ ****************************************************************************/
+
+#include <math.h>
+
+#if 0
+#ifdef __cplusplus
+__EXPORT int matherr(struct __exception *e)
+{
+	return 1;
+}
+#else
+__EXPORT int matherr(struct exception *e)
+{
+	return 1;
+}
+#endif
+#endif
+
+__EXPORT int nsh_archinitialize(void)
+{
+
+	/* configure the high-resolution time/callout interface */
+	hrt_init();
+
+	/* configure CPU load estimation */
+#ifdef CONFIG_SCHED_INSTRUMENTATION
+	cpuload_initialize_once();
+#endif
+
+	/* set up the serial DMA polling */
+	static struct hrt_call serial_dma_call;
+	struct timespec ts;
+
+	/*
+	 * Poll at 1ms intervals for received bytes that have not triggered
+	 * a DMA event.
+	 */
+	ts.tv_sec = 0;
+	ts.tv_nsec = 1000000;
+
+	hrt_call_every(&serial_dma_call,
+		       ts_to_abstime(&ts),
+		       ts_to_abstime(&ts),
+		       (hrt_callout)stm32_serial_dma_poll,
+		       NULL);
+
+
+
+	result = board_i2c_initialize();
+
+//	if (result != OK) {
+//		up_ledon(LED_AMBER);
+//		return -ENODEV;
+//	}
+
+
+
+	// TODO: Initialize i2c buses right here
+
+	return OK;
+}

src/drivers/boards/crazyflie/crazyflie_led.c

@@ -0,0 +1,96 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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 crazyflie_led.c
+ *
+ * Crazyflie LED backend.
+ */
+
+#include <nuttx/config.h>
+
+#include <stdbool.h>
+
+#include "stm32.h"
+#include "board_config.h"
+
+#include <arch/board/board.h>
+
+/*
+ * Ideally we'd be able to get these from up_internal.h,
+ * but since we want to be able to disable the NuttX use
+ * of leds for system indication at will and there is no
+ * separate switch, we need to build independent of the
+ * CONFIG_ARCH_LEDS configuration switch.
+ */
+__BEGIN_DECLS
+extern void led_init(void);
+extern void led_on(int led);
+extern void led_off(int led);
+extern void led_toggle(int led);
+__END_DECLS
+
+__EXPORT void led_init()
+{
+	/* Configure LED1 GPIO for output */
+
+	stm32_configgpio(GPIO_LED1);
+}
+
+__EXPORT void led_on(int led)
+{
+	if (led == 1) {
+		/* Pull down to switch on */
+		stm32_gpiowrite(GPIO_LED1, false);
+	}
+}
+
+__EXPORT void led_off(int led)
+{
+	if (led == 1) {
+		/* Pull up to switch off */
+		stm32_gpiowrite(GPIO_LED1, true);
+	}
+}
+
+__EXPORT void led_toggle(int led)
+{
+	if (led == 1) {
+		if (stm32_gpioread(GPIO_LED1)) {
+			stm32_gpiowrite(GPIO_LED1, false);
+
+		} else {
+			stm32_gpiowrite(GPIO_LED1, true);
+		}
+	}
+}

src/drivers/boards/crazyflie/crazyflie_timer_config.c

@@ -0,0 +1,109 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2016 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 crazyflie_timer_config.c
+ *
+ * Configuration data for the stm32 pwm_servo driver.
+ *
+ * Note that these arrays must always be fully-sized.
+ */
+
+#include <stdint.h>
+
+#include <stm32.h>
+#include <stm32_gpio_out.h>
+#include <stm32_tim.h>
+
+#include <drivers/stm32/drv_io_timer.h>
+#include <drivers/drv_pwm_output.h>
+
+#include "board_config.h"
+
+__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
+		.first_channel_index = 0,
+		.last_channel_index = 2,
+		.handler = io_timer_handler0,
+		.vectorno =  STM32_IRQ_TIM2,
+	},
+	{
+		.base = STM32_TIM4_BASE,
+		.clock_register = STM32_RCC_APB1ENR,
+		.clock_bit = RCC_APB1ENR_TIM4EN,
+		.clock_freq = STM32_APB1_TIM4_CLKIN
+		.first_channel_index = 3,
+		.last_channel_index = 3,
+		.handler = io_timer_handler1,
+		.vectorno =  STM32_IRQ_TIM4,
+	}
+};
+
+__EXPORT const timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = {
+	{
+		.gpio_out = GPIO_TIM2_CH2OUT,
+		.gpio_in = GPIO_TIM2_CH2IN,
+		.timer_index = 0,
+		.timer_channel = 2,
+		.ccr_offset = STM32_GTIM_CCR2_OFFSET,
+		.masks  = GTIM_SR_CC2IF | GTIM_SR_CC2OF
+	},
+	{
+		.gpio_out = GPIO_TIM2_CH4OUT,
+		.gpio_in = GPIO_TIM2_CH4IN,
+		.timer_index = 0,
+		.timer_channel = 4,
+		.ccr_offset = STM32_GTIM_CCR4_OFFSET,
+		.masks  = GTIM_SR_CC4IF | GTIM_SR_CC4OF
+	},
+	{
+		.gpio_out = GPIO_TIM2_CH1OUT,
+		.gpio_in = GPIO_TIM2_CH1IN,
+		.timer_index = 0,
+		.timer_channel = 1,
+		.ccr_offset = STM32_GTIM_CCR1_OFFSET,
+		.masks  = GTIM_SR_CC1IF | GTIM_SR_CC1OF
+	},
+	{
+		.gpio_out = GPIO_TIM4_CH4OUT,
+		.gpio_in = GPIO_TIM4_CH4IN,
+		.timer_index = 1,
+		.timer_channel = 4,
+		.ccr_offset = STM32_GTIM_CCR4_OFFSET,
+		.masks  = GTIM_SR_CC4IF | GTIM_SR_CC4OF
+	}
+};

src/drivers/boards/crazyflie/crazyflie_usb.c

@@ -0,0 +1,108 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 crazyflie_usb.c
+ *
+ * Board-specific USB functions.
+ */
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <debug.h>
+
+#include <nuttx/usb/usbdev.h>
+#include <nuttx/usb/usbdev_trace.h>
+
+#include <up_arch.h>
+#include <stm32.h>
+#include "board_config.h"
+
+/************************************************************************************
+ * Definitions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Private Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Public Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Name: stm32_usbinitialize
+ *
+ * Description:
+ *   Called to setup USB-related GPIO pins for the PX4-STM32F4Discovery board.
+ *
+ ************************************************************************************/
+
+__EXPORT void stm32_usbinitialize(void)
+{
+	/* The OTG FS has an internal soft pull-up */
+
+	/* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */
+
+#ifdef CONFIG_STM32_OTGFS
+	stm32_configgpio(GPIO_OTGFS_VBUS);
+	/* XXX We only support device mode
+	stm32_configgpio(GPIO_OTGFS_PWRON);
+	stm32_configgpio(GPIO_OTGFS_OVER);
+	*/
+#endif
+}
+
+/************************************************************************************
+ * Name:  stm32_usbsuspend
+ *
+ * Description:
+ *   Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is
+ *   used.  This function is called whenever the USB enters or leaves suspend mode.
+ *   This is an opportunity for the board logic to shutdown clocks, power, etc.
+ *   while the USB is suspended.
+ *
+ ************************************************************************************/
+
+__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume)
+{
+	ulldbg("resume: %d\n", resume);
+}
+

src/drivers/crazyflie/CMakeLists.txt

@@ -0,0 +1,44 @@
+############################################################################
+#
+#   Copyright (c) 2015 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.
+#
+############################################################################
+px4_add_module(
+	MODULE drivers__crazyflie
+	MAIN crazyflie
+	STACK 1200
+	COMPILE_FLAGS
+		-Os
+	SRCS
+		crazyflie.cpp
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 

src/drivers/drv_gpio.h

@@ -159,6 +159,10 @@
 /* no GPIO driver on the ASC board */
 #endif
 
+#ifdef CONFIG_ARCH_BOARD_CRAZYFLIE
+/* no GPIO driver on the CRAZYFLIE board */
+#endif
+
 #ifdef CONFIG_ARCH_BOARD_SITL
 /* no GPIO driver on the SITL configuration */
 #endif
@@ -168,7 +172,8 @@
 	!defined(CONFIG_ARCH_BOARD_AEROCORE) && !defined(CONFIG_ARCH_BOARD_PX4_STM32F4DISCOVERY) && \
 	!defined(CONFIG_ARCH_BOARD_MINDPX_V2) && \
 	!defined(CONFIG_ARCH_BOARD_PX4FMU_V4) && !defined(CONFIG_ARCH_BOARD_SITL) && \
-	!defined(CONFIG_ARCH_BOARD_TAP_V1) && !defined(CONFIG_ARCH_BOARD_ASC_V1)
+	!defined(CONFIG_ARCH_BOARD_TAP_V1) && !defined(CONFIG_ARCH_BOARD_ASC_V1) && \
+	!defined(CONFIG_ARCH_BOARD_CRAZYFLIE)
 # error No CONFIG_ARCH_BOARD_xxxx set
 #endif
 /*

src/drivers/drv_sensor.h

@@ -136,4 +136,3 @@
 #define SENSORIOCCALTEST	_SENSORIOC(7)
 
 #endif /* _DRV_SENSOR_H */
-

src/drivers/lps25h/CMakeLists.txt

@@ -0,0 +1,48 @@
+############################################################################
+#
+#   Copyright (c) 2015 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.
+#
+############################################################################
+px4_add_module(
+	MODULE drivers__lps25h
+	MAIN lps25h
+	STACK 1200
+	COMPILE_FLAGS
+		-Weffc++
+		-Os
+	SRCS
+		lps25h.cpp
+		lps25h_i2c.cpp
+		lps25h_spi.cpp
+
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 

src/drivers/lps25h/lps25h.h

@@ -0,0 +1,49 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 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 lps25h.h
+ *
+ * Shared defines for the lps25h driver.
+ */
+
+#pragma once
+
+#define ADDR_WHO_AM_I		0x0F
+
+#define ID_WHO_AM_I		0xBD
+
+/* interface factories */
+extern device::Device *LPS25H_SPI_interface(int bus);
+extern device::Device *LPS25H_I2C_interface(int bus);
+typedef device::Device *(*LPS25H_constructor)(int);

src/drivers/lps25h/lps25h_i2c.cpp

@@ -0,0 +1,179 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2015 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 LPS25H_I2C.cpp
+ *
+ * I2C interface for LPS25H
+ */
+
+/* XXX trim includes */
+#include <px4_config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <string.h>
+#include <assert.h>
+#include <debug.h>
+#include <errno.h>
+#include <unistd.h>
+
+#include <arch/board/board.h>
+
+#include <drivers/device/i2c.h>
+#include <drivers/drv_mag.h>
+#include <drivers/drv_device.h>
+
+#include "lps25h.h"
+
+#include "board_config.h"
+
+#define LPS25H_ADDRESS		0x5D
+
+device::Device *LPS25H_I2C_interface(int bus);
+
+class LPS25H_I2C : public device::I2C
+{
+public:
+	LPS25H_I2C(int bus);
+	virtual ~LPS25H_I2C();
+
+	virtual int	init();
+	virtual int	read(unsigned address, void *data, unsigned count);
+	virtual int	write(unsigned address, void *data, unsigned count);
+
+	virtual int	ioctl(unsigned operation, unsigned &arg);
+
+protected:
+	virtual int	probe();
+
+};
+
+device::Device *
+LPS25H_I2C_interface(int bus)
+{
+	return new LPS25H_I2C(bus);
+}
+
+LPS25H_I2C::LPS25H_I2C(int bus) :
+	I2C("LPS25H_I2C", nullptr, bus, LPS25H_ADDRESS, 400000)
+{
+}
+
+LPS25H_I2C::~LPS25H_I2C()
+{
+}
+
+int
+LPS25H_I2C::init()
+{
+	/* this will call probe() */
+	return I2C::init();
+}
+
+int
+LPS25H_I2C::ioctl(unsigned operation, unsigned &arg)
+{
+	int ret;
+
+	switch (operation) {
+
+	case MAGIOCGEXTERNAL:
+// On PX4v1 the MAG can be on an internal I2C
+// On everything else its always external
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+		if (_bus == PX4_I2C_BUS_EXPANSION) {
+			return 1;
+
+		} else {
+			return 0;
+		}
+
+#else
+		return 1;
+#endif
+
+	case DEVIOCGDEVICEID:
+		return CDev::ioctl(nullptr, operation, arg);
+
+	default:
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+int
+LPS25H_I2C::probe()
+{
+	uint8_t id;
+
+	_retries = 10;
+
+	if (read(ADDR_WHO_AM_I, &id, 1)) {
+		DEVICE_DEBUG("read_reg fail");
+		return -EIO;
+	}
+
+	_retries = 2;
+
+	if (id != ID_WHO_AM_I) {
+		DEVICE_DEBUG("ID byte mismatch (%02x != %02x)", ID_WHO_AM_I, id);
+		return -EIO;
+	}
+
+	return OK;
+}
+
+int
+LPS25H_I2C::write(unsigned address, void *data, unsigned count)
+{
+	uint8_t buf[32];
+
+	if (sizeof(buf) < (count + 1)) {
+		return -EIO;
+	}
+
+	buf[0] = address;
+	memcpy(&buf[1], data, count);
+
+	return transfer(&buf[0], count + 1, nullptr, 0);
+}
+
+int
+LPS25H_I2C::read(unsigned address, void *data, unsigned count)
+{
+	uint8_t cmd = address;
+	return transfer(&cmd, 1, (uint8_t *)data, count);
+}

src/drivers/lps25h/lps25h_spi.cpp

@@ -0,0 +1,188 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2015 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 LPS25H_SPI.cpp
+ *
+ * SPI interface for LPS25H
+ */
+
+/* XXX trim includes */
+#include <px4_config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <string.h>
+#include <assert.h>
+#include <debug.h>
+#include <errno.h>
+#include <unistd.h>
+
+#include <arch/board/board.h>
+
+#include <drivers/device/spi.h>
+#include <drivers/drv_mag.h>
+#include <drivers/drv_device.h>
+
+#include "lps25h.h"
+#include <board_config.h>
+
+#ifdef PX4_SPIDEV_HMC
+
+/* SPI protocol address bits */
+#define DIR_READ			(1<<7)
+#define DIR_WRITE			(0<<7)
+#define ADDR_INCREMENT			(1<<6)
+
+#define HMC_MAX_SEND_LEN		4
+#define HMC_MAX_RCV_LEN			8
+
+device::Device *LPS25H_SPI_interface(int bus);
+
+class LPS25H_SPI : public device::SPI
+{
+public:
+	LPS25H_SPI(int bus, spi_dev_e device);
+	virtual ~LPS25H_SPI();
+
+	virtual int	init();
+	virtual int	read(unsigned address, void *data, unsigned count);
+	virtual int	write(unsigned address, void *data, unsigned count);
+
+	virtual int	ioctl(unsigned operation, unsigned &arg);
+
+};
+
+device::Device *
+LPS25H_SPI_interface(int bus)
+{
+	return new LPS25H_SPI(bus, (spi_dev_e)PX4_SPIDEV_HMC);
+}
+
+LPS25H_SPI::LPS25H_SPI(int bus, spi_dev_e device) :
+	SPI("LPS25H_SPI", nullptr, bus, device, SPIDEV_MODE3, 11 * 1000 * 1000 /* will be rounded to 10.4 MHz */)
+{
+	_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_LPS25H;
+}
+
+LPS25H_SPI::~LPS25H_SPI()
+{
+}
+
+int
+LPS25H_SPI::init()
+{
+	int ret;
+
+	ret = SPI::init();
+
+	if (ret != OK) {
+		DEVICE_DEBUG("SPI init failed");
+		return -EIO;
+	}
+
+	// read WHO_AM_I value
+	uint8_t id;
+
+	if (read(ADDR_ID, &id, 1)) {
+		DEVICE_DEBUG("read_reg fail");
+		return -EIO;
+	}
+
+	if (id != ID_WHO_AM_I) {
+		DEVICE_DEBUG("ID byte mismatch (%02x != %02x)", ID_WHO_AM_I, id);
+		return -EIO;
+	}
+
+	return OK;
+}
+
+int
+LPS25H_SPI::ioctl(unsigned operation, unsigned &arg)
+{
+	int ret;
+
+	switch (operation) {
+
+	case MAGIOCGEXTERNAL:
+		/*
+		 * Even if this sensor is on the external SPI
+		 * bus it is still internal to the autopilot
+		 * assembly, so always return 0 for internal.
+		 */
+		return 0;
+
+	case DEVIOCGDEVICEID:
+		return CDev::ioctl(nullptr, operation, arg);
+
+	default: {
+			ret = -EINVAL;
+		}
+	}
+
+	return ret;
+}
+
+int
+LPS25H_SPI::write(unsigned address, void *data, unsigned count)
+{
+	uint8_t buf[32];
+
+	if (sizeof(buf) < (count + 1)) {
+		return -EIO;
+	}
+
+	buf[0] = address | DIR_WRITE;
+	memcpy(&buf[1], data, count);
+
+	return transfer(&buf[0], &buf[0], count + 1);
+}
+
+int
+LPS25H_SPI::read(unsigned address, void *data, unsigned count)
+{
+	uint8_t buf[32];
+
+	if (sizeof(buf) < (count + 1)) {
+		return -EIO;
+	}
+
+	buf[0] = address | DIR_READ | ADDR_INCREMENT;
+
+	int ret = transfer(&buf[0], &buf[0], count + 1);
+	memcpy(data, &buf[1], count);
+	return ret;
+}
+
+#endif /* PX4_SPIDEV_HMC */

src/drivers/mpu9250/CMakeLists.txt

@@ -39,10 +39,12 @@ px4_add_module(
 		-Os
 	SRCS
 		mpu9250.cpp
+		mpu9250_i2c.cpp
+		mpu9250_spi.cpp
 		main.cpp
 		gyro.cpp
 		mag.cpp
 	DEPENDS
 		platforms__common
 	)
-# vim: set noet ft=cmake fenc=utf-8 ff=unix : 
+# vim: set noet ft=cmake fenc=utf-8 ff=unix :

src/drivers/mpu9250/main.cpp

@@ -89,61 +89,111 @@ extern "C" { __EXPORT int mpu9250_main(int argc, char *argv[]); }
 namespace mpu9250
 {
 
-MPU9250	*g_dev_int; // on internal bus
-MPU9250	*g_dev_ext; // on external bus
+enum MPU9250_BUS {
+	MPU9250_BUS_ALL = 0,
+	MPU9250_BUS_I2C_INTERNAL,
+	MPU9250_BUS_I2C_EXTERNAL,
+	MPU9250_BUS_SPI_INTERNAL,
+	MPU9250_BUS_SPI_EXTERNAL
+};
 
-void	start(bool, enum Rotation);
-void	stop(bool);
-void	test(bool);
-void	reset(bool);
-void	info(bool);
-void	regdump(bool);
-void	testerror(bool);
+/*
+  list of supported bus configurations
+ */
+
+struct mpu9250_bus_option {
+	enum MPU9250_BUS busid;
+	const char *accelpath;
+	const char *gyropath;
+	const char *magpath;
+	MPU9250_constructor interface_constructor;
+	uint8_t busnum;
+	MPU6000	*dev;
+} bus_options[] = {
+#if defined (USE_I2C)
+#  if defined(PX4_I2C_BUS_ONBOARD)
+	{ MPU9250_BUS_I2C_INTERNAL, MPU_DEVICE_PATH_ACCEL, MPU_DEVICE_PATH_GYRO, MPU_DEVICE_PATH_MAG,  &MPU9250_I2C_interface, PX4_I2C_BUS_ONBOARD, NULL },
+#  endif
+#  if defined(PX4_I2C_BUS_EXPANSION)
+	{ MPU9250_BUS_I2C_EXTERNAL, MPU_DEVICE_PATH_ACCEL_EXT, MPU_DEVICE_PATH_GYRO_EXT, MPU_DEVICE_PATH_MAG_EXT, &MPU9250_I2C_interface, PX4_I2C_BUS_EXPANSION, NULL },
+#  endif
+#endif
+#ifdef PX4_SPIDEV_MPU
+	{ MPU9250_BUS_SPI_INTERNAL, MPU_DEVICE_PATH_ACCEL, MPU_DEVICE_PATH_GYRO, MPU_DEVICE_PATH_MAG, &MPU9250_SPI_interface, PX4_SPI_BUS_SENSORS, NULL },
+#endif
+#if defined(PX4_SPI_BUS_EXT)
+	{ MPU9250_BUS_SPI_EXTERNAL, MPU_DEVICE_PATH_ACCEL_EXT, MPU_DEVICE_PATH_GYRO_EXT, MPU_DEVICE_PATH_MAG_EXT &MPU9250_SPI_interface, PX4_SPI_BUS_EXT, NULL },
+#endif
+};
+
+#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0]))
+
+
+void	start(enum MPU9250_BUS busid, enum Rotation rotation);
+bool	start_bus(struct mpu9250_bus_option &bus, enum Rotation rotation);
+struct mpu9250_bus_option &find_bus(enum MPU9250_BUS busid);
+void	stop(enum MPU9250_BUS busid);
+void	test(enum MPU9250_BUS busid);
+void	reset(enum MPU9250_BUS busid);
+void	info(enum MPU9250_BUS busid);
+void	regdump(enum MPU9250_BUS busid);
+void	testerror(enum MPU9250_BUS busid);
 void	usage();
 
 /**
- * Start the driver.
- *
- * This function only returns if the driver is up and running
- * or failed to detect the sensor.
+ * find a bus structure for a busid
  */
-void
-start(bool external_bus, enum Rotation rotation)
+struct mpu9250_bus_option &find_bus(enum MPU9250_BUS busid)
 {
-	int fd;
-	MPU9250 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int;
-	const char *path_accel = external_bus ? MPU_DEVICE_PATH_ACCEL_EXT : MPU_DEVICE_PATH_ACCEL;
-	const char *path_gyro  = external_bus ? MPU_DEVICE_PATH_GYRO_EXT : MPU_DEVICE_PATH_GYRO;
-	const char *path_mag   = external_bus ? MPU_DEVICE_PATH_MAG_EXT : MPU_DEVICE_PATH_MAG;
-
-	if (*g_dev_ptr != nullptr)
-		/* if already started, the still command succeeded */
-	{
-		errx(0, "already started");
+	for (uint8_t i = 0; i < NUM_BUS_OPTIONS; i++) {
+		if ((busid == MPU9250_BUS_ALL ||
+		     busid == bus_options[i].busid) && bus_options[i].dev != NULL) {
+			return bus_options[i];
+		}
 	}
 
-	/* create the driver */
-	if (external_bus) {
-#ifdef PX4_SPI_BUS_EXT
-		*g_dev_ptr = new MPU9250(PX4_SPI_BUS_EXT, path_accel, path_gyro, path_mag, (spi_dev_e)PX4_SPIDEV_EXT_MPU, rotation);
-#else
-		errx(0, "External SPI not available");
-#endif
+	errx(1, "bus %u not started", (unsigned)busid);
+}
 
-	} else {
-		*g_dev_ptr = new MPU9250(PX4_SPI_BUS_SENSORS, path_accel, path_gyro, path_mag, (spi_dev_e)PX4_SPIDEV_MPU, rotation);
+/**
+ * start driver for a specific bus option
+ */
+bool
+start_bus(struct mpu9250_bus_option &bus, enum Rotation rotation, int range, int device_type, bool external)
+{
+	int fd = -1;
+
+	if (bus.dev != nullptr) {
+		warnx("%s SPI not available", external ? "External" : "Internal");
+		return false;
 	}
 
-	if (*g_dev_ptr == nullptr) {
-		goto fail;
+	device::Device *interface = bus.interface_constructor(bus.busnum, device_type, external);
+
+	if (interface == nullptr) {
+		warnx("no device on bus %u", (unsigned)bus.busid);
+		return false;
 	}
 
-	if (OK != (*g_dev_ptr)->init()) {
+	if (interface->init() != OK) {
+		delete interface;
+		warnx("no device on bus %u", (unsigned)bus.busid);
+		return false;
+	}
+
+	bus.dev = new MPU9250(interface, bus.accelpath, bus.gyropath, bus.magpath, rotation, device_type);
+
+	if (bus.dev == nullptr) {
+		delete interface;
+		return false;
+	}
+
+	if (OK != bus.dev->init()) {
 		goto fail;
 	}
 
 	/* set the poll rate to default, starts automatic data collection */
-	fd = open(path_accel, O_RDONLY);
+	fd = open(bus.accelpath, O_RDONLY);
 
 	if (fd < 0) {
 		goto fail;
@@ -153,27 +203,67 @@ start(bool external_bus, enum Rotation rotation)
 		goto fail;
 	}
 
+	if (ioctl(fd, ACCELIOCSRANGE, range) < 0) {
+		goto fail;
+	}
+
 	close(fd);
 
-	exit(0);
+	return true;
+
 fail:
 
-	if (*g_dev_ptr != nullptr) {
-		delete(*g_dev_ptr);
-		*g_dev_ptr = nullptr;
+	if (fd >= 0) {
+		close(fd);
+	}
+
+	if (bus.dev != nullptr) {
+		delete(bus.dev);
+		bus.dev = nullptr;
 	}
 
 	errx(1, "driver start failed");
 }
 
+/**
+ * Start the driver.
+ *
+ * This function only returns if the driver is up and running
+ * or failed to detect the sensor.
+ */
 void
-stop(bool external_bus)
+start(enum MPU9250_BUS busid, enum Rotation rotation, int range, int device_type, bool external)
 {
-	MPU9250 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int;
 
-	if (*g_dev_ptr != nullptr) {
-		delete *g_dev_ptr;
-		*g_dev_ptr = nullptr;
+	bool started = false;
+
+	for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) {
+		if (busid == MPU9250_BUS_ALL && bus_options[i].dev != NULL) {
+			// this device is already started
+			continue;
+		}
+
+		if (busid != MPU9250_BUS_ALL && bus_options[i].busid != busid) {
+			// not the one that is asked for
+			continue;
+		}
+
+		started |= start_bus(bus_options[i], rotation, range, device_type, external);
+	}
+
+	exit(started ? 0 : 1);
+
+}
+
+void
+stop(enum MPU9250_BUS busid)
+{
+	struct mpu9250_bus_option &bus = find_bus(busid);
+
+
+	if (bus.dev != nullptr) {
+		delete bus.dev;
+		bus.dev = nullptr;
 
 	} else {
 		/* warn, but not an error */
@@ -189,35 +279,33 @@ stop(bool external_bus)
  * and automatic modes.
  */
 void
-test(bool external_bus)
+test(enum MPU9250_BUS busid)
 {
-	const char *path_accel = external_bus ? MPU_DEVICE_PATH_ACCEL_EXT : MPU_DEVICE_PATH_ACCEL;
-	const char *path_gyro  = external_bus ? MPU_DEVICE_PATH_GYRO_EXT : MPU_DEVICE_PATH_GYRO;
-	const char *path_mag   = external_bus ? MPU_DEVICE_PATH_MAG_EXT : MPU_DEVICE_PATH_MAG;
+	struct mpu9250_bus_option &bus = find_bus(busid);
 	accel_report a_report;
 	gyro_report g_report;
 	mag_report m_report;
 	ssize_t sz;
 
 	/* get the driver */
-	int fd = open(path_accel, O_RDONLY);
+	int fd = open(bus.accelpath, O_RDONLY);
 
 	if (fd < 0) {
-		err(1, "%s open failed (try 'm start')", path_accel);
+		err(1, "%s open failed (try 'm start')", bus.accelpath);
 	}
 
 	/* get the driver */
-	int fd_gyro = open(path_gyro, O_RDONLY);
+	int fd_gyro = open(bus.gyropath, O_RDONLY);
 
 	if (fd_gyro < 0) {
-		err(1, "%s open failed", path_gyro);
+		err(1, "%s open failed", bus.gyropath);
 	}
 
 	/* get the driver */
-	int fd_mag = open(path_mag, O_RDONLY);
+	int fd_mag = open(bus.magpath, O_RDONLY);
 
 	if (fd_mag < 0) {
-		err(1, "%s open failed", path_mag);
+		err(1, "%s open failed", bus.magpath);
 	}
 
 	/* reset to manual polling */
@@ -292,7 +380,7 @@ test(bool external_bus)
 
 	/* XXX add poll-rate tests here too */
 
-	reset(external_bus);
+	reset(busid);
 	errx(0, "PASS");
 }
 
@@ -300,10 +388,10 @@ test(bool external_bus)
  * Reset the driver.
  */
 void
-reset(bool external_bus)
+reset(enum MPU9250_BUS busid)
 {
-	const char *path_accel = external_bus ? MPU_DEVICE_PATH_ACCEL_EXT : MPU_DEVICE_PATH_ACCEL;
-	int fd = open(path_accel, O_RDONLY);
+	struct mpu9250_bus_option &bus = find_bus(busid);
+	int fd = open(bus.accelpath, O_RDONLY);
 
 	if (fd < 0) {
 		err(1, "failed ");
@@ -326,15 +414,17 @@ reset(bool external_bus)
  * Print a little info about the driver.
  */
 void
-info(bool external_bus)
+info(enum MPU9250_BUS busid)
 {
-	MPU9250 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int;
+	struct mpu9250_bus_option &bus = find_bus(busid);
 
-	if (*g_dev_ptr == nullptr) {
+
+	if (bus.dev == nullptr) {
 		errx(1, "driver not running");
 	}
 
-	(*g_dev_ptr)->print_info();
+	printf("state @ %p\n", bus.dev);
+	bus.dev->print_info();
 
 	exit(0);
 }
@@ -343,15 +433,17 @@ info(bool external_bus)
  * Dump the register information
  */
 void
-regdump(bool external_bus)
+regdump(enum MPU9250_BUS busid)
 {
-	MPU9250 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int;
+	struct mpu9250_bus_option &bus = find_bus(busid);
 
-	if (*g_dev_ptr == nullptr) {
+
+	if (bus.dev == nullptr) {
 		errx(1, "driver not running");
 	}
 
-	(*g_dev_ptr)->print_registers();
+	printf("regdump @ %p\n", bus.dev);
+	bus.dev->print_registers();
 
 	exit(0);
 }
@@ -360,15 +452,16 @@ regdump(bool external_bus)
  * deliberately produce an error to test recovery
  */
 void
-testerror(bool external_bus)
+testerror(enum MPU9250_BUS busid)
 {
-	MPU9250 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int;
+	struct mpu9250_bus_option &bus = find_bus(busid);
 
-	if (*g_dev_ptr == nullptr) {
+
+	if (bus.dev == nullptr) {
 		errx(1, "driver not running");
 	}
 
-	(*g_dev_ptr)->test_error();
+	bus.dev->test_error();
 
 	exit(0);
 }
@@ -380,6 +473,7 @@ usage()
 	warnx("options:");
 	warnx("    -X    (external bus)");
 	warnx("    -R rotation");
+	warnx("    -a accel range (in g)");
 }
 
 } // namespace
@@ -387,70 +481,91 @@ usage()
 int
 mpu9250_main(int argc, char *argv[])
 {
-	bool external_bus = false;
+	enum MPU9250_BUS busid = MPU9250_BUS_ALL;
 	int ch;
+	bool external = false;
 	enum Rotation rotation = ROTATION_NONE;
+	int accel_range = 8;
 
 	/* jump over start/off/etc and look at options first */
-	while ((ch = getopt(argc, argv, "XR:")) != EOF) {
+	while ((ch = getopt(argc, argv, "XISsR:a:")) != EOF) {
 		switch (ch) {
 		case 'X':
-			external_bus = true;
+			busid = MPU9260_BUS_I2C_EXTERNAL;
+			break;
+
+		case 'I':
+			busid = MPU9250_BUS_I2C_INTERNAL;
+			break;
+
+		case 'S':
+			busid = MPU9250_BUS_SPI_EXTERNAL;
+			break;
+
+		case 's':
+			busid = MPU9260_BUS_SPI_INTERNAL;
 			break;
 
 		case 'R':
 			rotation = (enum Rotation)atoi(optarg);
 			break;
 
+		case 'a':
+			accel_range = atoi(optarg);
+			break;
+
 		default:
 			mpu9250::usage();
 			exit(0);
 		}
 	}
 
+	external = (busid == MPU9250_BUS_I2C_EXTERNAL || busid == MPU9250_BUS_SPI_EXTERNAL);
+
 	const char *verb = argv[optind];
 
 	/*
 	 * Start/load the driver.
+
 	 */
 	if (!strcmp(verb, "start")) {
-		mpu9250::start(external_bus, rotation);
+		mpu9250::start(busid, rotation, accel_range, device_type, external);
 	}
 
 	if (!strcmp(verb, "stop")) {
-		mpu9250::stop(external_bus);
+		mpu9250::stop(busid);
 	}
 
 	/*
 	 * Test the driver/device.
 	 */
 	if (!strcmp(verb, "test")) {
-		mpu9250::test(external_bus);
+		mpu9250::test(busid);
 	}
 
 	/*
 	 * Reset the driver.
 	 */
 	if (!strcmp(verb, "reset")) {
-		mpu9250::reset(external_bus);
+		mpu9250::reset(busid);
 	}
 
 	/*
 	 * Print driver information.
 	 */
 	if (!strcmp(verb, "info")) {
-		mpu9250::info(external_bus);
+		mpu9250::info(busid);
 	}
 
 	/*
 	 * Print register information.
 	 */
 	if (!strcmp(verb, "regdump")) {
-		mpu9250::regdump(external_bus);
+		mpu9250::regdump(busid);
 	}
 
 	if (!strcmp(verb, "testerror")) {
-		mpu9250::testerror(external_bus);
+		mpu9250::testerror(busid);
 	}
 
 	mpu9250::usage();

src/drivers/mpu9250/mpu9250.cpp

@@ -34,7 +34,7 @@
 /**
  * @file mpu9250.cpp
  *
- * Driver for the Invensense MPU9250 connected via SPI.
+ * Driver for the Invensense MPU9250 connected via I2C or SPI.
  *
  * @author Andrew Tridgell
  *
@@ -215,9 +215,12 @@ const uint8_t MPU9250::_checked_registers[MPU9250_NUM_CHECKED_REGISTERS] = { MPU
 									   };
 
 
-MPU9250::MPU9250(int bus, const char *path_accel, const char *path_gyro, const char *path_mag, spi_dev_e device,
+MPU9250::MPU9250(device::Device *interface, const char *path_accel, const char *path_gyro, const char *path_mag,
 		 enum Rotation rotation) :
-	SPI("MPU9250", path_accel, bus, device, SPIDEV_MODE3, MPU9250_LOW_BUS_SPEED),
+	CDev("MPU9250", path_accel),
+	_interface(interface),
+	_interface_bus(interface_bus),
+	//SPI("MPU9250", path_accel, bus, device, SPIDEV_MODE3, MPU9250_LOW_BUS_SPEED),
 	_gyro(new MPU9250_gyro(this, path_gyro)),
 	_mag(new MPU9250_mag(this, path_mag)),
 	_whoami(0),
@@ -333,15 +336,28 @@ MPU9250::init()
 {
 	int ret;
 
-	/* do SPI init (and probe) first */
-	ret = SPI::init();
 
-	/* if probe/setup failed, bail now */
+
+	/* do init */
+
+	ret = CDev::init();
+
+	/* if init failed, bail now */
 	if (ret != OK) {
-		DEVICE_DEBUG("SPI setup failed");
+		DEVICE_DEBUG("CDev init failed");
 		return ret;
 	}
 
+
+//	/* do SPI init (and probe) first */
+//	ret = SPI::init();
+
+//	/* if probe/setup failed, bail now */
+//	if (ret != OK) {
+//		DEVICE_DEBUG("SPI setup failed");
+//		return ret;
+//	}
+
 	ret = probe();
 
 	if (ret != OK) {
@@ -742,7 +758,8 @@ MPU9250::test_error()
 	// development as a handy way to test the reset logic
 	uint8_t data[16];
 	memset(data, 0, sizeof(data));
-	transfer(data, data, sizeof(data));
+	_interface->read(MPU6000_SET_SPEED(MPUREG_INT_STATUS, MPU6000_LOW_BUS_SPEED), data, sizeof(data));
+	//transfer(data, data, sizeof(data));
 	::printf("error triggered\n");
 	print_registers();
 }
@@ -1047,13 +1064,22 @@ MPU9250::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
 
 	default:
 		/* give it to the superclass */
-		return SPI::ioctl(filp, cmd, arg);
+		return CDev::ioctl(filp, cmd, arg);
 	}
 }
 
 uint8_t
 MPU9250::read_reg(unsigned reg, uint32_t speed)
 {
+
+	// From MPU6000 implementation
+	uint8_t buf;
+	_interface->read(MPU6000_SET_SPEED(reg, speed), &buf, 1);
+	return buf;
+
+
+
+
 	uint8_t cmd[2] = { (uint8_t)(reg | DIR_READ), 0};
 
 	// general register transfer at low clock speed
@@ -1067,6 +1093,16 @@ MPU9250::read_reg(unsigned reg, uint32_t speed)
 uint16_t
 MPU9250::read_reg16(unsigned reg)
 {
+	uint8_t buf[2];
+
+	// general register transfer at low clock speed
+
+	_interface->read(MPU9250_LOW_SPEED_OP(reg), &buf, arraySize(buf));
+	return (uint16_t)(buf[0] << 8) | buf[1];
+
+
+
+
 	uint8_t cmd[3] = { (uint8_t)(reg | DIR_READ), 0, 0 };
 
 	// general register transfer at low clock speed
@@ -1080,6 +1116,12 @@ MPU9250::read_reg16(unsigned reg)
 void
 MPU9250::write_reg(unsigned reg, uint8_t value)
 {
+	// general register transfer at low clock speed
+
+	return _interface->write(MPU9250_LOW_SPEED_OP(reg), &value, 1);
+
+
+
 	uint8_t	cmd[2];
 
 	cmd[0] = reg | DIR_WRITE;
@@ -1313,10 +1355,9 @@ MPU9250::measure()
 	 */
 	mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS;
 
-	// sensor transfer at high clock speed
-	set_frequency(MPU9250_HIGH_BUS_SPEED);
-
-	if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report))) {
+	if (sizeof(mpu_report) != _interface->read(MPU9250_SET_SPEED(MPUREG_INT_STATUS, MPU9250_HIGH_BUS_SPEED),
+			(uint8_t *)&mpu_report,
+			sizeof(mpu_report))) {
 		return;
 	}
 

src/drivers/mpu9250/mpu9250.h

@@ -1,26 +1,13 @@
-#ifdef PX4_SPI_BUS_EXT
-#define EXTERNAL_BUS PX4_SPI_BUS_EXT
-#else
-#define EXTERNAL_BUS 0
-#endif
-
-/*
-  the MPU9250 can only handle high SPI bus speeds on the sensor and
-  interrupt status registers. All other registers have a maximum 1MHz
-  SPI speed
- */
-#define MPU9250_LOW_BUS_SPEED				1000*1000
-#define MPU9250_HIGH_BUS_SPEED				11*1000*1000
 
 #define MPU9250_ONE_G					9.80665f
 
 class MPU9250_mag;
 class MPU9250_gyro;
 
-class MPU9250 : public device::SPI
+class MPU9250 : public device::CDev
 {
 public:
-	MPU9250(int bus, const char *path_accel, const char *path_gyro, const char *path_mag, spi_dev_e device,
+	MPU9250(device::Device *interface, const char *path_accel, const char *path_gyro, const char *path_mag,
 		enum Rotation rotation);
 	virtual ~MPU9250();
 
@@ -40,6 +27,8 @@ public:
 	void 			test_error();
 
 protected:
+	Device			*_interface;
+
 	virtual int		probe();
 
 	friend class MPU9250_mag;
@@ -265,3 +254,28 @@ private:
 	};
 #pragma pack(pop)
 };
+
+
+
+/*
+  The MPU9250 can only handle high bus speeds on the sensor and
+  interrupt status registers. All other registers have a maximum 1MHz
+  Communication with all registers of the device is performed using either
+  I2C at 400kHz or SPI at 1MHz. For applications requiring faster communications,
+  the sensor and interrupt registers may be read using SPI at 20MHz
+ */
+#define MPU9250_LOW_BUS_SPEED				0
+#define MPU9250_HIGH_BUS_SPEED				0x8000
+#  define MPU9250_IS_HIGH_SPEED(r) 			((r) & MPU9250_HIGH_BUS_SPEED)
+#  define MPU9250_REG(r) 					((r) &~MPU9250_HIGH_BUS_SPEED)
+#  define MPU9250_SET_SPEED(r, s) 			((r)|(s))
+#  define MPU9250_HIGH_SPEED_OP(r) 			MPU9250_SET_SPEED((r), MPU9250_HIGH_BUS_SPEED)
+#  define MPU9250_LOW_SPEED_OP(r)			MPU9250_REG((r))
+
+
+/* interface factories */
+extern device::Device *MPU9250_SPI_interface(int bus, int device_type, bool external_bus);
+extern device::Device *MPU9250_I2C_interface(int bus, int device_type, bool external_bus);
+extern int MPU9250_probe(device::Device *dev, int device_type);
+
+typedef device::Device *(*MPU9250_constructor)(int, int, bool);

src/drivers/mpu9250/mpu9250_i2c.cpp

@@ -0,0 +1,172 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2016 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 mpu9250_i2c.cpp
+ *
+ * I2C interface for MPU9250
+ */
+
+/* XXX trim includes */
+#include <px4_config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <string.h>
+#include <assert.h>
+#include <debug.h>
+#include <errno.h>
+#include <unistd.h>
+
+#include <arch/board/board.h>
+
+#include <drivers/device/i2c.h>
+#include <drivers/drv_accel.h>
+#include <drivers/drv_device.h>
+
+#include "MPU9250.h"
+
+#include "board_config.h"
+
+#ifdef USE_I2C
+
+device::Device *MPU9250_I2C_interface(int bus, int device_type, bool external_bus);
+
+class MPU9250_I2C : public device::I2C
+{
+public:
+	MPU9250_I2C(int bus, int device_type);
+	virtual ~MPU9250_I2C();
+
+	virtual int	init();
+	virtual int	read(unsigned address, void *data, unsigned count);
+	virtual int	write(unsigned address, void *data, unsigned count);
+
+	virtual int	ioctl(unsigned operation, unsigned &arg);
+
+protected:
+	virtual int	probe();
+
+private:
+	int _device_type;
+
+};
+
+
+device::Device *
+MPU9250_I2C_interface(int bus, int device_type, bool external_bus)
+{
+	return new MPU9250_I2C(bus, device_type);
+}
+
+MPU9250_I2C::MPU9250_I2C(int bus, int device_type) :
+	I2C("MPU9250_I2C", nullptr, bus, PX4_I2C_OBDEV_MPU9250, 400000),
+	_device_type(device_type)
+{
+	_device_id.devid_s.devtype =  DRV_ACC_DEVTYPE_MPU9250;
+}
+
+MPU9250_I2C::~MPU9250_I2C()
+{
+}
+
+int
+MPU9250_I2C::init()
+{
+	/* this will call probe() */
+	return I2C::init();
+}
+
+int
+MPU9250_I2C::ioctl(unsigned operation, unsigned &arg)
+{
+	int ret;
+
+	switch (operation) {
+
+	case ACCELIOCGEXTERNAL:
+		return _bus == PX4_I2C_BUS_EXPANSION ? 1 : 0;
+
+	case DEVIOCGDEVICEID:
+		return CDev::ioctl(nullptr, operation, arg);
+
+	case MPUIOCGIS_I2C:
+		return 1;
+
+	default:
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+int
+MPU9250_I2C::write(unsigned reg_speed, void *data, unsigned count)
+{
+	uint8_t cmd[MPU_MAX_WRITE_BUFFER_SIZE];
+
+	if (sizeof(cmd) < (count + 1)) {
+		return -EIO;
+	}
+
+	cmd[0] = MPU9250_REG(reg_speed);
+	cmd[1] = *(uint8_t *)data;
+	return transfer(&cmd[0], count + 1, nullptr, 0);
+}
+
+int
+MPU9250_I2C::read(unsigned reg_speed, void *data, unsigned count)
+{
+	/* We want to avoid copying the data of MPUReport: So if the caller
+	 * supplies a buffer not MPUReport in size, it is assume to be a reg or
+	 * reg 16 read
+	 * Since MPUReport has a cmd at front, we must return the data
+	 * after that. Foe anthing else we must return it
+	 */
+	uint32_t offset = count < sizeof(MPUReport) ? 0 : offsetof(MPUReport, status);
+	uint8_t cmd = MPU9250_REG(reg_speed);
+	int ret = transfer(&cmd, 1, &((uint8_t *)data)[offset], count);
+	return ret == OK ? count : ret;
+}
+
+
+int
+MPU9250_I2C::probe()
+{
+	uint8_t whoami = 0;
+	uint8_t expected = _device_type == 6000 ? MPU_WHOAMI_6000 : ICM_WHOAMI_20608;
+	return (read(MPUREG_WHOAMI, &whoami, 1) > 0 && (whoami == expected)) ? 0 : -EIO;
+
+}
+#endif /* PX4_I2C_OBDEV_HMC5883 */

src/drivers/mpu9250/mpu9250_spi.cpp

@@ -0,0 +1,281 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2016 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 mpu9250_spi.cpp
+ *
+ * Driver for the Invensense MPU9250 connected via SPI.
+ *
+ * @author Andrew Tridgell
+ * @author Pat Hickey
+ * @author David sidrane
+ */
+
+#include <px4_config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <string.h>
+#include <assert.h>
+#include <debug.h>
+#include <errno.h>
+#include <unistd.h>
+
+#include <arch/board/board.h>
+
+#include <drivers/device/spi.h>
+#include <drivers/drv_accel.h>
+#include <drivers/drv_device.h>
+
+#include "MPU9250.h"
+#include <board_config.h>
+
+#define DIR_READ			0x80
+#define DIR_WRITE			0x00
+
+#if PX4_SPIDEV_MPU
+#ifdef PX4_SPI_BUS_EXT
+#define EXTERNAL_BUS PX4_SPI_BUS_EXT
+#else
+#define EXTERNAL_BUS 0
+#endif
+
+
+/*
+  The MPU9250 can only handle high SPI bus speeds on the sensor and
+  interrupt status registers. All other registers have a maximum 1MHz
+  SPI speed
+ */
+#define MPU9250_LOW_SPI_BUS_SPEED	1000*1000
+#define MPU9250_HIGH_SPI_BUS_SPEED	11*1000*1000 /* will be rounded to 10.4 MHz, within margins for MPU6K */
+
+
+device::Device *MPU9250_SPI_interface(int bus, int device_type, bool external_bus);
+
+
+class MPU9250_SPI : public device::SPI
+{
+public:
+	MPU9250_SPI(int bus, spi_dev_e device, int device_type);
+	virtual ~MPU9250_SPI();
+
+	virtual int	init();
+	virtual int	read(unsigned address, void *data, unsigned count);
+	virtual int	write(unsigned address, void *data, unsigned count);
+
+	virtual int	ioctl(unsigned operation, unsigned &arg);
+protected:
+	virtual int probe();
+
+private:
+
+	int _device_type;
+	/* Helper to set the desired speed and isolate the register on return */
+
+	void set_bus_frequency(unsigned &reg_speed_reg_out);
+};
+
+device::Device *
+MPU9250_SPI_interface(int bus, int device_type, bool external_bus)
+{
+	spi_dev_e cs = SPIDEV_NONE;
+	device::Device *interface = nullptr;
+
+	if (external_bus) {
+#ifdef PX4_SPI_BUS_EXT
+#  if defined(PX4_SPIDEV_EXT_ICM)
+		cs = (spi_dev_e)(device_type == 6000 ? PX4_SPIDEV_EXT_MPU : PX4_SPIDEV_EXT_ICM);
+#   else
+		cs = (spi_dev_e) PX4_SPIDEV_EXT_MPU;
+#  endif
+#endif
+
+	} else {
+
+#if defined(PX4_SPIDEV_ICM)
+		cs = (spi_dev_e)(device_type == 6000 ? PX4_SPIDEV_MPU : PX4_SPIDEV_ICM);
+#else
+		cs = (spi_dev_e) PX4_SPIDEV_MPU;
+#endif
+	}
+
+	if (cs != SPIDEV_NONE) {
+
+		interface = new MPU9250_SPI(bus, cs, device_type);
+	}
+
+	return interface;
+}
+
+MPU9250_SPI::MPU9250_SPI(int bus, spi_dev_e device, int device_type) :
+	SPI("MPU9250", nullptr, bus, device, SPIDEV_MODE3, MPU9250_LOW_SPI_BUS_SPEED),
+	_device_type(device_type)
+{
+	_device_id.devid_s.devtype =  DRV_ACC_DEVTYPE_MPU9250;
+}
+
+MPU9250_SPI::~MPU9250_SPI()
+{
+}
+
+int
+MPU9250_SPI::init()
+{
+	int ret;
+
+	ret = SPI::init();
+
+	if (ret != OK) {
+		DEVICE_DEBUG("SPI init failed");
+		return -EIO;
+	}
+
+	return OK;
+}
+
+int
+MPU9250_SPI::ioctl(unsigned operation, unsigned &arg)
+{
+	int ret;
+
+	switch (operation) {
+
+	case ACCELIOCGEXTERNAL:
+#if defined(PX4_SPI_BUS_EXT)
+		return _bus == PX4_SPI_BUS_EXT ? 1 : 0;
+#else
+		return 0;
+#endif
+
+	case DEVIOCGDEVICEID:
+		return CDev::ioctl(nullptr, operation, arg);
+
+	case MPUIOCGIS_I2C:
+		return 0;
+
+	default: {
+			ret = -EINVAL;
+		}
+	}
+
+	return ret;
+}
+
+void
+MPU9250_SPI::set_bus_frequency(unsigned &reg_speed)
+{
+	/* Set the desired speed */
+
+	set_frequency(MPU9250_IS_HIGH_SPEED(reg_speed) ? MPU9250_HIGH_SPI_BUS_SPEED : MPU9250_LOW_SPI_BUS_SPEED);
+
+	/* Isoolate the register on return */
+
+	reg_speed = MPU9250_REG(reg_speed);
+}
+
+
+int
+MPU9250_SPI::write(unsigned reg_speed, void *data, unsigned count)
+{
+	uint8_t cmd[MPU_MAX_WRITE_BUFFER_SIZE];
+
+	if (sizeof(cmd) < (count + 1)) {
+		return -EIO;
+	}
+
+	/* Set the desired speed and isolate the register */
+
+	set_bus_frequency(reg_speed);
+
+	cmd[0] = reg_speed | DIR_WRITE;
+	cmd[1] = *(uint8_t *)data;
+
+	return transfer(&cmd[0], &cmd[0], count + 1);
+}
+
+int
+MPU9250_SPI::read(unsigned reg_speed, void *data, unsigned count)
+{
+	/* We want to avoid copying the data of MPUReport: So if the caller
+	 * supplies a buffer not MPUReport in size, it is assume to be a reg or reg 16 read
+	 * and we need to provied the buffer large enough for the callers data
+	 * and our command.
+	 */
+	uint8_t cmd[3] = {0, 0, 0};
+
+	uint8_t *pbuff  =  count < sizeof(MPUReport) ? cmd : (uint8_t *) data ;
+
+
+	if (count < sizeof(MPUReport))  {
+
+		/* add command */
+
+		count++;
+	}
+
+	set_bus_frequency(reg_speed);
+
+	/* Set command */
+
+	pbuff[0] = reg_speed | DIR_READ ;
+
+	/* Transfer the command and get the data */
+
+	int ret = transfer(pbuff, pbuff, count);
+
+	if (ret == OK && pbuff == &cmd[0]) {
+
+		/* Adjust the count back */
+
+		count--;
+
+		/* Return the data */
+
+		memcpy(data, &cmd[1], count);
+
+	}
+
+	return ret == OK ? count : ret;
+}
+
+int
+MPU9250_SPI::probe()
+{
+	uint8_t whoami = 0;
+	uint8_t expected = _device_type == 6000 ? MPU_WHOAMI_6000 : ICM_WHOAMI_20608;
+	return (read(MPUREG_WHOAMI, &whoami, 1) > 0 && (whoami == expected)) ? 0 : -EIO;
+
+}
+
+#endif // PX4_SPIDEV_MPU

src/drivers/stm32/drv_io_timer.c

@@ -379,8 +379,13 @@ static int allocate_channel(unsigned channel, io_timer_channel_mode_t mode)
 
 static int timer_set_rate(unsigned timer, unsigned rate)
 {
+#if defined(CONFIG_ARCH_BOARD_CRAZYFLIE)
+	/* configure the timer to update at 328.125 kHz (recommended) */
+	rARR(timer) = 255;
+#else
 	/* configure the timer to update at the desired rate */
 	rARR(timer) = 1000000 / rate;
+#endif
 
 	/* generate an update event; reloads the counter and all registers */
 	rEGR(timer) = GTIM_EGR_UG;
@@ -427,9 +432,14 @@ static int io_timer_init_timer(unsigned timer)
 			rBDTR(timer) = ATIM_BDTR_MOE;
 		}
 
+#if defined(CONFIG_ARCH_BOARD_CRAZYFLIE)
+		/* configure the timer to free-run at timer frequency */
+		rPSC(timer) = 0;
+#else
 		/* configure the timer to free-run at 1MHz */
 
 		rPSC(timer) = (io_timers[timer].clock_freq / 1000000) - 1;
+#endif
 
 
 		/*