aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--Images/px4fmuv2.prototype12
-rw-r--r--Images/px4iov2.prototype12
-rw-r--r--Makefile4
-rw-r--r--makefiles/board_px4fmuv2.mk10
-rw-r--r--makefiles/board_px4iov2.mk10
-rw-r--r--makefiles/config_px4fmu_default.mk1
-rw-r--r--makefiles/config_px4fmuv2_default.mk121
-rw-r--r--makefiles/config_px4iov2_default.mk10
-rw-r--r--nuttx/arch/arm/include/stm32/chip.h97
-rw-r--r--nuttx/arch/arm/include/stm32/stm32f40xxx_irq.h11
-rw-r--r--nuttx/arch/arm/src/stm32/Kconfig110
-rw-r--r--nuttx/arch/arm/src/stm32/chip/stm32_flash.h25
-rw-r--r--nuttx/arch/arm/src/stm32/chip/stm32_i2c.h20
-rw-r--r--nuttx/arch/arm/src/stm32/chip/stm32_pwr.h11
-rw-r--r--nuttx/arch/arm/src/stm32/chip/stm32_syscfg.h5
-rw-r--r--nuttx/arch/arm/src/stm32/chip/stm32_uart.h18
-rw-r--r--nuttx/arch/arm/src/stm32/chip/stm32f40xxx_dma.h33
-rw-r--r--nuttx/arch/arm/src/stm32/chip/stm32f40xxx_memorymap.h5
-rw-r--r--nuttx/arch/arm/src/stm32/chip/stm32f40xxx_pinmap.h50
-rw-r--r--nuttx/arch/arm/src/stm32/chip/stm32f40xxx_rcc.h50
-rw-r--r--nuttx/arch/arm/src/stm32/chip/stm32f40xxx_vectors.h20
-rw-r--r--nuttx/arch/arm/src/stm32/stm32_allocateheap.c26
-rw-r--r--nuttx/arch/arm/src/stm32/stm32_lowputc.c34
-rw-r--r--nuttx/arch/arm/src/stm32/stm32_sdio.c3
-rw-r--r--nuttx/arch/arm/src/stm32/stm32_serial.c193
-rw-r--r--nuttx/arch/arm/src/stm32/stm32_spi.c194
-rw-r--r--nuttx/arch/arm/src/stm32/stm32_spi.h22
-rw-r--r--nuttx/arch/arm/src/stm32/stm32_uart.h70
-rw-r--r--nuttx/arch/arm/src/stm32/stm32f40xxx_dma.c2
-rw-r--r--nuttx/arch/arm/src/stm32/stm32f40xxx_rcc.c36
-rw-r--r--nuttx/configs/px4fmuv2/Kconfig7
-rw-r--r--nuttx/configs/px4fmuv2/common/Make.defs184
-rw-r--r--nuttx/configs/px4fmuv2/common/ld.script150
-rwxr-xr-xnuttx/configs/px4fmuv2/include/board.h364
-rw-r--r--nuttx/configs/px4fmuv2/include/nsh_romfsimg.h42
-rw-r--r--nuttx/configs/px4fmuv2/nsh/Make.defs3
-rw-r--r--nuttx/configs/px4fmuv2/nsh/appconfig52
-rwxr-xr-xnuttx/configs/px4fmuv2/nsh/defconfig1082
-rwxr-xr-xnuttx/configs/px4fmuv2/nsh/setenv.sh67
-rw-r--r--nuttx/configs/px4fmuv2/src/Makefile84
-rw-r--r--nuttx/configs/px4fmuv2/src/empty.c4
-rwxr-xr-xnuttx/configs/px4iov2/README.txt806
-rw-r--r--nuttx/configs/px4iov2/common/Make.defs175
-rwxr-xr-xnuttx/configs/px4iov2/common/ld.script129
-rwxr-xr-xnuttx/configs/px4iov2/common/setenv.sh47
-rwxr-xr-xnuttx/configs/px4iov2/include/README.txt1
-rwxr-xr-xnuttx/configs/px4iov2/include/board.h184
-rw-r--r--nuttx/configs/px4iov2/io/Make.defs3
-rw-r--r--nuttx/configs/px4iov2/io/appconfig32
-rwxr-xr-xnuttx/configs/px4iov2/io/defconfig548
-rwxr-xr-xnuttx/configs/px4iov2/io/setenv.sh47
-rw-r--r--nuttx/configs/px4iov2/nsh/Make.defs3
-rw-r--r--nuttx/configs/px4iov2/nsh/appconfig44
-rwxr-xr-xnuttx/configs/px4iov2/nsh/defconfig567
-rwxr-xr-xnuttx/configs/px4iov2/nsh/setenv.sh47
-rw-r--r--nuttx/configs/px4iov2/src/Makefile84
-rw-r--r--nuttx/configs/px4iov2/src/README.txt1
-rw-r--r--nuttx/configs/px4iov2/src/empty.c4
-rw-r--r--nuttx/drivers/mtd/ramtron.c8
-rw-r--r--nuttx/drivers/serial/Kconfig194
-rw-r--r--src/drivers/boards/px4fmuv2/module.mk9
-rw-r--r--src/drivers/boards/px4fmuv2/px4fmu_can.c144
-rw-r--r--src/drivers/boards/px4fmuv2/px4fmu_init.c255
-rw-r--r--src/drivers/boards/px4fmuv2/px4fmu_internal.h129
-rw-r--r--src/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c105
-rw-r--r--src/drivers/boards/px4fmuv2/px4fmu_spi.c141
-rw-r--r--src/drivers/boards/px4fmuv2/px4fmu_usb.c108
-rw-r--r--src/drivers/boards/px4iov2/module.mk6
-rw-r--r--src/drivers/boards/px4iov2/px4iov2_init.c171
-rwxr-xr-xsrc/drivers/boards/px4iov2/px4iov2_internal.h112
-rw-r--r--src/drivers/boards/px4iov2/px4iov2_pwm_servo.c123
-rw-r--r--src/drivers/drv_gpio.h26
-rw-r--r--src/drivers/lsm303d/lsm303d.cpp1470
-rw-r--r--src/drivers/lsm303d/module.mk6
-rw-r--r--src/drivers/px4fmu/fmu.cpp207
-rw-r--r--src/drivers/rgbled/module.mk6
-rw-r--r--src/drivers/rgbled/rgbled.cpp497
-rw-r--r--src/include/device/rgbled.h67
-rw-r--r--src/modules/px4iofirmware/i2c.c15
-rw-r--r--src/modules/px4iofirmware/module.mk11
-rw-r--r--src/modules/px4iofirmware/protocol.h42
-rw-r--r--src/modules/px4iofirmware/px4io.c24
-rw-r--r--src/modules/px4iofirmware/px4io.h22
-rw-r--r--src/modules/px4iofirmware/serial.c133
-rw-r--r--src/modules/systemlib/hx_stream.c42
-rw-r--r--src/modules/systemlib/hx_stream.h18
-rw-r--r--src/systemcmds/ramtron/module.mk6
-rw-r--r--src/systemcmds/ramtron/ramtron.c268
88 files changed, 10219 insertions, 122 deletions
diff --git a/Images/px4fmuv2.prototype b/Images/px4fmuv2.prototype
new file mode 100644
index 000000000..5109b77d1
--- /dev/null
+++ b/Images/px4fmuv2.prototype
@@ -0,0 +1,12 @@
+{
+ "board_id": 9,
+ "magic": "PX4FWv1",
+ "description": "Firmware for the PX4FMUv2 board",
+ "image": "",
+ "build_time": 0,
+ "summary": "PX4FMUv2",
+ "version": "0.1",
+ "image_size": 0,
+ "git_identity": "",
+ "board_revision": 0
+}
diff --git a/Images/px4iov2.prototype b/Images/px4iov2.prototype
new file mode 100644
index 000000000..af87924e9
--- /dev/null
+++ b/Images/px4iov2.prototype
@@ -0,0 +1,12 @@
+{
+ "board_id": 10,
+ "magic": "PX4FWv2",
+ "description": "Firmware for the PX4IOv2 board",
+ "image": "",
+ "build_time": 0,
+ "summary": "PX4IOv2",
+ "version": "2.0",
+ "image_size": 0,
+ "git_identity": "",
+ "board_revision": 0
+}
diff --git a/Makefile b/Makefile
index dea344390..2094cf90e 100644
--- a/Makefile
+++ b/Makefile
@@ -145,9 +145,9 @@ ifneq ($(filter archives,$(MAKECMDGOALS)),)
endif
$(ARCHIVE_DIR)%.export: board = $(notdir $(basename $@))
-$(ARCHIVE_DIR)%.export: configuration = $(if $(filter $(board),px4io),io,nsh)
+$(ARCHIVE_DIR)%.export: configuration = $(if $(filter px4io px4iov2,$(board)),io,nsh)
$(NUTTX_ARCHIVES): $(ARCHIVE_DIR)%.export: $(NUTTX_SRC) $(NUTTX_APPS)
- @echo %% Configuring NuttX for $(board)
+ @echo %% Configuring NuttX for $(board)/$(configuration)
$(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export)
$(Q) make -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) distclean
$(Q) (cd $(NUTTX_SRC)tools && ./configure.sh $(board)/$(configuration))
diff --git a/makefiles/board_px4fmuv2.mk b/makefiles/board_px4fmuv2.mk
new file mode 100644
index 000000000..4b3b7e585
--- /dev/null
+++ b/makefiles/board_px4fmuv2.mk
@@ -0,0 +1,10 @@
+#
+# Board-specific definitions for the PX4FMUv2
+#
+
+#
+# Configure the toolchain
+#
+CONFIG_ARCH = CORTEXM4F
+
+include $(PX4_MK_DIR)/toolchain_gnu-arm-eabi.mk
diff --git a/makefiles/board_px4iov2.mk b/makefiles/board_px4iov2.mk
new file mode 100644
index 000000000..ee6b6125e
--- /dev/null
+++ b/makefiles/board_px4iov2.mk
@@ -0,0 +1,10 @@
+#
+# Board-specific definitions for the PX4IOv2
+#
+
+#
+# Configure the toolchain
+#
+CONFIG_ARCH = CORTEXM3
+
+include $(PX4_MK_DIR)/toolchain_gnu-arm-eabi.mk
diff --git a/makefiles/config_px4fmu_default.mk b/makefiles/config_px4fmu_default.mk
index d50eb1e50..7117a8218 100644
--- a/makefiles/config_px4fmu_default.mk
+++ b/makefiles/config_px4fmu_default.mk
@@ -18,6 +18,7 @@ MODULES += drivers/led
MODULES += drivers/px4io
MODULES += drivers/px4fmu
MODULES += drivers/boards/px4fmu
+MODULES += drivers/lsm303d
MODULES += drivers/ardrone_interface
MODULES += drivers/l3gd20
MODULES += drivers/bma180
diff --git a/makefiles/config_px4fmuv2_default.mk b/makefiles/config_px4fmuv2_default.mk
new file mode 100644
index 000000000..26c249901
--- /dev/null
+++ b/makefiles/config_px4fmuv2_default.mk
@@ -0,0 +1,121 @@
+#
+# Makefile for the px4fmu_default configuration
+#
+
+#
+# Use the configuration's ROMFS.
+#
+ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common
+
+#
+# Board support modules
+#
+MODULES += drivers/device
+MODULES += drivers/stm32
+MODULES += drivers/stm32/adc
+MODULES += drivers/stm32/tone_alarm
+MODULES += drivers/px4fmu
+MODULES += drivers/boards/px4fmuv2
+MODULES += drivers/rgbled
+MODULES += drivers/lsm303d
+MODULES += drivers/l3gd20
+#MODULES += drivers/mpu6000
+MODULES += drivers/hmc5883
+#MODULES += drivers/ms5611
+MODULES += drivers/mb12xx
+MODULES += drivers/gps
+MODULES += drivers/hil
+MODULES += drivers/hott_telemetry
+MODULES += drivers/blinkm
+MODULES += modules/sensors
+
+#
+# System commands
+#
+MODULES += systemcmds/ramtron
+MODULES += systemcmds/bl_update
+MODULES += systemcmds/boardinfo
+MODULES += systemcmds/mixer
+MODULES += systemcmds/param
+MODULES += systemcmds/perf
+MODULES += systemcmds/preflight_check
+MODULES += systemcmds/pwm
+MODULES += systemcmds/reboot
+MODULES += systemcmds/top
+MODULES += systemcmds/tests
+
+#
+# General system control
+#
+MODULES += modules/commander
+MODULES += modules/mavlink
+MODULES += modules/mavlink_onboard
+
+#
+# Estimation modules (EKF / other filters)
+#
+MODULES += modules/attitude_estimator_ekf
+MODULES += modules/position_estimator_mc
+MODULES += modules/position_estimator
+MODULES += modules/att_pos_estimator_ekf
+
+#
+# Vehicle Control
+#
+MODULES += modules/fixedwing_backside
+MODULES += modules/fixedwing_att_control
+MODULES += modules/fixedwing_pos_control
+MODULES += modules/multirotor_att_control
+MODULES += modules/multirotor_pos_control
+
+#
+# Logging
+#
+MODULES += modules/sdlog
+
+#
+# Library modules
+#
+MODULES += modules/systemlib
+MODULES += modules/systemlib/mixer
+MODULES += modules/mathlib
+MODULES += modules/controllib
+MODULES += modules/uORB
+
+#
+# Libraries
+#
+LIBRARIES += modules/mathlib/CMSIS
+
+#
+# Demo apps
+#
+#MODULES += examples/math_demo
+# Tutorial code from
+# https://pixhawk.ethz.ch/px4/dev/hello_sky
+#MODULES += examples/px4_simple_app
+
+# Tutorial code from
+# https://pixhawk.ethz.ch/px4/dev/daemon
+#MODULES += examples/px4_daemon_app
+
+# Tutorial code from
+# https://pixhawk.ethz.ch/px4/dev/debug_values
+#MODULES += examples/px4_mavlink_debug
+
+#
+# Transitional support - add commands from the NuttX export archive.
+#
+# In general, these should move to modules over time.
+#
+# Each entry here is <command>.<priority>.<stacksize>.<entrypoint> but we use a helper macro
+# to make the table a bit more readable.
+#
+define _B
+ $(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4)
+endef
+
+# command priority stack entrypoint
+BUILTIN_COMMANDS := \
+ $(call _B, sercon, , 2048, sercon_main ) \
+ $(call _B, serdis, , 2048, serdis_main )
diff --git a/makefiles/config_px4iov2_default.mk b/makefiles/config_px4iov2_default.mk
new file mode 100644
index 000000000..f9f35d629
--- /dev/null
+++ b/makefiles/config_px4iov2_default.mk
@@ -0,0 +1,10 @@
+#
+# Makefile for the px4iov2_default configuration
+#
+
+#
+# Board support modules
+#
+MODULES += drivers/stm32
+MODULES += drivers/boards/px4iov2
+MODULES += modules/px4iofirmware \ No newline at end of file
diff --git a/nuttx/arch/arm/include/stm32/chip.h b/nuttx/arch/arm/include/stm32/chip.h
index 14d92ea3d..473885471 100644
--- a/nuttx/arch/arm/include/stm32/chip.h
+++ b/nuttx/arch/arm/include/stm32/chip.h
@@ -688,6 +688,103 @@
# define STM32_NRNG 1 /* Random number generator (RNG) */
# define STM32_NDCMI 1 /* Digital camera interface (DCMI) */
+#elif defined(CONFIG_ARCH_CHIP_STM32F427I) /* BGA176; LQFP176 1024/2048KiB flash 256KiB SRAM */
+# undef CONFIG_STM32_STM32F10XX /* STM32F10xxx family */
+# undef CONFIG_STM32_LOWDENSITY /* STM32F100x, STM32F101x, STM32F102x and STM32F103x w/ 16/32 Kbytes */
+# undef CONFIG_STM32_MEDIUMDENSITY /* STM32F100x, STM32F101x, STM32F102x and STM32F103x w/ 64/128 Kbytes */
+# undef CONFIG_STM32_HIGHDENSITY /* STM32F100x, STM32F101x, and STM32F103x w/ 256/512 Kbytes */
+# undef CONFIG_STM32_VALUELINE /* STM32F100x */
+# undef CONFIG_STM32_CONNECTIVITYLINE /* STM32F105x and STM32F107x */
+# undef CONFIG_STM32_STM32F20XX /* STM32F205x and STM32F207x */
+# undef CONFIG_STM32_STM32F30XX /* STM32F30xxx family */
+# define CONFIG_STM32_STM32F40XX 1 /* STM32F405xx, STM32407xx and STM32F427/437 */
+# define STM32_NFSMC 1 /* FSMC */
+# define STM32_NATIM 2 /* Two advanced timers TIM1 and 8 */
+# define STM32_NGTIM 4 /* 16-bit general timers TIM3 and 4 with DMA
+ * 32-bit general timers TIM2 and 5 with DMA */
+# define STM32_NGTIMNDMA 6 /* 16-bit general timers TIM9-14 without DMA */
+# define STM32_NBTIM 2 /* Two basic timers, TIM6-7 */
+# define STM32_NDMA 2 /* DMA1-2 */
+# define STM32_NSPI 6 /* SPI1-6 */
+# define STM32_NI2S 2 /* I2S1-2 (multiplexed with SPI2-3) */
+# define STM32_NUSART 8 /* USART1-3 and 6, UART 4-5 and 7-8 */
+# define STM32_NI2C 3 /* I2C1-3 */
+# define STM32_NCAN 2 /* CAN1-2 */
+# define STM32_NSDIO 1 /* SDIO */
+# define STM32_NUSBOTG 1 /* USB OTG FS/HS */
+# define STM32_NGPIO 139 /* GPIOA-I */
+# define STM32_NADC 3 /* 12-bit ADC1-3, 24 channels */
+# define STM32_NDAC 2 /* 12-bit DAC1-2 */
+# define STM32_NCRC 1 /* CRC */
+# define STM32_NETHERNET 1 /* 100/100 Ethernet MAC */
+# define STM32_NRNG 1 /* Random number generator (RNG) */
+# define STM32_NDCMI 1 /* Digital camera interface (DCMI) */
+
+#elif defined(CONFIG_ARCH_CHIP_STM32F427Z) /* LQFP144 1024/2048KiB flash 256KiB SRAM */
+# undef CONFIG_STM32_STM32F10XX /* STM32F10xxx family */
+# undef CONFIG_STM32_LOWDENSITY /* STM32F100x, STM32F101x, STM32F102x and STM32F103x w/ 16/32 Kbytes */
+# undef CONFIG_STM32_MEDIUMDENSITY /* STM32F100x, STM32F101x, STM32F102x and STM32F103x w/ 64/128 Kbytes */
+# undef CONFIG_STM32_HIGHDENSITY /* STM32F100x, STM32F101x, and STM32F103x w/ 256/512 Kbytes */
+# undef CONFIG_STM32_VALUELINE /* STM32F100x */
+# undef CONFIG_STM32_CONNECTIVITYLINE /* STM32F105x and STM32F107x */
+# undef CONFIG_STM32_STM32F20XX /* STM32F205x and STM32F207x */
+# undef CONFIG_STM32_STM32F30XX /* STM32F30xxx family */
+# define CONFIG_STM32_STM32F40XX 1 /* STM32F405xx, STM32407xx and STM32F427/437 */
+# define STM32_NFSMC 1 /* FSMC */
+# define STM32_NATIM 2 /* Two advanced timers TIM1 and 8 */
+# define STM32_NGTIM 4 /* 16-bit general timers TIM3 and 4 with DMA
+ * 32-bit general timers TIM2 and 5 with DMA */
+# define STM32_NGTIMNDMA 6 /* 16-bit general timers TIM9-14 without DMA */
+# define STM32_NBTIM 2 /* Two basic timers, TIM6-7 */
+# define STM32_NDMA 2 /* DMA1-2 */
+# define STM32_NSPI 6 /* SPI1-6 */
+# define STM32_NI2S 2 /* I2S1-2 (multiplexed with SPI2-3) */
+# define STM32_NUSART 8 /* USART1-3 and 6, UART 4-5 and 7-8 */
+# define STM32_NI2C 3 /* I2C1-3 */
+# define STM32_NCAN 2 /* CAN1-2 */
+# define STM32_NSDIO 1 /* SDIO */
+# define STM32_NUSBOTG 1 /* USB OTG FS/HS */
+# define STM32_NGPIO 139 /* GPIOA-I */
+# define STM32_NADC 3 /* 12-bit ADC1-3, 24 channels */
+# define STM32_NDAC 2 /* 12-bit DAC1-2 */
+# define STM32_NCRC 1 /* CRC */
+# define STM32_NETHERNET 1 /* 100/100 Ethernet MAC */
+# define STM32_NRNG 1 /* Random number generator (RNG) */
+# define STM32_NDCMI 1 /* Digital camera interface (DCMI) */
+
+#elif defined(CONFIG_ARCH_CHIP_STM32F427V) /* LQFP100 1024/2048KiB flash 256KiB SRAM */
+# undef CONFIG_STM32_STM32F10XX /* STM32F10xxx family */
+# undef CONFIG_STM32_LOWDENSITY /* STM32F100x, STM32F101x, STM32F102x and STM32F103x w/ 16/32 Kbytes */
+# undef CONFIG_STM32_MEDIUMDENSITY /* STM32F100x, STM32F101x, STM32F102x and STM32F103x w/ 64/128 Kbytes */
+# undef CONFIG_STM32_HIGHDENSITY /* STM32F100x, STM32F101x, and STM32F103x w/ 256/512 Kbytes */
+# undef CONFIG_STM32_VALUELINE /* STM32F100x */
+# undef CONFIG_STM32_CONNECTIVITYLINE /* STM32F105x and STM32F107x */
+# undef CONFIG_STM32_STM32F20XX /* STM32F205x and STM32F207x */
+# undef CONFIG_STM32_STM32F30XX /* STM32F30xxx family */
+# define CONFIG_STM32_STM32F40XX 1 /* STM32F405xx, STM32407xx and STM32F427/437 */
+# define STM32_NFSMC 1 /* FSMC */
+# define STM32_NATIM 2 /* Two advanced timers TIM1 and 8 */
+# define STM32_NGTIM 4 /* 16-bit general timers TIM3 and 4 with DMA
+ * 32-bit general timers TIM2 and 5 with DMA */
+# define STM32_NGTIMNDMA 6 /* 16-bit general timers TIM9-14 without DMA */
+# define STM32_NBTIM 2 /* Two basic timers, TIM6-7 */
+# define STM32_NDMA 2 /* DMA1-2 */
+# define STM32_NSPI 4 /* SPI1-4 */
+# define STM32_NI2S 2 /* I2S1-2 (multiplexed with SPI2-3) */
+# define STM32_NUSART 8 /* USART1-3 and 6, UART 4-5 and 7-8 */
+# define STM32_NI2C 3 /* I2C1-3 */
+# define STM32_NCAN 2 /* CAN1-2 */
+# define STM32_NSDIO 1 /* SDIO */
+# define STM32_NUSBOTG 1 /* USB OTG FS/HS */
+# define STM32_NGPIO 139 /* GPIOA-I */
+# define STM32_NADC 3 /* 12-bit ADC1-3, 24 channels */
+# define STM32_NDAC 2 /* 12-bit DAC1-2 */
+# define STM32_NCRC 1 /* CRC */
+# define STM32_NETHERNET 1 /* 100/100 Ethernet MAC */
+# define STM32_NRNG 1 /* Random number generator (RNG) */
+# define STM32_NDCMI 1 /* Digital camera interface (DCMI) */
+
+
#else
# error "Unsupported STM32 chip"
#endif
diff --git a/nuttx/arch/arm/include/stm32/stm32f40xxx_irq.h b/nuttx/arch/arm/include/stm32/stm32f40xxx_irq.h
index cd97b9c9d..524568778 100644
--- a/nuttx/arch/arm/include/stm32/stm32f40xxx_irq.h
+++ b/nuttx/arch/arm/include/stm32/stm32f40xxx_irq.h
@@ -153,7 +153,16 @@
#define STM32_IRQ_RNG (STM32_IRQ_INTERRUPTS+80) /* 80: Hash and Rng global interrupt */
#define STM32_IRQ_FPU (STM32_IRQ_INTERRUPTS+81) /* 81: FPU global interrupt */
-#define NR_IRQS (STM32_IRQ_INTERRUPTS+82)
+#ifndef CONFIG_STM32_STM32F427
+# define NR_IRQS (STM32_IRQ_INTERRUPTS+87)
+#else
+# define STM32_IRQ_UART7 (STM32_IRQ_INTERRUPTS+82) /* 82: UART7 interrupt */
+# define STM32_IRQ_UART8 (STM32_IRQ_INTERRUPTS+83) /* 83: UART8 interrupt */
+# define STM32_IRQ_SPI4 (STM32_IRQ_INTERRUPTS+84) /* 84: SPI4 interrupt */
+# define STM32_IRQ_SPI5 (STM32_IRQ_INTERRUPTS+85) /* 85: SPI5 interrupt */
+# define STM32_IRQ_SPI6 (STM32_IRQ_INTERRUPTS+86) /* 86: SPI6 interrupt */
+# define NR_IRQS (STM32_IRQ_INTERRUPTS+87)
+#endif
/****************************************************************************************************
* Public Types
diff --git a/nuttx/arch/arm/src/stm32/Kconfig b/nuttx/arch/arm/src/stm32/Kconfig
index 41724be2d..ca6a82867 100644
--- a/nuttx/arch/arm/src/stm32/Kconfig
+++ b/nuttx/arch/arm/src/stm32/Kconfig
@@ -173,6 +173,24 @@ config ARCH_CHIP_STM32F407IG
select ARCH_CORTEXM4
select STM32_STM32F40XX
+config ARCH_CHIP_STM32F427V
+ bool "STM32F427V"
+ select ARCH_CORTEXM4
+ select STM32_STM32F40XX
+ select STM32_STM32F427
+
+config ARCH_CHIP_STM32F427Z
+ bool "STM32F427Z"
+ select ARCH_CORTEXM4
+ select STM32_STM32F40XX
+ select STM32_STM32F427
+
+config ARCH_CHIP_STM32F427I
+ bool "STM32F427I"
+ select ARCH_CORTEXM4
+ select STM32_STM32F40XX
+ select STM32_STM32F427
+
endchoice
config STM32_STM32F10XX
@@ -193,6 +211,10 @@ config STM32_STM32F20XX
config STM32_STM32F40XX
bool
+# This is really 427/437, but we treat the two the same.
+config STM32_STM32F427
+ bool
+
config STM32_DFU
bool "DFU bootloader"
default n
@@ -370,6 +392,27 @@ config STM32_SPI3
select SPI
select STM32_SPI
+config STM32_SPI4
+ bool "SPI4"
+ default n
+ depends on STM32_STM32F427
+ select SPI
+ select STM32_SPI
+
+config STM32_SPI5
+ bool "SPI5"
+ default n
+ depends on STM32_STM32F427
+ select SPI
+ select STM32_SPI
+
+config STM32_SPI6
+ bool "SPI6"
+ default n
+ depends on STM32_STM32F427
+ select SPI
+ select STM32_SPI
+
config STM32_SYSCFG
bool "SYSCFG"
default y
@@ -490,6 +533,20 @@ config STM32_USART6
select ARCH_HAVE_USART6
select STM32_USART
+config STM32_UART7
+ bool "UART7"
+ default n
+ depends on STM32_STM32F427
+ select ARCH_HAVE_UART7
+ select STM32_USART
+
+config STM32_UART8
+ bool "UART8"
+ default n
+ depends on STM32_STM32F427
+ select ARCH_HAVE_UART8
+ select STM32_USART
+
config STM32_USB
bool "USB Device"
default n
@@ -698,6 +755,7 @@ endmenu
config STM32_FLASH_PREFETCH
bool "Enable FLASH Pre-fetch"
depends on STM32_STM32F20XX || STM32_STM32F40XX
+ default y if STM32_STM32F427
default n
---help---
Enable FLASH prefetch and F2 and F4 parts (FLASH pre-fetch is always enabled
@@ -1966,9 +2024,59 @@ config USART6_RXDMA
---help---
In high data rate usage, Rx DMA may eliminate Rx overrun errors
+config USART7_RS485
+ bool "RS-485 on USART7"
+ default n
+ depends on STM32_USART7
+ ---help---
+ Enable RS-485 interface on USART7. Your board config will have to
+ provide GPIO_USART7_RS485_DIR pin definition. Currently it cannot be
+ used with USART7_RXDMA.
+
+config USART7_RS485_DIR_POLARITY
+ int "USART7 RS-485 DIR pin polarity"
+ default 1
+ range 0 1
+ depends on USART7_RS485
+ ---help---
+ Polarity of DIR pin for RS-485 on USART7. Set to state on DIR pin which
+ enables TX (0 - low / nTXEN, 1 - high / TXEN).
+
+config USART7_RXDMA
+ bool "USART7 Rx DMA"
+ default n
+ depends on STM32_STM32F40XX && STM32_DMA2
+ ---help---
+ In high data rate usage, Rx DMA may eliminate Rx overrun errors
+
+config USART8_RS485
+ bool "RS-485 on USART8"
+ default n
+ depends on STM32_USART8
+ ---help---
+ Enable RS-485 interface on USART8. Your board config will have to
+ provide GPIO_USART8_RS485_DIR pin definition. Currently it cannot be
+ used with USART8_RXDMA.
+
+config USART8_RS485_DIR_POLARITY
+ int "USART8 RS-485 DIR pin polarity"
+ default 1
+ range 0 1
+ depends on USART8_RS485
+ ---help---
+ Polarity of DIR pin for RS-485 on USART8. Set to state on DIR pin which
+ enables TX (0 - low / nTXEN, 1 - high / TXEN).
+
+config USART8_RXDMA
+ bool "USART8 Rx DMA"
+ default n
+ depends on STM32_STM32F40XX && STM32_DMA2
+ ---help---
+ In high data rate usage, Rx DMA may eliminate Rx overrun errors
+
config SERIAL_TERMIOS
bool "Serial driver TERMIOS supported"
- depends on STM32_USART1 || STM32_USART2 || STM32_USART3 || STM32_UART4 || STM32_UART5 || STM32_USART6
+ depends on STM32_USART1 || STM32_USART2 || STM32_USART3 || STM32_UART4 || STM32_UART5 || STM32_USART6 || STM32_USART7 || STM32_USART8
default n
---help---
Serial driver supports termios.h interfaces (tcsetattr, tcflush, etc.).
diff --git a/nuttx/arch/arm/src/stm32/chip/stm32_flash.h b/nuttx/arch/arm/src/stm32/chip/stm32_flash.h
index d6fcecc11..9de83893d 100644
--- a/nuttx/arch/arm/src/stm32/chip/stm32_flash.h
+++ b/nuttx/arch/arm/src/stm32/chip/stm32_flash.h
@@ -55,6 +55,7 @@
#elif defined(CONFIG_STM32_STM32F20XX) || defined(CONFIG_STM32_STM32F40XX)
# define STM32_FLASH_NPAGES 8
# define STM32_FLASH_PAGESIZE (128*1024)
+ /* XXX this is wrong for 427, and not really right for 40x due to mixed page sizes */
#endif
#define STM32_FLASH_SIZE (STM32_FLASH_NPAGES * STM32_FLASH_PAGESIZE)
@@ -74,6 +75,9 @@
#elif defined(CONFIG_STM32_STM32F20XX) || defined(CONFIG_STM32_STM32F40XX)
# define STM32_FLASH_OPTCR_OFFSET 0x0014
#endif
+#if defined(CONFIG_STM32_STM32F427)
+# define STM32_FLASH_OPTCR1_OFFSET 0x0018
+#endif
/* Register Addresses ***************************************************************/
@@ -88,7 +92,10 @@
# define STM32_FLASH_OBR (STM32_FLASHIF_BASE+STM32_FLASH_OBR_OFFSET)
# define STM32_FLASH_WRPR (STM32_FLASHIF_BASE+STM32_FLASH_WRPR_OFFSET)
#elif defined(CONFIG_STM32_STM32F20XX) || defined(CONFIG_STM32_STM32F40XX)
-# define STM32_FLASH_OPTCR (STM32_FLASHIF_BASE+STM32_FLASH_OPTCR_OFFSET)
+# define STM32_FLASH_OPTCR (STM32_FLASHIF_BASE+STM32_FLASH_OPTCR_OFFSET)
+#endif
+#if defined(CONFIG_STM32_STM32F427)
+# define STM32_FLASH_OPTCR1 (STM32_FLASHIF_BASE+STM32_FLASH_OPTCR1_OFFSET)
#endif
/* Register Bitfield Definitions ****************************************************/
@@ -150,10 +157,14 @@
#elif defined(CONFIG_STM32_STM32F20XX) || defined(CONFIG_STM32_STM32F40XX)
# define FLASH_CR_PG (1 << 0) /* Bit 0: Programming */
# define FLASH_CR_SER (1 << 1) /* Bit 1: Sector Erase */
-# define FLASH_CR_MER (1 << 2) /* Bit 2: Mass Erase */
+# define FLASH_CR_MER (1 << 2) /* Bit 2: Mass Erase sectors 0..11 */
# define FLASH_CR_SNB_SHIFT (3) /* Bits 3-6: Sector number */
# define FLASH_CR_SNB_MASK (15 << FLASH_CR_SNB_SHIFT)
+#if defined(CONFIG_STM32_STM32F427)
+# define FLASH_CR_SNB(n) (((n % 12) << FLASH_CR_SNB_SHIFT) | ((n / 12) << 7)) /* Sector n, n=0..23 */
+#else
# define FLASH_CR_SNB(n) ((n) << FLASH_CR_SNB_SHIFT) /* Sector n, n=0..11 */
+#endif
# define FLASH_CR_PSIZE_SHIFT (8) /* Bits 8-9: Program size */
# define FLASH_CR_PSIZE_MASK (3 << FLASH_CR_PSIZE_SHIFT)
# define FLASH_CR_PSIZE_X8 (0 << FLASH_CR_PSIZE_SHIFT) /* 00 program x8 */
@@ -164,6 +175,9 @@
# define FLASH_CR_ERRIE (1 << 25) /* Bit 25: Error interrupt enable */
# define FLASH_CR_LOCK (1 << 31) /* Bit 31: Lock */
#endif
+#if defined(CONFIG_STM32_STM32F427)
+# define FLASH_CR_MER1 (1 << 15) /* Bit 15: Mass Erase sectors 12..23 */
+#endif
/* Flash Option Control Register (OPTCR) */
@@ -187,5 +201,12 @@
# define FLASH_OPTCR_NWRP_MASK (0xfff << FLASH_OPTCR_NWRP_SHIFT)
#endif
+/* Flash Option Control Register (OPTCR1) */
+
+#if defined(CONFIG_STM32_STM32F427)
+# define FLASH_OPTCR1_NWRP_SHIFT (16) /* Bits 16-27: Not write protect (high bank) */
+# define FLASH_OPTCR1_NWRP_MASK (0xfff << FLASH_OPTCR_NWRP_SHIFT)
+#endif
+
#endif /* __ARCH_ARM_SRC_STM32_CHIP_STM32_FLASH_H */
diff --git a/nuttx/arch/arm/src/stm32/chip/stm32_i2c.h b/nuttx/arch/arm/src/stm32/chip/stm32_i2c.h
index f481245e0..cb2934d10 100644
--- a/nuttx/arch/arm/src/stm32/chip/stm32_i2c.h
+++ b/nuttx/arch/arm/src/stm32/chip/stm32_i2c.h
@@ -51,6 +51,9 @@
#define STM32_I2C_SR2_OFFSET 0x0018 /* Status register 2 (16-bit) */
#define STM32_I2C_CCR_OFFSET 0x001c /* Clock control register (16-bit) */
#define STM32_I2C_TRISE_OFFSET 0x0020 /* TRISE Register (16-bit) */
+#ifdef CONFIG_STM32_STM32F427
+# define STM32_I2C_FLTR_OFFSET 0x0024 /* FLTR Register (16-bit) */
+#endif
/* Register Addresses ***************************************************************/
@@ -64,6 +67,9 @@
# define STM32_I2C1_SR2 (STM32_I2C1_BASE+STM32_I2C_SR2_OFFSET)
# define STM32_I2C1_CCR (STM32_I2C1_BASE+STM32_I2C_CCR_OFFSET)
# define STM32_I2C1_TRISE (STM32_I2C1_BASE+STM32_I2C_TRISE_OFFSET)
+# ifdef STM32_I2C_FLTR_OFFSET
+# define STM32_I2C1_FLTR (STM32_I2C1_BASE+STM32_I2C_FLTR_OFFSET)
+# endif
#endif
#if STM32_NI2C > 1
@@ -76,6 +82,9 @@
# define STM32_I2C2_SR2 (STM32_I2C2_BASE+STM32_I2C_SR2_OFFSET)
# define STM32_I2C2_CCR (STM32_I2C2_BASE+STM32_I2C_CCR_OFFSET)
# define STM32_I2C2_TRISE (STM32_I2C2_BASE+STM32_I2C_TRISE_OFFSET)
+# ifdef STM32_I2C_FLTR_OFFSET
+# define STM32_I2C2_FLTR (STM32_I2C2_BASE+STM32_I2C_FLTR_OFFSET)
+# endif
#endif
#if STM32_NI2C > 2
@@ -88,6 +97,9 @@
# define STM32_I2C3_SR2 (STM32_I2C3_BASE+STM32_I2C_SR2_OFFSET)
# define STM32_I2C3_CCR (STM32_I2C3_BASE+STM32_I2C_CCR_OFFSET)
# define STM32_I2C3_TRISE (STM32_I2C3_BASE+STM32_I2C_TRISE_OFFSET)
+# ifdef STM32_I2C_FLTR_OFFSET
+# define STM32_I2C3_FLTR (STM32_I2C3_BASE+STM32_I2C_FLTR_OFFSET)
+# endif
#endif
/* Register Bitfield Definitions ****************************************************/
@@ -188,5 +200,13 @@
#define I2C_TRISE_SHIFT (0) /* Bits 5-0: Maximum Rise Time in Fast/Standard mode (Master mode) */
#define I2C_TRISE_MASK (0x3f << I2C_TRISE_SHIFT)
+/* FLTR Register */
+
+#ifdef STM32_I2C_FLTR_OFFSET
+# define I2C_FLTR_ANOFF (1 << 4) /* Bit 4: Analog noise filter disable */
+# define I2C_FLTR_DNF_SHIFT 0 /* Bits 0-3: Digital noise filter */
+# define I2C_FLTR_DNF_MASK (0xf << I2C_FLTR_DNF_SHIFT)
+#endif
+
#endif /* __ARCH_ARM_SRC_STM32_CHIP_STM32_I2C_H */
diff --git a/nuttx/arch/arm/src/stm32/chip/stm32_pwr.h b/nuttx/arch/arm/src/stm32/chip/stm32_pwr.h
index 2ece6a357..72f19b6df 100644
--- a/nuttx/arch/arm/src/stm32/chip/stm32_pwr.h
+++ b/nuttx/arch/arm/src/stm32/chip/stm32_pwr.h
@@ -80,7 +80,16 @@
#if defined(CONFIG_STM32_STM32F20XX) || defined(CONFIG_STM32_STM32F40XX)
# define PWR_CR_FPDS (1 << 9) /* Bit 9: Flash power down in Stop mode */
-# define PWR_CR_VOS (1 << 14) /* Bit 14: Regulator voltage scaling output selection */
+# if defined(CONFIG_STM32_STM32F427)
+# define PWR_CR_ADCDC1 (1 << 13) /* Bit 13: see AN4073 for details */
+# define PWR_CR_VOS_SCALE_1 (3 << 14) /* Fmax = 168MHz */
+# define PWR_CR_VOS_SCALE_2 (2 << 14) /* Fmax = 144MHz */
+# define PWR_CR_VOS_SCALE_3 (1 << 14) /* Fmax = 120MHz */
+# define PWR_CR_VOS_MASK (3 << 14) /* Bits 14-15: Regulator voltage scaling output selection */
+# else
+# define PWR_CR_VOS (1 << 14) /* Bit 14: Regulator voltage scaling output selection */
+ /* 0: Fmax = 144MHz 1: Fmax = 168MHz */
+# endif
#endif
/* Power control/status register */
diff --git a/nuttx/arch/arm/src/stm32/chip/stm32_syscfg.h b/nuttx/arch/arm/src/stm32/chip/stm32_syscfg.h
index 6642b1305..55133b3da 100644
--- a/nuttx/arch/arm/src/stm32/chip/stm32_syscfg.h
+++ b/nuttx/arch/arm/src/stm32/chip/stm32_syscfg.h
@@ -89,6 +89,11 @@
/* SYSCFG peripheral mode configuration register */
#define SYSCFG_PMC_MII_RMII_SEL (1 << 23) /* Bit 23: Ethernet PHY interface selection */
+#ifdef CONFIG_STM32_STM32F427
+# define SYSCFG_PMC_ADC3DC2 (1 << 18) /* Bit 18: See AN4073 */
+# define SYSCFG_PMC_ADC2DC2 (1 << 17) /* Bit 17: See AN4073 */
+# define SYSCFG_PMC_ADC1DC2 (1 << 16) /* Bit 16: See AN4073 */
+#endif
/* SYSCFG external interrupt configuration register 1-4 */
diff --git a/nuttx/arch/arm/src/stm32/chip/stm32_uart.h b/nuttx/arch/arm/src/stm32/chip/stm32_uart.h
index d3c1e137e..38852553d 100644
--- a/nuttx/arch/arm/src/stm32/chip/stm32_uart.h
+++ b/nuttx/arch/arm/src/stm32/chip/stm32_uart.h
@@ -118,6 +118,24 @@
# define STM32_USART6_GTPR (STM32_USART6_BASE+STM32_USART_GTPR_OFFSET)
#endif
+#if STM32_NUSART > 6
+# define STM32_UART7_SR (STM32_UART7_BASE+STM32_USART_SR_OFFSET)
+# define STM32_UART7_DR (STM32_UART7_BASE+STM32_USART_DR_OFFSET)
+# define STM32_UART7_BRR (STM32_UART7_BASE+STM32_USART_BRR_OFFSET)
+# define STM32_UART7_CR1 (STM32_UART7_BASE+STM32_USART_CR1_OFFSET)
+# define STM32_UART7_CR2 (STM32_UART7_BASE+STM32_USART_CR2_OFFSET)
+# define STM32_UART7_CR3 (STM32_UART7_BASE+STM32_USART_CR3_OFFSET)
+#endif
+
+#if STM32_NUSART > 7
+# define STM32_UART8_SR (STM32_UART8_BASE+STM32_USART_SR_OFFSET)
+# define STM32_UART8_DR (STM32_UART8_BASE+STM32_USART_DR_OFFSET)
+# define STM32_UART8_BRR (STM32_UART8_BASE+STM32_USART_BRR_OFFSET)
+# define STM32_UART8_CR1 (STM32_UART8_BASE+STM32_USART_CR1_OFFSET)
+# define STM32_UART8_CR2 (STM32_UART8_BASE+STM32_USART_CR2_OFFSET)
+# define STM32_UART8_CR3 (STM32_UART8_BASE+STM32_USART_CR3_OFFSET)
+#endif
+
/* Register Bitfield Definitions ****************************************************/
/* Status register */
diff --git a/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_dma.h b/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_dma.h
index ab0cfceac..ddd0413a5 100644
--- a/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_dma.h
+++ b/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_dma.h
@@ -438,11 +438,21 @@
#define DMAMAP_USART2_TX STM32_DMA_MAP(DMA1,DMA_STREAM6,DMA_CHAN4)
#define DMAMAP_UART5_TX STM32_DMA_MAP(DMA1,DMA_STREAM7,DMA_CHAN4)
+#ifdef CONFIG_STM32_STM32F427
+# define DMAMAP_UART8_TX STM32_DMA_MAP(CMA1,DMA_STREAM0,DMA_CHAN5)
+# define DMAMAP_UART7_TX STM32_DMA_MAP(CMA1,DMA_STREAM1,DMA_CHAN5)
+#endif
#define DMAMAP_TIM3_CH4 STM32_DMA_MAP(DMA1,DMA_STREAM2,DMA_CHAN5)
#define DMAMAP_TIM3_UP STM32_DMA_MAP(DMA1,DMA_STREAM2,DMA_CHAN5)
+#ifdef CONFIG_STM32_STM32F427
+# define DMAMAP_UART7_RX STM32_DMA_MAP(CMA1,DMA_STREAM3,DMA_CHAN5)
+#endif
#define DMAMAP_TIM3_CH1 STM32_DMA_MAP(DMA1,DMA_STREAM4,DMA_CHAN5)
#define DMAMAP_TIM3_TRIG STM32_DMA_MAP(DMA1,DMA_STREAM4,DMA_CHAN5)
#define DMAMAP_TIM3_CH2 STM32_DMA_MAP(DMA1,DMA_STREAM5,DMA_CHAN5)
+#ifdef CONFIG_STM32_STM32F427
+# define DMAMAP_UART8_RX STM32_DMA_MAP(CMA1,DMA_STREAM6,DMA_CHAN5)
+#endif
#define DMAMAP_TIM3_CH3 STM32_DMA_MAP(DMA1,DMA_STREAM7,DMA_CHAN5)
#define DMAMAP_TIM5_CH3 STM32_DMA_MAP(DMA1,DMA_STREAM0,DMA_CHAN6)
@@ -475,10 +485,18 @@
#define DMAMAP_DCMI_1 STM32_DMA_MAP(DMA2,DMA_STREAM1,DMA_CHAN1)
#define DMAMAP_ADC2_1 STM32_DMA_MAP(DMA2,DMA_STREAM2,DMA_CHAN1)
#define DMAMAP_ADC2_2 STM32_DMA_MAP(DMA2,DMA_STREAM3,DMA_CHAN1)
+#ifdef CONFIG_STM32_STM32F427
+# define DMAMAP_SPI6_TX STM32_DMA_MAP(DMA2,DMA_STREAM5,DMA_CHAN1)
+# define DMAMAP_SPI6_RX STM32_DMA_MAP(DMA2,DMA_STREAM6,DMA_CHAN1)
+#endif
#define DMAMAP_DCMI_2 STM32_DMA_MAP(DMA2,DMA_STREAM7,DMA_CHAN1)
#define DMAMAP_ADC3_1 STM32_DMA_MAP(DMA2,DMA_STREAM0,DMA_CHAN2)
#define DMAMAP_ADC3_2 STM32_DMA_MAP(DMA2,DMA_STREAM1,DMA_CHAN2)
+#ifdef CONFIG_STM32_STM32F427
+# define DMAMAP_SPI5_RX_1 STM32_DMA_MAP(DMA2,DMA_STREAM3,DMA_CHAN2)
+# define DMAMAP_SPI5_TX_1 STM32_DMA_MAP(DMA2,DMA_STREAM4,DMA_CHAN2)
+#endif
#define DMAMAP_CRYP_OUT STM32_DMA_MAP(DMA2,DMA_STREAM5,DMA_CHAN2)
#define DMAMAP_CRYP_IN STM32_DMA_MAP(DMA2,DMA_STREAM6,DMA_CHAN2)
#define DMAMAP_HASH_IN STM32_DMA_MAP(DMA2,DMA_STREAM7,DMA_CHAN2)
@@ -488,6 +506,11 @@
#define DMAMAP_SPI1_TX_1 STM32_DMA_MAP(DMA2,DMA_STREAM3,DMA_CHAN3)
#define DMAMAP_SPI1_TX_2 STM32_DMA_MAP(DMA2,DMA_STREAM5,DMA_CHAN3)
+#ifdef CONFIG_STM32_STM32F427
+# define DMAMAP_SPI4_RX_1 STM32_DMA_MAP(DMA2,DMA_STREAM0,DMA_CHAN4)
+# define DMAMAP_SPI4_TX_1 STM32_DMA_MAP(DMA2,DMA_STREAM1,DMA_CHAN4)
+#endif
+#define DMAMAP_SPI4_TX_1 STM32_DMA_MAP(DMA2,DMA_STREAM1,DMA_CHAN4)
#define DMAMAP_USART1_RX_1 STM32_DMA_MAP(DMA2,DMA_STREAM2,DMA_CHAN4)
#define DMAMAP_SDIO_1 STM32_DMA_MAP(DMA2,DMA_STREAM3,DMA_CHAN4)
#define DMAMAP_USART1_RX_2 STM32_DMA_MAP(DMA2,DMA_STREAM5,DMA_CHAN4)
@@ -496,6 +519,10 @@
#define DMAMAP_USART6_RX_1 STM32_DMA_MAP(DMA2,DMA_STREAM1,DMA_CHAN5)
#define DMAMAP_USART6_RX_2 STM32_DMA_MAP(DMA2,DMA_STREAM2,DMA_CHAN5)
+#ifdef CONFIG_STM32_STM32F427
+# define DMAMAP_SPI4_RX_2 STM32_DMA_MAP(DMA2,DMA_STREAM3,DMA_CHAN5)
+# define DMAMAP_SPI4_TX_2 STM32_DMA_MAP(DMA2,DMA_STREAM4,DMA_CHAN5)
+#endif
#define DMAMAP_USART6_TX_1 STM32_DMA_MAP(DMA2,DMA_STREAM6,DMA_CHAN5)
#define DMAMAP_USART6_TX_2 STM32_DMA_MAP(DMA2,DMA_STREAM7,DMA_CHAN5)
@@ -512,7 +539,11 @@
#define DMAMAP_TIM8_UP STM32_DMA_MAP(DMA2,DMA_STREAM1,DMA_CHAN7)
#define DMAMAP_TIM8_CH1_2 STM32_DMA_MAP(DMA2,DMA_STREAM2,DMA_CHAN7)
#define DMAMAP_TIM8_CH2_2 STM32_DMA_MAP(DMA2,DMA_STREAM3,DMA_CHAN7)
-#define DMAMAP_TIM8_CH3_2 STM32_DMA_MAP(DMA2,DMA_STREAM5,DMA_CHAN7)
+#define DMAMAP_TIM8_CH3_2 STM32_DMA_MAP(DMA2,DMA_STREAM4,DMA_CHAN7)
+#ifdef CONFIG_STM32_STM32F427
+# define DMAMAP_SPI5_RX_2 STM32_DMA_MAP(DMA2,DMA_STREAM5,DMA_CHAN7)
+# define DMAMAP_SPI5_TX_2 STM32_DMA_MAP(DMA2,DMA_STREAM6,DMA_CHAN7)
+#endif
#define DMAMAP_TIM8_CH4 STM32_DMA_MAP(DMA2,DMA_STREAM7,DMA_CHAN7)
#define DMAMAP_TIM8_TRIG STM32_DMA_MAP(DMA2,DMA_STREAM7,DMA_CHAN7)
#define DMAMAP_TIM8_COM STM32_DMA_MAP(DMA2,DMA_STREAM7,DMA_CHAN7)
diff --git a/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_memorymap.h b/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_memorymap.h
index 6b9912121..6dc9530fb 100644
--- a/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_memorymap.h
+++ b/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_memorymap.h
@@ -140,6 +140,8 @@
#define STM32_CAN2_BASE 0x40006800 /* 0x40006800-0x40006bff: bxCAN2 */
#define STM32_PWR_BASE 0x40007000 /* 0x40007000-0x400073ff: Power control PWR */
#define STM32_DAC_BASE 0x40007400 /* 0x40007400-0x400077ff: DAC */
+#define STM32_UART7_BASE 0x40007800 /* 0x40007800-0x40007bff: UART7 */
+#define STM32_UART8_BASE 0x40007c00 /* 0x40007c00-0x40007fff: UART8 */
/* APB2 Base Addresses **************************************************************/
@@ -154,11 +156,14 @@
# define STM32_ADCCMN_BASE 0x40012300 /* Common */
#define STM32_SDIO_BASE 0x40012c00 /* 0x40012c00-0x40012fff: SDIO */
#define STM32_SPI1_BASE 0x40013000 /* 0x40013000-0x400133ff: SPI1 */
+#define STM32_SPI4_BASE 0x40013400 /* 0x40013000-0x400137ff: SPI4 */
#define STM32_SYSCFG_BASE 0x40013800 /* 0x40013800-0x40013bff: SYSCFG */
#define STM32_EXTI_BASE 0x40013c00 /* 0x40013c00-0x40013fff: EXTI */
#define STM32_TIM9_BASE 0x40014000 /* 0x40014000-0x400143ff: TIM9 timer */
#define STM32_TIM10_BASE 0x40014400 /* 0x40014400-0x400147ff: TIM10 timer */
#define STM32_TIM11_BASE 0x40014800 /* 0x40014800-0x40014bff: TIM11 timer */
+#define STM32_SPI5_BASE 0x40015000 /* 0x40015000-0x400153ff: SPI5 */
+#define STM32_SPI6_BASE 0x40015400 /* 0x40015400-0x400157ff: SPI6 */
/* AHB1 Base Addresses **************************************************************/
diff --git a/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_pinmap.h b/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_pinmap.h
index 31e51caf0..ed3f09c01 100644
--- a/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_pinmap.h
+++ b/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_pinmap.h
@@ -331,6 +331,9 @@
#define GPIO_I2S2_CK_1 (GPIO_ALT|GPIO_AF5|GPIO_PORTB|GPIO_PIN10)
#define GPIO_I2S2_CK_2 (GPIO_ALT|GPIO_AF5|GPIO_PORTB|GPIO_PIN13)
#define GPIO_I2S2_CK_3 (GPIO_ALT|GPIO_AF5|GPIO_PORTI|GPIO_PIN1)
+#ifdef CONFIG_STM32_STM32F427
+# define GPIO_I2S2_CK_4 (GPIO_ALT|GPIO_AF5|GPIO_PORTD|GPIO_PIN3)
+#endif
#define GPIO_I2S2_MCK (GPIO_ALT|GPIO_AF5|GPIO_PORTC|GPIO_PIN6)
#define GPIO_I2S2_SD_1 (GPIO_ALT|GPIO_AF5|GPIO_PORTC|GPIO_PIN15)
#define GPIO_I2S2_SD_2 (GPIO_ALT|GPIO_AF5|GPIO_PORTC|GPIO_PIN3)
@@ -339,6 +342,9 @@
#define GPIO_I2S2_WS_2 (GPIO_ALT|GPIO_AF5|GPIO_PORTB|GPIO_PIN6)
#define GPIO_I2S2_WS_3 (GPIO_ALT|GPIO_AF5|GPIO_PORTB|GPIO_PIN9)
#define GPIO_I2S2_WS_4 (GPIO_ALT|GPIO_AF5|GPIO_PORTI|GPIO_PIN0)
+#ifdef CONFIG_STM32_STM32F427
+# define GPIO_I2S2_WS_6 (GPIO_ALT|GPIO_AF5|GPIO_PORTB|GPIO_PIN6)
+#endif
#define GPIO_I2S2EXT_SD_1 (GPIO_ALT|GPIO_AF6|GPIO_PORTB|GPIO_PIN14)
#define GPIO_I2S2EXT_SD_2 (GPIO_ALT|GPIO_AF6|GPIO_PORTC|GPIO_PIN2)
@@ -349,6 +355,9 @@
#define GPIO_I2S3_MCK (GPIO_ALT|GPIO_AF6|GPIO_PORTC|GPIO_PIN7)
#define GPIO_I2S3_SD_1 (GPIO_ALT|GPIO_AF6|GPIO_PORTB|GPIO_PIN5)
#define GPIO_I2S3_SD_2 (GPIO_ALT|GPIO_AF6|GPIO_PORTC|GPIO_PIN12)
+#ifdef CONFIG_STM32_STM32F427
+# define GPIO_I2S3_SD_3 (GPIO_ALT|GPIO_AF6|GPIO_PORTD|GPIO_PIN6)
+#endif
#define GPIO_I2S3_WS_1 (GPIO_ALT|GPIO_AF6|GPIO_PORTA|GPIO_PIN4)
#define GPIO_I2S3_WS_2 (GPIO_ALT|GPIO_AF6|GPIO_PORTA|GPIO_PIN15)
@@ -428,7 +437,7 @@
#define GPIO_SPI2_MISO_1 (GPIO_ALT|GPIO_AF5|GPIO_PORTB|GPIO_PIN14)
#define GPIO_SPI2_MISO_2 (GPIO_ALT|GPIO_AF5|GPIO_PORTC|GPIO_PIN2)
#define GPIO_SPI2_MISO_3 (GPIO_ALT|GPIO_AF5|GPIO_PORTI|GPIO_PIN2)
-#define GPIO_SPI2_MOSI_1 (GPIO_ALT|GPIO_AF5|GPIO_PORTC|GPIO_PIN15)
+#define GPIO_SPI2_MOSI_1 (GPIO_ALT|GPIO_AF5|GPIO_PORTB|GPIO_PIN15)
#define GPIO_SPI2_MOSI_2 (GPIO_ALT|GPIO_AF5|GPIO_PORTC|GPIO_PIN3)
#define GPIO_SPI2_MOSI_3 (GPIO_ALT|GPIO_AF5|GPIO_PORTI|GPIO_PIN3)
#define GPIO_SPI2_NSS_1 (GPIO_ALT|GPIO_AF5|GPIO_PORTB|GPIO_PIN12)
@@ -437,16 +446,45 @@
#define GPIO_SPI2_SCK_1 (GPIO_ALT|GPIO_AF5|GPIO_PORTB|GPIO_PIN10)
#define GPIO_SPI2_SCK_2 (GPIO_ALT|GPIO_AF5|GPIO_PORTB|GPIO_PIN13)
#define GPIO_SPI2_SCK_3 (GPIO_ALT|GPIO_AF5|GPIO_PORTI|GPIO_PIN1)
+#ifdef CONFIG_STM32_STM32F427
+# define GPIO_SPI2_SCK_4 (GPIO_ALT|GPIO_AF5|GPIO_PORTD|GPIO_PIN3)
+#endif
#define GPIO_SPI3_MISO_1 (GPIO_ALT|GPIO_AF6|GPIO_PORTB|GPIO_PIN4)
#define GPIO_SPI3_MISO_2 (GPIO_ALT|GPIO_AF6|GPIO_PORTC|GPIO_PIN11)
#define GPIO_SPI3_MOSI_1 (GPIO_ALT|GPIO_AF6|GPIO_PORTB|GPIO_PIN5)
#define GPIO_SPI3_MOSI_2 (GPIO_ALT|GPIO_AF6|GPIO_PORTC|GPIO_PIN12)
+#ifdef CONFIG_STM32_STM32F427
+# define GPIO_SPI3_MOSI_3 (GPIO_ALT|GPIO_AF6|GPIO_PORTD|GPIO_PIN6)
+#endif
#define GPIO_SPI3_NSS_1 (GPIO_ALT|GPIO_AF6|GPIO_PORTA|GPIO_PIN15)
#define GPIO_SPI3_NSS_2 (GPIO_ALT|GPIO_AF6|GPIO_PORTA|GPIO_PIN4)
#define GPIO_SPI3_SCK_1 (GPIO_ALT|GPIO_AF6|GPIO_PORTB|GPIO_PIN3)
#define GPIO_SPI3_SCK_2 (GPIO_ALT|GPIO_AF6|GPIO_PORTC|GPIO_PIN10)
+#define GPIO_SPI4_MISO_1 (GPIO_ALT|GPIO_AF5|GPIO_PORTE|GPIO_PIN5)
+#define GPIO_SPI4_MISO_2 (GPIO_ALT|GPIO_AF5|GPIO_PORTE|GPIO_PIN13)
+#define GPIO_SPI4_MOSI_1 (GPIO_ALT|GPIO_AF5|GPIO_PORTE|GPIO_PIN6)
+#define GPIO_SPI4_MOSI_2 (GPIO_ALT|GPIO_AF5|GPIO_PORTE|GPIO_PIN14)
+#define GPIO_SPI4_NSS_1 (GPIO_ALT|GPIO_AF5|GPIO_PORTE|GPIO_PIN4)
+#define GPIO_SPI4_NSS_2 (GPIO_ALT|GPIO_AF5|GPIO_PORTE|GPIO_PIN11)
+#define GPIO_SPI4_SCK_1 (GPIO_ALT|GPIO_AF5|GPIO_PORTE|GPIO_PIN2)
+#define GPIO_SPI4_SCK_2 (GPIO_ALT|GPIO_AF5|GPIO_PORTE|GPIO_PIN12)
+
+#define GPIO_SPI5_MISO_1 (GPIO_ALT|GPIO_AF5|GPIO_PORTF|GPIO_PIN8)
+#define GPIO_SPI5_MISO_2 (GPIO_ALT|GPIO_AF5|GPIO_PORTH|GPIO_PIN7)
+#define GPIO_SPI5_MOSI_1 (GPIO_ALT|GPIO_AF5|GPIO_PORTF|GPIO_PIN9)
+#define GPIO_SPI5_MOSI_2 (GPIO_ALT|GPIO_AF5|GPIO_PORTF|GPIO_PIN11)
+#define GPIO_SPI5_NSS_1 (GPIO_ALT|GPIO_AF5|GPIO_PORTF|GPIO_PIN6)
+#define GPIO_SPI5_NSS_2 (GPIO_ALT|GPIO_AF5|GPIO_PORTH|GPIO_PIN5)
+#define GPIO_SPI5_SCK_1 (GPIO_ALT|GPIO_AF5|GPIO_PORTF|GPIO_PIN7)
+#define GPIO_SPI5_SCK_2 (GPIO_ALT|GPIO_AF5|GPIO_PORTH|GPIO_PIN6)
+
+#define GPIO_SPI6_MISO (GPIO_ALT|GPIO_AF5|GPIO_PORTG|GPIO_PIN12)
+#define GPIO_SPI6_MOSI (GPIO_ALT|GPIO_AF5|GPIO_PORTG|GPIO_PIN14)
+#define GPIO_SPI6_NSS (GPIO_ALT|GPIO_AF5|GPIO_PORTG|GPIO_PIN8)
+#define GPIO_SPI6_SCK (GPIO_ALT|GPIO_AF5|GPIO_PORTG|GPIO_PIN13)
+
/* Timers */
#define GPIO_TIM1_BKIN_1 (GPIO_ALT|GPIO_AF1|GPIO_PORTA|GPIO_PIN6)
@@ -691,5 +729,15 @@
#define GPIO_USART6_TX_1 (GPIO_ALT|GPIO_AF8|GPIO_PULLUP|GPIO_SPEED_100MHz|GPIO_PUSHPULL|GPIO_PORTC|GPIO_PIN6)
#define GPIO_USART6_TX_2 (GPIO_ALT|GPIO_AF8|GPIO_PULLUP|GPIO_SPEED_100MHz|GPIO_PUSHPULL|GPIO_PORTG|GPIO_PIN14)
+#ifdef CONFIG_STM32_STM32F427
+# define GPIO_UART7_RX_1 (GPIO_ALT|GPIO_AF8|GPIO_PULLUP|GPIO_SPEED_100MHz|GPIO_PUSHPULL|GPIO_PORTE|GPIO_PIN7)
+# define GPIO_UART7_RX_2 (GPIO_ALT|GPIO_AF8|GPIO_PULLUP|GPIO_SPEED_100MHz|GPIO_PUSHPULL|GPIO_PORTF|GPIO_PIN6)
+# define GPIO_UART7_TX_1 (GPIO_ALT|GPIO_AF8|GPIO_PULLUP|GPIO_SPEED_100MHz|GPIO_PUSHPULL|GPIO_PORTE|GPIO_PIN8)
+# define GPIO_UART7_TX_2 (GPIO_ALT|GPIO_AF8|GPIO_PULLUP|GPIO_SPEED_100MHz|GPIO_PUSHPULL|GPIO_PORTF|GPIO_PIN7)
+
+# define GPIO_UART8_RX (GPIO_ALT|GPIO_AF8|GPIO_PULLUP|GPIO_SPEED_100MHz|GPIO_PUSHPULL|GPIO_PORTE|GPIO_PIN0)
+# define GPIO_UART8_TX (GPIO_ALT|GPIO_AF8|GPIO_PULLUP|GPIO_SPEED_100MHz|GPIO_PUSHPULL|GPIO_PORTE|GPIO_PIN1)
+#endif
+
#endif /* __ARCH_ARM_SRC_STM32_CHIP_STM32F40XXX_PINMAP_H */
diff --git a/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_rcc.h b/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_rcc.h
index 04cb05741..8ab09c478 100644
--- a/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_rcc.h
+++ b/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_rcc.h
@@ -65,6 +65,9 @@
#define STM32_RCC_CSR_OFFSET 0x0074 /* Control/status register */
#define STM32_RCC_SSCGR_OFFSET 0x0080 /* Spread spectrum clock generation register */
#define STM32_RCC_PLLI2SCFGR_OFFSET 0x0084 /* PLLI2S configuration register */
+#ifdef CONFIG_STM32_STM32F427
+# define STM32_RCC_DCKCFGR_OFFSET 0x008c /* Dedicated clocks configuration register */
+#endif
/* Register Addresses *******************************************************************************/
@@ -91,6 +94,9 @@
#define STM32_RCC_CSR (STM32_RCC_BASE+STM32_RCC_CSR_OFFSET)
#define STM32_RCC_SSCGR (STM32_RCC_BASE+STM32_RCC_SSCGR_OFFSET)
#define STM32_RCC_PLLI2SCFGR (STM32_RCC_BASE+STM32_RCC_PLLI2SCFGR_OFFSET)
+#ifdef CONFIG_STM32_STM32F427
+# define STM32_RCC_DCKCFGR (STM32_RCC_BASE+STM32_RCC_DCKCFGR_OFFSET)
+#endif
/* Register Bitfield Definitions ********************************************************************/
@@ -282,6 +288,10 @@
#define RCC_APB1RSTR_CAN2RST (1 << 26) /* Bit 26: CAN2 reset */
#define RCC_APB1RSTR_PWRRST (1 << 28) /* Bit 28: Power interface reset */
#define RCC_APB1RSTR_DACRST (1 << 29) /* Bit 29: DAC reset */
+#ifdef CONFIG_STM32_STM32F427
+# define RCC_APB1RSTR_UART7RST (1 << 30) /* Bit 30: USART 7 reset */
+# define RCC_APB1RSTR_UART8RST (1 << 31) /* Bit 31: USART 8 reset */
+#endif
/* APB2 Peripheral reset register */
@@ -291,11 +301,18 @@
#define RCC_APB2RSTR_USART6RST (1 << 5) /* Bit 5: USART6 reset */
#define RCC_APB2RSTR_ADCRST (1 << 8) /* Bit 8: ADC interface reset (common to all ADCs) */
#define RCC_APB2RSTR_SDIORST (1 << 11) /* Bit 11: SDIO reset */
-#define RCC_APB2RSTR_SPI1RST (1 << 12) /* Bit 12: SPI 1 reset */
+#define RCC_APB2RSTR_SPI1RST (1 << 12) /* Bit 12: SPI1 reset */
+#ifdef CONFIG_STM32_STM32F427
+# define RCC_APB2RSTR_SPI4RST (1 << 13) /* Bit 13: SPI4 reset */
+#endif
#define RCC_APB2RSTR_SYSCFGRST (1 << 14) /* Bit 14: System configuration controller reset */
#define RCC_APB2RSTR_TIM9RST (1 << 16) /* Bit 16: TIM9 reset */
#define RCC_APB2RSTR_TIM10RST (1 << 17) /* Bit 17: TIM10 reset */
#define RCC_APB2RSTR_TIM11RST (1 << 18) /* Bit 18: TIM11 reset */
+#ifdef CONFIG_STM32_STM32F427
+# define RCC_APB2RSTR_SPI5RST (1 << 20) /* Bit 20: SPI 5 reset */
+# define RCC_APB2RSTR_SPI6RST (1 << 21) /* Bit 21: SPI 6 reset */
+#endif
/* AHB1 Peripheral Clock enable register */
@@ -358,6 +375,10 @@
#define RCC_APB1ENR_CAN2EN (1 << 26) /* Bit 26: CAN 2 clock enable */
#define RCC_APB1ENR_PWREN (1 << 28) /* Bit 28: Power interface clock enable */
#define RCC_APB1ENR_DACEN (1 << 29) /* Bit 29: DAC interface clock enable */
+#ifdef CONFIG_STM32_STM32F427
+# define RCC_APB1ENR_UART7EN (1 << 30) /* Bit 30: UART7 clock enable */
+# define RCC_APB1ENR_UART8EN (1 << 31) /* Bit 31: UART8 clock enable */
+#endif
/* APB2 Peripheral Clock enable register */
@@ -370,10 +391,17 @@
#define RCC_APB2ENR_ADC3EN (1 << 10) /* Bit 10: ADC3 clock enable */
#define RCC_APB2ENR_SDIOEN (1 << 11) /* Bit 11: SDIO clock enable */
#define RCC_APB2ENR_SPI1EN (1 << 12) /* Bit 12: SPI1 clock enable */
+#ifdef CONFIG_STM32_STM32F427
+# define RCC_APB2ENR_SPI4EN (1 << 13) /* Bit 13: SPI4 clock enable */
+#endif
#define RCC_APB2ENR_SYSCFGEN (1 << 14) /* Bit 14: System configuration controller clock enable */
#define RCC_APB2ENR_TIM9EN (1 << 16) /* Bit 16: TIM9 clock enable */
#define RCC_APB2ENR_TIM10EN (1 << 17) /* Bit 17: TIM10 clock enable */
#define RCC_APB2ENR_TIM11EN (1 << 18) /* Bit 18: TIM11 clock enable */
+#ifdef CONFIG_STM32_STM32F427
+# define RCC_APB2ENR_SPI5EN (1 << 20) /* Bit 20: SPI5 clock enable */
+# define RCC_APB2ENR_SPI6EN (1 << 21) /* Bit 21: SPI6 clock enable */
+#endif
/* RCC AHB1 low power modeperipheral clock enable register */
@@ -392,6 +420,9 @@
#define RCC_AHB1LPENR_SRAM1LPEN (1 << 16) /* Bit 16: SRAM 1 interface clock enable during Sleep mode */
#define RCC_AHB1LPENR_SRAM2LPEN (1 << 17) /* Bit 17: SRAM 2 interface clock enable during Sleep mode */
#define RCC_AHB1LPENR_BKPSRAMLPEN (1 << 18) /* Bit 18: Backup SRAM interface clock enable during Sleep mode */
+#ifdef CONFIG_STM32_STM32F427
+# define RCC_AHB1LPENR_SRAM3LPEN (1 << 19) /* Bit 19: SRAM 3 interface clock enable during Sleep mode */
+#endif
#define RCC_AHB1LPENR_CCMDATARAMLPEN (1 << 20) /* Bit 20: CCM data RAM clock enable during Sleep mode */
#define RCC_AHB1LPENR_DMA1LPEN (1 << 21) /* Bit 21: DMA1 clock enable during Sleep mode */
#define RCC_AHB1LPENR_DMA2LPEN (1 << 22) /* Bit 22: DMA2 clock enable during Sleep mode */
@@ -440,6 +471,10 @@
#define RCC_APB1LPENR_CAN2LPEN (1 << 26) /* Bit 26: CAN 2 clock enable during Sleep mode */
#define RCC_APB1LPENR_PWRLPEN (1 << 28) /* Bit 28: Power interface clock enable during Sleep mode */
#define RCC_APB1LPENR_DACLPEN (1 << 29) /* Bit 29: DAC interface clock enable during Sleep mode */
+#ifdef CONFIG_STM32_STM32F427
+# define RCC_APB1LPENR_UART7LPEN (1 << 30) /* Bit 30: UART7 clock enable during Sleep mode */
+# define RCC_APB1LPENR_UART8LPEN (1 << 31) /* Bit 31: UART8 clock enable during Sleep mode */
+#endif
/* RCC APB2 low power modeperipheral clock enable register */
@@ -452,10 +487,17 @@
#define RCC_APB2LPENR_ADC3LPEN (1 << 10) /* Bit 10: ADC3 clock enable during Sleep mode */
#define RCC_APB2LPENR_SDIOLPEN (1 << 11) /* Bit 11: SDIO clock enable during Sleep mode */
#define RCC_APB2LPENR_SPI1LPEN (1 << 12) /* Bit 12: SPI1 clock enable during Sleep mode */
+#ifdef CONFIG_STM32_STM32F427
+# define RCC_APB2LPENR_SPI4LPEN (1 << 13) /* Bit 13: SPI4 clock enable during Sleep mode */
+#endif
#define RCC_APB2LPENR_SYSCFGLPEN (1 << 14) /* Bit 14: System configuration controller clock enable during Sleep mode */
#define RCC_APB2LPENR_TIM9LPEN (1 << 16) /* Bit 16: TIM9 clock enable during Sleep mode */
#define RCC_APB2LPENR_TIM10LPEN (1 << 17) /* Bit 17: TIM10 clock enable during Sleep mode */
#define RCC_APB2LPENR_TIM11LPEN (1 << 18) /* Bit 18: TIM11 clock enable during Sleep mode */
+#ifdef CONFIG_STM32_STM32F427
+# define RCC_APB2LPENR_SPI5LPEN (1 << 20) /* Bit 20: SPI5 clock enable during Sleep mode */
+# define RCC_APB2LPENR_SPI6LPEN (1 << 21) /* Bit 21: SPI6 clock enable during Sleep mode */
+#endif
/* Backup domain control register */
@@ -502,5 +544,11 @@
#define RCC_PLLI2SCFGR_PLLI2SR_SHIFT (28) /* Bits 28-30: PLLI2S division factor for I2S clocks */
#define RCC_PLLI2SCFGR_PLLI2SR_MASK (7 << RCC_PLLI2SCFGR_PLLI2SR_SHIFT)
+/* Dedicated clocks configuration register */
+
+#ifdef CONFIG_STM32_STM32F427
+# define RCC_DCKCFGR_TIMPRE (1 << 24) /* Bit 24: Timer clock prescaler selection */
+#endif
+
#endif /* __ARCH_ARM_SRC_STM32_CHIP_STM32F40XXX_RCC_H */
diff --git a/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_vectors.h b/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_vectors.h
index 8db1bd19e..31453c6a4 100644
--- a/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_vectors.h
+++ b/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_vectors.h
@@ -40,7 +40,7 @@
/* This file is included by stm32_vectors.S. It provides the macro VECTOR that
* supplies ach STM32F40xxx vector in terms of a (lower-case) ISR label and an
* (upper-case) IRQ number as defined in arch/arm/include/stm32/stm32f40xxx_irq.h.
- * stm32_vectors.S will defined the VECTOR in different ways in order to generate
+ * stm32_vectors.S will define the VECTOR macro in different ways in order to generate
* the interrupt vectors and handlers in their final form.
*/
@@ -50,9 +50,13 @@
#ifdef CONFIG_ARMV7M_CMNVECTOR
-/* Reserve 82 interrupt table entries for I/O interrupts. */
+/* Reserve interrupt table entries for I/O interrupts. */
-# define ARMV7M_PERIPHERAL_INTERRUPTS 82
+# ifdef CONFIG_STM32_STM32F427
+# define ARMV7M_PERIPHERAL_INTERRUPTS 87
+# else
+# define ARMV7M_PERIPHERAL_INTERRUPTS 82
+# endif
#else
@@ -133,10 +137,18 @@ VECTOR(stm32_i2c3er, STM32_IRQ_I2C3ER) /* Vector 16+73: I2C3 error int
VECTOR(stm32_otghsep1out, STM32_IRQ_OTGHSEP1OUT) /* Vector 16+74: USB On The Go HS End Point 1 Out global interrupt */
VECTOR(stm32_otghsep1in, STM32_IRQ_OTGHSEP1IN) /* Vector 16+75: USB On The Go HS End Point 1 In global interrupt */
VECTOR(stm32_otghswkup, STM32_IRQ_OTGHSWKUP) /* Vector 16+76: USB On The Go HS Wakeup through EXTI interrupt */
-VECTOR(stm32_otghs, STM32_IRQ_OTGHS ) /* Vector 16+77: USB On The Go HS global interrupt */
+VECTOR(stm32_otghs, STM32_IRQ_OTGHS) /* Vector 16+77: USB On The Go HS global interrupt */
VECTOR(stm32_dcmi, STM32_IRQ_DCMI) /* Vector 16+78: DCMI global interrupt */
VECTOR(stm32_cryp, STM32_IRQ_CRYP) /* Vector 16+79: CRYP crypto global interrupt */
VECTOR(stm32_hash, STM32_IRQ_HASH) /* Vector 16+80: Hash and Rng global interrupt */
VECTOR(stm32_fpu, STM32_IRQ_FPU) /* Vector 16+81: FPU global interrupt */
+#ifdef CONFIG_STM32_STM32F427
+VECTOR(stm32_uart7, STM32_IRQ_UART7) /* Vector 16+82: UART7 interrupt */
+VECTOR(stm32_uart8, STM32_IRQ_UART8) /* Vector 16+83: UART8 interrupt */
+VECTOR(stm32_spi4, STM32_IRQ_SPI4) /* Vector 16+84: SPI4 interrupt */
+VECTOR(stm32_spi5, STM32_IRQ_SPI5) /* Vector 16+85: SPI5 interrupt */
+VECTOR(stm32_spi6, STM32_IRQ_SPI6) /* Vector 16+86: SPI6 interrupt */
+#endif
+
#endif /* CONFIG_ARMV7M_CMNVECTOR */
diff --git a/nuttx/arch/arm/src/stm32/stm32_allocateheap.c b/nuttx/arch/arm/src/stm32/stm32_allocateheap.c
index 4b8707a2b..c53ff3137 100644
--- a/nuttx/arch/arm/src/stm32/stm32_allocateheap.c
+++ b/nuttx/arch/arm/src/stm32/stm32_allocateheap.c
@@ -118,19 +118,23 @@
# undef CONFIG_STM32_CCMEXCLUDE
# define CONFIG_STM32_CCMEXCLUDE 1
-/* All members of the STM32F20xxx and STM32F40xxx families have 192Kb in three banks:
+/* All members of the STM32F20xxx and STM32F40xxx families have 192KiB in three banks:
*
- * 1) 112Kb of System SRAM beginning at address 0x2000:0000
- * 2) 16Kb of System SRAM beginning at address 0x2001:c000
- * 3) 64Kb of CCM SRAM beginning at address 0x1000:0000
+ * 1) 112KiB of System SRAM beginning at address 0x2000:0000
+ * 2) 16KiB of System SRAM beginning at address 0x2001:c000
+ * 3) 64KiB of CCM SRAM beginning at address 0x1000:0000
*
- * As determined by ld.script, g_heapbase lies in the 112Kb memory
+ * The STM32F427/437 have an additional 64KiB of System SRAM beginning at
+ * address 0x2002:0000 for a total of 256KiB.
+ *
+ * As determined by ld.script, g_heapbase lies in the 112KiB memory
* region and that extends to 0x2001:0000. But the first and second memory
* regions are contiguous and treated as one in this logic that extends to
- * 0x2002:0000.
+ * 0x2002:0000 (or 0x2003:0000 for the F427/F437).
*
- * As a complication, it appears that CCM SRAM cannot be used for DMA. So, if
- * STM32 DMA is enabled, CCM SRAM should probably be excluded from the heap.
+ * As a complication, CCM SRAM cannot be used for DMA. So, if STM32 DMA is enabled,
+ * CCM SRAM should probably be excluded from the heap or the application must take
+ * extra care to ensure that DMA buffers are not allocated in CCM SRAM.
*
* In addition, external FSMC SRAM may be available.
*/
@@ -147,7 +151,11 @@
/* Set the end of system SRAM */
-# define SRAM1_END 0x20020000
+# if defined(CONFIG_STM32_STM32F427)
+# define SRAM1_END 0x20030000
+# else
+# define SRAM1_END 0x20020000
+# endif
/* Set the range of CCM SRAM as well (although we may not use it) */
diff --git a/nuttx/arch/arm/src/stm32/stm32_lowputc.c b/nuttx/arch/arm/src/stm32/stm32_lowputc.c
index 6cb07dad9..5ad0ce7d3 100644
--- a/nuttx/arch/arm/src/stm32/stm32_lowputc.c
+++ b/nuttx/arch/arm/src/stm32/stm32_lowputc.c
@@ -160,6 +160,40 @@
# define STM32_CONSOLE_RS485_DIR_POLARITY true
# endif
# endif
+#elif defined(CONFIG_UART7_SERIAL_CONSOLE)
+# define STM32_CONSOLE_BASE STM32_UART7_BASE
+# define STM32_APBCLOCK STM32_PCLK1_FREQUENCY
+# define STM32_CONSOLE_BAUD CONFIG_UART7_BAUD
+# define STM32_CONSOLE_BITS CONFIG_UART7_BITS
+# define STM32_CONSOLE_PARITY CONFIG_UART7_PARITY
+# define STM32_CONSOLE_2STOP CONFIG_UART7_2STOP
+# define STM32_CONSOLE_TX GPIO_UART7_TX
+# define STM32_CONSOLE_RX GPIO_UART7_RX
+# ifdef CONFIG_UART7_RS485
+# define STM32_CONSOLE_RS485_DIR GPIO_UART7_RS485_DIR
+# if (CONFIG_UART7_RS485_DIR_POLARITY == 0)
+# define STM32_CONSOLE_RS485_DIR_POLARITY false
+# else
+# define STM32_CONSOLE_RS485_DIR_POLARITY true
+# endif
+# endif
+#elif defined(CONFIG_UART8_SERIAL_CONSOLE)
+# define STM32_CONSOLE_BASE STM32_UART8_BASE
+# define STM32_APBCLOCK STM32_PCLK1_FREQUENCY
+# define STM32_CONSOLE_BAUD CONFIG_UART8_BAUD
+# define STM32_CONSOLE_BITS CONFIG_UART8_BITS
+# define STM32_CONSOLE_PARITY CONFIG_UART8_PARITY
+# define STM32_CONSOLE_2STOP CONFIG_UART8_2STOP
+# define STM32_CONSOLE_TX GPIO_UART8_TX
+# define STM32_CONSOLE_RX GPIO_UART8_RX
+# ifdef CONFIG_UART8_RS485
+# define STM32_CONSOLE_RS485_DIR GPIO_UART8_RS485_DIR
+# if (CONFIG_UART8_RS485_DIR_POLARITY == 0)
+# define STM32_CONSOLE_RS485_DIR_POLARITY false
+# else
+# define STM32_CONSOLE_RS485_DIR_POLARITY true
+# endif
+# endif
#endif
/* CR1 settings */
diff --git a/nuttx/arch/arm/src/stm32/stm32_sdio.c b/nuttx/arch/arm/src/stm32/stm32_sdio.c
index a8bcae307..dad94e8a1 100644
--- a/nuttx/arch/arm/src/stm32/stm32_sdio.c
+++ b/nuttx/arch/arm/src/stm32/stm32_sdio.c
@@ -2797,13 +2797,14 @@ void sdio_mediachange(FAR struct sdio_dev_s *dev, bool cardinslot)
}
fvdbg("cdstatus OLD: %02x NEW: %02x\n", cdstatus, priv->cdstatus);
+ irqrestore(flags);
+
/* Perform any requested callback if the status has changed */
if (cdstatus != priv->cdstatus)
{
stm32_callback(priv);
}
- irqrestore(flags);
}
/****************************************************************************
diff --git a/nuttx/arch/arm/src/stm32/stm32_serial.c b/nuttx/arch/arm/src/stm32/stm32_serial.c
index c91f6a6d7..3273e52da 100644
--- a/nuttx/arch/arm/src/stm32/stm32_serial.c
+++ b/nuttx/arch/arm/src/stm32/stm32_serial.c
@@ -90,9 +90,10 @@
# endif
# if defined(CONFIG_USART2_RXDMA) || defined(CONFIG_USART3_RXDMA) || \
- defined(CONFIG_UART4_RXDMA) || defined(CONFIG_UART5_RXDMA)
+ defined(CONFIG_UART4_RXDMA) || defined(CONFIG_UART5_RXDMA) || \
+ defined(CONFIG_UART7_RXDMA) || defined(CONFIG_UART8_RXDMA)
# ifndef CONFIG_STM32_DMA1
-# error STM32 USART2/3/4/5 receive DMA requires CONFIG_STM32_DMA1
+# error STM32 USART2/3/4/5/7/8 receive DMA requires CONFIG_STM32_DMA1
# endif
# endif
@@ -105,7 +106,9 @@
(defined(CONFIG_USART3_RXDMA) && defined(CONFIG_USART3_RS485)) || \
(defined(CONFIG_UART4_RXDMA) && defined(CONFIG_UART4_RS485)) || \
(defined(CONFIG_UART5_RXDMA) && defined(CONFIG_UART5_RS485)) || \
- (defined(CONFIG_USART6_RXDMA) && defined(CONFIG_USART6_RS485))
+ (defined(CONFIG_USART6_RXDMA) && defined(CONFIG_USART6_RS485)) || \
+ (defined(CONFIG_UART7_RXDMA) && defined(CONFIG_UART7_RS485)) || \
+ (defined(CONFIG_UART8_RXDMA) && defined(CONFIG_UART8_RS485))
# error "RXDMA and RS-485 cannot be enabled at the same time for the same U[S]ART"
# endif
@@ -138,6 +141,14 @@
# error "USART6 DMA channel not defined (DMAMAP_USART6_RX)"
# endif
+# if defined(CONFIG_UART7_RXDMA) && !defined(DMAMAP_UART7_RX)
+# error "UART7 DMA channel not defined (DMAMAP_UART7_RX)"
+# endif
+
+# if defined(CONFIG_UART8_RXDMA) && !defined(DMAMAP_UART8_RX)
+# error "UART8 DMA channel not defined (DMAMAP_UART8_RX)"
+# endif
+
# elif defined(CONFIG_STM32_STM32F10XX)
# if defined(CONFIG_USART1_RXDMA) || defined(CONFIG_USART2_RXDMA) || \
@@ -337,6 +348,12 @@ static int up_interrupt_uart5(int irq, void *context);
#ifdef CONFIG_STM32_USART6
static int up_interrupt_usart6(int irq, void *context);
#endif
+#ifdef CONFIG_STM32_UART7
+static int up_interrupt_uart7(int irq, void *context);
+#endif
+#ifdef CONFIG_STM32_UART8
+static int up_interrupt_uart8(int irq, void *context);
+#endif
/****************************************************************************
* Private Variables
@@ -428,6 +445,22 @@ static char g_usart6rxfifo[RXDMA_BUFFER_SIZE];
# endif
#endif
+#ifdef CONFIG_STM32_UART7
+static char g_uart7rxbuffer[CONFIG_UART7_RXBUFSIZE];
+static char g_uart7txbuffer[CONFIG_UART7_TXBUFSIZE];
+# ifdef CONFIG_UART7_RXDMA
+static char g_uart7rxfifo[RXDMA_BUFFER_SIZE];
+# endif
+#endif
+
+#ifdef CONFIG_STM32_UART8
+static char g_uart8rxbuffer[CONFIG_UART8_RXBUFSIZE];
+static char g_uart8txbuffer[CONFIG_UART8_TXBUFSIZE];
+# ifdef CONFIG_UART8_RXDMA
+static char g_uart8rxfifo[RXDMA_BUFFER_SIZE];
+# endif
+#endif
+
/* This describes the state of the STM32 USART1 ports. */
#ifdef CONFIG_STM32_USART1
@@ -792,6 +825,126 @@ static struct up_dev_s g_usart6priv =
};
#endif
+/* This describes the state of the STM32 UART7 port. */
+
+#ifdef CONFIG_STM32_UART7
+static struct up_dev_s g_uart7priv =
+{
+ .dev =
+ {
+#if CONSOLE_UART == 7
+ .isconsole = true,
+#endif
+ .recv =
+ {
+ .size = CONFIG_UART7_RXBUFSIZE,
+ .buffer = g_uart7rxbuffer,
+ },
+ .xmit =
+ {
+ .size = CONFIG_UART7_TXBUFSIZE,
+ .buffer = g_uart7txbuffer,
+ },
+#ifdef CONFIG_UART7_RXDMA
+ .ops = &g_uart_dma_ops,
+#else
+ .ops = &g_uart_ops,
+#endif
+ .priv = &g_uart7priv,
+ },
+
+ .irq = STM32_IRQ_UART7,
+ .parity = CONFIG_UART7_PARITY,
+ .bits = CONFIG_UART7_BITS,
+ .stopbits2 = CONFIG_UART7_2STOP,
+ .baud = CONFIG_UART7_BAUD,
+ .apbclock = STM32_PCLK1_FREQUENCY,
+ .usartbase = STM32_UART7_BASE,
+ .tx_gpio = GPIO_UART7_TX,
+ .rx_gpio = GPIO_UART7_RX,
+#ifdef GPIO_UART7_CTS
+ .cts_gpio = GPIO_UART7_CTS,
+#endif
+#ifdef GPIO_UART7_RTS
+ .rts_gpio = GPIO_UART7_RTS,
+#endif
+#ifdef CONFIG_UART7_RXDMA
+ .rxdma_channel = DMAMAP_UART7_RX,
+ .rxfifo = g_uart7rxfifo,
+#endif
+ .vector = up_interrupt_uart7,
+
+#ifdef CONFIG_UART7_RS485
+ .rs485_dir_gpio = GPIO_UART7_RS485_DIR,
+# if (CONFIG_UART7_RS485_DIR_POLARITY == 0)
+ .rs485_dir_polarity = false,
+# else
+ .rs485_dir_polarity = true,
+# endif
+#endif
+};
+#endif
+
+/* This describes the state of the STM32 UART8 port. */
+
+#ifdef CONFIG_STM32_UART8
+static struct up_dev_s g_uart8priv =
+{
+ .dev =
+ {
+#if CONSOLE_UART == 8
+ .isconsole = true,
+#endif
+ .recv =
+ {
+ .size = CONFIG_UART8_RXBUFSIZE,
+ .buffer = g_uart8rxbuffer,
+ },
+ .xmit =
+ {
+ .size = CONFIG_UART8_TXBUFSIZE,
+ .buffer = g_uart8txbuffer,
+ },
+#ifdef CONFIG_UART8_RXDMA
+ .ops = &g_uart_dma_ops,
+#else
+ .ops = &g_uart_ops,
+#endif
+ .priv = &g_uart8priv,
+ },
+
+ .irq = STM32_IRQ_UART8,
+ .parity = CONFIG_UART8_PARITY,
+ .bits = CONFIG_UART8_BITS,
+ .stopbits2 = CONFIG_UART8_2STOP,
+ .baud = CONFIG_UART8_BAUD,
+ .apbclock = STM32_PCLK1_FREQUENCY,
+ .usartbase = STM32_UART8_BASE,
+ .tx_gpio = GPIO_UART8_TX,
+ .rx_gpio = GPIO_UART8_RX,
+#ifdef GPIO_UART8_CTS
+ .cts_gpio = GPIO_UART8_CTS,
+#endif
+#ifdef GPIO_UART8_RTS
+ .rts_gpio = GPIO_UART8_RTS,
+#endif
+#ifdef CONFIG_UART8_RXDMA
+ .rxdma_channel = DMAMAP_UART8_RX,
+ .rxfifo = g_uart8rxfifo,
+#endif
+ .vector = up_interrupt_uart8,
+
+#ifdef CONFIG_UART8_RS485
+ .rs485_dir_gpio = GPIO_UART8_RS485_DIR,
+# if (CONFIG_UART8_RS485_DIR_POLARITY == 0)
+ .rs485_dir_polarity = false,
+# else
+ .rs485_dir_polarity = true,
+# endif
+#endif
+};
+#endif
+
/* This table lets us iterate over the configured USARTs */
static struct up_dev_s *uart_devs[STM32_NUSART] =
@@ -814,6 +967,12 @@ static struct up_dev_s *uart_devs[STM32_NUSART] =
#ifdef CONFIG_STM32_USART6
[5] = &g_usart6priv,
#endif
+#ifdef CONFIG_STM32_UART7
+ [6] = &g_uart7priv,
+#endif
+#ifdef CONFIG_STM32_UART8
+ [7] = &g_uart8priv,
+#endif
};
#ifdef CONFIG_PM
@@ -1899,6 +2058,20 @@ static int up_interrupt_usart6(int irq, void *context)
}
#endif
+#ifdef CONFIG_STM32_UART7
+static int up_interrupt_uart7(int irq, void *context)
+{
+ return up_interrupt_common(&g_uart7priv);
+}
+#endif
+
+#ifdef CONFIG_STM32_UART8
+static int up_interrupt_uart8(int irq, void *context)
+{
+ return up_interrupt_common(&g_uart8priv);
+}
+#endif
+
/****************************************************************************
* Name: up_dma_rxcallback
*
@@ -2187,6 +2360,20 @@ void stm32_serial_dma_poll(void)
}
#endif
+#ifdef CONFIG_UART7_RXDMA
+ if (g_uart7priv.rxdma != NULL)
+ {
+ up_dma_rxcallback(g_uart7priv.rxdma, 0, &g_uart7priv);
+ }
+#endif
+
+#ifdef CONFIG_UART8_RXDMA
+ if (g_uart8priv.rxdma != NULL)
+ {
+ up_dma_rxcallback(g_uart8priv.rxdma, 0, &g_uart8priv);
+ }
+#endif
+
irqrestore(flags);
}
#endif
diff --git a/nuttx/arch/arm/src/stm32/stm32_spi.c b/nuttx/arch/arm/src/stm32/stm32_spi.c
index b4a4f36ab..c8a8faa66 100644
--- a/nuttx/arch/arm/src/stm32/stm32_spi.c
+++ b/nuttx/arch/arm/src/stm32/stm32_spi.c
@@ -82,7 +82,8 @@
#include "stm32_dma.h"
#include "stm32_spi.h"
-#if defined(CONFIG_STM32_SPI1) || defined(CONFIG_STM32_SPI2) || defined(CONFIG_STM32_SPI3)
+#if defined(CONFIG_STM32_SPI1) || defined(CONFIG_STM32_SPI2) || defined(CONFIG_STM32_SPI3) || \
+ defined(CONFIG_STM32_SPI4) || defined(CONFIG_STM32_SPI5) || defined(CONFIG_STM32_SPI6)
/************************************************************************************
* Definitions
@@ -377,6 +378,123 @@ static struct stm32_spidev_s g_spi3dev =
};
#endif
+#ifdef CONFIG_STM32_SPI4
+static const struct spi_ops_s g_sp4iops =
+{
+#ifndef CONFIG_SPI_OWNBUS
+ .lock = spi_lock,
+#endif
+ .select = stm32_spi4select,
+ .setfrequency = spi_setfrequency,
+ .setmode = spi_setmode,
+ .setbits = spi_setbits,
+ .status = stm32_spi4status,
+#ifdef CONFIG_SPI_CMDDATA
+ .cmddata = stm32_spi4cmddata,
+#endif
+ .send = spi_send,
+#ifdef CONFIG_SPI_EXCHANGE
+ .exchange = spi_exchange,
+#else
+ .sndblock = spi_sndblock,
+ .recvblock = spi_recvblock,
+#endif
+ .registercallback = 0,
+};
+
+static struct stm32_spidev_s g_spi4dev =
+{
+ .spidev = { &g_sp4iops },
+ .spibase = STM32_SPI4_BASE,
+ .spiclock = STM32_PCLK1_FREQUENCY,
+#ifdef CONFIG_STM32_SPI_INTERRUPTS
+ .spiirq = STM32_IRQ_SPI4,
+#endif
+#ifdef CONFIG_STM32_SPI_DMA
+ .rxch = DMACHAN_SPI4_RX,
+ .txch = DMACHAN_SPI4_TX,
+#endif
+};
+#endif
+
+#ifdef CONFIG_STM32_SPI5
+static const struct spi_ops_s g_sp5iops =
+{
+#ifndef CONFIG_SPI_OWNBUS
+ .lock = spi_lock,
+#endif
+ .select = stm32_spi5select,
+ .setfrequency = spi_setfrequency,
+ .setmode = spi_setmode,
+ .setbits = spi_setbits,
+ .status = stm32_spi5status,
+#ifdef CONFIG_SPI_CMDDATA
+ .cmddata = stm32_spi5cmddata,
+#endif
+ .send = spi_send,
+#ifdef CONFIG_SPI_EXCHANGE
+ .exchange = spi_exchange,
+#else
+ .sndblock = spi_sndblock,
+ .recvblock = spi_recvblock,
+#endif
+ .registercallback = 0,
+};
+
+static struct stm32_spidev_s g_spi5dev =
+{
+ .spidev = { &g_sp5iops },
+ .spibase = STM32_SPI5_BASE,
+ .spiclock = STM32_PCLK1_FREQUENCY,
+#ifdef CONFIG_STM32_SPI_INTERRUPTS
+ .spiirq = STM32_IRQ_SPI5,
+#endif
+#ifdef CONFIG_STM32_SPI_DMA
+ .rxch = DMACHAN_SPI5_RX,
+ .txch = DMACHAN_SPI5_TX,
+#endif
+};
+#endif
+
+#ifdef CONFIG_STM32_SPI6
+static const struct spi_ops_s g_sp6iops =
+{
+#ifndef CONFIG_SPI_OWNBUS
+ .lock = spi_lock,
+#endif
+ .select = stm32_spi6select,
+ .setfrequency = spi_setfrequency,
+ .setmode = spi_setmode,
+ .setbits = spi_setbits,
+ .status = stm32_spi6status,
+#ifdef CONFIG_SPI_CMDDATA
+ .cmddata = stm32_spi3cmddata,
+#endif
+ .send = spi_send,
+#ifdef CONFIG_SPI_EXCHANGE
+ .exchange = spi_exchange,
+#else
+ .sndblock = spi_sndblock,
+ .recvblock = spi_recvblock,
+#endif
+ .registercallback = 0,
+};
+
+static struct stm32_spidev_s g_spi6dev =
+{
+ .spidev = { &g_sp6iops },
+ .spibase = STM32_SPI6_BASE,
+ .spiclock = STM32_PCLK1_FREQUENCY,
+#ifdef CONFIG_STM32_SPI_INTERRUPTS
+ .spiirq = STM32_IRQ_SPI6,
+#endif
+#ifdef CONFIG_STM32_SPI_DMA
+ .rxch = DMACHAN_SPI6_RX,
+ .txch = DMACHAN_SPI6_TX,
+#endif
+};
+#endif
+
/************************************************************************************
* Public Data
************************************************************************************/
@@ -1464,6 +1582,78 @@ FAR struct spi_dev_s *up_spiinitialize(int port)
}
else
#endif
+#ifdef CONFIG_STM32_SPI4
+ if (port == 4)
+ {
+ /* Select SPI4 */
+
+ priv = &g_spi4dev;
+
+ /* Only configure if the port is not already configured */
+
+ if ((spi_getreg(priv, STM32_SPI_CR1_OFFSET) & SPI_CR1_SPE) == 0)
+ {
+ /* Configure SPI4 pins: SCK, MISO, and MOSI */
+
+ stm32_configgpio(GPIO_SPI4_SCK);
+ stm32_configgpio(GPIO_SPI4_MISO);
+ stm32_configgpio(GPIO_SPI4_MOSI);
+
+ /* Set up default configuration: Master, 8-bit, etc. */
+
+ spi_portinitialize(priv);
+ }
+ }
+ else
+#endif
+#ifdef CONFIG_STM32_SPI5
+ if (port == 5)
+ {
+ /* Select SPI5 */
+
+ priv = &g_spi5dev;
+
+ /* Only configure if the port is not already configured */
+
+ if ((spi_getreg(priv, STM32_SPI_CR1_OFFSET) & SPI_CR1_SPE) == 0)
+ {
+ /* Configure SPI5 pins: SCK, MISO, and MOSI */
+
+ stm32_configgpio(GPIO_SPI5_SCK);
+ stm32_configgpio(GPIO_SPI5_MISO);
+ stm32_configgpio(GPIO_SPI5_MOSI);
+
+ /* Set up default configuration: Master, 8-bit, etc. */
+
+ spi_portinitialize(priv);
+ }
+ }
+ else
+#endif
+#ifdef CONFIG_STM32_SPI6
+ if (port == 6)
+ {
+ /* Select SPI6 */
+
+ priv = &g_spi6dev;
+
+ /* Only configure if the port is not already configured */
+
+ if ((spi_getreg(priv, STM32_SPI_CR1_OFFSET) & SPI_CR1_SPE) == 0)
+ {
+ /* Configure SPI6 pins: SCK, MISO, and MOSI */
+
+ stm32_configgpio(GPIO_SPI6_SCK);
+ stm32_configgpio(GPIO_SPI6_MISO);
+ stm32_configgpio(GPIO_SPI6_MOSI);
+
+ /* Set up default configuration: Master, 8-bit, etc. */
+
+ spi_portinitialize(priv);
+ }
+ }
+ else
+#endif
{
spidbg("ERROR: Unsupported SPI port: %d\n", port);
return NULL;
@@ -1473,4 +1663,4 @@ FAR struct spi_dev_s *up_spiinitialize(int port)
return (FAR struct spi_dev_s *)priv;
}
-#endif /* CONFIG_STM32_SPI1 || CONFIG_STM32_SPI2 || CONFIG_STM32_SPI3 */
+#endif /* CONFIG_STM32_SPI1 || CONFIG_STM32_SPI2 || CONFIG_STM32_SPI3 || CONFIG_STM32_SPI4 || CONFIG_STM32_SPI5 || CONFIG_STM32_SPI6 */
diff --git a/nuttx/arch/arm/src/stm32/stm32_spi.h b/nuttx/arch/arm/src/stm32/stm32_spi.h
index 6030ddfdd..58c3f0da7 100644
--- a/nuttx/arch/arm/src/stm32/stm32_spi.h
+++ b/nuttx/arch/arm/src/stm32/stm32_spi.h
@@ -71,11 +71,11 @@ enum spi_dev_e;
************************************************************************************/
/************************************************************************************
- * Name: stm32_spi1/2/3select and stm32_spi1/2/3status
+ * Name: stm32_spi1/2/...select and stm32_spi1/2/...status
*
* Description:
- * The external functions, stm32_spi1/2/3select, stm32_spi1/2/3status, and
- * stm32_spi1/2/3cmddata must be provided by board-specific logic. These are
+ * The external functions, stm32_spi1/2/...select, stm32_spi1/2/...status, and
+ * stm32_spi1/2/...cmddata must be provided by board-specific logic. These are
* implementations of the select, status, and cmddata methods of the SPI interface
* defined by struct spi_ops_s (see include/nuttx/spi.h). All other methods
* (including up_spiinitialize()) are provided by common STM32 logic. To use this
@@ -83,11 +83,11 @@ enum spi_dev_e;
*
* 1. Provide logic in stm32_boardinitialize() to configure SPI chip select
* pins.
- * 2. Provide stm32_spi1/2/3select() and stm32_spi1/2/3status() functions in your
+ * 2. Provide stm32_spi1/2/...select() and stm32_spi1/2/...status() functions in your
* board-specific logic. These functions will perform chip selection and
* status operations using GPIOs in the way your board is configured.
* 3. If CONFIG_SPI_CMDDATA is defined in your NuttX configuration file, then
- * provide stm32_spi1/2/3cmddata() functions in your board-specific logic.
+ * provide stm32_spi1/2/...cmddata() functions in your board-specific logic.
* These functions will perform cmd/data selection operations using GPIOs in the
* way your board is configured.
* 4. Add a calls to up_spiinitialize() in your low level application
@@ -111,6 +111,18 @@ EXTERN void stm32_spi3select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, b
EXTERN uint8_t stm32_spi3status(FAR struct spi_dev_s *dev, enum spi_dev_e devid);
EXTERN int stm32_spi3cmddata(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool cmd);
+EXTERN void stm32_spi4select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected);
+EXTERN uint8_t stm32_spi4status(FAR struct spi_dev_s *dev, enum spi_dev_e devid);
+EXTERN int stm32_spi4cmddata(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool cmd);
+
+EXTERN void stm32_spi5select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected);
+EXTERN uint8_t stm32_spi5status(FAR struct spi_dev_s *dev, enum spi_dev_e devid);
+EXTERN int stm32_spi5cmddata(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool cmd);
+
+EXTERN void stm32_spi6select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected);
+EXTERN uint8_t stm32_spi6status(FAR struct spi_dev_s *dev, enum spi_dev_e devid);
+EXTERN int stm32_spi6cmddata(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool cmd);
+
#undef EXTERN
#if defined(__cplusplus)
}
diff --git a/nuttx/arch/arm/src/stm32/stm32_uart.h b/nuttx/arch/arm/src/stm32/stm32_uart.h
index a26ea2009..e77122d82 100644
--- a/nuttx/arch/arm/src/stm32/stm32_uart.h
+++ b/nuttx/arch/arm/src/stm32/stm32_uart.h
@@ -52,6 +52,12 @@
* the device.
*/
+#if STM32_NUSART < 8
+# undef CONFIG_STM32_UART8
+#endif
+#if STM32_NUSART < 7
+# undef CONFIG_STM32_UART7
+#endif
#if STM32_NUSART < 6
# undef CONFIG_STM32_USART6
#endif
@@ -75,7 +81,8 @@
#if defined(CONFIG_STM32_USART1) || defined(CONFIG_STM32_USART2) || \
defined(CONFIG_STM32_USART3) || defined(CONFIG_STM32_UART4) || \
- defined(CONFIG_STM32_UART5) || defined(CONFIG_STM32_USART6)
+ defined(CONFIG_STM32_UART5) || defined(CONFIG_STM32_USART6) || \
+ defined(CONFIG_STM32_UART7) || defined(CONFIG_STM32_UART8)
# define HAVE_UART 1
#endif
@@ -87,6 +94,8 @@
# undef CONFIG_UART4_SERIAL_CONSOLE
# undef CONFIG_UART5_SERIAL_CONSOLE
# undef CONFIG_USART6_SERIAL_CONSOLE
+# undef CONFIG_UART7_SERIAL_CONSOLE
+# undef CONFIG_UART8_SERIAL_CONSOLE
# define CONSOLE_UART 1
# define HAVE_CONSOLE 1
#elif defined(CONFIG_USART2_SERIAL_CONSOLE) && defined(CONFIG_STM32_USART2)
@@ -95,6 +104,8 @@
# undef CONFIG_UART4_SERIAL_CONSOLE
# undef CONFIG_UART5_SERIAL_CONSOLE
# undef CONFIG_USART6_SERIAL_CONSOLE
+# undef CONFIG_UART7_SERIAL_CONSOLE
+# undef CONFIG_UART8_SERIAL_CONSOLE
# define CONSOLE_UART 2
# define HAVE_CONSOLE 1
#elif defined(CONFIG_USART3_SERIAL_CONSOLE) && defined(CONFIG_STM32_USART3)
@@ -103,6 +114,8 @@
# undef CONFIG_UART4_SERIAL_CONSOLE
# undef CONFIG_UART5_SERIAL_CONSOLE
# undef CONFIG_USART6_SERIAL_CONSOLE
+# undef CONFIG_UART7_SERIAL_CONSOLE
+# undef CONFIG_UART8_SERIAL_CONSOLE
# define CONSOLE_UART 3
# define HAVE_CONSOLE 1
#elif defined(CONFIG_UART4_SERIAL_CONSOLE) && defined(CONFIG_STM32_UART4)
@@ -111,6 +124,8 @@
# undef CONFIG_USART3_SERIAL_CONSOLE
# undef CONFIG_UART5_SERIAL_CONSOLE
# undef CONFIG_USART6_SERIAL_CONSOLE
+# undef CONFIG_UART7_SERIAL_CONSOLE
+# undef CONFIG_UART8_SERIAL_CONSOLE
# define CONSOLE_UART 4
# define HAVE_CONSOLE 1
#elif defined(CONFIG_UART5_SERIAL_CONSOLE) && defined(CONFIG_STM32_UART5)
@@ -119,6 +134,8 @@
# undef CONFIG_USART3_SERIAL_CONSOLE
# undef CONFIG_UART4_SERIAL_CONSOLE
# undef CONFIG_USART6_SERIAL_CONSOLE
+# undef CONFIG_UART7_SERIAL_CONSOLE
+# undef CONFIG_UART8_SERIAL_CONSOLE
# define CONSOLE_UART 5
# define HAVE_CONSOLE 1
#elif defined(CONFIG_USART6_SERIAL_CONSOLE) && defined(CONFIG_STM32_USART6)
@@ -127,8 +144,31 @@
# undef CONFIG_USART3_SERIAL_CONSOLE
# undef CONFIG_UART4_SERIAL_CONSOLE
# undef CONFIG_UART5_SERIAL_CONSOLE
+# undef CONFIG_UART7_SERIAL_CONSOLE
+# undef CONFIG_UART8_SERIAL_CONSOLE
# define CONSOLE_UART 6
# define HAVE_CONSOLE 1
+#elif defined(CONFIG_UART7_SERIAL_CONSOLE) && defined(CONFIG_STM32_UART7)
+# undef CONFIG_USART1_SERIAL_CONSOLE
+# undef CONFIG_USART2_SERIAL_CONSOLE
+# undef CONFIG_USART3_SERIAL_CONSOLE
+# undef CONFIG_UART4_SERIAL_CONSOLE
+# undef CONFIG_UART5_SERIAL_CONSOLE
+# undef CONFIG_USART6_SERIAL_CONSOLE
+# undef CONFIG_UART5_SERIAL_CONSOLE
+# undef CONFIG_UART8_SERIAL_CONSOLE
+# define CONSOLE_UART 7
+# define HAVE_CONSOLE 1
+#elif defined(CONFIG_UART8_SERIAL_CONSOLE) && defined(CONFIG_STM32_UART8)
+# undef CONFIG_USART1_SERIAL_CONSOLE
+# undef CONFIG_USART2_SERIAL_CONSOLE
+# undef CONFIG_USART3_SERIAL_CONSOLE
+# undef CONFIG_UART4_SERIAL_CONSOLE
+# undef CONFIG_UART6_SERIAL_CONSOLE
+# undef CONFIG_USART6_SERIAL_CONSOLE
+# undef CONFIG_UART7_SERIAL_CONSOLE
+# define CONSOLE_UART 8
+# define HAVE_CONSOLE 1
#else
# undef CONFIG_USART1_SERIAL_CONSOLE
# undef CONFIG_USART2_SERIAL_CONSOLE
@@ -136,6 +176,8 @@
# undef CONFIG_UART4_SERIAL_CONSOLE
# undef CONFIG_UART5_SERIAL_CONSOLE
# undef CONFIG_USART6_SERIAL_CONSOLE
+# undef CONFIG_UART7_SERIAL_CONSOLE
+# undef CONFIG_UART8_SERIAL_CONSOLE
# define CONSOLE_UART 0
# undef HAVE_CONSOLE
#endif
@@ -149,6 +191,8 @@
# undef CONFIG_UART4_RXDMA
# undef CONFIG_UART5_RXDMA
# undef CONFIG_USART6_RXDMA
+# undef CONFIG_UART7_RXDMA
+# undef CONFIG_UART8_RXDMA
#endif
/* Disable the DMA configuration on all unused USARTs */
@@ -177,12 +221,21 @@
# undef CONFIG_USART6_RXDMA
#endif
+#ifndef CONFIG_STM32_UART7
+# undef CONFIG_UART7_RXDMA
+#endif
+
+#ifndef CONFIG_STM32_UART8
+# undef CONFIG_UART8_RXDMA
+#endif
+
/* Is DMA available on any (enabled) USART? */
#undef SERIAL_HAVE_DMA
#if defined(CONFIG_USART1_RXDMA) || defined(CONFIG_USART2_RXDMA) || \
- defined(CONFIG_USART3_RXDMA) || defined(CONFIG_UART4_RXDMA) || \
- defined(CONFIG_UART5_RXDMA) || defined(CONFIG_USART6_RXDMA)
+ defined(CONFIG_USART3_RXDMA) || defined(CONFIG_UART4_RXDMA) || \
+ defined(CONFIG_UART5_RXDMA) || defined(CONFIG_USART6_RXDMA) || \
+ defined(CONFIG_UART7_RXDMA) || defined(CONFIG_UART8_RXDMA)
# define SERIAL_HAVE_DMA 1
#endif
@@ -201,6 +254,10 @@
# define SERIAL_HAVE_CONSOLE_DMA 1
#elif defined(CONFIG_USART6_SERIAL_CONSOLE) && defined(CONFIG_USART6_RXDMA)
# define SERIAL_HAVE_CONSOLE_DMA 1
+#elif defined(CONFIG_UART7_SERIAL_CONSOLE) && defined(CONFIG_UART7_RXDMA)
+# define SERIAL_HAVE_CONSOLE_DMA 1
+#elif defined(CONFIG_UART8_SERIAL_CONSOLE) && defined(CONFIG_UART8_RXDMA)
+# define SERIAL_HAVE_CONSOLE_DMA 1
#endif
/* Is DMA used on all (enabled) USARTs */
@@ -218,13 +275,18 @@
# undef SERIAL_HAVE_ONLY_DMA
#elif defined(CONFIG_STM32_USART6) && !defined(CONFIG_USART6_RXDMA)
# undef SERIAL_HAVE_ONLY_DMA
+#elif defined(CONFIG_STM32_UART7) && !defined(CONFIG_UART7_RXDMA)
+# undef SERIAL_HAVE_ONLY_DMA
+#elif defined(CONFIG_STM32_UART8) && !defined(CONFIG_UART8_RXDMA)
+# undef SERIAL_HAVE_ONLY_DMA
#endif
/* Is RS-485 used? */
#if defined(CONFIG_USART1_RS485) || defined(CONFIG_USART2_RS485) || \
defined(CONFIG_USART3_RS485) || defined(CONFIG_UART4_RS485) || \
- defined(CONFIG_UART5_RS485) || defined(CONFIG_USART6_RS485)
+ defined(CONFIG_UART5_RS485) || defined(CONFIG_USART6_RS485) || \
+ defined(CONFIG_UART7_RS485) || defined(CONFIG_UART8_RS485)
# define HAVE_RS485 1
#endif
diff --git a/nuttx/arch/arm/src/stm32/stm32f40xxx_dma.c b/nuttx/arch/arm/src/stm32/stm32f40xxx_dma.c
index 40fce8cb5..5140ee4f9 100644
--- a/nuttx/arch/arm/src/stm32/stm32f40xxx_dma.c
+++ b/nuttx/arch/arm/src/stm32/stm32f40xxx_dma.c
@@ -130,7 +130,7 @@ static struct stm32_dma_s g_dma[DMA_NSTREAMS] =
.stream = 3,
.irq = STM32_IRQ_DMA1S3,
.shift = DMA_INT_STREAM3_SHIFT,
- .base = STM32_DMA1_BASE + STM32_DMA_OFFSET(4),
+ .base = STM32_DMA1_BASE + STM32_DMA_OFFSET(3),
},
{
.stream = 4,
diff --git a/nuttx/arch/arm/src/stm32/stm32f40xxx_rcc.c b/nuttx/arch/arm/src/stm32/stm32f40xxx_rcc.c
index c6c0b2382..fc7fe1697 100644
--- a/nuttx/arch/arm/src/stm32/stm32f40xxx_rcc.c
+++ b/nuttx/arch/arm/src/stm32/stm32f40xxx_rcc.c
@@ -437,6 +437,19 @@ static inline void rcc_enableapb1(void)
regval |= RCC_APB1ENR_DACEN;
#endif
+#ifdef CONFIG_STM32_UART7
+ /* UART7 clock enable */
+
+ regval |= RCC_APB1ENR_UART7EN;
+#endif
+
+#ifdef CONFIG_STM32_UART8
+ /* UART8 clock enable */
+
+ regval |= RCC_APB1ENR_UART8EN;
+#endif
+
+
putreg32(regval, STM32_RCC_APB1ENR); /* Enable peripherals */
}
@@ -512,6 +525,12 @@ static inline void rcc_enableapb2(void)
regval |= RCC_APB2ENR_SPI1EN;
#endif
+#ifdef CONFIG_STM32_SPI4
+ /* SPI4 clock enable */
+
+ regval |= RCC_APB2ENR_SPI4EN;
+#endif
+
#ifdef CONFIG_STM32_SYSCFG
/* System configuration controller clock enable */
@@ -536,6 +555,18 @@ static inline void rcc_enableapb2(void)
regval |= RCC_APB2ENR_TIM11EN;
#endif
+#ifdef CONFIG_STM32_SPI5
+ /* SPI5 clock enable */
+
+ regval |= RCC_APB2ENR_SPI5EN;
+#endif
+
+#ifdef CONFIG_STM32_SPI6
+ /* SPI6 clock enable */
+
+ regval |= RCC_APB2ENR_SPI6EN;
+#endif
+
putreg32(regval, STM32_RCC_APB2ENR); /* Enable peripherals */
}
@@ -591,7 +622,12 @@ static void stm32_stdclockconfig(void)
putreg32(regval, STM32_RCC_APB1ENR);
regval = getreg32(STM32_PWR_CR);
+#if defined(CONFIG_STM32_STM32F427)
+ regval &= ~PWR_CR_VOS_MASK;
+ regval |= PWR_CR_VOS_SCALE_1;
+#else
regval |= PWR_CR_VOS;
+#endif
putreg32(regval, STM32_PWR_CR);
/* Set the HCLK source/divider */
diff --git a/nuttx/configs/px4fmuv2/Kconfig b/nuttx/configs/px4fmuv2/Kconfig
new file mode 100644
index 000000000..a10a02ba4
--- /dev/null
+++ b/nuttx/configs/px4fmuv2/Kconfig
@@ -0,0 +1,7 @@
+#
+# For a description of the syntax of this configuration file,
+# see misc/tools/kconfig-language.txt.
+#
+
+if ARCH_BOARD_PX4FMUV2
+endif
diff --git a/nuttx/configs/px4fmuv2/common/Make.defs b/nuttx/configs/px4fmuv2/common/Make.defs
new file mode 100644
index 000000000..be387dce1
--- /dev/null
+++ b/nuttx/configs/px4fmuv2/common/Make.defs
@@ -0,0 +1,184 @@
+############################################################################
+# configs/px4fmu/common/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.
+#
+############################################################################
+
+#
+# Generic Make.defs for the PX4FMUV2
+# Do not specify/use this file directly - it is included by config-specific
+# Make.defs in the per-config directories.
+#
+
+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 = -O3
+ARCHCPUFLAGS = -mcpu=cortex-m4 \
+ -mthumb \
+ -march=armv7e-m \
+ -mfpu=fpv4-sp-d16 \
+ -mfloat-abi=hard
+
+
+# enable precise stack overflow tracking
+INSTRUMENTATIONDEFINES = -finstrument-functions \
+ -ffixed-r10
+
+# 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)/common/$(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)/common/$(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)/common/$(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 \
+ -Wextra \
+ -Wdouble-promotion \
+ -Wshadow \
+ -Wfloat-equal \
+ -Wframe-larger-than=1024 \
+ -Wpointer-arith \
+ -Wlogical-op \
+ -Wmissing-declarations \
+ -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 \
+ -Wmissing-prototypes \
+ -Wnested-externs \
+ -Wunsuffixed-float-constants
+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/px4fmuv2/common/ld.script b/nuttx/configs/px4fmuv2/common/ld.script
new file mode 100644
index 000000000..1be39fb87
--- /dev/null
+++ b/nuttx/configs/px4fmuv2/common/ld.script
@@ -0,0 +1,150 @@
+/****************************************************************************
+ * configs/px4fmu/common/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 STM32F427 has 2048Kb of FLASH beginning at address 0x0800:0000 and
+ * 256Kb 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 SRAM beginning at address 0x2002:0000
+ * 4) 64Kb of TCM 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 = 2032K
+ sram (rwx) : ORIGIN = 0x20000000, LENGTH = 192K
+ 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/px4fmuv2/include/board.h b/nuttx/configs/px4fmuv2/include/board.h
new file mode 100755
index 000000000..0055d65e1
--- /dev/null
+++ b/nuttx/configs/px4fmuv2/include/board.h
@@ -0,0 +1,364 @@
+/************************************************************************************
+ * configs/px4fmu/include/board.h
+ * include/arch/board/board.h
+ *
+ * Copyright (C) 2009 Gregory Nutt. All rights reserved.
+ * Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ************************************************************************************/
+
+#ifndef __ARCH_BOARD_BOARD_H
+#define __ARCH_BOARD_BOARD_H
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <nuttx/config.h>
+#ifndef __ASSEMBLY__
+# include <stdint.h>
+#endif
+
+/************************************************************************************
+ * Definitions
+ ************************************************************************************/
+
+/* Clocking *************************************************************************/
+/* The PX4FMUV2 uses a 24MHz crystal connected to the HSE.
+ *
+ * This is the "standard" configuration as set up by arch/arm/src/stm32f40xx_rcc.c:
+ * 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_PPQ)
+ * 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 24000000ul
+
+#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
+ * = (25,000,000 / 25) * 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(24)
+#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_PCLK1_FREQUENCY)
+#define STM32_APB2_TIM10_CLKIN (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB2_TIM11_CLKIN (2*STM32_PCLK1_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)
+
+/* SDIO dividers. Note that slower clocking is required when DMA is disabled
+ * in order to avoid RX overrun/TX underrun errors due to delayed responses
+ * to service FIFOs in interrupt driven mode. These values have not been
+ * tuned!!!
+ *
+ * HCLK=72MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(178+2)=400 KHz
+ */
+
+#define SDIO_INIT_CLKDIV (178 << SDIO_CLKCR_CLKDIV_SHIFT)
+
+/* DMA ON: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(2+2)=18 MHz
+ * DMA OFF: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(3+2)=14.4 MHz
+ */
+
+#ifdef CONFIG_SDIO_DMA
+# define SDIO_MMCXFR_CLKDIV (2 << SDIO_CLKCR_CLKDIV_SHIFT)
+#else
+# define SDIO_MMCXFR_CLKDIV (3 << SDIO_CLKCR_CLKDIV_SHIFT)
+#endif
+
+/* DMA ON: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(1+2)=24 MHz
+ * DMA OFF: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(3+2)=14.4 MHz
+ */
+
+#ifdef CONFIG_SDIO_DMA
+# define SDIO_SDXFR_CLKDIV (1 << SDIO_CLKCR_CLKDIV_SHIFT)
+#else
+# define SDIO_SDXFR_CLKDIV (3 << SDIO_CLKCR_CLKDIV_SHIFT)
+#endif
+
+/* DMA Channl/Stream Selections *****************************************************/
+/* Stream selections are arbitrary for now but might become important in the future
+ * is we set aside more DMA channels/streams.
+ *
+ * SDIO DMA
+ *   DMAMAP_SDIO_1 = Channel 4, Stream 3 <- may later be used by SPI DMA
+ *   DMAMAP_SDIO_2 = Channel 4, Stream 6
+ */
+
+#define DMAMAP_SDIO DMAMAP_SDIO_1
+
+/* High-resolution timer
+ */
+#ifdef CONFIG_HRT_TIMER
+# define HRT_TIMER 8 /* use timer8 for the HRT */
+# define HRT_TIMER_CHANNEL 1 /* use capture/compare channel */
+#endif
+
+/* Alternate function pin selections ************************************************/
+
+/*
+ * UARTs.
+ */
+#define GPIO_USART1_RX GPIO_USART1_RX_1 /* console in from IO */
+#define GPIO_USART1_TX 0 /* USART1 is RX-only */
+
+#define GPIO_USART2_RX GPIO_USART2_RX_2
+#define GPIO_USART2_TX GPIO_USART2_TX_2
+#define GPIO_USART2_RTS GPIO_USART2_RTS_2
+#define GPIO_USART2_CTS GPIO_USART2_CTS_2
+
+#define GPIO_USART3_RX GPIO_USART3_RX_3
+#define GPIO_USART3_TX GPIO_USART3_TX_3
+#define GPIO_USART2_RTS GPIO_USART2_RTS_2
+#define GPIO_USART2_CTS GPIO_USART2_CTS_2
+
+#define GPIO_UART4_RX GPIO_UART4_RX_1
+#define GPIO_UART4_TX GPIO_UART4_TX_1
+
+#define GPIO_USART6_RX GPIO_USART6_RX_1
+#define GPIO_USART6_TX GPIO_USART6_TX_1
+
+#define GPIO_UART7_RX GPIO_UART7_RX_1
+#define GPIO_UART7_TX GPIO_UART7_TX_1
+
+/* UART8 has no alternate pin config */
+
+/* UART RX DMA configurations */
+#define DMAMAP_USART1_RX DMAMAP_USART1_RX_2
+#define DMAMAP_USART6_RX DMAMAP_USART6_RX_2
+
+/*
+ * PWM
+ *
+ * Six PWM outputs are configured.
+ *
+ * Pins:
+ *
+ * CH1 : PE14 : TIM1_CH4
+ * CH2 : PE13 : TIM1_CH3
+ * CH3 : PE11 : TIM1_CH2
+ * CH4 : PE9 : TIM1_CH1
+ * CH5 : PD13 : TIM4_CH2
+ * CH6 : PD14 : TIM4_CH3
+ *
+ */
+#define GPIO_TIM1_CH1OUT GPIO_TIM1_CH1OUT_2
+#define GPIO_TIM1_CH2OUT GPIO_TIM1_CH2OUT_2
+#define GPIO_TIM1_CH3OUT GPIO_TIM1_CH3OUT_2
+#define GPIO_TIM1_CH4OUT GPIO_TIM1_CH4OUT_2
+#define GPIO_TIM4_CH2OUT GPIO_TIM4_CH2OUT_2
+#define GPIO_TIM4_CH3OUT GPIO_TIM4_CH3OUT_2
+
+/*
+ * CAN
+ *
+ * CAN1 is routed to the onboard transceiver.
+ * CAN2 is routed to the expansion connector.
+ */
+
+#define GPIO_CAN1_RX GPIO_CAN1_RX_3
+#define GPIO_CAN1_TX GPIO_CAN1_TX_3
+#define GPIO_CAN2_RX GPIO_CAN2_RX_1
+#define GPIO_CAN2_TX GPIO_CAN2_TX_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_2
+#define GPIO_I2C1_SDA GPIO_I2C1_SDA_2
+#define GPIO_I2C1_SCL_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN8)
+#define GPIO_I2C1_SDA_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN9)
+
+#define GPIO_I2C2_SCL GPIO_I2C2_SCL_1
+#define GPIO_I2C2_SDA GPIO_I2C2_SDA_1
+#define GPIO_I2C2_SCL_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN10)
+#define GPIO_I2C2_SDA_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN11)
+
+/*
+ * I2C busses
+ */
+#define PX4_I2C_BUS_EXPANSION 1
+#define PX4_I2C_BUS_LED 2
+
+/*
+ * Devices on the onboard bus.
+ *
+ * Note that these are unshifted addresses.
+ */
+#define PX4_I2C_OBDEV_LED 0x55
+#define PX4_I2C_OBDEV_HMC5883 0x1e
+
+/*
+ * SPI
+ *
+ * There are sensors on SPI1, and SPI2 is connected to the FRAM.
+ */
+#define GPIO_SPI1_MISO GPIO_SPI1_MISO_1
+#define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_1
+#define GPIO_SPI1_SCK GPIO_SPI1_SCK_1
+
+#define GPIO_SPI2_MISO GPIO_SPI2_MISO_1
+#define GPIO_SPI2_MOSI GPIO_SPI2_MOSI_1
+#define GPIO_SPI2_SCK GPIO_SPI2_SCK_2
+
+/*
+ * Use these in place of the spi_dev_e enumeration to
+ * select a specific SPI device on SPI1
+ */
+#define PX4_SPIDEV_GYRO 1
+#define PX4_SPIDEV_ACCEL_MAG 2
+#define PX4_SPIDEV_BARO 3
+
+/*
+ * Tone alarm output
+ */
+#define TONE_ALARM_TIMER 2 /* timer 2 */
+#define TONE_ALARM_CHANNEL 1 /* channel 1 */
+#define GPIO_TONE_ALARM_IDLE (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN15)
+#define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF1|GPIO_SPEED_2MHz|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN15)
+
+/************************************************************************************
+ * 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);
+
+#undef EXTERN
+#if defined(__cplusplus)
+}
+#endif
+
+#endif /* __ASSEMBLY__ */
+#endif /* __ARCH_BOARD_BOARD_H */
diff --git a/nuttx/configs/px4fmuv2/include/nsh_romfsimg.h b/nuttx/configs/px4fmuv2/include/nsh_romfsimg.h
new file mode 100644
index 000000000..15e4e7a8d
--- /dev/null
+++ b/nuttx/configs/px4fmuv2/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/px4fmuv2/nsh/Make.defs b/nuttx/configs/px4fmuv2/nsh/Make.defs
new file mode 100644
index 000000000..3306e4bf1
--- /dev/null
+++ b/nuttx/configs/px4fmuv2/nsh/Make.defs
@@ -0,0 +1,3 @@
+include ${TOPDIR}/.config
+
+include $(TOPDIR)/configs/px4fmuv2/common/Make.defs
diff --git a/nuttx/configs/px4fmuv2/nsh/appconfig b/nuttx/configs/px4fmuv2/nsh/appconfig
new file mode 100644
index 000000000..0e18aa8ef
--- /dev/null
+++ b/nuttx/configs/px4fmuv2/nsh/appconfig
@@ -0,0 +1,52 @@
+############################################################################
+# configs/px4fmu/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/px4fmuv2/nsh/defconfig b/nuttx/configs/px4fmuv2/nsh/defconfig
new file mode 100755
index 000000000..307c5079c
--- /dev/null
+++ b/nuttx/configs/px4fmuv2/nsh/defconfig
@@ -0,0 +1,1082 @@
+############################################################################
+# configs/px4fmu/nsh/defconfig
+#
+# Copyright (C) 2012 PX4 Development Team. All rights reserved.
+# Copyright (C) 2011-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.
+#
+############################################################################
+#
+# architecture selection
+#
+# CONFIG_ARCH - identifies the arch subdirectory and, hence, the
+# processor architecture.
+# CONFIG_ARCH_family - for use in C code. This identifies the
+# particular chip family that the architecture is implemented
+# in.
+# CONFIG_ARCH_architecture - for use in C code. This identifies the
+# specific architecture within the chip family.
+# CONFIG_ARCH_CHIP - Identifies the arch/*/chip subdirectory
+# CONFIG_ARCH_CHIP_name - For use in C code
+# CONFIG_ARCH_BOARD - identifies the configs subdirectory and, hence,
+# the board that supports the particular chip or SoC.
+# CONFIG_ARCH_BOARD_name - for use in C code
+# CONFIG_ENDIAN_BIG - define if big endian (default is little endian)
+# CONFIG_BOARD_LOOPSPERMSEC - for delay loops
+# CONFIG_DRAM_SIZE - Describes the installed DRAM.
+# CONFIG_DRAM_START - The start address of DRAM (physical)
+# CONFIG_ARCH_IRQPRIO - The STM3240xxx supports interrupt prioritization
+# CONFIG_ARCH_FPU - The STM3240xxx supports a floating point unit (FPU).
+# CONFIG_ARCH_INTERRUPTSTACK - This architecture supports an interrupt
+# stack. If defined, this symbol is the size of the interrupt
+# stack in bytes. If not defined, the user task stacks will be
+# used during interrupt handling.
+# CONFIG_ARCH_STACKDUMP - Do stack dumps after assertions
+# CONFIG_ARCH_BOOTLOADER - Set if you are using a bootloader.
+# CONFIG_ARCH_LEDS - Use LEDs to show state. Unique to board architecture.
+# CONFIG_ARCH_BUTTONS - Enable support for buttons. Unique to board architecture.
+# CONFIG_ARCH_CALIBRATION - Enables some build in instrumentation that
+# cause a 100 second delay during boot-up. This 100 second delay
+# serves no purpose other than it allows you to calibrate
+# CONFIG_BOARD_LOOPSPERMSEC. You simply use a stop watch to measure
+# the 100 second delay then adjust CONFIG_BOARD_LOOPSPERMSEC until
+# the delay actually is 100 seconds.
+# CONFIG_ARCH_DMA - Support DMA initialization
+#
+CONFIG_ARCH="arm"
+CONFIG_ARCH_ARM=y
+CONFIG_ARCH_CORTEXM4=y
+CONFIG_ARCH_CHIP="stm32"
+CONFIG_ARCH_CHIP_STM32F427V=y
+CONFIG_ARCH_BOARD="px4fmuv2"
+CONFIG_ARCH_BOARD_PX4FMUV2=y
+CONFIG_BOARD_LOOPSPERMSEC=16717
+CONFIG_DRAM_SIZE=0x00040000
+CONFIG_DRAM_START=0x20000000
+CONFIG_ARCH_IRQPRIO=y
+CONFIG_ARCH_FPU=y
+CONFIG_ARCH_INTERRUPTSTACK=n
+CONFIG_ARCH_STACKDUMP=y
+CONFIG_ARCH_BOOTLOADER=n
+CONFIG_ARCH_LEDS=n
+CONFIG_ARCH_BUTTONS=n
+CONFIG_ARCH_CALIBRATION=n
+CONFIG_ARCH_DMA=y
+CONFIG_ARCH_MATH_H=y
+
+CONFIG_ARMV7M_CMNVECTOR=y
+CONFIG_STM32_STM32F427=y
+
+#
+# JTAG Enable settings (by default JTAG-DP and SW-DP are enabled):
+#
+# CONFIG_STM32_DFU - Use the DFU bootloader, not JTAG (ignored)
+#
+# JTAG Enable options:
+#
+# CONFIG_STM32_JTAG_FULL_ENABLE - Enables full SWJ (JTAG-DP + SW-DP)
+# CONFIG_STM32_JTAG_NOJNTRST_ENABLE - Enables full SWJ (JTAG-DP + SW-DP)
+# but without JNTRST.
+# CONFIG_STM32_JTAG_SW_ENABLE - Set JTAG-DP disabled and SW-DP enabled
+#
+CONFIG_STM32_DFU=n
+CONFIG_STM32_JTAG_FULL_ENABLE=n
+CONFIG_STM32_JTAG_NOJNTRST_ENABLE=n
+CONFIG_STM32_JTAG_SW_ENABLE=y
+
+#
+# On-chip CCM SRAM configuration
+#
+# CONFIG_STM32_CCMEXCLUDE - Exclude CCM SRAM from the HEAP. You would need
+# to do this if DMA is enabled to prevent non-DMA-able CCM memory from
+# being a part of the stack.
+#
+CONFIG_STM32_CCMEXCLUDE=y
+
+#
+# On-board FSMC SRAM configuration
+#
+# CONFIG_STM32_FSMC - Required. See below
+# CONFIG_MM_REGIONS - Required. Must be 2 or 3 (see above)
+#
+# CONFIG_STM32_FSMC_SRAM=y - Indicates that SRAM is available via the
+# FSMC (as opposed to an LCD or FLASH).
+# CONFIG_HEAP2_BASE - The base address of the SRAM in the FSMC address space
+# CONFIG_HEAP2_END - The end (+1) of the SRAM in the FSMC address space
+#
+#CONFIG_STM32_FSMC_SRAM=n
+#CONFIG_HEAP2_BASE=0x64000000
+#CONFIG_HEAP2_END=(0x64000000+(2*1024*1024))
+
+#
+# Individual subsystems can be enabled:
+#
+# This set is exhaustive for PX4FMU and should be safe to cut and
+# paste into any other config.
+#
+# AHB1:
+CONFIG_STM32_CRC=n
+CONFIG_STM32_BKPSRAM=y
+CONFIG_STM32_CCMDATARAM=y
+CONFIG_STM32_DMA1=y
+CONFIG_STM32_DMA2=y
+CONFIG_STM32_ETHMAC=n
+CONFIG_STM32_OTGHS=n
+# AHB2:
+CONFIG_STM32_DCMI=n
+CONFIG_STM32_CRYP=n
+CONFIG_STM32_HASH=n
+CONFIG_STM32_RNG=n
+CONFIG_STM32_OTGFS=y
+# AHB3:
+CONFIG_STM32_FSMC=n
+# APB1:
+CONFIG_STM32_TIM2=n
+CONFIG_STM32_TIM3=y
+CONFIG_STM32_TIM4=y
+CONFIG_STM32_TIM5=n
+CONFIG_STM32_TIM6=n
+CONFIG_STM32_TIM7=n
+CONFIG_STM32_TIM12=n
+CONFIG_STM32_TIM13=n
+CONFIG_STM32_TIM14=n
+CONFIG_STM32_WWDG=y
+CONFIG_STM32_IWDG=n
+CONFIG_STM32_SPI2=y
+CONFIG_STM32_SPI3=n
+CONFIG_STM32_USART2=y
+CONFIG_STM32_USART3=y
+CONFIG_STM32_UART4=y
+CONFIG_STM32_UART5=n
+CONFIG_STM32_UART7=y
+CONFIG_STM32_UART8=y
+CONFIG_STM32_I2C1=y
+CONFIG_STM32_I2C2=y
+CONFIG_STM32_I2C3=n
+CONFIG_STM32_CAN1=n
+CONFIG_STM32_CAN2=n
+CONFIG_STM32_DAC=n
+CONFIG_STM32_PWR=y
+# APB2:
+CONFIG_STM32_TIM1=y
+CONFIG_STM32_TIM8=n
+CONFIG_STM32_USART1=y
+CONFIG_STM32_USART6=y
+# We use our own driver, but leave this on.
+CONFIG_STM32_ADC1=y
+CONFIG_STM32_ADC2=n
+CONFIG_STM32_ADC3=n
+CONFIG_STM32_SDIO=y
+CONFIG_STM32_SPI1=y
+CONFIG_STM32_SYSCFG=y
+CONFIG_STM32_TIM9=y
+CONFIG_STM32_TIM10=y
+CONFIG_STM32_TIM11=y
+
+#
+# Enable single wire support. If this is not defined, then this mode cannot
+# be enabled.
+#
+CONFIG_STM32_USART_SINGLEWIRE=y
+
+#
+# We want the flash prefetch on for max performance.
+#
+STM32_FLASH_PREFETCH=y
+
+#
+# STM32F40xxx specific serial device driver settings
+#
+# CONFIG_SERIAL_TERMIOS - Serial driver supports termios.h interfaces (tcsetattr,
+# tcflush, etc.). If this is not defined, then the terminal settings (baud,
+# parity, etc.) are not configurable at runtime; serial streams cannot be
+# flushed, etc.
+# CONFIG_SERIAL_CONSOLE_REINIT - re-initializes the console serial port
+# immediately after creating the /dev/console device. This is required
+# if the console serial port has RX DMA enabled.
+#
+# CONFIG_USARTn_SERIAL_CONSOLE - selects the USARTn for the
+# console and ttys0 (default is the USART1).
+# CONFIG_USARTn_RXBUFSIZE - Characters are buffered as received.
+# This specific the size of the receive buffer
+# CONFIG_USARTn_TXBUFSIZE - Characters are buffered before
+# being sent. This specific the size of the transmit buffer
+# CONFIG_USARTn_BAUD - The configure BAUD of the UART. Must be
+# CONFIG_USARTn_BITS - The number of bits. Must be either 7 or 8.
+# CONFIG_USARTn_PARTIY - 0=no parity, 1=odd parity, 2=even parity
+# CONFIG_USARTn_2STOP - Two stop bits
+#
+CONFIG_SERIAL_TERMIOS=y
+CONFIG_SERIAL_CONSOLE_REINIT=y
+CONFIG_STANDARD_SERIAL=y
+
+CONFIG_UART8_SERIAL_CONSOLE=y
+
+#Mavlink messages can be bigger than 128
+CONFIG_USART1_TXBUFSIZE=512
+CONFIG_USART2_TXBUFSIZE=256
+CONFIG_USART3_TXBUFSIZE=256
+CONFIG_UART4_TXBUFSIZE=256
+CONFIG_USART6_TXBUFSIZE=128
+CONFIG_UART7_TXBUFSIZE=256
+CONFIG_UART8_TXBUFSIZE=256
+
+CONFIG_USART1_RXBUFSIZE=512
+CONFIG_USART2_RXBUFSIZE=256
+CONFIG_USART3_RXBUFSIZE=256
+CONFIG_UART4_RXBUFSIZE=256
+CONFIG_USART6_RXBUFSIZE=256
+CONFIG_UART7_RXBUFSIZE=256
+CONFIG_UART8_RXBUFSIZE=256
+
+CONFIG_USART1_BAUD=115200
+CONFIG_USART2_BAUD=115200
+CONFIG_USART3_BAUD=115200
+CONFIG_UART4_BAUD=115200
+CONFIG_USART6_BAUD=9600
+CONFIG_UART7_BAUD=115200
+CONFIG_UART8_BAUD=57600
+
+CONFIG_USART1_BITS=8
+CONFIG_USART2_BITS=8
+CONFIG_USART3_BITS=8
+CONFIG_UART4_BITS=8
+CONFIG_USART6_BITS=8
+CONFIG_UART7_BITS=8
+CONFIG_UART8_BITS=8
+
+CONFIG_USART1_PARITY=0
+CONFIG_USART2_PARITY=0
+CONFIG_USART3_PARITY=0
+CONFIG_UART4_PARITY=0
+CONFIG_USART6_PARITY=0
+CONFIG_UART7_PARITY=0
+CONFIG_UART8_PARITY=0
+
+CONFIG_USART1_2STOP=0
+CONFIG_USART2_2STOP=0
+CONFIG_USART3_2STOP=0
+CONFIG_UART4_2STOP=0
+CONFIG_USART6_2STOP=0
+CONFIG_UART7_2STOP=0
+CONFIG_UART8_2STOP=0
+
+CONFIG_USART1_RXDMA=y
+CONFIG_USART2_RXDMA=y
+CONFIG_USART3_RXDMA=n
+CONFIG_UART4_RXDMA=n
+CONFIG_USART6_RXDMA=y
+CONFIG_UART7_RXDMA=n
+CONFIG_UART8_RXDMA=n
+
+#
+# PX4FMU specific driver settings
+#
+# CONFIG_HRT_TIMER
+# Enables the high-resolution timer. The board definition must
+# set HRT_TIMER and HRT_TIMER_CHANNEL to the timer and capture/
+# compare channels to be used.
+# CONFIG_HRT_PPM
+# Enables R/C PPM input using the HRT. The board definition must
+# set HRT_PPM_CHANNEL to the timer capture/compare channel to be
+# used, and define GPIO_PPM_IN to configure the appropriate timer
+# GPIO.
+#
+CONFIG_HRT_TIMER=y
+CONFIG_HRT_PPM=n
+
+#
+# STM32F40xxx specific SPI device driver settings
+#
+CONFIG_SPI_EXCHANGE=y
+# DMA needs more work, not implemented on STM32F4x yet
+#CONFIG_STM32_SPI_DMA=y
+
+#
+# STM32F40xxx specific CAN device driver settings
+#
+# CONFIG_CAN - Enables CAN support (one or both of CONFIG_STM32_CAN1 or
+# CONFIG_STM32_CAN2 must also be defined)
+# CONFIG_CAN_EXTID - Enables support for the 29-bit extended ID. Default
+# Standard 11-bit IDs.
+# CONFIG_CAN_FIFOSIZE - The size of the circular buffer of CAN messages.
+# Default: 8
+# CONFIG_CAN_NPENDINGRTR - The size of the list of pending RTR requests.
+# Default: 4
+# CONFIG_CAN_LOOPBACK - A CAN driver may or may not support a loopback
+# mode for testing. The STM32 CAN driver does support loopback mode.
+# CONFIG_CAN1_BAUD - CAN1 BAUD rate. Required if CONFIG_STM32_CAN1 is defined.
+# CONFIG_CAN2_BAUD - CAN1 BAUD rate. Required if CONFIG_STM32_CAN2 is defined.
+# CONFIG_CAN_TSEG1 - The number of CAN time quanta in segment 1. Default: 6
+# CONFIG_CAN_TSEG2 - the number of CAN time quanta in segment 2. Default: 7
+#
+CONFIG_CAN=n
+CONFIG_CAN_EXTID=n
+#CONFIG_CAN_FIFOSIZE
+#CONFIG_CAN_NPENDINGRTR
+CONFIG_CAN_LOOPBACK=n
+CONFIG_CAN1_BAUD=700000
+CONFIG_CAN2_BAUD=700000
+
+
+# XXX remove after integration testing
+# Allow 180 us per byte, a wide margin for the 400 KHz clock we're using
+# e.g. 9.6 ms for an EEPROM page write, 0.9 ms for a MAG update
+CONFIG_STM32_I2CTIMEOUS_PER_BYTE=200
+# Constant overhead for generating I2C start / stop conditions
+CONFIG_STM32_I2CTIMEOUS_START_STOP=700
+# XXX this is bad and we want it gone
+CONFIG_I2C_WRITEREAD=y
+
+#
+# I2C configuration
+#
+CONFIG_I2C=y
+CONFIG_I2C_POLLED=n
+CONFIG_I2C_TRANSFER=y
+CONFIG_I2C_TRACE=n
+CONFIG_I2C_RESET=y
+# XXX fixed per-transaction timeout
+CONFIG_STM32_I2CTIMEOMS=10
+
+#
+# MTD support
+#
+CONFIG_MTD=y
+
+# XXX re-enable after integration testing
+
+#
+# I2C configuration
+#
+#CONFIG_I2C=y
+#CONFIG_I2C_POLLED=y
+#CONFIG_I2C_TRANSFER=y
+#CONFIG_I2C_TRACE=n
+#CONFIG_I2C_RESET=y
+
+# Dynamic timeout
+#CONFIG_STM32_I2C_DYNTIMEO=y
+#CONFIG_STM32_I2C_DYNTIMEO_STARTSTOP=500
+#CONFIG_STM32_I2C_DYNTIMEO_USECPERBYTE=200
+
+# Fixed per-transaction timeout
+#CONFIG_STM32_I2CTIMEOSEC=0
+#CONFIG_STM32_I2CTIMEOMS=10
+
+
+
+
+
+
+#
+# General build options
+#
+# CONFIG_RRLOAD_BINARY - make the rrload binary format used with
+# BSPs from www.ridgerun.com using the tools/mkimage.sh script
+# CONFIG_INTELHEX_BINARY - make the Intel HEX binary format
+# used with many different loaders using the GNU objcopy program
+# Should not be selected if you are not using the GNU toolchain.
+# CONFIG_MOTOROLA_SREC - make the Motorola S-Record binary format
+# used with many different loaders using the GNU objcopy program
+# Should not be selected if you are not using the GNU toolchain.
+# CONFIG_RAW_BINARY - make a raw binary format file used with many
+# different loaders using the GNU objcopy program. This option
+# should not be selected if you are not using the GNU toolchain.
+# CONFIG_HAVE_LIBM - toolchain supports libm.a
+#
+CONFIG_RRLOAD_BINARY=n
+CONFIG_INTELHEX_BINARY=n
+CONFIG_MOTOROLA_SREC=n
+CONFIG_RAW_BINARY=y
+CONFIG_HAVE_LIBM=y
+
+#
+# General OS setup
+#
+# CONFIG_APPS_DIR - Identifies the relative path to the directory
+# that builds the application to link with NuttX. Default: ../apps
+# CONFIG_DEBUG - enables built-in debug options
+# CONFIG_DEBUG_VERBOSE - enables verbose debug output
+# CONFIG_DEBUG_SYMBOLS - build without optimization and with
+# debug symbols (needed for use with a debugger).
+# CONFIG_HAVE_CXX - Enable support for C++
+# CONFIG_HAVE_CXXINITIALIZE - The platform-specific logic includes support
+# for initialization of static C++ instances for this architecture
+# and for the selected toolchain (via up_cxxinitialize()).
+# CONFIG_MM_REGIONS - If the architecture includes multiple
+# regions of memory to allocate from, this specifies the
+# number of memory regions that the memory manager must
+# handle and enables the API mm_addregion(start, end);
+# CONFIG_ARCH_LOWPUTC - architecture supports low-level, boot
+# time console output
+# CONFIG_MSEC_PER_TICK - The default system timer is 100Hz
+# or MSEC_PER_TICK=10. This setting may be defined to
+# inform NuttX that the processor hardware is providing
+# system timer interrupts at some interrupt interval other
+# than 10 msec.
+# CONFIG_RR_INTERVAL - The round robin timeslice will be set
+# this number of milliseconds; Round robin scheduling can
+# be disabled by setting this value to zero.
+# CONFIG_SCHED_INSTRUMENTATION - enables instrumentation in
+# scheduler to monitor system performance
+# CONFIG_TASK_NAME_SIZE - Spcifies that maximum size of a
+# task name to save in the TCB. Useful if scheduler
+# instrumentation is selected. Set to zero to disable.
+# CONFIG_START_YEAR, CONFIG_START_MONTH, CONFIG_START_DAY -
+# Used to initialize the internal time logic.
+# CONFIG_GREGORIAN_TIME - Enables Gregorian time conversions.
+# You would only need this if you are concerned about accurate
+# time conversions in the past or in the distant future.
+# CONFIG_JULIAN_TIME - Enables Julian time conversions. You
+# would only need this if you are concerned about accurate
+# time conversion in the distand past. You must also define
+# CONFIG_GREGORIAN_TIME in order to use Julian time.
+# CONFIG_DEV_CONSOLE - Set if architecture-specific logic
+# provides /dev/console. Enables stdout, stderr, stdin.
+# CONFIG_DEV_LOWCONSOLE - Use the simple, low-level serial console
+# driver (minimul support)
+# CONFIG_MUTEX_TYPES: Set to enable support for recursive and
+# errorcheck mutexes. Enables pthread_mutexattr_settype().
+# CONFIG_PRIORITY_INHERITANCE : Set to enable support for priority
+# inheritance on mutexes and semaphores.
+# CONFIG_SEM_PREALLOCHOLDERS: This setting is only used if priority
+# inheritance is enabled. It defines the maximum number of
+# different threads (minus one) that can take counts on a
+# semaphore with priority inheritance support. This may be
+# set to zero if priority inheritance is disabled OR if you
+# are only using semaphores as mutexes (only one holder) OR
+# if no more than two threads participate using a counting
+# semaphore.
+# CONFIG_SEM_NNESTPRIO. If priority inheritance is enabled,
+# then this setting is the maximum number of higher priority
+# threads (minus 1) than can be waiting for another thread
+# to release a count on a semaphore. This value may be set
+# to zero if no more than one thread is expected to wait for
+# a semaphore.
+# CONFIG_FDCLONE_DISABLE. Disable cloning of all file descriptors
+# by task_create() when a new task is started. If set, all
+# files/drivers will appear to be closed in the new task.
+# CONFIG_FDCLONE_STDIO. Disable cloning of all but the first
+# three file descriptors (stdin, stdout, stderr) by task_create()
+# when a new task is started. If set, all files/drivers will
+# appear to be closed in the new task except for stdin, stdout,
+# and stderr.
+# CONFIG_SDCLONE_DISABLE. Disable cloning of all socket
+# desciptors by task_create() when a new task is started. If
+# set, all sockets will appear to be closed in the new task.
+# CONFIG_SCHED_WORKQUEUE. Create a dedicated "worker" thread to
+# handle delayed processing from interrupt handlers. This feature
+# is required for some drivers but, if there are not complaints,
+# can be safely disabled. The worker thread also performs
+# garbage collection -- completing any delayed memory deallocations
+# from interrupt handlers. If the worker thread is disabled,
+# then that clean will be performed by the IDLE thread instead
+# (which runs at the lowest of priority and may not be appropriate
+# if memory reclamation is of high priority). If CONFIG_SCHED_WORKQUEUE
+# is enabled, then the following options can also be used:
+# CONFIG_SCHED_WORKPRIORITY - The execution priority of the worker
+# thread. Default: 192
+# CONFIG_SCHED_WORKPERIOD - How often the worker thread checks for
+# work in units of microseconds. Default: 50*1000 (50 MS).
+# CONFIG_SCHED_WORKSTACKSIZE - The stack size allocated for the worker
+# thread. Default: CONFIG_IDLETHREAD_STACKSIZE.
+# CONFIG_SIG_SIGWORK - The signal number that will be used to wake-up
+# the worker thread. Default: 4
+#
+# CONFIG_SCHED_LPWORK. If CONFIG_SCHED_WORKQUEUE is defined, then a single
+# work queue is created by default. If CONFIG_SCHED_LPWORK is also defined
+# then an additional, lower-priority work queue will also be created. This
+# lower priority work queue is better suited for more extended processing
+# (such as file system clean-up operations)
+# CONFIG_SCHED_LPWORKPRIORITY - The execution priority of the lower priority
+# worker thread. Default: 50
+# CONFIG_SCHED_LPWORKPERIOD - How often the lower priority worker thread
+# checks for work in units of microseconds. Default: 50*1000 (50 MS).
+# CONFIG_SCHED_LPWORKSTACKSIZE - The stack size allocated for the lower
+# priority worker thread. Default: CONFIG_IDLETHREAD_STACKSIZE.
+# CONFIG_SCHED_WAITPID - Enable the waitpid() API
+# CONFIG_SCHED_ATEXIT - Enabled the atexit() API
+#
+CONFIG_USER_ENTRYPOINT="nsh_main"
+#CONFIG_APPS_DIR=
+CONFIG_DEBUG=y
+CONFIG_DEBUG_VERBOSE=y
+CONFIG_DEBUG_SYMBOLS=y
+CONFIG_DEBUG_FS=n
+CONFIG_DEBUG_GRAPHICS=n
+CONFIG_DEBUG_LCD=n
+CONFIG_DEBUG_USB=n
+CONFIG_DEBUG_NET=n
+CONFIG_DEBUG_RTC=n
+CONFIG_DEBUG_ANALOG=n
+CONFIG_DEBUG_PWM=n
+CONFIG_DEBUG_CAN=n
+CONFIG_DEBUG_I2C=n
+CONFIG_DEBUG_INPUT=n
+
+CONFIG_HAVE_CXX=y
+CONFIG_HAVE_CXXINITIALIZE=y
+CONFIG_MM_REGIONS=2
+CONFIG_ARCH_LOWPUTC=y
+CONFIG_MSEC_PER_TICK=1
+CONFIG_RR_INTERVAL=0
+CONFIG_SCHED_INSTRUMENTATION=y
+CONFIG_TASK_NAME_SIZE=24
+CONFIG_START_YEAR=1970
+CONFIG_START_MONTH=1
+CONFIG_START_DAY=1
+CONFIG_GREGORIAN_TIME=n
+CONFIG_JULIAN_TIME=n
+CONFIG_DEV_CONSOLE=y
+CONFIG_DEV_LOWCONSOLE=n
+CONFIG_MUTEX_TYPES=n
+CONFIG_PRIORITY_INHERITANCE=y
+CONFIG_SEM_PREALLOCHOLDERS=0
+CONFIG_SEM_NNESTPRIO=8
+CONFIG_FDCLONE_DISABLE=n
+CONFIG_FDCLONE_STDIO=y
+CONFIG_SDCLONE_DISABLE=y
+CONFIG_SCHED_WORKQUEUE=y
+CONFIG_SCHED_WORKPRIORITY=192
+CONFIG_SCHED_WORKPERIOD=5000
+CONFIG_SCHED_WORKSTACKSIZE=2048
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKPERIOD=50000
+CONFIG_SCHED_LPWORKSTACKSIZE=2048
+CONFIG_SIG_SIGWORK=4
+CONFIG_SCHED_WAITPID=y
+CONFIG_SCHED_ATEXIT=n
+
+#
+# System Logging
+#
+# CONFIG_SYSLOG - Enables the System Logging feature.
+# CONFIG_RAMLOG - Enables the RAM logging feature
+# CONFIG_RAMLOG_CONSOLE - Use the RAM logging device as a system console.
+# If this feature is enabled (along with CONFIG_DEV_CONSOLE), then all
+# console output will be re-directed to a circular buffer in RAM. This
+# is useful, for example, if the only console is a Telnet console. Then
+# in that case, console output from non-Telnet threads will go to the
+# circular buffer and can be viewed using the NSH 'dmesg' command.
+# CONFIG_RAMLOG_SYSLOG - Use the RAM logging device for the syslogging
+# interface. If this feature is enabled (along with CONFIG_SYSLOG),
+# then all debug output (only) will be re-directed to the circular
+# buffer in RAM. This RAM log can be view from NSH using the 'dmesg'
+# command.
+# CONFIG_RAMLOG_NPOLLWAITERS - The number of threads than can be waiting
+# for this driver on poll(). Default: 4
+#
+# If CONFIG_RAMLOG_CONSOLE or CONFIG_RAMLOG_SYSLOG is selected, then the
+# following may also be provided:
+#
+# CONFIG_RAMLOG_CONSOLE_BUFSIZE - Size of the console RAM log. Default: 1024
+#
+
+CONFIG_SYSLOG=n
+CONFIG_RAMLOG=n
+CONFIG_RAMLOG_CONSOLE=n
+CONFIG_RAMLOG_SYSLOG=n
+#CONFIG_RAMLOG_NPOLLWAITERS
+#CONFIG_RAMLOG_CONSOLE_BUFSIZE
+
+#
+# The following can be used to disable categories of
+# APIs supported by the OS. If the compiler supports
+# weak functions, then it should not be necessary to
+# disable functions unless you want to restrict usage
+# of those APIs.
+#
+# There are certain dependency relationships in these
+# features.
+#
+# o mq_notify logic depends on signals to awaken tasks
+# waiting for queues to become full or empty.
+# o pthread_condtimedwait() depends on signals to wake
+# up waiting tasks.
+#
+CONFIG_DISABLE_CLOCK=n
+CONFIG_DISABLE_POSIX_TIMERS=n
+CONFIG_DISABLE_PTHREAD=n
+CONFIG_DISABLE_SIGNALS=n
+CONFIG_DISABLE_MQUEUE=n
+CONFIG_DISABLE_MOUNTPOINT=n
+CONFIG_DISABLE_ENVIRON=n
+CONFIG_DISABLE_POLL=n
+
+#
+# Misc libc settings
+#
+# CONFIG_NOPRINTF_FIELDWIDTH - sprintf-related logic is a
+# little smaller if we do not support fieldwidthes
+# CONFIG_LIBC_FLOATINGPOINT - Enables printf("%f")
+# CONFIG_LIBC_FIXEDPRECISION - Sets 7 digits after dot for printing:
+# 5.1234567
+# CONFIG_HAVE_LONG_LONG - Enabled printf("%llu)
+#
+CONFIG_NOPRINTF_FIELDWIDTH=n
+CONFIG_LIBC_FLOATINGPOINT=y
+CONFIG_HAVE_LONG_LONG=y
+
+#
+# Allow for architecture optimized implementations
+#
+# The architecture can provide optimized versions of the
+# following to improve system performance
+#
+CONFIG_ARCH_MEMCPY=y
+CONFIG_ARCH_MEMCMP=n
+CONFIG_ARCH_MEMMOVE=n
+CONFIG_ARCH_MEMSET=n
+CONFIG_ARCH_STRCMP=n
+CONFIG_ARCH_STRCPY=n
+CONFIG_ARCH_STRNCPY=n
+CONFIG_ARCH_STRLEN=n
+CONFIG_ARCH_STRNLEN=n
+CONFIG_ARCH_BZERO=n
+
+#
+# Sizes of configurable things (0 disables)
+#
+# CONFIG_MAX_TASKS - The maximum number of simultaneously
+# active tasks. This value must be a power of two.
+# CONFIG_MAX_TASK_ARGS - This controls the maximum number of
+# of parameters that a task may receive (i.e., maxmum value
+# of 'argc')
+# CONFIG_NPTHREAD_KEYS - The number of items of thread-
+# specific data that can be retained
+# CONFIG_NFILE_DESCRIPTORS - The maximum number of file
+# descriptors (one for each open)
+# CONFIG_NFILE_STREAMS - The maximum number of streams that
+# can be fopen'ed
+# CONFIG_NAME_MAX - The maximum size of a file name.
+# CONFIG_STDIO_BUFFER_SIZE - Size of the buffer to allocate
+# on fopen. (Only if CONFIG_NFILE_STREAMS > 0)
+# CONFIG_STDIO_LINEBUFFER - If standard C buffered I/O is enabled
+# (CONFIG_STDIO_BUFFER_SIZE > 0), then this option may be added
+# to force automatic, line-oriented flushing the output buffer
+# for putc(), fputc(), putchar(), puts(), fputs(), printf(),
+# fprintf(), and vfprintf(). When a newline is encountered in
+# the output string, the output buffer will be flushed. This
+# (slightly) increases the NuttX footprint but supports the kind
+# of behavior that people expect for printf().
+# CONFIG_NUNGET_CHARS - Number of characters that can be
+# buffered by ungetc() (Only if CONFIG_NFILE_STREAMS > 0)
+# CONFIG_PREALLOC_MQ_MSGS - The number of pre-allocated message
+# structures. The system manages a pool of preallocated
+# message structures to minimize dynamic allocations
+# CONFIG_MQ_MAXMSGSIZE - Message structures are allocated with
+# a fixed payload size given by this settin (does not include
+# other message structure overhead.
+# CONFIG_MAX_WDOGPARMS - Maximum number of parameters that
+# can be passed to a watchdog handler
+# CONFIG_PREALLOC_WDOGS - The number of pre-allocated watchdog
+# structures. The system manages a pool of preallocated
+# watchdog structures to minimize dynamic allocations
+# CONFIG_PREALLOC_TIMERS - The number of pre-allocated POSIX
+# timer structures. The system manages a pool of preallocated
+# timer structures to minimize dynamic allocations. Set to
+# zero for all dynamic allocations.
+#
+CONFIG_MAX_TASKS=32
+CONFIG_MAX_TASK_ARGS=8
+CONFIG_NPTHREAD_KEYS=4
+CONFIG_NFILE_DESCRIPTORS=32
+CONFIG_NFILE_STREAMS=25
+CONFIG_NAME_MAX=32
+CONFIG_STDIO_BUFFER_SIZE=256
+CONFIG_STDIO_LINEBUFFER=y
+CONFIG_NUNGET_CHARS=2
+CONFIG_PREALLOC_MQ_MSGS=4
+CONFIG_MQ_MAXMSGSIZE=32
+CONFIG_MAX_WDOGPARMS=2
+CONFIG_PREALLOC_WDOGS=50
+CONFIG_PREALLOC_TIMERS=50
+
+#
+# Filesystem configuration
+#
+# CONFIG_FS_FAT - Enable FAT filesystem support
+# CONFIG_FAT_SECTORSIZE - Max supported sector size
+# CONFIG_FAT_LCNAMES - Enable use of the NT-style upper/lower case 8.3
+# file name support.
+# CONFIG_FAT_LFN - Enable FAT long file names. NOTE: Microsoft claims
+# patents on FAT long file name technology. Please read the
+# disclaimer in the top-level COPYING file and only enable this
+# feature if you understand these issues.
+# CONFIG_FAT_MAXFNAME - If CONFIG_FAT_LFN is defined, then the
+# default, maximum long file name is 255 bytes. This can eat up
+# a lot of memory (especially stack space). If you are willing
+# to live with some non-standard, short long file names, then
+# define this value. A good choice would be the same value as
+# selected for CONFIG_NAME_MAX which will limit the visibility
+# of longer file names anyway.
+# CONFIG_FS_NXFFS: Enable NuttX FLASH file system (NXFF) support.
+# CONFIG_NXFFS_ERASEDSTATE: The erased state of FLASH.
+# This must have one of the values of 0xff or 0x00.
+# Default: 0xff.
+# CONFIG_NXFFS_PACKTHRESHOLD: When packing flash file data,
+# don't both with file chunks smaller than this number of data bytes.
+# CONFIG_NXFFS_MAXNAMLEN: The maximum size of an NXFFS file name.
+# Default: 255.
+# CONFIG_NXFFS_PACKTHRESHOLD: When packing flash file data,
+# don't both with file chunks smaller than this number of data bytes.
+# Default: 32.
+# CONFIG_NXFFS_TAILTHRESHOLD: clean-up can either mean
+# packing files together toward the end of the file or, if file are
+# deleted at the end of the file, clean up can simply mean erasing
+# the end of FLASH memory so that it can be re-used again. However,
+# doing this can also harm the life of the FLASH part because it can
+# mean that the tail end of the FLASH is re-used too often. This
+# threshold determines if/when it is worth erased the tail end of FLASH
+# and making it available for re-use (and possible over-wear).
+# Default: 8192.
+# CONFIG_FS_ROMFS - Enable ROMFS filesystem support
+# CONFIG_FS_RAMMAP - For file systems that do not support XIP, this
+# option will enable a limited form of memory mapping that is
+# implemented by copying whole files into memory.
+#
+CONFIG_FS_FAT=y
+CONFIG_FAT_LCNAMES=y
+CONFIG_FAT_LFN=y
+CONFIG_FAT_MAXFNAME=32
+CONFIG_FS_NXFFS=y
+CONFIG_NXFFS_MAXNAMLEN=32
+CONFIG_NXFFS_TAILTHRESHOLD=2048
+CONFIG_NXFFS_PREALLOCATED=y
+CONFIG_FS_ROMFS=y
+CONFIG_FS_BINFS=y
+
+#
+# SPI-based MMC/SD driver
+#
+# CONFIG_MMCSD_NSLOTS
+# Number of MMC/SD slots supported by the driver
+# CONFIG_MMCSD_READONLY
+# Provide read-only access (default is read/write)
+# CONFIG_MMCSD_SPICLOCK - Maximum SPI clock to drive MMC/SD card.
+# Default is 20MHz, current setting 24 MHz
+#
+#CONFIG_MMCSD=n
+# XXX need to rejig this for SDIO
+#CONFIG_MMCSD_SPI=y
+#CONFIG_MMCSD_NSLOTS=1
+#CONFIG_MMCSD_READONLY=n
+#CONFIG_MMCSD_SPICLOCK=24000000
+
+#
+# Maintain legacy build behavior (revisit)
+#
+
+CONFIG_MMCSD=y
+#CONFIG_MMCSD_SPI=y
+CONFIG_MMCSD_SDIO=y
+CONFIG_MTD=y
+
+#
+# SPI-based MMC/SD driver
+#
+#CONFIG_MMCSD_NSLOTS=1
+#CONFIG_MMCSD_READONLY=n
+#CONFIG_MMCSD_SPICLOCK=12500000
+
+#
+# STM32 SDIO-based MMC/SD driver
+#
+CONFIG_SDIO_DMA=y
+#CONFIG_SDIO_PRI=128
+#CONFIG_SDIO_DMAPRIO
+#CONFIG_SDIO_WIDTH_D1_ONLY
+CONFIG_MMCSD_MULTIBLOCK_DISABLE=y
+CONFIG_MMCSD_MMCSUPPORT=n
+CONFIG_MMCSD_HAVECARDDETECT=n
+
+#
+# Block driver buffering
+#
+# CONFIG_FS_READAHEAD
+# Enable read-ahead buffering
+# CONFIG_FS_WRITEBUFFER
+# Enable write buffering
+#
+CONFIG_FS_READAHEAD=n
+CONFIG_FS_WRITEBUFFER=n
+
+#
+# RTC Configuration
+#
+# CONFIG_RTC - Enables general support for a hardware RTC. Specific
+# architectures may require other specific settings.
+# CONFIG_RTC_DATETIME - There are two general types of RTC: (1) A simple
+# battery backed counter that keeps the time when power is down, and (2)
+# A full date / time RTC the provides the date and time information, often
+# in BCD format. If CONFIG_RTC_DATETIME is selected, it specifies this
+# second kind of RTC. In this case, the RTC is used to "seed" the normal
+# NuttX timer and the NuttX system timer provides for higher resoution
+# time.
+# CONFIG_RTC_HIRES - If CONFIG_RTC_DATETIME not selected, then the simple,
+# battery backed counter is used. There are two different implementations
+# of such simple counters based on the time resolution of the counter:
+# The typical RTC keeps time to resolution of 1 second, usually
+# supporting a 32-bit time_t value. In this case, the RTC is used to
+# "seed" the normal NuttX timer and the NuttX timer provides for higher
+# resoution time. If CONFIG_RTC_HIRES is enabled in the NuttX configuration,
+# then the RTC provides higher resolution time and completely replaces the
+# system timer for purpose of date and time.
+# CONFIG_RTC_FREQUENCY - If CONFIG_RTC_HIRES is defined, then the frequency
+# of the high resolution RTC must be provided. If CONFIG_RTC_HIRES is
+# not defined, CONFIG_RTC_FREQUENCY is assumed to be one.
+# CONFIG_RTC_ALARM - Enable if the RTC hardware supports setting of an
+# alarm. A callback function will be executed when the alarm goes off
+#
+CONFIG_RTC=n
+CONFIG_RTC_DATETIME=y
+CONFIG_RTC_HIRES=n
+CONFIG_RTC_FREQUENCY=n
+CONFIG_RTC_ALARM=n
+
+#
+# USB Device Configuration
+#
+# CONFIG_USBDEV
+# Enables USB device support
+# CONFIG_USBDEV_ISOCHRONOUS
+# Build in extra support for isochronous endpoints
+# CONFIG_USBDEV_DUALSPEED
+# Hardware handles high and full speed operation (USB 2.0)
+# CONFIG_USBDEV_SELFPOWERED
+# Will cause USB features to indicate that the device is
+# self-powered
+# CONFIG_USBDEV_MAXPOWER
+# Maximum power consumption in mA
+# CONFIG_USBDEV_TRACE
+# Enables USB tracing for debug
+# CONFIG_USBDEV_TRACE_NRECORDS
+# Number of trace entries to remember
+#
+CONFIG_USBDEV=y
+CONFIG_USBDEV_ISOCHRONOUS=n
+CONFIG_USBDEV_DUALSPEED=n
+CONFIG_USBDEV_SELFPOWERED=y
+CONFIG_USBDEV_REMOTEWAKEUP=n
+CONFIG_USBDEV_MAXPOWER=500
+CONFIG_USBDEV_TRACE=n
+CONFIG_USBDEV_TRACE_NRECORDS=512
+
+#
+# USB serial device class driver (Standard CDC ACM class)
+#
+# CONFIG_CDCACM
+# Enable compilation of the USB serial driver
+# CONFIG_CDCACM_CONSOLE
+# Configures the CDC/ACM serial port as the console device.
+# CONFIG_CDCACM_EP0MAXPACKET
+# Endpoint 0 max packet size. Default 64
+# CONFIG_CDCACM_EPINTIN
+# The logical 7-bit address of a hardware endpoint that supports
+# interrupt IN operation. Default 2.
+# CONFIG_CDCACM_EPINTIN_FSSIZE
+# Max package size for the interrupt IN endpoint if full speed mode.
+# Default 64.
+# CONFIG_CDCACM_EPINTIN_HSSIZE
+# Max package size for the interrupt IN endpoint if high speed mode.
+# Default 64
+# CONFIG_CDCACM_EPBULKOUT
+# The logical 7-bit address of a hardware endpoint that supports
+# bulk OUT operation. Default 4.
+# CONFIG_CDCACM_EPBULKOUT_FSSIZE
+# Max package size for the bulk OUT endpoint if full speed mode.
+# Default 64.
+# CONFIG_CDCACM_EPBULKOUT_HSSIZE
+# Max package size for the bulk OUT endpoint if high speed mode.
+# Default 512.
+# CONFIG_CDCACM_EPBULKIN
+# The logical 7-bit address of a hardware endpoint that supports
+# bulk IN operation. Default 3.
+# CONFIG_CDCACM_EPBULKIN_FSSIZE
+# Max package size for the bulk IN endpoint if full speed mode.
+# Default 64.
+# CONFIG_CDCACM_EPBULKIN_HSSIZE
+# Max package size for the bulk IN endpoint if high speed mode.
+# Default 512.
+# CONFIG_CDCACM_NWRREQS and CONFIG_CDCACM_NRDREQS
+# The number of write/read requests that can be in flight.
+# Default 256.
+# CONFIG_CDCACM_VENDORID and CONFIG_CDCACM_VENDORSTR
+# The vendor ID code/string. Default 0x0525 and "NuttX"
+# 0x0525 is the Netchip vendor and should not be used in any
+# products. This default VID was selected for compatibility with
+# the Linux CDC ACM default VID.
+# CONFIG_CDCACM_PRODUCTID and CONFIG_CDCACM_PRODUCTSTR
+# The product ID code/string. Default 0xa4a7 and "CDC/ACM Serial"
+# 0xa4a7 was selected for compatibility with the Linux CDC ACM
+# default PID.
+# CONFIG_CDCACM_RXBUFSIZE and CONFIG_CDCACM_TXBUFSIZE
+# Size of the serial receive/transmit buffers. Default 256.
+#
+CONFIG_CDCACM=y
+CONFIG_CDCACM_CONSOLE=n
+#CONFIG_CDCACM_EP0MAXPACKET
+CONFIG_CDCACM_EPINTIN=1
+#CONFIG_CDCACM_EPINTIN_FSSIZE
+#CONFIG_CDCACM_EPINTIN_HSSIZE
+CONFIG_CDCACM_EPBULKOUT=3
+#CONFIG_CDCACM_EPBULKOUT_FSSIZE
+#CONFIG_CDCACM_EPBULKOUT_HSSIZE
+CONFIG_CDCACM_EPBULKIN=2
+#CONFIG_CDCACM_EPBULKIN_FSSIZE
+#CONFIG_CDCACM_EPBULKIN_HSSIZE
+#CONFIG_CDCACM_NWRREQS
+#CONFIG_CDCACM_NRDREQS
+CONFIG_CDCACM_VENDORID=0x26AC
+CONFIG_CDCACM_VENDORSTR="3D Robotics"
+CONFIG_CDCACM_PRODUCTID=0x0010
+CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v1.6"
+#CONFIG_CDCACM_RXBUFSIZE
+#CONFIG_CDCACM_TXBUFSIZE
+
+
+#
+# Settings for apps/nshlib
+#
+# CONFIG_NSH_BUILTIN_APPS - Support external registered,
+# "named" applications that can be executed from the NSH
+# command line (see apps/README.txt for more information).
+# CONFIG_NSH_FILEIOSIZE - Size of a static I/O buffer
+# CONFIG_NSH_STRERROR - Use strerror(errno)
+# CONFIG_NSH_LINELEN - Maximum length of one command line
+# CONFIG_NSH_MAX_ARGUMENTS - Maximum number of arguments for command line
+# CONFIG_NSH_NESTDEPTH - Max number of nested if-then[-else]-fi
+# CONFIG_NSH_DISABLESCRIPT - Disable scripting support
+# CONFIG_NSH_DISABLEBG - Disable background commands
+# CONFIG_NSH_ROMFSETC - Use startup script in /etc
+# CONFIG_NSH_CONSOLE - Use serial console front end
+# CONFIG_NSH_TELNET - Use telnetd console front end
+# CONFIG_NSH_ARCHINIT - Platform provides architecture
+# specific initialization (nsh_archinitialize()).
+#
+# If CONFIG_NSH_TELNET is selected:
+# CONFIG_NSH_IOBUFFER_SIZE -- Telnetd I/O buffer size
+# CONFIG_NSH_DHCPC - Obtain address using DHCP
+# CONFIG_NSH_IPADDR - Provides static IP address
+# CONFIG_NSH_DRIPADDR - Provides static router IP address
+# CONFIG_NSH_NETMASK - Provides static network mask
+# CONFIG_NSH_NOMAC - Use a bogus MAC address
+#
+# If CONFIG_NSH_ROMFSETC is selected:
+# CONFIG_NSH_ROMFSMOUNTPT - ROMFS mountpoint
+# CONFIG_NSH_INITSCRIPT - Relative path to init script
+# CONFIG_NSH_ROMFSDEVNO - ROMFS RAM device minor
+# CONFIG_NSH_ROMFSSECTSIZE - ROMF sector size
+# CONFIG_NSH_FATDEVNO - FAT FS RAM device minor
+# CONFIG_NSH_FATSECTSIZE - FAT FS sector size
+# CONFIG_NSH_FATNSECTORS - FAT FS number of sectors
+# CONFIG_NSH_FATMOUNTPT - FAT FS mountpoint
+#
+CONFIG_BUILTIN=y
+CONFIG_NSH_BUILTIN_APPS=y
+CONFIG_NSH_FILEIOSIZE=512
+CONFIG_NSH_STRERROR=y
+CONFIG_NSH_LINELEN=128
+CONFIG_NSH_MAX_ARGUMENTS=12
+CONFIG_NSH_NESTDEPTH=8
+CONFIG_NSH_DISABLESCRIPT=n
+CONFIG_NSH_DISABLEBG=n
+CONFIG_NSH_ROMFSETC=y
+CONFIG_NSH_ARCHROMFS=y
+CONFIG_NSH_CONSOLE=y
+CONFIG_NSH_USBCONSOLE=n
+CONFIG_NSH_USBCONDEV="/dev/ttyACM0"
+CONFIG_NSH_TELNET=n
+CONFIG_NSH_ARCHINIT=y
+CONFIG_NSH_IOBUFFER_SIZE=512
+CONFIG_NSH_DHCPC=n
+CONFIG_NSH_NOMAC=y
+CONFIG_NSH_IPADDR=0x0a000002
+CONFIG_NSH_DRIPADDR=0x0a000001
+CONFIG_NSH_NETMASK=0xffffff00
+CONFIG_NSH_ROMFSMOUNTPT="/etc"
+CONFIG_NSH_INITSCRIPT="init.d/rcS"
+CONFIG_NSH_ROMFSDEVNO=0
+CONFIG_NSH_ROMFSSECTSIZE=128 # Default 64, increased to allow for more than 64 folders on the sdcard
+CONFIG_NSH_FATDEVNO=1
+CONFIG_NSH_FATSECTSIZE=512
+CONFIG_NSH_FATNSECTORS=1024
+CONFIG_NSH_FATMOUNTPT=/tmp
+
+#
+# Architecture-specific NSH options
+#
+#CONFIG_NSH_MMCSDSPIPORTNO=3
+CONFIG_NSH_MMCSDSLOTNO=0
+CONFIG_NSH_MMCSDMINOR=0
+
+
+#
+# Stack and heap information
+#
+# CONFIG_BOOT_RUNFROMFLASH - Some configurations support XIP
+# operation from FLASH but must copy initialized .data sections to RAM.
+# (should also be =n for the STM3240G-EVAL which always runs from flash)
+# CONFIG_BOOT_COPYTORAM - Some configurations boot in FLASH
+# but copy themselves entirely into RAM for better performance.
+# CONFIG_CUSTOM_STACK - The up_ implementation will handle
+# all stack operations outside of the nuttx model.
+# CONFIG_STACK_POINTER - The initial stack pointer (arm7tdmi only)
+# CONFIG_IDLETHREAD_STACKSIZE - The size of the initial stack.
+# This is the thread that (1) performs the inital boot of the system up
+# to the point where user_start() is spawned, and (2) there after is the
+# IDLE thread that executes only when there is no other thread ready to
+# run.
+# CONFIG_USERMAIN_STACKSIZE - The size of the stack to allocate
+# for the main user thread that begins at the user_start() entry point.
+# CONFIG_PTHREAD_STACK_MIN - Minimum pthread stack size
+# CONFIG_PTHREAD_STACK_DEFAULT - Default pthread stack size
+# CONFIG_HEAP_BASE - The beginning of the heap
+# CONFIG_HEAP_SIZE - The size of the heap
+#
+CONFIG_BOOT_RUNFROMFLASH=n
+CONFIG_BOOT_COPYTORAM=n
+CONFIG_CUSTOM_STACK=n
+CONFIG_STACK_POINTER=
+# Idle thread needs 4096 bytes
+# default 1 KB is not enough
+# 4096 bytes
+CONFIG_IDLETHREAD_STACKSIZE=6000
+# USERMAIN stack size probably needs to be around 4096 bytes
+CONFIG_USERMAIN_STACKSIZE=4096
+CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_PTHREAD_STACK_DEFAULT=2048
+CONFIG_HEAP_BASE=
+CONFIG_HEAP_SIZE=
+
+# enable bindir
+CONFIG_APPS_BINDIR=y
diff --git a/nuttx/configs/px4fmuv2/nsh/setenv.sh b/nuttx/configs/px4fmuv2/nsh/setenv.sh
new file mode 100755
index 000000000..265520997
--- /dev/null
+++ b/nuttx/configs/px4fmuv2/nsh/setenv.sh
@@ -0,0 +1,67 @@
+#!/bin/bash
+# configs/stm3240g-eval/nsh/setenv.sh
+#
+# 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.
+#
+
+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 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 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"
+
+# This 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/px4fmuv2/src/Makefile b/nuttx/configs/px4fmuv2/src/Makefile
new file mode 100644
index 000000000..d4276f7fc
--- /dev/null
+++ b/nuttx/configs/px4fmuv2/src/Makefile
@@ -0,0 +1,84 @@
+############################################################################
+# configs/px4fmu/src/Makefile
+#
+# 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)/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/px4fmuv2/src/empty.c b/nuttx/configs/px4fmuv2/src/empty.c
new file mode 100644
index 000000000..ace900866
--- /dev/null
+++ b/nuttx/configs/px4fmuv2/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/nuttx/configs/px4iov2/README.txt b/nuttx/configs/px4iov2/README.txt
new file mode 100755
index 000000000..9b1615f42
--- /dev/null
+++ b/nuttx/configs/px4iov2/README.txt
@@ -0,0 +1,806 @@
+README
+======
+
+This README discusses issues unique to NuttX configurations for the
+STMicro STM3210E-EVAL development board.
+
+Contents
+========
+
+ - Development Environment
+ - GNU Toolchain Options
+ - IDEs
+ - NuttX buildroot Toolchain
+ - DFU and JTAG
+ - OpenOCD
+ - LEDs
+ - Temperature Sensor
+ - RTC
+ - STM3210E-EVAL-specific Configuration Options
+ - Configurations
+
+Development Environment
+=======================
+
+ Either Linux or Cygwin on Windows can be used for the development environment.
+ The source has been built only using the GNU toolchain (see below). Other
+ toolchains will likely cause problems. Testing was performed using the Cygwin
+ environment because the Raisonance R-Link emulatator and some RIDE7 development tools
+ were used and those tools works only under Windows.
+
+GNU Toolchain Options
+=====================
+
+ The NuttX make system has been modified to support the following different
+ toolchain options.
+
+ 1. The CodeSourcery GNU toolchain,
+ 2. The devkitARM GNU toolchain,
+ 3. Raisonance GNU toolchain, or
+ 4. The NuttX buildroot Toolchain (see below).
+
+ All testing has been conducted using the NuttX buildroot toolchain. However,
+ the make system is setup to default to use the devkitARM toolchain. To use
+ the CodeSourcery, devkitARM or Raisonance GNU toolchain, you simply need to
+ add one of the following configuration options to your .config (or defconfig)
+ file:
+
+ CONFIG_STM32_CODESOURCERYW=y : CodeSourcery under Windows
+ CONFIG_STM32_CODESOURCERYL=y : CodeSourcery under Linux
+ CONFIG_STM32_DEVKITARM=y : devkitARM under Windows
+ CONFIG_STM32_RAISONANCE=y : Raisonance RIDE7 under Windows
+ CONFIG_STM32_BUILDROOT=y : NuttX buildroot under Linux or Cygwin (default)
+
+ If you are not using CONFIG_STM32_BUILDROOT, then you may also have to modify
+ the PATH in the setenv.h file if your make cannot find the tools.
+
+ NOTE: the CodeSourcery (for Windows), devkitARM, and Raisonance toolchains are
+ Windows native toolchains. The CodeSourcey (for Linux) and NuttX buildroot
+ toolchains are Cygwin and/or Linux native toolchains. There are several limitations
+ to using a Windows based toolchain in a Cygwin environment. The three biggest are:
+
+ 1. The Windows toolchain cannot follow Cygwin paths. Path conversions are
+ performed automatically in the Cygwin makefiles using the 'cygpath' utility
+ but you might easily find some new path problems. If so, check out 'cygpath -w'
+
+ 2. Windows toolchains cannot follow Cygwin symbolic links. Many symbolic links
+ are used in Nuttx (e.g., include/arch). The make system works around these
+ problems for the Windows tools by copying directories instead of linking them.
+ But this can also cause some confusion for you: For example, you may edit
+ a file in a "linked" directory and find that your changes had no effect.
+ That is because you are building the copy of the file in the "fake" symbolic
+ directory. If you use a Windows toolchain, you should get in the habit of
+ making like this:
+
+ make clean_context all
+
+ An alias in your .bashrc file might make that less painful.
+
+ 3. Dependencies are not made when using Windows versions of the GCC. This is
+ because the dependencies are generated using Windows pathes which do not
+ work with the Cygwin make.
+
+ Support has been added for making dependencies with the windows-native toolchains.
+ That support can be enabled by modifying your Make.defs file as follows:
+
+ - MKDEP = $(TOPDIR)/tools/mknulldeps.sh
+ + MKDEP = $(TOPDIR)/tools/mkdeps.sh --winpaths "$(TOPDIR)"
+
+ If you have problems with the dependency build (for example, if you are not
+ building on C:), then you may need to modify tools/mkdeps.sh
+
+ NOTE 1: The CodeSourcery toolchain (2009q1) does not work with default optimization
+ level of -Os (See Make.defs). It will work with -O0, -O1, or -O2, but not with
+ -Os.
+
+ NOTE 2: The devkitARM toolchain includes a version of MSYS make. Make sure that
+ the paths to Cygwin's /bin and /usr/bin directories appear BEFORE the devkitARM
+ path or will get the wrong version of make.
+
+IDEs
+====
+
+ NuttX is built using command-line make. It can be used with an IDE, but some
+ effort will be required to create the project (There is a simple RIDE project
+ in the RIDE subdirectory).
+
+ Makefile Build
+ --------------
+ Under Eclipse, it is pretty easy to set up an "empty makefile project" and
+ simply use the NuttX makefile to build the system. That is almost for free
+ under Linux. Under Windows, you will need to set up the "Cygwin GCC" empty
+ makefile project in order to work with Windows (Google for "Eclipse Cygwin" -
+ there is a lot of help on the internet).
+
+ Native Build
+ ------------
+ Here are a few tips before you start that effort:
+
+ 1) Select the toolchain that you will be using in your .config file
+ 2) Start the NuttX build at least one time from the Cygwin command line
+ before trying to create your project. This is necessary to create
+ certain auto-generated files and directories that will be needed.
+ 3) Set up include pathes: You will need include/, arch/arm/src/stm32,
+ arch/arm/src/common, arch/arm/src/armv7-m, and sched/.
+ 4) All assembly files need to have the definition option -D __ASSEMBLY__
+ on the command line.
+
+ Startup files will probably cause you some headaches. The NuttX startup file
+ is arch/arm/src/stm32/stm32_vectors.S. With RIDE, I have to build NuttX
+ one time from the Cygwin command line in order to obtain the pre-built
+ startup object needed by RIDE.
+
+NuttX buildroot Toolchain
+=========================
+
+ A GNU GCC-based toolchain is assumed. The files */setenv.sh should
+ be modified to point to the correct path to the Cortex-M3 GCC toolchain (if
+ different from the default in your PATH variable).
+
+ If you have no Cortex-M3 toolchain, one can be downloaded from the NuttX
+ SourceForge download site (https://sourceforge.net/project/showfiles.php?group_id=189573).
+ This GNU toolchain builds and executes in the Linux or Cygwin environment.
+
+ 1. You must have already configured Nuttx in <some-dir>/nuttx.
+
+ cd tools
+ ./configure.sh stm3210e-eval/<sub-dir>
+
+ 2. Download the latest buildroot package into <some-dir>
+
+ 3. unpack the buildroot tarball. The resulting directory may
+ have versioning information on it like buildroot-x.y.z. If so,
+ rename <some-dir>/buildroot-x.y.z to <some-dir>/buildroot.
+
+ 4. cd <some-dir>/buildroot
+
+ 5. cp configs/cortexm3-defconfig-4.3.3 .config
+
+ 6. make oldconfig
+
+ 7. make
+
+ 8. Edit setenv.h, if necessary, so that the PATH variable includes
+ the path to the newly built binaries.
+
+ See the file configs/README.txt in the buildroot source tree. That has more
+ detailed PLUS some special instructions that you will need to follow if you are
+ building a Cortex-M3 toolchain for Cygwin under Windows.
+
+DFU and JTAG
+============
+
+ Enbling Support for the DFU Bootloader
+ --------------------------------------
+ The linker files in these projects can be configured to indicate that you
+ will be loading code using STMicro built-in USB Device Firmware Upgrade (DFU)
+ loader or via some JTAG emulator. You can specify the DFU bootloader by
+ adding the following line:
+
+ CONFIG_STM32_DFU=y
+
+ to your .config file. Most of the configurations in this directory are set
+ up to use the DFU loader.
+
+ If CONFIG_STM32_DFU is defined, the code will not be positioned at the beginning
+ of FLASH (0x08000000) but will be offset to 0x08003000. This offset is needed
+ to make space for the DFU loader and 0x08003000 is where the DFU loader expects
+ to find new applications at boot time. If you need to change that origin for some
+ other bootloader, you will need to edit the file(s) ld.script.dfu for each
+ configuration.
+
+ The DFU SE PC-based software is available from the STMicro website,
+ http://www.st.com. General usage instructions:
+
+ 1. Convert the NuttX Intel Hex file (nuttx.ihx) into a special DFU
+ file (nuttx.dfu)... see below for details.
+ 2. Connect the STM3210E-EVAL board to your computer using a USB
+ cable.
+ 3. Start the DFU loader on the STM3210E-EVAL board. You do this by
+ resetting the board while holding the "Key" button. Windows should
+ recognize that the DFU loader has been installed.
+ 3. Run the DFU SE program to load nuttx.dfu into FLASH.
+
+ What if the DFU loader is not in FLASH? The loader code is available
+ inside of the Demo dirctory of the USBLib ZIP file that can be downloaded
+ from the STMicro Website. You can build it using RIDE (or other toolchains);
+ you will need a JTAG emulator to burn it into FLASH the first time.
+
+ In order to use STMicro's built-in DFU loader, you will have to get
+ the NuttX binary into a special format with a .dfu extension. The
+ DFU SE PC_based software installation includes a file "DFU File Manager"
+ conversion program that a file in Intel Hex format to the special DFU
+ format. When you successfully build NuttX, you will find a file called
+ nutt.ihx in the top-level directory. That is the file that you should
+ provide to the DFU File Manager. You will need to rename it to nuttx.hex
+ in order to find it with the DFU File Manager. You will end up with
+ a file called nuttx.dfu that you can use with the STMicro DFU SE program.
+
+ Enabling JTAG
+ -------------
+ If you are not using the DFU, then you will probably also need to enable
+ JTAG support. By default, all JTAG support is disabled but there NuttX
+ configuration options to enable JTAG in various different ways.
+
+ These configurations effect the setting of the SWJ_CFG[2:0] bits in the AFIO
+ MAPR register. These bits are used to configure the SWJ and trace alternate function I/Os. The SWJ (SerialWire JTAG) supports JTAG or SWD access to the
+ Cortex debug port. The default state in this port is for all JTAG support
+ to be disable.
+
+ CONFIG_STM32_JTAG_FULL_ENABLE - sets SWJ_CFG[2:0] to 000 which enables full
+ SWJ (JTAG-DP + SW-DP)
+
+ CONFIG_STM32_JTAG_NOJNTRST_ENABLE - sets SWJ_CFG[2:0] to 001 which enable
+ full SWJ (JTAG-DP + SW-DP) but without JNTRST.
+
+ CONFIG_STM32_JTAG_SW_ENABLE - sets SWJ_CFG[2:0] to 010 which would set JTAG-DP
+ disabled and SW-DP enabled
+
+ The default setting (none of the above defined) is SWJ_CFG[2:0] set to 100
+ which disable JTAG-DP and SW-DP.
+
+OpenOCD
+=======
+
+I have also used OpenOCD with the STM3210E-EVAL. In this case, I used
+the Olimex USB ARM OCD. See the script in configs/stm3210e-eval/tools/oocd.sh
+for more information. Using the script:
+
+1) Start the OpenOCD GDB server
+
+ cd <nuttx-build-directory>
+ configs/stm3210e-eval/tools/oocd.sh $PWD
+
+2) Load Nuttx
+
+ cd <nuttx-built-directory>
+ arm-none-eabi-gdb nuttx
+ gdb> target remote localhost:3333
+ gdb> mon reset
+ gdb> mon halt
+ gdb> load nuttx
+
+3) Running NuttX
+
+ gdb> mon reset
+ gdb> c
+
+LEDs
+====
+
+The STM3210E-EVAL board has four LEDs labeled LD1, LD2, LD3 and LD4 on the
+board.. These LEDs are not used by the board port unless CONFIG_ARCH_LEDS is
+defined. In that case, the usage by the board port is defined in
+include/board.h and src/up_leds.c. The LEDs are used to encode OS-related
+events as follows:
+
+ SYMBOL Meaning LED1* LED2 LED3 LED4
+ ---------------- ----------------------- ----- ----- ----- -----
+ LED_STARTED NuttX has been started ON OFF OFF OFF
+ LED_HEAPALLOCATE Heap has been allocated OFF ON OFF OFF
+ LED_IRQSENABLED Interrupts enabled ON ON OFF OFF
+ LED_STACKCREATED Idle stack created OFF OFF ON OFF
+ LED_INIRQ In an interrupt** ON N/C N/C OFF
+ LED_SIGNAL In a signal handler*** N/C ON N/C OFF
+ LED_ASSERTION An assertion failed ON ON N/C OFF
+ LED_PANIC The system has crashed N/C N/C N/C ON
+ LED_IDLE STM32 is is sleep mode (Optional, not used)
+
+ * If LED1, LED2, LED3 are statically on, then NuttX probably failed to boot
+ and these LEDs will give you some indication of where the failure was
+ ** The normal state is LED3 ON and LED1 faintly glowing. This faint glow
+ is because of timer interupts that result in the LED being illuminated
+ on a small proportion of the time.
+*** LED2 may also flicker normally if signals are processed.
+
+Temperature Sensor
+==================
+
+Support for the on-board LM-75 temperature sensor is available. This supported
+has been verified, but has not been included in any of the available the
+configurations. To set up the temperature sensor, add the following to the
+NuttX configuration file
+
+ CONFIG_I2C=y
+ CONFIG_I2C_LM75=y
+
+Then you can implement logic like the following to use the temperature sensor:
+
+ #include <nuttx/sensors/lm75.h>
+ #include <arch/board/board.h>
+
+ ret = stm32_lm75initialize("/dev/temp"); /* Register the temperature sensor */
+ fd = open("/dev/temp", O_RDONLY); /* Open the temperature sensor device */
+ ret = ioctl(fd, SNIOC_FAHRENHEIT, 0); /* Select Fahrenheit */
+ bytesread = read(fd, buffer, 8*sizeof(b16_t)); /* Read temperature samples */
+
+More complex temperature sensor operations are also available. See the IOCTAL
+commands enumerated in include/nuttx/sensors/lm75.h. Also read the descriptions
+of the stm32_lm75initialize() and stm32_lm75attach() interfaces in the
+arch/board/board.h file (sames as configs/stm3210e-eval/include/board.h).
+
+RTC
+===
+
+ The STM32 RTC may configured using the following settings.
+
+ CONFIG_RTC - Enables general support for a hardware RTC. Specific
+ architectures may require other specific settings.
+ CONFIG_RTC_HIRES - The typical RTC keeps time to resolution of 1
+ second, usually supporting a 32-bit time_t value. In this case,
+ the RTC is used to &quot;seed&quot; the normal NuttX timer and the
+ NuttX timer provides for higher resoution time. If CONFIG_RTC_HIRES
+ is enabled in the NuttX configuration, then the RTC provides higher
+ resolution time and completely replaces the system timer for purpose of
+ date and time.
+ CONFIG_RTC_FREQUENCY - If CONFIG_RTC_HIRES is defined, then the
+ frequency of the high resolution RTC must be provided. If CONFIG_RTC_HIRES
+ is not defined, CONFIG_RTC_FREQUENCY is assumed to be one.
+ CONFIG_RTC_ALARM - Enable if the RTC hardware supports setting of an alarm.
+ A callback function will be executed when the alarm goes off
+
+ In hi-res mode, the STM32 RTC operates only at 16384Hz. Overflow interrupts
+ are handled when the 32-bit RTC counter overflows every 3 days and 43 minutes.
+ A BKP register is incremented on each overflow interrupt creating, effectively,
+ a 48-bit RTC counter.
+
+ In the lo-res mode, the RTC operates at 1Hz. Overflow interrupts are not handled
+ (because the next overflow is not expected until the year 2106.
+
+ WARNING: Overflow interrupts are lost whenever the STM32 is powered down. The
+ overflow interrupt may be lost even if the STM32 is powered down only momentarily.
+ Therefore hi-res solution is only useful in systems where the power is always on.
+
+STM3210E-EVAL-specific Configuration Options
+============================================
+
+ CONFIG_ARCH - Identifies the arch/ subdirectory. This should
+ be set to:
+
+ CONFIG_ARCH=arm
+
+ CONFIG_ARCH_family - For use in C code:
+
+ CONFIG_ARCH_ARM=y
+
+ CONFIG_ARCH_architecture - For use in C code:
+
+ CONFIG_ARCH_CORTEXM3=y
+
+ CONFIG_ARCH_CHIP - Identifies the arch/*/chip subdirectory
+
+ CONFIG_ARCH_CHIP=stm32
+
+ CONFIG_ARCH_CHIP_name - For use in C code to identify the exact
+ chip:
+
+ CONFIG_ARCH_CHIP_STM32F103ZET6
+
+ CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG - Enables special STM32 clock
+ configuration features.
+
+ CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG=n
+
+ CONFIG_ARCH_BOARD - Identifies the configs subdirectory and
+ hence, the board that supports the particular chip or SoC.
+
+ CONFIG_ARCH_BOARD=stm3210e_eval (for the STM3210E-EVAL development board)
+
+ CONFIG_ARCH_BOARD_name - For use in C code
+
+ CONFIG_ARCH_BOARD_STM3210E_EVAL=y
+
+ CONFIG_ARCH_LOOPSPERMSEC - Must be calibrated for correct operation
+ of delay loops
+
+ CONFIG_ENDIAN_BIG - define if big endian (default is little
+ endian)
+
+ CONFIG_DRAM_SIZE - Describes the installed DRAM (SRAM in this case):
+
+ CONFIG_DRAM_SIZE=0x00010000 (64Kb)
+
+ CONFIG_DRAM_START - The start address of installed DRAM
+
+ CONFIG_DRAM_START=0x20000000
+
+ CONFIG_DRAM_END - Last address+1 of installed RAM
+
+ CONFIG_DRAM_END=(CONFIG_DRAM_START+CONFIG_DRAM_SIZE)
+
+ CONFIG_ARCH_IRQPRIO - The STM32F103Z supports interrupt prioritization
+
+ CONFIG_ARCH_IRQPRIO=y
+
+ CONFIG_ARCH_LEDS - Use LEDs to show state. Unique to boards that
+ have LEDs
+
+ CONFIG_ARCH_INTERRUPTSTACK - This architecture supports an interrupt
+ stack. If defined, this symbol is the size of the interrupt
+ stack in bytes. If not defined, the user task stacks will be
+ used during interrupt handling.
+
+ CONFIG_ARCH_STACKDUMP - Do stack dumps after assertions
+
+ CONFIG_ARCH_LEDS - Use LEDs to show state. Unique to board architecture.
+
+ CONFIG_ARCH_CALIBRATION - Enables some build in instrumentation that
+ cause a 100 second delay during boot-up. This 100 second delay
+ serves no purpose other than it allows you to calibratre
+ CONFIG_ARCH_LOOPSPERMSEC. You simply use a stop watch to measure
+ the 100 second delay then adjust CONFIG_ARCH_LOOPSPERMSEC until
+ the delay actually is 100 seconds.
+
+ Individual subsystems can be enabled:
+ AHB
+ ---
+ CONFIG_STM32_DMA1
+ CONFIG_STM32_DMA2
+ CONFIG_STM32_CRC
+ CONFIG_STM32_FSMC
+ CONFIG_STM32_SDIO
+
+ APB1
+ ----
+ CONFIG_STM32_TIM2
+ CONFIG_STM32_TIM3
+ CONFIG_STM32_TIM4
+ CONFIG_STM32_TIM5
+ CONFIG_STM32_TIM6
+ CONFIG_STM32_TIM7
+ CONFIG_STM32_WWDG
+ CONFIG_STM32_SPI2
+ CONFIG_STM32_SPI4
+ CONFIG_STM32_USART2
+ CONFIG_STM32_USART3
+ CONFIG_STM32_UART4
+ CONFIG_STM32_UART5
+ CONFIG_STM32_I2C1
+ CONFIG_STM32_I2C2
+ CONFIG_STM32_USB
+ CONFIG_STM32_CAN
+ CONFIG_STM32_BKP
+ CONFIG_STM32_PWR
+ CONFIG_STM32_DAC1
+ CONFIG_STM32_DAC2
+ CONFIG_STM32_USB
+
+ APB2
+ ----
+ CONFIG_STM32_ADC1
+ CONFIG_STM32_ADC2
+ CONFIG_STM32_TIM1
+ CONFIG_STM32_SPI1
+ CONFIG_STM32_TIM8
+ CONFIG_STM32_USART1
+ CONFIG_STM32_ADC3
+
+ Timer and I2C devices may need to the following to force power to be applied
+ unconditionally at power up. (Otherwise, the device is powered when it is
+ initialized).
+
+ CONFIG_STM32_FORCEPOWER
+
+ Timer devices may be used for different purposes. One special purpose is
+ to generate modulated outputs for such things as motor control. If CONFIG_STM32_TIMn
+ is defined (as above) then the following may also be defined to indicate that
+ the timer is intended to be used for pulsed output modulation, ADC conversion,
+ or DAC conversion. Note that ADC/DAC require two definition: Not only do you have
+ to assign the timer (n) for used by the ADC or DAC, but then you also have to
+ configure which ADC or DAC (m) it is assigned to.
+
+ CONFIG_STM32_TIMn_PWM Reserve timer n for use by PWM, n=1,..,8
+ CONFIG_STM32_TIMn_ADC Reserve timer n for use by ADC, n=1,..,8
+ CONFIG_STM32_TIMn_ADCm Reserve timer n to trigger ADCm, n=1,..,8, m=1,..,3
+ CONFIG_STM32_TIMn_DAC Reserve timer n for use by DAC, n=1,..,8
+ CONFIG_STM32_TIMn_DACm Reserve timer n to trigger DACm, n=1,..,8, m=1,..,2
+
+ For each timer that is enabled for PWM usage, we need the following additional
+ configuration settings:
+
+ CONFIG_STM32_TIMx_CHANNEL - Specifies the timer output channel {1,..,4}
+
+ NOTE: The STM32 timers are each capable of generating different signals on
+ each of the four channels with different duty cycles. That capability is
+ not supported by this driver: Only one output channel per timer.
+
+ Alternate pin mappings (should not be used with the STM3210E-EVAL board):
+
+ CONFIG_STM32_TIM1_FULL_REMAP
+ CONFIG_STM32_TIM1_PARTIAL_REMAP
+ CONFIG_STM32_TIM2_FULL_REMAP
+ CONFIG_STM32_TIM2_PARTIAL_REMAP_1
+ CONFIG_STM32_TIM2_PARTIAL_REMAP_2
+ CONFIG_STM32_TIM3_FULL_REMAP
+ CONFIG_STM32_TIM3_PARTIAL_REMAP
+ CONFIG_STM32_TIM4_REMAP
+ CONFIG_STM32_USART1_REMAP
+ CONFIG_STM32_USART2_REMAP
+ CONFIG_STM32_USART3_FULL_REMAP
+ CONFIG_STM32_USART3_PARTIAL_REMAP
+ CONFIG_STM32_SPI1_REMAP
+ CONFIG_STM32_SPI3_REMAP
+ CONFIG_STM32_I2C1_REMAP
+ CONFIG_STM32_CAN1_FULL_REMAP
+ CONFIG_STM32_CAN1_PARTIAL_REMAP
+ CONFIG_STM32_CAN2_REMAP
+
+ JTAG Enable settings (by default JTAG-DP and SW-DP are disabled):
+ CONFIG_STM32_JTAG_FULL_ENABLE - Enables full SWJ (JTAG-DP + SW-DP)
+ CONFIG_STM32_JTAG_NOJNTRST_ENABLE - Enables full SWJ (JTAG-DP + SW-DP)
+ but without JNTRST.
+ CONFIG_STM32_JTAG_SW_ENABLE - Set JTAG-DP disabled and SW-DP enabled
+
+ STM32F103Z specific device driver settings
+
+ CONFIG_U[S]ARTn_SERIAL_CONSOLE - selects the USARTn (n=1,2,3) or UART
+ m (m=4,5) for the console and ttys0 (default is the USART1).
+ CONFIG_U[S]ARTn_RXBUFSIZE - Characters are buffered as received.
+ This specific the size of the receive buffer
+ CONFIG_U[S]ARTn_TXBUFSIZE - Characters are buffered before
+ being sent. This specific the size of the transmit buffer
+ CONFIG_U[S]ARTn_BAUD - The configure BAUD of the UART. Must be
+ CONFIG_U[S]ARTn_BITS - The number of bits. Must be either 7 or 8.
+ CONFIG_U[S]ARTn_PARTIY - 0=no parity, 1=odd parity, 2=even parity
+ CONFIG_U[S]ARTn_2STOP - Two stop bits
+
+ CONFIG_STM32_SPI_INTERRUPTS - Select to enable interrupt driven SPI
+ support. Non-interrupt-driven, poll-waiting is recommended if the
+ interrupt rate would be to high in the interrupt driven case.
+ CONFIG_STM32_SPI_DMA - Use DMA to improve SPI transfer performance.
+ Cannot be used with CONFIG_STM32_SPI_INTERRUPT.
+
+ CONFIG_SDIO_DMA - Support DMA data transfers. Requires CONFIG_STM32_SDIO
+ and CONFIG_STM32_DMA2.
+ CONFIG_SDIO_PRI - Select SDIO interrupt prority. Default: 128
+ CONFIG_SDIO_DMAPRIO - Select SDIO DMA interrupt priority.
+ Default: Medium
+ CONFIG_SDIO_WIDTH_D1_ONLY - Select 1-bit transfer mode. Default:
+ 4-bit transfer mode.
+
+ STM3210E-EVAL CAN Configuration
+
+ CONFIG_CAN - Enables CAN support (one or both of CONFIG_STM32_CAN1 or
+ CONFIG_STM32_CAN2 must also be defined)
+ CONFIG_CAN_EXTID - Enables support for the 29-bit extended ID. Default
+ Standard 11-bit IDs.
+ CONFIG_CAN_FIFOSIZE - The size of the circular buffer of CAN messages.
+ Default: 8
+ CONFIG_CAN_NPENDINGRTR - The size of the list of pending RTR requests.
+ Default: 4
+ CONFIG_CAN_LOOPBACK - A CAN driver may or may not support a loopback
+ mode for testing. The STM32 CAN driver does support loopback mode.
+ CONFIG_CAN1_BAUD - CAN1 BAUD rate. Required if CONFIG_STM32_CAN1 is defined.
+ CONFIG_CAN2_BAUD - CAN1 BAUD rate. Required if CONFIG_STM32_CAN2 is defined.
+ CONFIG_CAN_TSEG1 - The number of CAN time quanta in segment 1. Default: 6
+ CONFIG_CAN_TSEG2 - the number of CAN time quanta in segment 2. Default: 7
+ CONFIG_CAN_REGDEBUG - If CONFIG_DEBUG is set, this will generate an
+ dump of all CAN registers.
+
+ STM3210E-EVAL LCD Hardware Configuration
+
+ CONFIG_LCD_LANDSCAPE - Define for 320x240 display "landscape"
+ support. Default is this 320x240 "landscape" orientation
+ (this setting is informative only... not used).
+ CONFIG_LCD_PORTRAIT - Define for 240x320 display "portrait"
+ orientation support. In this orientation, the STM3210E-EVAL's
+ LCD ribbon cable is at the bottom of the display. Default is
+ 320x240 "landscape" orientation.
+ CONFIG_LCD_RPORTRAIT - Define for 240x320 display "reverse
+ portrait" orientation support. In this orientation, the
+ STM3210E-EVAL's LCD ribbon cable is at the top of the display.
+ Default is 320x240 "landscape" orientation.
+ CONFIG_LCD_BACKLIGHT - Define to support a backlight.
+ CONFIG_LCD_PWM - If CONFIG_STM32_TIM1 is also defined, then an
+ adjustable backlight will be provided using timer 1 to generate
+ various pulse widthes. The granularity of the settings is
+ determined by CONFIG_LCD_MAXPOWER. If CONFIG_LCD_PWM (or
+ CONFIG_STM32_TIM1) is not defined, then a simple on/off backlight
+ is provided.
+ CONFIG_LCD_RDSHIFT - When reading 16-bit gram data, there appears
+ to be a shift in the returned data. This value fixes the offset.
+ Default 5.
+
+ The LCD driver dynamically selects the LCD based on the reported LCD
+ ID value. However, code size can be reduced by suppressing support for
+ individual LCDs using:
+
+ CONFIG_STM32_AM240320_DISABLE
+ CONFIG_STM32_SPFD5408B_DISABLE
+ CONFIG_STM32_R61580_DISABLE
+
+Configurations
+==============
+
+Each STM3210E-EVAL configuration is maintained in a sudirectory and
+can be selected as follow:
+
+ cd tools
+ ./configure.sh stm3210e-eval/<subdir>
+ cd -
+ . ./setenv.sh
+
+Where <subdir> is one of the following:
+
+ buttons:
+ --------
+
+ Uses apps/examples/buttons to exercise STM3210E-EVAL buttons and
+ button interrupts.
+
+ CONFIG_STM32_CODESOURCERYW=y : CodeSourcery under Windows
+
+ composite
+ ---------
+
+ This configuration exercises a composite USB interface consisting
+ of a CDC/ACM device and a USB mass storage device. This configuration
+ uses apps/examples/composite.
+
+ nsh and nsh2:
+ ------------
+ Configure the NuttShell (nsh) located at examples/nsh.
+
+ Differences between the two NSH configurations:
+
+ =========== ======================= ================================
+ nsh nsh2
+ =========== ======================= ================================
+ Toolchain: NuttX buildroot for Codesourcery for Windows (1)
+ Linux or Cygwin (1,2)
+ ----------- ----------------------- --------------------------------
+ Loader: DfuSe DfuSe
+ ----------- ----------------------- --------------------------------
+ Serial Debug output: USART1 Debug output: USART1
+ Console: NSH output: USART1 NSH output: USART1 (3)
+ ----------- ----------------------- --------------------------------
+ microSD Yes Yes
+ Support
+ ----------- ----------------------- --------------------------------
+ FAT FS CONFIG_FAT_LCNAME=y CONFIG_FAT_LCNAME=y
+ Config CONFIG_FAT_LFN=n CONFIG_FAT_LFN=y (4)
+ ----------- ----------------------- --------------------------------
+ Support for No Yes
+ Built-in
+ Apps
+ ----------- ----------------------- --------------------------------
+ Built-in None apps/examples/nx
+ Apps apps/examples/nxhello
+ apps/examples/usbstorage (5)
+ =========== ======================= ================================
+
+ (1) You will probably need to modify nsh/setenv.sh or nsh2/setenv.sh
+ to set up the correct PATH variable for whichever toolchain you
+ may use.
+ (2) Since DfuSe is assumed, this configuration may only work under
+ Cygwin without modification.
+ (3) When any other device other than /dev/console is used for a user
+ interface, (1) linefeeds (\n) will not be expanded to carriage return
+ / linefeeds \r\n). You will need to configure your terminal program
+ to account for this. And (2) input is not automatically echoed so
+ you will have to turn local echo on.
+ (4) Microsoft holds several patents related to the design of
+ long file names in the FAT file system. Please refer to the
+ details in the top-level COPYING file. Please do not use FAT
+ long file name unless you are familiar with these patent issues.
+ (5) When built as an NSH add-on command (CONFIG_EXAMPLES_USBMSC_BUILTIN=y),
+ Caution should be used to assure that the SD drive is not in use when
+ the USB storage device is configured. Specifically, the SD driver
+ should be unmounted like:
+
+ nsh> mount -t vfat /dev/mmcsd0 /mnt/sdcard # Card is mounted in NSH
+ ...
+ nsh> umount /mnd/sdcard # Unmount before connecting USB!!!
+ nsh> msconn # Connect the USB storage device
+ ...
+ nsh> msdis # Disconnect USB storate device
+ nsh> mount -t vfat /dev/mmcsd0 /mnt/sdcard # Restore the mount
+
+ Failure to do this could result in corruption of the SD card format.
+
+ nx:
+ ---
+ An example using the NuttX graphics system (NX). This example
+ focuses on general window controls, movement, mouse and keyboard
+ input.
+
+ CONFIG_STM32_CODESOURCERYW=y : CodeSourcery under Windows
+ CONFIG_LCD_RPORTRAIT=y : 240x320 reverse portrait
+
+ nxlines:
+ ------
+ Another example using the NuttX graphics system (NX). This
+ example focuses on placing lines on the background in various
+ orientations.
+
+ CONFIG_STM32_CODESOURCERYW=y : CodeSourcery under Windows
+ CONFIG_LCD_RPORTRAIT=y : 240x320 reverse portrait
+
+ nxtext:
+ ------
+ Another example using the NuttX graphics system (NX). This
+ example focuses on placing text on the background while pop-up
+ windows occur. Text should continue to update normally with
+ or without the popup windows present.
+
+ CONFIG_STM32_BUILDROOT=y : NuttX buildroot under Linux or Cygwin
+ CONFIG_LCD_RPORTRAIT=y : 240x320 reverse portrait
+
+ NOTE: When I tried building this example with the CodeSourcery
+ tools, I got a hardfault inside of its libgcc. I haven't
+ retested since then, but beware if you choose to change the
+ toolchain.
+
+ ostest:
+ ------
+ This configuration directory, performs a simple OS test using
+ examples/ostest. By default, this project assumes that you are
+ using the DFU bootloader.
+
+ CONFIG_STM32_BUILDROOT=y : NuttX buildroot under Linux or Cygwin
+
+ RIDE
+ ----
+ This configuration builds a trivial bring-up binary. It is
+ useful only because it words with the RIDE7 IDE and R-Link debugger.
+
+ CONFIG_STM32_RAISONANCE=y : Raisonance RIDE7 under Windows
+
+ usbserial:
+ ---------
+ This configuration directory exercises the USB serial class
+ driver at examples/usbserial. See examples/README.txt for
+ more information.
+
+ CONFIG_STM32_BUILDROOT=y : NuttX buildroot under Linux or Cygwin
+
+ USB debug output can be enabled as by changing the following
+ settings in the configuration file:
+
+ -CONFIG_DEBUG=n
+ -CONFIG_DEBUG_VERBOSE=n
+ -CONFIG_DEBUG_USB=n
+ +CONFIG_DEBUG=y
+ +CONFIG_DEBUG_VERBOSE=y
+ +CONFIG_DEBUG_USB=y
+
+ -CONFIG_EXAMPLES_USBSERIAL_TRACEINIT=n
+ -CONFIG_EXAMPLES_USBSERIAL_TRACECLASS=n
+ -CONFIG_EXAMPLES_USBSERIAL_TRACETRANSFERS=n
+ -CONFIG_EXAMPLES_USBSERIAL_TRACECONTROLLER=n
+ -CONFIG_EXAMPLES_USBSERIAL_TRACEINTERRUPTS=n
+ +CONFIG_EXAMPLES_USBSERIAL_TRACEINIT=y
+ +CONFIG_EXAMPLES_USBSERIAL_TRACECLASS=y
+ +CONFIG_EXAMPLES_USBSERIAL_TRACETRANSFERS=y
+ +CONFIG_EXAMPLES_USBSERIAL_TRACECONTROLLER=y
+ +CONFIG_EXAMPLES_USBSERIAL_TRACEINTERRUPTS=y
+
+ By default, the usbserial example uses the Prolific PL2303
+ serial/USB converter emulation. The example can be modified
+ to use the CDC/ACM serial class by making the following changes
+ to the configuration file:
+
+ -CONFIG_PL2303=y
+ +CONFIG_PL2303=n
+
+ -CONFIG_CDCACM=n
+ +CONFIG_CDCACM=y
+
+ The example can also be converted to use the alternative
+ USB serial example at apps/examples/usbterm by changing the
+ following:
+
+ -CONFIGURED_APPS += examples/usbserial
+ +CONFIGURED_APPS += examples/usbterm
+
+ In either the original appconfig file (before configuring)
+ or in the final apps/.config file (after configuring).
+
+ usbstorage:
+ ----------
+ This configuration directory exercises the USB mass storage
+ class driver at examples/usbstorage. See examples/README.txt for
+ more information.
+
+ CONFIG_STM32_BUILDROOT=y : NuttX buildroot under Linux or Cygwin
+
diff --git a/nuttx/configs/px4iov2/common/Make.defs b/nuttx/configs/px4iov2/common/Make.defs
new file mode 100644
index 000000000..7f782b5b2
--- /dev/null
+++ b/nuttx/configs/px4iov2/common/Make.defs
@@ -0,0 +1,175 @@
+############################################################################
+# configs/px4fmu/common/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.
+#
+############################################################################
+
+#
+# Generic Make.defs for the PX4FMU
+# Do not specify/use this file directly - it is included by config-specific
+# Make.defs in the per-config directories.
+#
+
+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 = -O3
+ARCHCPUFLAGS = -mcpu=cortex-m3 \
+ -mthumb \
+ -march=armv7-m
+
+# enable precise stack overflow tracking
+#INSTRUMENTATIONDEFINES = -finstrument-functions \
+# -ffixed-r10
+
+# 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)/common/$(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)/common/$(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)/common/$(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 \
+ -Wextra \
+ -Wdouble-promotion \
+ -Wshadow \
+ -Wfloat-equal \
+ -Wframe-larger-than=1024 \
+ -Wpointer-arith \
+ -Wlogical-op \
+ -Wmissing-declarations \
+ -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 \
+ -Wmissing-prototypes \
+ -Wnested-externs \
+ -Wunsuffixed-float-constants
+ARCHWARNINGSXX = $(ARCHWARNINGS)
+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/px4iov2/common/ld.script b/nuttx/configs/px4iov2/common/ld.script
new file mode 100755
index 000000000..69c2f9cb2
--- /dev/null
+++ b/nuttx/configs/px4iov2/common/ld.script
@@ -0,0 +1,129 @@
+/****************************************************************************
+ * configs/stm3210e-eval/nsh/ld.script
+ *
+ * Copyright (C) 2009, 2011 Gregory Nutt. All rights reserved.
+ * Author: Gregory Nutt <spudmonkey@racsa.co.cr>
+ *
+ * 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 STM32F100C8 has 64Kb of FLASH beginning at address 0x0800:0000 and
+ * 8Kb of SRAM beginning at address 0x2000: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.
+ */
+
+MEMORY
+{
+ flash (rx) : ORIGIN = 0x08001000, LENGTH = 60K
+ sram (rwx) : ORIGIN = 0x20000000, LENGTH = 8K
+}
+
+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.*)
+ *(.glue_7)
+ *(.glue_7t)
+ *(.got)
+ *(.gcc_except_table)
+ *(.gnu.linkonce.r.*)
+ _etext = ABSOLUTE(.);
+ } > flash
+
+ /*
+ * Init functions (static constructors and the like)
+ */
+ .init_section : {
+ _sinit = ABSOLUTE(.);
+ KEEP(*(.init_array .init_array.*))
+ _einit = ABSOLUTE(.);
+ } > flash
+
+ .ARM.extab : {
+ *(.ARM.extab*)
+ } > flash
+
+ __exidx_start = ABSOLUTE(.);
+ .ARM.exidx : {
+ *(.ARM.exidx*)
+ } > flash
+ __exidx_end = ABSOLUTE(.);
+
+ _eronly = ABSOLUTE(.);
+
+ /* The STM32F100CB has 8Kb of SRAM beginning at the following address */
+
+ .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/px4iov2/common/setenv.sh b/nuttx/configs/px4iov2/common/setenv.sh
new file mode 100755
index 000000000..ff9a4bf8a
--- /dev/null
+++ b/nuttx/configs/px4iov2/common/setenv.sh
@@ -0,0 +1,47 @@
+#!/bin/bash
+# configs/stm3210e-eval/dfu/setenv.sh
+#
+# Copyright (C) 2009 Gregory Nutt. All rights reserved.
+# Author: Gregory Nutt <gnutt@nuttx.org>
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+# notice, this list of conditions and the following disclaimer in
+# the documentation and/or other materials provided with the
+# distribution.
+# 3. Neither the name NuttX nor the names of its contributors may be
+# used to endorse or promote products derived from this software
+# without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+
+if [ "$(basename $0)" = "setenv.sh" ] ; then
+ echo "You must source this script, not run it!" 1>&2
+ exit 1
+fi
+
+if [ -z "${PATH_ORIG}" ]; then export PATH_ORIG="${PATH}"; fi
+
+WD=`pwd`
+export RIDE_BIN="/cygdrive/c/Program Files/Raisonance/Ride/arm-gcc/bin"
+export BUILDROOT_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin"
+export PATH="${BUILDROOT_BIN}:${RIDE_BIN}:/sbin:/usr/sbin:${PATH_ORIG}"
+
+echo "PATH : ${PATH}"
diff --git a/nuttx/configs/px4iov2/include/README.txt b/nuttx/configs/px4iov2/include/README.txt
new file mode 100755
index 000000000..2264a80aa
--- /dev/null
+++ b/nuttx/configs/px4iov2/include/README.txt
@@ -0,0 +1 @@
+This directory contains header files unique to the PX4IO board.
diff --git a/nuttx/configs/px4iov2/include/board.h b/nuttx/configs/px4iov2/include/board.h
new file mode 100755
index 000000000..21aacda87
--- /dev/null
+++ b/nuttx/configs/px4iov2/include/board.h
@@ -0,0 +1,184 @@
+/************************************************************************************
+ * configs/px4io/include/board.h
+ * include/arch/board/board.h
+ *
+ * Copyright (C) 2009 Gregory Nutt. All rights reserved.
+ * Author: Gregory Nutt <gnutt@nuttx.org>
+ * 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 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 __ARCH_BOARD_BOARD_H
+#define __ARCH_BOARD_BOARD_H
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <nuttx/config.h>
+#ifndef __ASSEMBLY__
+# include <stdint.h>
+# include <stdbool.h>
+#endif
+#include <stm32_rcc.h>
+#include <stm32_sdio.h>
+#include <stm32_internal.h>
+
+/************************************************************************************
+ * Definitions
+ ************************************************************************************/
+
+/* Clocking *************************************************************************/
+
+/* On-board crystal frequency is 24MHz (HSE) */
+
+#define STM32_BOARD_XTAL 24000000ul
+
+/* Use the HSE output as the system clock */
+
+#define STM32_SYSCLK_SW RCC_CFGR_SW_HSE
+#define STM32_SYSCLK_SWS RCC_CFGR_SWS_HSE
+#define STM32_SYSCLK_FREQUENCY STM32_BOARD_XTAL
+
+/* AHB clock (HCLK) is SYSCLK (24MHz) */
+
+#define STM32_RCC_CFGR_HPRE RCC_CFGR_HPRE_SYSCLK
+#define STM32_HCLK_FREQUENCY STM32_SYSCLK_FREQUENCY
+#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* same as above, to satisfy compiler */
+
+/* APB2 clock (PCLK2) is HCLK (24MHz) */
+
+#define STM32_RCC_CFGR_PPRE2 RCC_CFGR_PPRE2_HCLK
+#define STM32_PCLK2_FREQUENCY STM32_HCLK_FREQUENCY
+#define STM32_APB2_CLKIN (STM32_PCLK2_FREQUENCY) /* Timers 2-4 */
+
+/* APB2 timer 1 will receive PCLK2. */
+
+#define STM32_APB2_TIM1_CLKIN (STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM8_CLKIN (STM32_PCLK2_FREQUENCY)
+
+/* APB1 clock (PCLK1) is HCLK (24MHz) */
+
+#define STM32_RCC_CFGR_PPRE1 RCC_CFGR_PPRE1_HCLK
+#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY)
+
+/* All timers run off PCLK */
+
+#define STM32_APB1_TIM1_CLKIN (STM32_PCLK2_FREQUENCY)
+#define STM32_APB1_TIM2_CLKIN (STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM3_CLKIN (STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM4_CLKIN (STM32_PCLK1_FREQUENCY)
+
+/*
+ * Some of the USART pins are not available; override the GPIO
+ * definitions with an invalid pin configuration.
+ */
+#undef GPIO_USART2_CTS
+#define GPIO_USART2_CTS 0xffffffff
+#undef GPIO_USART2_RTS
+#define GPIO_USART2_RTS 0xffffffff
+#undef GPIO_USART2_CK
+#define GPIO_USART2_CK 0xffffffff
+#undef GPIO_USART3_TX
+#define GPIO_USART3_TX 0xffffffff
+#undef GPIO_USART3_CK
+#define GPIO_USART3_CK 0xffffffff
+#undef GPIO_USART3_CTS
+#define GPIO_USART3_CTS 0xffffffff
+#undef GPIO_USART3_RTS
+#define GPIO_USART3_RTS 0xffffffff
+
+/*
+ * High-resolution timer
+ */
+#ifdef CONFIG_HRT_TIMER
+# define HRT_TIMER 1 /* use timer1 for the HRT */
+# define HRT_TIMER_CHANNEL 2 /* use capture/compare channel 2 */
+#endif
+
+/*
+ * PPM
+ *
+ * PPM input is handled by the HRT timer.
+ *
+ * Pin is PA8, timer 1, channel 1
+ */
+#if defined(CONFIG_HRT_TIMER) && defined (CONFIG_HRT_PPM)
+# define HRT_PPM_CHANNEL 1 /* use capture/compare channel 1 */
+# define GPIO_PPM_IN GPIO_TIM1_CH1IN
+#endif
+
+/* LED definitions ******************************************************************/
+/* PX4 has two LEDs that we will encode as: */
+
+#define LED_STARTED 0 /* LED? */
+#define LED_HEAPALLOCATE 1 /* LED? */
+#define LED_IRQSENABLED 2 /* LED? + LED? */
+#define LED_STACKCREATED 3 /* LED? */
+#define LED_INIRQ 4 /* LED? + LED? */
+#define LED_SIGNAL 5 /* LED? + LED? */
+#define LED_ASSERTION 6 /* LED? + LED? + LED? */
+#define LED_PANIC 7 /* N/C + N/C + N/C + LED? */
+
+/************************************************************************************
+ * 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);
+
+#if defined(__cplusplus)
+}
+#endif
+
+#endif /* __ASSEMBLY__ */
+#endif /* __ARCH_BOARD_BOARD_H */
diff --git a/nuttx/configs/px4iov2/io/Make.defs b/nuttx/configs/px4iov2/io/Make.defs
new file mode 100644
index 000000000..369772d03
--- /dev/null
+++ b/nuttx/configs/px4iov2/io/Make.defs
@@ -0,0 +1,3 @@
+include ${TOPDIR}/.config
+
+include $(TOPDIR)/configs/px4io/common/Make.defs
diff --git a/nuttx/configs/px4iov2/io/appconfig b/nuttx/configs/px4iov2/io/appconfig
new file mode 100644
index 000000000..48a41bcdb
--- /dev/null
+++ b/nuttx/configs/px4iov2/io/appconfig
@@ -0,0 +1,32 @@
+############################################################################
+#
+# 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.
+#
+############################################################################
diff --git a/nuttx/configs/px4iov2/io/defconfig b/nuttx/configs/px4iov2/io/defconfig
new file mode 100755
index 000000000..1eaf4f2d1
--- /dev/null
+++ b/nuttx/configs/px4iov2/io/defconfig
@@ -0,0 +1,548 @@
+############################################################################
+# configs/px4io/nsh/defconfig
+#
+# Copyright (C) 2012 PX4 Development Team. All rights reserved.
+# Copyright (C) 2011-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.
+#
+############################################################################
+#
+# architecture selection
+#
+# CONFIG_ARCH - identifies the arch subdirectory and, hence, the
+# processor architecture.
+# CONFIG_ARCH_family - for use in C code. This identifies the
+# particular chip family that the architecture is implemented
+# in.
+# CONFIG_ARCH_architecture - for use in C code. This identifies the
+# specific architecture within the chip family.
+# CONFIG_ARCH_CHIP - Identifies the arch/*/chip subdirectory
+# CONFIG_ARCH_CHIP_name - For use in C code
+# CONFIG_ARCH_BOARD - identifies the configs subdirectory and, hence,
+# the board that supports the particular chip or SoC.
+# CONFIG_ARCH_BOARD_name - for use in C code
+# CONFIG_ENDIAN_BIG - define if big endian (default is little endian)
+# CONFIG_BOARD_LOOPSPERMSEC - for delay loops
+# CONFIG_DRAM_SIZE - Describes the installed DRAM.
+# CONFIG_DRAM_START - The start address of DRAM (physical)
+# CONFIG_ARCH_IRQPRIO - The ST32F100CB supports interrupt prioritization
+# CONFIG_ARCH_INTERRUPTSTACK - This architecture supports an interrupt
+# stack. If defined, this symbol is the size of the interrupt
+# stack in bytes. If not defined, the user task stacks will be
+# used during interrupt handling.
+# CONFIG_ARCH_STACKDUMP - Do stack dumps after assertions
+# CONFIG_ARCH_BOOTLOADER - Set if you are using a bootloader.
+# CONFIG_ARCH_LEDS - Use LEDs to show state. Unique to board architecture.
+# CONFIG_ARCH_BUTTONS - Enable support for buttons. Unique to board architecture.
+# CONFIG_ARCH_CALIBRATION - Enables some build in instrumentation that
+# cause a 100 second delay during boot-up. This 100 second delay
+# serves no purpose other than it allows you to calibrate
+# CONFIG_BOARD_LOOPSPERMSEC. You simply use a stop watch to measure
+# the 100 second delay then adjust CONFIG_BOARD_LOOPSPERMSEC until
+# the delay actually is 100 seconds.
+# CONFIG_ARCH_DMA - Support DMA initialization
+#
+CONFIG_ARCH="arm"
+CONFIG_ARCH_ARM=y
+CONFIG_ARCH_CORTEXM3=y
+CONFIG_ARCH_CHIP="stm32"
+CONFIG_ARCH_CHIP_STM32F100C8=y
+CONFIG_ARCH_BOARD="px4iov2"
+CONFIG_ARCH_BOARD_PX4IOV2=y
+CONFIG_BOARD_LOOPSPERMSEC=2000
+CONFIG_DRAM_SIZE=0x00002000
+CONFIG_DRAM_START=0x20000000
+CONFIG_ARCH_IRQPRIO=y
+CONFIG_ARCH_INTERRUPTSTACK=n
+CONFIG_ARCH_STACKDUMP=y
+CONFIG_ARCH_BOOTLOADER=n
+CONFIG_ARCH_LEDS=n
+CONFIG_ARCH_BUTTONS=n
+CONFIG_ARCH_CALIBRATION=n
+CONFIG_ARCH_DMA=y
+CONFIG_ARCH_MATH_H=y
+
+CONFIG_ARMV7M_CMNVECTOR=y
+
+#
+# JTAG Enable settings (by default JTAG-DP and SW-DP are disabled):
+#
+# CONFIG_STM32_DFU - Use the DFU bootloader, not JTAG
+#
+# JTAG Enable options:
+#
+# CONFIG_STM32_JTAG_FULL_ENABLE - Enables full SWJ (JTAG-DP + SW-DP)
+# CONFIG_STM32_JTAG_NOJNTRST_ENABLE - Enables full SWJ (JTAG-DP + SW-DP)
+# but without JNTRST.
+# CONFIG_STM32_JTAG_SW_ENABLE - Set JTAG-DP disabled and SW-DP enabled
+#
+CONFIG_STM32_DFU=n
+CONFIG_STM32_JTAG_FULL_ENABLE=y
+CONFIG_STM32_JTAG_NOJNTRST_ENABLE=n
+CONFIG_STM32_JTAG_SW_ENABLE=n
+
+#
+# Individual subsystems can be enabled:
+#
+# AHB:
+CONFIG_STM32_DMA1=y
+CONFIG_STM32_DMA2=n
+CONFIG_STM32_CRC=n
+# APB1:
+# Timers 2,3 and 4 are owned by the PWM driver
+CONFIG_STM32_TIM2=n
+CONFIG_STM32_TIM3=n
+CONFIG_STM32_TIM4=n
+CONFIG_STM32_TIM5=n
+CONFIG_STM32_TIM6=n
+CONFIG_STM32_TIM7=n
+CONFIG_STM32_WWDG=n
+CONFIG_STM32_SPI2=n
+CONFIG_STM32_USART2=y
+CONFIG_STM32_USART3=y
+CONFIG_STM32_I2C1=n
+CONFIG_STM32_I2C2=n
+CONFIG_STM32_BKP=n
+CONFIG_STM32_PWR=n
+CONFIG_STM32_DAC=n
+# APB2:
+# We use our own ADC driver, but leave this on for clocking purposes.
+CONFIG_STM32_ADC1=y
+CONFIG_STM32_ADC2=n
+# TIM1 is owned by the HRT
+CONFIG_STM32_TIM1=n
+CONFIG_STM32_SPI1=n
+CONFIG_STM32_TIM8=n
+CONFIG_STM32_USART1=y
+CONFIG_STM32_ADC3=n
+
+
+#
+# STM32F100 specific serial device driver settings
+#
+# CONFIG_USARTn_SERIAL_CONSOLE - selects the USARTn for the
+# console and ttys0 (default is the USART1).
+# CONFIG_USARTn_RXBUFSIZE - Characters are buffered as received.
+# This specific the size of the receive buffer
+# CONFIG_USARTn_TXBUFSIZE - Characters are buffered before
+# being sent. This specific the size of the transmit buffer
+# CONFIG_USARTn_BAUD - The configure BAUD of the UART. Must be
+# CONFIG_USARTn_BITS - The number of bits. Must be either 7 or 8.
+# CONFIG_USARTn_PARTIY - 0=no parity, 1=odd parity, 2=even parity
+# CONFIG_USARTn_2STOP - Two stop bits
+#
+CONFIG_SERIAL_TERMIOS=y
+CONFIG_STANDARD_SERIAL=y
+
+CONFIG_USART1_SERIAL_CONSOLE=y
+CONFIG_USART2_SERIAL_CONSOLE=n
+CONFIG_USART3_SERIAL_CONSOLE=n
+
+CONFIG_USART1_TXBUFSIZE=64
+CONFIG_USART2_TXBUFSIZE=64
+CONFIG_USART3_TXBUFSIZE=64
+
+CONFIG_USART1_RXBUFSIZE=64
+CONFIG_USART2_RXBUFSIZE=64
+CONFIG_USART3_RXBUFSIZE=64
+
+CONFIG_USART1_BAUD=115200
+CONFIG_USART2_BAUD=115200
+CONFIG_USART3_BAUD=115200
+
+CONFIG_USART1_BITS=8
+CONFIG_USART2_BITS=8
+CONFIG_USART3_BITS=8
+
+CONFIG_USART1_PARITY=0
+CONFIG_USART2_PARITY=0
+CONFIG_USART3_PARITY=0
+
+CONFIG_USART1_2STOP=0
+CONFIG_USART2_2STOP=0
+CONFIG_USART3_2STOP=0
+
+CONFIG_USART1_RXDMA=y
+SERIAL_HAVE_CONSOLE_DMA=y
+# Conflicts with I2C1 DMA
+CONFIG_USART2_RXDMA=n
+CONFIG_USART3_RXDMA=y
+
+#
+# PX4IO specific driver settings
+#
+# CONFIG_HRT_TIMER
+# Enables the high-resolution timer. The board definition must
+# set HRT_TIMER and HRT_TIMER_CHANNEL to the timer and capture/
+# compare channels to be used.
+# CONFIG_HRT_PPM
+# Enables R/C PPM input using the HRT. The board definition must
+# set HRT_PPM_CHANNEL to the timer capture/compare channel to be
+# used, and define GPIO_PPM_IN to configure the appropriate timer
+# GPIO.
+# CONFIG_PWM_SERVO
+# Enables the PWM servo driver. The driver configuration must be
+# supplied by the board support at initialisation time.
+# Note that USART2 must be disabled on the PX4 board for this to
+# be available.
+#
+#
+CONFIG_HRT_TIMER=y
+CONFIG_HRT_PPM=y
+
+#
+# General build options
+#
+# CONFIG_RRLOAD_BINARY - make the rrload binary format used with
+# BSPs from www.ridgerun.com using the tools/mkimage.sh script
+# CONFIG_INTELHEX_BINARY - make the Intel HEX binary format
+# used with many different loaders using the GNU objcopy program
+# Should not be selected if you are not using the GNU toolchain.
+# CONFIG_MOTOROLA_SREC - make the Motorola S-Record binary format
+# used with many different loaders using the GNU objcopy program
+# Should not be selected if you are not using the GNU toolchain.
+# CONFIG_RAW_BINARY - make a raw binary format file used with many
+# different loaders using the GNU objcopy program. This option
+# should not be selected if you are not using the GNU toolchain.
+# CONFIG_HAVE_LIBM - toolchain supports libm.a
+#
+CONFIG_RRLOAD_BINARY=n
+CONFIG_INTELHEX_BINARY=n
+CONFIG_MOTOROLA_SREC=n
+CONFIG_RAW_BINARY=y
+CONFIG_HAVE_LIBM=n
+
+#
+# General OS setup
+#
+# CONFIG_APPS_DIR - Identifies the relative path to the directory
+# that builds the application to link with NuttX. Default: ../apps
+# CONFIG_DEBUG - enables built-in debug options
+# CONFIG_DEBUG_VERBOSE - enables verbose debug output
+# CONFIG_DEBUG_SYMBOLS - build without optimization and with
+# debug symbols (needed for use with a debugger).
+# CONFIG_HAVE_CXX - Enable support for C++
+# CONFIG_HAVE_CXXINITIALIZE - The platform-specific logic includes support
+# for initialization of static C++ instances for this architecture
+# and for the selected toolchain (via up_cxxinitialize()).
+# CONFIG_MM_REGIONS - If the architecture includes multiple
+# regions of memory to allocate from, this specifies the
+# number of memory regions that the memory manager must
+# handle and enables the API mm_addregion(start, end);
+# CONFIG_ARCH_LOWPUTC - architecture supports low-level, boot
+# time console output
+# CONFIG_MSEC_PER_TICK - The default system timer is 100Hz
+# or MSEC_PER_TICK=10. This setting may be defined to
+# inform NuttX that the processor hardware is providing
+# system timer interrupts at some interrupt interval other
+# than 10 msec.
+# CONFIG_RR_INTERVAL - The round robin timeslice will be set
+# this number of milliseconds; Round robin scheduling can
+# be disabled by setting this value to zero.
+# CONFIG_SCHED_INSTRUMENTATION - enables instrumentation in
+# scheduler to monitor system performance
+# CONFIG_TASK_NAME_SIZE - Spcifies that maximum size of a
+# task name to save in the TCB. Useful if scheduler
+# instrumentation is selected. Set to zero to disable.
+# CONFIG_START_YEAR, CONFIG_START_MONTH, CONFIG_START_DAY -
+# Used to initialize the internal time logic.
+# CONFIG_GREGORIAN_TIME - Enables Gregorian time conversions.
+# You would only need this if you are concerned about accurate
+# time conversions in the past or in the distant future.
+# CONFIG_JULIAN_TIME - Enables Julian time conversions. You
+# would only need this if you are concerned about accurate
+# time conversion in the distand past. You must also define
+# CONFIG_GREGORIAN_TIME in order to use Julian time.
+# CONFIG_DEV_CONSOLE - Set if architecture-specific logic
+# provides /dev/console. Enables stdout, stderr, stdin.
+# CONFIG_DEV_LOWCONSOLE - Use the simple, low-level serial console
+# driver (minimul support)
+# CONFIG_MUTEX_TYPES: Set to enable support for recursive and
+# errorcheck mutexes. Enables pthread_mutexattr_settype().
+# CONFIG_PRIORITY_INHERITANCE : Set to enable support for priority
+# inheritance on mutexes and semaphores.
+# CONFIG_SEM_PREALLOCHOLDERS: This setting is only used if priority
+# inheritance is enabled. It defines the maximum number of
+# different threads (minus one) that can take counts on a
+# semaphore with priority inheritance support. This may be
+# set to zero if priority inheritance is disabled OR if you
+# are only using semaphores as mutexes (only one holder) OR
+# if no more than two threads participate using a counting
+# semaphore.
+# CONFIG_SEM_NNESTPRIO. If priority inheritance is enabled,
+# then this setting is the maximum number of higher priority
+# threads (minus 1) than can be waiting for another thread
+# to release a count on a semaphore. This value may be set
+# to zero if no more than one thread is expected to wait for
+# a semaphore.
+# CONFIG_FDCLONE_DISABLE. Disable cloning of all file descriptors
+# by task_create() when a new task is started. If set, all
+# files/drivers will appear to be closed in the new task.
+# CONFIG_FDCLONE_STDIO. Disable cloning of all but the first
+# three file descriptors (stdin, stdout, stderr) by task_create()
+# when a new task is started. If set, all files/drivers will
+# appear to be closed in the new task except for stdin, stdout,
+# and stderr.
+# CONFIG_SDCLONE_DISABLE. Disable cloning of all socket
+# desciptors by task_create() when a new task is started. If
+# set, all sockets will appear to be closed in the new task.
+# CONFIG_SCHED_WORKQUEUE. Create a dedicated "worker" thread to
+# handle delayed processing from interrupt handlers. This feature
+# is required for some drivers but, if there are not complaints,
+# can be safely disabled. The worker thread also performs
+# garbage collection -- completing any delayed memory deallocations
+# from interrupt handlers. If the worker thread is disabled,
+# then that clean will be performed by the IDLE thread instead
+# (which runs at the lowest of priority and may not be appropriate
+# if memory reclamation is of high priority). If CONFIG_SCHED_WORKQUEUE
+# is enabled, then the following options can also be used:
+# CONFIG_SCHED_WORKPRIORITY - The execution priority of the worker
+# thread. Default: 50
+# CONFIG_SCHED_WORKPERIOD - How often the worker thread checks for
+# work in units of microseconds. Default: 50*1000 (50 MS).
+# CONFIG_SCHED_WORKSTACKSIZE - The stack size allocated for the worker
+# thread. Default: CONFIG_IDLETHREAD_STACKSIZE.
+# CONFIG_SIG_SIGWORK - The signal number that will be used to wake-up
+# the worker thread. Default: 4
+# CONFIG_SCHED_WAITPID - Enable the waitpid() API
+# CONFIG_SCHED_ATEXIT - Enabled the atexit() API
+#
+CONFIG_USER_ENTRYPOINT="user_start"
+#CONFIG_APPS_DIR=
+CONFIG_DEBUG=n
+CONFIG_DEBUG_VERBOSE=n
+CONFIG_DEBUG_SYMBOLS=y
+CONFIG_DEBUG_FS=n
+CONFIG_DEBUG_GRAPHICS=n
+CONFIG_DEBUG_LCD=n
+CONFIG_DEBUG_USB=n
+CONFIG_DEBUG_NET=n
+CONFIG_DEBUG_RTC=n
+CONFIG_DEBUG_ANALOG=n
+CONFIG_DEBUG_PWM=n
+CONFIG_DEBUG_CAN=n
+CONFIG_DEBUG_I2C=n
+CONFIG_DEBUG_INPUT=n
+
+CONFIG_MSEC_PER_TICK=1
+CONFIG_HAVE_CXX=y
+CONFIG_HAVE_CXXINITIALIZE=y
+CONFIG_MM_REGIONS=1
+CONFIG_MM_SMALL=y
+CONFIG_ARCH_LOWPUTC=y
+CONFIG_RR_INTERVAL=0
+CONFIG_SCHED_INSTRUMENTATION=n
+CONFIG_TASK_NAME_SIZE=8
+CONFIG_START_YEAR=1970
+CONFIG_START_MONTH=1
+CONFIG_START_DAY=1
+CONFIG_GREGORIAN_TIME=n
+CONFIG_JULIAN_TIME=n
+# this eats ~1KiB of RAM ... work out why
+CONFIG_DEV_CONSOLE=y
+CONFIG_DEV_LOWCONSOLE=n
+CONFIG_MUTEX_TYPES=n
+CONFIG_PRIORITY_INHERITANCE=n
+CONFIG_SEM_PREALLOCHOLDERS=0
+CONFIG_SEM_NNESTPRIO=0
+CONFIG_FDCLONE_DISABLE=y
+CONFIG_FDCLONE_STDIO=y
+CONFIG_SDCLONE_DISABLE=y
+CONFIG_SCHED_WORKQUEUE=n
+CONFIG_SCHED_WORKPRIORITY=50
+CONFIG_SCHED_WORKPERIOD=50000
+CONFIG_SCHED_WORKSTACKSIZE=1024
+CONFIG_SIG_SIGWORK=4
+CONFIG_SCHED_WAITPID=n
+CONFIG_SCHED_ATEXIT=n
+
+#
+# The following can be used to disable categories of
+# APIs supported by the OS. If the compiler supports
+# weak functions, then it should not be necessary to
+# disable functions unless you want to restrict usage
+# of those APIs.
+#
+# There are certain dependency relationships in these
+# features.
+#
+# o mq_notify logic depends on signals to awaken tasks
+# waiting for queues to become full or empty.
+# o pthread_condtimedwait() depends on signals to wake
+# up waiting tasks.
+#
+CONFIG_DISABLE_CLOCK=n
+CONFIG_DISABLE_POSIX_TIMERS=y
+CONFIG_DISABLE_PTHREAD=y
+CONFIG_DISABLE_SIGNALS=y
+CONFIG_DISABLE_MQUEUE=y
+CONFIG_DISABLE_MOUNTPOINT=y
+CONFIG_DISABLE_ENVIRON=y
+CONFIG_DISABLE_POLL=y
+
+#
+# Misc libc settings
+#
+# CONFIG_NOPRINTF_FIELDWIDTH - sprintf-related logic is a
+# little smaller if we do not support fieldwidthes
+#
+CONFIG_NOPRINTF_FIELDWIDTH=n
+
+#
+# Allow for architecture optimized implementations
+#
+# The architecture can provide optimized versions of the
+# following to improve system performance
+#
+CONFIG_ARCH_MEMCPY=n
+CONFIG_ARCH_MEMCMP=n
+CONFIG_ARCH_MEMMOVE=n
+CONFIG_ARCH_MEMSET=n
+CONFIG_ARCH_STRCMP=n
+CONFIG_ARCH_STRCPY=n
+CONFIG_ARCH_STRNCPY=n
+CONFIG_ARCH_STRLEN=n
+CONFIG_ARCH_STRNLEN=n
+CONFIG_ARCH_BZERO=n
+
+#
+# Sizes of configurable things (0 disables)
+#
+# CONFIG_MAX_TASKS - The maximum number of simultaneously
+# active tasks. This value must be a power of two.
+# CONFIG_MAX_TASK_ARGS - This controls the maximum number of
+# of parameters that a task may receive (i.e., maxmum value
+# of 'argc')
+# CONFIG_NPTHREAD_KEYS - The number of items of thread-
+# specific data that can be retained
+# CONFIG_NFILE_DESCRIPTORS - The maximum number of file
+# descriptors (one for each open)
+# CONFIG_NFILE_STREAMS - The maximum number of streams that
+# can be fopen'ed
+# CONFIG_NAME_MAX - The maximum size of a file name.
+# CONFIG_STDIO_BUFFER_SIZE - Size of the buffer to allocate
+# on fopen. (Only if CONFIG_NFILE_STREAMS > 0)
+# CONFIG_STDIO_LINEBUFFER - If standard C buffered I/O is enabled
+# (CONFIG_STDIO_BUFFER_SIZE > 0), then this option may be added
+# to force automatic, line-oriented flushing the output buffer
+# for putc(), fputc(), putchar(), puts(), fputs(), printf(),
+# fprintf(), and vfprintf(). When a newline is encountered in
+# the output string, the output buffer will be flushed. This
+# (slightly) increases the NuttX footprint but supports the kind
+# of behavior that people expect for printf().
+# CONFIG_NUNGET_CHARS - Number of characters that can be
+# buffered by ungetc() (Only if CONFIG_NFILE_STREAMS > 0)
+# CONFIG_PREALLOC_MQ_MSGS - The number of pre-allocated message
+# structures. The system manages a pool of preallocated
+# message structures to minimize dynamic allocations
+# CONFIG_MQ_MAXMSGSIZE - Message structures are allocated with
+# a fixed payload size given by this settin (does not include
+# other message structure overhead.
+# CONFIG_MAX_WDOGPARMS - Maximum number of parameters that
+# can be passed to a watchdog handler
+# CONFIG_PREALLOC_WDOGS - The number of pre-allocated watchdog
+# structures. The system manages a pool of preallocated
+# watchdog structures to minimize dynamic allocations
+# CONFIG_PREALLOC_TIMERS - The number of pre-allocated POSIX
+# timer structures. The system manages a pool of preallocated
+# timer structures to minimize dynamic allocations. Set to
+# zero for all dynamic allocations.
+#
+CONFIG_MAX_TASKS=4
+CONFIG_MAX_TASK_ARGS=4
+CONFIG_NPTHREAD_KEYS=2
+CONFIG_NFILE_DESCRIPTORS=8
+CONFIG_NFILE_STREAMS=0
+CONFIG_NAME_MAX=12
+CONFIG_STDIO_BUFFER_SIZE=32
+CONFIG_STDIO_LINEBUFFER=n
+CONFIG_NUNGET_CHARS=2
+CONFIG_PREALLOC_MQ_MSGS=4
+CONFIG_MQ_MAXMSGSIZE=32
+CONFIG_MAX_WDOGPARMS=2
+CONFIG_PREALLOC_WDOGS=4
+CONFIG_PREALLOC_TIMERS=0
+
+
+#
+# Settings for apps/nshlib
+#
+# CONFIG_NSH_BUILTIN_APPS - Support external registered,
+# "named" applications that can be executed from the NSH
+# command line (see apps/README.txt for more information).
+# CONFIG_NSH_FILEIOSIZE - Size of a static I/O buffer
+# CONFIG_NSH_STRERROR - Use strerror(errno)
+# CONFIG_NSH_LINELEN - Maximum length of one command line
+# CONFIG_NSH_NESTDEPTH - Max number of nested if-then[-else]-fi
+# CONFIG_NSH_DISABLESCRIPT - Disable scripting support
+# CONFIG_NSH_DISABLEBG - Disable background commands
+# CONFIG_NSH_ROMFSETC - Use startup script in /etc
+# CONFIG_NSH_CONSOLE - Use serial console front end
+# CONFIG_NSH_TELNET - Use telnetd console front end
+# CONFIG_NSH_ARCHINIT - Platform provides architecture
+# specific initialization (nsh_archinitialize()).
+#
+
+# Disable NSH completely
+CONFIG_NSH_CONSOLE=n
+
+#
+# Stack and heap information
+#
+# CONFIG_BOOT_RUNFROMFLASH - Some configurations support XIP
+# operation from FLASH but must copy initialized .data sections to RAM.
+# (should also be =n for the STM3210E-EVAL which always runs from flash)
+# CONFIG_BOOT_COPYTORAM - Some configurations boot in FLASH
+# but copy themselves entirely into RAM for better performance.
+# CONFIG_CUSTOM_STACK - The up_ implementation will handle
+# all stack operations outside of the nuttx model.
+# CONFIG_STACK_POINTER - The initial stack pointer (arm7tdmi only)
+# CONFIG_IDLETHREAD_STACKSIZE - The size of the initial stack.
+# This is the thread that (1) performs the inital boot of the system up
+# to the point where user_start() is spawned, and (2) there after is the
+# IDLE thread that executes only when there is no other thread ready to
+# run.
+# CONFIG_USERMAIN_STACKSIZE - The size of the stack to allocate
+# for the main user thread that begins at the user_start() entry point.
+# CONFIG_PTHREAD_STACK_MIN - Minimum pthread stack size
+# CONFIG_PTHREAD_STACK_DEFAULT - Default pthread stack size
+# CONFIG_HEAP_BASE - The beginning of the heap
+# CONFIG_HEAP_SIZE - The size of the heap
+#
+CONFIG_BOOT_RUNFROMFLASH=n
+CONFIG_BOOT_COPYTORAM=n
+CONFIG_CUSTOM_STACK=n
+CONFIG_STACK_POINTER=
+CONFIG_IDLETHREAD_STACKSIZE=1024
+CONFIG_USERMAIN_STACKSIZE=1200
+CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_PTHREAD_STACK_DEFAULT=1024
+CONFIG_HEAP_BASE=
+CONFIG_HEAP_SIZE=
diff --git a/nuttx/configs/px4iov2/io/setenv.sh b/nuttx/configs/px4iov2/io/setenv.sh
new file mode 100755
index 000000000..ff9a4bf8a
--- /dev/null
+++ b/nuttx/configs/px4iov2/io/setenv.sh
@@ -0,0 +1,47 @@
+#!/bin/bash
+# configs/stm3210e-eval/dfu/setenv.sh
+#
+# Copyright (C) 2009 Gregory Nutt. All rights reserved.
+# Author: Gregory Nutt <gnutt@nuttx.org>
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+# notice, this list of conditions and the following disclaimer in
+# the documentation and/or other materials provided with the
+# distribution.
+# 3. Neither the name NuttX nor the names of its contributors may be
+# used to endorse or promote products derived from this software
+# without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+
+if [ "$(basename $0)" = "setenv.sh" ] ; then
+ echo "You must source this script, not run it!" 1>&2
+ exit 1
+fi
+
+if [ -z "${PATH_ORIG}" ]; then export PATH_ORIG="${PATH}"; fi
+
+WD=`pwd`
+export RIDE_BIN="/cygdrive/c/Program Files/Raisonance/Ride/arm-gcc/bin"
+export BUILDROOT_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin"
+export PATH="${BUILDROOT_BIN}:${RIDE_BIN}:/sbin:/usr/sbin:${PATH_ORIG}"
+
+echo "PATH : ${PATH}"
diff --git a/nuttx/configs/px4iov2/nsh/Make.defs b/nuttx/configs/px4iov2/nsh/Make.defs
new file mode 100644
index 000000000..87508e22e
--- /dev/null
+++ b/nuttx/configs/px4iov2/nsh/Make.defs
@@ -0,0 +1,3 @@
+include ${TOPDIR}/.config
+
+include $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/Make.defs
diff --git a/nuttx/configs/px4iov2/nsh/appconfig b/nuttx/configs/px4iov2/nsh/appconfig
new file mode 100644
index 000000000..3cfc41b43
--- /dev/null
+++ b/nuttx/configs/px4iov2/nsh/appconfig
@@ -0,0 +1,44 @@
+############################################################################
+# configs/stm3210e-eval/nsh/appconfig
+#
+# Copyright (C) 2011-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.
+#
+############################################################################
+
+# Path to example in apps/examples containing the user_start entry point
+
+CONFIGURED_APPS += examples/nsh
+
+CONFIGURED_APPS += system/readline
+CONFIGURED_APPS += nshlib
+CONFIGURED_APPS += reboot
+
+CONFIGURED_APPS += drivers/boards/px4iov2
diff --git a/nuttx/configs/px4iov2/nsh/defconfig b/nuttx/configs/px4iov2/nsh/defconfig
new file mode 100755
index 000000000..14d7a6401
--- /dev/null
+++ b/nuttx/configs/px4iov2/nsh/defconfig
@@ -0,0 +1,567 @@
+############################################################################
+# configs/px4io/nsh/defconfig
+#
+# Copyright (C) 2009-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.
+#
+############################################################################
+#
+# architecture selection
+#
+# CONFIG_ARCH - identifies the arch subdirectory and, hence, the
+# processor architecture.
+# CONFIG_ARCH_family - for use in C code. This identifies the
+# particular chip family that the architecture is implemented
+# in.
+# CONFIG_ARCH_architecture - for use in C code. This identifies the
+# specific architecture within the chip familyl.
+# CONFIG_ARCH_CHIP - Identifies the arch/*/chip subdirectory
+# CONFIG_ARCH_CHIP_name - For use in C code
+# CONFIG_ARCH_BOARD - identifies the configs subdirectory and, hence,
+# the board that supports the particular chip or SoC.
+# CONFIG_ARCH_BOARD_name - for use in C code
+# CONFIG_ENDIAN_BIG - define if big endian (default is little endian)
+# CONFIG_BOARD_LOOPSPERMSEC - for delay loops
+# CONFIG_DRAM_SIZE - Describes the installed DRAM.
+# CONFIG_DRAM_START - The start address of DRAM (physical)
+# CONFIG_DRAM_END - Last address+1 of installed RAM
+# CONFIG_ARCH_IRQPRIO - The ST32F100CB supports interrupt prioritization
+# CONFIG_ARCH_INTERRUPTSTACK - This architecture supports an interrupt
+# stack. If defined, this symbol is the size of the interrupt
+# stack in bytes. If not defined, the user task stacks will be
+# used during interrupt handling.
+# CONFIG_ARCH_STACKDUMP - Do stack dumps after assertions
+# CONFIG_ARCH_BOOTLOADER - Set if you are using a bootloader.
+# CONFIG_ARCH_LEDS - Use LEDs to show state. Unique to board architecture.
+# CONFIG_ARCH_BUTTONS - Enable support for buttons. Unique to board architecture.
+# CONFIG_ARCH_CALIBRATION - Enables some build in instrumentation that
+# cause a 100 second delay during boot-up. This 100 second delay
+# serves no purpose other than it allows you to calibrate
+# CONFIG_BOARD_LOOPSPERMSEC. You simply use a stop watch to measure
+# the 100 second delay then adjust CONFIG_BOARD_LOOPSPERMSEC until
+# the delay actually is 100 seconds.
+# CONFIG_ARCH_DMA - Support DMA initialization
+#
+CONFIG_ARCH=arm
+CONFIG_ARCH_ARM=y
+CONFIG_ARCH_CORTEXM3=y
+CONFIG_ARCH_CHIP=stm32
+CONFIG_ARCH_CHIP_STM32F100C8=y
+CONFIG_ARCH_BOARD=px4iov2
+CONFIG_ARCH_BOARD_PX4IOV2=y
+CONFIG_BOARD_LOOPSPERMSEC=24000
+CONFIG_DRAM_SIZE=0x00002000
+CONFIG_DRAM_START=0x20000000
+CONFIG_DRAM_END=(CONFIG_DRAM_START+CONFIG_DRAM_SIZE)
+CONFIG_ARCH_IRQPRIO=y
+CONFIG_ARCH_INTERRUPTSTACK=n
+CONFIG_ARCH_STACKDUMP=y
+CONFIG_ARCH_BOOTLOADER=n
+CONFIG_ARCH_LEDS=n
+CONFIG_ARCH_BUTTONS=y
+CONFIG_ARCH_CALIBRATION=n
+CONFIG_ARCH_DMA=n
+CONFIG_ARMV7M_CMNVECTOR=y
+
+#
+# JTAG Enable settings (by default JTAG-DP and SW-DP are disabled):
+#
+# CONFIG_STM32_DFU - Use the DFU bootloader, not JTAG
+#
+# JTAG Enable options:
+#
+# CONFIG_STM32_JTAG_FULL_ENABLE - Enables full SWJ (JTAG-DP + SW-DP)
+# CONFIG_STM32_JTAG_NOJNTRST_ENABLE - Enables full SWJ (JTAG-DP + SW-DP)
+# but without JNTRST.
+# CONFIG_STM32_JTAG_SW_ENABLE - Set JTAG-DP disabled and SW-DP enabled
+#
+CONFIG_STM32_DFU=n
+CONFIG_STM32_JTAG_FULL_ENABLE=y
+CONFIG_STM32_JTAG_NOJNTRST_ENABLE=n
+CONFIG_STM32_JTAG_SW_ENABLE=n
+
+#
+# Individual subsystems can be enabled:
+# AHB:
+CONFIG_STM32_DMA1=n
+CONFIG_STM32_DMA2=n
+CONFIG_STM32_CRC=n
+# APB1:
+# Timers 2,3 and 4 are owned by the PWM driver
+CONFIG_STM32_TIM2=n
+CONFIG_STM32_TIM3=n
+CONFIG_STM32_TIM4=n
+CONFIG_STM32_TIM5=n
+CONFIG_STM32_TIM6=n
+CONFIG_STM32_TIM7=n
+CONFIG_STM32_WWDG=n
+CONFIG_STM32_SPI2=n
+CONFIG_STM32_USART2=y
+CONFIG_STM32_USART3=y
+CONFIG_STM32_I2C1=y
+CONFIG_STM32_I2C2=n
+CONFIG_STM32_BKP=n
+CONFIG_STM32_PWR=n
+CONFIG_STM32_DAC=n
+# APB2:
+CONFIG_STM32_ADC1=y
+CONFIG_STM32_ADC2=n
+# TIM1 is owned by the HRT
+CONFIG_STM32_TIM1=n
+CONFIG_STM32_SPI1=n
+CONFIG_STM32_TIM8=n
+CONFIG_STM32_USART1=y
+CONFIG_STM32_ADC3=n
+
+#
+# Timer and I2C devices may need to the following to force power to be applied:
+#
+#CONFIG_STM32_FORCEPOWER=y
+
+#
+# STM32F100 specific serial device driver settings
+#
+# CONFIG_USARTn_SERIAL_CONSOLE - selects the USARTn for the
+# console and ttys0 (default is the USART1).
+# CONFIG_USARTn_RXBUFSIZE - Characters are buffered as received.
+# This specific the size of the receive buffer
+# CONFIG_USARTn_TXBUFSIZE - Characters are buffered before
+# being sent. This specific the size of the transmit buffer
+# CONFIG_USARTn_BAUD - The configure BAUD of the UART. Must be
+# CONFIG_USARTn_BITS - The number of bits. Must be either 7 or 8.
+# CONFIG_USARTn_PARTIY - 0=no parity, 1=odd parity, 2=even parity
+# CONFIG_USARTn_2STOP - Two stop bits
+#
+CONFIG_USART1_SERIAL_CONSOLE=y
+CONFIG_USART2_SERIAL_CONSOLE=n
+CONFIG_USART3_SERIAL_CONSOLE=n
+
+CONFIG_USART1_TXBUFSIZE=64
+CONFIG_USART2_TXBUFSIZE=64
+CONFIG_USART3_TXBUFSIZE=64
+
+CONFIG_USART1_RXBUFSIZE=64
+CONFIG_USART2_RXBUFSIZE=64
+CONFIG_USART3_RXBUFSIZE=64
+
+CONFIG_USART1_BAUD=57600
+CONFIG_USART2_BAUD=115200
+CONFIG_USART3_BAUD=115200
+
+CONFIG_USART1_BITS=8
+CONFIG_USART2_BITS=8
+CONFIG_USART3_BITS=8
+
+CONFIG_USART1_PARITY=0
+CONFIG_USART2_PARITY=0
+CONFIG_USART3_PARITY=0
+
+CONFIG_USART1_2STOP=0
+CONFIG_USART2_2STOP=0
+CONFIG_USART3_2STOP=0
+
+#
+# PX4IO specific driver settings
+#
+# CONFIG_HRT_TIMER
+# Enables the high-resolution timer. The board definition must
+# set HRT_TIMER and HRT_TIMER_CHANNEL to the timer and capture/
+# compare channels to be used.
+# CONFIG_HRT_PPM
+# Enables R/C PPM input using the HRT. The board definition must
+# set HRT_PPM_CHANNEL to the timer capture/compare channel to be
+# used, and define GPIO_PPM_IN to configure the appropriate timer
+# GPIO.
+# CONFIG_PWM_SERVO
+# Enables the PWM servo driver. The driver configuration must be
+# supplied by the board support at initialisation time.
+# Note that USART2 must be disabled on the PX4 board for this to
+# be available.
+#
+#
+CONFIG_HRT_TIMER=y
+CONFIG_HRT_PPM=y
+CONFIG_PWM_SERVO=y
+
+#
+# General build options
+#
+# CONFIG_RRLOAD_BINARY - make the rrload binary format used with
+# BSPs from www.ridgerun.com using the tools/mkimage.sh script
+# CONFIG_INTELHEX_BINARY - make the Intel HEX binary format
+# used with many different loaders using the GNU objcopy program
+# Should not be selected if you are not using the GNU toolchain.
+# CONFIG_MOTOROLA_SREC - make the Motorola S-Record binary format
+# used with many different loaders using the GNU objcopy program
+# Should not be selected if you are not using the GNU toolchain.
+# CONFIG_RAW_BINARY - make a raw binary format file used with many
+# different loaders using the GNU objcopy program. This option
+# should not be selected if you are not using the GNU toolchain.
+# CONFIG_HAVE_LIBM - toolchain supports libm.a
+#
+CONFIG_RRLOAD_BINARY=n
+CONFIG_INTELHEX_BINARY=n
+CONFIG_MOTOROLA_SREC=n
+CONFIG_RAW_BINARY=y
+CONFIG_HAVE_LIBM=n
+
+#
+# General OS setup
+#
+# CONFIG_APPS_DIR - Identifies the relative path to the directory
+# that builds the application to link with NuttX. Default: ../apps
+# CONFIG_DEBUG - enables built-in debug options
+# CONFIG_DEBUG_VERBOSE - enables verbose debug output
+# CONFIG_DEBUG_SYMBOLS - build without optimization and with
+# debug symbols (needed for use with a debugger).
+# CONFIG_HAVE_CXX - Enable support for C++
+# CONFIG_HAVE_CXXINITIALIZE - The platform-specific logic includes support
+# for initialization of static C++ instances for this architecture
+# and for the selected toolchain (via up_cxxinitialize()).
+# CONFIG_MM_REGIONS - If the architecture includes multiple
+# regions of memory to allocate from, this specifies the
+# number of memory regions that the memory manager must
+# handle and enables the API mm_addregion(start, end);
+# CONFIG_ARCH_LOWPUTC - architecture supports low-level, boot
+# time console output
+# CONFIG_MSEC_PER_TICK - The default system timer is 100Hz
+# or MSEC_PER_TICK=10. This setting may be defined to
+# inform NuttX that the processor hardware is providing
+# system timer interrupts at some interrupt interval other
+# than 10 msec.
+# CONFIG_RR_INTERVAL - The round robin timeslice will be set
+# this number of milliseconds; Round robin scheduling can
+# be disabled by setting this value to zero.
+# CONFIG_SCHED_INSTRUMENTATION - enables instrumentation in
+# scheduler to monitor system performance
+# CONFIG_TASK_NAME_SIZE - Spcifies that maximum size of a
+# task name to save in the TCB. Useful if scheduler
+# instrumentation is selected. Set to zero to disable.
+# CONFIG_START_YEAR, CONFIG_START_MONTH, CONFIG_START_DAY -
+# Used to initialize the internal time logic.
+# CONFIG_GREGORIAN_TIME - Enables Gregorian time conversions.
+# You would only need this if you are concerned about accurate
+# time conversions in the past or in the distant future.
+# CONFIG_JULIAN_TIME - Enables Julian time conversions. You
+# would only need this if you are concerned about accurate
+# time conversion in the distand past. You must also define
+# CONFIG_GREGORIAN_TIME in order to use Julian time.
+# CONFIG_DEV_CONSOLE - Set if architecture-specific logic
+# provides /dev/console. Enables stdout, stderr, stdin.
+# CONFIG_DEV_LOWCONSOLE - Use the simple, low-level serial console
+# driver (minimul support)
+# CONFIG_MUTEX_TYPES: Set to enable support for recursive and
+# errorcheck mutexes. Enables pthread_mutexattr_settype().
+# CONFIG_PRIORITY_INHERITANCE : Set to enable support for priority
+# inheritance on mutexes and semaphores.
+# CONFIG_SEM_PREALLOCHOLDERS: This setting is only used if priority
+# inheritance is enabled. It defines the maximum number of
+# different threads (minus one) that can take counts on a
+# semaphore with priority inheritance support. This may be
+# set to zero if priority inheritance is disabled OR if you
+# are only using semaphores as mutexes (only one holder) OR
+# if no more than two threads participate using a counting
+# semaphore.
+# CONFIG_SEM_NNESTPRIO. If priority inheritance is enabled,
+# then this setting is the maximum number of higher priority
+# threads (minus 1) than can be waiting for another thread
+# to release a count on a semaphore. This value may be set
+# to zero if no more than one thread is expected to wait for
+# a semaphore.
+# CONFIG_FDCLONE_DISABLE. Disable cloning of all file descriptors
+# by task_create() when a new task is started. If set, all
+# files/drivers will appear to be closed in the new task.
+# CONFIG_FDCLONE_STDIO. Disable cloning of all but the first
+# three file descriptors (stdin, stdout, stderr) by task_create()
+# when a new task is started. If set, all files/drivers will
+# appear to be closed in the new task except for stdin, stdout,
+# and stderr.
+# CONFIG_SDCLONE_DISABLE. Disable cloning of all socket
+# desciptors by task_create() when a new task is started. If
+# set, all sockets will appear to be closed in the new task.
+# CONFIG_NXFLAT. Enable support for the NXFLAT binary format.
+# This format will support execution of NuttX binaries located
+# in a ROMFS filesystem (see examples/nxflat).
+# CONFIG_SCHED_WORKQUEUE. Create a dedicated "worker" thread to
+# handle delayed processing from interrupt handlers. This feature
+# is required for some drivers but, if there are not complaints,
+# can be safely disabled. The worker thread also performs
+# garbage collection -- completing any delayed memory deallocations
+# from interrupt handlers. If the worker thread is disabled,
+# then that clean will be performed by the IDLE thread instead
+# (which runs at the lowest of priority and may not be appropriate
+# if memory reclamation is of high priority). If CONFIG_SCHED_WORKQUEUE
+# is enabled, then the following options can also be used:
+# CONFIG_SCHED_WORKPRIORITY - The execution priority of the worker
+# thread. Default: 50
+# CONFIG_SCHED_WORKPERIOD - How often the worker thread checks for
+# work in units of microseconds. Default: 50*1000 (50 MS).
+# CONFIG_SCHED_WORKSTACKSIZE - The stack size allocated for the worker
+# thread. Default: CONFIG_IDLETHREAD_STACKSIZE.
+# CONFIG_SIG_SIGWORK - The signal number that will be used to wake-up
+# the worker thread. Default: 4
+#
+#CONFIG_APPS_DIR=
+CONFIG_DEBUG=n
+CONFIG_DEBUG_VERBOSE=n
+CONFIG_DEBUG_SYMBOLS=y
+CONFIG_HAVE_CXX=y
+CONFIG_HAVE_CXXINITIALIZE=n
+CONFIG_MM_REGIONS=1
+CONFIG_MM_SMALL=y
+CONFIG_ARCH_LOWPUTC=y
+CONFIG_RR_INTERVAL=200
+CONFIG_SCHED_INSTRUMENTATION=n
+CONFIG_TASK_NAME_SIZE=0
+CONFIG_START_YEAR=2009
+CONFIG_START_MONTH=9
+CONFIG_START_DAY=21
+CONFIG_GREGORIAN_TIME=n
+CONFIG_JULIAN_TIME=n
+CONFIG_DEV_CONSOLE=y
+CONFIG_DEV_LOWCONSOLE=n
+CONFIG_MUTEX_TYPES=n
+CONFIG_PRIORITY_INHERITANCE=n
+CONFIG_SEM_PREALLOCHOLDERS=0
+CONFIG_SEM_NNESTPRIO=0
+CONFIG_FDCLONE_DISABLE=n
+CONFIG_FDCLONE_STDIO=y
+CONFIG_SDCLONE_DISABLE=y
+CONFIG_NXFLAT=n
+CONFIG_SCHED_WORKQUEUE=n
+CONFIG_SCHED_WORKPRIORITY=50
+CONFIG_SCHED_WORKPERIOD=(50*1000)
+CONFIG_SCHED_WORKSTACKSIZE=512
+CONFIG_SIG_SIGWORK=4
+
+CONFIG_USER_ENTRYPOINT="nsh_main"
+#
+# The following can be used to disable categories of
+# APIs supported by the OS. If the compiler supports
+# weak functions, then it should not be necessary to
+# disable functions unless you want to restrict usage
+# of those APIs.
+#
+# There are certain dependency relationships in these
+# features.
+#
+# o mq_notify logic depends on signals to awaken tasks
+# waiting for queues to become full or empty.
+# o pthread_condtimedwait() depends on signals to wake
+# up waiting tasks.
+#
+CONFIG_DISABLE_CLOCK=n
+CONFIG_DISABLE_POSIX_TIMERS=y
+CONFIG_DISABLE_PTHREAD=n
+CONFIG_DISABLE_SIGNALS=n
+CONFIG_DISABLE_MQUEUE=y
+CONFIG_DISABLE_MOUNTPOINT=y
+CONFIG_DISABLE_ENVIRON=y
+CONFIG_DISABLE_POLL=y
+
+#
+# Misc libc settings
+#
+# CONFIG_NOPRINTF_FIELDWIDTH - sprintf-related logic is a
+# little smaller if we do not support fieldwidthes
+#
+CONFIG_NOPRINTF_FIELDWIDTH=n
+
+#
+# Allow for architecture optimized implementations
+#
+# The architecture can provide optimized versions of the
+# following to improve system performance
+#
+CONFIG_ARCH_MEMCPY=n
+CONFIG_ARCH_MEMCMP=n
+CONFIG_ARCH_MEMMOVE=n
+CONFIG_ARCH_MEMSET=n
+CONFIG_ARCH_STRCMP=n
+CONFIG_ARCH_STRCPY=n
+CONFIG_ARCH_STRNCPY=n
+CONFIG_ARCH_STRLEN=n
+CONFIG_ARCH_STRNLEN=n
+CONFIG_ARCH_BZERO=n
+
+#
+# Sizes of configurable things (0 disables)
+#
+# CONFIG_MAX_TASKS - The maximum number of simultaneously
+# active tasks. This value must be a power of two.
+# CONFIG_MAX_TASK_ARGS - This controls the maximum number of
+# of parameters that a task may receive (i.e., maxmum value
+# of 'argc')
+# CONFIG_NPTHREAD_KEYS - The number of items of thread-
+# specific data that can be retained
+# CONFIG_NFILE_DESCRIPTORS - The maximum number of file
+# descriptors (one for each open)
+# CONFIG_NFILE_STREAMS - The maximum number of streams that
+# can be fopen'ed
+# CONFIG_NAME_MAX - The maximum size of a file name.
+# CONFIG_STDIO_BUFFER_SIZE - Size of the buffer to allocate
+# on fopen. (Only if CONFIG_NFILE_STREAMS > 0)
+# CONFIG_NUNGET_CHARS - Number of characters that can be
+# buffered by ungetc() (Only if CONFIG_NFILE_STREAMS > 0)
+# CONFIG_PREALLOC_MQ_MSGS - The number of pre-allocated message
+# structures. The system manages a pool of preallocated
+# message structures to minimize dynamic allocations
+# CONFIG_MQ_MAXMSGSIZE - Message structures are allocated with
+# a fixed payload size given by this settin (does not include
+# other message structure overhead.
+# CONFIG_MAX_WDOGPARMS - Maximum number of parameters that
+# can be passed to a watchdog handler
+# CONFIG_PREALLOC_WDOGS - The number of pre-allocated watchdog
+# structures. The system manages a pool of preallocated
+# watchdog structures to minimize dynamic allocations
+# CONFIG_PREALLOC_TIMERS - The number of pre-allocated POSIX
+# timer structures. The system manages a pool of preallocated
+# timer structures to minimize dynamic allocations. Set to
+# zero for all dynamic allocations.
+#
+CONFIG_MAX_TASKS=4
+CONFIG_MAX_TASK_ARGS=4
+CONFIG_NPTHREAD_KEYS=2
+CONFIG_NFILE_DESCRIPTORS=6
+CONFIG_NFILE_STREAMS=4
+CONFIG_NAME_MAX=32
+CONFIG_STDIO_BUFFER_SIZE=64
+CONFIG_NUNGET_CHARS=2
+CONFIG_PREALLOC_MQ_MSGS=1
+CONFIG_MQ_MAXMSGSIZE=32
+CONFIG_MAX_WDOGPARMS=2
+CONFIG_PREALLOC_WDOGS=3
+CONFIG_PREALLOC_TIMERS=1
+
+
+#
+# Settings for apps/nshlib
+#
+# CONFIG_NSH_BUILTIN_APPS - Support external registered,
+# "named" applications that can be executed from the NSH
+# command line (see apps/README.txt for more information).
+# CONFIG_NSH_FILEIOSIZE - Size of a static I/O buffer
+# CONFIG_NSH_STRERROR - Use strerror(errno)
+# CONFIG_NSH_LINELEN - Maximum length of one command line
+# CONFIG_NSH_NESTDEPTH - Max number of nested if-then[-else]-fi
+# CONFIG_NSH_DISABLESCRIPT - Disable scripting support
+# CONFIG_NSH_DISABLEBG - Disable background commands
+# CONFIG_NSH_ROMFSETC - Use startup script in /etc
+# CONFIG_NSH_CONSOLE - Use serial console front end
+# CONFIG_NSH_TELNET - Use telnetd console front end
+# CONFIG_NSH_ARCHINIT - Platform provides architecture
+# specific initialization (nsh_archinitialize()).
+#
+# If CONFIG_NSH_TELNET is selected:
+# CONFIG_NSH_IOBUFFER_SIZE -- Telnetd I/O buffer size
+# CONFIG_NSH_DHCPC - Obtain address using DHCP
+# CONFIG_NSH_IPADDR - Provides static IP address
+# CONFIG_NSH_DRIPADDR - Provides static router IP address
+# CONFIG_NSH_NETMASK - Provides static network mask
+# CONFIG_NSH_NOMAC - Use a bogus MAC address
+#
+# If CONFIG_NSH_ROMFSETC is selected:
+# CONFIG_NSH_ROMFSMOUNTPT - ROMFS mountpoint
+# CONFIG_NSH_INITSCRIPT - Relative path to init script
+# CONFIG_NSH_ROMFSDEVNO - ROMFS RAM device minor
+# CONFIG_NSH_ROMFSSECTSIZE - ROMF sector size
+# CONFIG_NSH_FATDEVNO - FAT FS RAM device minor
+# CONFIG_NSH_FATSECTSIZE - FAT FS sector size
+# CONFIG_NSH_FATNSECTORS - FAT FS number of sectors
+# CONFIG_NSH_FATMOUNTPT - FAT FS mountpoint
+#
+CONFIG_BUILTIN=y
+CONFIG_NSH_BUILTIN_APPS=y
+CONFIG_NSH_FILEIOSIZE=64
+CONFIG_NSH_STRERROR=n
+CONFIG_NSH_LINELEN=64
+CONFIG_NSH_NESTDEPTH=1
+CONFIG_NSH_DISABLESCRIPT=y
+CONFIG_NSH_DISABLEBG=n
+CONFIG_NSH_ROMFSETC=n
+CONFIG_NSH_CONSOLE=y
+CONFIG_NSH_TELNET=n
+CONFIG_NSH_ARCHINIT=n
+CONFIG_NSH_IOBUFFER_SIZE=256
+CONFIG_NSH_STACKSIZE=1024
+CONFIG_NSH_DHCPC=n
+CONFIG_NSH_NOMAC=n
+CONFIG_NSH_IPADDR=(10<<24|0<<16|0<<8|2)
+CONFIG_NSH_DRIPADDR=(10<<24|0<<16|0<<8|1)
+CONFIG_NSH_NETMASK=(255<<24|255<<16|255<<8|0)
+CONFIG_NSH_ROMFSMOUNTPT="/etc"
+CONFIG_NSH_INITSCRIPT="init.d/rcS"
+CONFIG_NSH_ROMFSDEVNO=0
+CONFIG_NSH_ROMFSSECTSIZE=64
+CONFIG_NSH_FATDEVNO=1
+CONFIG_NSH_FATSECTSIZE=512
+CONFIG_NSH_FATNSECTORS=1024
+CONFIG_NSH_FATMOUNTPT=/tmp
+
+#
+# Architecture-specific NSH options
+#
+CONFIG_NSH_MMCSDSPIPORTNO=0
+CONFIG_NSH_MMCSDSLOTNO=0
+CONFIG_NSH_MMCSDMINOR=0
+
+#
+# Stack and heap information
+#
+# CONFIG_BOOT_RUNFROMFLASH - Some configurations support XIP
+# operation from FLASH but must copy initialized .data sections to RAM.
+# (should also be =n for the STM3210E-EVAL which always runs from flash)
+# CONFIG_BOOT_COPYTORAM - Some configurations boot in FLASH
+# but copy themselves entirely into RAM for better performance.
+# CONFIG_CUSTOM_STACK - The up_ implementation will handle
+# all stack operations outside of the nuttx model.
+# CONFIG_STACK_POINTER - The initial stack pointer (arm7tdmi only)
+# CONFIG_IDLETHREAD_STACKSIZE - The size of the initial stack.
+# This is the thread that (1) performs the inital boot of the system up
+# to the point where user_start() is spawned, and (2) there after is the
+# IDLE thread that executes only when there is no other thread ready to
+# run.
+# CONFIG_USERMAIN_STACKSIZE - The size of the stack to allocate
+# for the main user thread that begins at the user_start() entry point.
+# CONFIG_PTHREAD_STACK_MIN - Minimum pthread stack size
+# CONFIG_PTHREAD_STACK_DEFAULT - Default pthread stack size
+# CONFIG_HEAP_BASE - The beginning of the heap
+# CONFIG_HEAP_SIZE - The size of the heap
+#
+CONFIG_BOOT_RUNFROMFLASH=n
+CONFIG_BOOT_COPYTORAM=n
+CONFIG_CUSTOM_STACK=n
+CONFIG_STACK_POINTER=
+CONFIG_IDLETHREAD_STACKSIZE=800
+CONFIG_USERMAIN_STACKSIZE=1024
+CONFIG_PTHREAD_STACK_MIN=256
+CONFIG_PTHREAD_STACK_DEFAULT=512
+CONFIG_HEAP_BASE=
+CONFIG_HEAP_SIZE=
diff --git a/nuttx/configs/px4iov2/nsh/setenv.sh b/nuttx/configs/px4iov2/nsh/setenv.sh
new file mode 100755
index 000000000..d83685192
--- /dev/null
+++ b/nuttx/configs/px4iov2/nsh/setenv.sh
@@ -0,0 +1,47 @@
+#!/bin/bash
+# configs/stm3210e-eval/dfu/setenv.sh
+#
+# Copyright (C) 2009 Gregory Nutt. All rights reserved.
+# Author: Gregory Nutt <spudmonkey@racsa.co.cr>
+#
+# 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 [ "$(basename $0)" = "setenv.sh" ] ; then
+ echo "You must source this script, not run it!" 1>&2
+ exit 1
+fi
+
+if [ -z "${PATH_ORIG}" ]; then export PATH_ORIG="${PATH}"; fi
+
+WD=`pwd`
+export RIDE_BIN="/cygdrive/c/Program Files/Raisonance/Ride/arm-gcc/bin"
+export BUILDROOT_BIN="${WD}/../buildroot/build_arm_nofpu/staging_dir/bin"
+export PATH="${BUILDROOT_BIN}:${RIDE_BIN}:/sbin:/usr/sbin:${PATH_ORIG}"
+
+echo "PATH : ${PATH}"
diff --git a/nuttx/configs/px4iov2/src/Makefile b/nuttx/configs/px4iov2/src/Makefile
new file mode 100644
index 000000000..bb9539d16
--- /dev/null
+++ b/nuttx/configs/px4iov2/src/Makefile
@@ -0,0 +1,84 @@
+############################################################################
+# configs/stm3210e-eval/src/Makefile
+#
+# Copyright (C) 2009-2010 Gregory Nutt. All rights reserved.
+# Author: Gregory Nutt <spudmonkey@racsa.co.cr>
+#
+# 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)
+ @$(MKDEP) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep
+ @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/px4iov2/src/README.txt b/nuttx/configs/px4iov2/src/README.txt
new file mode 100644
index 000000000..d4eda82fd
--- /dev/null
+++ b/nuttx/configs/px4iov2/src/README.txt
@@ -0,0 +1 @@
+This directory contains drivers unique to the STMicro STM3210E-EVAL development board.
diff --git a/nuttx/configs/px4iov2/src/empty.c b/nuttx/configs/px4iov2/src/empty.c
new file mode 100644
index 000000000..ace900866
--- /dev/null
+++ b/nuttx/configs/px4iov2/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/nuttx/drivers/mtd/ramtron.c b/nuttx/drivers/mtd/ramtron.c
index 34273bccf..45aff59cc 100644
--- a/nuttx/drivers/mtd/ramtron.c
+++ b/nuttx/drivers/mtd/ramtron.c
@@ -162,6 +162,14 @@ struct ramtron_dev_s
static struct ramtron_parts_s ramtron_parts[] =
{
{
+ "FM25V01", /* name */
+ 0x21, /* id1 */
+ 0x00, /* id2 */
+ 16L*1024L, /* size */
+ 2, /* addr_len */
+ 40000000 /* speed */
+ },
+ {
"FM25V02", /* name */
0x22, /* id1 */
0x00, /* id2 */
diff --git a/nuttx/drivers/serial/Kconfig b/nuttx/drivers/serial/Kconfig
index 119923a69..e91246612 100644
--- a/nuttx/drivers/serial/Kconfig
+++ b/nuttx/drivers/serial/Kconfig
@@ -292,6 +292,10 @@ config ARCH_HAVE_UART5
bool
config ARCH_HAVE_UART6
bool
+config ARCH_HAVE_UART7
+ bool
+config ARCH_HAVE_UART8
+ bool
config ARCH_HAVE_USART0
bool
@@ -307,6 +311,10 @@ config ARCH_HAVE_USART5
bool
config ARCH_HAVE_USART6
bool
+config ARCH_HAVE_USART7
+ bool
+config ARCH_HAVE_USART8
+ bool
config MCU_SERIAL
bool
@@ -403,6 +411,22 @@ config USART6_SERIAL_CONSOLE
bool "USART6"
depends on ARCH_HAVE_USART6
+config UART7_SERIAL_CONSOLE
+ bool "UART7"
+ depends on ARCH_HAVE_UART7
+
+config USART7_SERIAL_CONSOLE
+ bool "USART7"
+ depends on ARCH_HAVE_USART7
+
+config UART8_SERIAL_CONSOLE
+ bool "UART8"
+ depends on ARCH_HAVE_UART8
+
+config USART8_SERIAL_CONSOLE
+ bool "USART8"
+ depends on ARCH_HAVE_USART8
+
config NO_SERIAL_CONSOLE
bool "No serial console"
@@ -1052,3 +1076,173 @@ config UART6_2STOP
1=Two stop bits
endmenu
+
+menu "USART7 Configuration"
+ depends on ARCH_HAVE_USART7
+
+config USART7_RXBUFSIZE
+ int "Receive buffer size"
+ default 256
+ help
+ Characters are buffered as they are received. This specifies
+ the size of the receive buffer.
+
+config USART7_TXBUFSIZE
+ int "Transmit buffer size"
+ default 256
+ help
+ Characters are buffered before being sent. This specifies
+ the size of the transmit buffer.
+
+config USART7_BAUD
+ int "BAUD rate"
+ default 115200
+ help
+ The configured BAUD of the USART.
+
+config USART7_BITS
+ int "Character size"
+ default 8
+ help
+ The number of bits. Must be either 7 or 8.
+
+config USART7_PARITY
+ int "Parity setting"
+ default 0
+ help
+ 0=no parity, 1=odd parity, 2=even parity
+
+config USART7_2STOP
+ int "Uses 2 stop bits"
+ default 0
+ help
+ 1=Two stop bits
+
+endmenu
+
+menu "UART7 Configuration"
+ depends on ARCH_HAVE_UART7
+
+config UART7_RXBUFSIZE
+ int "Receive buffer size"
+ default 256
+ help
+ Characters are buffered as they are received. This specifies
+ the size of the receive buffer.
+
+config UART7_TXBUFSIZE
+ int "Transmit buffer size"
+ default 256
+ help
+ Characters are buffered before being sent. This specifies
+ the size of the transmit buffer.
+
+config UART7_BAUD
+ int "BAUD rate"
+ default 115200
+ help
+ The configured BAUD of the UART.
+
+config UART7_BITS
+ int "Character size"
+ default 8
+ help
+ The number of bits. Must be either 7 or 8.
+
+config UART7_PARITY
+ int "Parity setting"
+ default 0
+ help
+ 0=no parity, 1=odd parity, 2=even parity
+
+config UART7_2STOP
+ int "Uses 2 stop bits"
+ default 0
+ help
+ 1=Two stop bits
+
+menu "USART8 Configuration"
+ depends on ARCH_HAVE_USART8
+
+config USART8_RXBUFSIZE
+ int "Receive buffer size"
+ default 256
+ help
+ Characters are buffered as they are received. This specifies
+ the size of the receive buffer.
+
+config USART8_TXBUFSIZE
+ int "Transmit buffer size"
+ default 256
+ help
+ Characters are buffered before being sent. This specifies
+ the size of the transmit buffer.
+
+config USART8_BAUD
+ int "BAUD rate"
+ default 115200
+ help
+ The configured BAUD of the USART.
+
+config USART8_BITS
+ int "Character size"
+ default 8
+ help
+ The number of bits. Must be either 7 or 8.
+
+config USART8_PARITY
+ int "Parity setting"
+ default 0
+ help
+ 0=no parity, 1=odd parity, 2=even parity
+
+config USART8_2STOP
+ int "Uses 2 stop bits"
+ default 0
+ help
+ 1=Two stop bits
+
+endmenu
+
+menu "UART8 Configuration"
+ depends on ARCH_HAVE_UART8
+
+config UART8_RXBUFSIZE
+ int "Receive buffer size"
+ default 256
+ help
+ Characters are buffered as they are received. This specifies
+ the size of the receive buffer.
+
+config UART8_TXBUFSIZE
+ int "Transmit buffer size"
+ default 256
+ help
+ Characters are buffered before being sent. This specifies
+ the size of the transmit buffer.
+
+config UART8_BAUD
+ int "BAUD rate"
+ default 115200
+ help
+ The configured BAUD of the UART.
+
+config UART8_BITS
+ int "Character size"
+ default 8
+ help
+ The number of bits. Must be either 7 or 8.
+
+config UART8_PARITY
+ int "Parity setting"
+ default 0
+ help
+ 0=no parity, 1=odd parity, 2=even parity
+
+config UART8_2STOP
+ int "Uses 2 stop bits"
+ default 0
+ help
+ 1=Two stop bits
+
+endmenu
diff --git a/src/drivers/boards/px4fmuv2/module.mk b/src/drivers/boards/px4fmuv2/module.mk
new file mode 100644
index 000000000..eb8841e4d
--- /dev/null
+++ b/src/drivers/boards/px4fmuv2/module.mk
@@ -0,0 +1,9 @@
+#
+# Board-specific startup code for the PX4FMUv2
+#
+
+SRCS = px4fmu_can.c \
+ px4fmu_init.c \
+ px4fmu_pwm_servo.c \
+ px4fmu_spi.c \
+ px4fmu_usb.c
diff --git a/src/drivers/boards/px4fmuv2/px4fmu_can.c b/src/drivers/boards/px4fmuv2/px4fmu_can.c
new file mode 100644
index 000000000..86ba183b8
--- /dev/null
+++ b/src/drivers/boards/px4fmuv2/px4fmu_can.c
@@ -0,0 +1,144 @@
+/****************************************************************************
+ *
+ * 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 px4fmu_can.c
+ *
+ * Board-specific CAN functions.
+ */
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <errno.h>
+#include <debug.h>
+
+#include <nuttx/can.h>
+#include <arch/board/board.h>
+
+#include "chip.h"
+#include "up_arch.h"
+
+#include "stm32.h"
+#include "stm32_can.h"
+#include "px4fmu_internal.h"
+
+#ifdef CONFIG_CAN
+
+/************************************************************************************
+ * Pre-processor Definitions
+ ************************************************************************************/
+/* Configuration ********************************************************************/
+
+#if defined(CONFIG_STM32_CAN1) && defined(CONFIG_STM32_CAN2)
+# warning "Both CAN1 and CAN2 are enabled. Assuming only CAN1."
+# undef CONFIG_STM32_CAN2
+#endif
+
+#ifdef CONFIG_STM32_CAN1
+# define CAN_PORT 1
+#else
+# define CAN_PORT 2
+#endif
+
+/* Debug ***************************************************************************/
+/* Non-standard debug that may be enabled just for testing CAN */
+
+#ifdef CONFIG_DEBUG_CAN
+# define candbg dbg
+# define canvdbg vdbg
+# define canlldbg lldbg
+# define canllvdbg llvdbg
+#else
+# define candbg(x...)
+# define canvdbg(x...)
+# define canlldbg(x...)
+# define canllvdbg(x...)
+#endif
+
+/************************************************************************************
+ * Private Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Public Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Name: can_devinit
+ *
+ * Description:
+ * All STM32 architectures must provide the following interface to work with
+ * examples/can.
+ *
+ ************************************************************************************/
+
+int can_devinit(void)
+{
+ static bool initialized = false;
+ struct can_dev_s *can;
+ int ret;
+
+ /* Check if we have already initialized */
+
+ if (!initialized) {
+ /* Call stm32_caninitialize() to get an instance of the CAN interface */
+
+ can = stm32_caninitialize(CAN_PORT);
+
+ if (can == NULL) {
+ candbg("ERROR: Failed to get CAN interface\n");
+ return -ENODEV;
+ }
+
+ /* Register the CAN driver at "/dev/can0" */
+
+ ret = can_register("/dev/can0", can);
+
+ if (ret < 0) {
+ candbg("ERROR: can_register failed: %d\n", ret);
+ return ret;
+ }
+
+ /* Now we are initialized */
+
+ initialized = true;
+ }
+
+ return OK;
+}
+
+#endif \ No newline at end of file
diff --git a/src/drivers/boards/px4fmuv2/px4fmu_init.c b/src/drivers/boards/px4fmuv2/px4fmu_init.c
new file mode 100644
index 000000000..03ec5a255
--- /dev/null
+++ b/src/drivers/boards/px4fmuv2/px4fmu_init.c
@@ -0,0 +1,255 @@
+/****************************************************************************
+ *
+ * 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 px4fmu_init.c
+ *
+ * PX4FMU-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 "stm32_internal.h"
+#include "px4fmu_internal.h"
+#include "stm32_uart.h"
+
+#include <arch/board/board.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_led.h>
+
+#include <systemlib/cpuload.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 SPI interfaces */
+ stm32_spiinitialize();
+
+ /* configure LEDs */
+ up_ledinit();
+}
+
+/****************************************************************************
+ * Name: nsh_archinitialize
+ *
+ * Description:
+ * Perform architecture specific initialization
+ *
+ ****************************************************************************/
+
+static struct spi_dev_s *spi1;
+static struct spi_dev_s *spi2;
+static struct sdio_dev_s *sdio;
+
+#include <math.h>
+
+#ifdef __cplusplus
+__EXPORT int matherr(struct __exception *e)
+{
+ return 1;
+}
+#else
+__EXPORT int matherr(struct exception *e)
+{
+ return 1;
+}
+#endif
+
+__EXPORT int nsh_archinitialize(void)
+{
+
+ /* configure ADC pins */
+ stm32_configgpio(GPIO_ADC1_IN10);
+ stm32_configgpio(GPIO_ADC1_IN11);
+ stm32_configgpio(GPIO_ADC1_IN12);
+
+ /* configure power supply control pins */
+ stm32_configgpio(GPIO_VDD_5V_PERIPH_EN);
+ stm32_configgpio(GPIO_VDD_2V8_SENSORS_EN);
+ stm32_configgpio(GPIO_VDD_5V_HIPOWER_OC);
+ stm32_configgpio(GPIO_VDD_5V_PERIPH_OC);
+
+ /* 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);
+
+ // initial LED state
+ // XXX need to make this work again
+// drv_led_start();
+ up_ledoff(LED_AMBER);
+
+ /* Configure SPI-based devices */
+
+ spi1 = up_spiinitialize(1);
+
+ if (!spi1) {
+ message("[boot] FAILED to initialize SPI port 1\n");
+ up_ledon(LED_AMBER);
+ return -ENODEV;
+ }
+
+ /* Default SPI1 to 1MHz and de-assert the known chip selects. */
+ SPI_SETFREQUENCY(spi1, 10000000);
+ SPI_SETBITS(spi1, 8);
+ SPI_SETMODE(spi1, SPIDEV_MODE3);
+ SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false);
+ SPI_SELECT(spi1, PX4_SPIDEV_ACCEL_MAG, false);
+ SPI_SELECT(spi1, PX4_SPIDEV_BARO, false);
+ up_udelay(20);
+
+ message("[boot] Successfully initialized SPI port 1\n");
+
+ /* Get the SPI port for the FRAM */
+
+ spi2 = up_spiinitialize(2);
+
+ if (!spi2) {
+ message("[boot] FAILED to initialize SPI port 2\n");
+ up_ledon(LED_AMBER);
+ return -ENODEV;
+ }
+
+ /* Default SPI2 to 37.5 MHz (F4 max) and de-assert the known chip selects. */
+ SPI_SETFREQUENCY(spi2, 375000000);
+ SPI_SETBITS(spi2, 8);
+ SPI_SETMODE(spi2, SPIDEV_MODE3);
+ SPI_SELECT(spi2, SPIDEV_FLASH, false);
+
+ message("[boot] Successfully initialized SPI port 2\n");
+
+ #ifdef CONFIG_MMCSD
+ /* First, get an instance of the SDIO interface */
+
+ sdio = sdio_initialize(CONFIG_NSH_MMCSDSLOTNO);
+ if (!sdio) {
+ message("nsh_archinitialize: Failed to initialize SDIO slot %d\n",
+ CONFIG_NSH_MMCSDSLOTNO);
+ return -ENODEV;
+ }
+
+ /* Now bind the SDIO interface to the MMC/SD driver */
+ int ret = mmcsd_slotinitialize(CONFIG_NSH_MMCSDMINOR, sdio);
+ if (ret != OK) {
+ message("nsh_archinitialize: Failed to bind SDIO to the MMC/SD driver: %d\n", ret);
+ return ret;
+ }
+
+ /* Then let's guess and say that there is a card in the slot. There is no card detect GPIO. */
+ sdio_mediachange(sdio, true);
+
+ message("[boot] Initialized SDIO\n");
+ #endif
+
+ return OK;
+}
diff --git a/src/drivers/boards/px4fmuv2/px4fmu_internal.h b/src/drivers/boards/px4fmuv2/px4fmu_internal.h
new file mode 100644
index 000000000..78f6a2e65
--- /dev/null
+++ b/src/drivers/boards/px4fmuv2/px4fmu_internal.h
@@ -0,0 +1,129 @@
+/****************************************************************************
+ *
+ * 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 px4fmu_internal.h
+ *
+ * PX4FMU 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_internal.h>
+
+
+/****************************************************************************************************
+ * Definitions
+ ****************************************************************************************************/
+/* Configuration ************************************************************************************/
+
+/* PX4FMU GPIOs ***********************************************************************************/
+/* LEDs */
+
+#define GPIO_LED1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN12)
+
+/* External interrupts */
+
+/* SPI chip selects */
+#define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)
+#define GPIO_SPI_CS_ACCEL_MAG (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15)
+#define GPIO_SPI_CS_BARO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7)
+#define GPIO_SPI_CS_FRAM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10)
+
+/* User GPIOs
+ *
+ * GPIO0-5 are the PWM servo outputs.
+ */
+#define GPIO_GPIO0_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN14)
+#define GPIO_GPIO1_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN13)
+#define GPIO_GPIO2_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN11)
+#define GPIO_GPIO3_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN9)
+#define GPIO_GPIO4_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN13)
+#define GPIO_GPIO5_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN14)
+#define GPIO_GPIO0_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN14)
+#define GPIO_GPIO1_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN13)
+#define GPIO_GPIO2_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN11)
+#define GPIO_GPIO3_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN9)
+#define GPIO_GPIO4_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13)
+#define GPIO_GPIO5_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14)
+
+/* Power supply control and monitoring GPIOs */
+#define GPIO_VDD_5V_PERIPH_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN8)
+#define GPIO_VDD_2V8_SENSORS_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3)
+#define GPIO_VDD_5V_HIPOWER_OC (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN10)
+#define GPIO_VDD_5V_PERIPH_OC (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN15)
+
+/* USB OTG FS
+ *
+ * PA9 OTG_FS_VBUS VBUS sensing (also connected to the green LED)
+ */
+#define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9)
+
+/****************************************************************************************************
+ * 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);
+
+#endif /* __ASSEMBLY__ */
+
+__END_DECLS
diff --git a/src/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c b/src/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c
new file mode 100644
index 000000000..14e4052be
--- /dev/null
+++ b/src/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c
@@ -0,0 +1,105 @@
+/****************************************************************************
+ *
+ * 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 px4fmu_pwm_servo.c
+ *
+ * Configuration data for the stm32 pwm_servo driver.
+ *
+ * Note that these arrays must always be fully-sized.
+ */
+
+#include <stdint.h>
+
+#include <drivers/stm32/drv_pwm_servo.h>
+
+#include <arch/board/board.h>
+#include <drivers/drv_pwm_output.h>
+
+#include <stm32_internal.h>
+#include <stm32_gpio.h>
+#include <stm32_tim.h>
+
+__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = {
+ {
+ .base = STM32_TIM1_BASE,
+ .clock_register = STM32_RCC_APB1ENR,
+ .clock_bit = RCC_APB2ENR_TIM1EN,
+ .clock_freq = STM32_APB2_TIM1_CLKIN
+ },
+ {
+ .base = STM32_TIM4_BASE,
+ .clock_register = STM32_RCC_APB1ENR,
+ .clock_bit = RCC_APB1ENR_TIM4EN,
+ .clock_freq = STM32_APB1_TIM4_CLKIN
+ }
+};
+
+__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = {
+ {
+ .gpio = GPIO_TIM1_CH4OUT,
+ .timer_index = 0,
+ .timer_channel = 4,
+ .default_value = 1000,
+ },
+ {
+ .gpio = GPIO_TIM1_CH3OUT,
+ .timer_index = 0,
+ .timer_channel = 3,
+ .default_value = 1000,
+ },
+ {
+ .gpio = GPIO_TIM1_CH2OUT,
+ .timer_index = 0,
+ .timer_channel = 2,
+ .default_value = 1000,
+ },
+ {
+ .gpio = GPIO_TIM1_CH1OUT,
+ .timer_index = 0,
+ .timer_channel = 1,
+ .default_value = 1000,
+ },
+ {
+ .gpio = GPIO_TIM4_CH2OUT,
+ .timer_index = 1,
+ .timer_channel = 2,
+ .default_value = 1000,
+ },
+ {
+ .gpio = GPIO_TIM4_CH3OUT,
+ .timer_index = 1,
+ .timer_channel = 3,
+ .default_value = 1000,
+ }
+};
diff --git a/src/drivers/boards/px4fmuv2/px4fmu_spi.c b/src/drivers/boards/px4fmuv2/px4fmu_spi.c
new file mode 100644
index 000000000..f90f25071
--- /dev/null
+++ b/src/drivers/boards/px4fmuv2/px4fmu_spi.c
@@ -0,0 +1,141 @@
+/****************************************************************************
+ *
+ * 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 px4fmu_spi.c
+ *
+ * Board-specific SPI functions.
+ */
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <debug.h>
+
+#include <nuttx/spi.h>
+#include <arch/board/board.h>
+
+#include "up_arch.h"
+#include "chip.h"
+#include "stm32_internal.h"
+#include "px4fmu_internal.h"
+
+/************************************************************************************
+ * Public Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Name: stm32_spiinitialize
+ *
+ * Description:
+ * Called to configure SPI chip select GPIO pins for the PX4FMU board.
+ *
+ ************************************************************************************/
+
+__EXPORT void weak_function stm32_spiinitialize(void)
+{
+#ifdef CONFIG_STM32_SPI1
+ stm32_configgpio(GPIO_SPI_CS_GYRO);
+ stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG);
+ stm32_configgpio(GPIO_SPI_CS_BARO);
+
+ /* De-activate all peripherals,
+ * required for some peripheral
+ * state machines
+ */
+ stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+ stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
+ stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
+#endif
+
+#ifdef CONFIG_STM32_SPI2
+ stm32_configgpio(GPIO_SPI_CS_FRAM);
+ stm32_gpiowrite(GPIO_SPI_CS_FRAM, 1);
+#endif
+}
+
+__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
+{
+ /* SPI select is active low, so write !selected to select the device */
+
+ switch (devid) {
+ case PX4_SPIDEV_GYRO:
+ /* Making sure the other peripherals are not selected */
+ stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected);
+ stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
+ stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
+ break;
+
+ case PX4_SPIDEV_ACCEL_MAG:
+ /* Making sure the other peripherals are not selected */
+ stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+ stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, !selected);
+ stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
+ break;
+
+ case PX4_SPIDEV_BARO:
+ /* Making sure the other peripherals are not selected */
+ stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+ stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
+ stm32_gpiowrite(GPIO_SPI_CS_BARO, !selected);
+ break;
+
+ default:
+ break;
+ }
+}
+
+__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
+{
+ return SPI_STATUS_PRESENT;
+}
+
+
+#ifdef CONFIG_STM32_SPI2
+__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
+{
+ /* there can only be one device on this bus, so always select it */
+ stm32_gpiowrite(GPIO_SPI_CS_FRAM, !selected);
+}
+
+__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
+{
+ /* FRAM is always present */
+ return SPI_STATUS_PRESENT;
+}
+#endif
diff --git a/src/drivers/boards/px4fmuv2/px4fmu_usb.c b/src/drivers/boards/px4fmuv2/px4fmu_usb.c
new file mode 100644
index 000000000..b0b669fbe
--- /dev/null
+++ b/src/drivers/boards/px4fmuv2/px4fmu_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 px4fmu_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_internal.h"
+#include "px4fmu_internal.h"
+
+/************************************************************************************
+ * Definitions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Private Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Public Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Name: stm32_usbinitialize
+ *
+ * Description:
+ * Called to setup USB-related GPIO pins for the PX4FMU 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/boards/px4iov2/module.mk b/src/drivers/boards/px4iov2/module.mk
new file mode 100644
index 000000000..85f94e8be
--- /dev/null
+++ b/src/drivers/boards/px4iov2/module.mk
@@ -0,0 +1,6 @@
+#
+# Board-specific startup code for the PX4IOv2
+#
+
+SRCS = px4iov2_init.c \
+ px4iov2_pwm_servo.c
diff --git a/src/drivers/boards/px4iov2/px4iov2_init.c b/src/drivers/boards/px4iov2/px4iov2_init.c
new file mode 100644
index 000000000..09469d7e8
--- /dev/null
+++ b/src/drivers/boards/px4iov2/px4iov2_init.c
@@ -0,0 +1,171 @@
+/****************************************************************************
+ *
+ * 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 px4iov2_init.c
+ *
+ * PX4FMU-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 "stm32_internal.h"
+#include "px4iov2_internal.h"
+
+#include <arch/board/board.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 GPIOs */
+
+ /* turn off - all leds are active low */
+ stm32_gpiowrite(GPIO_LED1, true);
+ stm32_gpiowrite(GPIO_LED2, true);
+ stm32_gpiowrite(GPIO_LED3, true);
+ stm32_configgpio(GPIO_LED1);
+ stm32_configgpio(GPIO_LED2);
+ stm32_configgpio(GPIO_LED3);
+
+
+ stm32_configgpio(GPIO_BTN_SAFETY);
+
+ /* spektrum power enable is active high - disable it by default */
+ stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, false);
+ stm32_configgpio(GPIO_SPEKTRUM_PWR_EN);
+
+ /* servo power enable is active low, and has a pull down resistor
+ * to keep it low during boot (since it may power the whole board.)
+ */
+ stm32_gpiowrite(GPIO_SERVO_PWR_EN, false);
+ stm32_configgpio(GPIO_SERVO_PWR_EN);
+
+ stm32_configgpio(GPIO_SERVO_FAULT_DETECT);
+
+ stm32_configgpio(GPIO_TIM_RSSI); /* xxx alternate function */
+ stm32_configgpio(GPIO_ADC_RSSI);
+ stm32_configgpio(GPIO_ADC_VSERVO);
+
+ stm32_configgpio(GPIO_SBUS_INPUT); /* xxx alternate function */
+
+ stm32_gpiowrite(GPIO_SBUS_OUTPUT, false);
+ stm32_configgpio(GPIO_SBUS_OUTPUT);
+
+ /* sbus output enable is active low - disable it by default */
+ stm32_gpiowrite(GPIO_SBUS_OENABLE, true);
+ stm32_configgpio(GPIO_SBUS_OENABLE);
+
+
+ stm32_configgpio(GPIO_PPM); /* xxx alternate function */
+
+ stm32_gpiowrite(GPIO_PWM1, false);
+ stm32_configgpio(GPIO_PWM1);
+
+ stm32_gpiowrite(GPIO_PWM2, false);
+ stm32_configgpio(GPIO_PWM2);
+
+ stm32_gpiowrite(GPIO_PWM3, false);
+ stm32_configgpio(GPIO_PWM3);
+
+ stm32_gpiowrite(GPIO_PWM4, false);
+ stm32_configgpio(GPIO_PWM4);
+
+ stm32_gpiowrite(GPIO_PWM5, false);
+ stm32_configgpio(GPIO_PWM5);
+
+ stm32_gpiowrite(GPIO_PWM6, false);
+ stm32_configgpio(GPIO_PWM6);
+
+ stm32_gpiowrite(GPIO_PWM7, false);
+ stm32_configgpio(GPIO_PWM7);
+
+ stm32_gpiowrite(GPIO_PWM8, false);
+ stm32_configgpio(GPIO_PWM8);
+
+// message("[boot] Successfully initialized px4iov2 gpios\n");
+}
diff --git a/src/drivers/boards/px4iov2/px4iov2_internal.h b/src/drivers/boards/px4iov2/px4iov2_internal.h
new file mode 100755
index 000000000..c4c592fe4
--- /dev/null
+++ b/src/drivers/boards/px4iov2/px4iov2_internal.h
@@ -0,0 +1,112 @@
+/****************************************************************************
+ *
+ * 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 px4iov2_internal.h
+ *
+ * PX4IOV2 internal definitions
+ */
+
+#pragma once
+
+/******************************************************************************
+ * Included Files
+ ******************************************************************************/
+
+#include <nuttx/config.h>
+#include <nuttx/compiler.h>
+#include <stdint.h>
+
+/* these headers are not C++ safe */
+#include <stm32_internal.h>
+
+
+/******************************************************************************
+ * Definitions
+ ******************************************************************************/
+/* Configuration **************************************************************/
+
+/******************************************************************************
+ * GPIOS
+ ******************************************************************************/
+
+/* LEDS **********************************************************************/
+
+#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN14)
+#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN15)
+#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN13)
+
+#define GPIO_LED_BLUE BOARD_GPIO_LED1
+#define GPIO_LED_AMBER BOARD_GPIO_LED2
+#define GPIO_LED_SAFETY BOARD_GPIO_LED3
+
+/* Safety switch button *******************************************************/
+
+#define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN5)
+
+/* Power switch controls ******************************************************/
+
+#define GPIO_SPEKTRUM_PWR_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13)
+
+#define GPIO_SERVO_PWR_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN15)
+
+#define GPIO_SERVO_FAULT_DETECT (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN13)
+
+
+/* Analog inputs **************************************************************/
+
+#define GPIO_ADC_VSERVO (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN4)
+/* the same rssi signal goes to both an adc and a timer input */
+#define GPIO_ADC_RSSI (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN5)
+/* floating pin */
+#define GPIO_TIM_RSSI (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN12)
+
+/* PWM pins **************************************************************/
+
+#define GPIO_PPM (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN8)
+
+#define GPIO_PWM1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0)
+#define GPIO_PWM2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN1)
+#define GPIO_PWM3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN8)
+#define GPIO_PWM4 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN9)
+#define GPIO_PWM5 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN6)
+#define GPIO_PWM6 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN7)
+#define GPIO_PWM7 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN0)
+#define GPIO_PWM8 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN1)
+
+/* SBUS pins *************************************************************/
+
+#define GPIO_SBUS_INPUT (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN11)
+#define GPIO_SBUS_OUTPUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
+#define GPIO_SBUS_OENABLE (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN4)
+
diff --git a/src/drivers/boards/px4iov2/px4iov2_pwm_servo.c b/src/drivers/boards/px4iov2/px4iov2_pwm_servo.c
new file mode 100644
index 000000000..5e1aafa49
--- /dev/null
+++ b/src/drivers/boards/px4iov2/px4iov2_pwm_servo.c
@@ -0,0 +1,123 @@
+/****************************************************************************
+ *
+ * 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 px4iov2_pwm_servo.c
+ *
+ * Configuration data for the stm32 pwm_servo driver.
+ *
+ * Note that these arrays must always be fully-sized.
+ */
+
+#include <stdint.h>
+
+#include <drivers/stm32/drv_pwm_servo.h>
+
+#include <arch/board/board.h>
+#include <drivers/drv_pwm_output.h>
+
+#include <stm32_internal.h>
+#include <stm32_gpio.h>
+#include <stm32_tim.h>
+
+__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = {
+ {
+ .base = STM32_TIM2_BASE,
+ .clock_register = STM32_RCC_APB1ENR,
+ .clock_bit = RCC_APB1ENR_TIM2EN,
+ .clock_freq = STM32_APB1_TIM2_CLKIN
+ },
+ {
+ .base = STM32_TIM3_BASE,
+ .clock_register = STM32_RCC_APB1ENR,
+ .clock_bit = RCC_APB1ENR_TIM3EN,
+ .clock_freq = STM32_APB1_TIM3_CLKIN
+ },
+ {
+ .base = STM32_TIM4_BASE,
+ .clock_register = STM32_RCC_APB1ENR,
+ .clock_bit = RCC_APB1ENR_TIM4EN,
+ .clock_freq = STM32_APB1_TIM4_CLKIN
+ }
+};
+
+__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = {
+ {
+ .gpio = GPIO_TIM2_CH1OUT,
+ .timer_index = 0,
+ .timer_channel = 1,
+ .default_value = 1000,
+ },
+ {
+ .gpio = GPIO_TIM2_CH2OUT,
+ .timer_index = 0,
+ .timer_channel = 2,
+ .default_value = 1000,
+ },
+ {
+ .gpio = GPIO_TIM4_CH3OUT,
+ .timer_index = 2,
+ .timer_channel = 3,
+ .default_value = 1000,
+ },
+ {
+ .gpio = GPIO_TIM4_CH4OUT,
+ .timer_index = 2,
+ .timer_channel = 4,
+ .default_value = 1000,
+ },
+ {
+ .gpio = GPIO_TIM3_CH1OUT,
+ .timer_index = 1,
+ .timer_channel = 1,
+ .default_value = 1000,
+ },
+ {
+ .gpio = GPIO_TIM3_CH2OUT,
+ .timer_index = 1,
+ .timer_channel = 2,
+ .default_value = 1000,
+ },
+ {
+ .gpio = GPIO_TIM3_CH3OUT,
+ .timer_index = 1,
+ .timer_channel = 3,
+ .default_value = 1000,
+ },
+ {
+ .gpio = GPIO_TIM3_CH4OUT,
+ .timer_index = 1,
+ .timer_channel = 4,
+ .default_value = 1000,
+ }
+};
diff --git a/src/drivers/drv_gpio.h b/src/drivers/drv_gpio.h
index 92d184326..a4c59d218 100644
--- a/src/drivers/drv_gpio.h
+++ b/src/drivers/drv_gpio.h
@@ -67,6 +67,32 @@
#endif
+#ifdef CONFIG_ARCH_BOARD_PX4FMUV2
+/*
+ * PX4FMUv2 GPIO numbers.
+ *
+ * There are no alternate functions on this board.
+ */
+# define GPIO_SERVO_1 (1<<0) /**< servo 1 output */
+# define GPIO_SERVO_2 (1<<1) /**< servo 2 output */
+# define GPIO_SERVO_3 (1<<2) /**< servo 3 output */
+# define GPIO_SERVO_4 (1<<3) /**< servo 4 output */
+# define GPIO_SERVO_5 (1<<4) /**< servo 5 output */
+# define GPIO_SERVO_6 (1<<5) /**< servo 6 output */
+
+# define GPIO_5V_PERIPH_EN (1<<6) /**< PA8 - VDD_5V_PERIPH_EN */
+# define GPIO_5V_HIPOWER_OC (1<<7) /**< PE10 - !VDD_5V_HIPOWER_OC */
+# define GPIO_5V_PERIPH_OC (1<<8) /**< PE15 - !VDD_5V_PERIPH_OC */
+
+/**
+ * Default GPIO device - other devices may also support this protocol if
+ * they also export GPIO-like things. This is always the GPIOs on the
+ * main board.
+ */
+# define GPIO_DEVICE_PATH "/dev/px4fmu"
+
+#endif
+
#ifdef CONFIG_ARCH_BOARD_PX4IO
/*
* PX4IO GPIO numbers.
diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
new file mode 100644
index 000000000..ba7316e55
--- /dev/null
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -0,0 +1,1470 @@
+/****************************************************************************
+ *
+ * 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 lsm303d.cpp
+ * Driver for the ST LSM303D MEMS accelerometer / magnetometer connected via SPI.
+ */
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <stddef.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 <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+
+#include <nuttx/arch.h>
+#include <nuttx/clock.h>
+
+#include <drivers/drv_hrt.h>
+#include <arch/board/board.h>
+
+#include <drivers/device/spi.h>
+#include <drivers/drv_accel.h>
+#include <drivers/drv_mag.h>
+
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+/* SPI protocol address bits */
+#define DIR_READ (1<<7)
+#define DIR_WRITE (0<<7)
+#define ADDR_INCREMENT (1<<6)
+
+
+
+/* register addresses: A: accel, M: mag, T: temp */
+#define ADDR_WHO_AM_I 0x0F
+#define WHO_I_AM 0x49
+
+#define ADDR_OUT_L_T 0x05
+#define ADDR_OUT_H_T 0x06
+#define ADDR_STATUS_M 0x07
+#define ADDR_OUT_X_L_M 0x08
+#define ADDR_OUT_X_H_M 0x09
+#define ADDR_OUT_Y_L_M 0x0A
+#define ADDR_OUT_Y_H_M 0x0B
+#define ADDR_OUT_Z_L_M 0x0C
+#define ADDR_OUT_Z_H_M 0x0D
+
+#define ADDR_OUT_TEMP_A 0x26
+#define ADDR_STATUS_A 0x27
+#define ADDR_OUT_X_L_A 0x28
+#define ADDR_OUT_X_H_A 0x29
+#define ADDR_OUT_Y_L_A 0x2A
+#define ADDR_OUT_Y_H_A 0x2B
+#define ADDR_OUT_Z_L_A 0x2C
+#define ADDR_OUT_Z_H_A 0x2D
+
+#define ADDR_CTRL_REG0 0x1F
+#define ADDR_CTRL_REG1 0x20
+#define ADDR_CTRL_REG2 0x21
+#define ADDR_CTRL_REG3 0x22
+#define ADDR_CTRL_REG4 0x23
+#define ADDR_CTRL_REG5 0x24
+#define ADDR_CTRL_REG6 0x25
+#define ADDR_CTRL_REG7 0x26
+
+#define REG1_RATE_BITS_A ((1<<7) | (1<<6) | (1<<5) | (1<<4))
+#define REG1_POWERDOWN_A ((0<<7) | (0<<6) | (0<<5) | (0<<4))
+#define REG1_RATE_3_125HZ_A ((0<<7) | (0<<6) | (0<<5) | (1<<4))
+#define REG1_RATE_6_25HZ_A ((0<<7) | (0<<6) | (1<<5) | (0<<4))
+#define REG1_RATE_12_5HZ_A ((0<<7) | (0<<6) | (1<<5) | (1<<4))
+#define REG1_RATE_25HZ_A ((0<<7) | (1<<6) | (0<<5) | (0<<4))
+#define REG1_RATE_50HZ_A ((0<<7) | (1<<6) | (0<<5) | (1<<4))
+#define REG1_RATE_100HZ_A ((0<<7) | (1<<6) | (1<<5) | (0<<4))
+#define REG1_RATE_200HZ_A ((0<<7) | (1<<6) | (1<<5) | (1<<4))
+#define REG1_RATE_400HZ_A ((1<<7) | (0<<6) | (0<<5) | (0<<4))
+#define REG1_RATE_800HZ_A ((1<<7) | (0<<6) | (0<<5) | (1<<4))
+#define REG1_RATE_1600HZ_A ((1<<7) | (0<<6) | (1<<5) | (0<<4))
+
+#define REG1_CONT_UPDATE_A (0<<3)
+#define REG1_Z_ENABLE_A (1<<2)
+#define REG1_Y_ENABLE_A (1<<1)
+#define REG1_X_ENABLE_A (1<<0)
+
+#define REG2_ANTIALIAS_FILTER_BW_BITS_A ((1<<7) | (1<<6))
+#define REG2_AA_FILTER_BW_773HZ_A ((0<<7) | (0<<6))
+#define REG2_AA_FILTER_BW_194HZ_A ((0<<7) | (1<<6))
+#define REG2_AA_FILTER_BW_362HZ_A ((1<<7) | (0<<6))
+#define REG2_AA_FILTER_BW_50HZ_A ((1<<7) | (1<<6))
+
+#define REG2_FULL_SCALE_BITS_A ((1<<5) | (1<<4) | (1<<3))
+#define REG2_FULL_SCALE_2G_A ((0<<5) | (0<<4) | (0<<3))
+#define REG2_FULL_SCALE_4G_A ((0<<5) | (0<<4) | (1<<3))
+#define REG2_FULL_SCALE_6G_A ((0<<5) | (1<<4) | (0<<3))
+#define REG2_FULL_SCALE_8G_A ((0<<5) | (1<<4) | (1<<3))
+#define REG2_FULL_SCALE_16G_A ((1<<5) | (0<<4) | (0<<3))
+
+#define REG5_ENABLE_T (1<<7)
+
+#define REG5_RES_HIGH_M ((1<<6) | (1<<5))
+#define REG5_RES_LOW_M ((0<<6) | (0<<5))
+
+#define REG5_RATE_BITS_M ((1<<4) | (1<<3) | (1<<2))
+#define REG5_RATE_3_125HZ_M ((0<<4) | (0<<3) | (0<<2))
+#define REG5_RATE_6_25HZ_M ((0<<4) | (0<<3) | (1<<2))
+#define REG5_RATE_12_5HZ_M ((0<<4) | (1<<3) | (0<<2))
+#define REG5_RATE_25HZ_M ((0<<4) | (1<<3) | (1<<2))
+#define REG5_RATE_50HZ_M ((1<<4) | (0<<3) | (0<<2))
+#define REG5_RATE_100HZ_M ((1<<4) | (0<<3) | (1<<2))
+#define REG5_RATE_DO_NOT_USE_M ((1<<4) | (1<<3) | (0<<2))
+
+#define REG6_FULL_SCALE_BITS_M ((1<<7) | (1<<6))
+#define REG6_FULL_SCALE_2GA_M ((0<<7) | (0<<6))
+#define REG6_FULL_SCALE_4GA_M ((0<<7) | (1<<6))
+#define REG6_FULL_SCALE_8GA_M ((1<<7) | (0<<6))
+#define REG6_FULL_SCALE_12GA_M ((1<<7) | (1<<6))
+
+#define REG7_CONT_MODE_M ((0<<1) | (0<<0))
+
+
+#define INT_CTRL_M 0x12
+#define INT_SRC_M 0x13
+
+
+extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); }
+
+
+class LSM303D_mag;
+
+class LSM303D : public device::SPI
+{
+public:
+ LSM303D(int bus, const char* path, spi_dev_e device);
+ virtual ~LSM303D();
+
+ 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:
+ virtual int probe();
+
+ friend class LSM303D_mag;
+
+ virtual ssize_t mag_read(struct file *filp, char *buffer, size_t buflen);
+ virtual int mag_ioctl(struct file *filp, int cmd, unsigned long arg);
+
+private:
+
+ LSM303D_mag *_mag;
+
+ struct hrt_call _accel_call;
+ struct hrt_call _mag_call;
+
+ unsigned _call_accel_interval;
+ unsigned _call_mag_interval;
+
+ unsigned _num_accel_reports;
+ volatile unsigned _next_accel_report;
+ volatile unsigned _oldest_accel_report;
+ struct accel_report *_accel_reports;
+
+ struct accel_scale _accel_scale;
+ float _accel_range_scale;
+ float _accel_range_m_s2;
+ orb_advert_t _accel_topic;
+
+ unsigned _num_mag_reports;
+ volatile unsigned _next_mag_report;
+ volatile unsigned _oldest_mag_report;
+ struct mag_report *_mag_reports;
+
+ struct mag_scale _mag_scale;
+ float _mag_range_scale;
+ float _mag_range_ga;
+ orb_advert_t _mag_topic;
+
+ perf_counter_t _accel_sample_perf;
+ perf_counter_t _mag_sample_perf;
+
+ /**
+ * Start automatic measurement.
+ */
+ void start();
+
+ /**
+ * Stop automatic measurement.
+ */
+ void stop();
+
+ /**
+ * Static trampoline from the hrt_call context; because we don't have a
+ * generic hrt wrapper yet.
+ *
+ * Called by the HRT in interrupt context at the specified rate if
+ * automatic polling is enabled.
+ *
+ * @param arg Instance pointer for the driver that is polling.
+ */
+ static void measure_trampoline(void *arg);
+
+ /**
+ * Static trampoline for the mag because it runs at a lower rate
+ *
+ * @param arg Instance pointer for the driver that is polling.
+ */
+ static void mag_measure_trampoline(void *arg);
+
+ /**
+ * Fetch accel measurements from the sensor and update the report ring.
+ */
+ void measure();
+
+ /**
+ * Fetch mag measurements from the sensor and update the report ring.
+ */
+ void mag_measure();
+
+ /**
+ * Read a register from the LSM303D
+ *
+ * @param The register to read.
+ * @return The value that was read.
+ */
+ uint8_t read_reg(unsigned reg);
+
+ /**
+ * Write a register in the LSM303D
+ *
+ * @param reg The register to write.
+ * @param value The new value to write.
+ */
+ void write_reg(unsigned reg, uint8_t value);
+
+ /**
+ * Modify a register in the LSM303D
+ *
+ * Bits are cleared before bits are set.
+ *
+ * @param reg The register to modify.
+ * @param clearbits Bits in the register to clear.
+ * @param setbits Bits in the register to set.
+ */
+ void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
+
+ /**
+ * Set the LSM303D accel measurement range.
+ *
+ * @param max_g The measurement range of the accel is in g (9.81m/s^2)
+ * Zero selects the maximum supported range.
+ * @return OK if the value can be supported, -ERANGE otherwise.
+ */
+ int set_range(unsigned max_g);
+
+ /**
+ * Set the LSM303D mag measurement range.
+ *
+ * @param max_ga The measurement range of the mag is in Ga
+ * Zero selects the maximum supported range.
+ * @return OK if the value can be supported, -ERANGE otherwise.
+ */
+ int mag_set_range(unsigned max_g);
+
+ /**
+ * Set the LSM303D accel anti-alias filter.
+ *
+ * @param bandwidth The anti-alias filter bandwidth in Hz
+ * Zero selects the highest bandwidth
+ * @return OK if the value can be supported, -ERANGE otherwise.
+ */
+ int set_antialias_filter_bandwidth(unsigned bandwith);
+
+ /**
+ * Get the LSM303D accel anti-alias filter.
+ *
+ * @param bandwidth The anti-alias filter bandwidth in Hz
+ * @return OK if the value was read and supported, ERROR otherwise.
+ */
+ int get_antialias_filter_bandwidth(unsigned &bandwidth);
+
+ /**
+ * Set the LSM303D internal accel sampling frequency.
+ *
+ * @param frequency The internal accel sampling frequency is set to not less than
+ * this value.
+ * Zero selects the maximum rate supported.
+ * @return OK if the value can be supported.
+ */
+ int set_samplerate(unsigned frequency);
+
+ /**
+ * Set the LSM303D internal mag sampling frequency.
+ *
+ * @param frequency The internal mag sampling frequency is set to not less than
+ * this value.
+ * Zero selects the maximum rate supported.
+ * @return OK if the value can be supported.
+ */
+ int mag_set_samplerate(unsigned frequency);
+};
+
+/**
+ * Helper class implementing the mag driver node.
+ */
+class LSM303D_mag : public device::CDev
+{
+public:
+ LSM303D_mag(LSM303D *parent);
+ ~LSM303D_mag();
+
+ virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
+ virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
+
+protected:
+ friend class LSM303D;
+
+ void parent_poll_notify();
+private:
+ LSM303D *_parent;
+
+ void measure();
+
+ void measure_trampoline(void *arg);
+};
+
+
+/* helper macro for handling report buffer indices */
+#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0)
+
+
+LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
+ SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 8000000),
+ _mag(new LSM303D_mag(this)),
+ _call_accel_interval(0),
+ _call_mag_interval(0),
+ _num_accel_reports(0),
+ _next_accel_report(0),
+ _oldest_accel_report(0),
+ _accel_reports(nullptr),
+ _accel_range_scale(0.0f),
+ _accel_range_m_s2(0.0f),
+ _accel_topic(-1),
+ _num_mag_reports(0),
+ _next_mag_report(0),
+ _oldest_mag_report(0),
+ _mag_reports(nullptr),
+ _mag_range_scale(0.0f),
+ _mag_range_ga(0.0f),
+ _accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")),
+ _mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read"))
+{
+ // enable debug() calls
+ _debug_enabled = true;
+
+ _mag_range_scale = 12.0f/32767.0f;
+
+ // default scale factors
+ _accel_scale.x_offset = 0;
+ _accel_scale.x_scale = 1.0f;
+ _accel_scale.y_offset = 0;
+ _accel_scale.y_scale = 1.0f;
+ _accel_scale.z_offset = 0;
+ _accel_scale.z_scale = 1.0f;
+
+ _mag_scale.x_offset = 0;
+ _mag_scale.x_scale = 1.0f;
+ _mag_scale.y_offset = 0;
+ _mag_scale.y_scale = 1.0f;
+ _mag_scale.z_offset = 0;
+ _mag_scale.z_scale = 1.0f;
+}
+
+LSM303D::~LSM303D()
+{
+ /* make sure we are truly inactive */
+ stop();
+
+ /* free any existing reports */
+ if (_accel_reports != nullptr)
+ delete[] _accel_reports;
+ if (_mag_reports != nullptr)
+ delete[] _mag_reports;
+
+ delete _mag;
+
+ /* delete the perf counter */
+ perf_free(_accel_sample_perf);
+ perf_free(_mag_sample_perf);
+}
+
+int
+LSM303D::init()
+{
+ int ret = ERROR;
+ int mag_ret;
+ int fd_mag;
+
+ /* do SPI init (and probe) first */
+ if (SPI::init() != OK)
+ goto out;
+
+ /* allocate basic report buffers */
+ _num_accel_reports = 2;
+ _oldest_accel_report = _next_accel_report = 0;
+ _accel_reports = new struct accel_report[_num_accel_reports];
+
+ if (_accel_reports == nullptr)
+ goto out;
+
+ /* advertise accel topic */
+ memset(&_accel_reports[0], 0, sizeof(_accel_reports[0]));
+ _accel_topic = orb_advertise(ORB_ID(sensor_accel), &_accel_reports[0]);
+
+ _num_mag_reports = 2;
+ _oldest_mag_report = _next_mag_report = 0;
+ _mag_reports = new struct mag_report[_num_mag_reports];
+
+ if (_mag_reports == nullptr)
+ goto out;
+
+ /* advertise mag topic */
+ memset(&_mag_reports[0], 0, sizeof(_mag_reports[0]));
+ _mag_topic = orb_advertise(ORB_ID(sensor_mag), &_mag_reports[0]);
+
+ /* enable accel, XXX do this with an ioctl? */
+ write_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A);
+
+ /* enable mag, XXX do this with an ioctl? */
+ write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M);
+ write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M);
+
+ /* XXX should we enable FIFO? */
+
+ set_range(8); /* XXX 16G mode seems wrong (shows 6 instead of 9.8m/s^2, therefore use 8G for now */
+ set_antialias_filter_bandwidth(50); /* available bandwidths: 50, 194, 362 or 773 Hz */
+ set_samplerate(400); /* max sample rate */
+
+ mag_set_range(4); /* XXX: take highest sensor range of 12GA? */
+ mag_set_samplerate(100);
+
+ /* XXX test this when another mag is used */
+ /* do CDev init for the mag device node, keep it optional */
+ mag_ret = _mag->init();
+
+ if (mag_ret != OK) {
+ _mag_topic = -1;
+ }
+
+ ret = OK;
+out:
+ return ret;
+}
+
+int
+LSM303D::probe()
+{
+ /* read dummy value to void to clear SPI statemachine on sensor */
+ (void)read_reg(ADDR_WHO_AM_I);
+
+ /* verify that the device is attached and functioning */
+ if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM)
+ return OK;
+
+ return -EIO;
+}
+
+ssize_t
+LSM303D::read(struct file *filp, char *buffer, size_t buflen)
+{
+ unsigned count = buflen / sizeof(struct accel_report);
+ int ret = 0;
+
+ /* buffer must be large enough */
+ if (count < 1)
+ return -ENOSPC;
+
+ /* if automatic measurement is enabled */
+ if (_call_accel_interval > 0) {
+
+ /*
+ * While there is space in the caller's buffer, and reports, copy them.
+ * Note that we may be pre-empted by the measurement code while we are doing this;
+ * we are careful to avoid racing with it.
+ */
+ while (count--) {
+ if (_oldest_accel_report != _next_accel_report) {
+ memcpy(buffer, _accel_reports + _oldest_accel_report, sizeof(*_accel_reports));
+ ret += sizeof(_accel_reports[0]);
+ INCREMENT(_oldest_accel_report, _num_accel_reports);
+ }
+ }
+
+ /* if there was no data, warn the caller */
+ return ret ? ret : -EAGAIN;
+ }
+
+ /* manual measurement */
+ _oldest_accel_report = _next_accel_report = 0;
+ measure();
+
+ /* measurement will have generated a report, copy it out */
+ memcpy(buffer, _accel_reports, sizeof(*_accel_reports));
+ ret = sizeof(*_accel_reports);
+
+ return ret;
+}
+
+ssize_t
+LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen)
+{
+ unsigned count = buflen / sizeof(struct mag_report);
+ int ret = 0;
+
+ /* buffer must be large enough */
+ if (count < 1)
+ return -ENOSPC;
+
+ /* if automatic measurement is enabled */
+ if (_call_mag_interval > 0) {
+
+ /*
+ * While there is space in the caller's buffer, and reports, copy them.
+ * Note that we may be pre-empted by the measurement code while we are doing this;
+ * we are careful to avoid racing with it.
+ */
+ while (count--) {
+ if (_oldest_mag_report != _next_mag_report) {
+ memcpy(buffer, _mag_reports + _oldest_mag_report, sizeof(*_mag_reports));
+ ret += sizeof(_mag_reports[0]);
+ INCREMENT(_oldest_mag_report, _num_mag_reports);
+ }
+ }
+
+ /* if there was no data, warn the caller */
+ return ret ? ret : -EAGAIN;
+ }
+
+ /* manual measurement */
+ _oldest_mag_report = _next_mag_report = 0;
+ measure();
+
+ /* measurement will have generated a report, copy it out */
+ memcpy(buffer, _mag_reports, sizeof(*_mag_reports));
+ ret = sizeof(*_mag_reports);
+
+ return ret;
+}
+
+int
+LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+ switch (cmd) {
+
+ case SENSORIOCSPOLLRATE: {
+ switch (arg) {
+
+ /* switching to manual polling */
+ case SENSOR_POLLRATE_MANUAL:
+ stop();
+ _call_accel_interval = 0;
+ return OK;
+
+ /* external signalling not supported */
+ case SENSOR_POLLRATE_EXTERNAL:
+
+ /* zero would be bad */
+ case 0:
+ return -EINVAL;
+
+ /* set default/max polling rate */
+ case SENSOR_POLLRATE_MAX:
+
+ return ioctl(filp, SENSORIOCSPOLLRATE, 1600);
+
+ case SENSOR_POLLRATE_DEFAULT:
+ /* With internal low pass filters enabled, 250 Hz is sufficient */
+ /* XXX for vibration tests with 800 Hz */
+ return ioctl(filp, SENSORIOCSPOLLRATE, 800);
+
+ /* adjust to a legal polling interval in Hz */
+ default: {
+ /* do we need to start internal polling? */
+ bool want_start = (_call_accel_interval == 0);
+
+ /* convert hz to hrt interval via microseconds */
+ unsigned ticks = 1000000 / arg;
+
+ /* check against maximum sane rate */
+ if (ticks < 1000)
+ return -EINVAL;
+
+ /* adjust sample rate of sensor */
+ set_samplerate(arg);
+
+ /* update interval for next measurement */
+ /* XXX this is a bit shady, but no other way to adjust... */
+ _accel_call.period = _call_accel_interval = ticks;
+
+ /* if we need to start the poll state machine, do it */
+ if (want_start)
+ start();
+
+ return OK;
+ }
+ }
+ }
+
+ case SENSORIOCGPOLLRATE:
+ if (_call_accel_interval == 0)
+ return SENSOR_POLLRATE_MANUAL;
+
+ return 1000000 / _call_accel_interval;
+
+ case SENSORIOCSQUEUEDEPTH: {
+ /* account for sentinel in the ring */
+ arg++;
+
+ /* lower bound is mandatory, upper bound is a sanity check */
+ if ((arg < 2) || (arg > 100))
+ return -EINVAL;
+
+ /* allocate new buffer */
+ struct accel_report *buf = new struct accel_report[arg];
+
+ if (nullptr == buf)
+ return -ENOMEM;
+
+ /* reset the measurement state machine with the new buffer, free the old */
+ stop();
+ delete[] _accel_reports;
+ _num_accel_reports = arg;
+ _accel_reports = buf;
+ start();
+
+ return OK;
+ }
+
+ case SENSORIOCGQUEUEDEPTH:
+ return _num_accel_reports - 1;
+
+ case SENSORIOCRESET:
+ /* XXX implement */
+ return -EINVAL;
+
+// case ACCELIOCSLOWPASS:
+ case ACCELIOCGLOWPASS:
+
+ unsigned bandwidth;
+
+ if (OK == get_antialias_filter_bandwidth(bandwidth))
+ return bandwidth;
+ else
+ return -EINVAL;
+
+
+ default:
+ /* give it to the superclass */
+ return SPI::ioctl(filp, cmd, arg);
+ }
+}
+
+int
+LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+ switch (cmd) {
+
+ case SENSORIOCSPOLLRATE: {
+ switch (arg) {
+
+ /* switching to manual polling */
+ case SENSOR_POLLRATE_MANUAL:
+ stop();
+ _call_mag_interval = 0;
+ return OK;
+
+ /* external signalling 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:
+ /* 50 Hz is max for mag */
+ return mag_ioctl(filp, SENSORIOCSPOLLRATE, 100);
+
+ /* adjust to a legal polling interval in Hz */
+ default: {
+ /* do we need to start internal polling? */
+ bool want_start = (_call_mag_interval == 0);
+
+ /* convert hz to hrt interval via microseconds */
+ unsigned ticks = 1000000 / arg;
+
+ /* check against maximum sane rate */
+ if (ticks < 1000)
+ return -EINVAL;
+
+ /* adjust sample rate of sensor */
+ mag_set_samplerate(arg);
+
+ /* update interval for next measurement */
+ /* XXX this is a bit shady, but no other way to adjust... */
+ _mag_call.period = _call_mag_interval = ticks;
+
+ /* if we need to start the poll state machine, do it */
+ if (want_start)
+ start();
+
+ return OK;
+ }
+ }
+ }
+
+ case SENSORIOCGPOLLRATE:
+ if (_call_mag_interval == 0)
+ return SENSOR_POLLRATE_MANUAL;
+
+ return 1000000 / _call_mag_interval;
+ case SENSORIOCSQUEUEDEPTH:
+ case SENSORIOCGQUEUEDEPTH:
+ case SENSORIOCRESET:
+ return ioctl(filp, cmd, arg);
+
+ case MAGIOCSSAMPLERATE:
+// case MAGIOCGSAMPLERATE:
+ /* XXX not implemented */
+ return -EINVAL;
+
+ case MAGIOCSLOWPASS:
+// case MAGIOCGLOWPASS:
+ /* XXX not implemented */
+// _set_dlpf_filter((uint16_t)arg);
+ return -EINVAL;
+
+ case MAGIOCSSCALE:
+ /* copy scale in */
+ memcpy(&_mag_scale, (struct mag_scale *) arg, sizeof(_mag_scale));
+ return OK;
+
+ case MAGIOCGSCALE:
+ /* copy scale out */
+ memcpy((struct mag_scale *) arg, &_mag_scale, sizeof(_mag_scale));
+ return OK;
+
+ case MAGIOCSRANGE:
+// case MAGIOCGRANGE:
+ /* XXX not implemented */
+ // XXX change these two values on set:
+ // _mag_range_scale = xx
+ // _mag_range_ga = xx
+ return -EINVAL;
+
+ case MAGIOCSELFTEST:
+ /* XXX not implemented */
+// return self_test();
+ return -EINVAL;
+
+ default:
+ /* give it to the superclass */
+ return SPI::ioctl(filp, cmd, arg);
+ }
+}
+
+uint8_t
+LSM303D::read_reg(unsigned reg)
+{
+ uint8_t cmd[2];
+
+ cmd[0] = reg | DIR_READ;
+
+ transfer(cmd, cmd, sizeof(cmd));
+
+ return cmd[1];
+}
+
+void
+LSM303D::write_reg(unsigned reg, uint8_t value)
+{
+ uint8_t cmd[2];
+
+ cmd[0] = reg | DIR_WRITE;
+ cmd[1] = value;
+
+ transfer(cmd, nullptr, sizeof(cmd));
+}
+
+void
+LSM303D::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
+{
+ uint8_t val;
+
+ val = read_reg(reg);
+ val &= ~clearbits;
+ val |= setbits;
+ write_reg(reg, val);
+}
+
+int
+LSM303D::set_range(unsigned max_g)
+{
+ uint8_t setbits = 0;
+ uint8_t clearbits = REG2_FULL_SCALE_BITS_A;
+ float new_range = 0.0f;
+
+ if (max_g == 0)
+ max_g = 16;
+
+ if (max_g <= 2) {
+ new_range = 2.0f;
+ setbits |= REG2_FULL_SCALE_2G_A;
+
+ } else if (max_g <= 4) {
+ new_range = 4.0f;
+ setbits |= REG2_FULL_SCALE_4G_A;
+
+ } else if (max_g <= 6) {
+ new_range = 6.0f;
+ setbits |= REG2_FULL_SCALE_6G_A;
+
+ } else if (max_g <= 8) {
+ new_range = 8.0f;
+ setbits |= REG2_FULL_SCALE_8G_A;
+
+ } else if (max_g <= 16) {
+ new_range = 16.0f;
+ setbits |= REG2_FULL_SCALE_16G_A;
+
+ } else {
+ return -EINVAL;
+ }
+
+ _accel_range_m_s2 = new_range * 9.80665f;
+ _accel_range_scale = _accel_range_m_s2 / 32768.0f;
+
+ modify_reg(ADDR_CTRL_REG2, clearbits, setbits);
+
+ return OK;
+}
+
+int
+LSM303D::mag_set_range(unsigned max_ga)
+{
+ uint8_t setbits = 0;
+ uint8_t clearbits = REG6_FULL_SCALE_BITS_M;
+ float new_range = 0.0f;
+
+ if (max_ga == 0)
+ max_ga = 12;
+
+ if (max_ga <= 2) {
+ new_range = 2.0f;
+ setbits |= REG6_FULL_SCALE_2GA_M;
+
+ } else if (max_ga <= 4) {
+ new_range = 4.0f;
+ setbits |= REG6_FULL_SCALE_4GA_M;
+
+ } else if (max_ga <= 8) {
+ new_range = 8.0f;
+ setbits |= REG6_FULL_SCALE_8GA_M;
+
+ } else if (max_ga <= 12) {
+ new_range = 12.0f;
+ setbits |= REG6_FULL_SCALE_12GA_M;
+
+ } else {
+ return -EINVAL;
+ }
+
+ _mag_range_ga = new_range;
+ _mag_range_scale = _mag_range_ga / 32768.0f;
+
+ modify_reg(ADDR_CTRL_REG6, clearbits, setbits);
+
+ return OK;
+}
+
+int
+LSM303D::set_antialias_filter_bandwidth(unsigned bandwidth)
+{
+ uint8_t setbits = 0;
+ uint8_t clearbits = REG2_ANTIALIAS_FILTER_BW_BITS_A;
+
+ if (bandwidth == 0)
+ bandwidth = 773;
+
+ if (bandwidth <= 50) {
+ setbits |= REG2_AA_FILTER_BW_50HZ_A;
+
+ } else if (bandwidth <= 194) {
+ setbits |= REG2_AA_FILTER_BW_194HZ_A;
+
+ } else if (bandwidth <= 362) {
+ setbits |= REG2_AA_FILTER_BW_362HZ_A;
+
+ } else if (bandwidth <= 773) {
+ setbits |= REG2_AA_FILTER_BW_773HZ_A;
+
+ } else {
+ return -EINVAL;
+ }
+
+ modify_reg(ADDR_CTRL_REG2, clearbits, setbits);
+
+ return OK;
+}
+
+int
+LSM303D::get_antialias_filter_bandwidth(unsigned &bandwidth)
+{
+ uint8_t readbits = read_reg(ADDR_CTRL_REG2);
+
+ if ((readbits & REG2_ANTIALIAS_FILTER_BW_BITS_A) == REG2_AA_FILTER_BW_50HZ_A)
+ bandwidth = 50;
+ else if ((readbits & REG2_ANTIALIAS_FILTER_BW_BITS_A) == REG2_AA_FILTER_BW_194HZ_A)
+ bandwidth = 194;
+ else if ((readbits & REG2_ANTIALIAS_FILTER_BW_BITS_A) == REG2_AA_FILTER_BW_362HZ_A)
+ bandwidth = 362;
+ else if ((readbits & REG2_ANTIALIAS_FILTER_BW_BITS_A) == REG2_AA_FILTER_BW_773HZ_A)
+ bandwidth = 773;
+ else
+ return ERROR;
+
+ return OK;
+}
+
+int
+LSM303D::set_samplerate(unsigned frequency)
+{
+ uint8_t setbits = 0;
+ uint8_t clearbits = REG1_RATE_BITS_A;
+
+ if (frequency == 0)
+ frequency = 1600;
+
+ if (frequency <= 100) {
+ setbits |= REG1_RATE_100HZ_A;
+
+ } else if (frequency <= 200) {
+ setbits |= REG1_RATE_200HZ_A;
+
+ } else if (frequency <= 400) {
+ setbits |= REG1_RATE_400HZ_A;
+
+ } else if (frequency <= 800) {
+ setbits |= REG1_RATE_800HZ_A;
+
+ } else if (frequency <= 1600) {
+ setbits |= REG1_RATE_1600HZ_A;
+
+ } else {
+ return -EINVAL;
+ }
+
+ modify_reg(ADDR_CTRL_REG1, clearbits, setbits);
+
+ return OK;
+}
+
+int
+LSM303D::mag_set_samplerate(unsigned frequency)
+{
+ uint8_t setbits = 0;
+ uint8_t clearbits = REG5_RATE_BITS_M;
+
+ if (frequency == 0)
+ frequency = 100;
+
+ if (frequency <= 25) {
+ setbits |= REG5_RATE_25HZ_M;
+
+ } else if (frequency <= 50) {
+ setbits |= REG5_RATE_50HZ_M;
+
+ } else if (frequency <= 100) {
+ setbits |= REG5_RATE_100HZ_M;
+
+
+ } else {
+ return -EINVAL;
+ }
+
+ modify_reg(ADDR_CTRL_REG5, clearbits, setbits);
+
+ return OK;
+}
+
+void
+LSM303D::start()
+{
+ /* make sure we are stopped first */
+ stop();
+
+ /* reset the report ring */
+ _oldest_accel_report = _next_accel_report = 0;
+ _oldest_mag_report = _next_mag_report = 0;
+
+ /* start polling at the specified rate */
+ hrt_call_every(&_accel_call, 1000, _call_accel_interval, (hrt_callout)&LSM303D::measure_trampoline, this);
+ hrt_call_every(&_mag_call, 1000, _call_mag_interval, (hrt_callout)&LSM303D::mag_measure_trampoline, this);
+}
+
+void
+LSM303D::stop()
+{
+ hrt_cancel(&_accel_call);
+ hrt_cancel(&_mag_call);
+}
+
+void
+LSM303D::measure_trampoline(void *arg)
+{
+ LSM303D *dev = (LSM303D *)arg;
+
+ /* make another measurement */
+ dev->measure();
+}
+
+void
+LSM303D::mag_measure_trampoline(void *arg)
+{
+ LSM303D *dev = (LSM303D *)arg;
+
+ /* make another measurement */
+ dev->mag_measure();
+}
+
+void
+LSM303D::measure()
+{
+ /* status register and data as read back from the device */
+
+#pragma pack(push, 1)
+ struct {
+ uint8_t cmd;
+ uint8_t status;
+ int16_t x;
+ int16_t y;
+ int16_t z;
+ } raw_accel_report;
+#pragma pack(pop)
+
+ accel_report *accel_report = &_accel_reports[_next_accel_report];
+
+ /* start the performance counter */
+ perf_begin(_accel_sample_perf);
+
+ /* fetch data from the sensor */
+ raw_accel_report.cmd = ADDR_STATUS_A | DIR_READ | ADDR_INCREMENT;
+ transfer((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report));
+
+ /*
+ * 1) Scale raw value to SI units using scaling from datasheet.
+ * 2) Subtract static offset (in SI units)
+ * 3) Scale the statically calibrated values with a linear
+ * dynamically obtained factor
+ *
+ * Note: the static sensor offset is the number the sensor outputs
+ * at a nominally 'zero' input. Therefore the offset has to
+ * be subtracted.
+ *
+ * Example: A gyro outputs a value of 74 at zero angular rate
+ * the offset is 74 from the origin and subtracting
+ * 74 from all measurements centers them around zero.
+ */
+
+
+ accel_report->timestamp = hrt_absolute_time();
+
+ accel_report->x_raw = raw_accel_report.x;
+ accel_report->y_raw = raw_accel_report.y;
+ accel_report->z_raw = raw_accel_report.z;
+
+ accel_report->x = ((accel_report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
+ accel_report->y = ((accel_report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
+ accel_report->z = ((accel_report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
+ accel_report->scaling = _accel_range_scale;
+ accel_report->range_m_s2 = _accel_range_m_s2;
+
+ /* post a report to the ring - note, not locked */
+ INCREMENT(_next_accel_report, _num_accel_reports);
+
+ /* if we are running up against the oldest report, fix it */
+ if (_next_accel_report == _oldest_accel_report)
+ INCREMENT(_oldest_accel_report, _num_accel_reports);
+
+ /* notify anyone waiting for data */
+ poll_notify(POLLIN);
+
+ /* publish for subscribers */
+ orb_publish(ORB_ID(sensor_accel), _accel_topic, accel_report);
+
+ /* stop the perf counter */
+ perf_end(_accel_sample_perf);
+}
+
+void
+LSM303D::mag_measure()
+{
+ /* status register and data as read back from the device */
+#pragma pack(push, 1)
+ struct {
+ uint8_t cmd;
+ uint8_t status;
+ int16_t x;
+ int16_t y;
+ int16_t z;
+ } raw_mag_report;
+#pragma pack(pop)
+
+ mag_report *mag_report = &_mag_reports[_next_mag_report];
+
+ /* start the performance counter */
+ perf_begin(_mag_sample_perf);
+
+ /* fetch data from the sensor */
+ raw_mag_report.cmd = ADDR_STATUS_M | DIR_READ | ADDR_INCREMENT;
+ transfer((uint8_t *)&raw_mag_report, (uint8_t *)&raw_mag_report, sizeof(raw_mag_report));
+
+ /*
+ * 1) Scale raw value to SI units using scaling from datasheet.
+ * 2) Subtract static offset (in SI units)
+ * 3) Scale the statically calibrated values with a linear
+ * dynamically obtained factor
+ *
+ * Note: the static sensor offset is the number the sensor outputs
+ * at a nominally 'zero' input. Therefore the offset has to
+ * be subtracted.
+ *
+ * Example: A gyro outputs a value of 74 at zero angular rate
+ * the offset is 74 from the origin and subtracting
+ * 74 from all measurements centers them around zero.
+ */
+
+
+ mag_report->timestamp = hrt_absolute_time();
+
+ mag_report->x_raw = raw_mag_report.x;
+ mag_report->y_raw = raw_mag_report.y;
+ mag_report->z_raw = raw_mag_report.z;
+ mag_report->x = ((mag_report->x_raw * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale;
+ mag_report->y = ((mag_report->y_raw * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale;
+ mag_report->z = ((mag_report->z_raw * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale;
+ mag_report->scaling = _mag_range_scale;
+ mag_report->range_ga = _mag_range_ga;
+
+ /* post a report to the ring - note, not locked */
+ INCREMENT(_next_mag_report, _num_mag_reports);
+
+ /* if we are running up against the oldest report, fix it */
+ if (_next_mag_report == _oldest_mag_report)
+ INCREMENT(_oldest_mag_report, _num_mag_reports);
+
+ /* XXX please check this poll_notify, is it the right one? */
+ /* notify anyone waiting for data */
+ poll_notify(POLLIN);
+
+ /* publish for subscribers */
+ orb_publish(ORB_ID(sensor_mag), _mag_topic, mag_report);
+
+ /* stop the perf counter */
+ perf_end(_mag_sample_perf);
+}
+
+void
+LSM303D::print_info()
+{
+ perf_print_counter(_accel_sample_perf);
+ printf("report queue: %u (%u/%u @ %p)\n",
+ _num_accel_reports, _oldest_accel_report, _next_accel_report, _accel_reports);
+ perf_print_counter(_mag_sample_perf);
+ printf("report queue: %u (%u/%u @ %p)\n",
+ _num_mag_reports, _oldest_mag_report, _next_mag_report, _mag_reports);
+}
+
+LSM303D_mag::LSM303D_mag(LSM303D *parent) :
+ CDev("LSM303D_mag", MAG_DEVICE_PATH),
+ _parent(parent)
+{
+}
+
+LSM303D_mag::~LSM303D_mag()
+{
+}
+
+void
+LSM303D_mag::parent_poll_notify()
+{
+ poll_notify(POLLIN);
+}
+
+ssize_t
+LSM303D_mag::read(struct file *filp, char *buffer, size_t buflen)
+{
+ return _parent->mag_read(filp, buffer, buflen);
+}
+
+int
+LSM303D_mag::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+ return _parent->mag_ioctl(filp, cmd, arg);
+}
+
+void
+LSM303D_mag::measure()
+{
+ _parent->mag_measure();
+}
+
+void
+LSM303D_mag::measure_trampoline(void *arg)
+{
+ _parent->mag_measure_trampoline(arg);
+}
+
+/**
+ * Local functions in support of the shell command.
+ */
+namespace lsm303d
+{
+
+LSM303D *g_dev;
+
+void start();
+void test();
+void reset();
+void info();
+
+/**
+ * Start the driver.
+ */
+void
+start()
+{
+ int fd, fd_mag;
+
+ if (g_dev != nullptr)
+ errx(1, "already started");
+
+ /* create the driver */
+ g_dev = new LSM303D(1 /* XXX magic number */, ACCEL_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG);
+
+ if (g_dev == nullptr)
+ goto fail;
+
+ if (OK != g_dev->init())
+ goto fail;
+
+ /* set the poll rate to default, starts automatic data collection */
+ fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
+
+ if (fd < 0)
+ goto fail;
+
+ if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+ goto fail;
+
+ fd_mag = open(MAG_DEVICE_PATH, O_RDONLY);
+
+ /* don't fail if open cannot be opened */
+ if (0 <= fd_mag) {
+ if (ioctl(fd_mag, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
+ goto fail;
+ }
+ }
+
+
+ exit(0);
+fail:
+
+ if (g_dev != nullptr) {
+ delete g_dev;
+ g_dev = nullptr;
+ }
+
+ errx(1, "driver start failed");
+}
+
+/**
+ * Perform some basic functional tests on the driver;
+ * make sure we can collect data from the sensor in polled
+ * and automatic modes.
+ */
+void
+test()
+{
+ int fd_accel = -1;
+ struct accel_report a_report;
+ ssize_t sz;
+ int filter_bandwidth;
+
+ /* get the driver */
+ fd_accel = open(ACCEL_DEVICE_PATH, O_RDONLY);
+
+ if (fd_accel < 0)
+ err(1, "%s open failed", ACCEL_DEVICE_PATH);
+
+ /* do a simple demand read */
+ sz = read(fd_accel, &a_report, sizeof(a_report));
+
+ if (sz != sizeof(a_report))
+ err(1, "immediate read failed");
+
+
+ warnx("accel x: \t% 9.5f\tm/s^2", (double)a_report.x);
+ warnx("accel y: \t% 9.5f\tm/s^2", (double)a_report.y);
+ warnx("accel z: \t% 9.5f\tm/s^2", (double)a_report.z);
+ warnx("accel x: \t%d\traw", (int)a_report.x_raw);
+ warnx("accel y: \t%d\traw", (int)a_report.y_raw);
+ warnx("accel z: \t%d\traw", (int)a_report.z_raw);
+
+ warnx("accel range: %8.4f m/s^2", (double)a_report.range_m_s2);
+ if (ERROR == (filter_bandwidth = ioctl(fd_accel, ACCELIOCGLOWPASS, 0)))
+ warnx("accel antialias filter bandwidth: fail");
+ else
+ warnx("accel antialias filter bandwidth: %d Hz", filter_bandwidth);
+
+ int fd_mag = -1;
+ struct mag_report m_report;
+
+ /* get the driver */
+ fd_mag = open(MAG_DEVICE_PATH, O_RDONLY);
+
+ if (fd_mag < 0)
+ err(1, "%s open failed", MAG_DEVICE_PATH);
+
+ /* do a simple demand read */
+ sz = read(fd_mag, &m_report, sizeof(m_report));
+
+ if (sz != sizeof(m_report))
+ err(1, "immediate read failed");
+
+ warnx("mag x: \t% 9.5f\tga", (double)m_report.x);
+ warnx("mag y: \t% 9.5f\tga", (double)m_report.y);
+ warnx("mag z: \t% 9.5f\tga", (double)m_report.z);
+ warnx("mag x: \t%d\traw", (int)m_report.x_raw);
+ warnx("mag y: \t%d\traw", (int)m_report.y_raw);
+ warnx("mag z: \t%d\traw", (int)m_report.z_raw);
+ warnx("mag range: %8.4f ga", (double)m_report.range_ga);
+
+ /* XXX add poll-rate tests here too */
+
+// reset();
+ errx(0, "PASS");
+}
+
+/**
+ * Reset the driver.
+ */
+void
+reset()
+{
+ int fd = open(ACCEL_DEVICE_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);
+}
+
+/**
+ * Print a little info about the driver.
+ */
+void
+info()
+{
+ if (g_dev == nullptr)
+ errx(1, "driver not running\n");
+
+ printf("state @ %p\n", g_dev);
+ g_dev->print_info();
+
+ exit(0);
+}
+
+
+} // namespace
+
+int
+lsm303d_main(int argc, char *argv[])
+{
+ /*
+ * Start/load the driver.
+
+ */
+ if (!strcmp(argv[1], "start"))
+ lsm303d::start();
+
+ /*
+ * Test the driver/device.
+ */
+ if (!strcmp(argv[1], "test"))
+ lsm303d::test();
+
+ /*
+ * Reset the driver.
+ */
+ if (!strcmp(argv[1], "reset"))
+ lsm303d::reset();
+
+ /*
+ * Print driver information.
+ */
+ if (!strcmp(argv[1], "info"))
+ lsm303d::info();
+
+ errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
+}
diff --git a/src/drivers/lsm303d/module.mk b/src/drivers/lsm303d/module.mk
new file mode 100644
index 000000000..e93b1e331
--- /dev/null
+++ b/src/drivers/lsm303d/module.mk
@@ -0,0 +1,6 @@
+#
+# LSM303D accel/mag driver
+#
+
+MODULE_COMMAND = lsm303d
+SRCS = lsm303d.cpp
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index bf72892eb..e199a5998 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -34,7 +34,7 @@
/**
* @file fmu.cpp
*
- * Driver/configurator for the PX4 FMU multi-purpose port.
+ * Driver/configurator for the PX4 FMU multi-purpose port on v1 and v2 boards.
*/
#include <nuttx/config.h>
@@ -57,9 +57,18 @@
#include <drivers/device/device.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_gpio.h>
-#include <drivers/boards/px4fmu/px4fmu_internal.h>
#include <drivers/drv_hrt.h>
+#if defined(CONFIG_ARCH_BOARD_PX4FMU)
+# include <drivers/boards/px4fmu/px4fmu_internal.h>
+# define FMU_HAVE_PPM
+#elif defined(CONFIG_ARCH_BOARD_PX4FMUV2)
+# include <drivers/boards/px4fmuv2/px4fmu_internal.h>
+# undef FMU_HAVE_PPM
+#else
+# error Unrecognised FMU board.
+#endif
+
#include <systemlib/systemlib.h>
#include <systemlib/err.h>
#include <systemlib/mixer/mixer.h>
@@ -71,15 +80,18 @@
#include <uORB/topics/actuator_outputs.h>
#include <systemlib/err.h>
-#include <systemlib/ppm_decode.h>
+#ifdef FMU_HAVE_PPM
+# include <systemlib/ppm_decode.h>
+#endif
class PX4FMU : public device::CDev
{
public:
enum Mode {
+ MODE_NONE,
MODE_2PWM,
MODE_4PWM,
- MODE_NONE
+ MODE_6PWM,
};
PX4FMU();
virtual ~PX4FMU();
@@ -146,6 +158,7 @@ private:
};
const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = {
+#if defined(CONFIG_ARCH_BOARD_PX4FMU)
{GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0},
{GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0},
{GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, GPIO_USART2_CTS_1},
@@ -154,6 +167,18 @@ const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = {
{GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, GPIO_USART2_RX_1},
{GPIO_GPIO6_INPUT, GPIO_GPIO6_OUTPUT, GPIO_CAN2_TX_2},
{GPIO_GPIO7_INPUT, GPIO_GPIO7_OUTPUT, GPIO_CAN2_RX_2},
+#endif
+#if defined(CONFIG_ARCH_BOARD_PX4FMUV2)
+ {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0},
+ {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0},
+ {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, 0},
+ {GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, 0},
+ {GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, 0},
+ {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, 0},
+ {0, GPIO_VDD_5V_PERIPH_EN, 0},
+ {GPIO_5V_HIPOWER_OC, 0, 0},
+ {GPIO_5V_PERIPH_OC, 0, 0},
+#endif
};
const unsigned PX4FMU::_ngpio = sizeof(PX4FMU::_gpio_tab) / sizeof(PX4FMU::_gpio_tab[0]);
@@ -271,9 +296,22 @@ PX4FMU::set_mode(Mode mode)
* are presented on the output pins.
*/
switch (mode) {
- case MODE_2PWM: // multi-port with flow control lines as PWM
- case MODE_4PWM: // multi-port as 4 PWM outs
- debug("MODE_%dPWM", (mode == MODE_2PWM) ? 2 : 4);
+ case MODE_2PWM: // v1 multi-port with flow control lines as PWM
+ debug("MODE_2PWM");
+
+ /* default output rates */
+ _pwm_default_rate = 50;
+ _pwm_alt_rate = 50;
+ _pwm_alt_rate_channels = 0;
+
+ /* XXX magic numbers */
+ up_pwm_servo_init(0x3);
+ set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate);
+
+ break;
+
+ case MODE_4PWM: // v1 multi-port as 4 PWM outs
+ debug("MODE_4PWM");
/* default output rates */
_pwm_default_rate = 50;
@@ -281,7 +319,21 @@ PX4FMU::set_mode(Mode mode)
_pwm_alt_rate_channels = 0;
/* XXX magic numbers */
- up_pwm_servo_init((mode == MODE_2PWM) ? 0x3 : 0xf);
+ up_pwm_servo_init(0xf);
+ set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate);
+
+ break;
+
+ case MODE_6PWM: // v2 PWMs as 6 PWM outs
+ debug("MODE_6PWM");
+
+ /* default output rates */
+ _pwm_default_rate = 50;
+ _pwm_alt_rate = 50;
+ _pwm_alt_rate_channels = 0;
+
+ /* XXX magic numbers */
+ up_pwm_servo_init(0x3f);
set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate);
break;
@@ -399,14 +451,14 @@ PX4FMU::task_main()
fds[1].fd = _t_armed;
fds[1].events = POLLIN;
- unsigned num_outputs = (_mode == MODE_2PWM) ? 2 : 4;
-
+#ifdef FMU_HAVE_PPM
// rc input, published to ORB
struct rc_input_values rc_in;
orb_advert_t to_input_rc = 0;
memset(&rc_in, 0, sizeof(rc_in));
rc_in.input_source = RC_INPUT_SOURCE_PX4FMU_PPM;
+#endif
log("starting");
@@ -460,6 +512,23 @@ PX4FMU::task_main()
/* can we mix? */
if (_mixers != nullptr) {
+ unsigned num_outputs;
+
+ switch (_mode) {
+ case MODE_2PWM:
+ num_outputs = 2;
+ break;
+ case MODE_4PWM:
+ num_outputs = 4;
+ break;
+ case MODE_6PWM:
+ num_outputs = 6;
+ break;
+ default:
+ num_outputs = 0;
+ break;
+ }
+
/* do mixing */
outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs);
outputs.timestamp = hrt_absolute_time();
@@ -512,6 +581,7 @@ PX4FMU::task_main()
}
}
+#ifdef FMU_HAVE_PPM
// see if we have new PPM input data
if (ppm_last_valid_decode != rc_in.timestamp) {
// we have a new PPM frame. Publish it.
@@ -531,6 +601,8 @@ PX4FMU::task_main()
orb_publish(ORB_ID(input_rc), to_input_rc, &rc_in);
}
}
+#endif
+
}
::close(_t_actuators);
@@ -579,6 +651,7 @@ PX4FMU::ioctl(file *filp, int cmd, unsigned long arg)
switch (_mode) {
case MODE_2PWM:
case MODE_4PWM:
+ case MODE_6PWM:
ret = pwm_ioctl(filp, cmd, arg);
break;
@@ -623,16 +696,24 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
ret = set_pwm_rate(arg, _pwm_default_rate, _pwm_alt_rate);
break;
- case PWM_SERVO_SET(2):
+ case PWM_SERVO_SET(5):
+ case PWM_SERVO_SET(4):
+ if (_mode < MODE_6PWM) {
+ ret = -EINVAL;
+ break;
+ }
+
+ /* FALLTHROUGH */
case PWM_SERVO_SET(3):
- if (_mode != MODE_4PWM) {
+ case PWM_SERVO_SET(2):
+ if (_mode < MODE_4PWM) {
ret = -EINVAL;
break;
}
/* FALLTHROUGH */
- case PWM_SERVO_SET(0):
case PWM_SERVO_SET(1):
+ case PWM_SERVO_SET(0):
if (arg < 2100) {
up_pwm_servo_set(cmd - PWM_SERVO_SET(0), arg);
} else {
@@ -641,16 +722,24 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
break;
- case PWM_SERVO_GET(2):
+ case PWM_SERVO_GET(5):
+ case PWM_SERVO_GET(4):
+ if (_mode < MODE_6PWM) {
+ ret = -EINVAL;
+ break;
+ }
+
+ /* FALLTHROUGH */
case PWM_SERVO_GET(3):
- if (_mode != MODE_4PWM) {
+ case PWM_SERVO_GET(2):
+ if (_mode < MODE_4PWM) {
ret = -EINVAL;
break;
}
/* FALLTHROUGH */
- case PWM_SERVO_GET(0):
case PWM_SERVO_GET(1):
+ case PWM_SERVO_GET(0):
*(servo_position_t *)arg = up_pwm_servo_get(cmd - PWM_SERVO_GET(0));
break;
@@ -658,16 +747,26 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
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:
- if (_mode == MODE_4PWM) {
+ switch (_mode) {
+ case MODE_6PWM:
+ *(unsigned *)arg = 6;
+ break;
+ case MODE_4PWM:
*(unsigned *)arg = 4;
-
- } else {
+ break;
+ case MODE_2PWM:
*(unsigned *)arg = 2;
+ break;
+ default:
+ ret = -EINVAL;
+ break;
}
break;
@@ -743,17 +842,17 @@ ssize_t
PX4FMU::write(file *filp, const char *buffer, size_t len)
{
unsigned count = len / 2;
- uint16_t values[4];
+ uint16_t values[6];
- if (count > 4) {
- // we only have 4 PWM outputs on the FMU
- count = 4;
+ if (count > 6) {
+ // we have at most 6 outputs
+ count = 6;
}
// allow for misaligned values
- memcpy(values, buffer, count*2);
+ memcpy(values, buffer, count * 2);
- for (uint8_t i=0; i<count; i++) {
+ for (uint8_t i = 0; i < count; i++) {
up_pwm_servo_set(i, values[i]);
}
return count * 2;
@@ -763,19 +862,28 @@ void
PX4FMU::gpio_reset(void)
{
/*
- * Setup default GPIO config - all pins as GPIOs, GPIO driver chip
- * to input mode.
+ * Setup default GPIO config - all pins as GPIOs, input if
+ * possible otherwise output if possible.
*/
- for (unsigned i = 0; i < _ngpio; i++)
- stm32_configgpio(_gpio_tab[i].input);
+ for (unsigned i = 0; i < _ngpio; i++) {
+ if (_gpio_tab[i].input != 0) {
+ stm32_configgpio(_gpio_tab[i].input);
+ } else if (_gpio_tab[i].output != 0) {
+ stm32_configgpio(_gpio_tab[i].output);
+ }
+ }
+#if defined(CONFIG_ARCH_BOARD_PX4FMU)
+ /* if we have a GPIO direction control, set it to zero (input) */
stm32_gpiowrite(GPIO_GPIO_DIR, 0);
stm32_configgpio(GPIO_GPIO_DIR);
+#endif
}
void
PX4FMU::gpio_set_function(uint32_t gpios, int function)
{
+#if defined(CONFIG_ARCH_BOARD_PX4FMU)
/*
* GPIOs 0 and 1 must have the same direction as they are buffered
* by a shared 2-port driver. Any attempt to set either sets both.
@@ -787,6 +895,7 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function)
if (GPIO_SET_OUTPUT == function)
stm32_gpiowrite(GPIO_GPIO_DIR, 1);
}
+#endif
/* configure selected GPIOs as required */
for (unsigned i = 0; i < _ngpio; i++) {
@@ -809,9 +918,11 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function)
}
}
+#if defined(CONFIG_ARCH_BOARD_PX4FMU)
/* flip buffer to input mode if required */
if ((GPIO_SET_INPUT == function) && (gpios & 3))
stm32_gpiowrite(GPIO_GPIO_DIR, 0);
+#endif
}
void
@@ -912,14 +1023,21 @@ fmu_new_mode(PortMode new_mode)
/* nothing more to do here */
break;
- case PORT_FULL_SERIAL:
- /* set all multi-GPIOs to serial mode */
- gpio_bits = GPIO_MULTI_1 | GPIO_MULTI_2 | GPIO_MULTI_3 | GPIO_MULTI_4;
- break;
-
case PORT_FULL_PWM:
+#if defined(CONFIG_ARCH_BOARD_PX4FMU)
/* select 4-pin PWM mode */
servo_mode = PX4FMU::MODE_4PWM;
+#endif
+#if defined(CONFIG_ARCH_BOARD_PX4FMUV2)
+ servo_mode = PX4FMU::MODE_6PWM;
+#endif
+ break;
+
+ /* mixed modes supported on v1 board only */
+#if defined(CONFIG_ARCH_BOARD_PX4FMU)
+ case PORT_FULL_SERIAL:
+ /* set all multi-GPIOs to serial mode */
+ gpio_bits = GPIO_MULTI_1 | GPIO_MULTI_2 | GPIO_MULTI_3 | GPIO_MULTI_4;
break;
case PORT_GPIO_AND_SERIAL:
@@ -938,6 +1056,9 @@ fmu_new_mode(PortMode new_mode)
/* select 2-pin PWM mode */
servo_mode = PX4FMU::MODE_2PWM;
break;
+#endif
+ default:
+ return -1;
}
/* adjust GPIO config for serial mode(s) */
@@ -995,6 +1116,12 @@ test(void)
if (ioctl(fd, PWM_SERVO_SET(3), 1600) < 0) err(1, "servo 4 set failed");
+#if defined(CONFIG_ARCH_BOARD_PX4FMUV2)
+ if (ioctl(fd, PWM_SERVO_SET(4), 1800) < 0) err(1, "servo 5 set failed");
+
+ if (ioctl(fd, PWM_SERVO_SET(5), 2000) < 0) err(1, "servo 6 set failed");
+#endif
+
close(fd);
exit(0);
@@ -1053,12 +1180,13 @@ fmu_main(int argc, char *argv[])
if (!strcmp(verb, "mode_gpio")) {
new_mode = PORT_FULL_GPIO;
- } else if (!strcmp(verb, "mode_serial")) {
- new_mode = PORT_FULL_SERIAL;
-
} else if (!strcmp(verb, "mode_pwm")) {
new_mode = PORT_FULL_PWM;
+#if defined(CONFIG_ARCH_BOARD_PX4FMU)
+ } else if (!strcmp(verb, "mode_serial")) {
+ new_mode = PORT_FULL_SERIAL;
+
} else if (!strcmp(verb, "mode_gpio_serial")) {
new_mode = PORT_GPIO_AND_SERIAL;
@@ -1067,6 +1195,7 @@ fmu_main(int argc, char *argv[])
} else if (!strcmp(verb, "mode_pwm_gpio")) {
new_mode = PORT_PWM_AND_GPIO;
+#endif
}
/* was a new mode set? */
@@ -1088,6 +1217,10 @@ fmu_main(int argc, char *argv[])
fake(argc - 1, argv + 1);
fprintf(stderr, "FMU: unrecognised command, try:\n");
+#if defined(CONFIG_ARCH_BOARD_PX4FMU)
fprintf(stderr, " mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio\n");
+#elif defined(CONFIG_ARCH_BOARD_PX4FMUV2)
+ fprintf(stderr, " mode_gpio, mode_pwm\n");
+#endif
exit(1);
}
diff --git a/src/drivers/rgbled/module.mk b/src/drivers/rgbled/module.mk
new file mode 100644
index 000000000..39b53ec9e
--- /dev/null
+++ b/src/drivers/rgbled/module.mk
@@ -0,0 +1,6 @@
+#
+# TCA62724FMG driver for RGB LED
+#
+
+MODULE_COMMAND = rgbled
+SRCS = rgbled.cpp
diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp
new file mode 100644
index 000000000..dc1e469c0
--- /dev/null
+++ b/src/drivers/rgbled/rgbled.cpp
@@ -0,0 +1,497 @@
+/****************************************************************************
+ *
+ * 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 rgbled.cpp
+ *
+ * Driver for the onboard RGB LED controller (TCA62724FMG) connected via I2C.
+ *
+ *
+ */
+
+#include <nuttx/config.h>
+
+#include <arch/board/board.h>
+#include <drivers/device/i2c.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <ctype.h>
+
+#include <nuttx/wqueue.h>
+
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+#include <systemlib/systemlib.h>
+
+#include "device/rgbled.h"
+
+#define LED_ONTIME 120
+#define LED_OFFTIME 120
+
+#define ADDR PX4_I2C_OBDEV_LED /**< I2C adress of TCA62724FMG */
+#define SUB_ADDR_START 0x01 /**< write everything (with auto-increment) */
+#define SUB_ADDR_PWM0 0x81 /**< blue (without auto-increment) */
+#define SUB_ADDR_PWM1 0x82 /**< green (without auto-increment) */
+#define SUB_ADDR_PWM2 0x83 /**< red (without auto-increment) */
+#define SUB_ADDR_SETTINGS 0x84 /**< settings (without auto-increment)*/
+
+#define SETTING_NOT_POWERSAVE 0x01 /**< power-save mode not off */
+#define SETTING_ENABLE 0x02 /**< on */
+
+
+enum ledModes {
+ LED_MODE_TEST,
+ LED_MODE_SYSTEMSTATE,
+ LED_MODE_OFF
+};
+
+class RGBLED : public device::I2C
+{
+public:
+ RGBLED(int bus, int rgbled);
+ virtual ~RGBLED();
+
+
+ virtual int init();
+ virtual int probe();
+ virtual int info();
+ virtual int setMode(enum ledModes mode);
+ virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
+
+private:
+
+ enum ledColors {
+ LED_COLOR_OFF,
+ LED_COLOR_RED,
+ LED_COLOR_YELLOW,
+ LED_COLOR_PURPLE,
+ LED_COLOR_GREEN,
+ LED_COLOR_BLUE,
+ LED_COLOR_WHITE,
+ LED_COLOR_AMBER,
+ };
+
+ enum ledBlinkModes {
+ LED_BLINK_ON,
+ LED_BLINK_OFF
+ };
+
+ work_s _work;
+
+ int led_colors[8];
+ int led_blink;
+
+ int mode;
+ int running;
+
+ void setLEDColor(int ledcolor);
+ static void led_trampoline(void *arg);
+ void led();
+
+ int set(bool on, uint8_t r, uint8_t g, uint8_t b);
+ int set_on(bool on);
+ int set_rgb(uint8_t r, uint8_t g, uint8_t b);
+ int get(bool &on, bool &not_powersave, uint8_t &r, uint8_t &g, uint8_t &b);
+};
+
+/* for now, we only support one RGBLED */
+namespace
+{
+ RGBLED *g_rgbled;
+}
+
+
+extern "C" __EXPORT int rgbled_main(int argc, char *argv[]);
+
+RGBLED::RGBLED(int bus, int rgbled) :
+ I2C("rgbled", RGBLED_DEVICE_PATH, bus, rgbled, 100000),
+ led_colors({0,0,0,0,0,0,0,0}),
+ led_blink(LED_BLINK_OFF),
+ mode(LED_MODE_OFF),
+ running(false)
+{
+ memset(&_work, 0, sizeof(_work));
+}
+
+RGBLED::~RGBLED()
+{
+}
+
+int
+RGBLED::init()
+{
+ int ret;
+ ret = I2C::init();
+
+ if (ret != OK) {
+ warnx("I2C init failed");
+ return ret;
+ }
+
+ /* start off */
+ set(false, 0, 0, 0);
+
+ return OK;
+}
+
+int
+RGBLED::setMode(enum ledModes new_mode)
+{
+ switch (new_mode) {
+ case LED_MODE_SYSTEMSTATE:
+ case LED_MODE_TEST:
+ mode = new_mode;
+ if (!running) {
+ running = true;
+ set_on(true);
+ work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, 1);
+ }
+ break;
+ case LED_MODE_OFF:
+
+ default:
+ if (running) {
+ running = false;
+ set_on(false);
+ }
+ mode = LED_MODE_OFF;
+ break;
+ }
+
+ return OK;
+}
+
+int
+RGBLED::probe()
+{
+ int ret;
+ bool on, not_powersave;
+ uint8_t r, g, b;
+
+ ret = get(on, not_powersave, r, g, b);
+
+ return ret;
+}
+
+int
+RGBLED::info()
+{
+ int ret;
+ bool on, not_powersave;
+ uint8_t r, g, b;
+
+ ret = get(on, not_powersave, r, g, b);
+
+ if (ret == OK) {
+ /* we don't care about power-save mode */
+ log("state: %s", on ? "ON" : "OFF");
+ log("red: %u, green: %u, blue: %u", (unsigned)r, (unsigned)g, (unsigned)b);
+ } else {
+ warnx("failed to read led");
+ }
+
+ return ret;
+}
+
+int
+RGBLED::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+ int ret = ENOTTY;
+
+ switch (cmd) {
+
+ default:
+ break;
+ }
+
+ return ret;
+}
+
+
+void
+RGBLED::led_trampoline(void *arg)
+{
+ RGBLED *rgbl = (RGBLED *)arg;
+
+ rgbl->led();
+}
+
+
+
+void
+RGBLED::led()
+{
+ static int led_thread_runcount=0;
+ static int led_interval = 1000;
+
+ switch (mode) {
+ case LED_MODE_TEST:
+ /* Demo LED pattern for now */
+ led_colors[0] = LED_COLOR_YELLOW;
+ led_colors[1] = LED_COLOR_AMBER;
+ led_colors[2] = LED_COLOR_RED;
+ led_colors[3] = LED_COLOR_PURPLE;
+ led_colors[4] = LED_COLOR_BLUE;
+ led_colors[5] = LED_COLOR_GREEN;
+ led_colors[6] = LED_COLOR_WHITE;
+ led_colors[7] = LED_COLOR_OFF;
+ led_blink = LED_BLINK_ON;
+ break;
+
+ case LED_MODE_SYSTEMSTATE:
+ /* XXX TODO set pattern */
+ led_colors[0] = LED_COLOR_OFF;
+ led_colors[1] = LED_COLOR_OFF;
+ led_colors[2] = LED_COLOR_OFF;
+ led_colors[3] = LED_COLOR_OFF;
+ led_colors[4] = LED_COLOR_OFF;
+ led_colors[5] = LED_COLOR_OFF;
+ led_colors[6] = LED_COLOR_OFF;
+ led_colors[7] = LED_COLOR_OFF;
+ led_blink = LED_BLINK_OFF;
+
+ break;
+
+ case LED_MODE_OFF:
+ default:
+ return;
+ break;
+ }
+
+
+ if (led_thread_runcount & 1) {
+ if (led_blink == LED_BLINK_ON)
+ setLEDColor(LED_COLOR_OFF);
+ led_interval = LED_OFFTIME;
+ } else {
+ setLEDColor(led_colors[(led_thread_runcount/2) % 8]);
+ led_interval = LED_ONTIME;
+ }
+
+ led_thread_runcount++;
+
+
+ if(running) {
+ /* re-queue ourselves to run again later */
+ work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, led_interval);
+ } else {
+ set_on(false);
+ }
+}
+
+void RGBLED::setLEDColor(int ledcolor) {
+ switch (ledcolor) {
+ case LED_COLOR_OFF: // off
+ set_rgb(0,0,0);
+ break;
+ case LED_COLOR_RED: // red
+ set_rgb(255,0,0);
+ break;
+ case LED_COLOR_YELLOW: // yellow
+ set_rgb(255,70,0);
+ break;
+ case LED_COLOR_PURPLE: // purple
+ set_rgb(255,0,255);
+ break;
+ case LED_COLOR_GREEN: // green
+ set_rgb(0,255,0);
+ break;
+ case LED_COLOR_BLUE: // blue
+ set_rgb(0,0,255);
+ break;
+ case LED_COLOR_WHITE: // white
+ set_rgb(255,255,255);
+ break;
+ case LED_COLOR_AMBER: // amber
+ set_rgb(255,20,0);
+ break;
+ }
+}
+
+int
+RGBLED::set(bool on, uint8_t r, uint8_t g, uint8_t b)
+{
+ uint8_t settings_byte = 0;
+
+ if (on)
+ settings_byte |= SETTING_ENABLE;
+/* powersave not used */
+// if (not_powersave)
+ settings_byte |= SETTING_NOT_POWERSAVE;
+
+ const uint8_t msg[5] = { SUB_ADDR_START, (uint8_t)(b*15/255), (uint8_t)(g*15/255), (uint8_t)(r*15/255), settings_byte};
+
+ return transfer(msg, sizeof(msg), nullptr, 0);
+}
+
+int
+RGBLED::set_on(bool on)
+{
+ uint8_t settings_byte = 0;
+
+ if (on)
+ settings_byte |= SETTING_ENABLE;
+
+/* powersave not used */
+// if (not_powersave)
+ settings_byte |= SETTING_NOT_POWERSAVE;
+
+ const uint8_t msg[2] = { SUB_ADDR_SETTINGS, settings_byte};
+
+ return transfer(msg, sizeof(msg), nullptr, 0);
+}
+
+int
+RGBLED::set_rgb(uint8_t r, uint8_t g, uint8_t b)
+{
+ const uint8_t msg[6] = { SUB_ADDR_PWM0, (uint8_t)(b*15/255), SUB_ADDR_PWM1, (uint8_t)(g*15/255), SUB_ADDR_PWM2, (uint8_t)(r*15/255)};
+
+ return transfer(msg, sizeof(msg), nullptr, 0);
+}
+
+
+int
+RGBLED::get(bool &on, bool &not_powersave, uint8_t &r, uint8_t &g, uint8_t &b)
+{
+ uint8_t result[2];
+ int ret;
+
+ ret = transfer(nullptr, 0, &result[0], 2);
+
+ if (ret == OK) {
+ on = result[0] & SETTING_ENABLE;
+ not_powersave = result[0] & SETTING_NOT_POWERSAVE;
+ /* XXX check, looks wrong */
+ r = (result[0] & 0x0f)*255/15;
+ g = (result[1] & 0xf0)*255/15;
+ b = (result[1] & 0x0f)*255/15;
+ }
+
+ return ret;
+}
+
+void rgbled_usage();
+
+
+void rgbled_usage() {
+ warnx("missing command: try 'start', 'systemstate', 'test', 'info', 'off'");
+ warnx("options:");
+ warnx("\t-b --bus i2cbus (3)");
+ warnx("\t-a --ddr addr (9)");
+}
+
+int
+rgbled_main(int argc, char *argv[])
+{
+
+ int i2cdevice = PX4_I2C_BUS_LED;
+ int rgbledadr = ADDR; /* 7bit */
+
+ int x;
+
+ for (x = 1; x < argc; x++) {
+ if (strcmp(argv[x], "-b") == 0 || strcmp(argv[x], "--bus") == 0) {
+ if (argc > x + 1) {
+ i2cdevice = atoi(argv[x + 1]);
+ }
+ }
+
+ if (strcmp(argv[x], "-a") == 0 || strcmp(argv[x], "--addr") == 0) {
+ if (argc > x + 1) {
+ rgbledadr = atoi(argv[x + 1]);
+ }
+ }
+
+ }
+
+ if (!strcmp(argv[1], "start")) {
+ if (g_rgbled != nullptr)
+ errx(1, "already started");
+
+ g_rgbled = new RGBLED(i2cdevice, rgbledadr);
+
+ if (g_rgbled == nullptr)
+ errx(1, "new failed");
+
+ if (OK != g_rgbled->init()) {
+ delete g_rgbled;
+ g_rgbled = nullptr;
+ errx(1, "init failed");
+ }
+
+ exit(0);
+ }
+
+
+ if (g_rgbled == nullptr) {
+ fprintf(stderr, "not started\n");
+ rgbled_usage();
+ exit(0);
+ }
+
+ if (!strcmp(argv[1], "test")) {
+ g_rgbled->setMode(LED_MODE_TEST);
+ exit(0);
+ }
+
+ if (!strcmp(argv[1], "systemstate")) {
+ g_rgbled->setMode(LED_MODE_SYSTEMSTATE);
+ exit(0);
+ }
+
+ if (!strcmp(argv[1], "info")) {
+ g_rgbled->info();
+ exit(0);
+ }
+
+ if (!strcmp(argv[1], "off")) {
+ g_rgbled->setMode(LED_MODE_OFF);
+ exit(0);
+ }
+
+
+ /* things that require access to the device */
+ int fd = open(RGBLED_DEVICE_PATH, 0);
+ if (fd < 0)
+ err(1, "can't open RGBLED device");
+
+ rgbled_usage();
+ exit(0);
+}
diff --git a/src/include/device/rgbled.h b/src/include/device/rgbled.h
new file mode 100644
index 000000000..a18920892
--- /dev/null
+++ b/src/include/device/rgbled.h
@@ -0,0 +1,67 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file rgbled.h
+ *
+ * RGB led device API
+ */
+
+#pragma once
+
+#include <stdint.h>
+#include <sys/ioctl.h>
+
+/* more devices will be 1, 2, etc */
+#define RGBLED_DEVICE_PATH "/dev/rgbled0"
+
+/*
+ * ioctl() definitions
+ */
+
+#define _RGBLEDIOCBASE (0x2900)
+#define _RGBLEDIOC(_n) (_IOC(_RGBLEDIOCBASE, _n))
+
+/** play the named script in *(char *)arg, repeating forever */
+#define RGBLED_PLAY_SCRIPT_NAMED _RGBLEDIOC(1)
+
+/** play the numbered script in (arg), repeating forever */
+#define RGBLED_PLAY_SCRIPT _RGBLEDIOC(2)
+
+/**
+ * Set the user script; (arg) is a pointer to an array of script lines,
+ * where each line is an array of four bytes giving <duration>, <command>, arg[0-2]
+ *
+ * The script is terminated by a zero command.
+ */
+#define RGBLED_SET_USER_SCRIPT _RGBLEDIOC(3)
diff --git a/src/modules/px4iofirmware/i2c.c b/src/modules/px4iofirmware/i2c.c
index 4485daa5b..10aeb5c9f 100644
--- a/src/modules/px4iofirmware/i2c.c
+++ b/src/modules/px4iofirmware/i2c.c
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ * Copyright (C) 2012,2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -69,6 +69,7 @@ static void i2c_rx_setup(void);
static void i2c_tx_setup(void);
static void i2c_rx_complete(void);
static void i2c_tx_complete(void);
+static void i2c_dump(void);
static DMA_HANDLE rx_dma;
static DMA_HANDLE tx_dma;
@@ -92,7 +93,7 @@ enum {
} direction;
void
-i2c_init(void)
+interface_init(void)
{
debug("i2c init");
@@ -148,12 +149,18 @@ i2c_init(void)
#endif
}
+void
+interface_tick()
+{
+}
+
/*
reset the I2C bus
used to recover from lockups
*/
-void i2c_reset(void)
+void
+i2c_reset(void)
{
rCR1 |= I2C_CR1_SWRST;
rCR1 = 0;
@@ -330,7 +337,7 @@ i2c_tx_complete(void)
i2c_tx_setup();
}
-void
+static void
i2c_dump(void)
{
debug("CR1 0x%08x CR2 0x%08x", rCR1, rCR2);
diff --git a/src/modules/px4iofirmware/module.mk b/src/modules/px4iofirmware/module.mk
index 6379366f4..4dd1aa8d7 100644
--- a/src/modules/px4iofirmware/module.mk
+++ b/src/modules/px4iofirmware/module.mk
@@ -3,17 +3,22 @@
SRCS = adc.c \
controls.c \
dsm.c \
- i2c.c \
px4io.c \
registers.c \
safety.c \
sbus.c \
../systemlib/up_cxxinitialize.c \
- ../systemlib/hx_stream.c \
../systemlib/perf_counter.c \
mixer.cpp \
../systemlib/mixer/mixer.cpp \
../systemlib/mixer/mixer_group.cpp \
../systemlib/mixer/mixer_multirotor.cpp \
../systemlib/mixer/mixer_simple.cpp \
- \ No newline at end of file
+
+ifeq ($(BOARD),px4io)
+SRCS += i2c.c
+endif
+ifeq ($(BOARD),px4iov2)
+SRCS += serial.c \
+ ../systemlib/hx_stream.c
+endif
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index 674f9dddd..90d63ea1a 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -36,7 +36,8 @@
/**
* @file protocol.h
*
- * PX4IO I2C interface protocol.
+ * PX4IO interface protocol
+ * ========================
*
* Communication is performed via writes to and reads from 16-bit virtual
* registers organised into pages of 255 registers each.
@@ -62,6 +63,29 @@
* Note that the implementation of readable pages prefers registers within
* readable pages to be densely packed. Page numbers do not need to be
* packed.
+ *
+ * PX4IO I2C interface notes
+ * -------------------------
+ *
+ * Register read/write operations are mapped directly to PX4IO register
+ * read/write operations.
+ *
+ * PX4IO Serial interface notes
+ * ----------------------------
+ *
+ * The MSB of the page number is used to distinguish between read and
+ * write operations. If set, the operation is a write and additional
+ * data is expected to follow in the packet as for I2C writes.
+ *
+ * If clear, the packet is expected to contain a single byte giving the
+ * number of registers to be read. PX4IO will respond with a packet containing
+ * the same header (page, offset) and the requested data.
+ *
+ * If a read is requested when PX4IO does not have buffer space to store
+ * the reply, the request will be dropped. PX4IO is always configured with
+ * enough space to receive one full-sized write and one read request, and
+ * to send one full-sized read response.
+ *
*/
#define PX4IO_CONTROL_CHANNELS 8
@@ -75,12 +99,14 @@
#define REG_TO_FLOAT(_reg) ((float)REG_TO_SIGNED(_reg) / 10000.0f)
#define FLOAT_TO_REG(_float) SIGNED_TO_REG((int16_t)((_float) * 10000.0f))
+#define PX4IO_PAGE_WRITE (1<<7)
+
/* static configuration page */
#define PX4IO_PAGE_CONFIG 0
#define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* magic numbers TBD */
#define PX4IO_P_CONFIG_SOFTWARE_VERSION 1 /* magic numbers TBD */
#define PX4IO_P_CONFIG_BOOTLOADER_VERSION 2 /* get this how? */
-#define PX4IO_P_CONFIG_MAX_TRANSFER 3 /* maximum I2C transfer size */
+#define PX4IO_P_CONFIG_MAX_TRANSFER 3 /* maximum packet transfer size */
#define PX4IO_P_CONFIG_CONTROL_COUNT 4 /* hardcoded max control count supported */
#define PX4IO_P_CONFIG_ACTUATOR_COUNT 5 /* hardcoded max actuator output count */
#define PX4IO_P_CONFIG_RC_INPUT_COUNT 6 /* hardcoded max R/C input count supported */
@@ -142,7 +168,7 @@
#define PX4IO_RATE_MAP_BASE 0 /* 0..CONFIG_ACTUATOR_COUNT bitmaps of PWM rate groups */
/* setup page */
-#define PX4IO_PAGE_SETUP 100
+#define PX4IO_PAGE_SETUP 64
#define PX4IO_P_SETUP_FEATURES 0
#define PX4IO_P_SETUP_ARMING 1 /* arming controls */
@@ -160,13 +186,13 @@
#define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */
/* autopilot control values, -10000..10000 */
-#define PX4IO_PAGE_CONTROLS 101 /* 0..CONFIG_CONTROL_COUNT */
+#define PX4IO_PAGE_CONTROLS 65 /* 0..CONFIG_CONTROL_COUNT */
/* raw text load to the mixer parser - ignores offset */
-#define PX4IO_PAGE_MIXERLOAD 102
+#define PX4IO_PAGE_MIXERLOAD 66 /* see px4io_mixdata structure below */
/* R/C channel config */
-#define PX4IO_PAGE_RC_CONFIG 103 /* R/C input configuration */
+#define PX4IO_PAGE_RC_CONFIG 67 /* R/C input configuration */
#define PX4IO_P_RC_CONFIG_MIN 0 /* lowest input value */
#define PX4IO_P_RC_CONFIG_CENTER 1 /* center input value */
#define PX4IO_P_RC_CONFIG_MAX 2 /* highest input value */
@@ -178,10 +204,10 @@
#define PX4IO_P_RC_CONFIG_STRIDE 6 /* spacing between channel config data */
/* PWM output - overrides mixer */
-#define PX4IO_PAGE_DIRECT_PWM 104 /* 0..CONFIG_ACTUATOR_COUNT-1 */
+#define PX4IO_PAGE_DIRECT_PWM 68 /* 0..CONFIG_ACTUATOR_COUNT-1 */
/* PWM failsafe values - zero disables the output */
-#define PX4IO_PAGE_FAILSAFE_PWM 105 /* 0..CONFIG_ACTUATOR_COUNT-1 */
+#define PX4IO_PAGE_FAILSAFE_PWM 69 /* 0..CONFIG_ACTUATOR_COUNT-1 */
/**
* As-needed mixer data upload.
diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c
index bc8dfc116..385920d53 100644
--- a/src/modules/px4iofirmware/px4io.c
+++ b/src/modules/px4iofirmware/px4io.c
@@ -64,11 +64,6 @@ struct sys_state_s system_state;
static struct hrt_call serial_dma_call;
-#ifdef CONFIG_STM32_I2C1
-/* store i2c reset count XXX this should be a register, together with other error counters */
-volatile uint32_t i2c_loop_resets = 0;
-#endif
-
/*
* a set of debug buffers to allow us to send debug information from ISRs
*/
@@ -147,7 +142,7 @@ user_start(int argc, char *argv[])
LED_BLUE(false);
LED_SAFETY(false);
- /* turn on servo power */
+ /* turn on servo power (if supported) */
POWER_SERVO(true);
/* start the safety switch handler */
@@ -159,10 +154,11 @@ user_start(int argc, char *argv[])
/* initialise the control inputs */
controls_init();
-#ifdef CONFIG_STM32_I2C1
- /* start the i2c handler */
- i2c_init();
-#endif
+ /* start the FMU interface */
+ interface_init();
+
+ /* add a performance counter for the interface */
+ perf_counter_t interface_perf = perf_alloc(PC_ELAPSED, "interface");
/* add a performance counter for mixing */
perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix");
@@ -205,6 +201,11 @@ user_start(int argc, char *argv[])
/* track the rate at which the loop is running */
perf_count(loop_perf);
+ /* kick the interface */
+ perf_begin(interface_perf);
+ interface_tick();
+ perf_end(interface_perf);
+
/* kick the mixer */
perf_begin(mixer_perf);
mixer_tick();
@@ -223,12 +224,11 @@ user_start(int argc, char *argv[])
struct mallinfo minfo = mallinfo();
- isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x r=%u m=%u",
+ isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x m=%u",
(unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG],
(unsigned)r_status_flags,
(unsigned)r_setup_arming,
(unsigned)r_setup_features,
- (unsigned)i2c_loop_resets,
(unsigned)minfo.mxordblk);
last_debug_time = hrt_absolute_time();
}
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index 272cdb7bf..2077e6244 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -125,16 +125,16 @@ extern struct sys_state_s system_state;
#define POWER_SERVO(_s) stm32_gpiowrite(GPIO_SERVO_PWR_EN, (_s))
#ifdef GPIO_ACC1_PWR_EN
- #define POWER_ACC1(_s) stm32_gpiowrite(GPIO_ACC1_PWR_EN, (_s))
+# define POWER_ACC1(_s) stm32_gpiowrite(GPIO_ACC1_PWR_EN, (_s))
#endif
#ifdef GPIO_ACC2_PWR_EN
- #define POWER_ACC2(_s) stm32_gpiowrite(GPIO_ACC2_PWR_EN, (_s))
+# define POWER_ACC2(_s) stm32_gpiowrite(GPIO_ACC2_PWR_EN, (_s))
#endif
#ifdef GPIO_RELAY1_EN
- #define POWER_RELAY1(_s) stm32_gpiowrite(GPIO_RELAY1_EN, (_s))
+# define POWER_RELAY1(_s) stm32_gpiowrite(GPIO_RELAY1_EN, (_s))
#endif
#ifdef GPIO_RELAY2_EN
- #define POWER_RELAY2(_s) stm32_gpiowrite(GPIO_RELAY2_EN, (_s))
+# define POWER_RELAY2(_s) stm32_gpiowrite(GPIO_RELAY2_EN, (_s))
#endif
#define OVERCURRENT_ACC (!stm32_gpioread(GPIO_ACC_OC_DETECT))
@@ -156,12 +156,11 @@ extern void mixer_handle_text(const void *buffer, size_t length);
*/
extern void safety_init(void);
-#ifdef CONFIG_STM32_I2C1
/**
* FMU communications
*/
-extern void i2c_init(void);
-#endif
+extern void interface_init(void);
+extern void interface_tick(void);
/**
* Register space
@@ -190,10 +189,5 @@ extern bool sbus_input(uint16_t *values, uint16_t *num_values);
/** global debug level for isr_debug() */
extern volatile uint8_t debug_level;
-/* send a debug message to the console */
-extern void isr_debug(uint8_t level, const char *fmt, ...);
-
-#ifdef CONFIG_STM32_I2C1
-void i2c_dump(void);
-void i2c_reset(void);
-#endif
+/** send a debug message to the console */
+extern void isr_debug(uint8_t level, const char *fmt, ...);
diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c
new file mode 100644
index 000000000..bf9456e94
--- /dev/null
+++ b/src/modules/px4iofirmware/serial.c
@@ -0,0 +1,133 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2012,2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file serial.c
+ *
+ * Serial communication for the PX4IO module.
+ */
+
+#include <stdint.h>
+#include <unistd.h>
+#include <termios.h>
+#include <fcntl.h>
+#include <string.h>
+
+#include <nuttx/arch.h>
+#include <arch/board/board.h>
+
+#include <systemlib/hx_stream.h>
+
+//#define DEBUG
+#include "px4io.h"
+
+static uint8_t tx_buf[66]; /* XXX hardcoded magic number */
+
+static hx_stream_t if_stream;
+
+static void serial_callback(void *arg, const void *data, unsigned length);
+
+void
+interface_init(void)
+{
+
+ int fd = open("/dev/ttyS1", O_RDWR, O_NONBLOCK);
+ if (fd < 0) {
+ debug("serial fail");
+ return;
+ }
+
+ /* configure serial port - XXX increase port speed? */
+ struct termios t;
+ tcgetattr(fd, &t);
+ cfsetspeed(&t, 115200);
+ t.c_cflag &= ~(CSTOPB | PARENB);
+ tcsetattr(fd, TCSANOW, &t);
+
+ /* allocate the HX stream we'll use for communication */
+ if_stream = hx_stream_init(fd, serial_callback, NULL);
+
+ /* XXX add stream stats counters? */
+
+ debug("serial init");
+}
+
+void
+interface_tick()
+{
+ /* process incoming bytes */
+ hx_stream_rx(if_stream);
+}
+
+static void
+serial_callback(void *arg, const void *data, unsigned length)
+{
+ const uint8_t *message = (const uint8_t *)data;
+
+ /* malformed frame, ignore it */
+ if (length < 2)
+ return;
+
+ /* it's a write operation, pass it to the register API */
+ if (message[0] & PX4IO_PAGE_WRITE) {
+ registers_set(message[0] & ~PX4IO_PAGE_WRITE, message[1],
+ (const uint16_t *)&message[2],
+ (length - 2) / 2);
+
+ return;
+ }
+
+ /* it's a read - must contain length byte */
+ if (length != 3)
+ return;
+ uint16_t *registers;
+ unsigned count;
+
+ tx_buf[0] = message[0];
+ tx_buf[1] = message[1];
+
+ /* get registers for response, send an empty reply on error */
+ if (registers_get(message[0], message[1], &registers, &count) < 0)
+ count = 0;
+
+ /* fill buffer with message, limited by length */
+#define TX_MAX ((sizeof(tx_buf) - 2) / 2)
+ if (count > TX_MAX)
+ count = TX_MAX;
+ if (count > message[2])
+ count = message[2];
+ memcpy(&tx_buf[2], registers, count * 2);
+
+ /* try to send the message */
+ hx_stream_send(if_stream, tx_buf, count * 2 + 2);
+} \ No newline at end of file
diff --git a/src/modules/systemlib/hx_stream.c b/src/modules/systemlib/hx_stream.c
index 8d77f14a8..88f7f762c 100644
--- a/src/modules/systemlib/hx_stream.c
+++ b/src/modules/systemlib/hx_stream.c
@@ -76,6 +76,7 @@ struct hx_stream {
static void hx_tx_raw(hx_stream_t stream, uint8_t c);
static void hx_tx_raw(hx_stream_t stream, uint8_t c);
static int hx_rx_frame(hx_stream_t stream);
+static bool hx_rx_char(hx_stream_t stream, uint8_t c);
static void
hx_tx_raw(hx_stream_t stream, uint8_t c)
@@ -224,13 +225,13 @@ hx_stream_send(hx_stream_t stream,
return 0;
}
-void
-hx_stream_rx(hx_stream_t stream, uint8_t c)
+static bool
+hx_rx_char(hx_stream_t stream, uint8_t c)
{
/* frame end? */
if (c == FBO) {
hx_rx_frame(stream);
- return;
+ return true;
}
/* escaped? */
@@ -241,10 +242,43 @@ hx_stream_rx(hx_stream_t stream, uint8_t c)
} else if (c == CEO) {
/* now escaped, ignore the byte */
stream->escaped = true;
- return;
+ return false;
}
/* save for later */
if (stream->frame_bytes < sizeof(stream->buf))
stream->buf[stream->frame_bytes++] = c;
+
+ return false;
+}
+
+void
+hx_stream_rx_char(hx_stream_t stream, uint8_t c)
+{
+ hx_rx_char(stream, c);
+}
+
+int
+hx_stream_rx(hx_stream_t stream)
+{
+ uint16_t buf[16];
+ ssize_t len;
+
+ /* read bytes */
+ len = read(stream->fd, buf, sizeof(buf));
+ if (len <= 0) {
+
+ /* nonblocking stream and no data */
+ if (errno == EWOULDBLOCK)
+ return 0;
+
+ /* error or EOF */
+ return -errno;
+ }
+
+ /* process received characters */
+ for (int i = 0; i < len; i++)
+ hx_rx_char(stream, buf[i]);
+
+ return 0;
}
diff --git a/src/modules/systemlib/hx_stream.h b/src/modules/systemlib/hx_stream.h
index 128689953..be4850f74 100644
--- a/src/modules/systemlib/hx_stream.h
+++ b/src/modules/systemlib/hx_stream.h
@@ -114,9 +114,25 @@ __EXPORT extern int hx_stream_send(hx_stream_t stream,
* @param stream A handle returned from hx_stream_init.
* @param c The character to process.
*/
-__EXPORT extern void hx_stream_rx(hx_stream_t stream,
+__EXPORT extern void hx_stream_rx_char(hx_stream_t stream,
uint8_t c);
+/**
+ * Handle received bytes from the stream.
+ *
+ * Note that this interface should only be used with blocking streams
+ * when it is OK for the call to block until a frame is received.
+ *
+ * When used with a non-blocking stream, it will typically return
+ * immediately, or after handling a received frame.
+ *
+ * @param stream A handle returned from hx_stream_init.
+ * @return -errno on error, nonzero if a frame
+ * has been received, or if not enough
+ * bytes are available to complete a frame.
+ */
+__EXPORT extern int hx_stream_rx(hx_stream_t stream);
+
__END_DECLS
#endif
diff --git a/src/systemcmds/ramtron/module.mk b/src/systemcmds/ramtron/module.mk
new file mode 100644
index 000000000..e4eb1d143
--- /dev/null
+++ b/src/systemcmds/ramtron/module.mk
@@ -0,0 +1,6 @@
+#
+# RAMTRON file system driver
+#
+
+MODULE_COMMAND = ramtron
+SRCS = ramtron.c
diff --git a/src/systemcmds/ramtron/ramtron.c b/src/systemcmds/ramtron/ramtron.c
new file mode 100644
index 000000000..5e9499c55
--- /dev/null
+++ b/src/systemcmds/ramtron/ramtron.c
@@ -0,0 +1,268 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ * Author: Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file ramtron.c
+ *
+ * ramtron service and utility app.
+ */
+
+#include <nuttx/config.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdbool.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <sys/mount.h>
+#include <sys/ioctl.h>
+#include <sys/stat.h>
+
+#include <nuttx/spi.h>
+#include <nuttx/mtd.h>
+#include <nuttx/fs/nxffs.h>
+#include <nuttx/fs/ioctl.h>
+
+#include <arch/board/board.h>
+
+#include "systemlib/systemlib.h"
+#include "systemlib/param/param.h"
+#include "systemlib/err.h"
+
+__EXPORT int ramtron_main(int argc, char *argv[]);
+
+static void ramtron_attach(void);
+static void ramtron_start(void);
+static void ramtron_erase(void);
+static void ramtron_ioctl(unsigned operation);
+static void ramtron_save(const char *name);
+static void ramtron_load(const char *name);
+static void ramtron_test(void);
+
+static bool attached = false;
+static bool started = false;
+static struct mtd_dev_s *ramtron_mtd;
+
+int ramtron_main(int argc, char *argv[])
+{
+ if (argc >= 2) {
+ if (!strcmp(argv[1], "start"))
+ ramtron_start();
+
+ if (!strcmp(argv[1], "save_param"))
+ ramtron_save(argv[2]);
+
+ if (!strcmp(argv[1], "load_param"))
+ ramtron_load(argv[2]);
+
+ if (!strcmp(argv[1], "erase"))
+ ramtron_erase();
+
+ if (!strcmp(argv[1], "test"))
+ ramtron_test();
+
+ if (0) { /* these actually require a file on the filesystem... */
+
+ if (!strcmp(argv[1], "reformat"))
+ ramtron_ioctl(FIOC_REFORMAT);
+
+ if (!strcmp(argv[1], "repack"))
+ ramtron_ioctl(FIOC_OPTIMIZE);
+ }
+ }
+
+ errx(1, "expected a command, try 'start'\n\t'save_param /ramtron/parameters'\n\t'load_param /ramtron/parameters'\n\t'erase'\n");
+}
+
+struct mtd_dev_s *ramtron_initialize(FAR struct spi_dev_s *dev);
+
+
+static void
+ramtron_attach(void)
+{
+ /* find the right spi */
+ struct spi_dev_s *spi = up_spiinitialize(2);
+ /* this resets the spi bus, set correct bus speed again */
+ // xxx set in ramtron driver, leave this out
+// SPI_SETFREQUENCY(spi, 4000000);
+ SPI_SETFREQUENCY(spi, 375000000);
+ SPI_SETBITS(spi, 8);
+ SPI_SETMODE(spi, SPIDEV_MODE3);
+ SPI_SELECT(spi, SPIDEV_FLASH, false);
+
+ if (spi == NULL)
+ errx(1, "failed to locate spi bus");
+
+ /* start the MTD driver, attempt 5 times */
+ for (int i = 0; i < 5; i++) {
+ ramtron_mtd = ramtron_initialize(spi);
+ if (ramtron_mtd) {
+ /* abort on first valid result */
+ if (i > 0) {
+ warnx("warning: ramtron needed %d attempts to attach", i+1);
+ }
+ break;
+ }
+ }
+
+ /* if last attempt is still unsuccessful, abort */
+ if (ramtron_mtd == NULL)
+ errx(1, "failed to initialize ramtron driver");
+
+ attached = true;
+}
+
+static void
+ramtron_start(void)
+{
+ int ret;
+
+ if (started)
+ errx(1, "ramtron already mounted");
+
+ if (!attached)
+ ramtron_attach();
+
+ /* start NXFFS */
+ ret = nxffs_initialize(ramtron_mtd);
+
+ if (ret < 0)
+ errx(1, "failed to initialize NXFFS - erase ramtron to reformat");
+
+ /* mount the ramtron */
+ ret = mount(NULL, "/ramtron", "nxffs", 0, NULL);
+
+ if (ret < 0)
+ errx(1, "failed to mount /ramtron - erase ramtron to reformat");
+
+ started = true;
+ warnx("mounted ramtron at /ramtron");
+ exit(0);
+}
+
+//extern int at24c_nuke(void);
+
+static void
+ramtron_erase(void)
+{
+ if (!attached)
+ ramtron_attach();
+
+// if (at24c_nuke())
+ errx(1, "erase failed");
+
+ errx(0, "erase done, reboot now");
+}
+
+static void
+ramtron_ioctl(unsigned operation)
+{
+ int fd;
+
+ fd = open("/ramtron/.", 0);
+
+ if (fd < 0)
+ err(1, "open /ramtron");
+
+ if (ioctl(fd, operation, 0) < 0)
+ err(1, "ioctl");
+
+ exit(0);
+}
+
+static void
+ramtron_save(const char *name)
+{
+ if (!started)
+ errx(1, "must be started first");
+
+ if (!name)
+ err(1, "missing argument for device name, try '/ramtron/parameters'");
+
+ warnx("WARNING: 'ramtron save_param' deprecated - use 'param save' instead");
+
+ /* delete the file in case it exists */
+ unlink(name);
+
+ /* create the file */
+ int fd = open(name, O_WRONLY | O_CREAT | O_EXCL);
+
+ if (fd < 0)
+ err(1, "opening '%s' failed", name);
+
+ int result = param_export(fd, false);
+ close(fd);
+
+ if (result < 0) {
+ unlink(name);
+ errx(1, "error exporting to '%s'", name);
+ }
+
+ exit(0);
+}
+
+static void
+ramtron_load(const char *name)
+{
+ if (!started)
+ errx(1, "must be started first");
+
+ if (!name)
+ err(1, "missing argument for device name, try '/ramtron/parameters'");
+
+ warnx("WARNING: 'ramtron load_param' deprecated - use 'param load' instead");
+
+ int fd = open(name, O_RDONLY);
+
+ if (fd < 0)
+ err(1, "open '%s'", name);
+
+ int result = param_load(fd);
+ close(fd);
+
+ if (result < 0)
+ errx(1, "error importing from '%s'", name);
+
+ exit(0);
+}
+
+//extern void at24c_test(void);
+
+static void
+ramtron_test(void)
+{
+// at24c_test();
+ exit(0);
+}