diff --git a/Images/crazyflie.prototype b/Images/crazyflie.prototype
new file mode 100644
index 0000000000000000000000000000000000000000..e02ac101c2ac12961bb92043788062c18714dbf8
--- /dev/null
+++ b/Images/crazyflie.prototype
@@ -0,0 +1,12 @@
+{
+ "board_id": 5,
+ "magic": "Crazyflie",
+ "description": "Firmware for the Crazyflie 2.0",
+ "image": "",
+ "build_time": 0,
+ "summary": "CRAZYFLIE",
+ "version": "0.1",
+ "image_size": 0,
+ "git_identity": "",
+ "board_revision": 0
+}
diff --git a/Makefile b/Makefile
index e942c8e40527704fc4fa5bb8e993e3465f9c3a7c..53f21c0354fdda3d455393f3f23472c053ad96b0 100644
--- a/Makefile
+++ b/Makefile
@@ -171,6 +171,9 @@ px4-stm32f4discovery_default:
mindpx-v2_default:
$(call cmake-build,nuttx_mindpx-v2_default)
+crazyflie_default:
+ $(call cmake-build,nuttx_crazyflie_default)
+
posix_sitl_default:
$(call cmake-build,$@)
diff --git a/ROMFS/px4fmu_common/init.d/4900_crazyflie b/ROMFS/px4fmu_common/init.d/4900_crazyflie
new file mode 100644
index 0000000000000000000000000000000000000000..3d0d037f13ab415f7690a3a5b7b63ba7f0ef2984
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/4900_crazyflie
@@ -0,0 +1,54 @@
+#!nsh
+#
+# @name Crazyflie config
+#
+# @type Quadrotor x
+#
+# @maintainer Tim Dyer <dyer.ti@gmail.com>
+#
+
+# uorb start
+
+# Params
+
+# System status LEDs
+
+# waypoint storage
+
+# Sensors
+mpu9250 -R 12 start
+ak8963 start
+lps25h start
+sensors start
+
+commander start
+
+# Output mode
+crazyflie start
+
+# MAVLINK
+# mavlink start -r 1200 -d /dev/ttyS***
+
+# MAVTYPE
+param set MAV_TYPE 2
+
+exit 1
+
+set OUTPUT_DEV /dev/pwm_output0
+set MIXER_FILE /etc/mixers/quad_x.main.mix
+if mixer load $OUTPUT_DEV $MIXER_FILE
+then
+ echo "[i] Mixer: $MIXER_FILE on $OUTPUT_DEV"
+else
+ echo "[i] Error loading mixer: $MIXER_FILE"
+ echo "ERROR: Could not load mixer: $MIXER_FILE" >> $LOG_FILE
+ tone_alarm $TUNE_ERR
+fi
+
+sh /etc/init.d/rc.mc_apps
+
+# Navigator
+navigator start
+
+# Boot complete
+mavlink boot_complete
\ No newline at end of file
diff --git a/cmake/configs/nuttx_crazyflie_default.cmake b/cmake/configs/nuttx_crazyflie_default.cmake
new file mode 100644
index 0000000000000000000000000000000000000000..ca6da271711d1806e5a61b5ae935d785ab677644
--- /dev/null
+++ b/cmake/configs/nuttx_crazyflie_default.cmake
@@ -0,0 +1,152 @@
+include(nuttx/px4_impl_nuttx)
+
+set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake)
+
+set(config_module_list
+ #
+ # Board support modules
+ #
+ drivers/device
+ drivers/stm32
+ drivers/led
+ drivers/boards/crazyflie
+ drivers/crazyflie
+ drivers/mpu9250
+ drivers/ak8963
+ drivers/lps25h
+ drivers/gps
+ # drivers/pwm_out_sim
+ modules/sensors
+
+ #
+ # System commands
+ #
+ systemcmds/bl_update
+ systemcmds/mixer
+ systemcmds/param
+ systemcmds/perf
+ systemcmds/pwm
+ systemcmds/esc_calib
+ systemcmds/reboot
+ systemcmds/top
+ systemcmds/config
+ systemcmds/nshterm
+ systemcmds/mtd
+ systemcmds/dumpfile
+ systemcmds/ver
+
+ #
+ # General system control
+ #
+ modules/commander
+ modules/navigator
+ modules/mavlink
+ modules/land_detector
+
+ modules/dummy
+
+ #
+ # Estimation modules (EKF/ SO3 / other filters)
+ #
+ # Too high RAM usage due to static allocations
+ # modules/attitude_estimator_ekf
+ modules/attitude_estimator_q
+ modules/ekf_att_pos_estimator
+ modules/position_estimator_inav
+
+ #
+ # Vehicle Control
+ #
+ # modules/segway # XXX Needs GCC 4.7 fix
+ modules/fw_pos_control_l1
+ modules/fw_att_control
+ modules/mc_att_control
+ modules/mc_pos_control
+ modules/vtol_att_control
+
+ #
+ # Logging
+ #
+ modules/sdlog2
+ modules/screen
+
+ #
+ # Library modules
+ #
+ modules/param
+ modules/systemlib
+ modules/systemlib/mixer
+ modules/controllib
+ modules/uORB
+ modules/dataman
+
+ #
+ # Libraries
+ #
+ #lib/mathlib/CMSIS
+ lib/mathlib
+ lib/mathlib/math/filter
+ lib/ecl
+ lib/external_lgpl
+ lib/geo
+ lib/geo_lookup
+ lib/conversion
+ lib/launchdetection
+ lib/terrain_estimation
+ lib/runway_takeoff
+ platforms/nuttx
+
+ # had to add for cmake, not sure why wasn't in original config
+ platforms/common
+ platforms/nuttx/px4_layer
+
+ #
+ # OBC challenge
+ #
+ modules/bottle_drop
+
+ #
+ # Rover apps
+ #
+ examples/rover_steering_control
+
+ #
+ # Demo apps
+ #
+ #examples/math_demo
+ # Tutorial code from
+ # https://px4.io/dev/px4_simple_app
+ #examples/px4_simple_app
+
+ # Tutorial code from
+ # https://px4.io/dev/daemon
+ #examples/px4_daemon_app
+
+ # Tutorial code from
+ # https://px4.io/dev/debug_values
+ #examples/px4_mavlink_debug
+
+ # Tutorial code from
+ # https://px4.io/dev/example_fixedwing_control
+ #examples/fixedwing_control
+
+ # Hardware test
+ #examples/hwtest
+)
+
+set(config_extra_builtin_cmds
+ serdis
+ sercon
+ )
+
+set(config_extra_libs
+ ${CMAKE_SOURCE_DIR}/src/lib/mathlib/CMSIS/libarm_cortexM4lf_math.a
+ )
+
+add_custom_target(sercon)
+set_target_properties(sercon PROPERTIES
+ MAIN "sercon" STACK "2048")
+
+add_custom_target(serdis)
+set_target_properties(serdis PROPERTIES
+ MAIN "serdis" STACK "2048")
diff --git a/nuttx-configs/crazyflie/include/board.h b/nuttx-configs/crazyflie/include/board.h
new file mode 100644
index 0000000000000000000000000000000000000000..9857eb03a2400ad802d8512b827d1f06c6f77721
--- /dev/null
+++ b/nuttx-configs/crazyflie/include/board.h
@@ -0,0 +1,261 @@
+/************************************************************************************
+ * configs/crazyflie/include/board.h
+ * include/arch/board/board.h
+ *
+ * Copyright (C) 2012 Gregory Nutt. All rights reserved.
+ * Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ************************************************************************************/
+
+#ifndef __CONFIG_CRAZYFLIE_INCLUDE_BOARD_H
+#define __CONFIG_CRAZYFLIE_INCLUDE_BOARD_H
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <nuttx/config.h>
+
+#ifndef __ASSEMBLY__
+# include <stdint.h>
+#endif
+
+#include "stm32_rcc.h"
+#include "stm32_sdio.h"
+#include "stm32.h"
+
+/************************************************************************************
+ * Definitions
+ ************************************************************************************/
+
+/* Clocking *************************************************************************/
+/* The PX4FMU uses a 24MHz crystal connected to the HSE.
+ *
+ * This is the canonical configuration:
+ * System Clock source : PLL (HSE)
+ * SYSCLK(Hz) : 168000000 Determined by PLL configuration
+ * HCLK(Hz) : 168000000 (STM32_RCC_CFGR_HPRE)
+ * AHB Prescaler : 1 (STM32_RCC_CFGR_HPRE)
+ * APB1 Prescaler : 4 (STM32_RCC_CFGR_PPRE1)
+ * APB2 Prescaler : 2 (STM32_RCC_CFGR_PPRE2)
+ * HSE Frequency(Hz) : 24000000 (STM32_BOARD_XTAL)
+ * PLLM : 24 (STM32_PLLCFG_PLLM)
+ * PLLN : 336 (STM32_PLLCFG_PLLN)
+ * PLLP : 2 (STM32_PLLCFG_PLLP)
+ * PLLQ : 7 (STM32_PLLCFG_PLLQ)
+ * Main regulator output voltage : Scale1 mode Needed for high speed SYSCLK
+ * Flash Latency(WS) : 5
+ * Prefetch Buffer : OFF
+ * Instruction cache : ON
+ * Data cache : ON
+ * Require 48MHz for USB OTG FS, : Enabled
+ * SDIO and RNG clock
+ */
+
+/* HSI - 16 MHz RC factory-trimmed
+ * LSI - 32 KHz RC
+ * HSE - On-board crystal frequency is 24MHz
+ * LSE - not installed
+ */
+
+#define STM32_BOARD_XTAL 8000000ul
+
+#define STM32_HSI_FREQUENCY 16000000ul
+#define STM32_LSI_FREQUENCY 32000
+#define STM32_HSE_FREQUENCY STM32_BOARD_XTAL
+//#define STM32_LSE_FREQUENCY 32768
+
+/* Main PLL Configuration.
+ *
+ * PLL source is HSE
+ * PLL_VCO = (STM32_HSE_FREQUENCY / PLLM) * PLLN
+ * = (8,000,000 / 8) * 336
+ * = 336,000,000
+ * SYSCLK = PLL_VCO / PLLP
+ * = 336,000,000 / 2 = 168,000,000
+ * USB OTG FS, SDIO and RNG Clock
+ * = PLL_VCO / PLLQ
+ * = 48,000,000
+ */
+
+#define STM32_PLLCFG_PLLM RCC_PLLCFG_PLLM(8)
+#define STM32_PLLCFG_PLLN RCC_PLLCFG_PLLN(336)
+#define STM32_PLLCFG_PLLP RCC_PLLCFG_PLLP_2
+#define STM32_PLLCFG_PLLQ RCC_PLLCFG_PLLQ(7)
+
+#define STM32_SYSCLK_FREQUENCY 168000000ul
+
+/* AHB clock (HCLK) is SYSCLK (168MHz) */
+
+#define STM32_RCC_CFGR_HPRE RCC_CFGR_HPRE_SYSCLK /* HCLK = SYSCLK / 1 */
+#define STM32_HCLK_FREQUENCY STM32_SYSCLK_FREQUENCY
+#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* same as above, to satisfy compiler */
+
+/* APB1 clock (PCLK1) is HCLK/4 (42MHz) */
+
+#define STM32_RCC_CFGR_PPRE1 RCC_CFGR_PPRE1_HCLKd4 /* PCLK1 = HCLK / 4 */
+#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY/4)
+
+/* Timers driven from APB1 will be twice PCLK1 */
+
+#define STM32_APB1_TIM2_CLKIN (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM3_CLKIN (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM4_CLKIN (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM5_CLKIN (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM6_CLKIN (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM7_CLKIN (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM12_CLKIN (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM13_CLKIN (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM14_CLKIN (2*STM32_PCLK1_FREQUENCY)
+
+/* APB2 clock (PCLK2) is HCLK/2 (84MHz) */
+
+#define STM32_RCC_CFGR_PPRE2 RCC_CFGR_PPRE2_HCLKd2 /* PCLK2 = HCLK / 2 */
+#define STM32_PCLK2_FREQUENCY (STM32_HCLK_FREQUENCY/2)
+
+/* Timers driven from APB2 will be twice PCLK2 */
+
+#define STM32_APB2_TIM1_CLKIN (2*STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM8_CLKIN (2*STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM9_CLKIN (2*STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM10_CLKIN (2*STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM11_CLKIN (2*STM32_PCLK2_FREQUENCY)
+
+/* Timer Frequencies, if APBx is set to 1, frequency is same to APBx
+ * otherwise frequency is 2xAPBx.
+ * Note: TIM1,8 are on APB2, others on APB1
+ */
+
+#define STM32_TIM18_FREQUENCY (2*STM32_PCLK2_FREQUENCY)
+#define STM32_TIM27_FREQUENCY (2*STM32_PCLK1_FREQUENCY)
+
+/* Alternate function pin selections ************************************************/
+
+/*
+ * UARTs.
+ *
+ */
+#define GPIO_USART2_RX GPIO_USART2_RX_1
+#define GPIO_USART2_TX GPIO_USART2_TX_1
+
+#define GPIO_USART3_RX GPIO_USART3_RX_2
+#define GPIO_USART3_TX GPIO_USART3_TX_2
+
+#define GPIO_USART6_RX GPIO_USART6_RX_1
+#define GPIO_USART6_TX GPIO_USART6_TX_1
+
+/* UART DMA configuration for USART6 */
+#define DMAMAP_USART6_RX DMAMAP_USART6_RX_2
+
+/*
+ * I2C
+ *
+ * The optional _GPIO configurations allow the I2C driver to manually
+ * reset the bus to clear stuck slaves. They match the pin configuration,
+ * but are normally-high GPIOs.
+ */
+#define GPIO_I2C1_SCL GPIO_I2C1_SCL_1
+#define GPIO_I2C1_SDA GPIO_I2C1_SDA_1
+#define GPIO_I2C1_SCL_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN6)
+#define GPIO_I2C1_SDA_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN7)
+
+#define GPIO_I2C3_SCL GPIO_I2C3_SCL_1
+#define GPIO_I2C3_SDA GPIO_I2C3_SDA_1
+#define GPIO_I2C3_SCL_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN8)
+#define GPIO_I2C3_SDA_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN9)
+
+/*
+ * SPI
+ *
+ * There are sensors on SPI1, and SPI3 is connected to the microSD slot.
+ */
+#define GPIO_SPI1_MISO (GPIO_SPI1_MISO_1|GPIO_SPEED_50MHz)
+#define GPIO_SPI1_MOSI (GPIO_SPI1_MOSI_1|GPIO_SPEED_50MHz)
+#define GPIO_SPI1_SCK (GPIO_SPI1_SCK_1|GPIO_SPEED_50MHz)
+
+/* XXX since we allocate the HP work stack from CCM RAM on normal system startup,
+ SPI1 will never run in DMA mode - so we can just give it a random config here.
+ What we really need to do is to make DMA configurable per channel, and always
+ disable it for SPI1. */
+#define DMACHAN_SPI1_RX DMAMAP_SPI1_RX_1
+#define DMACHAN_SPI1_TX DMAMAP_SPI1_TX_2
+
+/************************************************************************************
+ * Public Data
+ ************************************************************************************/
+
+#ifndef __ASSEMBLY__
+
+#undef EXTERN
+#if defined(__cplusplus)
+#define EXTERN extern "C"
+extern "C" {
+#else
+#define EXTERN extern
+#endif
+
+/************************************************************************************
+ * Public Function Prototypes
+ ************************************************************************************/
+/************************************************************************************
+ * 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.
+ *
+ ************************************************************************************/
+
+EXTERN void stm32_boardinitialize(void);
+
+/************************************************************************************
+ * Name: stm32_ledinit, stm32_setled, and stm32_setleds
+ *
+ * Description:
+ * If CONFIG_ARCH_LEDS is defined, then NuttX will control the on-board LEDs. If
+ * CONFIG_ARCH_LEDS is not defined, then the following interfacesare available to
+ * control the LEDs from user applications.
+ *
+ ************************************************************************************/
+
+#ifndef CONFIG_ARCH_LEDS
+EXTERN void stm32_ledinit(void);
+EXTERN void stm32_setled(int led, bool ledon);
+EXTERN void stm32_setleds(uint8_t ledset);
+#endif
+
+#undef EXTERN
+#if defined(__cplusplus)
+}
+#endif
+
+#endif /* __ASSEMBLY__ */
+#endif /* __CONFIG_CRAZYFLIE_INCLUDE_BOARD_H */
diff --git a/nuttx-configs/crazyflie/include/nsh_romfsimg.h b/nuttx-configs/crazyflie/include/nsh_romfsimg.h
new file mode 100644
index 0000000000000000000000000000000000000000..15e4e7a8d5a6cdc29fcaf1e67edd8a2087f5178a
--- /dev/null
+++ b/nuttx-configs/crazyflie/include/nsh_romfsimg.h
@@ -0,0 +1,42 @@
+/****************************************************************************
+ *
+ * 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.
+ *
+ ****************************************************************************/
+
+/**
+ * nsh_romfsetc.h
+ *
+ * This file is a stub for 'make export' purposes; the actual ROMFS
+ * must be supplied by the library client.
+ */
+
+extern unsigned char romfs_img[];
+extern unsigned int romfs_img_len;
diff --git a/nuttx-configs/crazyflie/nsh/Make.defs b/nuttx-configs/crazyflie/nsh/Make.defs
new file mode 100644
index 0000000000000000000000000000000000000000..be0222effbf1ab0411e24a2b364ec5fba24bdb01
--- /dev/null
+++ b/nuttx-configs/crazyflie/nsh/Make.defs
@@ -0,0 +1,177 @@
+############################################################################
+# configs/crazyflie/nsh/Make.defs
+#
+# Copyright (C) 2011 Gregory Nutt. All rights reserved.
+# Author: Gregory Nutt <gnutt@nuttx.org>
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+# notice, this list of conditions and the following disclaimer in
+# the documentation and/or other materials provided with the
+# distribution.
+# 3. Neither the name NuttX nor the names of its contributors may be
+# used to endorse or promote products derived from this software
+# without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+include ${TOPDIR}/.config
+include ${TOPDIR}/tools/Config.mk
+
+#
+# We only support building with the ARM bare-metal toolchain from
+# https://launchpad.net/gcc-arm-embedded on Windows, Linux or Mac OS.
+#
+CONFIG_ARMV7M_TOOLCHAIN := GNU_EABI
+
+include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs
+
+CC = $(CROSSDEV)gcc
+CXX = $(CROSSDEV)g++
+CPP = $(CROSSDEV)gcc -E
+LD = $(CROSSDEV)ld
+AR = $(CROSSDEV)ar rcs
+NM = $(CROSSDEV)nm
+OBJCOPY = $(CROSSDEV)objcopy
+OBJDUMP = $(CROSSDEV)objdump
+
+MAXOPTIMIZATION = -Os
+ARCHCPUFLAGS = -mcpu=cortex-m4 \
+ -mthumb \
+ -march=armv7e-m \
+ -mfpu=fpv4-sp-d16 \
+ -mfloat-abi=hard
+
+
+# enable precise stack overflow tracking
+ifeq ($(CONFIG_ARMV7M_STACKCHECK),y)
+INSTRUMENTATIONDEFINES = -finstrument-functions -ffixed-r10
+endif
+
+# pull in *just* libm from the toolchain ... this is grody
+LIBM = "${shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a}"
+EXTRA_LIBS += $(LIBM)
+
+# use our linker script
+LDSCRIPT = ld.script
+
+ifeq ($(WINTOOL),y)
+ # Windows-native toolchains
+ DIRLINK = $(TOPDIR)/tools/copydir.sh
+ DIRUNLINK = $(TOPDIR)/tools/unlink.sh
+ MKDEP = $(TOPDIR)/tools/mknulldeps.sh
+ ARCHINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}"
+ ARCHXXINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}"
+ ARCHSCRIPT = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)}"
+else
+ ifeq ($(PX4_WINTOOL),y)
+ # Windows-native toolchains (MSYS)
+ DIRLINK = $(TOPDIR)/tools/copydir.sh
+ DIRUNLINK = $(TOPDIR)/tools/unlink.sh
+ MKDEP = $(TOPDIR)/tools/mknulldeps.sh
+ ARCHINCLUDES = -I. -isystem $(TOPDIR)/include
+ ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx
+ ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)
+ else
+ # Linux/Cygwin-native toolchain
+ MKDEP = $(TOPDIR)/tools/mkdeps.sh
+ ARCHINCLUDES = -I. -isystem $(TOPDIR)/include
+ ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx
+ ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)
+ endif
+endif
+
+# tool versions
+ARCHCCVERSION = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'}
+ARCHCCMAJOR = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1}
+
+# optimisation flags
+ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \
+ -fno-strict-aliasing \
+ -fno-strength-reduce \
+ -fomit-frame-pointer \
+ -funsafe-math-optimizations \
+ -fno-builtin-printf \
+ -ffunction-sections \
+ -fdata-sections
+
+ifeq ("${CONFIG_DEBUG_SYMBOLS}","y")
+ARCHOPTIMIZATION += -g
+endif
+
+ARCHCFLAGS = -std=gnu99
+ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x
+ARCHWARNINGS = -Wall \
+ -Wno-sign-compare \
+ -Wextra \
+ -Wdouble-promotion \
+ -Wshadow \
+ -Wframe-larger-than=1024 \
+ -Wpointer-arith \
+ -Wlogical-op \
+ -Wpacked \
+ -Wno-unused-parameter
+# -Wcast-qual - generates spurious noreturn attribute warnings, try again later
+# -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code
+# -Wcast-align - would help catch bad casts in some cases, but generates too many false positives
+
+ARCHCWARNINGS = $(ARCHWARNINGS) \
+ -Wbad-function-cast \
+ -Wstrict-prototypes \
+ -Wold-style-declaration \
+ -Wmissing-parameter-type \
+ -Wnested-externs
+ARCHWARNINGSXX = $(ARCHWARNINGS) \
+ -Wno-psabi
+ARCHDEFINES =
+ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10
+
+# this seems to be the only way to add linker flags
+EXTRA_LIBS += --warn-common \
+ --gc-sections
+
+CFLAGS = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common
+CPICFLAGS = $(ARCHPICFLAGS) $(CFLAGS)
+CXXFLAGS = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe
+CXXPICFLAGS = $(ARCHPICFLAGS) $(CXXFLAGS)
+CPPFLAGS = $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES)
+AFLAGS = $(CFLAGS) -D__ASSEMBLY__
+
+NXFLATLDFLAGS1 = -r -d -warn-common
+NXFLATLDFLAGS2 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld -no-check-sections
+LDNXFLATFLAGS = -e main -s 2048
+
+OBJEXT = .o
+LIBEXT = .a
+EXEEXT =
+
+
+# produce partially-linked $1 from files in $2
+define PRELINK
+ @echo "PRELINK: $1"
+ $(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1
+endef
+
+HOSTCC = gcc
+HOSTINCLUDES = -I.
+HOSTCFLAGS = -Wall -Wstrict-prototypes -Wshadow -g -pipe
+HOSTLDFLAGS =
+
diff --git a/nuttx-configs/crazyflie/nsh/appconfig b/nuttx-configs/crazyflie/nsh/appconfig
new file mode 100644
index 0000000000000000000000000000000000000000..4d5b8d79c9a97fffba8bcacf6909a589278be0ba
--- /dev/null
+++ b/nuttx-configs/crazyflie/nsh/appconfig
@@ -0,0 +1,52 @@
+############################################################################
+# configs/crazyflie/nsh/appconfig
+#
+# Copyright (C) 2011 Gregory Nutt. All rights reserved.
+# Author: Gregory Nutt <gnutt@nuttx.org>
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+# notice, this list of conditions and the following disclaimer in
+# the documentation and/or other materials provided with the
+# distribution.
+# 3. Neither the name NuttX nor the names of its contributors may be
+# used to endorse or promote products derived from this software
+# without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+# Path to example in apps/examples containing the user_start entry point
+
+CONFIGURED_APPS += examples/nsh
+
+# The NSH application library
+CONFIGURED_APPS += nshlib
+CONFIGURED_APPS += system/readline
+
+ifeq ($(CONFIG_CAN),y)
+#CONFIGURED_APPS += examples/can
+endif
+
+#ifeq ($(CONFIG_USBDEV),y)
+#ifeq ($(CONFIG_CDCACM),y)
+CONFIGURED_APPS += examples/cdcacm
+#endif
+#endif
diff --git a/nuttx-configs/crazyflie/nsh/defconfig b/nuttx-configs/crazyflie/nsh/defconfig
new file mode 100644
index 0000000000000000000000000000000000000000..dae2963998dfb3cdb07d50545a77a59239747596
--- /dev/null
+++ b/nuttx-configs/crazyflie/nsh/defconfig
@@ -0,0 +1,919 @@
+#
+# Automatically generated file; DO NOT EDIT.
+# Nuttx/ Configuration
+#
+CONFIG_NUTTX_NEWCONFIG=y
+
+#
+# Build Setup
+#
+# CONFIG_EXPERIMENTAL is not set
+# CONFIG_HOST_LINUX is not set
+CONFIG_HOST_OSX=y
+# CONFIG_HOST_WINDOWS is not set
+# CONFIG_HOST_OTHER is not set
+
+#
+# Build Configuration
+#
+CONFIG_APPS_DIR="../apps"
+# CONFIG_BUILD_2PASS is not set
+
+#
+# Binary Output Formats
+#
+# CONFIG_RRLOAD_BINARY is not set
+# CONFIG_INTELHEX_BINARY is not set
+# CONFIG_MOTOROLA_SREC is not set
+CONFIG_RAW_BINARY=y
+
+#
+# Customize Header Files
+#
+# CONFIG_ARCH_STDBOOL_H is not set
+CONFIG_ARCH_MATH_H=y
+# CONFIG_ARCH_FLOAT_H is not set
+# CONFIG_ARCH_STDARG_H is not set
+
+#
+# Debug Options
+#
+# CONFIG_DEBUG is not set
+CONFIG_DEBUG_SYMBOLS=y
+
+#
+# System Type
+#
+# CONFIG_ARCH_8051 is not set
+CONFIG_ARCH_ARM=y
+# CONFIG_ARCH_AVR is not set
+# CONFIG_ARCH_HC is not set
+# CONFIG_ARCH_MIPS is not set
+# CONFIG_ARCH_RGMP is not set
+# CONFIG_ARCH_SH is not set
+# CONFIG_ARCH_SIM is not set
+# CONFIG_ARCH_X86 is not set
+# CONFIG_ARCH_Z16 is not set
+# CONFIG_ARCH_Z80 is not set
+CONFIG_ARCH="arm"
+
+#
+# ARM Options
+#
+# CONFIG_ARCH_CHIP_C5471 is not set
+# CONFIG_ARCH_CHIP_CALYPSO is not set
+# CONFIG_ARCH_CHIP_DM320 is not set
+# CONFIG_ARCH_CHIP_IMX is not set
+# CONFIG_ARCH_CHIP_KINETIS is not set
+# CONFIG_ARCH_CHIP_KL is not set
+# CONFIG_ARCH_CHIP_LM is not set
+# CONFIG_ARCH_CHIP_LPC17XX is not set
+# CONFIG_ARCH_CHIP_LPC214X is not set
+# CONFIG_ARCH_CHIP_LPC2378 is not set
+# CONFIG_ARCH_CHIP_LPC31XX is not set
+# CONFIG_ARCH_CHIP_LPC43XX is not set
+# CONFIG_ARCH_CHIP_NUC1XX is not set
+# CONFIG_ARCH_CHIP_SAM34 is not set
+CONFIG_ARCH_CHIP_STM32=y
+# CONFIG_ARCH_CHIP_STR71X is not set
+CONFIG_ARCH_CORTEXM4=y
+CONFIG_ARCH_FAMILY="armv7-m"
+CONFIG_ARCH_CHIP="stm32"
+CONFIG_ARMV7M_USEBASEPRI=y
+CONFIG_ARCH_HAVE_CMNVECTOR=y
+CONFIG_ARMV7M_CMNVECTOR=y
+CONFIG_ARCH_HAVE_FPU=y
+CONFIG_ARCH_FPU=y
+CONFIG_ARCH_HAVE_MPU=y
+# CONFIG_ARMV7M_MPU is not set
+
+#
+# ARMV7M Configuration Options
+#
+# CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set
+CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y
+# CONFIG_ARMV7M_STACKCHECK is not set
+CONFIG_SERIAL_TERMIOS=y
+
+#
+# STM32 Configuration Options
+#
+# CONFIG_ARCH_CHIP_STM32L151C6 is not set
+# CONFIG_ARCH_CHIP_STM32L151C8 is not set
+# CONFIG_ARCH_CHIP_STM32L151CB is not set
+# CONFIG_ARCH_CHIP_STM32L151R6 is not set
+# CONFIG_ARCH_CHIP_STM32L151R8 is not set
+# CONFIG_ARCH_CHIP_STM32L151RB is not set
+# CONFIG_ARCH_CHIP_STM32L151V6 is not set
+# CONFIG_ARCH_CHIP_STM32L151V8 is not set
+# CONFIG_ARCH_CHIP_STM32L151VB is not set
+# CONFIG_ARCH_CHIP_STM32L152C6 is not set
+# CONFIG_ARCH_CHIP_STM32L152C8 is not set
+# CONFIG_ARCH_CHIP_STM32L152CB is not set
+# CONFIG_ARCH_CHIP_STM32L152R6 is not set
+# CONFIG_ARCH_CHIP_STM32L152R8 is not set
+# CONFIG_ARCH_CHIP_STM32L152RB is not set
+# CONFIG_ARCH_CHIP_STM32L152V6 is not set
+# CONFIG_ARCH_CHIP_STM32L152V8 is not set
+# CONFIG_ARCH_CHIP_STM32L152VB is not set
+# CONFIG_ARCH_CHIP_STM32F100C8 is not set
+# CONFIG_ARCH_CHIP_STM32F100CB is not set
+# CONFIG_ARCH_CHIP_STM32F100R8 is not set
+# CONFIG_ARCH_CHIP_STM32F100RB is not set
+# CONFIG_ARCH_CHIP_STM32F100RC is not set
+# CONFIG_ARCH_CHIP_STM32F100RD is not set
+# CONFIG_ARCH_CHIP_STM32F100RE is not set
+# CONFIG_ARCH_CHIP_STM32F100V8 is not set
+# CONFIG_ARCH_CHIP_STM32F100VB is not set
+# CONFIG_ARCH_CHIP_STM32F100VC is not set
+# CONFIG_ARCH_CHIP_STM32F100VD is not set
+# CONFIG_ARCH_CHIP_STM32F100VE is not set
+# CONFIG_ARCH_CHIP_STM32F102CB is not set
+# CONFIG_ARCH_CHIP_STM32F103C4 is not set
+# CONFIG_ARCH_CHIP_STM32F103C8 is not set
+# CONFIG_ARCH_CHIP_STM32F103RET6 is not set
+# CONFIG_ARCH_CHIP_STM32F103VCT6 is not set
+# CONFIG_ARCH_CHIP_STM32F103VET6 is not set
+# CONFIG_ARCH_CHIP_STM32F103ZET6 is not set
+# CONFIG_ARCH_CHIP_STM32F105VBT7 is not set
+# CONFIG_ARCH_CHIP_STM32F107VC is not set
+# CONFIG_ARCH_CHIP_STM32F207IG is not set
+# CONFIG_ARCH_CHIP_STM32F302CB is not set
+# CONFIG_ARCH_CHIP_STM32F302CC is not set
+# CONFIG_ARCH_CHIP_STM32F302RB is not set
+# CONFIG_ARCH_CHIP_STM32F302RC is not set
+# CONFIG_ARCH_CHIP_STM32F302VB is not set
+# CONFIG_ARCH_CHIP_STM32F302VC is not set
+# CONFIG_ARCH_CHIP_STM32F303CB is not set
+# CONFIG_ARCH_CHIP_STM32F303CC is not set
+# CONFIG_ARCH_CHIP_STM32F303RB is not set
+# CONFIG_ARCH_CHIP_STM32F303RC is not set
+# CONFIG_ARCH_CHIP_STM32F303VB is not set
+# CONFIG_ARCH_CHIP_STM32F303VC is not set
+CONFIG_ARCH_CHIP_STM32F405RG=y
+# CONFIG_ARCH_CHIP_STM32F405VG is not set
+# CONFIG_ARCH_CHIP_STM32F405ZG is not set
+# CONFIG_ARCH_CHIP_STM32F407VE is not set
+# CONFIG_ARCH_CHIP_STM32F407VG is not set
+# CONFIG_ARCH_CHIP_STM32F407ZE is not set
+# CONFIG_ARCH_CHIP_STM32F407ZG is not set
+# CONFIG_ARCH_CHIP_STM32F407IE is not set
+# CONFIG_ARCH_CHIP_STM32F407IG is not set
+# CONFIG_ARCH_CHIP_STM32F427V is not set
+# CONFIG_ARCH_CHIP_STM32F427Z is not set
+# CONFIG_ARCH_CHIP_STM32F427I is not set
+# CONFIG_STM32_STM32L15XX is not set
+# CONFIG_STM32_ENERGYLITE is not set
+# CONFIG_STM32_STM32F10XX is not set
+# CONFIG_STM32_VALUELINE is not set
+# CONFIG_STM32_CONNECTIVITYLINE is not set
+# CONFIG_STM32_PERFORMANCELINE is not set
+# CONFIG_STM32_HIGHDENSITY is not set
+# CONFIG_STM32_MEDIUMDENSITY is not set
+# CONFIG_STM32_LOWDENSITY is not set
+# CONFIG_STM32_STM32F20XX is not set
+# CONFIG_STM32_STM32F30XX is not set
+CONFIG_STM32_STM32F40XX=y
+# CONFIG_STM32_DFU is not set
+
+#
+# STM32 Peripheral Support
+#
+CONFIG_STM32_ADC1=y
+# CONFIG_STM32_ADC2 is not set
+# CONFIG_STM32_ADC3 is not set
+CONFIG_STM32_BKPSRAM=y
+# CONFIG_STM32_CAN1 is not set
+# CONFIG_STM32_CAN2 is not set
+CONFIG_STM32_CCMDATARAM=y
+# CONFIG_STM32_CRC is not set
+# CONFIG_STM32_CRYP is not set
+CONFIG_STM32_DMA1=y
+CONFIG_STM32_DMA2=y
+# CONFIG_STM32_DAC1 is not set
+# CONFIG_STM32_DAC2 is not set
+# CONFIG_STM32_DCMI is not set
+# CONFIG_STM32_ETHMAC is not set
+# CONFIG_STM32_FSMC is not set
+# CONFIG_STM32_HASH is not set
+CONFIG_STM32_I2C1=y
+# CONFIG_STM32_I2C2 is not set
+CONFIG_STM32_I2C3=y
+CONFIG_STM32_OTGFS=y
+# CONFIG_STM32_OTGHS is not set
+CONFIG_STM32_PWR=y
+# CONFIG_STM32_RNG is not set
+# CONFIG_STM32_SDIO is not set
+CONFIG_STM32_SPI1=y
+# CONFIG_STM32_SPI2 is not set
+# CONFIG_STM32_SPI3 is not set
+CONFIG_STM32_SYSCFG=y
+# CONFIG_STM32_TIM1 is not set
+CONFIG_STM32_TIM2=y
+# CONFIG_STM32_TIM3 is not set
+CONFIG_STM32_TIM4=y
+# CONFIG_STM32_TIM5 is not set
+# CONFIG_STM32_TIM6 is not set
+# CONFIG_STM32_TIM7 is not set
+# CONFIG_STM32_TIM8 is not set
+# CONFIG_STM32_TIM9 is not set
+# CONFIG_STM32_TIM10 is not set
+# CONFIG_STM32_TIM11 is not set
+# CONFIG_STM32_TIM12 is not set
+# CONFIG_STM32_TIM13 is not set
+# CONFIG_STM32_TIM14 is not set
+# CONFIG_STM32_USART1 is not set
+CONFIG_STM32_USART2=y
+CONFIG_STM32_USART3=y
+# CONFIG_STM32_UART4 is not set
+# CONFIG_STM32_UART5 is not set
+CONFIG_STM32_USART6=y
+# CONFIG_STM32_IWDG is not set
+CONFIG_STM32_WWDG=y
+CONFIG_STM32_ADC=y
+CONFIG_STM32_SPI=y
+CONFIG_STM32_I2C=y
+
+#
+# Alternate Pin Mapping
+#
+CONFIG_STM32_FLASH_PREFETCH=y
+# CONFIG_STM32_JTAG_DISABLE is not set
+# CONFIG_STM32_JTAG_FULL_ENABLE is not set
+# CONFIG_STM32_JTAG_NOJNTRST_ENABLE is not set
+CONFIG_STM32_JTAG_SW_ENABLE=y
+CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y
+# CONFIG_STM32_FORCEPOWER is not set
+# CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG is not set
+# CONFIG_STM32_CCMEXCLUDE is not set
+CONFIG_STM32_DMACAPABLE=y
+# CONFIG_STM32_TIM2_PWM is not set
+# CONFIG_STM32_TIM4_PWM is not set
+# CONFIG_STM32_TIM2_ADC is not set
+# CONFIG_STM32_TIM4_ADC is not set
+CONFIG_STM32_USART=y
+
+#
+# U[S]ART Configuration
+#
+# CONFIG_USART2_RS485 is not set
+CONFIG_USART2_RXDMA=y
+# CONFIG_USART3_RS485 is not set
+CONFIG_USART3_RXDMA=y
+# CONFIG_USART6_RS485 is not set
+CONFIG_USART6_RXDMA=y
+CONFIG_SERIAL_DISABLE_REORDERING=y
+CONFIG_STM32_USART_SINGLEWIRE=y
+
+#
+# SPI Configuration
+#
+# CONFIG_STM32_SPI_INTERRUPTS is not set
+# CONFIG_STM32_SPI_DMA is not set
+
+#
+# I2C Configuration
+#
+# CONFIG_STM32_I2C_DYNTIMEO is not set
+CONFIG_STM32_I2CTIMEOSEC=0
+CONFIG_STM32_I2CTIMEOMS=10
+CONFIG_STM32_I2CTIMEOTICKS=500
+# CONFIG_STM32_I2C_DUTY16_9 is not set
+
+#
+# USB Host Configuration
+#
+
+#
+# USB Device Configuration
+#
+
+#
+# External Memory Configuration
+#
+
+#
+# Architecture Options
+#
+# CONFIG_ARCH_NOINTC is not set
+# CONFIG_ARCH_VECNOTIRQ is not set
+CONFIG_ARCH_DMA=y
+CONFIG_ARCH_IRQPRIO=y
+# CONFIG_CUSTOM_STACK is not set
+# CONFIG_ADDRENV is not set
+CONFIG_ARCH_HAVE_VFORK=y
+CONFIG_ARCH_STACKDUMP=y
+# CONFIG_ENDIAN_BIG is not set
+# CONFIG_ARCH_HAVE_RAMFUNCS is not set
+CONFIG_ARCH_HAVE_RAMVECTORS=y
+# CONFIG_ARCH_RAMVECTORS is not set
+
+#
+# Board Settings
+#
+CONFIG_BOARD_LOOPSPERMSEC=16717
+# CONFIG_ARCH_CALIBRATION is not set
+CONFIG_DRAM_START=0x20000000
+CONFIG_DRAM_SIZE=196608
+CONFIG_ARCH_HAVE_INTERRUPTSTACK=y
+CONFIG_ARCH_INTERRUPTSTACK=750
+
+#
+# Boot options
+#
+# CONFIG_BOOT_RUNFROMEXTSRAM is not set
+CONFIG_BOOT_RUNFROMFLASH=y
+# CONFIG_BOOT_RUNFROMISRAM is not set
+# CONFIG_BOOT_RUNFROMSDRAM is not set
+# CONFIG_BOOT_COPYTORAM is not set
+
+#
+# Board Selection
+#
+CONFIG_ARCH_BOARD_CUSTOM=y
+CONFIG_ARCH_BOARD=""
+
+#
+# Common Board Options
+#
+CONFIG_NSH_MMCSDMINOR=0
+
+#
+# Board-Specific Options
+#
+
+#
+# RTOS Features
+#
+# CONFIG_BOARD_INITIALIZE is not set
+CONFIG_MSEC_PER_TICK=1
+CONFIG_RR_INTERVAL=0
+CONFIG_SCHED_INSTRUMENTATION=y
+CONFIG_TASK_NAME_SIZE=24
+# CONFIG_SCHED_HAVE_PARENT is not set
+# CONFIG_JULIAN_TIME is not set
+CONFIG_START_YEAR=1970
+CONFIG_START_MONTH=1
+CONFIG_START_DAY=1
+CONFIG_DEV_CONSOLE=y
+# CONFIG_MUTEX_TYPES is not set
+CONFIG_PRIORITY_INHERITANCE=y
+CONFIG_SEM_PREALLOCHOLDERS=0
+CONFIG_SEM_NNESTPRIO=8
+# CONFIG_FDCLONE_DISABLE is not set
+CONFIG_FDCLONE_STDIO=y
+CONFIG_SDCLONE_DISABLE=y
+CONFIG_SCHED_WAITPID=y
+# CONFIG_SCHED_STARTHOOK is not set
+CONFIG_SCHED_ATEXIT=y
+CONFIG_SCHED_ATEXIT_MAX=1
+# CONFIG_SCHED_ONEXIT is not set
+CONFIG_USER_ENTRYPOINT="nsh_main"
+# CONFIG_DISABLE_OS_API is not set
+
+#
+# Signal Numbers
+#
+CONFIG_SIG_SIGUSR1=1
+CONFIG_SIG_SIGUSR2=2
+CONFIG_SIG_SIGALARM=3
+CONFIG_SIG_SIGCONDTIMEDOUT=16
+CONFIG_SIG_SIGWORK=4
+
+#
+# Sizes of configurable things (0 disables)
+#
+CONFIG_MAX_TASKS=32
+CONFIG_MAX_TASK_ARGS=10
+CONFIG_NPTHREAD_KEYS=4
+CONFIG_NFILE_DESCRIPTORS=42
+CONFIG_NFILE_STREAMS=8
+CONFIG_NAME_MAX=32
+CONFIG_PREALLOC_MQ_MSGS=4
+CONFIG_MQ_MAXMSGSIZE=32
+CONFIG_MAX_WDOGPARMS=2
+CONFIG_PREALLOC_WDOGS=50
+CONFIG_PREALLOC_TIMERS=50
+
+#
+# Stack and heap information
+#
+CONFIG_IDLETHREAD_STACKSIZE=1000
+CONFIG_USERMAIN_STACKSIZE=2500
+CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_PTHREAD_STACK_DEFAULT=2048
+
+#
+# Device Drivers
+#
+# CONFIG_DISABLE_POLL is not set
+CONFIG_DEV_NULL=y
+# CONFIG_DEV_ZERO is not set
+# CONFIG_LOOP is not set
+# CONFIG_RAMDISK is not set
+# CONFIG_CAN is not set
+# CONFIG_PWM is not set
+CONFIG_I2C=y
+# CONFIG_I2C_SLAVE is not set
+CONFIG_I2C_TRANSFER=y
+# CONFIG_I2C_WRITEREAD is not set
+# CONFIG_I2C_POLLED is not set
+# CONFIG_I2C_TRACE is not set
+CONFIG_ARCH_HAVE_I2CRESET=y
+CONFIG_I2C_RESET=y
+CONFIG_SPI=y
+# CONFIG_SPI_OWNBUS is not set
+CONFIG_SPI_EXCHANGE=y
+# CONFIG_SPI_CMDDATA is not set
+# CONFIG_RTC is not set
+CONFIG_WATCHDOG=y
+# CONFIG_ANALOG is not set
+# CONFIG_AUDIO_DEVICES is not set
+# CONFIG_BCH is not set
+# CONFIG_INPUT is not set
+# CONFIG_LCD is not set
+# CONFIG_MMCSD is not set
+# CONFIG_MTD is not set
+CONFIG_PIPES=y
+# CONFIG_PM is not set
+# CONFIG_POWER is not set
+# CONFIG_SENSORS is not set
+CONFIG_SERIAL=y
+# CONFIG_DEV_LOWCONSOLE is not set
+CONFIG_SERIAL_REMOVABLE=y
+# CONFIG_16550_UART is not set
+CONFIG_ARCH_HAVE_USART2=y
+CONFIG_ARCH_HAVE_USART3=y
+CONFIG_ARCH_HAVE_USART6=y
+CONFIG_MCU_SERIAL=y
+CONFIG_STANDARD_SERIAL=y
+CONFIG_SERIAL_NPOLLWAITERS=2
+# CONFIG_USART2_SERIAL_CONSOLE is not set
+CONFIG_USART3_SERIAL_CONSOLE=y
+# CONFIG_USART6_SERIAL_CONSOLE is not set
+# CONFIG_NO_SERIAL_CONSOLE is not set
+
+#
+# USART2 Configuration
+#
+CONFIG_USART2_RXBUFSIZE=300
+CONFIG_USART2_TXBUFSIZE=300
+CONFIG_USART2_BAUD=57600
+CONFIG_USART2_BITS=8
+CONFIG_USART2_PARITY=0
+CONFIG_USART2_2STOP=0
+# CONFIG_USART2_IFLOWCONTROL is not set
+# CONFIG_USART2_OFLOWCONTROL is not set
+
+#
+# USART3 Configuration
+#
+CONFIG_USART3_RXBUFSIZE=256
+CONFIG_USART3_TXBUFSIZE=256
+CONFIG_USART3_BAUD=115200
+CONFIG_USART3_BITS=8
+CONFIG_USART3_PARITY=0
+CONFIG_USART3_2STOP=0
+# CONFIG_USART3_IFLOWCONTROL is not set
+# CONFIG_USART3_OFLOWCONTROL is not set
+
+#
+# USART6 Configuration
+#
+CONFIG_USART6_RXBUFSIZE=128
+CONFIG_USART6_TXBUFSIZE=64
+CONFIG_USART6_BAUD=57600
+CONFIG_USART6_BITS=8
+CONFIG_USART6_PARITY=0
+CONFIG_USART6_2STOP=0
+# CONFIG_USART6_IFLOWCONTROL is not set
+# CONFIG_USART6_OFLOWCONTROL is not set
+# CONFIG_SERIAL_IFLOWCONTROL is not set
+# CONFIG_SERIAL_OFLOWCONTROL is not set
+CONFIG_USBDEV=y
+
+#
+# USB Device Controller Driver Options
+#
+# CONFIG_USBDEV_ISOCHRONOUS is not set
+# CONFIG_USBDEV_DUALSPEED is not set
+# CONFIG_USBDEV_SELFPOWERED is not set
+CONFIG_USBDEV_BUSPOWERED=y
+CONFIG_USBDEV_MAXPOWER=500
+# CONFIG_USBDEV_DMA is not set
+# CONFIG_USBDEV_TRACE is not set
+
+#
+# USB Device Class Driver Options
+#
+# CONFIG_USBDEV_COMPOSITE is not set
+# CONFIG_PL2303 is not set
+CONFIG_CDCACM=y
+# CONFIG_CDCACM_CONSOLE is not set
+CONFIG_CDCACM_EP0MAXPACKET=64
+CONFIG_CDCACM_EPINTIN=1
+CONFIG_CDCACM_EPINTIN_FSSIZE=64
+CONFIG_CDCACM_EPINTIN_HSSIZE=64
+CONFIG_CDCACM_EPBULKOUT=3
+CONFIG_CDCACM_EPBULKOUT_FSSIZE=64
+CONFIG_CDCACM_EPBULKOUT_HSSIZE=512
+CONFIG_CDCACM_EPBULKIN=2
+CONFIG_CDCACM_EPBULKIN_FSSIZE=64
+CONFIG_CDCACM_EPBULKIN_HSSIZE=512
+CONFIG_CDCACM_NWRREQS=4
+CONFIG_CDCACM_NRDREQS=4
+CONFIG_CDCACM_BULKIN_REQLEN=96
+CONFIG_CDCACM_RXBUFSIZE=300
+CONFIG_CDCACM_TXBUFSIZE=1000
+CONFIG_CDCACM_VENDORID=0x26ac
+CONFIG_CDCACM_PRODUCTID=0x0016
+CONFIG_CDCACM_VENDORSTR="3D Robotics"
+CONFIG_CDCACM_PRODUCTSTR="PX4 Crazyflie v2.0"
+# CONFIG_USBMSC is not set
+# CONFIG_USBHOST is not set
+# CONFIG_WIRELESS is not set
+
+#
+# System Logging Device Options
+#
+
+#
+# System Logging
+#
+# CONFIG_RAMLOG is not set
+
+#
+# Networking Support
+#
+# CONFIG_NET is not set
+
+#
+# File Systems
+#
+
+#
+# File system configuration
+#
+# CONFIG_DISABLE_MOUNTPOINT is not set
+# CONFIG_FS_RAMMAP is not set
+CONFIG_FS_FAT=y
+# CONFIG_FAT_LCNAMES is not set
+# CONFIG_FAT_LFN is not set
+# CONFIG_FS_FATTIME is not set
+# CONFIG_FAT_DMAMEMORY is not set
+CONFIG_FS_NXFFS=y
+CONFIG_NXFFS_PREALLOCATED=y
+CONFIG_NXFFS_ERASEDSTATE=0xff
+CONFIG_NXFFS_PACKTHRESHOLD=32
+CONFIG_NXFFS_MAXNAMLEN=32
+CONFIG_NXFFS_TAILTHRESHOLD=2048
+CONFIG_FS_ROMFS=y
+# CONFIG_FS_SMARTFS is not set
+CONFIG_FS_BINFS=y
+
+#
+# System Logging
+#
+# CONFIG_SYSLOG_ENABLE is not set
+# CONFIG_SYSLOG is not set
+
+#
+# Graphics Support
+#
+# CONFIG_NX is not set
+
+#
+# Memory Management
+#
+# CONFIG_MM_MULTIHEAP is not set
+# CONFIG_MM_SMALL is not set
+CONFIG_MM_REGIONS=2
+CONFIG_GRAN=y
+CONFIG_GRAN_SINGLE=y
+CONFIG_GRAN_INTR=y
+
+#
+# Audio Support
+#
+# CONFIG_AUDIO is not set
+
+#
+# Binary Formats
+#
+# CONFIG_BINFMT_DISABLE is not set
+# CONFIG_BINFMT_EXEPATH is not set
+# CONFIG_NXFLAT is not set
+# CONFIG_ELF is not set
+CONFIG_BUILTIN=y
+# CONFIG_PIC is not set
+# CONFIG_SYMTAB_ORDEREDBYNAME is not set
+
+#
+# Library Routines
+#
+
+#
+# Standard C Library Options
+#
+CONFIG_STDIO_BUFFER_SIZE=180
+CONFIG_STDIO_LINEBUFFER=y
+CONFIG_NUNGET_CHARS=2
+CONFIG_LIB_HOMEDIR="/"
+# CONFIG_NOPRINTF_FIELDWIDTH is not set
+CONFIG_LIBC_FLOATINGPOINT=y
+CONFIG_LIB_RAND_ORDER=1
+# CONFIG_EOL_IS_CR is not set
+# CONFIG_EOL_IS_LF is not set
+# CONFIG_EOL_IS_BOTH_CRLF is not set
+CONFIG_EOL_IS_EITHER_CRLF=y
+# CONFIG_LIBC_EXECFUNCS is not set
+CONFIG_POSIX_SPAWN_PROXY_STACKSIZE=1024
+CONFIG_TASK_SPAWN_DEFAULT_STACKSIZE=2048
+CONFIG_LIBC_STRERROR=y
+# CONFIG_LIBC_STRERROR_SHORT is not set
+# CONFIG_LIBC_PERROR_STDOUT is not set
+CONFIG_ARCH_LOWPUTC=y
+CONFIG_LIB_SENDFILE_BUFSIZE=512
+# CONFIG_ARCH_ROMGETC is not set
+CONFIG_ARCH_OPTIMIZED_FUNCTIONS=y
+CONFIG_ARCH_MEMCPY=y
+# CONFIG_ARCH_MEMCMP is not set
+# CONFIG_ARCH_MEMMOVE is not set
+# CONFIG_ARCH_MEMSET is not set
+# CONFIG_MEMSET_OPTSPEED is not set
+# CONFIG_ARCH_STRCHR is not set
+# CONFIG_ARCH_STRCMP is not set
+# CONFIG_ARCH_STRCPY is not set
+# CONFIG_ARCH_STRNCPY is not set
+# CONFIG_ARCH_STRLEN is not set
+# CONFIG_ARCH_STRNLEN is not set
+# CONFIG_ARCH_BZERO is not set
+
+#
+# Non-standard Library Support
+#
+CONFIG_SCHED_WORKQUEUE=y
+CONFIG_SCHED_HPWORK=y
+CONFIG_SCHED_WORKPRIORITY=192
+CONFIG_SCHED_WORKPERIOD=5000
+CONFIG_SCHED_WORKSTACKSIZE=1600
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKPERIOD=50000
+CONFIG_SCHED_LPWORKSTACKSIZE=1600
+# CONFIG_LIB_KBDCODEC is not set
+# CONFIG_LIB_SLCDCODEC is not set
+
+#
+# Basic CXX Support
+#
+CONFIG_C99_BOOL8=y
+CONFIG_HAVE_CXX=y
+CONFIG_HAVE_CXXINITIALIZE=y
+# CONFIG_CXX_NEWLONG is not set
+
+#
+# uClibc++ Standard C++ Library
+#
+# CONFIG_UCLIBCXX is not set
+
+#
+# Application Configuration
+#
+
+#
+# Built-In Applications
+#
+CONFIG_BUILTIN_PROXY_STACKSIZE=1024
+
+#
+# Examples
+#
+# CONFIG_EXAMPLES_BUTTONS is not set
+# CONFIG_EXAMPLES_CAN is not set
+CONFIG_EXAMPLES_CDCACM=y
+# CONFIG_EXAMPLES_COMPOSITE is not set
+# CONFIG_EXAMPLES_CXXTEST is not set
+# CONFIG_EXAMPLES_DHCPD is not set
+# CONFIG_EXAMPLES_ELF is not set
+# CONFIG_EXAMPLES_FTPC is not set
+# CONFIG_EXAMPLES_FTPD is not set
+# CONFIG_EXAMPLES_HELLO is not set
+# CONFIG_EXAMPLES_HELLOXX is not set
+# CONFIG_EXAMPLES_JSON is not set
+# CONFIG_EXAMPLES_HIDKBD is not set
+# CONFIG_EXAMPLES_KEYPADTEST is not set
+# CONFIG_EXAMPLES_IGMP is not set
+# CONFIG_EXAMPLES_LCDRW is not set
+# CONFIG_EXAMPLES_MM is not set
+# CONFIG_EXAMPLES_MODBUS is not set
+CONFIG_EXAMPLES_MOUNT=y
+# CONFIG_EXAMPLES_NRF24L01TERM is not set
+CONFIG_EXAMPLES_NSH=y
+# CONFIG_EXAMPLES_NULL is not set
+# CONFIG_EXAMPLES_NX is not set
+# CONFIG_EXAMPLES_NXCONSOLE is not set
+# CONFIG_EXAMPLES_NXFFS is not set
+# CONFIG_EXAMPLES_NXFLAT is not set
+# CONFIG_EXAMPLES_NXHELLO is not set
+# CONFIG_EXAMPLES_NXIMAGE is not set
+# CONFIG_EXAMPLES_NXLINES is not set
+# CONFIG_EXAMPLES_NXTEXT is not set
+# CONFIG_EXAMPLES_OSTEST is not set
+# CONFIG_EXAMPLES_PASHELLO is not set
+# CONFIG_EXAMPLES_PIPE is not set
+# CONFIG_EXAMPLES_POSIXSPAWN is not set
+# CONFIG_EXAMPLES_QENCODER is not set
+# CONFIG_EXAMPLES_RGMP is not set
+# CONFIG_EXAMPLES_ROMFS is not set
+# CONFIG_EXAMPLES_SENDMAIL is not set
+# CONFIG_EXAMPLES_SERLOOP is not set
+# CONFIG_EXAMPLES_SLCD is not set
+# CONFIG_EXAMPLES_SMART_TEST is not set
+# CONFIG_EXAMPLES_SMART is not set
+# CONFIG_EXAMPLES_TCPECHO is not set
+# CONFIG_EXAMPLES_TELNETD is not set
+# CONFIG_EXAMPLES_THTTPD is not set
+# CONFIG_EXAMPLES_TIFF is not set
+# CONFIG_EXAMPLES_TOUCHSCREEN is not set
+# CONFIG_EXAMPLES_UDP is not set
+# CONFIG_EXAMPLES_UIP is not set
+# CONFIG_EXAMPLES_USBSERIAL is not set
+# CONFIG_EXAMPLES_USBMSC is not set
+# CONFIG_EXAMPLES_USBTERM is not set
+# CONFIG_EXAMPLES_WATCHDOG is not set
+
+#
+# Graphics Support
+#
+# CONFIG_TIFF is not set
+
+#
+# Interpreters
+#
+# CONFIG_INTERPRETERS_FICL is not set
+# CONFIG_INTERPRETERS_PCODE is not set
+
+#
+# Network Utilities
+#
+
+#
+# Networking Utilities
+#
+# CONFIG_NETUTILS_CODECS is not set
+# CONFIG_NETUTILS_DHCPC is not set
+# CONFIG_NETUTILS_DHCPD is not set
+# CONFIG_NETUTILS_FTPC is not set
+# CONFIG_NETUTILS_FTPD is not set
+# CONFIG_NETUTILS_JSON is not set
+# CONFIG_NETUTILS_RESOLV is not set
+# CONFIG_NETUTILS_SMTP is not set
+# CONFIG_NETUTILS_TELNETD is not set
+# CONFIG_NETUTILS_TFTPC is not set
+# CONFIG_NETUTILS_THTTPD is not set
+# CONFIG_NETUTILS_UIPLIB is not set
+# CONFIG_NETUTILS_WEBCLIENT is not set
+
+#
+# FreeModBus
+#
+# CONFIG_MODBUS is not set
+
+#
+# NSH Library
+#
+CONFIG_NSH_LIBRARY=y
+CONFIG_NSH_BUILTIN_APPS=y
+
+#
+# Disable Individual commands
+#
+# CONFIG_NSH_DISABLE_CAT is not set
+# CONFIG_NSH_DISABLE_CD is not set
+# CONFIG_NSH_DISABLE_CP is not set
+# CONFIG_NSH_DISABLE_DD is not set
+# CONFIG_NSH_DISABLE_ECHO is not set
+# CONFIG_NSH_DISABLE_EXEC is not set
+# CONFIG_NSH_DISABLE_EXIT is not set
+# CONFIG_NSH_DISABLE_FREE is not set
+# CONFIG_NSH_DISABLE_GET is not set
+# CONFIG_NSH_DISABLE_HELP is not set
+# CONFIG_NSH_DISABLE_HEXDUMP is not set
+# CONFIG_NSH_DISABLE_IFCONFIG is not set
+# CONFIG_NSH_DISABLE_KILL is not set
+# CONFIG_NSH_DISABLE_LOSETUP is not set
+# CONFIG_NSH_DISABLE_LS is not set
+# CONFIG_NSH_DISABLE_MB is not set
+# CONFIG_NSH_DISABLE_MKDIR is not set
+# CONFIG_NSH_DISABLE_MKFATFS is not set
+# CONFIG_NSH_DISABLE_MKFIFO is not set
+# CONFIG_NSH_DISABLE_MKRD is not set
+# CONFIG_NSH_DISABLE_MH is not set
+# CONFIG_NSH_DISABLE_MOUNT is not set
+# CONFIG_NSH_DISABLE_MW is not set
+# CONFIG_NSH_DISABLE_NSFMOUNT is not set
+# CONFIG_NSH_DISABLE_PS is not set
+# CONFIG_NSH_DISABLE_PING is not set
+# CONFIG_NSH_DISABLE_PUT is not set
+# CONFIG_NSH_DISABLE_PWD is not set
+# CONFIG_NSH_DISABLE_RM is not set
+# CONFIG_NSH_DISABLE_RMDIR is not set
+# CONFIG_NSH_DISABLE_SET is not set
+# CONFIG_NSH_DISABLE_SH is not set
+# CONFIG_NSH_DISABLE_SLEEP is not set
+# CONFIG_NSH_DISABLE_TEST is not set
+# CONFIG_NSH_DISABLE_UMOUNT is not set
+# CONFIG_NSH_DISABLE_UNSET is not set
+# CONFIG_NSH_DISABLE_USLEEP is not set
+# CONFIG_NSH_DISABLE_WGET is not set
+# CONFIG_NSH_DISABLE_XD is not set
+
+#
+# Configure Command Options
+#
+# CONFIG_NSH_CMDOPT_DF_H is not set
+CONFIG_NSH_CODECS_BUFSIZE=128
+CONFIG_NSH_FILEIOSIZE=512
+CONFIG_NSH_STRERROR=y
+CONFIG_NSH_LINELEN=128
+CONFIG_NSH_MAXARGUMENTS=12
+CONFIG_NSH_NESTDEPTH=8
+# CONFIG_NSH_DISABLESCRIPT is not set
+# CONFIG_NSH_DISABLEBG is not set
+CONFIG_NSH_ROMFSETC=y
+# CONFIG_NSH_ROMFSRC is not set
+CONFIG_NSH_ROMFSMOUNTPT="/etc"
+CONFIG_NSH_INITSCRIPT="init.d/rcS"
+CONFIG_NSH_ROMFSDEVNO=0
+CONFIG_NSH_ROMFSSECTSIZE=128
+CONFIG_NSH_ARCHROMFS=y
+CONFIG_NSH_FATDEVNO=0
+CONFIG_NSH_FATSECTSIZE=512
+CONFIG_NSH_FATNSECTORS=1024
+CONFIG_NSH_FATMOUNTPT="/tmp"
+CONFIG_NSH_CONSOLE=y
+# CONFIG_NSH_USBCONSOLE is not set
+
+#
+# USB Trace Support
+#
+# CONFIG_NSH_CONDEV is not set
+CONFIG_NSH_ARCHINIT=y
+
+#
+# NxWidgets/NxWM
+#
+
+#
+# System NSH Add-Ons
+#
+
+#
+# Custom Free Memory Command
+#
+# CONFIG_SYSTEM_FREE is not set
+
+#
+# I2C tool
+#
+# CONFIG_SYSTEM_I2CTOOL is not set
+
+#
+# FLASH Program Installation
+#
+# CONFIG_SYSTEM_INSTALL is not set
+
+#
+# FLASH Erase-all Command
+#
+
+#
+# readline()
+#
+CONFIG_SYSTEM_READLINE=y
+CONFIG_READLINE_ECHO=y
+
+#
+# Power Off
+#
+# CONFIG_SYSTEM_POWEROFF is not set
+
+#
+# RAMTRON
+#
+# CONFIG_SYSTEM_RAMTRON is not set
+
+#
+# SD Card
+#
+# CONFIG_SYSTEM_SDCARD is not set
+
+#
+# Sysinfo
+#
+CONFIG_SYSTEM_SYSINFO=y
+
+#
+# USB Monitor
+#
diff --git a/nuttx-configs/crazyflie/nsh/setenv.sh b/nuttx-configs/crazyflie/nsh/setenv.sh
new file mode 100755
index 0000000000000000000000000000000000000000..0f31bdbb6d85540165b392efecf4c94b58488c63
--- /dev/null
+++ b/nuttx-configs/crazyflie/nsh/setenv.sh
@@ -0,0 +1,75 @@
+#!/bin/bash
+# configs/crazyflie/usbnsh/setenv.sh
+#
+# Copyright (C) 2013 Gregory Nutt. All rights reserved.
+# Author: Gregory Nutt <gnutt@nuttx.org>
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+# notice, this list of conditions and the following disclaimer in
+# the documentation and/or other materials provided with the
+# distribution.
+# 3. Neither the name NuttX nor the names of its contributors may be
+# used to endorse or promote products derived from this software
+# without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+
+if [ "$_" = "$0" ] ; then
+ echo "You must source this script, not run it!" 1>&2
+ exit 1
+fi
+
+WD=`pwd`
+if [ ! -x "setenv.sh" ]; then
+ echo "This script must be executed from the top-level NuttX build directory"
+ exit 1
+fi
+
+if [ -z "${PATH_ORIG}" ]; then
+ export PATH_ORIG="${PATH}"
+fi
+
+# This is the Cygwin path to the location where I installed the RIDE
+# toolchain under windows. You will also have to edit this if you install
+# the RIDE toolchain in any other location
+#export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/Raisonance/Ride/arm-gcc/bin"
+
+# This is the Cygwin path to the location where I installed the CodeSourcery
+# toolchain under windows. You will also have to edit this if you install
+# the CodeSourcery toolchain in any other location
+export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/CodeSourcery/Sourcery G++ Lite/bin"
+
+# These are the Cygwin paths to the locations where I installed the Atollic
+# toolchain under windows. You will also have to edit this if you install
+# the Atollic toolchain in any other location. /usr/bin is added before
+# the Atollic bin path because there is are binaries named gcc.exe and g++.exe
+# at those locations as well.
+#export TOOLCHAIN_BIN="/usr/bin:/cygdrive/c/Program Files (x86)/Atollic/TrueSTUDIO for ARM Pro 2.3.0/ARMTools/bin"
+#export TOOLCHAIN_BIN="/usr/bin:/cygdrive/c/Program Files (x86)/Atollic/TrueSTUDIO for STMicroelectronics STM32 Lite 2.3.0/ARMTools/bin"
+
+# This is the Cygwin path to the location where I build the buildroot
+# toolchain.
+#export TOOLCHAIN_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin"
+
+# Add the path to the toolchain to the PATH varialble
+export PATH="${TOOLCHAIN_BIN}:/sbin:/usr/sbin:${PATH_ORIG}"
+
+echo "PATH : ${PATH}"
diff --git a/nuttx-configs/crazyflie/scripts/ld.script b/nuttx-configs/crazyflie/scripts/ld.script
new file mode 100644
index 0000000000000000000000000000000000000000..7b15304567a7f28179e95f5ec1f151f69aededc9
--- /dev/null
+++ b/nuttx-configs/crazyflie/scripts/ld.script
@@ -0,0 +1,149 @@
+/****************************************************************************
+ * configs/crazyflie/scripts/ld.script
+ *
+ * Copyright (C) 2011 Gregory Nutt. All rights reserved.
+ * Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/* The STM32F405RG has 1024Kb of FLASH beginning at address 0x0800:0000 and
+ * 192Kb of SRAM. SRAM is split up into three blocks:
+ *
+ * 1) 112Kb of SRAM beginning at address 0x2000:0000
+ * 2) 16Kb of SRAM beginning at address 0x2001:c000
+ * 3) 64Kb of CCM SRAM beginning at address 0x1000:0000
+ *
+ * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000
+ * where the code expects to begin execution by jumping to the entry point in
+ * the 0x0800:0000 address range.
+ *
+ * The first 0x4000 of flash is reserved for the bootloader.
+ */
+
+MEMORY
+{
+ flash (rx) : ORIGIN = 0x08004000, LENGTH = 1008K
+ sram (rwx) : ORIGIN = 0x20000000, LENGTH = 128K
+ ccsram (rwx) : ORIGIN = 0x10000000, LENGTH = 64K
+}
+
+OUTPUT_ARCH(arm)
+
+ENTRY(__start) /* treat __start as the anchor for dead code stripping */
+EXTERN(_vectors) /* force the vectors to be included in the output */
+
+/*
+ * Ensure that abort() is present in the final object. The exception handling
+ * code pulled in by libgcc.a requires it (and that code cannot be easily avoided).
+ */
+EXTERN(abort)
+
+SECTIONS
+{
+ .text : {
+ _stext = ABSOLUTE(.);
+ *(.vectors)
+ *(.text .text.*)
+ *(.fixup)
+ *(.gnu.warning)
+ *(.rodata .rodata.*)
+ *(.gnu.linkonce.t.*)
+ *(.got)
+ *(.gcc_except_table)
+ *(.gnu.linkonce.r.*)
+ _etext = ABSOLUTE(.);
+
+ /*
+ * This is a hack to make the newlib libm __errno() call
+ * use the NuttX get_errno_ptr() function.
+ */
+ __errno = get_errno_ptr;
+ } > flash
+
+ /*
+ * Init functions (static constructors and the like)
+ */
+ .init_section : {
+ _sinit = ABSOLUTE(.);
+ KEEP(*(.init_array .init_array.*))
+ _einit = ABSOLUTE(.);
+ } > flash
+
+ /*
+ * Construction data for parameters.
+ */
+ __param ALIGN(4): {
+ __param_start = ABSOLUTE(.);
+ KEEP(*(__param*))
+ __param_end = ABSOLUTE(.);
+ } > flash
+
+ .ARM.extab : {
+ *(.ARM.extab*)
+ } > flash
+
+ __exidx_start = ABSOLUTE(.);
+ .ARM.exidx : {
+ *(.ARM.exidx*)
+ } > flash
+ __exidx_end = ABSOLUTE(.);
+
+ _eronly = ABSOLUTE(.);
+
+ .data : {
+ _sdata = ABSOLUTE(.);
+ *(.data .data.*)
+ *(.gnu.linkonce.d.*)
+ CONSTRUCTORS
+ _edata = ABSOLUTE(.);
+ } > sram AT > flash
+
+ .bss : {
+ _sbss = ABSOLUTE(.);
+ *(.bss .bss.*)
+ *(.gnu.linkonce.b.*)
+ *(COMMON)
+ _ebss = ABSOLUTE(.);
+ } > sram
+
+ /* Stabs debugging sections. */
+ .stab 0 : { *(.stab) }
+ .stabstr 0 : { *(.stabstr) }
+ .stab.excl 0 : { *(.stab.excl) }
+ .stab.exclstr 0 : { *(.stab.exclstr) }
+ .stab.index 0 : { *(.stab.index) }
+ .stab.indexstr 0 : { *(.stab.indexstr) }
+ .comment 0 : { *(.comment) }
+ .debug_abbrev 0 : { *(.debug_abbrev) }
+ .debug_info 0 : { *(.debug_info) }
+ .debug_line 0 : { *(.debug_line) }
+ .debug_pubnames 0 : { *(.debug_pubnames) }
+ .debug_aranges 0 : { *(.debug_aranges) }
+}
diff --git a/nuttx-configs/crazyflie/src/Makefile b/nuttx-configs/crazyflie/src/Makefile
new file mode 100644
index 0000000000000000000000000000000000000000..e6e7d2dd005fc398784283dfb5ea9839955edd7e
--- /dev/null
+++ b/nuttx-configs/crazyflie/src/Makefile
@@ -0,0 +1,84 @@
+############################################################################
+# configs/crazyflie/src/Makefile
+#
+# Copyright (C) 2013 Gregory Nutt. All rights reserved.
+# Author: Gregory Nutt <gnutt@nuttx.org>
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+# notice, this list of conditions and the following disclaimer in
+# the documentation and/or other materials provided with the
+# distribution.
+# 3. Neither the name NuttX nor the names of its contributors may be
+# used to endorse or promote products derived from this software
+# without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+-include $(TOPDIR)/Make.defs
+
+CFLAGS += -I$(TOPDIR)/sched
+
+ASRCS =
+AOBJS = $(ASRCS:.S=$(OBJEXT))
+
+CSRCS = empty.c
+COBJS = $(CSRCS:.c=$(OBJEXT))
+
+SRCS = $(ASRCS) $(CSRCS)
+OBJS = $(AOBJS) $(COBJS)
+
+ARCH_SRCDIR = $(TOPDIR)/arch/$(CONFIG_ARCH)/src
+ifeq ($(WINTOOL),y)
+ CFLAGS += -I "${shell cygpath -w $(ARCH_SRCDIR)/chip}" \
+ -I "${shell cygpath -w $(ARCH_SRCDIR)/common}" \
+ -I "${shell cygpath -w $(ARCH_SRCDIR)/armv7-m}"
+else
+ CFLAGS += -I$(ARCH_SRCDIR)/chip -I$(ARCH_SRCDIR)/common -I$(ARCH_SRCDIR)/armv7-m
+endif
+
+all: libboard$(LIBEXT)
+
+$(AOBJS): %$(OBJEXT): %.S
+ $(call ASSEMBLE, $<, $@)
+
+$(COBJS) $(LINKOBJS): %$(OBJEXT): %.c
+ $(call COMPILE, $<, $@)
+
+libboard$(LIBEXT): $(OBJS)
+ $(call ARCHIVE, $@, $(OBJS))
+
+.depend: Makefile $(SRCS)
+ $(Q) $(MKDEP) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep
+ $(Q) touch $@
+
+depend: .depend
+
+clean:
+ $(call DELFILE, libboard$(LIBEXT))
+ $(call CLEAN)
+
+distclean: clean
+ $(call DELFILE, Make.dep)
+ $(call DELFILE, .depend)
+
+-include Make.dep
+
diff --git a/nuttx-configs/crazyflie/src/empty.c b/nuttx-configs/crazyflie/src/empty.c
new file mode 100644
index 0000000000000000000000000000000000000000..5de10699fbb42872cc118f96f4ba065d31964233
--- /dev/null
+++ b/nuttx-configs/crazyflie/src/empty.c
@@ -0,0 +1,4 @@
+/*
+ * There are no source files here, but libboard.a can't be empty, so
+ * we have this empty source file to keep it company.
+ */
diff --git a/src/drivers/boards/aerocore/CMakeLists.txt b/src/drivers/boards/aerocore/CMakeLists.txt
index 23be262b755eeb4947551bf4da6e032649cdc9df..36d865a9da8d602cc502df178b43cb45873243d9 100644
--- a/src/drivers/boards/aerocore/CMakeLists.txt
+++ b/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 :
diff --git a/src/drivers/boards/crazyflie/CMakeLists.txt b/src/drivers/boards/crazyflie/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..c5d9913bb09b03517e76e17fa4c9dd8d9c27c954
--- /dev/null
+++ b/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 :
diff --git a/src/drivers/boards/crazyflie/board_config.h b/src/drivers/boards/crazyflie/board_config.h
new file mode 100644
index 0000000000000000000000000000000000000000..948956cddacaf1b626029ad1842396ce1d953701
--- /dev/null
+++ b/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
diff --git a/src/drivers/boards/crazyflie/crazyflie_i2c.cpp b/src/drivers/boards/crazyflie/crazyflie_i2c.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ad8ff063b63cf0a081f2539f17d514dc8c727c61
--- /dev/null
+++ b/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;
+}
diff --git a/src/drivers/boards/crazyflie/crazyflie_init.c b/src/drivers/boards/crazyflie/crazyflie_init.c
new file mode 100644
index 0000000000000000000000000000000000000000..fb999f6b1b0cd4264567e21db58a042247d18407
--- /dev/null
+++ b/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;
+}
diff --git a/src/drivers/boards/crazyflie/crazyflie_led.c b/src/drivers/boards/crazyflie/crazyflie_led.c
new file mode 100644
index 0000000000000000000000000000000000000000..b499f7b8e6984b2a7d0c24664b333425b68a358f
--- /dev/null
+++ b/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);
+ }
+ }
+}
diff --git a/src/drivers/boards/crazyflie/crazyflie_timer_config.c b/src/drivers/boards/crazyflie/crazyflie_timer_config.c
new file mode 100644
index 0000000000000000000000000000000000000000..4617a23a994d47eb2491bfb824abd6cd96babd2f
--- /dev/null
+++ b/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
+ }
+};
diff --git a/src/drivers/boards/crazyflie/crazyflie_usb.c b/src/drivers/boards/crazyflie/crazyflie_usb.c
new file mode 100644
index 0000000000000000000000000000000000000000..697b619a0aa4927f2a3fe87a1e3437f8dbab5623
--- /dev/null
+++ b/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);
+}
+
diff --git a/src/drivers/crazyflie/CMakeLists.txt b/src/drivers/crazyflie/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..5ce9e5beb49b423ce34cda5078223ee8eb23626c
--- /dev/null
+++ b/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 :
diff --git a/src/drivers/crazyflie/crazyflie.cpp b/src/drivers/crazyflie/crazyflie.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e9c9ef87f63448e143f75ee26c6c5089763848ef
--- /dev/null
+++ b/src/drivers/crazyflie/crazyflie.cpp
@@ -0,0 +1,1096 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2012-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 crazyflie.cpp
+ *
+ * Driver/configurator for the PX4 FMU multi-purpose port on v1 and v2 boards.
+ */
+
+#include <px4_config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <stdlib.h>
+#include <semaphore.h>
+#include <string.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <errno.h>
+#include <stdio.h>
+#include <math.h>
+#include <unistd.h>
+
+#include <nuttx/arch.h>
+
+#include <drivers/device/device.h>
+#include <drivers/device/i2c.h>
+#include <drivers/drv_pwm_output.h>
+#include <drivers/drv_gpio.h>
+#include <drivers/drv_hrt.h>
+
+#include <board_config.h>
+
+#include <systemlib/systemlib.h>
+#include <systemlib/err.h>
+#include <systemlib/mixer/mixer.h>
+#include <systemlib/pwm_limit/pwm_limit.h>
+#include <systemlib/board_serial.h>
+#include <systemlib/param/param.h>
+#include <drivers/drv_mixer.h>
+#include <drivers/drv_rc_input.h>
+
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_0.h>
+#include <uORB/topics/actuator_controls_1.h>
+#include <uORB/topics/actuator_controls_2.h>
+#include <uORB/topics/actuator_controls_3.h>
+#include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/parameter_update.h>
+
+
+#ifdef HRT_PPM_CHANNEL
+# include <systemlib/ppm_decode.h>
+#endif
+
+/*
+ * This is the analog to FMU_INPUT_DROP_LIMIT_US on the IO side
+ */
+
+#define CONTROL_INPUT_DROP_LIMIT_MS 20
+#define NAN_VALUE (0.0f/0.0f)
+
+#define CRAZYFLIE_DEVICE_PATH PWM_OUTPUT0_DEVICE_PATH
+
+class Crazyflie : public device::CDev
+{
+public:
+ Crazyflie();
+ virtual ~Crazyflie();
+
+ virtual int ioctl(file *filp, int cmd, unsigned long arg);
+ virtual ssize_t write(file *filp, const char *buffer, size_t len);
+
+ virtual int init();
+
+private:
+ static const unsigned _max_actuators = 4;
+
+ unsigned _current_update_rate;
+ struct work_s _work;
+ int _armed_sub;
+ int _param_sub;
+ orb_advert_t _outputs_pub;
+ int _class_instance;
+
+ volatile bool _initialized;
+ bool _servo_armed;
+ bool _pwm_on;
+
+ MixerGroup *_mixers;
+
+ uint32_t _groups_required;
+ uint32_t _groups_subscribed;
+ int _control_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
+ actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
+ orb_id_t _control_topics[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
+ pollfd _poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
+ unsigned _poll_fds_num;
+
+ static pwm_limit_t _pwm_limit;
+ static actuator_armed_s _armed;
+ uint16_t _failsafe_pwm[_max_actuators];
+ uint16_t _disarmed_pwm[_max_actuators];
+ uint16_t _min_pwm[_max_actuators];
+ uint16_t _max_pwm[_max_actuators];
+ uint16_t _reverse_pwm_mask;
+ unsigned _num_failsafe_set;
+ unsigned _num_disarmed_set;
+
+ static bool arm_nothrottle() { return (_armed.prearmed && !_armed.armed); }
+
+ static void cycle_trampoline(void *arg);
+
+ void cycle();
+ void work_start();
+ void work_stop();
+
+ static int control_callback(uintptr_t handle,
+ uint8_t control_group,
+ uint8_t control_index,
+ float &input);
+ void subscribe();
+ void publish_pwm_outputs(uint16_t *values, size_t numvalues);
+
+ void set_bounded_pwm(uint16_t &out, struct pwm_output_values *pwm, size_t i);
+
+ /* do not allow to copy due to ptr data members */
+ Crazyflie(const Crazyflie &);
+ Crazyflie operator=(const Crazyflie &);
+};
+
+pwm_limit_t Crazyflie::_pwm_limit;
+actuator_armed_s Crazyflie::_armed = {};
+
+namespace
+{
+
+Crazyflie *g_crazyflie;
+
+} // namespace
+
+#define CRAZYFLIE_PWM_MIN 0
+#define CRAZYFLIE_PWM_MAX 255
+
+Crazyflie::Crazyflie() :
+ CDev("crazyflie", CRAZYFLIE_DEVICE_PATH),
+ _current_update_rate(0),
+ _work{},
+ _armed_sub(-1),
+ _param_sub(-1),
+ _outputs_pub(nullptr),
+ _class_instance(0),
+ _initialized(false),
+ _servo_armed(false),
+ _pwm_on(false),
+ _mixers(nullptr),
+ _groups_required(0),
+ _groups_subscribed(0),
+ _control_subs{ -1},
+ _poll_fds_num(0),
+ _failsafe_pwm{0},
+ _disarmed_pwm{0},
+ _reverse_pwm_mask(0),
+ _num_failsafe_set(0),
+ _num_disarmed_set(0)
+{
+ for (unsigned i = 0; i < _max_actuators; i++) {
+ _min_pwm[i] = CRAZYFLIE_PWM_MIN;
+ _max_pwm[i] = CRAZYFLIE_PWM_MAX;
+ }
+
+ _control_topics[0] = ORB_ID(actuator_controls_0);
+ _control_topics[1] = ORB_ID(actuator_controls_1);
+ _control_topics[2] = ORB_ID(actuator_controls_2);
+ _control_topics[3] = ORB_ID(actuator_controls_3);
+
+ memset(_controls, 0, sizeof(_controls));
+ memset(_poll_fds, 0, sizeof(_poll_fds));
+
+ /* only enable this during development */
+ _debug_enabled = false;
+}
+
+Crazyflie::~Crazyflie()
+{
+ if (_initialized) {
+ /* tell the task we want it to go away */
+ work_stop();
+
+ int i = 10;
+
+ do {
+ /* wait 50ms - it should wake every 100ms or so worst-case */
+ usleep(50000);
+ i--;
+
+ } while (_initialized && i > 0);
+ }
+
+ /* clean up the alternate device node */
+ unregister_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH, _class_instance);
+
+ g_crazyflie = nullptr;
+}
+
+int
+Crazyflie::init()
+{
+ int ret;
+
+ ASSERT(!_initialized);
+
+ /* do regular cdev init */
+ ret = CDev::init();
+
+ if (ret != OK) {
+ return ret;
+ }
+
+ /* try to claim the generic PWM output device node as well - it's OK if we fail at this */
+ _class_instance = register_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH);
+
+ if (_class_instance == CLASS_DEVICE_PRIMARY) {
+ /* lets not be too verbose */
+ } else if (_class_instance < 0) {
+ warnx("FAILED registering class device");
+ }
+
+ work_start();
+
+ return OK;
+}
+
+void
+Crazyflie::subscribe()
+{
+ /* subscribe/unsubscribe to required actuator control groups */
+ uint32_t sub_groups = _groups_required & ~_groups_subscribed;
+ uint32_t unsub_groups = _groups_subscribed & ~_groups_required;
+ _poll_fds_num = 0;
+
+ for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+ if (sub_groups & (1 << i)) {
+ DEVICE_DEBUG("subscribe to actuator_controls_%d", i);
+ _control_subs[i] = orb_subscribe(_control_topics[i]);
+ }
+
+ if (unsub_groups & (1 << i)) {
+ DEVICE_DEBUG("unsubscribe from actuator_controls_%d", i);
+ ::close(_control_subs[i]);
+ _control_subs[i] = -1;
+ }
+
+ if (_control_subs[i] > 0) {
+ _poll_fds[_poll_fds_num].fd = _control_subs[i];
+ _poll_fds[_poll_fds_num].events = POLLIN;
+ _poll_fds_num++;
+ }
+ }
+}
+
+void
+Crazyflie::publish_pwm_outputs(uint16_t *values, size_t numvalues)
+{
+ actuator_outputs_s outputs;
+ outputs.noutputs = numvalues;
+ outputs.timestamp = hrt_absolute_time();
+
+ for (size_t i = 0; i < _max_actuators; ++i) {
+ outputs.output[i] = i < numvalues ? (float)values[i] : 0;
+ }
+
+ if (_outputs_pub == nullptr) {
+ int instance = -1;
+ _outputs_pub = orb_advertise_multi(ORB_ID(actuator_outputs), &outputs, &instance, ORB_PRIO_DEFAULT);
+
+ } else {
+ orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &outputs);
+ }
+}
+
+void
+Crazyflie::work_start()
+{
+ /* schedule a cycle to start things */
+ work_queue(HPWORK, &_work, (worker_t)&Crazyflie::cycle_trampoline, this, 0);
+}
+
+void
+Crazyflie::cycle_trampoline(void *arg)
+{
+ Crazyflie *dev = reinterpret_cast<Crazyflie *>(arg);
+
+ dev->cycle();
+}
+
+void
+Crazyflie::cycle()
+{
+ if (!_initialized) {
+ /* force a reset of the update rate */
+ _current_update_rate = 0;
+
+ _armed_sub = orb_subscribe(ORB_ID(actuator_armed));
+ _param_sub = orb_subscribe(ORB_ID(parameter_update));
+
+ /* initialize PWM limit lib */
+ pwm_limit_init(&_pwm_limit);
+
+ up_pwm_servo_init(0x0f);
+
+ _initialized = true;
+ }
+
+
+ if (_groups_subscribed != _groups_required) {
+ subscribe();
+ _groups_subscribed = _groups_required;
+ /* force setting update rate */
+ _current_update_rate = 0;
+ }
+
+ /*
+ * Adjust actuator topic update rate to keep up with
+ * the highest servo update rate configured.
+ *
+ * We always mix at max rate; some channels may update slower.
+ */
+ unsigned max_rate = 500;
+
+ if (_current_update_rate != max_rate) {
+ _current_update_rate = max_rate;
+ int update_rate_in_ms = int(1000 / _current_update_rate);
+
+ /* reject faster than 500 Hz updates */
+ if (update_rate_in_ms < 2) {
+ update_rate_in_ms = 2;
+ }
+
+ /* reject slower than 10 Hz updates */
+ if (update_rate_in_ms > 100) {
+ update_rate_in_ms = 100;
+ }
+
+ DEVICE_DEBUG("adjusted actuator update interval to %ums", update_rate_in_ms);
+
+ for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+ if (_control_subs[i] > 0) {
+ orb_set_interval(_control_subs[i], update_rate_in_ms);
+ }
+ }
+
+ // set to current max rate, even if we are actually checking slower/faster
+ _current_update_rate = max_rate;
+ }
+
+ /* check if anything updated */
+ int ret = ::poll(_poll_fds, _poll_fds_num, 0);
+
+ /* this would be bad... */
+ if (ret < 0) {
+ DEVICE_LOG("poll error %d", errno);
+
+ } else if (ret == 0) {
+ /* timeout: no control data, switch to failsafe values */
+// warnx("no PWM: failsafe");
+
+ } else {
+
+ /* get controls for required topics */
+ unsigned poll_id = 0;
+
+ for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+ if (_control_subs[i] > 0) {
+ if (_poll_fds[poll_id].revents & POLLIN) {
+ orb_copy(_control_topics[i], _control_subs[i], &_controls[i]);
+ }
+
+ poll_id++;
+ }
+ }
+
+ /* can we mix? */
+ if (_mixers != nullptr) {
+
+ size_t num_outputs = 4;
+
+ /* do mixing */
+ float outputs[_max_actuators];
+ num_outputs = _mixers->mix(outputs, num_outputs, NULL);
+
+ /* disable unused ports by setting their output to NaN */
+ for (size_t i = 0; i < sizeof(outputs) / sizeof(outputs[0]); i++) {
+ if (i >= num_outputs) {
+ outputs[i] = NAN_VALUE;
+ }
+ }
+
+ uint16_t pwm_limited[_max_actuators];
+
+ /* the PWM limit call takes care of out of band errors, NaN and constrains */
+ pwm_limit_calc(_servo_armed, arm_nothrottle(), num_outputs, _reverse_pwm_mask, _disarmed_pwm, _min_pwm, _max_pwm,
+ outputs, pwm_limited, &_pwm_limit);
+
+ /* output to the servos */
+ for (size_t i = 0; i < num_outputs; i++) {
+ up_pwm_servo_set(i, pwm_limited[i]);
+ }
+
+ publish_pwm_outputs(pwm_limited, num_outputs);
+ }
+ }
+
+ /* check arming state */
+ bool updated = false;
+ orb_check(_armed_sub, &updated);
+
+ if (updated) {
+ orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
+
+ /* update the armed status and check that we're not locked down */
+ bool set_armed = (_armed.armed || _armed.prearmed) && !_armed.lockdown;
+
+ if (_servo_armed != set_armed) {
+ _servo_armed = set_armed;
+ }
+
+ /* update PWM status if armed or if disarmed PWM values are set */
+ bool pwm_on = (set_armed || _num_disarmed_set > 0);
+
+ if (_pwm_on != pwm_on) {
+ _pwm_on = pwm_on;
+ up_pwm_servo_arm(pwm_on);
+ }
+ }
+
+ orb_check(_param_sub, &updated);
+
+ if (updated) {
+ parameter_update_s pupdate;
+ orb_copy(ORB_ID(parameter_update), _param_sub, &pupdate);
+ }
+
+ work_queue(HPWORK, &_work, (worker_t)&Crazyflie::cycle_trampoline, this, USEC2TICK(CONTROL_INPUT_DROP_LIMIT_MS * 1000));
+}
+
+void Crazyflie::work_stop()
+{
+ work_cancel(HPWORK, &_work);
+
+ for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+ if (_control_subs[i] > 0) {
+ ::close(_control_subs[i]);
+ _control_subs[i] = -1;
+ }
+ }
+
+ ::close(_armed_sub);
+ ::close(_param_sub);
+
+ /* make sure servos are off */
+ up_pwm_servo_deinit();
+
+ DEVICE_LOG("stopping");
+
+ /* note - someone else is responsible for restoring the GPIO config */
+
+ /* tell the dtor that we are exiting */
+ _initialized = false;
+}
+
+int
+Crazyflie::control_callback(uintptr_t handle,
+ uint8_t control_group,
+ uint8_t control_index,
+ float &input)
+{
+ const actuator_controls_s *controls = (actuator_controls_s *)handle;
+
+ input = controls[control_group].control[control_index];
+
+ /* limit control input */
+ if (input > 1.0f) {
+ input = 1.0f;
+
+ } else if (input < -1.0f) {
+ input = -1.0f;
+ }
+
+ /* motor spinup phase - lock throttle to zero */
+ if (_pwm_limit.state == PWM_LIMIT_STATE_RAMP) {
+ if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE &&
+ control_index == actuator_controls_s::INDEX_THROTTLE) {
+ /* limit the throttle output to zero during motor spinup,
+ * as the motors cannot follow any demand yet
+ */
+ input = 0.0f;
+ }
+ }
+
+ /* throttle not arming - mark throttle input as invalid */
+ if (arm_nothrottle()) {
+ if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE &&
+ control_index == actuator_controls_s::INDEX_THROTTLE) {
+ /* set the throttle to an invalid value */
+ input = NAN_VALUE;
+ }
+ }
+
+ return 0;
+}
+
+void
+Crazyflie::set_bounded_pwm(uint16_t &out, struct pwm_output_values *pwm, size_t i)
+{
+ if (pwm->values[i] > 255) {
+ out = 255;
+
+ } else {
+ out = pwm->values[i];
+ }
+}
+
+int
+Crazyflie::ioctl(file *filp, int cmd, unsigned long arg)
+{
+ int ret = OK;
+
+ lock();
+
+ switch (cmd) {
+ case PWM_SERVO_ARM:
+ up_pwm_servo_arm(true);
+ break;
+
+ case PWM_SERVO_SET_ARM_OK:
+ case PWM_SERVO_CLEAR_ARM_OK:
+ case PWM_SERVO_SET_FORCE_SAFETY_OFF:
+ case PWM_SERVO_SET_FORCE_SAFETY_ON:
+ // these are no-ops, as no safety switch
+ break;
+
+ case PWM_SERVO_DISARM:
+ up_pwm_servo_arm(false);
+ break;
+
+ case PWM_SERVO_GET_DEFAULT_UPDATE_RATE:
+ case PWM_SERVO_GET_UPDATE_RATE:
+ *(uint32_t *)arg = 500;
+ break;
+
+ case PWM_SERVO_SET_UPDATE_RATE:
+ case PWM_SERVO_SET_SELECT_UPDATE_RATE:
+ // these are no-ops, as we have a fixed update rate
+ break;
+
+ case PWM_SERVO_GET_SELECT_UPDATE_RATE:
+ *(uint32_t *)arg = (1 << 4) - 1;
+ break;
+
+ case PWM_SERVO_SET_FAILSAFE_PWM: {
+ struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+
+ /* discard if too many values are sent */
+ if (pwm->channel_count > _max_actuators) {
+ ret = -EINVAL;
+ break;
+ }
+
+ for (unsigned i = 0; i < pwm->channel_count; i++) {
+ set_bounded_pwm(_failsafe_pwm[i], pwm, i);
+ }
+
+ /*
+ * update the counter
+ * this is needed to decide if disarmed PWM output should be turned on or not
+ */
+ _num_failsafe_set = 0;
+
+ for (unsigned i = 0; i < _max_actuators; i++) {
+ if (_failsafe_pwm[i] > 0) {
+ _num_failsafe_set++;
+ }
+ }
+
+ break;
+ }
+
+ case PWM_SERVO_GET_FAILSAFE_PWM: {
+ struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+
+ for (unsigned i = 0; i < _max_actuators; i++) {
+ pwm->values[i] = _failsafe_pwm[i];
+ }
+
+ pwm->channel_count = _max_actuators;
+ break;
+ }
+
+ case PWM_SERVO_SET_DISARMED_PWM: {
+ struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+
+ /* discard if too many values are sent */
+ if (pwm->channel_count > _max_actuators) {
+ ret = -EINVAL;
+ break;
+ }
+
+ for (unsigned i = 0; i < pwm->channel_count; i++) {
+ set_bounded_pwm(_disarmed_pwm[i], pwm, i);
+ }
+
+ /*
+ * update the counter
+ * this is needed to decide if disarmed PWM output should be turned on or not
+ */
+ _num_disarmed_set = 0;
+
+ for (unsigned i = 0; i < _max_actuators; i++) {
+ if (_disarmed_pwm[i] > 0) {
+ _num_disarmed_set++;
+ }
+ }
+
+ break;
+ }
+
+ case PWM_SERVO_GET_DISARMED_PWM: {
+ struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+
+ for (unsigned i = 0; i < _max_actuators; i++) {
+ pwm->values[i] = _disarmed_pwm[i];
+ }
+
+ pwm->channel_count = _max_actuators;
+ break;
+ }
+
+ case PWM_SERVO_SET_MIN_PWM: {
+ struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+
+ /* discard if too many values are sent */
+ if (pwm->channel_count > _max_actuators) {
+ ret = -EINVAL;
+ break;
+ }
+
+ for (unsigned i = 0; i < pwm->channel_count; i++) {
+ set_bounded_pwm(_min_pwm[i], pwm, i);
+ }
+
+ break;
+ }
+
+ case PWM_SERVO_GET_MIN_PWM: {
+ struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+
+ for (unsigned i = 0; i < _max_actuators; i++) {
+ pwm->values[i] = _min_pwm[i];
+ }
+
+ pwm->channel_count = _max_actuators;
+ arg = (unsigned long)&pwm;
+ break;
+ }
+
+ case PWM_SERVO_SET_MAX_PWM: {
+ struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+
+ /* discard if too many values are sent */
+ if (pwm->channel_count > _max_actuators) {
+ ret = -EINVAL;
+ break;
+ }
+
+ for (unsigned i = 0; i < pwm->channel_count; i++) {
+ set_bounded_pwm(_max_pwm[i], pwm, i);
+ }
+
+ break;
+ }
+
+ case PWM_SERVO_GET_MAX_PWM: {
+ struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+
+ for (unsigned i = 0; i < _max_actuators; i++) {
+ pwm->values[i] = _max_pwm[i];
+ }
+
+ pwm->channel_count = _max_actuators;
+ arg = (unsigned long)&pwm;
+ break;
+ }
+
+ case PWM_SERVO_SET(3):
+ case PWM_SERVO_SET(2):
+ case PWM_SERVO_SET(1):
+ case PWM_SERVO_SET(0):
+ if (arg <= 255) {
+ up_pwm_servo_set(cmd - PWM_SERVO_SET(0), arg);
+
+ } else {
+ ret = -EINVAL;
+ }
+
+ break;
+
+ case PWM_SERVO_GET(3):
+ case PWM_SERVO_GET(2):
+ case PWM_SERVO_GET(1):
+ case PWM_SERVO_GET(0):
+ *(servo_position_t *)arg = up_pwm_servo_get(cmd - PWM_SERVO_GET(0));
+ break;
+
+ case PWM_SERVO_GET_RATEGROUP(0):
+ case PWM_SERVO_GET_RATEGROUP(1):
+ case PWM_SERVO_GET_RATEGROUP(2):
+ case PWM_SERVO_GET_RATEGROUP(3):
+ case PWM_SERVO_GET_RATEGROUP(4):
+ case PWM_SERVO_GET_RATEGROUP(5):
+ *(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0));
+ break;
+
+ case PWM_SERVO_GET_COUNT:
+ case MIXERIOCGETOUTPUTCOUNT:
+ *(unsigned *)arg = 4;
+ break;
+
+ case PWM_SERVO_SET_COUNT:
+ ret = -EINVAL;
+
+ case MIXERIOCRESET:
+ if (_mixers != nullptr) {
+ delete _mixers;
+ _mixers = nullptr;
+ _groups_required = 0;
+ }
+
+ break;
+
+ case MIXERIOCADDSIMPLE: {
+ mixer_simple_s *mixinfo = (mixer_simple_s *)arg;
+
+ SimpleMixer *mixer = new SimpleMixer(control_callback,
+ (uintptr_t)_controls, mixinfo);
+
+ if (mixer->check()) {
+ delete mixer;
+ _groups_required = 0;
+ ret = -EINVAL;
+
+ } else {
+ if (_mixers == nullptr)
+ _mixers = new MixerGroup(control_callback,
+ (uintptr_t)_controls);
+
+ _mixers->add_mixer(mixer);
+ _mixers->groups_required(_groups_required);
+ }
+
+ break;
+ }
+
+ case MIXERIOCLOADBUF: {
+ const char *buf = (const char *)arg;
+ unsigned buflen = strnlen(buf, 1024);
+
+ if (_mixers == nullptr) {
+ _mixers = new MixerGroup(control_callback, (uintptr_t)_controls);
+ }
+
+ if (_mixers == nullptr) {
+ _groups_required = 0;
+ ret = -ENOMEM;
+
+ } else {
+
+ ret = _mixers->load_from_buf(buf, buflen);
+
+ if (ret != 0) {
+ DEVICE_DEBUG("mixer load failed with %d", ret);
+ delete _mixers;
+ _mixers = nullptr;
+ _groups_required = 0;
+ ret = -EINVAL;
+
+ } else {
+
+ _mixers->groups_required(_groups_required);
+ }
+ }
+
+ break;
+ }
+
+ default:
+ ret = -ENOTTY;
+ break;
+ }
+
+ unlock();
+
+ return ret;
+}
+
+/*
+ this implements PWM output via a write() method, for compatibility
+ with px4io
+ */
+ssize_t
+Crazyflie::write(file *filp, const char *buffer, size_t len)
+{
+ unsigned count = len / 2;
+ uint16_t values[4];
+
+ if (count > 4) {
+ // we have at most 4 outputs
+ count = 4;
+ }
+
+ // allow for misaligned values
+ memcpy(values, buffer, count * 2);
+
+ for (uint8_t i = 0; i < count; i++) {
+ if (values[i] != PWM_IGNORE_THIS_CHANNEL) {
+ up_pwm_servo_set(i, values[i]);
+ }
+ }
+
+ return count * 2;
+}
+
+namespace
+{
+int crazyflie_start(void);
+int crazyflie_stop(void);
+void test(void);
+void fake(int argc, char *argv[]);
+
+int
+crazyflie_start(void)
+{
+ int ret = OK;
+
+ if (g_crazyflie == nullptr) {
+
+ g_crazyflie = new Crazyflie;
+
+ if (g_crazyflie == nullptr) {
+ ret = -ENOMEM;
+
+ } else {
+ ret = g_crazyflie->init();
+
+ if (ret != OK) {
+ delete g_crazyflie;
+ g_crazyflie = nullptr;
+ }
+ }
+ }
+
+ return ret;
+}
+
+int
+crazyflie_stop(void)
+{
+ int ret = OK;
+
+ if (g_crazyflie != nullptr) {
+
+ delete g_crazyflie;
+ g_crazyflie = nullptr;
+ }
+
+ return ret;
+}
+
+void
+test(void)
+{
+ int fd;
+ unsigned servo_count = 0;
+ unsigned pwm_value = 1;
+ int direction = 1;
+ int ret;
+
+ fd = open(CRAZYFLIE_DEVICE_PATH, O_RDWR);
+
+ if (fd < 0) {
+ errx(1, "open fail");
+ }
+
+ if (ioctl(fd, PWM_SERVO_ARM, 0) < 0) { err(1, "servo arm failed"); }
+
+ if (ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count) != 0) {
+ err(1, "Unable to get servo count\n");
+ }
+
+ warnx("Testing %u motors", (unsigned)servo_count);
+
+ struct pollfd fds;
+ fds.fd = 0; /* stdin */
+ fds.events = POLLIN;
+
+ warnx("Press CTRL-C or 'c' to abort.");
+
+ for (;;) {
+ /* sweep all servos between 0..20 */
+ servo_position_t servos[servo_count];
+
+ for (unsigned i = 0; i < servo_count; i++) {
+ servos[i] = pwm_value;
+ }
+
+ if (direction == 1) {
+ // use ioctl interface for one direction
+ for (unsigned i = 0; i < servo_count; i++) {
+ if (ioctl(fd, PWM_SERVO_SET(i), servos[i]) < 0) {
+ err(1, "servo %u set failed", i);
+ }
+ }
+
+ } else {
+ // and use write interface for the other direction
+ ret = write(fd, servos, sizeof(servos));
+
+ if (ret != (int)sizeof(servos)) {
+ err(1, "error writing PWM servo data, wrote %u got %d", sizeof(servos), ret);
+ }
+ }
+
+ if (direction > 0) {
+ if (pwm_value < 20) {
+ pwm_value++;
+
+ } else {
+ direction = -1;
+ }
+
+ } else {
+ if (pwm_value > 1) {
+ pwm_value--;
+
+ } else {
+ direction = 1;
+ }
+ }
+
+ /* readback servo values */
+ for (unsigned i = 0; i < servo_count; i++) {
+ servo_position_t value;
+
+ if (ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&value)) {
+ err(1, "error reading PWM servo %d", i);
+ }
+
+ if (value != servos[i]) {
+ errx(1, "servo %d readback error, got %u expected %u", i, value, servos[i]);
+ }
+ }
+
+ /* Check if user wants to quit */
+ char c;
+ ret = poll(&fds, 1, 0);
+
+ if (ret > 0) {
+
+ read(0, &c, 1);
+
+ if (c == 0x03 || c == 0x63 || c == 'q') {
+ warnx("User abort\n");
+ break;
+ }
+ }
+
+ usleep(50000);
+ }
+
+ /* zero motors */
+ servo_position_t servos[servo_count];
+
+ for (unsigned i = 0; i < servo_count; i++) {
+ servos[i] = 0;
+ }
+
+ for (unsigned i = 0; i < servo_count; i++) {
+ if (ioctl(fd, PWM_SERVO_SET(i), servos[i]) < 0) {
+ err(1, "servo %u set failed", i);
+ }
+ }
+
+ close(fd);
+
+ exit(0);
+}
+
+void
+fake(int argc, char *argv[])
+{
+ if (argc < 5) {
+ errx(1, "crazyflie fake <roll> <pitch> <yaw> <thrust> (values -100 .. 100)");
+ }
+
+ actuator_controls_s ac;
+
+ ac.control[0] = strtol(argv[1], 0, 0) / 100.0f;
+
+ ac.control[1] = strtol(argv[2], 0, 0) / 100.0f;
+
+ ac.control[2] = strtol(argv[3], 0, 0) / 100.0f;
+
+ ac.control[3] = strtol(argv[4], 0, 0) / 100.0f;
+
+ orb_advert_t handle = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &ac);
+
+ if (handle == nullptr) {
+ errx(1, "advertise failed");
+ }
+
+ actuator_armed_s aa;
+
+ aa.armed = true;
+ aa.lockdown = false;
+
+ handle = orb_advertise(ORB_ID(actuator_armed), &aa);
+
+ if (handle == nullptr) {
+ errx(1, "advertise failed 2");
+ }
+
+ exit(0);
+}
+
+} // namespace
+
+extern "C" __EXPORT int crazyflie_main(int argc, char *argv[]);
+
+int
+crazyflie_main(int argc, char *argv[])
+{
+ const char *verb = argv[1];
+
+ if (!strcmp(verb, "stop")) {
+ crazyflie_stop();
+ errx(0, "Crazyflie driver stopped");
+ }
+
+ if (crazyflie_start() != OK) {
+ errx(1, "failed to start the Crazyflie driver");
+ }
+
+ if (!strcmp(verb, "test")) {
+ test();
+ }
+
+ if (!strcmp(verb, "fake")) {
+ fake(argc - 1, argv + 1);
+ }
+
+ fprintf(stderr, "Crazyflie: unrecognised command %s, try:\n", verb);
+ fprintf(stderr, " test, fake\n");
+ exit(1);
+}
diff --git a/src/drivers/drv_gpio.h b/src/drivers/drv_gpio.h
index 27ae59bd02ad603b3eec2c3f725dba90a720d9fc..f296bd8eb9fbf44aad244a6a665a2a1b540fcd86 100644
--- a/src/drivers/drv_gpio.h
+++ b/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
/*
diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h
index 5ed925f866adfd451a849c855c2218bdf8868548..b66e60c06d5b8d4394b0ca13156aceb28bbb6cfa 100644
--- a/src/drivers/drv_sensor.h
+++ b/src/drivers/drv_sensor.h
@@ -136,4 +136,3 @@
#define SENSORIOCCALTEST _SENSORIOC(7)
#endif /* _DRV_SENSOR_H */
-
diff --git a/src/drivers/lps25h/CMakeLists.txt b/src/drivers/lps25h/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..1eb87366e5d37b2cc1a12e5130feec673be6fe7a
--- /dev/null
+++ b/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 :
diff --git a/src/drivers/lps25h/lps25h.cpp b/src/drivers/lps25h/lps25h.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d66b0e8a4b61a059bbc4d20141fac5fa13a588ff
--- /dev/null
+++ b/src/drivers/lps25h/lps25h.cpp
@@ -0,0 +1,1402 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2012-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.cpp
+ *
+ * Driver for the LPS25H magnetometer connected via I2C or SPI.
+ */
+
+#include <px4_config.h>
+
+#include <drivers/device/i2c.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <semaphore.h>
+#include <string.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <errno.h>
+#include <stdio.h>
+#include <math.h>
+#include <unistd.h>
+
+#include <nuttx/arch.h>
+#include <nuttx/wqueue.h>
+#include <nuttx/clock.h>
+
+#include <board_config.h>
+
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+
+#include <drivers/drv_baro.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/device/ringbuffer.h>
+#include <drivers/drv_device.h>
+
+#include <uORB/uORB.h>
+
+#include <float.h>
+#include <getopt.h>
+
+#include "lps25h.h"
+
+/*
+ * LPS25H internal constants and data structures.
+ */
+
+/* Max measurement rate is 25Hz */
+#define LPS25H_CONVERSION_INTERVAL (1000000 / 25) /* microseconds */
+
+#define ADDR_REF_P_XL 0x08
+#define ADDR_REF_P_L 0x09
+#define ADDR_REF_P_H 0x0A
+#define ADDR_WHO_AM_I 0x0F
+#define ADDR_RES_CONF 0x10
+#define ADDR_CTRL_REG1 0x20
+#define ADDR_CTRL_REG2 0x21
+#define ADDR_CTRL_REG3 0x22
+#define ADDR_CTRL_REG4 0x23
+#define ADDR_INT_CFG 0x24
+#define ADDR_INT_SOURCE 0x25
+
+#define ADDR_STATUS_REG 0x27
+#define ADDR_P_OUT_XL 0x28
+#define ADDR_P_OUT_L 0x29
+#define ADDR_P_OUT_H 0x2A
+#define ADDR_TEMP_OUT_L 0x2B
+#define ADDR_TEMP_OUT_H 0x2C
+
+#define ADDR_FIFO_CTRL 0x2E
+#define ADDR_FIFO_STATUS 0x2F
+#define ADDR_THS_P_L 0x30
+#define ADDR_THS_P_H 0x31
+
+#define ADDR_RPDS_L 0x39
+#define ADDR_RPDS_H 0x3A
+
+/* Data sheet is ambigious if AVGT or AVGP is first */
+#define RES_CONF_AVGT_8 0x00
+#define RES_CONF_AVGT_32 0x01
+#define RES_CONF_AVGT_128 0x02
+#define RES_CONF_AVGT_512 0x03
+#define RES_CONF_AVGP_8 0x00
+#define RES_CONF_AVGP_32 0x04
+#define RES_CONF_AVGP_128 0x08
+#define RES_CONF_AVGP_512 0x0C
+
+#define CTRL_REG1_SIM (1 << 0)
+#define CTRL_REG1_RESET_AZ (1 << 1)
+#define CTRL_REG1_BDU (1 << 2)
+#define CTRL_REG1_DIFF_EN (1 << 3)
+#define CTRL_REG1_PD (1 << 7)
+#define CTRL_REG1_ODR_SINGLE (0 << 4)
+#define CTRL_REG1_ODR_1HZ (1 << 4)
+#define CTRL_REG1_ODR_7HZ (2 << 4)
+#define CTRL_REG1_ODR_12HZ5 (3 << 4)
+#define CTRL_REG1_ODR_25HZ (4 << 4)
+
+#define CTRL_REG2_ONE_SHOT (1 << 0)
+#define CTRL_REG2_AUTO_ZERO (1 << 1)
+#define CTRL_REG2_SWRESET (1 << 2)
+#define CTRL_REG2_FIFO_MEAN_DEC (1 << 4)
+#define CTRL_REG2_WTM_EN (1 << 5)
+#define CTRL_REG2_FIFO_EN (1 << 6)
+#define CTRL_REG2_BOOT (1 << 7)
+
+#define CTRL_REG3_INT1_S_DATA 0x0
+#define CTRL_REG3_INT1_S_P_HIGH 0x1
+#define CTRL_REG3_INT1_S_P_LOW 0x2
+#define CTRL_REG3_INT1_S_P_LIM 0x3
+#define CTRL_REG3_PP_OD (1 << 6)
+#define CTRL_REG3_INT_H_L (1 << 7)
+
+#define CTRL_REG4_P1_DRDY (1 << 0)
+#define CTRL_REG4_P1_OVERRUN (1 << 1)
+#define CTRL_REG4_P1_WTM (1 << 2)
+#define CTRL_REG4_P1_EMPTY (1 << 3)
+
+#define INTERRUPT_CFG_PH_E (1 << 0)
+#define INTERRUPT_CFG_PL_E (1 << 1)
+#define INTERRUPT_CFG_LIR (1 << 2)
+
+#define INT_SOURCE_PH (1 << 0)
+#define INT_SOURCE_PL (1 << 1)
+#define INT_SOURCE_IA (1 << 2)
+
+#define STATUS_REG_T_DA (1 << 0)
+#define STATUS_REG_P_DA (1 << 1)
+#define STATUS_REG_T_OR (1 << 4)
+#define STATUS_REG_P_OR (1 << 5)
+
+#define FIFO_CTRL_WTM_FMEAN_2 0x01
+#define FIFO_CTRL_WTM_FMEAN_4 0x03
+#define FIFO_CTRL_WTM_FMEAN_8 0x07
+#define FIFO_CTRL_WTM_FMEAN_16 0x0F
+#define FIFO_CTRL_WTM_FMEAN_32 0x1F
+#define FIFO_CTRL_F_MODE_BYPASS (0x0 << 5)
+#define FIFO_CTRL_F_MODE_FIFO (0x1 << 5)
+#define FIFO_CTRL_F_MODE_STREAM (0x2 << 5)
+#define FIFO_CTRL_F_MODE_SFIFO (0x3 << 5)
+#define FIFO_CTRL_F_MODE_BSTRM (0x4 << 5)
+#define FIFO_CTRL_F_MODE_FMEAN (0x6 << 5)
+#define FIFO_CTRL_F_MODE_BFIFO (0x7 << 5)
+
+#define FIFO_STATUS_EMPTY (1 << 5)
+#define FIFO_STATUS_FULL (1 << 6)
+#define FIFO_STATUS_WTM (1 << 7)
+
+enum LPS25H_BUS {
+ LPS25H_BUS_ALL = 0,
+ LPS25H_BUS_I2C_INTERNAL,
+ LPS25H_BUS_I2C_EXTERNAL,
+ LPS25H_BUS_SPI
+};
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+#ifndef CONFIG_SCHED_WORKQUEUE
+# error This requires CONFIG_SCHED_WORKQUEUE.
+#endif
+
+class LPS25H : public device::CDev
+{
+public:
+ LPS25H(device::Device *interface, const char *path);
+ virtual ~LPS25H();
+
+ virtual int init();
+
+ virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
+ virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
+
+ /**
+ * Diagnostics - print some basic information about the driver.
+ */
+ void print_info();
+
+protected:
+ Device *_interface;
+
+private:
+ work_s _work;
+ unsigned _measure_ticks;
+
+ ringbuffer::RingBuffer *_reports;
+ float _range_scale;
+ float _range_pa;
+ bool _collect_phase;
+ int _class_instance;
+ int _orb_class_instance;
+
+ orb_advert_t _baro_topic;
+
+ perf_counter_t _sample_perf;
+ perf_counter_t _comms_errors;
+ perf_counter_t _buffer_overflows;
+ perf_counter_t _range_errors;
+ perf_counter_t _conf_errors;
+
+ /* status reporting */
+ bool _sensor_ok; /**< sensor was found and reports ok */
+ bool _calibrated; /**< the calibration is valid */
+
+ struct baro_report _last_report; /**< used for info() */
+
+ uint8_t _conf_reg;
+ uint8_t _temperature_counter;
+ uint8_t _temperature_error_count;
+
+ /**
+ * Initialise the automatic measurement state machine and start it.
+ *
+ * @note This function is called at open and error time. It might make sense
+ * to make it more aggressive about resetting the bus in case of errors.
+ */
+ void start();
+
+ /**
+ * Stop the automatic measurement state machine.
+ */
+ void stop();
+
+ /**
+ * Reset the device
+ */
+ int reset();
+
+ /**
+ * Perform the on-sensor scale calibration routine.
+ *
+ * @note The sensor will continue to provide measurements, these
+ * will however reflect the uncalibrated sensor state until
+ * the calibration routine has been completed.
+ *
+ * @param enable set to 1 to enable self-test strap, 0 to disable
+ */
+ int calibrate(struct file *filp, unsigned enable);
+
+ /**
+ * check the sensor configuration.
+ *
+ * checks that the config of the sensor is correctly set, to
+ * cope with communication errors causing the configuration to
+ * change
+ */
+ void check_conf(void);
+
+ /**
+ * Perform a poll cycle; collect from the previous measurement
+ * and start a new one.
+ *
+ * This is the heart of the measurement state machine. This function
+ * alternately starts a measurement, or collects the data from the
+ * previous measurement.
+ *
+ * When the interval between measurements is greater than the minimum
+ * measurement interval, a gap is inserted between collection
+ * and measurement to provide the most recent measurement possible
+ * at the next interval.
+ */
+ void cycle();
+
+ /**
+ * Static trampoline from the workq context; because we don't have a
+ * generic workq wrapper yet.
+ *
+ * @param arg Instance pointer for the driver that is polling.
+ */
+ static void cycle_trampoline(void *arg);
+
+ /**
+ * Write a register.
+ *
+ * @param reg The register to write.
+ * @param val The value to write.
+ * @return OK on write success.
+ */
+ int write_reg(uint8_t reg, uint8_t val);
+
+ /**
+ * Read a register.
+ *
+ * @param reg The register to read.
+ * @param val The value read.
+ * @return OK on read success.
+ */
+ int read_reg(uint8_t reg, uint8_t &val);
+
+ /**
+ * Issue a measurement command.
+ *
+ * @return OK if the measurement command was successful.
+ */
+ int measure();
+
+ /**
+ * Collect the result of the most recent measurement.
+ */
+ int collect();
+
+ /**
+ * Convert a big-endian signed 16-bit value to a float.
+ *
+ * @param in A signed 16-bit big-endian value.
+ * @return The floating-point representation of the value.
+ */
+ float meas_to_float(uint8_t in[2]);
+
+ /**
+ * Check the current calibration and update device status
+ *
+ * @return 0 if calibration is ok, 1 else
+ */
+ int check_calibration();
+
+ /**
+ * Check the current scale calibration
+ *
+ * @return 0 if scale calibration is ok, 1 else
+ */
+ int check_scale();
+
+ /**
+ * Check the current offset calibration
+ *
+ * @return 0 if offset calibration is ok, 1 else
+ */
+ int check_offset();
+
+ /* this class has pointer data members, do not allow copying it */
+ LPS25H(const LPS25H &);
+ LPS25H operator=(const LPS25H &);
+};
+
+/*
+ * Driver 'main' command.
+ */
+extern "C" __EXPORT int lps25h_main(int argc, char *argv[]);
+
+
+LPS25H::LPS25H(device::Device *interface, const char *path) :
+ CDev("LPS25H", path),
+ _interface(interface),
+ _work{},
+ _measure_ticks(0),
+ _reports(nullptr),
+ _range_scale(0.00149536132f), /* default range scale from counts to gauss */
+ _range_pa(49.f),
+ _collect_phase(false),
+ _class_instance(-1),
+ _orb_class_instance(-1),
+ _baro_topic(nullptr),
+ _sample_perf(perf_alloc(PC_ELAPSED, "lps25h_read")),
+ _comms_errors(perf_alloc(PC_COUNT, "lps25h_comms_errors")),
+ _buffer_overflows(perf_alloc(PC_COUNT, "lps25h_buffer_overflows")),
+ _range_errors(perf_alloc(PC_COUNT, "lps25h_range_errors")),
+ _conf_errors(perf_alloc(PC_COUNT, "lps25h_conf_errors")),
+ _sensor_ok(false),
+ _calibrated(false),
+ _last_report{0},
+ _conf_reg(0),
+ _temperature_counter(0),
+ _temperature_error_count(0)
+{
+ // enable debug() calls
+ _debug_enabled = false;
+
+ // work_cancel in the dtor will explode if we don't do this...
+ memset(&_work, 0, sizeof(_work));
+}
+
+LPS25H::~LPS25H()
+{
+ /* make sure we are truly inactive */
+ stop();
+
+ if (_reports != nullptr) {
+ delete _reports;
+ }
+
+ if (_class_instance != -1) {
+ unregister_class_devname(BARO_BASE_DEVICE_PATH, _class_instance);
+ }
+
+ // free perf counters
+ perf_free(_sample_perf);
+ perf_free(_comms_errors);
+ perf_free(_buffer_overflows);
+ perf_free(_range_errors);
+ perf_free(_conf_errors);
+}
+
+int
+LPS25H::init()
+{
+ int ret = ERROR;
+
+ ret = CDev::init();
+
+ if (ret != OK) {
+ DEVICE_DEBUG("CDev init failed");
+ goto out;
+ }
+
+ /* allocate basic report buffers */
+ _reports = new ringbuffer::RingBuffer(2, sizeof(baro_report));
+
+ if (_reports == nullptr) {
+ goto out;
+ }
+
+ /* reset the device configuration */
+ reset();
+
+ _class_instance = register_class_devname(BARO_BASE_DEVICE_PATH);
+
+ ret = OK;
+ /* sensor is ok, but not calibrated */
+ _sensor_ok = true;
+out:
+ return ret;
+}
+
+/**
+ check that the configuration register has the right value. This is
+ done periodically to cope with I2C bus noise causing the
+ configuration of the compass to change.
+ */
+void LPS25H::check_conf(void)
+{
+ /* TODO */
+
+ /*
+ int ret;
+
+ uint8_t conf_reg_in = 0;
+ ret = read_reg(ADDR_CNTL1, conf_reg_in);
+
+ if (OK != ret) {
+ perf_count(_comms_errors);
+ return;
+ }
+
+ if (conf_reg_in | CNTL1_BIT) {
+ perf_count(_conf_errors);
+ ret = write_reg(ADDR_CNTL1, conf_reg_in | CNTL1_BIT);
+
+ if (OK != ret) {
+ perf_count(_comms_errors);
+ }
+ }
+ */
+}
+
+ssize_t
+LPS25H::read(struct file *filp, char *buffer, size_t buflen)
+{
+ unsigned count = buflen / sizeof(struct baro_report);
+ struct baro_report *mag_buf = reinterpret_cast<struct baro_report *>(buffer);
+ int ret = 0;
+
+ /* buffer must be large enough */
+ if (count < 1) {
+ return -ENOSPC;
+ }
+
+ /* if automatic measurement is enabled */
+ if (_measure_ticks > 0) {
+ /*
+ * While there is space in the caller's buffer, and reports, copy them.
+ * Note that we may be pre-empted by the workq thread while we are doing this;
+ * we are careful to avoid racing with them.
+ */
+ while (count--) {
+ if (_reports->get(mag_buf)) {
+ ret += sizeof(struct baro_report);
+ mag_buf++;
+ }
+ }
+
+ /* if there was no data, warn the caller */
+ return ret ? ret : -EAGAIN;
+ }
+
+ /* manual measurement - run one conversion */
+ /* XXX really it'd be nice to lock against other readers here */
+ do {
+ _reports->flush();
+
+ /* trigger a measurement */
+ if (OK != measure()) {
+ ret = -EIO;
+ break;
+ }
+
+ /* wait for it to complete */
+ usleep(LPS25H_CONVERSION_INTERVAL);
+
+ /* run the collection phase */
+ if (OK != collect()) {
+ ret = -EIO;
+ break;
+ }
+
+ if (_reports->get(mag_buf)) {
+ ret = sizeof(struct baro_report);
+ }
+ } while (0);
+
+ return ret;
+}
+
+int
+LPS25H::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+ unsigned dummy = arg;
+
+ switch (cmd) {
+ case SENSORIOCSPOLLRATE: {
+ switch (arg) {
+
+ /* switching to manual polling */
+ case SENSOR_POLLRATE_MANUAL:
+ stop();
+ _measure_ticks = 0;
+ return OK;
+
+ /* external signalling (DRDY) not supported */
+ case SENSOR_POLLRATE_EXTERNAL:
+
+ /* zero would be bad */
+ case 0:
+ return -EINVAL;
+
+ /* set default/max polling rate */
+ case SENSOR_POLLRATE_MAX:
+ case SENSOR_POLLRATE_DEFAULT: {
+ /* do we need to start internal polling? */
+ bool want_start = (_measure_ticks == 0);
+
+ /* set interval for next measurement to minimum legal value */
+ _measure_ticks = USEC2TICK(LPS25H_CONVERSION_INTERVAL);
+
+ /* if we need to start the poll state machine, do it */
+ if (want_start) {
+ start();
+ }
+
+ return OK;
+ }
+
+ /* adjust to a legal polling interval in Hz */
+ default: {
+ /* do we need to start internal polling? */
+ bool want_start = (_measure_ticks == 0);
+
+ /* convert hz to tick interval via microseconds */
+ unsigned ticks = USEC2TICK(1000000 / arg);
+
+ /* check against maximum rate */
+ if (ticks < USEC2TICK(LPS25H_CONVERSION_INTERVAL)) {
+ return -EINVAL;
+ }
+
+ /* update interval for next measurement */
+ _measure_ticks = ticks;
+
+ /* if we need to start the poll state machine, do it */
+ if (want_start) {
+ start();
+ }
+
+ return OK;
+ }
+ }
+ }
+
+ case SENSORIOCGPOLLRATE:
+ if (_measure_ticks == 0) {
+ return SENSOR_POLLRATE_MANUAL;
+ }
+
+ return 1000000 / TICK2USEC(_measure_ticks);
+
+ case SENSORIOCSQUEUEDEPTH: {
+ /* lower bound is mandatory, upper bound is a sanity check */
+ if ((arg < 1) || (arg > 100)) {
+ return -EINVAL;
+ }
+
+ irqstate_t flags = irqsave();
+
+ if (!_reports->resize(arg)) {
+ irqrestore(flags);
+ return -ENOMEM;
+ }
+
+ irqrestore(flags);
+
+ return OK;
+ }
+
+ case SENSORIOCGQUEUEDEPTH:
+ return _reports->size();
+
+ case SENSORIOCRESET:
+ return reset();
+
+ case BAROIOCGMSLPRESSURE:
+ // TODO
+ return 0;
+
+ case BAROIOCSMSLPRESSURE:
+ // TODO
+ return 0;
+
+ case DEVIOCGDEVICEID:
+ return _interface->ioctl(cmd, dummy);
+
+ default:
+ /* give it to the superclass */
+ return CDev::ioctl(filp, cmd, arg);
+ }
+}
+
+void
+LPS25H::start()
+{
+ /* reset the report ring and state machine */
+ _collect_phase = false;
+ _reports->flush();
+
+ /* schedule a cycle to start things */
+ work_queue(HPWORK, &_work, (worker_t)&LPS25H::cycle_trampoline, this, 1);
+}
+
+void
+LPS25H::stop()
+{
+ work_cancel(HPWORK, &_work);
+}
+
+int
+LPS25H::reset()
+{
+ int ret = 0;
+
+ // Power on
+ ret = write_reg(ADDR_CTRL_REG1, CTRL_REG1_PD);
+
+ usleep(1000);
+
+ // Reset
+ ret = write_reg(ADDR_CTRL_REG2, CTRL_REG2_BOOT | CTRL_REG2_SWRESET);
+
+ usleep(1000);
+
+ // Power on
+ ret = write_reg(ADDR_CTRL_REG1, CTRL_REG1_PD);
+
+ usleep(1000);
+
+ return ret;
+}
+
+void
+LPS25H::cycle_trampoline(void *arg)
+{
+ LPS25H *dev = (LPS25H *)arg;
+
+ dev->cycle();
+}
+
+void
+LPS25H::cycle()
+{
+ /* collection phase? */
+ if (_collect_phase) {
+
+ /* perform collection */
+ if (OK != collect()) {
+ DEVICE_DEBUG("collection error");
+ /* restart the measurement state machine */
+ start();
+ return;
+ }
+
+ /* next phase is measurement */
+ _collect_phase = false;
+
+ /*
+ * Is there a collect->measure gap?
+ */
+ if (_measure_ticks > USEC2TICK(LPS25H_CONVERSION_INTERVAL)) {
+
+ /* schedule a fresh cycle call when we are ready to measure again */
+ work_queue(HPWORK,
+ &_work,
+ (worker_t)&LPS25H::cycle_trampoline,
+ this,
+ _measure_ticks - USEC2TICK(LPS25H_CONVERSION_INTERVAL));
+
+ return;
+ }
+ }
+
+ /* measurement phase */
+ if (OK != measure()) {
+ DEVICE_DEBUG("measure error");
+ }
+
+ /* next phase is collection */
+ _collect_phase = true;
+
+ /* schedule a fresh cycle call when the measurement is done */
+ work_queue(HPWORK,
+ &_work,
+ (worker_t)&LPS25H::cycle_trampoline,
+ this,
+ USEC2TICK(LPS25H_CONVERSION_INTERVAL));
+}
+
+int
+LPS25H::measure()
+{
+ int ret;
+
+ /*
+ * Send the command to begin a 16-bit measurement.
+ */
+ ret = write_reg(ADDR_CTRL_REG2, CTRL_REG2_ONE_SHOT);
+
+
+ if (OK != ret) {
+ perf_count(_comms_errors);
+ }
+
+ return ret;
+}
+
+int
+LPS25H::collect()
+{
+#pragma pack(push, 1)
+ struct {
+ uint8_t status;
+ uint8_t p_xl, p_l, p_h;
+ int16_t t;
+ } report;
+#pragma pack(pop)
+
+ int ret;
+ uint8_t check_counter;
+
+ perf_begin(_sample_perf);
+ struct baro_report new_report;
+ bool sensor_is_onboard = false;
+
+ /* this should be fairly close to the end of the measurement, so the best approximation of the time */
+ new_report.timestamp = hrt_absolute_time();
+ new_report.error_count = perf_event_count(_comms_errors);
+
+ /*
+ * @note We could read the status register 1 here, which could tell us that
+ * we were too early and that the output registers are still being
+ * written. In the common case that would just slow us down, and
+ * we're better off just never being early.
+ */
+
+ /* get measurements from the device */
+ ret = _interface->read(ADDR_STATUS_REG, (uint8_t *)&report, sizeof(report));
+
+ if (ret != OK) {
+ perf_count(_comms_errors);
+ DEVICE_DEBUG("data/status read error");
+ goto out;
+ }
+
+ /* get measurements from the device */
+ new_report.temperature = 42.5 + (report.t / 480);
+
+ /*
+ * RAW outputs
+ */
+ new_report.pressure = report.p_xl + (report.p_l << 8) + (report.p_h << 16);
+
+ if (!(_pub_blocked)) {
+
+ if (_baro_topic != nullptr) {
+ /* publish it */
+ orb_publish(ORB_ID(sensor_baro), _baro_topic, &new_report);
+
+ } else {
+ _baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &new_report,
+ &_orb_class_instance, (sensor_is_onboard) ? ORB_PRIO_HIGH : ORB_PRIO_MAX);
+
+ if (_baro_topic == nullptr) {
+ DEVICE_DEBUG("ADVERT FAIL");
+ }
+ }
+ }
+
+ _last_report = new_report;
+
+ /* post a report to the ring */
+ if (_reports->force(&new_report)) {
+ perf_count(_buffer_overflows);
+ }
+
+ /* notify anyone waiting for data */
+ poll_notify(POLLIN);
+
+ /*
+ periodically check the range register and configuration
+ registers. With a bad I2C cable it is possible for the
+ registers to become corrupt, leading to bad readings. It
+ doesn't happen often, but given the poor cables some
+ vehicles have it is worth checking for.
+ */
+ check_counter = perf_event_count(_sample_perf) % 256;
+
+ if (check_counter == 128) {
+ check_conf();
+ }
+
+ ret = OK;
+
+out:
+ perf_end(_sample_perf);
+ return ret;
+}
+
+int LPS25H::calibrate(struct file *filp, unsigned enable)
+{
+ int ret = 1;
+
+ return ret;
+}
+
+int LPS25H::check_scale()
+{
+ bool scale_valid = false;
+
+ /* return 0 if calibrated, 1 else */
+ return !scale_valid;
+}
+
+int LPS25H::check_offset()
+{
+ bool offset_valid = false;
+
+ return !offset_valid;
+}
+
+int LPS25H::check_calibration()
+{
+ bool offset_valid = (check_offset() == OK);
+ bool scale_valid = (check_scale() == OK);
+
+ if (_calibrated != (offset_valid && scale_valid)) {
+ warnx("mag cal status changed %s%s", (scale_valid) ? "" : "scale invalid ",
+ (offset_valid) ? "" : "offset invalid");
+ _calibrated = (offset_valid && scale_valid);
+ }
+
+ /* return 0 if calibrated, 1 else */
+ return (!_calibrated);
+}
+
+int
+LPS25H::write_reg(uint8_t reg, uint8_t val)
+{
+ uint8_t buf = val;
+ return _interface->write(reg, &buf, 1);
+}
+
+int
+LPS25H::read_reg(uint8_t reg, uint8_t &val)
+{
+ uint8_t buf = val;
+ int ret = _interface->read(reg, &buf, 1);
+ val = buf;
+ return ret;
+}
+
+float
+LPS25H::meas_to_float(uint8_t in[2])
+{
+ union {
+ uint8_t b[2];
+ int16_t w;
+ } u;
+
+ u.b[0] = in[1];
+ u.b[1] = in[0];
+
+ return (float) u.w;
+}
+
+void
+LPS25H::print_info()
+{
+ perf_print_counter(_sample_perf);
+ perf_print_counter(_comms_errors);
+ perf_print_counter(_buffer_overflows);
+ printf("poll interval: %u ticks\n", _measure_ticks);
+ printf("pressure %.2f\n", (double)_last_report.pressure);
+ printf("temperature %.2f\n", (double)_last_report.temperature);
+ _reports->print_info("report queue");
+}
+
+/**
+ * Local functions in support of the shell command.
+ */
+namespace lps25h
+{
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+const int ERROR = -1;
+
+/*
+ list of supported bus configurations
+ */
+struct lps25h_bus_option {
+ enum LPS25H_BUS busid;
+ const char *devpath;
+ LPS25H_constructor interface_constructor;
+ uint8_t busnum;
+ LPS25H *dev;
+} bus_options[] = {
+ { LPS25H_BUS_I2C_EXTERNAL, "/dev/lps25h_ext", &LPS25H_I2C_interface, PX4_I2C_BUS_EXPANSION, NULL },
+#ifdef PX4_I2C_BUS_ONBOARD
+ { LPS25H_BUS_I2C_INTERNAL, "/dev/lps25h_int", &LPS25H_I2C_interface, PX4_I2C_BUS_ONBOARD, NULL },
+#endif
+#ifdef PX4_SPIDEV_HMC
+ { LPS25H_BUS_SPI, "/dev/lps25h_spi", &LPS25H_SPI_interface, PX4_SPI_BUS_SENSORS, NULL },
+#endif
+};
+#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0]))
+
+void start(enum LPS25H_BUS busid);
+bool start_bus(struct lps25h_bus_option &bus);
+struct lps25h_bus_option &find_bus(enum LPS25H_BUS busid);
+void test(enum LPS25H_BUS busid);
+void reset(enum LPS25H_BUS busid);
+int info(enum LPS25H_BUS busid);
+int calibrate(enum LPS25H_BUS busid);
+int temp_enable(LPS25H_BUS busid, bool enable);
+void usage();
+
+/**
+ * start driver for a specific bus option
+ */
+bool
+start_bus(struct lps25h_bus_option &bus)
+{
+ if (bus.dev != nullptr) {
+ errx(1, "bus option already started");
+ }
+
+ device::Device *interface = bus.interface_constructor(bus.busnum);
+
+ if (interface->init() != OK) {
+ delete interface;
+ warnx("no device on bus %u", (unsigned)bus.busid);
+ return false;
+ }
+
+ bus.dev = new LPS25H(interface, bus.devpath);
+
+ if (bus.dev != nullptr && OK != bus.dev->init()) {
+ delete bus.dev;
+ bus.dev = NULL;
+ return false;
+ }
+
+ int fd = open(bus.devpath, O_RDONLY);
+
+ if (fd < 0) {
+ return false;
+ }
+
+ if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
+ close(fd);
+ errx(1, "Failed to setup poll rate");
+ }
+
+ close(fd);
+
+ return true;
+}
+
+
+/**
+ * Start the driver.
+ *
+ * This function call only returns once the driver
+ * is either successfully up and running or failed to start.
+ */
+void
+start(enum LPS25H_BUS busid)
+{
+ bool started = false;
+
+ for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) {
+ if (busid == LPS25H_BUS_ALL && bus_options[i].dev != NULL) {
+ // this device is already started
+ continue;
+ }
+
+ if (busid != LPS25H_BUS_ALL && bus_options[i].busid != busid) {
+ // not the one that is asked for
+ continue;
+ }
+
+ started |= start_bus(bus_options[i]);
+ }
+
+ if (!started) {
+ exit(1);
+ }
+}
+
+/**
+ * find a bus structure for a busid
+ */
+struct lps25h_bus_option &find_bus(enum LPS25H_BUS busid)
+{
+ for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) {
+ if ((busid == LPS25H_BUS_ALL ||
+ busid == bus_options[i].busid) && bus_options[i].dev != NULL) {
+ return bus_options[i];
+ }
+ }
+
+ errx(1, "bus %u not started", (unsigned)busid);
+}
+
+
+/**
+ * Perform some basic functional tests on the driver;
+ * make sure we can collect data from the sensor in polled
+ * and automatic modes.
+ */
+void
+test(enum LPS25H_BUS busid)
+{
+ struct lps25h_bus_option &bus = find_bus(busid);
+ struct baro_report report;
+ ssize_t sz;
+ int ret = 0;
+ const char *path = bus.devpath;
+
+ int fd = open(path, O_RDONLY);
+
+ if (fd < 0) {
+ err(1, "%s open failed (try 'lps25h start')", path);
+ }
+
+ /* do a simple demand read */
+ sz = read(fd, &report, sizeof(report));
+
+ if (sz != sizeof(report)) {
+ err(1, "immediate read failed");
+ }
+
+ warnx("single read");
+ warnx("measurement: %.6f", (double)report.temperature);
+ warnx("time: %lld", report.timestamp);
+
+ /* check if mag is onboard or external */
+ //if ((ret = ioctl(fd, MAGIOCGEXTERNAL, 0)) < 0) {
+ // errx(1, "failed to get if mag is onboard or external");
+ //}
+
+ warnx("device active: %s", ret ? "external" : "onboard");
+
+ /* set the queue depth to 5 */
+ if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10)) {
+ errx(1, "failed to set queue depth");
+ }
+
+ /* start the sensor polling at 2Hz */
+ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
+ errx(1, "failed to set 2Hz poll rate");
+ }
+
+ /* read the sensor 5x and report each value */
+ for (unsigned i = 0; i < 5; i++) {
+ struct pollfd fds;
+
+ /* wait for data to be ready */
+ fds.fd = fd;
+ fds.events = POLLIN;
+ ret = poll(&fds, 1, 2000);
+
+ if (ret != 1) {
+ errx(1, "timed out waiting for sensor data");
+ }
+
+ /* now go get it */
+ sz = read(fd, &report, sizeof(report));
+
+ if (sz != sizeof(report)) {
+ err(1, "periodic read failed");
+ }
+
+ warnx("periodic read %u", i);
+ warnx("measurement: %.6f", (double)report.temperature);
+ warnx("time: %lld", report.timestamp);
+ }
+
+ errx(0, "PASS");
+}
+
+
+/**
+ * Automatic scale calibration.
+ *
+ * Basic idea:
+ *
+ * output = (ext field +- 1.1 Ga self-test) * scale factor
+ *
+ * and consequently:
+ *
+ * 1.1 Ga = (excited - normal) * scale factor
+ * scale factor = (excited - normal) / 1.1 Ga
+ *
+ * sxy = (excited - normal) / 766 | for conf reg. B set to 0x60 / Gain = 3
+ * sz = (excited - normal) / 713 | for conf reg. B set to 0x60 / Gain = 3
+ *
+ * By subtracting the non-excited measurement the pure 1.1 Ga reading
+ * can be extracted and the sensitivity of all axes can be matched.
+ *
+ * SELF TEST OPERATION
+ * To check the LPS25HL for proper operation, a self test feature in incorporated
+ * in which the sensor offset straps are excited to create a nominal field strength
+ * (bias field) to be measured. To implement self test, the least significant bits
+ * (MS1 and MS0) of configuration register A are changed from 00 to 01 (positive bias)
+ * or 10 (negetive bias), e.g. 0x11 or 0x12.
+ * Then, by placing the mode register into single-measurement mode (0x01),
+ * two data acquisition cycles will be made on each magnetic vector.
+ * The first acquisition will be a set pulse followed shortly by measurement
+ * data of the external field. The second acquisition will have the offset strap
+ * excited (about 10 mA) in the positive bias mode for X, Y, and Z axes to create
+ * about a ±1.1 gauss self test field plus the external field. The first acquisition
+ * values will be subtracted from the second acquisition, and the net measurement
+ * will be placed into the data output registers.
+ * Since self test adds ~1.1 Gauss additional field to the existing field strength,
+ * using a reduced gain setting prevents sensor from being saturated and data registers
+ * overflowed. For example, if the configuration register B is set to 0x60 (Gain=3),
+ * values around +766 LSB (1.16 Ga * 660 LSB/Ga) will be placed in the X and Y data
+ * output registers and around +713 (1.08 Ga * 660 LSB/Ga) will be placed in Z data
+ * output register. To leave the self test mode, change MS1 and MS0 bit of the
+ * configuration register A back to 00 (Normal Measurement Mode), e.g. 0x10.
+ * Using the self test method described above, the user can scale sensor
+ */
+int calibrate(enum LPS25H_BUS busid)
+{
+ int ret = 0;
+ struct lps25h_bus_option &bus = find_bus(busid);
+ const char *path = bus.devpath;
+
+ int fd = open(path, O_RDONLY);
+
+ if (fd < 0) {
+ err(1, "%s open failed (try 'lps25h start' if the driver is not running", path);
+ }
+
+ //if (OK != (ret = ioctl(fd, MAGIOCCALIBRATE, fd))) {
+ // warnx("failed to enable sensor calibration mode");
+ //}
+
+ close(fd);
+
+ return ret;
+}
+
+/**
+ * Reset the driver.
+ */
+void
+reset(enum LPS25H_BUS busid)
+{
+ struct lps25h_bus_option &bus = find_bus(busid);
+ const char *path = bus.devpath;
+
+ int fd = open(path, O_RDONLY);
+
+ if (fd < 0) {
+ err(1, "failed ");
+ }
+
+ if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
+ err(1, "driver reset failed");
+ }
+
+ if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
+ err(1, "driver poll restart failed");
+ }
+
+ exit(0);
+}
+
+
+/**
+ * enable/disable temperature compensation
+ */
+int
+temp_enable(enum LPS25H_BUS busid, bool enable)
+{
+ struct lps25h_bus_option &bus = find_bus(busid);
+ const char *path = bus.devpath;
+
+ int fd = open(path, O_RDONLY);
+
+ if (fd < 0) {
+ err(1, "failed ");
+ }
+
+ //if (ioctl(fd, MAGIOCSTEMPCOMP, (unsigned)enable) < 0) {
+ // err(1, "set temperature compensation failed");
+ //}
+
+ close(fd);
+ return 0;
+}
+
+/**
+ * Print a little info about the driver.
+ */
+int
+info(enum LPS25H_BUS busid)
+{
+ struct lps25h_bus_option &bus = find_bus(busid);
+
+ warnx("running on bus: %u (%s)\n", (unsigned)bus.busid, bus.devpath);
+ bus.dev->print_info();
+ exit(0);
+}
+
+void
+usage()
+{
+ warnx("missing command: try 'start', 'info', 'test', 'reset', 'info', 'calibrate'");
+ warnx("options:");
+ warnx(" -R rotation");
+ warnx(" -C calibrate on start");
+ warnx(" -X only external bus");
+#if (PX4_I2C_BUS_ONBOARD || PX4_SPIDEV_HMC)
+ warnx(" -I only internal bus");
+#endif
+}
+
+} // namespace
+
+int
+lps25h_main(int argc, char *argv[])
+{
+ int ch;
+ enum LPS25H_BUS busid = LPS25H_BUS_ALL;
+ bool calibrate = false;
+ bool temp_compensation = false;
+
+ while ((ch = getopt(argc, argv, "XIS:CT")) != EOF) {
+ switch (ch) {
+#if (PX4_I2C_BUS_ONBOARD || PX4_SPIDEV_HMC)
+ case 'I':
+ busid = LPS25H_BUS_I2C_INTERNAL;
+ break;
+#endif
+
+ case 'X':
+ busid = LPS25H_BUS_I2C_EXTERNAL;
+ break;
+
+ case 'S':
+ busid = LPS25H_BUS_SPI;
+ break;
+
+ case 'C':
+ calibrate = true;
+ break;
+
+ case 'T':
+ temp_compensation = true;
+ break;
+
+ default:
+ lps25h::usage();
+ exit(0);
+ }
+ }
+
+ const char *verb = argv[optind];
+
+ /*
+ * Start/load the driver.
+ */
+ if (!strcmp(verb, "start")) {
+ lps25h::start(busid);
+
+ if (calibrate && lps25h::calibrate(busid) != 0) {
+ errx(1, "calibration failed");
+ }
+
+ if (temp_compensation) {
+ // we consider failing to setup temperature
+ // compensation as non-fatal
+ lps25h::temp_enable(busid, true);
+ }
+
+ exit(0);
+ }
+
+ /*
+ * Test the driver/device.
+ */
+ if (!strcmp(verb, "test")) {
+ lps25h::test(busid);
+ }
+
+ /*
+ * Reset the driver.
+ */
+ if (!strcmp(verb, "reset")) {
+ lps25h::reset(busid);
+ }
+
+ /*
+ * enable/disable temperature compensation
+ */
+ if (!strcmp(verb, "tempoff")) {
+ lps25h::temp_enable(busid, false);
+ }
+
+ if (!strcmp(verb, "tempon")) {
+ lps25h::temp_enable(busid, true);
+ }
+
+ /*
+ * Print driver information.
+ */
+ if (!strcmp(verb, "info") || !strcmp(verb, "status")) {
+ lps25h::info(busid);
+ }
+
+ /*
+ * Autocalibrate the scaling
+ */
+ if (!strcmp(verb, "calibrate")) {
+ if (lps25h::calibrate(busid) == 0) {
+ errx(0, "calibration successful");
+
+ } else {
+ errx(1, "calibration failed");
+ }
+ }
+
+ errx(1, "unrecognized command, try 'start', 'test', 'reset' 'calibrate', 'tempoff', 'tempon' or 'info'");
+}
diff --git a/src/drivers/lps25h/lps25h.h b/src/drivers/lps25h/lps25h.h
new file mode 100644
index 0000000000000000000000000000000000000000..91a72dbf450617ce4eab1572846db7c38042ab50
--- /dev/null
+++ b/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);
diff --git a/src/drivers/lps25h/lps25h_i2c.cpp b/src/drivers/lps25h/lps25h_i2c.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5bd364a74bd90fbbf2ac7d93c3e421af15c2d919
--- /dev/null
+++ b/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);
+}
diff --git a/src/drivers/lps25h/lps25h_spi.cpp b/src/drivers/lps25h/lps25h_spi.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c48a964b4430f62d8eb0c03fcc819417fee922c4
--- /dev/null
+++ b/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 */
diff --git a/src/drivers/mpu9250/CMakeLists.txt b/src/drivers/mpu9250/CMakeLists.txt
index 94b1fa303691d5421bc37f6b757541096f491148..2616a19177c5bcae6e90a23eb5556fed95b08086 100644
--- a/src/drivers/mpu9250/CMakeLists.txt
+++ b/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 :
diff --git a/src/drivers/mpu9250/main.cpp b/src/drivers/mpu9250/main.cpp
index d178175cf3b067e7fa52a0ffc3c78dba0d0fb11b..01542e39893352b02dc379100053e0eeb60c4c6c 100644
--- a/src/drivers/mpu9250/main.cpp
+++ b/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
-
-void start(bool, enum Rotation);
-void stop(bool);
-void test(bool);
-void reset(bool);
-void info(bool);
-void regdump(bool);
-void testerror(bool);
+enum MPU9250_BUS {
+ MPU9250_BUS_ALL = 0,
+ MPU9250_BUS_I2C_INTERNAL,
+ MPU9250_BUS_I2C_EXTERNAL,
+ MPU9250_BUS_SPI_INTERNAL,
+ MPU9250_BUS_SPI_EXTERNAL
+};
+
+/*
+ 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");
- }
-
- /* 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
+ 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];
+ }
+ }
- } else {
- *g_dev_ptr = new MPU9250(PX4_SPI_BUS_SENSORS, path_accel, path_gyro, path_mag, (spi_dev_e)PX4_SPIDEV_MPU, rotation);
+ errx(1, "bus %u not started", (unsigned)busid);
+}
+
+/**
+ * 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 (interface->init() != OK) {
+ delete interface;
+ warnx("no device on bus %u", (unsigned)bus.busid);
+ return false;
}
- if (OK != (*g_dev_ptr)->init()) {
+ 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();
diff --git a/src/drivers/mpu9250/mpu9250.cpp b/src/drivers/mpu9250/mpu9250.cpp
index 02a21f52fdd194fd88f6a7ac3af7c21267bc634d..422ac7f574dd33e0cde84adeba528ef17dfe904f 100644
--- a/src/drivers/mpu9250/mpu9250.cpp
+++ b/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;
}
diff --git a/src/drivers/mpu9250/mpu9250.h b/src/drivers/mpu9250/mpu9250.h
index abae2b0310b497ef724a362e807a68509058ecdc..b47492a42b1ca2a23533a3b1b78ee34ea66ee07a 100644
--- a/src/drivers/mpu9250/mpu9250.h
+++ b/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);
diff --git a/src/drivers/mpu9250/mpu9250_i2c.cpp b/src/drivers/mpu9250/mpu9250_i2c.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d546331fad04883aec865810ce1b96838e8a2fa5
--- /dev/null
+++ b/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 */
diff --git a/src/drivers/mpu9250/mpu9250_spi.cpp b/src/drivers/mpu9250/mpu9250_spi.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6abdf4278ccacd69d29000fc9afb42bd34683f34
--- /dev/null
+++ b/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 ®_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 ®_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
diff --git a/src/drivers/stm32/drv_io_timer.c b/src/drivers/stm32/drv_io_timer.c
index 385d250ef08bf3e8478fe072ec45e808bc71af97..2ac9b7692ebfec43df0656309b6238b78c2dfe60 100644
--- a/src/drivers/stm32/drv_io_timer.c
+++ b/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
/*
diff --git a/src/firmware/nuttx/CMakeLists.txt b/src/firmware/nuttx/CMakeLists.txt
index 4d27d2758b76f10ae6ce970bd3bc9cc5b576d6dc..a617e093c58bed00fea9e4e7e35b7fdfe45ed449 100644
--- a/src/firmware/nuttx/CMakeLists.txt
+++ b/src/firmware/nuttx/CMakeLists.txt
@@ -45,7 +45,7 @@ if(NOT ${BOARD} STREQUAL "sim")
if (config_io_board)
set(extras "${PX4_BINARY_DIR}/src/modules/px4iofirmware/${config_io_board}.bin")
endif()
-
+
set(romfs_dir "ROMFS/px4fmu_common")
if (${BOARD} STREQUAL "tap-v1")
set(romfs_dir "ROMFS/tap_common")
diff --git a/src/lib/version/version.h b/src/lib/version/version.h
index ae2acbd4eb7ed19478d02e0806dfc70b7e193594..fe9e76eb8bea40b5dd8778adaba53dd3367377cc 100644
--- a/src/lib/version/version.h
+++ b/src/lib/version/version.h
@@ -63,6 +63,8 @@ __END_DECLS
# define HW_ARCH "RPI"
#elif defined(CONFIG_ARCH_BOARD_BEBOP)
# define HW_ARCH "BEBOP"
+#elif defined(CONFIG_ARCH_BOARD_CRAZYFLIE)
+# define HW_ARCH "CRAZYFLIE"
#else
#define HW_ARCH (board_name())
#endif
diff --git a/src/modules/dummy/CMakeLists.txt b/src/modules/dummy/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..26b585968d27d09717585dabe2513b0dab0ccfaf
--- /dev/null
+++ b/src/modules/dummy/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 modules__dummy
+ MAIN tone_alarm
+ COMPILE_FLAGS
+ -Os
+ SRCS
+ tone_alarm.c
+ DEPENDS
+ platforms__common
+ )
+# vim: set noet ft=cmake fenc=utf-8 ff=unix :
diff --git a/src/modules/dummy/tone_alarm.c b/src/modules/dummy/tone_alarm.c
new file mode 100644
index 0000000000000000000000000000000000000000..cd19ddeb918c6116f3c72ccf77c89df0ca646c67
--- /dev/null
+++ b/src/modules/dummy/tone_alarm.c
@@ -0,0 +1,50 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ * Author: Anton Babushkin <anton.babushkin@me.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 tone_alarm.c
+ *
+ * Dummy tone_alarm to prevent nsh errors.
+ *
+ * @author Tim Dyer <>
+ */
+
+#include <px4_defines.h>
+
+__EXPORT int tone_alarm_main(int argc, char *argv[]);
+
+int tone_alarm_main(int argc, char *argv[])
+{
+ return OK;
+}
\ No newline at end of file
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index e5073e32bdd0aab45c1cfb5a0eec0db69f194fcd..715d9394adc7a5774a08cce8f22f7573f1618da8 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -688,8 +688,10 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name)
case 921600: speed = B921600; break;
+ case 1000000: speed = B1000000; break;
+
default:
- warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\t9600, 19200, 38400, 57600\t\n115200\n230400\n460800\n921600\n",
+ warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\t9600, 19200, 38400, 57600\t\n115200\n230400\n460800\n921600\n1000000\n",
baud);
return -EINVAL;
}
@@ -1690,7 +1692,7 @@ Mavlink::task_main(int argc, char *argv[])
case 'b':
_baudrate = strtoul(myoptarg, NULL, 10);
- if (_baudrate < 9600 || _baudrate > 921600) {
+ if (_baudrate < 9600 || _baudrate > 1000000) {
warnx("invalid baud rate '%s'", myoptarg);
err_flag = true;
}
diff --git a/src/modules/screen/CMakeLists.txt b/src/modules/screen/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..6f8228271f565d0343c1c3cd32189d98b989f296
--- /dev/null
+++ b/src/modules/screen/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 modules__screen
+ MAIN screen
+ COMPILE_FLAGS
+ -Os
+ SRCS
+ screen.c
+ DEPENDS
+ platforms__common
+ )
+# vim: set noet ft=cmake fenc=utf-8 ff=unix :
diff --git a/src/modules/screen/screen.c b/src/modules/screen/screen.c
new file mode 100644
index 0000000000000000000000000000000000000000..214801956e9bbb005b6023049f6caaa4e41affad
--- /dev/null
+++ b/src/modules/screen/screen.c
@@ -0,0 +1,134 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ * Author: Anton Babushkin <anton.babushkin@me.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 screen.c
+ *
+ * Using screen to read serial ports.
+ *
+ * @author Tim Dyer <>
+ */
+
+#include <px4_defines.h>
+#include <fcntl.h>
+#include <stdio.h>
+#include <sys/ioctl.h>
+#include <systemlib/err.h>
+#include <poll.h>
+#include <termios.h>
+
+__EXPORT int screen_main(int argc, char *argv[]);
+
+int
+open_uart(const char *device);
+
+
+int screen_main(int argc, char *argv[])
+{
+ int uart = open_uart("/dev/ttyS2");
+
+ //static const int timeout_ms = 1000;
+
+ for (;;)
+ {
+ char c;
+ ssize_t size = read(uart, &c, 1);
+ if (size)
+ {
+ printf("%c", c);
+ }
+
+ /*
+ struct pollfd fds;
+ fds.fd = uart;
+ fds.events = POLLIN;
+
+ char c;
+
+ if (poll(&fds, 1, timeout_ms) > 0) {
+ read(uart, &c, 1);
+ printf("%c", c);
+ } else {
+ warnx("UART timeout on TX/RX port");
+ //return ERROR;
+ }
+ */
+ }
+
+ return OK;
+}
+
+int
+open_uart(const char *device)
+{
+ /* baud rate */
+ static const speed_t speed = B1000000;
+
+ /* open uart */
+ const int uart = open(device, O_RDWR | O_NOCTTY);
+
+ if (uart < 0) {
+ err(1, "ERR: opening %s", device);
+ }
+
+ /* Back up the original uart configuration to restore it after exit */
+ int termios_state;
+ struct termios uart_config_original;
+
+ if ((termios_state = tcgetattr(uart, &uart_config_original)) < 0) {
+ close(uart);
+ err(1, "ERR: %s: %d", device, termios_state);
+ }
+
+ /* Fill the struct for the new configuration */
+ struct termios uart_config;
+ tcgetattr(uart, &uart_config);
+
+ /* Clear ONLCR flag (which appends a CR for every LF) */
+ uart_config.c_oflag &= ~ONLCR;
+
+ /* Set baud rate */
+ if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
+ close(uart);
+ err(1, "ERR: %s: %d (cfsetispeed, cfsetospeed)",
+ device, termios_state);
+ }
+
+ if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
+ close(uart);
+ err(1, "ERR: %s (tcsetattr)", device);
+ }
+
+ return uart;
+}