diff --git a/conf/Makefile.stm32 b/conf/Makefile.stm32
index f2cc72d9db..ada3d31c48 100644
--- a/conf/Makefile.stm32
+++ b/conf/Makefile.stm32
@@ -37,7 +37,11 @@ Q=@
include $(PAPARAZZI_SRC)/conf/Makefile.arm-embedded
+ifeq ($(ARCH_L),f4)
+MCU = cortex-m4
+else
MCU = cortex-m3
+endif
#DEBUG = dwarf-2
OPT = s
#OPT = 2
@@ -98,7 +102,17 @@ $(info Using "$($(TARGET).LDSCRIPT)" as ldscript for target "$(TARGET)".)
endif
endif
-CFLAGS = -I. -I./$(ARCH) -I../ext/libopencm3/include $(INCLUDES) -D__thumb2__ -msoft-float -O$(OPT)
+CFLAGS = -I. -I./$(ARCH) -I../ext/libopencm3/include $(INCLUDES)
+CFLAGS += -D__thumb2__ -Wall -O$(OPT)
+ifeq ($(ARCH_L), )
+CFLAGS += -msoft-float
+else ifeq ($(ARCH_L),f4)
+ifndef HARD_FLOAT
+CFLAGS += -msoft-float
+else
+CFLAGS += -mfloat-abi=hard -mfpu=fpv4-sp-d16
+endif
+endif
CFLAGS += -Wl,--gc-sections
CFLAGS += -mfix-cortex-m3-ldrd
CFLAGS += -mcpu=$(MCU) -mthumb -ansi
@@ -107,33 +121,51 @@ CFLAGS += -std=gnu99
CFLAGS += -fno-common
CFLAGS += -g$(DEBUG)
CFLAGS += -ffunction-sections -fdata-sections
-
-# flags for warnings
-CFLAGS += -Wall -Wextra
CFLAGS += -Wimplicit
CFLAGS += -Wcast-align
CFLAGS += -Wpointer-arith -Wswitch
CFLAGS += -Wredundant-decls -Wreturn-type -Wshadow -Wunused
+CFLAGS += -Wa,-adhlns=$(OBJDIR)/$(notdir $(subst $(suffix $<),.lst,$<))
CFLAGS += -Wnested-externs
CFLAGS += -Wmissing-prototypes
CFLAGS += -Wstrict-prototypes
CFLAGS += -Wmissing-declarations
CFLAGS += -Wswitch-default
-
-CFLAGS += -Wa,-adhlns=$(OBJDIR)/$(notdir $(subst $(suffix $<),.lst,$<))
+ifneq ($(ARCH_L), )
+ifeq ($(ARCH_L),f4)
+CFLAGS += -DSTM32F4
+endif
+else
CFLAGS += -DSTM32F1
+endif
CFLAGS += $($(TARGET).CFLAGS)
-AFLAGS = -ahls
+AFLAGS = -ahls -mapcs-32
AFLAGS += -mcpu=$(MCU) -mthumb
AFLAGS += -x assembler-with-cpp -Wa,-adhlns=$(OBJDIR)/$(<:.S=.lst),--g$(DEBUG)
-LDFLAGS = -L../ext/libopencm3/lib
-LDFLAGS += -T$(LDSCRIPT) -nostartfiles -O$(OPT)
-LDFLAGS += -mthumb -mcpu=$(MCU) -msoft-float -mfix-cortex-m3-ldrd
+LDFLAGS += -L../ext/libopencm3/lib
+LDFLAGS += -T$(LDSCRIPT) -nostartfiles -O$(OPT) -mthumb -mcpu=$(MCU)
+
+ifeq ($(ARCH_L), )
+LDFLAGS += -mfix-cortex-m3-ldrd -msoft-float
+else ifeq ($(ARCH_L),f4)
+ifndef HARD_FLOAT
+LDFLAGS += -mfix-cortex-m3-ldrd -msoft-float
+else
+LDFLAGS += -lnosys -D__thumb2__\
+ -mfloat-abi=hard -mfpu=fpv4-sp-d16
+endif
+endif
+
LDFLAGS += -Wl,-Map=$(OBJDIR)/$(TARGET).map,--cref,--gc-sections
-LDLIBS += -lc -lm -lgcc -lopencm3_stm32f1
+ifneq ($(ARCH_L), )
+LDLIBS += -lopencm3_stm32$(ARCH_L)
+else
+LDLIBS += -lopencm3_stm32f1
+endif
+LDLIBS += -lc -lm -lgcc
CPFLAGS = -j .isr_vector -j .text -j .data
CPFLAGS_BIN = -Obinary
@@ -199,11 +231,18 @@ $(AOBJ) : $(OBJDIR)/%.o : %.S
# check which flash mode is configured
#
ifeq ($(FLASH_MODE),DFU)
+ifdef DFU_ADDR
+DFU_ADDR_CMD = --addr=$(DFU_ADDR)
+endif
+ifdef DFU_PRODUCT
+DFU_PRODUCT_CMD = --product=$(DFU_PRODUCT)
+endif
#
# DFU flash mode
upload: $(OBJDIR)/$(TARGET).bin
@echo "Using stm32 mem dfu loader"
- $(PYTHON) $(PAPARAZZI_SRC)/sw/tools/dfu/stm32_mem.py $^
+ $(PYTHON) $(PAPARAZZI_SRC)/sw/tools/dfu/stm32_mem.py $(DFU_PRODUCT_CMD) $(DFU_ADDR_CMD) $^
+
#
# serial flash mode
else ifeq ($(FLASH_MODE),SERIAL)
@@ -246,6 +285,12 @@ endif
# SWD flash mode
else ifeq ($(FLASH_MODE),SWD)
# only works if BMP_PORT is defined
+ifeq ($(STLINK),y)
+STLINK_ADDR ?= 0x08000000
+upload: $(OBJDIR)/$(TARGET).bin
+ @echo "Using ST-LINK with SWD at $(STLINK_ADDR)"
+ $(Q)st-flash write $^ $(STLINK_ADDR)
+else
BMP_PORT ?= /dev/ttyACM0
BMP_UPLOAD_SCRIPT ?= $(PAPARAZZI_SRC)/sw/tools/flash_scripts/bmp_swd_flash.scr
upload: $(OBJDIR)/$(TARGET).elf
@@ -257,6 +302,7 @@ upload: $(OBJDIR)/$(TARGET).elf
-ex 'target extended-remote $(BMP_PORT)' \
-x $(BMP_UPLOAD_SCRIPT) \
$<
+endif
#
# no known flash mode
else
diff --git a/conf/airframes/ENAC/fixed-wing/apogee.xml b/conf/airframes/ENAC/fixed-wing/apogee.xml
new file mode 100644
index 0000000000..afe5f8a96f
--- /dev/null
+++ b/conf/airframes/ENAC/fixed-wing/apogee.xml
@@ -0,0 +1,212 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml b/conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml
new file mode 100755
index 0000000000..00c0fd5239
--- /dev/null
+++ b/conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml
@@ -0,0 +1,249 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml b/conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml
new file mode 100755
index 0000000000..916d281c06
--- /dev/null
+++ b/conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml
@@ -0,0 +1,252 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml b/conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml
new file mode 100755
index 0000000000..c018d28048
--- /dev/null
+++ b/conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml
@@ -0,0 +1,241 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/examples/krooz_sd/krooz_sd_quad_pwm.xml b/conf/airframes/examples/krooz_sd/krooz_sd_quad_pwm.xml
new file mode 100755
index 0000000000..88b83d3cf3
--- /dev/null
+++ b/conf/airframes/examples/krooz_sd/krooz_sd_quad_pwm.xml
@@ -0,0 +1,236 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml
index e9e9aa9dba..1f4331d59a 100644
--- a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml
+++ b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml
@@ -32,7 +32,7 @@
-
+
diff --git a/conf/airframes/obsolete/mm/extra/logger_sd_adc.xml b/conf/airframes/obsolete/mm/extra/logger_sd_adc.xml
index 817515c109..b68667ad32 100644
--- a/conf/airframes/obsolete/mm/extra/logger_sd_adc.xml
+++ b/conf/airframes/obsolete/mm/extra/logger_sd_adc.xml
@@ -68,8 +68,8 @@ ap.CFLAGS += -DLOG_XBEE
#ap.CFLAGS += -DLOG_PPRZ
#set the speed
-ap.CFLAGS += -DUSE_UART0 -DUART0_BAUD=B57600 -DUSE_UART0_RX_ONLY
-ap.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600 -DUSE_UART1_RX_ONLY
+ap.CFLAGS += -DUSE_UART0 -DUART0_BAUD=B57600 -DUSE_UART0_TX=FALSE
+ap.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600 -DUSE_UART1_TX=FALSE
ap.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
#set SPI interface for SD card (0 or 1)
diff --git a/conf/airframes/obsolete/mm/extra/quiet.xml b/conf/airframes/obsolete/mm/extra/quiet.xml
index c1465c0da0..7c71a31966 100644
--- a/conf/airframes/obsolete/mm/extra/quiet.xml
+++ b/conf/airframes/obsolete/mm/extra/quiet.xml
@@ -21,8 +21,8 @@ ap.srcs = sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c main_logger.
ap.CFLAGS += -DLOG_XBEE
#set the speed
-ap.CFLAGS += -DUSE_UART0 -DUART0_BAUD=B57600 -DUSE_UART0_RX_ONLY
-ap.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600 -DUSE_UART1_RX_ONLY
+ap.CFLAGS += -DUSE_UART0 -DUART0_BAUD=B57600 -DUSE_UART0_TX=FALSE
+ap.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600 -DUSE_UART1_TX=FALSE
ap.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
diff --git a/conf/boards/apogee_0.99.makefile b/conf/boards/apogee_0.99.makefile
new file mode 100644
index 0000000000..e100a6f701
--- /dev/null
+++ b/conf/boards/apogee_0.99.makefile
@@ -0,0 +1,54 @@
+# Hey Emacs, this is a -*- makefile -*-
+#
+# apogee_0.99.makefile
+#
+#
+
+BOARD=apogee
+BOARD_VERSION=0.99
+BOARD_CFG=\"boards/$(BOARD)_$(BOARD_VERSION).h\"
+
+ARCH=stm32
+ARCH_L=f4
+ARCH_DIR=stm32
+SRC_ARCH=arch/$(ARCH_DIR)
+$(TARGET).ARCHDIR = $(ARCH)
+$(TARGET).LDSCRIPT=$(SRC_ARCH)/apogee.ld
+
+HARD_FLOAT=yes
+
+# default flash mode is via usb dfu bootloader
+# possibilities: DFU, SWD
+FLASH_MODE ?= SWD
+STLINK ?= y
+
+#
+# default LED configuration
+#
+RADIO_CONTROL_LED ?= none
+BARO_LED ?= none
+AHRS_ALIGNER_LED ?= 2
+GPS_LED ?= none
+SYS_TIME_LED ?= 1
+
+#
+# default MODEM and GPS configuration
+#
+
+MODEM_PORT ?= UART1
+MODEM_BAUD ?= B57600
+
+GPS_PORT ?= UART4
+GPS_BAUD ?= B38400
+
+
+#
+# default actuator configuration
+#
+# you can use different actuators by adding a configure option to your firmware section
+# e.g.
+#
+ACTUATORS ?= actuators_pwm
+
diff --git a/conf/boards/krooz_sd.makefile b/conf/boards/krooz_sd.makefile
new file mode 100644
index 0000000000..d5679f8539
--- /dev/null
+++ b/conf/boards/krooz_sd.makefile
@@ -0,0 +1,58 @@
+# Hey Emacs, this is a -*- makefile -*-
+#
+# krooz_sd.makefile
+#
+#
+#
+
+BOARD=krooz
+BOARD_VERSION=sd
+BOARD_CFG=\"boards/$(BOARD)_$(BOARD_VERSION).h\"
+
+ARCH=stm32
+ARCH_L=f4
+HARD_FLOAT=yes
+ARCH_DIR=stm32
+SRC_ARCH=arch/$(ARCH_DIR)
+$(TARGET).ARCHDIR = $(ARCH)
+# not needed?
+$(TARGET).OOCD_INTERFACE=flossjtag
+#$(TARGET).OOCD_INTERFACE=jtagkey-tiny
+$(TARGET).LDSCRIPT=$(SRC_ARCH)/krooz.ld
+
+# -----------------------------------------------------------------------
+
+ifndef FLASH_MODE
+FLASH_MODE = DFU
+#FLASH_MODE = JTAG
+#FLASH_MODE = SERIAL
+endif
+
+DFU_ADDR = 0x8004000
+DFU_PRODUCT = any
+
+#
+#
+# some default values shared between different firmwares
+#
+#
+
+#
+# default LED configuration
+#
+RADIO_CONTROL_LED ?= none
+BARO_LED ?= none
+AHRS_ALIGNER_LED ?= 2
+GPS_LED ?= none
+SYS_TIME_LED ?= 1
+
+#
+# default uart configuration
+#
+RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT ?= UART1
+
+MODEM_PORT ?= UART5
+MODEM_BAUD ?= B57600
+
+GPS_PORT ?= UART3
+GPS_BAUD ?= B38400
diff --git a/conf/firmwares/lisa_test_progs.makefile b/conf/firmwares/lisa_test_progs.makefile
index 4d57ba3bd6..10ae9bdc04 100644
--- a/conf/firmwares/lisa_test_progs.makefile
+++ b/conf/firmwares/lisa_test_progs.makefile
@@ -63,7 +63,7 @@ SRC_AIRBORNE=.
#
PERIODIC_FREQUENCY = 512
-COMMON_TEST_CFLAGS = -I$(SRC_FIRMWARE) -I$(ARCH) -DPERIPHERALS_AUTO_INIT
+COMMON_TEST_CFLAGS = -I$(SRC_FIRMWARE) -I$(SRC_BOARD) -DPERIPHERALS_AUTO_INIT
COMMON_TEST_CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
COMMON_TEST_SRCS = $(SRC_AIRBORNE)/mcu.c \
$(SRC_ARCH)/mcu_arch.c
@@ -187,7 +187,7 @@ test_baro.srcs = $(COMMON_TEST_SRCS)
test_baro.CFLAGS += $(COMMON_TELEMETRY_CFLAGS)
test_baro.srcs += $(COMMON_TELEMETRY_SRCS)
-test_baro.CFLAGS += -I$(SRC_LISA) -I$(SRC_BOARD)
+test_baro.CFLAGS += -I$(SRC_LISA)
ifeq ($(BOARD), lisa_l)
test_baro.CFLAGS += -DUSE_I2C2
@@ -243,9 +243,9 @@ ifneq ($(RADIO_CONTROL_LED),none)
endif
test_rc_spektrum.CFLAGS += -DRADIO_CONTROL_BIND_IMPL_FUNC=radio_control_spektrum_try_bind
test_rc_spektrum.CFLAGS += -DRADIO_CONTROL_TYPE_H=\"subsystems/radio_control/spektrum.h\"
-test_rc_spektrum.CFLAGS += -DRADIO_CONTROL_SPEKTRUM_PRIMARY_PORT=$(RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT)
-test_rc_spektrum.CFLAGS += -DRADIO_CONTROL_SPEKTRUM_SECONDARY_PORT=$(RADIO_CONTROL_SPEKTRUM_SECONDARY_PORT)
-test_rc_spektrum.CFLAGS += -DOVERRIDE_$(RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT)_IRQ_HANDLER -DUSE_TIM6_IRQ
+test_rc_spektrum.CFLAGS += -DRADIO_CONTROL_SPEKTRUM_PRIMARY_PORT=SPEKTRUM_$(RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT)
+test_rc_spektrum.CFLAGS += -DRADIO_CONTROL_SPEKTRUM_SECONDARY_PORT=SPEKTRUM_$(RADIO_CONTROL_SPEKTRUM_SECONDARY_PORT)
+test_rc_spektrum.CFLAGS += -DOVERRIDE_$(RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT)_IRQ_HANDLER
test_rc_spektrum.CFLAGS += -DOVERRIDE_$(RADIO_CONTROL_SPEKTRUM_SECONDARY_PORT)_IRQ_HANDLER
test_rc_spektrum.srcs += $(SRC_SUBSYSTEMS)/radio_control.c
test_rc_spektrum.srcs += $(SRC_SUBSYSTEMS)/radio_control/spektrum.c
@@ -655,7 +655,7 @@ test_can.CFLAGS = $(COMMON_TEST_CFLAGS)
test_can.srcs = $(COMMON_TEST_SRCS)
test_can.CFLAGS += $(COMMON_TELEMETRY_CFLAGS)
test_can.srcs += $(COMMON_TELEMETRY_SRCS)
-test_can.CFLAGS += -I$(SRC_LISA) -I$(SRC_BOARD)
+test_can.CFLAGS += -I$(SRC_LISA)
test_can.CFLAGS += -I$(SRC_LISA)
test_can.srcs += lisa/test_can.c
diff --git a/conf/firmwares/logger.makefile b/conf/firmwares/logger.makefile
index b9d9df514b..13d73eec69 100644
--- a/conf/firmwares/logger.makefile
+++ b/conf/firmwares/logger.makefile
@@ -44,8 +44,8 @@ LOG_MSG_FMT = LOG_PPRZ
endif
#set the speed
-ap.CFLAGS += -DUSE_UART0 -DUART0_BAUD=$(UART0_BAUD) -DUSE_UART0_RX_ONLY -DPERIPHERALS_AUTO_INIT
-ap.CFLAGS += -DUSE_UART1 -DUART1_BAUD=$(UART1_BAUD) -DUSE_UART1_RX_ONLY
+ap.CFLAGS += -DUSE_UART0 -DUART0_BAUD=$(UART0_BAUD) -DUSE_UART0_TX=FALSE -DPERIPHERALS_AUTO_INIT
+ap.CFLAGS += -DUSE_UART1 -DUART1_BAUD=$(UART1_BAUD) -DUSE_UART1_TX=FALSE
ap.CFLAGS += -DLOG_STOP_KEY=$(LOG_STOP_KEY)
ap.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
ap.srcs += mcu_periph/uart.c
diff --git a/conf/firmwares/rotorcraft.makefile b/conf/firmwares/rotorcraft.makefile
index ee428e90fa..e997c8d169 100644
--- a/conf/firmwares/rotorcraft.makefile
+++ b/conf/firmwares/rotorcraft.makefile
@@ -193,7 +193,16 @@ ap.CFLAGS += -DUSE_SPI_SLAVE0
ap.CFLAGS += -DUSE_SPI1
ap.srcs += peripherals/mcp355x.c
ap.srcs += $(SRC_BOARD)/baro_board.c
+
+# apogee baro
+else ifeq ($(BOARD), apogee)
+ap.CFLAGS += -DUSE_I2C1
+ap.CFLAGS += -DMPL3115_I2C_DEV=i2c1
+ap.CFLAGS += -DMPL3115_ALT_MODE=0
+ap.srcs += peripherals/mpl3115.c
+ap.srcs += $(SRC_BOARD)/baro_board.c
endif
+
ifneq ($(BARO_LED),none)
ap.CFLAGS += -DROTORCRAFT_BARO_LED=$(BARO_LED)
endif
@@ -213,7 +222,6 @@ ap.srcs += $(SRC_ARCH)/mcu_periph/dac_arch.c
endif
else ifeq ($(ARCH), stm32)
ap.CFLAGS += -DUSE_ADC
-ap.CFLAGS += -DUSE_AD1 -DUSE_AD1_1 -DUSE_AD1_2 -DUSE_AD1_3 -DUSE_AD1_4
ap.srcs += $(SRC_ARCH)/mcu_periph/adc_arch.c
ap.srcs += subsystems/electrical.c
else ifeq ($(BOARD)$(BOARD_TYPE), ardronesdk)
diff --git a/conf/firmwares/subsystems/fixedwing/autopilot.makefile b/conf/firmwares/subsystems/fixedwing/autopilot.makefile
index f15c206c9f..531a2e9ba9 100644
--- a/conf/firmwares/subsystems/fixedwing/autopilot.makefile
+++ b/conf/firmwares/subsystems/fixedwing/autopilot.makefile
@@ -131,11 +131,7 @@ ns_srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
# ANALOG
#
ns_CFLAGS += -DUSE_ADC
-#ifeq ($(ARCH), lpc21)
- ns_srcs += $(SRC_ARCH)/mcu_periph/adc_arch.c
-ifeq ($(ARCH), stm32)
- ns_CFLAGS += -DUSE_AD1 -DUSE_AD1_1 -DUSE_AD1_2 -DUSE_AD1_3 -DUSE_AD1_4
-endif
+ns_srcs += $(SRC_ARCH)/mcu_periph/adc_arch.c
######################################################################
##
diff --git a/conf/firmwares/subsystems/shared/imu_apogee.makefile b/conf/firmwares/subsystems/shared/imu_apogee.makefile
new file mode 100644
index 0000000000..c3e9e4da9b
--- /dev/null
+++ b/conf/firmwares/subsystems/shared/imu_apogee.makefile
@@ -0,0 +1,28 @@
+# Hey Emacs, this is a -*- makefile -*-
+#
+# Apogee IMU
+#
+
+IMU_APOGEE_CFLAGS = -DUSE_IMU
+IMU_APOGEE_CFLAGS += -DIMU_TYPE_H=\"boards/apogee/imu_apogee.h\"
+
+IMU_APOGEE_SRCS = $(SRC_SUBSYSTEMS)/imu.c \
+ $(SRC_BOARD)/imu_apogee.c
+
+IMU_APOGEE_I2C_DEV=i2c1
+IMU_APOGEE_CFLAGS += -DUSE_I2C -DUSE_I2C1
+
+IMU_APOGEE_CFLAGS += -DIMU_APOGEE_I2C_DEV=$(IMU_APOGEE_I2C_DEV)
+IMU_APOGEE_SRCS += peripherals/mpu60x0.c
+IMU_APOGEE_SRCS += peripherals/mpu60x0_i2c.c
+
+# with default APOGEE_SMPLRT_DIV (gyro output 100Hz)
+# the AHRS_PROPAGATE_FREQUENCY needs to be adjusted accordingly
+AHRS_PROPAGATE_FREQUENCY ?= 100
+AHRS_CORRECT_FREQUENCY ?= 100
+ap.CFLAGS += -DAHRS_PROPAGATE_FREQUENCY=$(AHRS_PROPAGATE_FREQUENCY)
+ap.CFLAGS += -DAHRS_CORRECT_FREQUENCY=$(AHRS_CORRECT_FREQUENCY)
+
+ap.CFLAGS += $(IMU_APOGEE_CFLAGS)
+ap.srcs += $(IMU_APOGEE_SRCS)
+
diff --git a/conf/firmwares/subsystems/shared/imu_aspirin_v2.1_new.makefile b/conf/firmwares/subsystems/shared/imu_aspirin_v2.1_new.makefile
new file mode 100644
index 0000000000..3ad6e9a953
--- /dev/null
+++ b/conf/firmwares/subsystems/shared/imu_aspirin_v2.1_new.makefile
@@ -0,0 +1,80 @@
+# Hey Emacs, this is a -*- makefile -*-
+#
+# Aspirin IMU v2.1
+#
+#
+# required xml:
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+
+
+# for fixedwing firmware and ap only
+ifeq ($(TARGET), ap)
+ IMU_ASPIRIN_2_CFLAGS = -DUSE_IMU
+endif
+
+IMU_ASPIRIN_2_CFLAGS += -DIMU_TYPE_H=\"imu/imu_aspirin_2.h\"
+IMU_ASPIRIN_2_SRCS = $(SRC_SUBSYSTEMS)/imu.c
+IMU_ASPIRIN_2_SRCS += $(SRC_SUBSYSTEMS)/imu/imu_aspirin_2.c
+IMU_ASPIRIN_2_SRCS += peripherals/mpu60x0.c
+IMU_ASPIRIN_2_SRCS += peripherals/mpu60x0_spi.c
+
+# Magnetometer
+#IMU_ASPIRIN_2_SRCS += peripherals/hmc58xx.c
+
+include $(CFG_SHARED)/spi_master.makefile
+
+ifeq ($(ARCH), lpc21)
+IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI_SLAVE0
+IMU_ASPIRIN_2_CFLAGS += -DASPIRIN_2_SPI_SLAVE_IDX=SPI_SLAVE0
+IMU_ASPIRIN_2_CFLAGS += -DASPIRIN_2_SPI_DEV=spi1
+IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI1
+else ifeq ($(ARCH), stm32)
+IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI2
+# Slave select configuration
+# SLAVE2 is on PB12 (NSS) (MPU600 CS)
+IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI_SLAVE2
+endif
+
+#IMU_ASPIRIN_2_CFLAGS += -DIMU_ASPIRIN_VERSION_2_1
+
+# Keep CFLAGS/Srcs for imu in separate expression so we can assign it to other targets
+# see: conf/autopilot/subsystems/lisa_passthrough/imu_b2_v1.1.makefile for example
+
+ap.CFLAGS += $(IMU_ASPIRIN_2_CFLAGS)
+ap.srcs += $(IMU_ASPIRIN_2_SRCS)
+
+
+#
+# NPS simulator
+#
+include $(CFG_SHARED)/imu_nps.makefile
diff --git a/conf/firmwares/subsystems/shared/radio_control_ppm.makefile b/conf/firmwares/subsystems/shared/radio_control_ppm.makefile
index e2b14e0154..f8e49d96d5 100644
--- a/conf/firmwares/subsystems/shared/radio_control_ppm.makefile
+++ b/conf/firmwares/subsystems/shared/radio_control_ppm.makefile
@@ -23,19 +23,13 @@ ifeq ($(NORADIO), False)
$(TARGET).srcs += $(SRC_ARCH)/subsystems/radio_control/ppm_arch.c
ifeq ($(ARCH),stm32)
-# default to PA.01 (Servo 6 on Lisa/M) if not already defined
- RADIO_CONTROL_PPM_PIN ?= PA_01
- ifeq ($(RADIO_CONTROL_PPM_PIN),$(filter $(RADIO_CONTROL_PPM_PIN),PA_10 UART1_RX))
- ap.CFLAGS += -DUSE_PPM_TIM1
- fbw.CFLAGS += -DUSE_PPM_TIM1
- else ifeq ($(RADIO_CONTROL_PPM_PIN),$(filter $(RADIO_CONTROL_PPM_PIN),PA_01 SERVO6))
-# TIM2 is used by ADC by default, tell it to use TIM1 instead
-# (also see sw/airborne/arch/stm32/TIM_usage_list.txt)
- ap.CFLAGS += -DUSE_PPM_TIM2 -DUSE_AD_TIM1
- fbw.CFLAGS += -DUSE_PPM_TIM2 -DUSE_AD_TIM1
- else
- $(error unknown configuration for RADIO_CONTROL_PPM_PIN)
+ ifdef RADIO_CONTROL_PPM_PIN
+ ifeq ($(RADIO_CONTROL_PPM_PIN),$(filter $(RADIO_CONTROL_PPM_PIN),PA_10 UART1_RX))
+ $(TARGET).CFLAGS += -DPPM_CONFIG=1
+ else ifeq ($(RADIO_CONTROL_PPM_PIN),$(filter $(RADIO_CONTROL_PPM_PIN),PA_01 SERVO6))
+ $(TARGET).CFLAGS += -DPPM_CONFIG=2
+ endif
endif
-endif
+ endif
endif
diff --git a/conf/firmwares/subsystems/shared/radio_control_spektrum.makefile b/conf/firmwares/subsystems/shared/radio_control_spektrum.makefile
index d89d66c964..05d9d555c4 100644
--- a/conf/firmwares/subsystems/shared/radio_control_spektrum.makefile
+++ b/conf/firmwares/subsystems/shared/radio_control_spektrum.makefile
@@ -13,13 +13,13 @@ endif
ifneq ($(RADIO_CONTROL_LED),none)
ap.CFLAGS += -DRADIO_CONTROL_LED=$(RADIO_CONTROL_LED)
endif
-ap.CFLAGS += -DRADIO_CONTROL_SPEKTRUM_PRIMARY_PORT=$(RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT)
-ap.CFLAGS += -DOVERRIDE_$(RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT)_IRQ_HANDLER -DUSE_TIM6_IRQ
+ap.CFLAGS += -DRADIO_CONTROL_SPEKTRUM_PRIMARY_PORT=SPEKTRUM_$(RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT)
+ap.CFLAGS += -DOVERRIDE_$(RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT)_IRQ_HANDLER
# enable the second spektrum port if so configured
ifdef USE_SECONDARY_SPEKTRUM_RECEIVER
ifneq ($(USE_SECONDARY_SPEKTRUM_RECEIVER),0)
-ap.CFLAGS += -DRADIO_CONTROL_SPEKTRUM_SECONDARY_PORT=$(RADIO_CONTROL_SPEKTRUM_SECONDARY_PORT)
+ap.CFLAGS += -DRADIO_CONTROL_SPEKTRUM_SECONDARY_PORT=SPEKTRUM_$(RADIO_CONTROL_SPEKTRUM_SECONDARY_PORT)
ap.CFLAGS += -DOVERRIDE_$(RADIO_CONTROL_SPEKTRUM_SECONDARY_PORT)_IRQ_HANDLER
endif
endif
diff --git a/conf/flight_plans/rotorcraft_krooz.xml b/conf/flight_plans/rotorcraft_krooz.xml
new file mode 100755
index 0000000000..0ce5f2a18e
--- /dev/null
+++ b/conf/flight_plans/rotorcraft_krooz.xml
@@ -0,0 +1,100 @@
+
+
+
+
+#include "autopilot.h"
+#include "../../subsystems/electrical.h"
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/radios/mx-16.xml b/conf/radios/mx-16.xml
new file mode 100755
index 0000000000..a26ef6342c
--- /dev/null
+++ b/conf/radios/mx-16.xml
@@ -0,0 +1,57 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sw/airborne/arch/lpc21/mcu_periph/uart_arch.c b/sw/airborne/arch/lpc21/mcu_periph/uart_arch.c
index a7d2e22da2..125dac1c6f 100644
--- a/sw/airborne/arch/lpc21/mcu_periph/uart_arch.c
+++ b/sw/airborne/arch/lpc21/mcu_periph/uart_arch.c
@@ -62,7 +62,7 @@ static inline void uart_set_baudrate(struct uart_periph* p, uint32_t baud) {
((uartRegs_t *)(p->reg_addr))->fcr = UART_FIFO_8;
}
-void uart_periph_set_baudrate(struct uart_periph* p, uint32_t baud, bool_t hw_flow_control __attribute__ ((unused))) {
+void uart_periph_set_baudrate(struct uart_periph* p, uint32_t baud) {
uart_disable_interrupts(p);
uart_set_baudrate(p, baud);
uart_enable_interrupts(p);
@@ -163,6 +163,19 @@ static inline void uart_ISR(struct uart_periph* p)
#define UART0_VIC_SLOT 5
#endif
+/* by default enable UART Tx and Rx */
+#ifndef USE_UART0_TX
+ #ifdef USE_UART0_RX_ONLY
+ #warning "USE_UART0_RX_ONLY is deprecated, please set USE_UART0_TX=FALSE instead"
+ #define USE_UART0_TX FALSE
+ #else
+ #define USE_UART0_TX TRUE
+ #endif
+#endif
+#ifndef USE_UART0_RX
+#define USE_UART0_RX TRUE
+#endif
+
void uart0_ISR(void) __attribute__((naked));
void uart0_ISR(void) {
@@ -180,16 +193,18 @@ void uart0_init( void ) {
uart_periph_init(&uart0);
uart0.reg_addr = UART0_BASE;
-#ifdef USE_UART0_RX_ONLY
- // only use the RX0 P0.1 pin, no TX
- PINSEL0 = (PINSEL0 & ~U0_PINMASK_RX) | U0_PINSEL_RX;
-#else
+#if USE_UART0_RX && USE_UART0_TX
// set port pins for UART0
PINSEL0 = (PINSEL0 & ~U0_PINMASK) | U0_PINSEL;
+#elif USE_UART0_RX
+ // only use the RX0 P0.1 pin, no TX
+ PINSEL0 = (PINSEL0 & ~U0_PINMASK_RX) | U0_PINSEL_RX;
+#elif USE_UART0_TX
+ // only use tx
+ PINSEL0 = (PINSEL0 & ~U0_PINMASK_TX) | U0_PINSEL_TX;
#endif
// initialize uart parameters
- uart_disable_interrupts(&uart0);
uart_set_baudrate(&uart0, UART0_BAUD);
// initialize the interrupt vector
@@ -209,6 +224,19 @@ void uart0_init( void ) {
#define UART1_VIC_SLOT 6
#endif
+/* by default enable UART Tx and Rx */
+#ifndef USE_UART1_TX
+ #ifdef USE_UART1_RX_ONLY
+ #warning "USE_UART1_RX_ONLY is deprecated, please set USE_UART1_TX=FALSE instead"
+ #define USE_UART1_TX FALSE
+ #else
+ #define USE_UART1_TX TRUE
+ #endif
+#endif
+#ifndef USE_UART1_RX
+#define USE_UART1_RX TRUE
+#endif
+
void uart1_ISR(void) __attribute__((naked));
void uart1_ISR(void) {
@@ -226,15 +254,17 @@ void uart1_init( void ) {
uart_periph_init(&uart1);
uart1.reg_addr = UART1_BASE;
-#ifdef USE_UART1_RX_ONLY
- // only use the RX1 P0.9 pin, no TX
- PINSEL0 = (PINSEL0 & ~U1_PINMASK_RX) | U1_PINSEL_RX;
-#else
+#if USE_UART1_RX && USE_UART0_TX
// set port pins for UART1
PINSEL0 = (PINSEL0 & ~U1_PINMASK) | U1_PINSEL;
+#elif USE_UART1_RX
+ // only use the RX0 P0.1 pin, no TX
+ PINSEL0 = (PINSEL0 & ~U1_PINMASK_RX) | U1_PINSEL_RX;
+#elif USE_UART1_TX
+ // only use tx
+ PINSEL0 = (PINSEL0 & ~U1_PINMASK_TX) | U1_PINSEL_TX;
#endif
- uart_disable_interrupts(&uart1);
uart_set_baudrate(&uart1, UART1_BAUD);
// initialize the interrupt vector
diff --git a/sw/airborne/arch/omap/mcu_periph/uart_arch.c b/sw/airborne/arch/omap/mcu_periph/uart_arch.c
index a4da7cc29f..1b7af017bc 100644
--- a/sw/airborne/arch/omap/mcu_periph/uart_arch.c
+++ b/sw/airborne/arch/omap/mcu_periph/uart_arch.c
@@ -34,7 +34,7 @@
#include "fms/fms_serial_port.h"
-void uart_periph_set_baudrate(struct uart_periph* p, uint32_t baud, bool_t hw_flow_control __attribute__ ((unused))) {
+void uart_periph_set_baudrate(struct uart_periph* p, uint32_t baud) {
struct FmsSerialPort* fmssp;
// close serial port if already open
if (p->reg_addr != NULL) {
@@ -106,7 +106,7 @@ static inline void uart_handler(struct uart_periph* p) {
void uart0_init( void ) {
uart_periph_init(&uart0);
strcpy(uart0.dev, UART0_DEV);
- uart_periph_set_baudrate(&uart0,UART0_BAUD,FALSE);
+ uart_periph_set_baudrate(&uart0, UART0_BAUD);
}
@@ -121,7 +121,7 @@ void uart0_handler(void) {
void uart1_init( void ) {
uart_periph_init(&uart1);
strcpy(uart1.dev, UART1_DEV);
- uart_periph_set_baudrate(&uart1,UART1_BAUD,FALSE);
+ uart_periph_set_baudrate(&uart1, UART1_BAUD);
}
void uart1_handler(void) {
diff --git a/sw/airborne/arch/stm32/TIM_usage_list.txt b/sw/airborne/arch/stm32/TIM_usage_list.txt
index b9e13ed62f..aa31faa86e 100644
--- a/sw/airborne/arch/stm32/TIM_usage_list.txt
+++ b/sw/airborne/arch/stm32/TIM_usage_list.txt
@@ -14,10 +14,10 @@ TIM2 adc (default)
TIM3 actuators/pwm (PWM1..4)
TIM4 adc (if USE_AD_TIM4)
- actuators/pwm (PWM6..7) (if ! REMAP_SERVOS_5AND6)
+ actuators/pwm (PWM6..7 on LisaL)
(PWM7..8) (if USE_SERVOS_7AND8)
-TIM5 actuators/pwm (PWM6..7) (if REMAP_SERVOS_5AND6)
+TIM5 actuators/pwm (PWM/Servos 5..6 on LisaM)
TIM6 radio_control/spektrum
diff --git a/sw/airborne/arch/stm32/apogee.ld b/sw/airborne/arch/stm32/apogee.ld
new file mode 100644
index 0000000000..8f184a6221
--- /dev/null
+++ b/sw/airborne/arch/stm32/apogee.ld
@@ -0,0 +1,34 @@
+/*
+ * Copyright (C) 2013 Gautier Hattenberger
+ *
+ * This file is part of Paparazzi.
+ *
+ * Paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * Paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/* Linker script for Apogee */
+
+/* Define memory regions. */
+MEMORY
+{
+ ram (rwx) : ORIGIN = 0x20000000, LENGTH = 128K
+ /* Leaving 2k of space at the end of rom for stored settings */
+ rom (rx) : ORIGIN = 0x08000000, LENGTH = 1022K
+}
+
+/* Include the common ld script. */
+INCLUDE libopencm3_stm32f4.ld
+
diff --git a/sw/airborne/arch/stm32/krooz.ld b/sw/airborne/arch/stm32/krooz.ld
new file mode 100644
index 0000000000..04d5a71a02
--- /dev/null
+++ b/sw/airborne/arch/stm32/krooz.ld
@@ -0,0 +1,35 @@
+/*
+ * Hey Emacs, this is a -*- makefile -*-
+ *
+ * Copyright (C) 2012 Sergey Krukowski
+ *
+ * This file is part of Paparazzi.
+ *
+ * Paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * Paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/* Linker script for Krooz (STM32F405, 1024K flash, 192K RAM). */
+
+/* Define memory regions. */
+MEMORY
+{
+ ram (rwx) : ORIGIN = 0x20000000, LENGTH = 128K
+ /* Leaving 2k of space at the end of rom for stored settings */
+ rom (rx) : ORIGIN = 0x08004000, LENGTH = 1022K
+}
+
+/* Include the common ld script. */
+INCLUDE libopencm3_stm32f4.ld
diff --git a/sw/airborne/arch/stm32/led_hw.h b/sw/airborne/arch/stm32/led_hw.h
index e47c324976..8f705997dd 100644
--- a/sw/airborne/arch/stm32/led_hw.h
+++ b/sw/airborne/arch/stm32/led_hw.h
@@ -22,14 +22,13 @@
#ifndef LED_HW_H
#define LED_HW_H
-#include
-#include
+#include
+#include
#include BOARD_CONFIG
#include "std.h"
-
/*
*
* Regular GPIO driven LEDs
@@ -52,19 +51,31 @@
#define LED_AFIO_REMAP(i) _LED_AFIO_REMAP(LED_ ## i ## _AFIO_REMAP)
/* set pin as output */
+#if defined(STM32F1) || defined(STM32F2)
#define LED_INIT(i) { \
rcc_peripheral_enable_clock(&RCC_APB2ENR, \
- LED_GPIO_CLK(i)); \
+ LED_GPIO_CLK(i)); \
gpio_set_mode(LED_GPIO(i), \
GPIO_MODE_OUTPUT_50_MHZ, \
GPIO_CNF_OUTPUT_PUSHPULL, \
LED_GPIO_PIN(i)); \
LED_AFIO_REMAP(i); \
}
+#elif defined(STM32F4)
+#define LED_INIT(i) { \
+ rcc_peripheral_enable_clock(&RCC_AHB1ENR, \
+ LED_GPIO_CLK(i)); \
+ gpio_mode_setup(LED_GPIO(i), \
+ GPIO_MODE_OUTPUT, \
+ GPIO_PUPD_NONE, \
+ LED_GPIO_PIN(i)); \
+ LED_AFIO_REMAP(i); \
+ }
+#endif
-#define LED_ON(i) { LED_GPIO_ON(i)(LED_GPIO(i)) = LED_GPIO_PIN(i);}
-#define LED_OFF(i) { LED_GPIO_OFF(i)(LED_GPIO(i)) = LED_GPIO_PIN(i);}
-#define LED_TOGGLE(i) { GPIO_ODR(LED_GPIO(i)) ^= LED_GPIO_PIN(i);}
+#define LED_ON(i) LED_GPIO_ON(i)(LED_GPIO(i), LED_GPIO_PIN(i))
+#define LED_OFF(i) LED_GPIO_OFF(i)(LED_GPIO(i), LED_GPIO_PIN(i))
+#define LED_TOGGLE(i) gpio_toggle(LED_GPIO(i), LED_GPIO_PIN(i))
#define LED_PERIODIC() {}
@@ -104,11 +115,11 @@ extern uint8_t led_status[NB_LED];
#define LED_PERIODIC() { \
for (uint8_t _cnt = 0; _cnt < NB_LED; _cnt++) { \
if (led_status[_cnt]) \
- GPIO_BSRR(GPIOC) = GPIO15; \
+ gpio_set(GPIOC, GPIO15); \
else \
- GPIO_BRR(GPIOC) = GPIO15; \
- GPIO_BSRR(GPIOA) = GPIO8; /* clock rising edge */ \
- GPIO_BRR(GPIOA) = GPIO8; /* clock falling edge */ \
+ gpio_clear(GPIOC, GPIO15); \
+ gpio_set(GPIOA, GPIO8); /* clock rising edge */ \
+ gpio_clear(GPIOA, GPIO8); /* clock falling edge */ \
} \
}
diff --git a/sw/airborne/arch/stm32/mcu_arch.c b/sw/airborne/arch/stm32/mcu_arch.c
index 73cd4d1aab..d5d8be1b28 100644
--- a/sw/airborne/arch/stm32/mcu_arch.c
+++ b/sw/airborne/arch/stm32/mcu_arch.c
@@ -31,9 +31,13 @@
#include BOARD_CONFIG
#include
-#include
-#include
+#include
+#include
+#if defined(STM32F1)
#include
+#elif defined(STM32F4)
+#include
+#endif
#include
#include "std.h"
@@ -44,11 +48,26 @@ PRINT_CONFIG_MSG("We are running luftboot, the interrupt vector is being relocat
SCB_VTOR = 0x00002000;
#endif
#if EXT_CLK == 8000000
+#if defined(STM32F1)
PRINT_CONFIG_MSG("Using 8MHz external clock to PLL it to 72MHz.")
rcc_clock_setup_in_hse_8mhz_out_72mhz();
+#elif defined(STM32F4)
+PRINT_CONFIG_MSG("Using 8MHz external clock to PLL it to 168MHz.")
+ rcc_clock_setup_hse_3v3(&hse_8mhz_3v3[CLOCK_3V3_168MHZ]);
+#endif
#elif EXT_CLK == 12000000
+#if defined(STM32F1)
PRINT_CONFIG_MSG("Using 12MHz external clock to PLL it to 72MHz.")
rcc_clock_setup_in_hse_12mhz_out_72mhz();
+#elif defined(STM32F4)
+PRINT_CONFIG_MSG("Using 12MHz external clock to PLL it to 168MHz.")
+ rcc_clock_setup_hse_3v3(&hse_12mhz_3v3[CLOCK_3V3_168MHZ]);
+#endif
+#elif EXT_CLK == 16000000
+#if defined(STM32F4)
+PRINT_CONFIG_MSG("Using 16MHz external clock to PLL it to 168MHz.")
+ rcc_clock_setup_hse_3v3(&hse_16mhz_3v3[CLOCK_3V3_168MHZ]);
+#endif
#else
#error EXT_CLK is either set to an unsupported frequency or not defined at all. Please check!
#endif
diff --git a/sw/airborne/arch/stm32/mcu_periph/adc_arch.c b/sw/airborne/arch/stm32/mcu_periph/adc_arch.c
index 117207edb9..b1cb5cd9d6 100644
--- a/sw/airborne/arch/stm32/mcu_periph/adc_arch.c
+++ b/sw/airborne/arch/stm32/mcu_periph/adc_arch.c
@@ -84,11 +84,18 @@
*/
#include "mcu_periph/adc.h"
-#include
+
+#include
+#include
+#if defined(STM32F1)
#include
-#include
+#define ADC_SAMPLE_TIME ADC_SMPR_SMP_41DOT5CYC
+#elif defined(STM32F4)
+#include
+#define ADC_SAMPLE_TIME ADC_SMPR_SMP_56CYC
+#endif
+#include
#include
-#include
#include
#include "std.h"
#include "led.h"
@@ -176,10 +183,6 @@ static struct adc_buf * adc1_buffers[NB_ADC1_CHANNELS];
static struct adc_buf * adc2_buffers[NB_ADC2_CHANNELS];
#endif
-/*
- Static mapping from channel index to channel injection
- index:
-*/
/**
* Maps integer value x to ADC_InjectedChannel_x.
@@ -247,8 +250,13 @@ static inline void adc_init_rcc( void )
/* Timer peripheral clock enable. */
rcc_peripheral_enable_clock(rcc_apbenr, rcc_apb);
/* GPIO peripheral clock enable. */
+#if defined(STM32F1)
rcc_peripheral_enable_clock(&RCC_APB2ENR, RCC_APB2ENR_IOPBEN |
RCC_APB2ENR_IOPCEN);
+#elif defined(STM32F4)
+ rcc_peripheral_enable_clock(&RCC_AHB1ENR, ADC_GPIO_CLOCK_PORT);
+ adc_set_clk_prescale(ADC_CCR_ADCPRE_BY2);
+#endif
/* Enable ADC peripheral clocks. */
#ifdef USE_AD1
@@ -262,9 +270,14 @@ static inline void adc_init_rcc( void )
timer_reset(timer);
timer_set_mode(timer, TIM_CR1_CKD_CK_INT,
TIM_CR1_CMS_EDGE, TIM_CR1_DIR_UP);
+#if defined(STM32F1)
timer_set_period(timer, 0xFF);
timer_set_prescaler(timer, 0x8);
- timer_set_clock_division(timer, 0x0);
+#elif defined(STM32F4)
+ timer_set_period(timer, 0xFFFF);
+ timer_set_prescaler(timer, 0x53);
+#endif
+ //timer_set_clock_division(timer, 0x0);
/* Generate TRGO on every update. */
timer_set_master_mode(timer, TIM_CR2_MMS_UPDATE);
timer_enable_counter(timer);
@@ -275,8 +288,13 @@ static inline void adc_init_rcc( void )
/* Configure and enable ADC interrupt */
static inline void adc_init_irq( void )
{
+#if defined(STM32F1)
nvic_set_priority(NVIC_ADC1_2_IRQ, 0);
nvic_enable_irq(NVIC_ADC1_2_IRQ);
+#elif defined(STM32F4)
+ nvic_set_priority(NVIC_ADC_IRQ, 0);
+ nvic_enable_irq(NVIC_ADC_IRQ);
+#endif
}
/**
@@ -340,7 +358,11 @@ static inline void adc_init_single(uint32_t adc,
/* Set CR2 register. */
/* Clear TSVREFE */
+#if defined(STM32F1)
adc_disable_temperature_sensor(adc);
+#elif defined(STM32F4)
+ adc_disable_temperature_sensor();
+#endif
/* Clear EXTTRIG */
adc_disable_external_trigger_regular(adc);
/* Clear ALIGN */
@@ -350,24 +372,24 @@ static inline void adc_init_single(uint32_t adc,
/* Clear CONT */
adc_set_single_conversion_mode(adc);
- rank = 0;
+ rank = 3;
if (chan1) {
- adc_set_sample_time(adc, adc_channel_map[0], ADC_SMPR1_SMP_41DOT5CYC);
+ adc_set_sample_time(adc, adc_channel_map[0], ADC_SAMPLE_TIME);
channels[rank] = adc_channel_map[0];
- rank++;
+ rank--;
}
if (chan2) {
- adc_set_sample_time(adc, adc_channel_map[1], ADC_SMPR1_SMP_41DOT5CYC);
+ adc_set_sample_time(adc, adc_channel_map[1], ADC_SAMPLE_TIME);
channels[rank] = adc_channel_map[1];
- rank++;
+ rank--;
}
if (chan3) {
- adc_set_sample_time(adc, adc_channel_map[2], ADC_SMPR1_SMP_41DOT5CYC);
+ adc_set_sample_time(adc, adc_channel_map[2], ADC_SAMPLE_TIME);
channels[rank] = adc_channel_map[2];
- rank++;
+ rank--;
}
if (chan4) {
- adc_set_sample_time(adc, adc_channel_map[3], ADC_SMPR1_SMP_41DOT5CYC);
+ adc_set_sample_time(adc, adc_channel_map[3], ADC_SAMPLE_TIME);
channels[rank] = adc_channel_map[3];
}
@@ -375,18 +397,30 @@ static inline void adc_init_single(uint32_t adc,
#if USE_AD_TIM4
PRINT_CONFIG_MSG("Info: Using TIM4 for ADC")
+#if defined(STM32F1)
adc_enable_external_trigger_injected(adc, ADC_CR2_JEXTSEL_TIM4_TRGO);
+#elif defined(STM32F4)
+ adc_enable_external_trigger_injected(adc, ADC_CR2_JEXTSEL_TIM4_TRGO, ADC_CR2_JEXTEN_BOTH_EDGES);
+#endif
#elif USE_AD_TIM1
PRINT_CONFIG_MSG("Info: Using TIM1 for ADC")
+#if defined(STM32F1)
adc_enable_external_trigger_injected(adc, ADC_CR2_JEXTSEL_TIM1_TRGO);
+#elif defined(STM32F4)
+ adc_enable_external_trigger_injected(adc, ADC_CR2_JEXTSEL_TIM1_TRGO, ADC_CR2_JEXTEN_BOTH_EDGES);
+#endif
#else
PRINT_CONFIG_MSG("Info: Using default TIM2 for ADC")
+#if defined(STM32F1)
adc_enable_external_trigger_injected(adc, ADC_CR2_JEXTSEL_TIM2_TRGO);
+#elif defined(STM32F4)
+ adc_enable_external_trigger_injected(adc, ADC_CR2_JEXTSEL_TIM2_TRGO, ADC_CR2_JEXTEN_BOTH_EDGES);
+#endif
#endif
/* Enable ADC */
adc_power_on(adc);
-
+#if defined(STM32F1)
/* Enable ADC reset calibaration register */
adc_reset_calibration(adc);
/* Check the end of ADC reset calibration */
@@ -395,7 +429,7 @@ PRINT_CONFIG_MSG("Info: Using default TIM2 for ADC")
adc_calibration(adc);
/* Check the end of ADC calibration */
while ((ADC_CR2(adc) & ADC_CR2_CAL) != 0);
-
+#endif
} // adc_init_single
@@ -411,19 +445,50 @@ void adc_init( void ) {
#ifdef USE_AD1
for(channel = 0; channel < NB_ADC1_CHANNELS; channel++)
adc1_buffers[channel] = NULL;
- adc_injected_channels[0] = &ADC_JDR1(ADC1);
- adc_injected_channels[1] = &ADC_JDR2(ADC1);
- adc_injected_channels[2] = &ADC_JDR3(ADC1);
- adc_injected_channels[3] = &ADC_JDR4(ADC1);
+ volatile uint32_t* tmp_channels_1[] = {
+ &ADC_JDR1(ADC1),
+ &ADC_JDR2(ADC1),
+ &ADC_JDR3(ADC1),
+ &ADC_JDR4(ADC1)
+ };
+#ifdef USE_AD1_1
+ adc_injected_channels[ADC1_C1] = tmp_channels_1[NB_ADC1_CHANNELS-1-ADC1_C1];
#endif
+#ifdef USE_AD1_2
+ adc_injected_channels[ADC1_C2] = tmp_channels_1[NB_ADC1_CHANNELS-1-ADC1_C2];
+#endif
+#ifdef USE_AD1_3
+ adc_injected_channels[ADC1_C3] = tmp_channels_1[NB_ADC1_CHANNELS-1-ADC1_C3];
+#endif
+#ifdef USE_AD1_4
+ adc_injected_channels[ADC1_C4] = tmp_channels_1[NB_ADC1_CHANNELS-1-ADC1_C4];
+#endif
+
+#endif // USE_AD1
+
#ifdef USE_AD2
for(channel = 0; channel < NB_ADC2_CHANNELS; channel++)
adc2_buffers[channel] = NULL;
- adc_injected_channels[0] = &ADC_JDR1(ADC2);
- adc_injected_channels[1] = &ADC_JDR2(ADC2);
- adc_injected_channels[2] = &ADC_JDR3(ADC2);
- adc_injected_channels[3] = &ADC_JDR4(ADC2);
+ volatile uint32_t* tmp_channels_2[] = {
+ &ADC_JDR1(ADC2),
+ &ADC_JDR2(ADC2),
+ &ADC_JDR3(ADC2),
+ &ADC_JDR4(ADC2)
+ };
+#ifdef USE_AD2_1
+ adc_injected_channels[ADC2_C1] = tmp_channels_2[NB_ADC2_CHANNELS-1-ADC2_C1];
#endif
+#ifdef USE_AD2_2
+ adc_injected_channels[ADC2_C2] = tmp_channels_2[NB_ADC2_CHANNELS-1-ADC2_C2];
+#endif
+#ifdef USE_AD2_3
+ adc_injected_channels[ADC2_C3] = tmp_channels_2[NB_ADC2_CHANNELS-1-ADC2_C3];
+#endif
+#ifdef USE_AD2_4
+ adc_injected_channels[ADC2_C4] = tmp_channels_2[NB_ADC2_CHANNELS-1-ADC2_C4];
+#endif
+
+#endif // USE_AD2
adc_new_data_trigger = FALSE;
adc_channel_map[0] = BOARD_ADC_CHANNEL_1;
@@ -502,7 +567,11 @@ static inline void adc_push_sample(struct adc_buf * buf, uint16_t value) {
/**
* ADC1+2 interrupt hander
*/
+#if defined(STM32F1)
void adc1_2_isr(void)
+#elif defined(STM32F4)
+void adc_isr(void)
+#endif
{
uint8_t channel = 0;
uint16_t value = 0;
diff --git a/sw/airborne/arch/stm32/mcu_periph/adc_arch.h b/sw/airborne/arch/stm32/mcu_periph/adc_arch.h
index ecf406a04e..5728fc2fd4 100644
--- a/sw/airborne/arch/stm32/mcu_periph/adc_arch.h
+++ b/sw/airborne/arch/stm32/mcu_periph/adc_arch.h
@@ -30,6 +30,8 @@
#ifndef ADC_ARCH_H
#define ADC_ARCH_H
+#include BOARD_CONFIG
+
// NB_ADCx_CHANNELS
enum adc1_channels {
#ifdef USE_AD1_1
diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.c
index 16c96f46f7..cab158c0b7 100644
--- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.c
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.c
@@ -28,10 +28,12 @@
#include "mcu_periph/i2c.h"
-#include
-#include
+#include BOARD_CONFIG
+
+#include
+#include
+#include
#include
-#include
#ifdef I2C_DEBUG_LED
@@ -909,11 +911,17 @@ void i2c1_hw_init(void) {
/* Enable I2C1 clock */
rcc_peripheral_enable_clock(&RCC_APB1ENR, RCC_APB1ENR_I2C1EN);
/* Enable GPIOB clock */
+#if defined(STM32F1)
rcc_peripheral_enable_clock(&RCC_APB2ENR, RCC_APB2ENR_IOPBEN);
-
gpio_set_mode(GPIOB, GPIO_MODE_OUTPUT_2_MHZ,
GPIO_CNF_OUTPUT_ALTFN_OPENDRAIN,
i2c1.scl_pin | i2c1.sda_pin);
+#elif defined(STM32F4)
+ rcc_peripheral_enable_clock(&RCC_AHB1ENR, RCC_AHB1ENR_IOPBEN);
+ gpio_mode_setup(GPIOB, GPIO_MODE_AF, GPIO_PUPD_NONE, i2c1.scl_pin | i2c1.sda_pin);
+ gpio_set_output_options(GPIOB, GPIO_OTYPE_OD, GPIO_OSPEED_25MHZ, i2c1.scl_pin | i2c1.sda_pin);
+ gpio_set_af(GPIOB, GPIO_AF4, i2c1.scl_pin | i2c1.sda_pin);
+#endif
i2c_reset(I2C1);
@@ -993,11 +1001,17 @@ void i2c2_hw_init(void) {
/* Enable I2C2 clock */
rcc_peripheral_enable_clock(&RCC_APB1ENR, RCC_APB1ENR_I2C2EN);
/* Enable GPIOB clock */
+#if defined(STM32F1)
rcc_peripheral_enable_clock(&RCC_APB2ENR, RCC_APB2ENR_IOPBEN);
-
gpio_set_mode(GPIOB, GPIO_MODE_OUTPUT_2_MHZ,
GPIO_CNF_OUTPUT_ALTFN_OPENDRAIN,
i2c2.scl_pin | i2c2.sda_pin);
+#elif defined(STM32F4)
+ rcc_peripheral_enable_clock(&RCC_AHB1ENR, RCC_AHB1ENR_IOPBEN);
+ gpio_mode_setup(GPIOB, GPIO_MODE_AF, GPIO_PUPD_NONE, i2c2.scl_pin | i2c2.sda_pin);
+ gpio_set_output_options(GPIOB, GPIO_OTYPE_OD, GPIO_OSPEED_25MHZ, i2c2.scl_pin | i2c2.sda_pin);
+ gpio_set_af(GPIOB, GPIO_AF4, i2c2.scl_pin | i2c2.sda_pin);
+#endif
i2c_reset(I2C2);
diff --git a/sw/airborne/arch/stm32/mcu_periph/spi_arch.c b/sw/airborne/arch/stm32/mcu_periph/spi_arch.c
index 315694aba4..833d7cd8b9 100644
--- a/sw/airborne/arch/stm32/mcu_periph/spi_arch.c
+++ b/sw/airborne/arch/stm32/mcu_periph/spi_arch.c
@@ -51,15 +51,17 @@
* - The after_cb callback happens BEFORE the slave is unselected as configured.
*/
-#include
-#include
-#include
+#include
+#include
+#include
#include
#include
#include
#include "mcu_periph/spi.h"
+#include BOARD_CONFIG
+
#ifdef SPI_MASTER
/**
@@ -122,61 +124,37 @@ static void spi_arch_int_disable(struct spi_periph *spi);
* Handling of Slave Select outputs
*
*****************************************************************************/
-/// @todo move the SS gpio defines to the board files
-#define SPI_SELECT_SLAVE0_PERIPH RCC_APB2ENR_IOPAEN
-#define SPI_SELECT_SLAVE0_PORT GPIOA
-#define SPI_SELECT_SLAVE0_PIN GPIO15
-
-#define SPI_SELECT_SLAVE1_PERIPH RCC_APB2ENR_IOPAEN
-#define SPI_SELECT_SLAVE1_PORT GPIOA
-#define SPI_SELECT_SLAVE1_PIN GPIO4
-
-#define SPI_SELECT_SLAVE2_PERIPH RCC_APB2ENR_IOPBEN
-#define SPI_SELECT_SLAVE2_PORT GPIOB
-#define SPI_SELECT_SLAVE2_PIN GPIO12
-
-#define SPI_SELECT_SLAVE3_PERIPH RCC_APB2ENR_IOPCEN
-#define SPI_SELECT_SLAVE3_PORT GPIOC
-#define SPI_SELECT_SLAVE3_PIN GPIO13
-
-#define SPI_SELECT_SLAVE4_PERIPH RCC_APB2ENR_IOPCEN
-#define SPI_SELECT_SLAVE4_PORT GPIOC
-#define SPI_SELECT_SLAVE4_PIN GPIO12
-
-#define SPI_SELECT_SLAVE5_PERIPH RCC_APB2ENR_IOPCEN
-#define SPI_SELECT_SLAVE5_PORT GPIOC
-#define SPI_SELECT_SLAVE5_PIN GPIO4
static inline void SpiSlaveUnselect(uint8_t slave) {
switch(slave) {
#if USE_SPI_SLAVE0
case 0:
- GPIO_BSRR(SPI_SELECT_SLAVE0_PORT) = SPI_SELECT_SLAVE0_PIN;
+ gpio_set(SPI_SELECT_SLAVE0_PORT, SPI_SELECT_SLAVE0_PIN);
break;
#endif // USE_SPI_SLAVE0
#if USE_SPI_SLAVE1
case 1:
- GPIO_BSRR(SPI_SELECT_SLAVE1_PORT) = SPI_SELECT_SLAVE1_PIN;
+ gpio_set(SPI_SELECT_SLAVE1_PORT, SPI_SELECT_SLAVE1_PIN);
break;
#endif //USE_SPI_SLAVE1
#if USE_SPI_SLAVE2
case 2:
- GPIO_BSRR(SPI_SELECT_SLAVE2_PORT) = SPI_SELECT_SLAVE2_PIN;
+ gpio_set(SPI_SELECT_SLAVE2_PORT, SPI_SELECT_SLAVE2_PIN);
break;
#endif //USE_SPI_SLAVE2
#if USE_SPI_SLAVE3
case 3:
- GPIO_BSRR(SPI_SELECT_SLAVE3_PORT) = SPI_SELECT_SLAVE3_PIN;
+ gpio_set(SPI_SELECT_SLAVE3_PORT, SPI_SELECT_SLAVE3_PIN);
break;
#endif //USE_SPI_SLAVE3
#if USE_SPI_SLAVE4
case 4:
- GPIO_BSRR(SPI_SELECT_SLAVE4_PORT) = SPI_SELECT_SLAVE4_PIN;
+ gpio_set(SPI_SELECT_SLAVE4_PORT, SPI_SELECT_SLAVE4_PIN);
break;
#endif //USE_SPI_SLAVE4
#if USE_SPI_SLAVE5
case 5:
- GPIO_BSRR(SPI_SELECT_SLAVE5_PORT) = SPI_SELECT_SLAVE5_PIN;
+ gpio_set(SPI_SELECT_SLAVE5_PORT, SPI_SELECT_SLAVE5_PIN);
break;
#endif //USE_SPI_SLAVE5
default:
@@ -188,32 +166,32 @@ static inline void SpiSlaveSelect(uint8_t slave) {
switch(slave) {
#if USE_SPI_SLAVE0
case 0:
- GPIO_BRR(SPI_SELECT_SLAVE0_PORT) = SPI_SELECT_SLAVE0_PIN;
+ gpio_clear(SPI_SELECT_SLAVE0_PORT, SPI_SELECT_SLAVE0_PIN);
break;
#endif // USE_SPI_SLAVE0
#if USE_SPI_SLAVE1
case 1:
- GPIO_BRR(SPI_SELECT_SLAVE1_PORT) = SPI_SELECT_SLAVE1_PIN;
+ gpio_clear(SPI_SELECT_SLAVE1_PORT, SPI_SELECT_SLAVE1_PIN);
break;
#endif //USE_SPI_SLAVE1
#if USE_SPI_SLAVE2
case 2:
- GPIO_BRR(SPI_SELECT_SLAVE2_PORT) = SPI_SELECT_SLAVE2_PIN;
+ gpio_clear(SPI_SELECT_SLAVE2_PORT, SPI_SELECT_SLAVE2_PIN);
break;
#endif //USE_SPI_SLAVE2
#if USE_SPI_SLAVE3
case 3:
- GPIO_BRR(SPI_SELECT_SLAVE3_PORT) = SPI_SELECT_SLAVE3_PIN;
+ gpio_clear(SPI_SELECT_SLAVE3_PORT, SPI_SELECT_SLAVE3_PIN);
break;
#endif //USE_SPI_SLAVE3
#if USE_SPI_SLAVE4
case 4:
- GPIO_BRR(SPI_SELECT_SLAVE4_PORT) = SPI_SELECT_SLAVE4_PIN;
+ gpio_clear(SPI_SELECT_SLAVE4_PORT, SPI_SELECT_SLAVE4_PIN);
break;
#endif //USE_SPI_SLAVE4
#if USE_SPI_SLAVE5
case 5:
- GPIO_BRR(SPI_SELECT_SLAVE5_PORT) = SPI_SELECT_SLAVE5_PIN;
+ gpio_clear(SPI_SELECT_SLAVE5_PORT, SPI_SELECT_SLAVE5_PIN);
break;
#endif //USE_SPI_SLAVE5
default:
diff --git a/sw/airborne/arch/stm32/mcu_periph/sys_time_arch.h b/sw/airborne/arch/stm32/mcu_periph/sys_time_arch.h
index 658d9d84a9..e286cbaabf 100644
--- a/sw/airborne/arch/stm32/mcu_periph/sys_time_arch.h
+++ b/sw/airborne/arch/stm32/mcu_periph/sys_time_arch.h
@@ -34,8 +34,8 @@
#include "mcu_periph/sys_time.h"
-#include
-#include
+#include
+#include
#include
#include "std.h"
diff --git a/sw/airborne/arch/stm32/mcu_periph/uart_arch.c b/sw/airborne/arch/stm32/mcu_periph/uart_arch.c
index 983c750e59..fd812c2f7c 100644
--- a/sw/airborne/arch/stm32/mcu_periph/uart_arch.c
+++ b/sw/airborne/arch/stm32/mcu_periph/uart_arch.c
@@ -28,27 +28,22 @@
#include "mcu_periph/uart.h"
-#include
-#include
+#include
+#include
#include
-#include
+#include
+
#include "std.h"
-void uart_periph_set_baudrate(struct uart_periph* p, uint32_t baud, bool_t hw_flow_control) {
+#include BOARD_CONFIG
+
+void uart_periph_set_baudrate(struct uart_periph* p, uint32_t baud) {
/* Configure USART */
usart_set_baudrate((u32)p->reg_addr, baud);
usart_set_databits((u32)p->reg_addr, 8);
usart_set_stopbits((u32)p->reg_addr, USART_STOPBITS_1);
usart_set_parity((u32)p->reg_addr, USART_PARITY_NONE);
- usart_set_mode((u32)p->reg_addr, USART_MODE_TX_RX);
-
- if (hw_flow_control) {
- usart_set_flow_control((u32)p->reg_addr, USART_FLOWCONTROL_RTS_CTS);
- }
- else {
- usart_set_flow_control((u32)p->reg_addr, USART_FLOWCONTROL_NONE);
- }
/* Disable Idle Line interrupt */
USART_CR1((u32)p->reg_addr) &= ~USART_CR1_IDLEIE;
@@ -63,7 +58,24 @@ void uart_periph_set_baudrate(struct uart_periph* p, uint32_t baud, bool_t hw_fl
usart_enable((u32)p->reg_addr);
}
-// XXX: TODO set_mode function
+
+void uart_periph_set_mode(struct uart_periph* p, bool_t tx_enabled, bool_t rx_enabled, bool_t hw_flow_control) {
+ u32 mode = 0;
+ if (tx_enabled)
+ mode |= USART_MODE_TX;
+ if (rx_enabled)
+ mode |= USART_MODE_RX;
+
+ /* set mode to Tx, Rx or TxRx */
+ usart_set_mode((u32)p->reg_addr, mode);
+
+ if (hw_flow_control) {
+ usart_set_flow_control((u32)p->reg_addr, USART_FLOWCONTROL_RTS_CTS);
+ }
+ else {
+ usart_set_flow_control((u32)p->reg_addr, USART_FLOWCONTROL_NONE);
+ }
+}
void uart_transmit(struct uart_periph* p, uint8_t data ) {
@@ -144,128 +156,328 @@ static inline void usart_enable_irq(u8 IRQn) {
nvic_enable_irq(IRQn);
}
+/** Set RCC and GPIO mode on the STM32F4
+ */
+#ifdef STM32F4
+static inline void set_uart_pin(u32 gpioport, u16 gpio, u8 alt_func_num) {
+ switch (gpioport) {
+ case GPIOA:
+ rcc_peripheral_enable_clock(&RCC_AHB1ENR, RCC_AHB1ENR_IOPAEN);
+ break;
+ case GPIOB:
+ rcc_peripheral_enable_clock(&RCC_AHB1ENR, RCC_AHB1ENR_IOPBEN);
+ break;
+ case GPIOC:
+ rcc_peripheral_enable_clock(&RCC_AHB1ENR, RCC_AHB1ENR_IOPCEN);
+ break;
+ case GPIOD:
+ rcc_peripheral_enable_clock(&RCC_AHB1ENR, RCC_AHB1ENR_IOPDEN);
+ break;
+ default:
+ break;
+ };
+ gpio_mode_setup(gpioport, GPIO_MODE_AF, GPIO_PUPD_NONE, gpio);
+ gpio_set_af(gpioport, alt_func_num, gpio);
+}
+#endif /* STM32F4 */
+
#ifdef USE_UART1
+/* by default enable UART Tx and Rx */
+#ifndef USE_UART1_TX
+#define USE_UART1_TX TRUE
+#endif
+#ifndef USE_UART1_RX
+#define USE_UART1_RX TRUE
+#endif
+
+#ifndef UART1_HW_FLOW_CONTROL
+#define UART1_HW_FLOW_CONTROL FALSE
+#endif
+
void uart1_init( void ) {
uart_periph_init(&uart1);
uart1.reg_addr = (void *)USART1;
- /* init RCC */
+ /* init RCC and GPIOs */
+#if defined(STM32F4)
+ rcc_peripheral_enable_clock(&RCC_APB2ENR, RCC_APB2ENR_USART1EN);
+ set_uart_pin(UART1_GPIO_PORT_RX, UART1_GPIO_RX, UART1_GPIO_AF);
+ set_uart_pin(UART1_GPIO_PORT_TX, UART1_GPIO_TX, UART1_GPIO_AF);
+
+#elif defined(STM32F1)
rcc_peripheral_enable_clock(&RCC_APB2ENR, RCC_APB2ENR_USART1EN);
rcc_peripheral_enable_clock(&RCC_APB2ENR, RCC_APB2ENR_IOPAEN);
- /* Enable USART interrupts in the interrupt controller */
- usart_enable_irq(NVIC_USART1_IRQ);
-
- /* Init GPIOS */
gpio_set_mode(GPIO_BANK_USART1_TX, GPIO_MODE_OUTPUT_50_MHZ,
GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, GPIO_USART1_TX);
gpio_set_mode(GPIO_BANK_USART1_RX, GPIO_MODE_INPUT,
GPIO_CNF_INPUT_FLOAT, GPIO_USART1_RX);
+#endif
+
+ /* Enable USART interrupts in the interrupt controller */
+ usart_enable_irq(NVIC_USART1_IRQ);
#if UART1_HW_FLOW_CONTROL
#warning "USING UART1 FLOW CONTROL. Make sure to pull down CTS if you are not connecting any flow-control-capable hardware."
+ /* setup CTS and RTS gpios */
+#if defined(STM32F4)
+ set_uart_pin(UART1_GPIO_PORT_CTS, UART1_GPIO_CTS, UART1_GPIO_AF);
+ set_uart_pin(UART1_GPIO_PORT_RTS, UART1_GPIO_RTS, UART1_GPIO_AF);
+#elif defined(STM32F1)
gpio_set_mode(GPIO_BANK_USART1_RTS, GPIO_MODE_OUTPUT_50_MHZ,
GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, GPIO_USART1_RTS);
gpio_set_mode(GPIO_BANK_USART1_CTS, GPIO_MODE_INPUT,
GPIO_CNF_INPUT_FLOAT, GPIO_USART1_CTS);
+#endif
+#endif
/* Configure USART1, enable hardware flow control*/
- uart_periph_set_baudrate(&uart1, UART1_BAUD, TRUE);
-#else
- /* Configure USART1, no flow control */
- uart_periph_set_baudrate(&uart1, UART1_BAUD, FALSE);
-#endif
+ uart_periph_set_mode(&uart1, USE_UART1_TX, USE_UART1_RX, UART1_HW_FLOW_CONTROL);
+
+ /* Set USART1 baudrate and enable interrupt */
+ uart_periph_set_baudrate(&uart1, UART1_BAUD);
}
void usart1_isr(void) { usart_isr(&uart1); }
#endif /* USE_UART1 */
+
#ifdef USE_UART2
+/* by default enable UART Tx and Rx */
+#ifndef USE_UART2_TX
+#define USE_UART2_TX TRUE
+#endif
+#ifndef USE_UART2_RX
+#define USE_UART2_RX TRUE
+#endif
+
+#ifndef UART2_HW_FLOW_CONTROL
+#define UART2_HW_FLOW_CONTROL FALSE
+#endif
+
void uart2_init( void ) {
uart_periph_init(&uart2);
uart2.reg_addr = (void *)USART2;
- /* init RCC */
+ /* init RCC and GPIOs */
+#if defined(STM32F4)
+ rcc_peripheral_enable_clock(&RCC_APB1ENR, RCC_APB1ENR_USART2EN);
+ set_uart_pin(UART2_GPIO_PORT_RX, UART2_GPIO_RX, UART2_GPIO_AF);
+ set_uart_pin(UART2_GPIO_PORT_TX, UART2_GPIO_TX, UART2_GPIO_AF);
+
+#elif defined(STM32F1)
rcc_peripheral_enable_clock(&RCC_APB1ENR, RCC_APB1ENR_USART2EN);
rcc_peripheral_enable_clock(&RCC_APB2ENR, RCC_APB2ENR_IOPAEN);
-
- /* Enable USART interrupts in the interrupt controller */
- usart_enable_irq(NVIC_USART2_IRQ);
-
- /* Init GPIOS */
gpio_set_mode(GPIO_BANK_USART2_TX, GPIO_MODE_OUTPUT_50_MHZ,
GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, GPIO_USART2_TX);
gpio_set_mode(GPIO_BANK_USART2_RX, GPIO_MODE_INPUT,
GPIO_CNF_INPUT_FLOAT, GPIO_USART2_RX);
+#endif
+
+ /* Enable USART interrupts in the interrupt controller */
+ usart_enable_irq(NVIC_USART2_IRQ);
+
+#if UART2_HW_FLOW_CONTROL && defined(STM32F4)
+#warning "USING UART2 FLOW CONTROL. Make sure to pull down CTS if you are not connecting any flow-control-capable hardware."
+ /* setup CTS and RTS pins */
+ set_uart_pin(UART2_GPIO_PORT_CTS, UART2_GPIO_CTS, UART2_GPIO_AF);
+ set_uart_pin(UART2_GPIO_PORT_RTS, UART2_GPIO_RTS, UART2_GPIO_AF);
+#endif
+
+ /* Configure USART Tx,Rx, and hardware flow control*/
+ uart_periph_set_mode(&uart2, USE_UART2_TX, USE_UART2_RX, UART2_HW_FLOW_CONTROL);
/* Configure USART */
- uart_periph_set_baudrate(&uart2, UART2_BAUD, FALSE);
+ uart_periph_set_baudrate(&uart2, UART2_BAUD);
}
void usart2_isr(void) { usart_isr(&uart2); }
#endif /* USE_UART2 */
+
#ifdef USE_UART3
+/* by default enable UART Tx and Rx */
+#ifndef USE_UART3_TX
+#define USE_UART3_TX TRUE
+#endif
+#ifndef USE_UART3_RX
+#define USE_UART3_RX TRUE
+#endif
+
+#ifndef UART3_HW_FLOW_CONTROL
+#define UART3_HW_FLOW_CONTROL FALSE
+#endif
+
void uart3_init( void ) {
uart_periph_init(&uart3);
uart3.reg_addr = (void *)USART3;
/* init RCC */
+#if defined(STM32F4)
+ rcc_peripheral_enable_clock(&RCC_APB1ENR, RCC_APB1ENR_USART3EN);
+ set_uart_pin(UART3_GPIO_PORT_RX, UART3_GPIO_RX, UART3_GPIO_AF);
+ set_uart_pin(UART3_GPIO_PORT_TX, UART3_GPIO_TX, UART3_GPIO_AF);
+
+#elif defined(STM32F1)
rcc_peripheral_enable_clock(&RCC_APB1ENR, RCC_APB1ENR_USART3EN);
rcc_peripheral_enable_clock(&RCC_APB2ENR, RCC_APB2ENR_IOPCEN);
rcc_peripheral_enable_clock(&RCC_APB2ENR, RCC_APB2ENR_AFIOEN);
- /* Enable USART interrupts in the interrupt controller */
- usart_enable_irq(NVIC_USART3_IRQ);
-
- /* Init GPIOS */
AFIO_MAPR |= AFIO_MAPR_USART3_REMAP_PARTIAL_REMAP;
gpio_set_mode(GPIO_BANK_USART3_PR_TX, GPIO_MODE_OUTPUT_50_MHZ,
GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, GPIO_USART3_PR_TX);
gpio_set_mode(GPIO_BANK_USART3_PR_RX, GPIO_MODE_INPUT,
GPIO_CNF_INPUT_FLOAT, GPIO_USART3_PR_RX);
+#endif
+
+ /* Enable USART interrupts in the interrupt controller */
+ usart_enable_irq(NVIC_USART3_IRQ);
+
+#if UART3_HW_FLOW_CONTROL && defined(STM32F4)
+#warning "USING UART3 FLOW CONTROL. Make sure to pull down CTS if you are not connecting any flow-control-capable hardware."
+ /* setup CTS and RTS pins */
+ set_uart_pin(UART3_GPIO_PORT_CTS, UART3_GPIO_CTS, UART3_GPIO_AF);
+ set_uart_pin(UART3_GPIO_PORT_RTS, UART3_GPIO_RTS, UART3_GPIO_AF);
+#endif
+
+ /* Configure USART Tx,Rx, and hardware flow control*/
+ uart_periph_set_mode(&uart3, USE_UART3_TX, USE_UART3_RX, UART3_HW_FLOW_CONTROL);
/* Configure USART */
- uart_periph_set_baudrate(&uart3, UART3_BAUD, FALSE);
+ uart_periph_set_baudrate(&uart3, UART3_BAUD);
}
void usart3_isr(void) { usart_isr(&uart3); }
#endif /* USE_UART3 */
+
+#if defined USE_UART4 && defined STM32F4
+
+/* by default enable UART Tx and Rx */
+#ifndef USE_UART4_TX
+#define USE_UART4_TX TRUE
+#endif
+#ifndef USE_UART4_RX
+#define USE_UART4_RX TRUE
+#endif
+
+void uart4_init( void ) {
+
+ uart_periph_init(&uart4);
+ uart4.reg_addr = (void *)UART4;
+
+ /* init RCC and GPIOs */
+ rcc_peripheral_enable_clock(&RCC_APB1ENR, RCC_APB1ENR_UART4EN);
+ set_uart_pin(UART4_GPIO_PORT_RX, UART4_GPIO_RX, UART4_GPIO_AF);
+ set_uart_pin(UART4_GPIO_PORT_TX, UART4_GPIO_TX, UART4_GPIO_AF);
+
+ /* Enable USART interrupts in the interrupt controller */
+ usart_enable_irq(NVIC_UART4_IRQ);
+
+ /* Configure USART */
+ uart_periph_set_mode(&uart4, USE_UART4_TX, USE_UART4_RX, FALSE);
+ uart_periph_set_baudrate(&uart4, UART4_BAUD);
+}
+
+void uart4_isr(void) { usart_isr(&uart4); }
+
+#endif /* USE_UART4 */
+
+
#ifdef USE_UART5
+/* by default enable UART Tx and Rx */
+#ifndef USE_UART5_TX
+#define USE_UART5_TX TRUE
+#endif
+#ifndef USE_UART5_RX
+#define USE_UART5_RX TRUE
+#endif
+
void uart5_init( void ) {
uart_periph_init(&uart5);
uart5.reg_addr = (void *)UART5;
- /* init RCC */
+ /* init RCC and GPIOs */
+#if defined(STM32F4)
+ rcc_peripheral_enable_clock(&RCC_APB1ENR, RCC_APB1ENR_UART5EN);
+ set_uart_pin(UART5_GPIO_PORT_RX, UART5_GPIO_RX, UART5_GPIO_AF);
+ set_uart_pin(UART5_GPIO_PORT_TX, UART5_GPIO_TX, UART5_GPIO_AF);
+#elif defined(STM32F1)
rcc_peripheral_enable_clock(&RCC_APB1ENR, RCC_APB1ENR_UART5EN);
rcc_peripheral_enable_clock(&RCC_APB2ENR, RCC_APB2ENR_IOPCEN);
rcc_peripheral_enable_clock(&RCC_APB2ENR, RCC_APB2ENR_IOPDEN);
- /* Enable USART interrupts in the interrupt controller */
- usart_enable_irq(NVIC_UART5_IRQ);
-
- /* Init GPIOS */
gpio_set_mode(GPIO_BANK_UART5_TX, GPIO_MODE_OUTPUT_50_MHZ,
GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, GPIO_UART5_TX);
gpio_set_mode(GPIO_BANK_UART5_RX, GPIO_MODE_INPUT,
GPIO_CNF_INPUT_FLOAT, GPIO_UART5_RX);
+#endif
+ /* Enable USART interrupts in the interrupt controller */
+ usart_enable_irq(NVIC_UART5_IRQ);
/* Configure USART */
- uart_periph_set_baudrate(&uart5, UART5_BAUD, FALSE);
+ uart_periph_set_mode(&uart5, USE_UART5_TX, USE_UART5_RX, FALSE);
+ uart_periph_set_baudrate(&uart5, UART5_BAUD);
}
void uart5_isr(void) { usart_isr(&uart5); }
#endif /* USE_UART5 */
+
+
+#if defined USE_UART6 && defined STM32F4
+
+/* by default enable UART Tx and Rx */
+#ifndef USE_UART6_TX
+#define USE_UART6_TX TRUE
+#endif
+#ifndef USE_UART6_RX
+#define USE_UART6_RX TRUE
+#endif
+
+#ifndef UART6_HW_FLOW_CONTROL
+#define UART6_HW_FLOW_CONTROL FALSE
+#endif
+
+void uart6_init( void ) {
+
+ uart_periph_init(&uart6);
+ uart6.reg_addr = (void *)USART6;
+
+ /* enable uart clock */
+ rcc_peripheral_enable_clock(&RCC_APB2ENR, RCC_APB2ENR_USART6EN);
+
+ /* init RCC and GPIOs */
+ set_uart_pin(UART6_GPIO_PORT_RX, UART6_GPIO_RX, UART6_GPIO_AF);
+ set_uart_pin(UART6_GPIO_PORT_TX, UART6_GPIO_TX, UART6_GPIO_AF);
+
+ /* Enable USART interrupts in the interrupt controller */
+ usart_enable_irq(NVIC_USART6_IRQ);
+
+#if UART6_HW_FLOW_CONTROL
+#warning "USING UART6 FLOW CONTROL. Make sure to pull down CTS if you are not connecting any flow-control-capable hardware."
+ /* setup CTS and RTS pins */
+ set_uart_pin(UART6_GPIO_PORT_CTS, UART6_GPIO_CTS, UART6_GPIO_AF);
+ set_uart_pin(UART6_GPIO_PORT_RTS, UART6_GPIO_RTS, UART6_GPIO_AF);
+#endif
+
+ /* Configure USART Tx,Rx and hardware flow control*/
+ uart_periph_set_mode(&uart6, USE_UART6_TX, USE_UART6_RX, UART6_HW_FLOW_CONTROL);
+
+ uart_periph_set_baudrate(&uart6, UART6_BAUD, FALSE);
+}
+
+void usart6_isr(void) { usart_isr(&uart6); }
+
+#endif /* USE_UART6 */
diff --git a/sw/airborne/arch/stm32/peripherals/hmc5843_arch.h b/sw/airborne/arch/stm32/peripherals/hmc5843_arch.h
index cee279a4e4..ba3f9611b1 100644
--- a/sw/airborne/arch/stm32/peripherals/hmc5843_arch.h
+++ b/sw/airborne/arch/stm32/peripherals/hmc5843_arch.h
@@ -22,7 +22,7 @@
#ifndef HMC5843_ARCH_H
#define HMC5843_ARCH_H
-#include
+#include
/* returns true if conversion done */
static inline int mag_eoc(void)
diff --git a/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.c b/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.c
index 3a9dff8fd3..a39b5c7e39 100644
--- a/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.c
+++ b/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.c
@@ -26,32 +26,49 @@
#include "subsystems/actuators/actuators_pwm_arch.h"
#include "subsystems/actuators/actuators_pwm.h"
-#include
-#include
+#include
+#include
#include
-#include BOARD_CONFIG
-
int32_t actuators_pwm_values[ACTUATORS_PWM_NB];
-#define PCLK 72000000
+#if defined(STM32F1)
+//#define PCLK 72000000
+#define PCLK AHB_CLK
+#elif defined(STM32F4)
+//#define PCLK 84000000
+#define PCLK AHB_CLK/2
+#endif
+
#define ONE_MHZ_CLK 1000000
+/** Default servo update rate in Hz */
#ifndef SERVO_HZ
#define SERVO_HZ 40
#endif
-#define _TIM_OC_INIT(n) TIM_OC##n##Init
-#define TIM_OC_INIT(n) _TIM_OC_INIT(n)
-
-#define _TIM_OC_PRELOADCONFIG(n) TIM_OC##n##PreloadConfig
-#define TIM_OC_PRELOADCONFIG(n) _TIM_OC_PRELOADCONFIG(n)
-
-#define _TIM_SETCOMPARE(n) TIM_SetCompare##n
-#define TIM_SETCOMPARE(n) _TIM_SETCOMPARE(n)
+// Update rate can be adapted for each timer
+#ifndef TIM1_SERVO_HZ
+#define TIM1_SERVO_HZ SERVO_HZ
+#endif
+#ifndef TIM2_SERVO_HZ
+#define TIM2_SERVO_HZ SERVO_HZ
+#endif
+#ifndef TIM3_SERVO_HZ
+#define TIM3_SERVO_HZ SERVO_HZ
+#endif
+#ifndef TIM4_SERVO_HZ
+#define TIM4_SERVO_HZ SERVO_HZ
+#endif
+#ifndef TIM5_SERVO_HZ
+#define TIM5_SERVO_HZ SERVO_HZ
+#endif
+/** Set PWM channel configuration
+ */
static inline void actuators_pwm_arch_channel_init(u32 timer_peripheral,
enum tim_oc_id oc_id) {
+ timer_disable_oc_output(timer_peripheral, oc_id);
timer_disable_oc_clear(timer_peripheral, oc_id);
timer_enable_oc_preload(timer_peripheral, oc_id);
timer_set_oc_slow_mode(timer_peripheral, oc_id);
@@ -60,276 +77,190 @@ static inline void actuators_pwm_arch_channel_init(u32 timer_peripheral,
timer_enable_oc_output(timer_peripheral, oc_id);
}
+/** Set GPIO configuration
+ */
+#if defined(STM32F4)
+static inline void set_servo_gpio(u32 gpioport, u16 pin, u8 af_num, u32 en) {
+ rcc_peripheral_enable_clock(&RCC_AHB1ENR, en);
+ gpio_mode_setup(gpioport, GPIO_MODE_AF, GPIO_PUPD_NONE, pin);
+ gpio_set_af(gpioport, af_num, pin);
+}
+#elif defined(STM32F1)
+static inline void set_servo_gpio(u32 gpioport, u16 pin, u8 none, u32 en) {
+ rcc_peripheral_enable_clock(&RCC_APB2ENR, en | RCC_APB2ENR_AFIOEN);
+ gpio_set_mode(gpioport, GPIO_MODE_OUTPUT_50_MHZ,
+ GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, pin);
+}
+#endif
+
+/** Set Timer configuration
+ */
+static inline void set_servo_timer(u32 timer, u32 period, u8 channels_mask) {
+ timer_reset(timer);
+
+ /* Timer global mode:
+ * - No divider.
+ * - Alignement edge.
+ * - Direction up.
+ */
+ timer_set_mode(timer, TIM_CR1_CKD_CK_INT, TIM_CR1_CMS_EDGE, TIM_CR1_DIR_UP);
+
+ timer_set_prescaler(timer, (PCLK / ONE_MHZ_CLK) - 1); // 1uS
+
+ timer_disable_preload(timer);
+
+ timer_continuous_mode(timer);
+
+ timer_set_period(timer, (ONE_MHZ_CLK / period) - 1);
+
+ /* Disable outputs and configure channel if needed. */
+ if (bit_is_set(channels_mask, 0)) {
+ actuators_pwm_arch_channel_init(timer, TIM_OC1);
+ }
+ if (bit_is_set(channels_mask, 1)) {
+ actuators_pwm_arch_channel_init(timer, TIM_OC2);
+ }
+ if (bit_is_set(channels_mask, 2)) {
+ actuators_pwm_arch_channel_init(timer, TIM_OC3);
+ }
+ if (bit_is_set(channels_mask, 3)) {
+ actuators_pwm_arch_channel_init(timer, TIM_OC4);
+ }
+
+ /*
+ * Set initial output compare values.
+ * Note: Maybe we should preload the compare registers with some sensible
+ * values before we enable the timer?
+ */
+ //timer_set_oc_value(timer, TIM_OC1, 1000);
+ //timer_set_oc_value(timer, TIM_OC2, 1000);
+ //timer_set_oc_value(timer, TIM_OC3, 1000);
+ //timer_set_oc_value(timer, TIM_OC4, 1000);
+
+ /* -- Enable timer -- */
+ /*
+ * ARR reload enable.
+ * Note: In our case it does not matter much if we do preload or not. As it
+ * is unlikely we will want to change the frequency of the timer during
+ * runtime anyways.
+ */
+ timer_enable_preload(timer);
+
+ /* Counter enable. */
+ timer_enable_counter(timer);
+}
+
+/** PWM arch init called by generic pwm driver
+ */
void actuators_pwm_arch_init(void) {
/*-----------------------------------
* Configure timer peripheral clocks
*-----------------------------------*/
- /* TIM3, TIM4 and TIM5 clock enable */
+#if PWM_USE_TIM1
+ rcc_peripheral_enable_clock(&RCC_APB2ENR, RCC_APB2ENR_TIM1EN);
+#endif
+#if PWM_USE_TIM2
+ rcc_peripheral_enable_clock(&RCC_APB1ENR, RCC_APB1ENR_TIM2EN);
+#endif
+#if PWM_USE_TIM3
rcc_peripheral_enable_clock(&RCC_APB1ENR, RCC_APB1ENR_TIM3EN);
-#if REMAP_SERVOS_5AND6
- rcc_peripheral_enable_clock(&RCC_APB1ENR, RCC_APB1ENR_TIM5EN);
-#else
+#endif
+#if PWM_USE_TIM4
rcc_peripheral_enable_clock(&RCC_APB1ENR, RCC_APB1ENR_TIM4EN);
#endif
-#if USE_SERVOS_7AND8
- rcc_peripheral_enable_clock(&RCC_APB1ENR, RCC_APB1ENR_TIM4EN);
+#if PWM_USE_TIM5
+ rcc_peripheral_enable_clock(&RCC_APB1ENR, RCC_APB1ENR_TIM5EN);
#endif
/*----------------
* Configure GPIO
*----------------*/
- /* GPIO A,B and C clock enable */
- rcc_peripheral_enable_clock(&RCC_APB2ENR, RCC_APB2ENR_IOPAEN |
- RCC_APB2ENR_IOPBEN |
- RCC_APB2ENR_IOPCEN |
- RCC_APB2ENR_AFIOEN);
-
+#if defined(STM32F1)
/* TIM3 GPIO for PWM1..4 */
AFIO_MAPR |= AFIO_MAPR_TIM3_REMAP_FULL_REMAP;
- gpio_set_mode(GPIO_BANK_TIM3_FR,
- GPIO_MODE_OUTPUT_50_MHZ,
- GPIO_CNF_OUTPUT_ALTFN_PUSHPULL,
- GPIO_TIM3_FR_CH1 |
- GPIO_TIM3_FR_CH2 |
- GPIO_TIM3_FR_CH3 |
- GPIO_TIM3_FR_CH4);
-
- /* TIM4 GPIO for PWM7..8 */
-#if USE_SERVOS_7AND8
- gpio_set_mode(GPIO_BANK_TIM4,
- GPIO_MODE_OUTPUT_50_MHZ,
- GPIO_CNF_OUTPUT_ALTFN_PUSHPULL,
- GPIO_TIM4_CH1 |
- GPIO_TIM4_CH2);
#endif
- /* TIM4/5 GPIO for PWM6..7 */
-#if REMAP_SERVOS_5AND6
- gpio_set_mode(GPIO_BANK_TIM5,
- GPIO_MODE_OUTPUT_50_MHZ,
- GPIO_CNF_OUTPUT_ALTFN_PUSHPULL,
- GPIO_TIM5_CH1 |
- GPIO_TIM5_CH2);
-#else
- gpio_set_mode(GPIO_BANK_TIM4,
- GPIO_MODE_OUTPUT_50_MHZ,
- GPIO_CNF_OUTPUT_ALTFN_PUSHPULL,
- GPIO_TIM4_CH3 |
- GPIO_TIM4_CH4);
+#ifdef PWM_SERVO_0
+ set_servo_gpio(PWM_SERVO_0_GPIO, PWM_SERVO_0_PIN, PWM_SERVO_0_AF, PWM_SERVO_0_RCC_IOP);
+#endif
+#ifdef PWM_SERVO_1
+ set_servo_gpio(PWM_SERVO_1_GPIO, PWM_SERVO_1_PIN, PWM_SERVO_1_AF, PWM_SERVO_1_RCC_IOP);
+#endif
+#ifdef PWM_SERVO_2
+ set_servo_gpio(PWM_SERVO_2_GPIO, PWM_SERVO_2_PIN, PWM_SERVO_2_AF, PWM_SERVO_2_RCC_IOP);
+#endif
+#ifdef PWM_SERVO_3
+ set_servo_gpio(PWM_SERVO_3_GPIO, PWM_SERVO_3_PIN, PWM_SERVO_3_AF, PWM_SERVO_3_RCC_IOP);
+#endif
+#ifdef PWM_SERVO_4
+ set_servo_gpio(PWM_SERVO_4_GPIO, PWM_SERVO_4_PIN, PWM_SERVO_4_AF, PWM_SERVO_4_RCC_IOP);
+#endif
+#ifdef PWM_SERVO_5
+ set_servo_gpio(PWM_SERVO_5_GPIO, PWM_SERVO_5_PIN, PWM_SERVO_5_AF, PWM_SERVO_5_RCC_IOP);
+#endif
+#ifdef PWM_SERVO_6
+ set_servo_gpio(PWM_SERVO_6_GPIO, PWM_SERVO_6_PIN, PWM_SERVO_6_AF, PWM_SERVO_6_RCC_IOP);
+#endif
+#ifdef PWM_SERVO_7
+ set_servo_gpio(PWM_SERVO_7_GPIO, PWM_SERVO_7_PIN, PWM_SERVO_7_AF, PWM_SERVO_7_RCC_IOP);
+#endif
+#ifdef PWM_SERVO_8
+ set_servo_gpio(PWM_SERVO_8_GPIO, PWM_SERVO_8_PIN, PWM_SERVO_8_AF, PWM_SERVO_8_RCC_IOP);
#endif
- /*---------------
- * Timer 3 setup
- *---------------*/
- timer_reset(TIM3);
- /* Timer global mode:
- * - No divider.
- * - Alignement edge.
- * - Direction up.
- */
- timer_set_mode(TIM3, TIM_CR1_CKD_CK_INT,
- TIM_CR1_CMS_EDGE, TIM_CR1_DIR_UP);
-
- timer_set_prescaler(TIM3, (PCLK / ONE_MHZ_CLK) - 1); // 1uS
-
- timer_disable_preload(TIM3);
-
- timer_continuous_mode(TIM3);
-
- timer_set_period(TIM3, (ONE_MHZ_CLK / SERVO_HZ) - 1);
-
- /* Disable outputs. */
- timer_disable_oc_output(TIM3, TIM_OC1);
- timer_disable_oc_output(TIM3, TIM_OC2);
- timer_disable_oc_output(TIM3, TIM_OC3);
- timer_disable_oc_output(TIM3, TIM_OC4);
-
- /* -- Channel configuration -- */
- actuators_pwm_arch_channel_init(TIM3, TIM_OC1);
- actuators_pwm_arch_channel_init(TIM3, TIM_OC2);
- actuators_pwm_arch_channel_init(TIM3, TIM_OC3);
- actuators_pwm_arch_channel_init(TIM3, TIM_OC4);
-
- /*
- * Set initial output compare values.
- * Note: Maybe we should preload the compare registers with some sensible
- * values before we enable the timer?
- */
- //timer_set_oc_value(TIM3, TIM_OC1, 1000);
- //timer_set_oc_value(TIM3, TIM_OC2, 1000);
- //timer_set_oc_value(TIM3, TIM_OC3, 1000);
- //timer_set_oc_value(TIM3, TIM_OC4, 1000);
-
- /* -- Enable timer -- */
- /*
- * ARR reload enable.
- * Note: In our case it does not matter much if we do preload or not. As it
- * is unlikely we will want to change the frequency of the timer during
- * runtime anyways.
- */
- timer_enable_preload(TIM3);
-
- /* Counter enable. */
- timer_enable_counter(TIM3);
-
-#if (!REMAP_SERVOS_5AND6 || USE_SERVOS_7AND8)
-#if !REMAP_SERVOS_5AND6
-PRINT_CONFIG_MSG("Not remapping servos 5 and 6 using PB8 and PB9 -> TIM4")
-#endif
-#if USE_SERVOS_7AND8
-PRINT_CONFIG_MSG("Enabling sevros 7 and 8 on PB6, PB7 -> TIM4")
-#endif
- /*---------------
- * Timer 4 setup
- *---------------*/
- timer_reset(TIM4);
-
- /* Timer global mode:
- * - No divider.
- * - Alignement edge.
- * - Direction up.
- */
- timer_set_mode(TIM4, TIM_CR1_CKD_CK_INT,
- TIM_CR1_CMS_EDGE, TIM_CR1_DIR_UP);
-
- timer_set_prescaler(TIM4, (PCLK / ONE_MHZ_CLK) - 1); // 1uS
-
- timer_disable_preload(TIM4);
-
- timer_continuous_mode(TIM4);
-
-#ifdef SERVO_HZ_SECONDARY
- timer_set_period(TIM4, (ONE_MHZ_CLK / SERVO_HZ_SECONDARY) - 1);
-#else
- timer_set_period(TIM4, (ONE_MHZ_CLK / SERVO_HZ) - 1);
+#if PWM_USE_TIM1
+ set_servo_timer(TIM1, TIM1_SERVO_HZ, PWM_TIM1_CHAN_MASK);
#endif
- /* Disable outputs. */
-#if USE_SERVOS_7AND8
- timer_disable_oc_output(TIM4, TIM_OC1);
- timer_disable_oc_output(TIM4, TIM_OC2);
-#endif
-#if !REMAP_SERVOS_5AND6
- timer_disable_oc_output(TIM4, TIM_OC3);
- timer_disable_oc_output(TIM4, TIM_OC4);
+#if PWM_USE_TIM2
+ set_servo_timer(TIM2, TIM2_SERVO_HZ, PWM_TIM2_CHAN_MASK);
#endif
- /* -- Channel configuration -- */
-#if USE_SERVOS_7AND8
- actuators_pwm_arch_channel_init(TIM4, TIM_OC1);
- actuators_pwm_arch_channel_init(TIM4, TIM_OC2);
-#endif
-#if !REMAP_SERVOS_5AND6
- actuators_pwm_arch_channel_init(TIM4, TIM_OC3);
- actuators_pwm_arch_channel_init(TIM4, TIM_OC4);
+#if PWM_USE_TIM3
+ set_servo_timer(TIM3, TIM3_SERVO_HZ, PWM_TIM3_CHAN_MASK);
#endif
- /*
- * Set initial output compare values.
- * Note: Maybe we should preload the compare registers with some sensible
- * values before we enable the timer?
- */
-#if USE_SERVOS_7AND8
- //timer_set_oc_value(TIM4, TIM_OC1, 1000);
- //timer_set_oc_value(TIM4, TIM_OC2, 1000);
-#endif
-#if ! REMAP_SERVOS_5AND6
- //timer_set_oc_value(TIM4, TIM_OC3, 1000);
- //timer_set_oc_value(TIM4, TIM_OC4, 1000);
+#if PWM_USE_TIM4
+ set_servo_timer(TIM4, TIM4_SERVO_HZ, PWM_TIM4_CHAN_MASK);
#endif
- /* -- Enable timer -- */
- /*
- * ARR reload enable.
- * Note: In our case it does not matter much if we do preload or not. As it
- * is unlikely we will want to change the frequency of the timer during
- * runtime anyways.
- */
- timer_enable_preload(TIM4);
-
- /* Counter enable. */
- timer_enable_counter(TIM4);
-
-#endif
-
-#if REMAP_SERVOS_5AND6
-PRINT_CONFIG_MSG("Remapping servo outputs 5 and 6 to PA0,PA1 -> TIM5")
- /*---------------
- * Timer 5 setup
- *---------------*/
- timer_reset(TIM5);
-
- /* Timer global mode:
- * - No divider.
- * - Alignement edge.
- * - Direction up.
- */
- timer_set_mode(TIM5, TIM_CR1_CKD_CK_INT,
- TIM_CR1_CMS_EDGE, TIM_CR1_DIR_UP);
-
- timer_set_prescaler(TIM5, (PCLK / ONE_MHZ_CLK) - 1); // 1uS
-
- timer_disable_preload(TIM5);
-
- timer_continuous_mode(TIM5);
-
-#ifdef SERVO_HZ_SECONDARY
- timer_set_period(TIM5, (ONE_MHZ_CLK / SERVO_HZ_SECONDARY) - 1);
-#else
- timer_set_period(TIM5, (ONE_MHZ_CLK / SERVO_HZ) - 1);
-#endif
-
- /* Disable outputs. */
- timer_disable_oc_output(TIM5, TIM_OC1);
- timer_disable_oc_output(TIM5, TIM_OC2);
-
- /* -- Channel configuration -- */
- actuators_pwm_arch_channel_init(TIM5, TIM_OC1);
- actuators_pwm_arch_channel_init(TIM5, TIM_OC2);
-
- /*
- * Set the capture compare value for OC1.
- * Note: Maybe we should preload the compare registers with some sensible
- * values before we enable the timer?
- */
- //timer_set_oc_value(TIM5, TIM_OC1, 1000);
- //timer_set_oc_value(TIM5, TIM_OC2, 1000);
-
- /* -- Enable timer -- */
- /*
- * ARR reload enable.
- * Note: In our case it does not matter much if we do preload or not. As it
- * is unlikely we will want to change the frequency of the timer during
- * runtime anyways.
- */
- timer_enable_preload(TIM5);
-
- /* Counter enable. */
- timer_enable_counter(TIM5);
-
+#if PWM_USE_TIM5
+ set_servo_timer(TIM5, TIM5_SERVO_HZ, PWM_TIM5_CHAN_MASK);
#endif
}
-/* set pulse widths from actuator values, assumed to be in us */
+/** Set pulse widths from actuator values, assumed to be in us
+ */
void actuators_pwm_commit(void) {
- timer_set_oc_value(TIM3, TIM_OC1, actuators_pwm_values[0]);
- timer_set_oc_value(TIM3, TIM_OC2, actuators_pwm_values[1]);
- timer_set_oc_value(TIM3, TIM_OC3, actuators_pwm_values[2]);
- timer_set_oc_value(TIM3, TIM_OC4, actuators_pwm_values[3]);
-
-#if USE_SERVOS_7AND8
- timer_set_oc_value(TIM4, TIM_OC1, actuators_pwm_values[6]);
- timer_set_oc_value(TIM4, TIM_OC2, actuators_pwm_values[7]);
+#ifdef PWM_SERVO_0
+ timer_set_oc_value(PWM_SERVO_0_TIMER, PWM_SERVO_0_OC, actuators_pwm_values[PWM_SERVO_0]);
#endif
-#if REMAP_SERVOS_5AND6
- timer_set_oc_value(TIM5, TIM_OC1, actuators_pwm_values[4]);
- timer_set_oc_value(TIM5, TIM_OC2, actuators_pwm_values[5]);
-#else
- timer_set_oc_value(TIM4, TIM_OC3, actuators_pwm_values[4]);
- timer_set_oc_value(TIM4, TIM_OC4, actuators_pwm_values[5]);
+#ifdef PWM_SERVO_1
+ timer_set_oc_value(PWM_SERVO_1_TIMER, PWM_SERVO_1_OC, actuators_pwm_values[PWM_SERVO_1]);
+#endif
+#ifdef PWM_SERVO_2
+ timer_set_oc_value(PWM_SERVO_2_TIMER, PWM_SERVO_2_OC, actuators_pwm_values[PWM_SERVO_2]);
+#endif
+#ifdef PWM_SERVO_3
+ timer_set_oc_value(PWM_SERVO_3_TIMER, PWM_SERVO_3_OC, actuators_pwm_values[PWM_SERVO_3]);
+#endif
+#ifdef PWM_SERVO_4
+ timer_set_oc_value(PWM_SERVO_4_TIMER, PWM_SERVO_4_OC, actuators_pwm_values[PWM_SERVO_4]);
+#endif
+#ifdef PWM_SERVO_5
+ timer_set_oc_value(PWM_SERVO_5_TIMER, PWM_SERVO_5_OC, actuators_pwm_values[PWM_SERVO_5]);
+#endif
+#ifdef PWM_SERVO_6
+ timer_set_oc_value(PWM_SERVO_6_TIMER, PWM_SERVO_6_OC, actuators_pwm_values[PWM_SERVO_6]);
+#endif
+#ifdef PWM_SERVO_7
+ timer_set_oc_value(PWM_SERVO_7_TIMER, PWM_SERVO_7_OC, actuators_pwm_values[PWM_SERVO_7]);
+#endif
+#ifdef PWM_SERVO_8
+ timer_set_oc_value(PWM_SERVO_8_TIMER, PWM_SERVO_8_OC, actuators_pwm_values[PWM_SERVO_8]);
#endif
}
diff --git a/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.h b/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.h
index 1c847b29f1..a5878455b1 100644
--- a/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.h
+++ b/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.h
@@ -28,15 +28,11 @@
#include "std.h"
-#if USE_SERVOS_7AND8
-#if (defined(BOARD_LISA_M) || defined(BOARD_LIA)) && USE_I2C1
-#error "You cannot use Servos 7and8 and I2C1 at the same time"
-#else
+#include BOARD_CONFIG
+
+#ifndef ACTUATORS_PWM_NB
#define ACTUATORS_PWM_NB 8
#endif
-#else
-#define ACTUATORS_PWM_NB 6
-#endif
extern int32_t actuators_pwm_values[ACTUATORS_PWM_NB];
diff --git a/sw/airborne/arch/stm32/subsystems/radio_control/ppm_arch.c b/sw/airborne/arch/stm32/subsystems/radio_control/ppm_arch.c
index 2bed0ba113..faac949563 100644
--- a/sw/airborne/arch/stm32/subsystems/radio_control/ppm_arch.c
+++ b/sw/airborne/arch/stm32/subsystems/radio_control/ppm_arch.c
@@ -34,14 +34,18 @@
#include "subsystems/radio_control.h"
#include "subsystems/radio_control/ppm.h"
-#include
-#include
+#include BOARD_CONFIG
+
+#include
+#include
#include
-#include
+#include
#include "mcu_periph/sys_time.h"
+#define ONE_MHZ_CLK 1000000
+
uint8_t ppm_cur_pulse;
uint32_t ppm_last_pulse_time;
bool_t ppm_data_valid;
@@ -49,34 +53,19 @@ static uint32_t timer_rollover_cnt;
#if USE_PPM_TIM2
-PRINT_CONFIG_MSG("Using TIM2 for PPM input on PA_10 (SERVO6) pin.")
+PRINT_CONFIG_MSG("Using TIM2 for PPM input.")
-#define PPM_RCC &RCC_APB1ENR
-#define PPM_PERIPHERAL RCC_APB1ENR_TIM2EN
-#define PPM_TIMER TIM2
-#define PPM_CHANNEL TIM_IC2
-#define PPM_TIMER_INPUT TIM_IC_IN_TI2
-#define PPM_IRQ NVIC_TIM2_IRQ
-#define PPM_IRQ_FLAGS (TIM_DIER_CC2IE | TIM_DIER_UIE)
-#define PPM_GPIO_PERIPHERAL RCC_APB2ENR_IOPAEN
-#define PPM_GPIO_PORT GPIOA
-#define PPM_GPIO_PIN GPIO1
+#define PPM_RCC &RCC_APB1ENR
+#define PPM_PERIPHERAL RCC_APB1ENR_TIM2EN
+#define PPM_TIMER TIM2
#elif USE_PPM_TIM1
-PRINT_CONFIG_MSG("Using TIM1 for PPM input on PA_01 (UART1_RX) pin.")
+PRINT_CONFIG_MSG("Using TIM1 for PPM input.")
-#define PPM_RCC &RCC_APB2ENR
-#define PPM_PERIPHERAL RCC_APB2ENR_TIM1EN
-#define PPM_TIMER TIM1
-#define PPM_CHANNEL TIM_IC3
-#define PPM_TIMER_INPUT TIM_IC_IN_TI3
-#define PPM_IRQ NVIC_TIM1_UP_IRQ
-#define PPM_IRQ2 NVIC_TIM1_CC_IRQ
-#define PPM_IRQ_FLAGS (TIM_DIER_CC3IE | TIM_DIER_UIE)
-#define PPM_GPIO_PERIPHERAL RCC_APB2ENR_IOPAEN
-#define PPM_GPIO_PORT GPIOA
-#define PPM_GPIO_PIN GPIO10
+#define PPM_RCC &RCC_APB2ENR
+#define PPM_PERIPHERAL RCC_APB2ENR_TIM1EN
+#define PPM_TIMER TIM1
#endif
@@ -85,24 +74,29 @@ void ppm_arch_init ( void ) {
/* timer clock enable */
rcc_peripheral_enable_clock(PPM_RCC, PPM_PERIPHERAL);
+#if defined(STM32F1)
/* GPIOA clock enable */
rcc_peripheral_enable_clock(&RCC_APB2ENR, PPM_GPIO_PERIPHERAL);
-
/* timer gpio configuration */
gpio_set_mode(PPM_GPIO_PORT, GPIO_MODE_INPUT,
- GPIO_CNF_INPUT_FLOAT, PPM_GPIO_PIN);
+ GPIO_CNF_INPUT_FLOAT, PPM_GPIO_PIN);
+#elif defined(STM32F4)
+ /* GPIO clock enable */
+ rcc_peripheral_enable_clock(&RCC_AHB1ENR, PPM_GPIO_PERIPHERAL);
+ /* timer gpio configuration */
+ gpio_mode_setup(PPM_GPIO_PORT, GPIO_MODE_AF, GPIO_PUPD_NONE, PPM_GPIO_PIN);
+ gpio_set_af(PPM_GPIO_PORT, PPM_GPIO_AF, PPM_GPIO_PIN);
+#endif
/* Time Base configuration */
timer_reset(PPM_TIMER);
timer_set_mode(PPM_TIMER, TIM_CR1_CKD_CK_INT,
- TIM_CR1_CMS_EDGE, TIM_CR1_DIR_UP);
+ TIM_CR1_CMS_EDGE, TIM_CR1_DIR_UP);
timer_set_period(PPM_TIMER, 0xFFFF);
- /* run ppm timer at cpu freq / 9 = 8MHz */
- timer_set_prescaler(PPM_TIMER, 8);
+ timer_set_prescaler(PPM_TIMER, (AHB_CLK / (RC_PPM_TICKS_PER_USEC*ONE_MHZ_CLK)) - 1);
/* TIM configuration: Input Capture mode ---------------------
- The Rising edge is used as active edge,
- Intput pin is either PA1 or PA10
+ The Rising edge is used as active edge
------------------------------------------------------------ */
#if defined PPM_PULSE_TYPE && PPM_PULSE_TYPE == PPM_PULSE_TYPE_POSITIVE
timer_ic_set_polarity(PPM_TIMER, PPM_CHANNEL, TIM_IC_RISING);
@@ -125,7 +119,7 @@ void ppm_arch_init ( void ) {
#endif
/* Enable the CC2 and Update interrupt requests. */
- timer_enable_irq(PPM_TIMER, PPM_IRQ_FLAGS);
+ timer_enable_irq(PPM_TIMER, (PPM_IRQ_FLAGS | TIM_DIER_UIE));
/* Enable capture channel. */
timer_ic_enable(PPM_TIMER, PPM_CHANNEL);
@@ -158,7 +152,11 @@ void tim2_isr(void) {
#elif USE_PPM_TIM1
+#if defined(STM32F1)
void tim1_up_isr(void) {
+#elif defined(STM32F4)
+void tim1_up_tim10_isr(void) {
+#endif
if((TIM1_SR & TIM_SR_UIF) != 0) {
timer_rollover_cnt+=(1<<16);
timer_clear_flag(TIM1, TIM_SR_UIF);
@@ -166,8 +164,8 @@ void tim1_up_isr(void) {
}
void tim1_cc_isr(void) {
- if((TIM2_SR & TIM_SR_CC3IF) != 0) {
- timer_clear_flag(TIM1, TIM_SR_CC3IF);
+ if((TIM1_SR & PPM_IRQ_CCIF) != 0) {
+ timer_clear_flag(TIM1, PPM_IRQ_CCIF);
uint32_t now = timer_get_counter(TIM1) + timer_rollover_cnt;
DecodePpmFrame(now);
@@ -175,3 +173,4 @@ void tim1_cc_isr(void) {
}
#endif
+
diff --git a/sw/airborne/arch/stm32/subsystems/radio_control/ppm_arch.h b/sw/airborne/arch/stm32/subsystems/radio_control/ppm_arch.h
index e1ccb0d9ea..fbe1ece8da 100644
--- a/sw/airborne/arch/stm32/subsystems/radio_control/ppm_arch.h
+++ b/sw/airborne/arch/stm32/subsystems/radio_control/ppm_arch.h
@@ -33,11 +33,14 @@
#include "mcu_periph/sys_time.h"
/**
- * The ppm counter is running at cpu freq / 9
+ * The ppm counter is running at cpu freq / 72 or 168 / 8
+ * so the counter has 1/8 us resolution
*/
-#define RC_PPM_TICKS_OF_USEC(_v) ((_v) * (AHB_CLK / 9000000))
-#define RC_PPM_SIGNED_TICKS_OF_USEC(_v) (int32_t)((_v) * (AHB_CLK / 9000000))
-#define USEC_OF_RC_PPM_TICKS(_v) ((_v) / (AHB_CLK / 9000000))
+#define RC_PPM_TICKS_PER_USEC 8
+
+#define RC_PPM_TICKS_OF_USEC(_v) ((_v)*RC_PPM_TICKS_PER_USEC)
+#define RC_PPM_SIGNED_TICKS_OF_USEC(_v) (int32_t)((_v)*RC_PPM_TICKS_PER_USEC)
+#define USEC_OF_RC_PPM_TICKS(_v) ((_v)/RC_PPM_TICKS_PER_USEC)
#define PPM_NB_CHANNEL RADIO_CONTROL_NB_CHANNEL
diff --git a/sw/airborne/arch/stm32/subsystems/radio_control/spektrum_arch.c b/sw/airborne/arch/stm32/subsystems/radio_control/spektrum_arch.c
index f83a9ebe7f..2d59d6188f 100644
--- a/sw/airborne/arch/stm32/subsystems/radio_control/spektrum_arch.c
+++ b/sw/airborne/arch/stm32/subsystems/radio_control/spektrum_arch.c
@@ -20,15 +20,18 @@
*/
#include
-#include
-#include
-#include
+#include
+#include
#include
#include
+#include
+
#include "subsystems/radio_control.h"
#include "subsystems/radio_control/spektrum_arch.h"
#include "mcu_periph/uart.h"
+#include BOARD_CONFIG
+
#define SPEKTRUM_CHANNELS_PER_FRAME 7
#define MAX_SPEKTRUM_FRAMES 2
#define MAX_SPEKTRUM_CHANNELS 16
@@ -40,11 +43,6 @@
#define MASTER_RECEIVER_PULSES 5
#define SLAVE_RECEIVER_PULSES 6
-/* The line that is pulled low at power up to initiate the bind process */
-#define BIND_PIN GPIO_Pin_3
-#define BIND_PIN_PORT GPIOC
-#define BIND_PIN_PERIPH RCC_APB2Periph_GPIOC
-
#define TIM_FREQ_1000000 1000000
#define TIM_TICS_FOR_100us 100
#define MIN_FRAME_SPACE 70 // 7ms
@@ -133,6 +131,13 @@ static void SpektrumDelayInit( void );
*
*****************************************************************************/
void radio_control_impl_init(void) {
+
+ PrimarySpektrumState.ReSync = 1;
+
+#ifdef RADIO_CONTROL_SPEKTRUM_SECONDARY_PORT
+ SecondarySpektrumState.ReSync = 1;
+#endif
+
SpektrumTimerInit();
// DebugInit();
SpektrumUartInit();
@@ -538,31 +543,33 @@ void SpektrumUartInit(void) {
#ifdef RADIO_CONTROL_SPEKTRUM_SECONDARY_PORT
/* init RCC */
- rcc_peripheral_enable_clock(&RCC_APB2ENR, RCC_APB2ENR_IOPDEN);
- rcc_peripheral_enable_clock(&RCC_APB1ENR, RCC_APB1ENR_UART5EN);
+ rcc_peripheral_enable_clock(&RCC_APB2ENR, SecondaryUart(_RCC_GPIO));
+ rcc_peripheral_enable_clock(SecondaryUart(_RCC_REG), SecondaryUart(_RCC_DEV));
/* Enable USART interrupts */
- nvic_set_priority(NVIC_UART5_IRQ, 3);
- nvic_enable_irq(NVIC_UART5_IRQ);
+ nvic_set_priority(SecondaryUart(_IRQ), 3);
+ nvic_enable_irq(SecondaryUart(_IRQ));
/* Init GPIOS */;
/* Secondary UART Rx pin as floating input */
- gpio_set_mode(GPIO_BANK_UART5_RX, GPIO_MODE_INPUT,
- GPIO_CNF_INPUT_FLOAT, GPIO_UART5_RX);
+ gpio_set_mode(SecondaryUart(_BANK), GPIO_MODE_INPUT,
+ GPIO_CNF_INPUT_FLOAT, SecondaryUart(_PIN));
+
+ SecondaryUart(_REMAP);
/* Configure secondary UART */
- usart_set_baudrate(UART5, 115200);
- usart_set_databits(UART5, 8);
- usart_set_stopbits(UART5, USART_STOPBITS_1);
- usart_set_parity(UART5, USART_PARITY_NONE);
- usart_set_flow_control(UART5, USART_FLOWCONTROL_NONE);
- usart_set_mode(UART5, USART_MODE_RX);
+ usart_set_baudrate(SecondaryUart(_DEV), 115200);
+ usart_set_databits(SecondaryUart(_DEV), 8);
+ usart_set_stopbits(SecondaryUart(_DEV), USART_STOPBITS_1);
+ usart_set_parity(SecondaryUart(_DEV), USART_PARITY_NONE);
+ usart_set_flow_control(SecondaryUart(_DEV), USART_FLOWCONTROL_NONE);
+ usart_set_mode(SecondaryUart(_DEV), USART_MODE_RX);
/* Enable Secondary UART Receive interrupts */
- USART_CR1(UART5) |= USART_CR1_RXNEIE;
+ USART_CR1(SecondaryUart(_DEV)) |= USART_CR1_RXNEIE;
/* Enable the Primary UART */
- usart_enable(UART5);
+ usart_enable(SecondaryUart(_DEV));
#endif
}
@@ -595,16 +602,16 @@ void PrimaryUart(_ISR)(void) {
*
*****************************************************************************/
#ifdef RADIO_CONTROL_SPEKTRUM_SECONDARY_PORT
-void uart5_isr(void) {
+void SecondaryUart(_ISR)(void) {
- if (((USART_CR1(UART5) & USART_CR1_TXEIE) != 0) &&
- ((USART_SR(UART5) & USART_SR_TXE) != 0)) {
- USART_CR1(UART5) &= ~USART_CR1_TXEIE;
+ if (((USART_CR1(SecondaryUart(_DEV)) & USART_CR1_TXEIE) != 0) &&
+ ((USART_SR(SecondaryUart(_DEV)) & USART_SR_TXE) != 0)) {
+ USART_CR1(SecondaryUart(_DEV)) &= ~USART_CR1_TXEIE;
}
- if (((USART_CR1(UART5) & USART_CR1_RXNEIE) != 0) &&
- ((USART_SR(UART5) & USART_SR_RXNE) != 0)) {
- uint8_t b = usart_recv(UART5);
+ if (((USART_CR1(SecondaryUart(_DEV)) & USART_CR1_RXNEIE) != 0) &&
+ ((USART_SR(SecondaryUart(_DEV)) & USART_SR_RXNE) != 0)) {
+ uint8_t b = usart_recv(SecondaryUart(_DEV));
SpektrumParser(b, &SecondarySpektrumState, TRUE);
}
@@ -643,15 +650,15 @@ void DebugInit(void) {
void radio_control_spektrum_try_bind(void) {
/* init RCC */
- rcc_peripheral_enable_clock(&RCC_APB2ENR, RCC_APB2ENR_IOPCEN);
+ rcc_peripheral_enable_clock(&RCC_APB2ENR, SPEKTRUM_BIND_PIN_RCC_IOP);
/* Init GPIO for the bind pin */
- gpio_set(GPIOC, GPIO3);
- gpio_set_mode(GPIOC, GPIO_MODE_INPUT,
- GPIO_CNF_INPUT_PULL_UPDOWN, GPIO3);
+ gpio_set(SPEKTRUM_BIND_PIN_PORT, SPEKTRUM_BIND_PIN);
+ gpio_set_mode(SPEKTRUM_BIND_PIN_PORT, GPIO_MODE_INPUT,
+ GPIO_CNF_INPUT_PULL_UPDOWN, SPEKTRUM_BIND_PIN);
/* exit if the BIND_PIN is high, it needs to
be pulled low at startup to initiate bind */
- if (gpio_get(GPIOC, GPIO3) != 0)
+ if (gpio_get(SPEKTRUM_BIND_PIN_PORT, SPEKTRUM_BIND_PIN) != 0)
return;
/* bind initiated, initialise the delay timer */
@@ -662,21 +669,21 @@ void radio_control_spektrum_try_bind(void) {
/* Master receiver Rx push-pull */
gpio_set_mode(PrimaryUart(_BANK), GPIO_MODE_OUTPUT_50_MHZ,
- GPIO_CNF_OUTPUT_PUSHPULL, PrimaryUart(_PIN));
+ GPIO_CNF_OUTPUT_PUSHPULL, PrimaryUart(_PIN));
/* Master receiver RX line, drive high */
gpio_set(PrimaryUart(_BANK), PrimaryUart(_PIN));
#ifdef RADIO_CONTROL_SPEKTRUM_SECONDARY_PORT
- rcc_peripheral_enable_clock(&RCC_APB2ENR, RCC_APB2ENR_IOPDEN);
+ rcc_peripheral_enable_clock(&RCC_APB2ENR, SecondaryUart(_RCC_GPIO));
/* Slave receiver Rx push-pull */
- gpio_set_mode(GPIO_BANK_UART5_RX, GPIO_MODE_OUTPUT_50_MHZ,
- GPIO_CNF_OUTPUT_PUSHPULL, GPIO_UART5_RX);
+ gpio_set_mode(SecondaryUart(_BANK), GPIO_MODE_OUTPUT_50_MHZ,
+ GPIO_CNF_OUTPUT_PUSHPULL, SecondaryUart(_PIN));
/* Slave receiver RX line, drive high */
- gpio_set(GPIO_BANK_UART5_RX, GPIO_UART5_RX);
+ gpio_set(SecondaryUart(_BANK), SecondaryUart(_PIN));
#endif
/* We have no idea how long the window for allowing binding after
@@ -694,9 +701,9 @@ void radio_control_spektrum_try_bind(void) {
#ifdef RADIO_CONTROL_SPEKTRUM_SECONDARY_PORT
for (int i = 0; i < SLAVE_RECEIVER_PULSES; i++)
{
- gpio_clear(GPIO_BANK_UART5_RX, GPIO_UART5_RX);
+ gpio_clear(SecondaryUart(_BANK), SecondaryUart(_PIN));
DelayUs(120);
- gpio_set(GPIO_BANK_UART5_RX, GPIO_UART5_RX);
+ gpio_set(SecondaryUart(_BANK), SecondaryUart(_PIN));
DelayUs(120);
}
#endif /* RADIO_CONTROL_SPEKTRUM_SECONDARY_PORT */
diff --git a/sw/airborne/arch/stm32/subsystems/radio_control/spektrum_arch.h b/sw/airborne/arch/stm32/subsystems/radio_control/spektrum_arch.h
index 1b708b2e57..d96250e7d7 100644
--- a/sw/airborne/arch/stm32/subsystems/radio_control/spektrum_arch.h
+++ b/sw/airborne/arch/stm32/subsystems/radio_control/spektrum_arch.h
@@ -63,24 +63,4 @@
extern void RadioControlEventImp(void (*_received_frame_handler)(void));
-#define UART1_RCC_GPIO RCC_APB2ENR_IOPAEN
-#define UART1_RCC_REG &RCC_APB2ENR
-#define UART1_RCC_DEV RCC_APB2ENR_USART1EN
-#define UART1_BANK GPIO_BANK_USART1_RX
-#define UART1_PIN GPIO_USART1_RX
-#define UART1_IRQ NVIC_USART1_IRQ
-#define UART1_ISR usart1_isr
-#define UART1_DEV USART1
-#define UART1_REMAP {}
-
-#define UART3_RCC_GPIO RCC_APB2ENR_IOPCEN
-#define UART3_RCC_REG &RCC_APB1ENR
-#define UART3_RCC_DEV RCC_APB1ENR_USART3EN
-#define UART3_BANK GPIO_BANK_USART3_PR_RX
-#define UART3_PIN GPIO_USART3_PR_RX
-#define UART3_IRQ NVIC_USART3_IRQ
-#define UART3_ISR usart3_isr
-#define UART3_DEV USART3
-#define UART3_REMAP {AFIO_MAPR |= AFIO_MAPR_USART3_REMAP_PARTIAL_REMAP;}
-
#endif /* RADIO_CONTROL_SPEKTRUM_ARCH_H */
diff --git a/sw/airborne/arch/stm32/subsystems/settings_arch.c b/sw/airborne/arch/stm32/subsystems/settings_arch.c
index 7cb01a5a94..411e97552d 100644
--- a/sw/airborne/arch/stm32/subsystems/settings_arch.c
+++ b/sw/airborne/arch/stm32/subsystems/settings_arch.c
@@ -35,7 +35,11 @@
#include "subsystems/settings.h"
+#if defined(STM32F1)
#include
+#elif defined(STM32F4)
+#include
+#endif
#include
#include
@@ -55,7 +59,11 @@ static int32_t pflash_program_bytes(struct FlashInfo* flash,
uint32_t size,
uint32_t chksum);
+#if defined(STM32F1)
#define FLASH_SIZE_ MMIO16(0x1FFFF7E0)
+#elif defined(STM32F4)
+#define FLASH_SIZE_ MMIO16(0x1FFF7A22)
+#endif
#define FLASH_BEGIN 0x08000000
#define FSIZ 8
@@ -192,6 +200,7 @@ static int32_t pflash_program_bytes(struct FlashInfo* flash,
uint32_t src,
uint32_t size,
uint32_t chksum) {
+#if defined(STM32F1)
uint32_t i;
/* erase */
@@ -232,6 +241,9 @@ static int32_t pflash_program_bytes(struct FlashInfo* flash,
}
if (*(uint32_t*) (flash->addr+flash->page_size-FSIZ) != size) return -3;
if (*(uint32_t*) (flash->addr+flash->page_size-FCHK) != chksum) return -4;
+#elif defined(STM32F4)
+
+#endif
return 0;
}
diff --git a/sw/airborne/boards/apogee/baro_board.c b/sw/airborne/boards/apogee/baro_board.c
new file mode 100644
index 0000000000..cfabf3b2a5
--- /dev/null
+++ b/sw/airborne/boards/apogee/baro_board.c
@@ -0,0 +1,65 @@
+ /*
+ * Copyright (C) 2010 ENAC
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/**
+ * @file boards/apogee/baro_board.c
+ *
+ * integrated barometer for Apogee boards (mpl3115)
+ */
+
+#include "subsystems/sensors/baro.h"
+
+// to get MPU status
+#include "boards/apogee/imu_apogee.h"
+
+
+/* Common Baro struct */
+struct Baro baro;
+
+/** Counter to init ads1114 at startup */
+#define BARO_STARTUP_COUNTER 200
+uint16_t startup_cnt;
+
+void baro_init( void ) {
+ mpl3115_init();
+ baro.status = BS_UNINITIALIZED;
+ baro.absolute = 0;
+ baro.differential = 1; /* not handled on this board, use extra module */
+ startup_cnt = BARO_STARTUP_COUNTER;
+}
+
+void baro_periodic( void ) {
+
+ if (baro.status == BS_UNINITIALIZED && mpl3115_data_available) {
+ // Run some loops to get correct readings from the adc
+ --startup_cnt;
+ mpl3115_data_available = FALSE;
+ if (startup_cnt == 0) {
+ baro.status = BS_RUNNING;
+ }
+ }
+
+ // Baro is slave of the MPU, only start reading it after MPU is configured
+ if (imu_apogee.mpu.config.initialized)
+ Mpl3115Periodic();
+}
+
diff --git a/sw/airborne/boards/apogee/baro_board.h b/sw/airborne/boards/apogee/baro_board.h
new file mode 100644
index 0000000000..c820fd1cb1
--- /dev/null
+++ b/sw/airborne/boards/apogee/baro_board.h
@@ -0,0 +1,60 @@
+/*
+ * Copyright (C) 2013 Gautier Hattenberger (ENAC)
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/**
+ * @file boards/apogee/baro_board.h
+ *
+ * integrated barometer for Apogee boards (mpl3115)
+ */
+
+#ifndef BOARDS_APOGEE_BARO_H
+#define BOARDS_APOGEE_BARO_H
+
+#include "std.h"
+#include "peripherals/mpl3115.h"
+
+/* There is no differential pressure on the board but
+ * it can be available from an external sensor
+ * */
+
+#define BaroAbs(_handler) { \
+ mpl3115_event(); \
+ if (mpl3115_data_available) { \
+ baro.absolute = mpl3115_pressure; \
+ if (baro.status == BS_RUNNING) { \
+ _handler(); \
+ mpl3115_data_available = FALSE; \
+ } \
+ } \
+}
+
+// TODO handle baro diff
+#ifndef BaroDiff
+#define BaroDiff(_h) {}
+#endif
+
+#define BaroEvent(_b_abs_handler, _b_diff_handler) { \
+ BaroAbs(_b_abs_handler); \
+ BaroDiff(_b_diff_handler); \
+}
+
+#endif // BOARDS_APOGEE_BARO_H
diff --git a/sw/airborne/boards/apogee/imu_apogee.c b/sw/airborne/boards/apogee/imu_apogee.c
new file mode 100644
index 0000000000..886afa1559
--- /dev/null
+++ b/sw/airborne/boards/apogee/imu_apogee.c
@@ -0,0 +1,106 @@
+/*
+ * Copyright (C) 2013 Gautier Hattenberger
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file boards/apogee/imu_apogee.c
+ *
+ * Driver for the IMU on the Apogee board.
+ *
+ * Invensense MPU-6050
+ */
+
+#include
+#include "boards/apogee/imu_apogee.h"
+#include "mcu_periph/i2c.h"
+#include "led.h"
+
+// Downlink
+#include "mcu_periph/uart.h"
+#include "messages.h"
+#include "subsystems/datalink/downlink.h"
+
+#ifndef DOWNLINK_DEVICE
+#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
+#endif
+
+#if !defined APOGEE_LOWPASS_FILTER && !defined APOGEE_SMPLRT_DIV
+#define APOGEE_LOWPASS_FILTER MPU60X0_DLPF_42HZ
+#define APOGEE_SMPLRT_DIV 9
+PRINT_CONFIG_MSG("Gyro/Accel output rate is 100Hz")
+#endif
+PRINT_CONFIG_VAR(APOGEE_SMPLRT_DIV)
+PRINT_CONFIG_VAR(APOGEE_LOWPASS_FILTER)
+
+#ifndef APOGEE_GYRO_RANGE
+#define APOGEE_GYRO_RANGE MPU60X0_GYRO_RANGE_1000
+#endif
+PRINT_CONFIG_VAR(APOGEE_GYRO_RANGE)
+
+#ifndef APOGEE_ACCEL_RANGE
+#define APOGEE_ACCEL_RANGE MPU60X0_ACCEL_RANGE_8G
+#endif
+PRINT_CONFIG_VAR(APOGEE_ACCEL_RANGE)
+
+struct ImuApogee imu_apogee;
+
+void imu_impl_init(void)
+{
+ /////////////////////////////////////////////////////////////////////
+ // MPU-60X0
+ mpu60x0_i2c_init(&imu_apogee.mpu, &(IMU_APOGEE_I2C_DEV), MPU60X0_ADDR_ALT);
+ // change the default configuration
+ imu_apogee.mpu.config.smplrt_div = APOGEE_SMPLRT_DIV;
+ imu_apogee.mpu.config.dlpf_cfg = APOGEE_LOWPASS_FILTER;
+ imu_apogee.mpu.config.gyro_range = APOGEE_GYRO_RANGE;
+ imu_apogee.mpu.config.accel_range = APOGEE_ACCEL_RANGE;
+
+ imu_apogee.gyr_valid = FALSE;
+ imu_apogee.acc_valid = FALSE;
+}
+
+void imu_periodic( void )
+{
+ // Start reading the latest gyroscope data
+ mpu60x0_i2c_periodic(&imu_apogee.mpu);
+
+ //RunOnceEvery(10,imu_apogee_downlink_raw());
+}
+
+void imu_apogee_downlink_raw( void )
+{
+ DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, DefaultDevice,&imu.gyro_unscaled.p,&imu.gyro_unscaled.q,&imu.gyro_unscaled.r);
+ DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DefaultDevice,&imu.accel_unscaled.x,&imu.accel_unscaled.y,&imu.accel_unscaled.z);
+}
+
+
+void imu_apogee_event( void )
+{
+ // If the itg3200 I2C transaction has succeeded: convert the data
+ mpu60x0_i2c_event(&imu_apogee.mpu);
+ if (imu_apogee.mpu.data_available) {
+ RATES_ASSIGN(imu.gyro_unscaled, imu_apogee.mpu.data_rates.rates.p, -imu_apogee.mpu.data_rates.rates.q, -imu_apogee.mpu.data_rates.rates.r);
+ VECT3_ASSIGN(imu.accel_unscaled, imu_apogee.mpu.data_accel.vect.x, -imu_apogee.mpu.data_accel.vect.y, -imu_apogee.mpu.data_accel.vect.z);
+ imu_apogee.mpu.data_available = FALSE;
+ imu_apogee.gyr_valid = TRUE;
+ imu_apogee.acc_valid = TRUE;
+ }
+}
+
diff --git a/sw/airborne/boards/apogee/imu_apogee.h b/sw/airborne/boards/apogee/imu_apogee.h
new file mode 100644
index 0000000000..0c49a81e34
--- /dev/null
+++ b/sw/airborne/boards/apogee/imu_apogee.h
@@ -0,0 +1,128 @@
+/*
+ * Copyright (C) 2011 Gautier Hattenberger
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file boards/apogee/imu_apogee.h
+ *
+ * Driver for the IMU on the Apogee board.
+ *
+ * Invensense MPU-6050
+ */
+
+#ifndef IMU_APOGEE_H
+#define IMU_APOGEE_H
+
+#include "std.h"
+#include "generated/airframe.h"
+#include "subsystems/imu.h"
+
+#include "peripherals/mpu60x0_i2c.h"
+
+// Default configuration
+#if !defined IMU_GYRO_P_SIGN & !defined IMU_GYRO_Q_SIGN & !defined IMU_GYRO_R_SIGN
+#define IMU_GYRO_P_SIGN 1
+#define IMU_GYRO_Q_SIGN 1
+#define IMU_GYRO_R_SIGN 1
+#endif
+#if !defined IMU_ACCEL_X_SIGN & !defined IMU_ACCEL_Y_SIGN & !defined IMU_ACCEL_Z_SIGN
+#define IMU_ACCEL_X_SIGN 1
+#define IMU_ACCEL_Y_SIGN 1
+#define IMU_ACCEL_Z_SIGN 1
+#endif
+
+/** default gyro sensitivy and neutral from the datasheet
+ * MPU with 1000 deg/s has 32.8 LSB/(deg/s)
+ * sens = 1/32.8 * pi/180 * 2^INT32_RATE_FRAC
+ * sens = 1/32.8 * pi/180 * 4096 = 2.17953
+ I*/
+#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
+// FIXME
+#define IMU_GYRO_P_SENS 2.17953
+#define IMU_GYRO_P_SENS_NUM 18271
+#define IMU_GYRO_P_SENS_DEN 8383
+#define IMU_GYRO_Q_SENS 2.17953
+#define IMU_GYRO_Q_SENS_NUM 18271
+#define IMU_GYRO_Q_SENS_DEN 8383
+#define IMU_GYRO_R_SENS 2.17953
+#define IMU_GYRO_R_SENS_NUM 18271
+#define IMU_GYRO_R_SENS_DEN 8383
+#endif
+#if !defined IMU_GYRO_P_NEUTRAL & !defined IMU_GYRO_Q_NEUTRAL & !defined IMU_GYRO_R_NEUTRAL
+#define IMU_GYRO_P_NEUTRAL 0
+#define IMU_GYRO_Q_NEUTRAL 0
+#define IMU_GYRO_R_NEUTRAL 0
+#endif
+
+
+/** default accel sensitivy from the datasheet
+ * MPU with 8g has 4096 LSB/g
+ * sens = 9.81 [m/s^2] / 4096 [LSB/g] * 2^INT32_ACCEL_FRAC = 2.4525
+ */
+#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS
+// FIXME
+#define IMU_ACCEL_X_SENS 2.4525
+#define IMU_ACCEL_X_SENS_NUM 981
+#define IMU_ACCEL_X_SENS_DEN 400
+#define IMU_ACCEL_Y_SENS 2.4525
+#define IMU_ACCEL_Y_SENS_NUM 981
+#define IMU_ACCEL_Y_SENS_DEN 400
+#define IMU_ACCEL_Z_SENS 2.4525
+#define IMU_ACCEL_Z_SENS_NUM 981
+#define IMU_ACCEL_Z_SENS_DEN 400
+#endif
+#if !defined IMU_ACCEL_X_NEUTRAL & !defined IMU_ACCEL_Y_NEUTRAL & !defined IMU_ACCEL_Z_NEUTRAL
+#define IMU_ACCEL_X_NEUTRAL 0
+#define IMU_ACCEL_Y_NEUTRAL 0
+#define IMU_ACCEL_Z_NEUTRAL 0
+#endif
+
+struct ImuApogee {
+ volatile bool_t gyr_valid;
+ volatile bool_t acc_valid;
+ struct Mpu60x0_I2c mpu;
+};
+
+extern struct ImuApogee imu_apogee;
+
+
+/* must be defined in order to be IMU code: declared in imu.h
+extern void imu_impl_init(void);
+extern void imu_periodic(void);
+*/
+
+/* Own Extra Functions */
+extern void imu_apogee_event( void );
+extern void imu_apogee_downlink_raw( void );
+
+
+static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void) __attribute__((unused))) {
+ imu_apogee_event();
+ if (imu_apogee.gyr_valid) {
+ imu_apogee.gyr_valid = FALSE;
+ _gyro_handler();
+ }
+ if (imu_apogee.acc_valid) {
+ imu_apogee.acc_valid = FALSE;
+ _accel_handler();
+ }
+}
+
+#endif // IMU_APOGEE_H
diff --git a/sw/airborne/boards/apogee_0.99.h b/sw/airborne/boards/apogee_0.99.h
new file mode 100644
index 0000000000..3a21e881b5
--- /dev/null
+++ b/sw/airborne/boards/apogee_0.99.h
@@ -0,0 +1,298 @@
+#ifndef CONFIG_APOGEE_0_99_H
+#define CONFIG_APOGEE_0_99_H
+
+#define BOARD_APOGEE
+
+/* Apogee has a 16MHz external clock and 168MHz internal. */
+#define EXT_CLK 16000000
+#define AHB_CLK 168000000
+
+/*
+ * Onboard LEDs
+ */
+
+/* red, on PC0 */
+#ifndef USE_LED_1
+#define USE_LED_1 1
+#endif
+#define LED_1_GPIO GPIOC
+#define LED_1_GPIO_CLK RCC_AHB1ENR_IOPCEN
+#define LED_1_GPIO_PIN GPIO0
+#define LED_1_GPIO_ON gpio_clear
+#define LED_1_GPIO_OFF gpio_set
+#define LED_1_AFIO_REMAP ((void)0)
+
+/* green, on PC13 */
+#ifndef USE_LED_2
+#define USE_LED_2 1
+#endif
+#define LED_2_GPIO GPIOC
+#define LED_2_GPIO_CLK RCC_AHB1ENR_IOPCEN
+#define LED_2_GPIO_PIN GPIO13
+#define LED_2_GPIO_ON gpio_clear
+#define LED_2_GPIO_OFF gpio_set
+#define LED_2_AFIO_REMAP ((void)0)
+
+/* Default actuators driver */
+#define DEFAULT_ACTUATORS "subsystems/actuators/actuators_pwm.h"
+#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y)
+#define ActuatorsDefaultInit() ActuatorsPwmInit()
+#define ActuatorsDefaultCommit() ActuatorsPwmCommit()
+
+#define DefaultVoltageOfAdc(adc) (0.006185*adc)
+
+/* UART */
+#define UART1_GPIO_AF GPIO_AF7
+#define UART1_GPIO_PORT_RX GPIOA
+#define UART1_GPIO_RX GPIO10
+#define UART1_GPIO_PORT_TX GPIOB
+#define UART1_GPIO_TX GPIO6
+
+#define UART4_GPIO_AF GPIO_AF8
+#define UART4_GPIO_PORT_RX GPIOA
+#define UART4_GPIO_RX GPIO1
+#define UART4_GPIO_PORT_TX GPIOA
+#define UART4_GPIO_TX GPIO0
+
+#define UART6_GPIO_AF GPIO_AF8
+#define UART6_GPIO_PORT_RX GPIOC
+#define UART6_GPIO_RX GPIO7
+#define UART6_GPIO_PORT_TX GPIOC
+#define UART6_GPIO_TX GPIO6
+
+
+/* Onboard ADCs */
+#define USE_AD_TIM4 1
+
+#define BOARD_ADC_CHANNEL_1 8
+#define BOARD_ADC_CHANNEL_2 9
+#define BOARD_ADC_CHANNEL_3 14
+#define BOARD_ADC_CHANNEL_4 4
+
+#ifndef USE_AD1
+#define USE_AD1 1
+#endif
+/* provide defines that can be used to access the ADC_x in the code or airframe file
+ * these directly map to the index number of the 4 adc channels defined above
+ * 4th (index 3) is used for bat monitoring by default
+ */
+#define ADC_1 ADC1_C1
+#ifdef USE_ADC_1
+#ifndef ADC_1_GPIO_CLOCK_PORT
+#define ADC_1_GPIO_CLOCK_PORT RCC_AHB1ENR_IOPBEN
+#define ADC_1_INIT() gpio_mode_setup(GPIOB, GPIO_MODE_ANALOG, GPIO_PUPD_NONE, GPIO0)
+#endif
+#define USE_AD1_1 1
+#else
+#define ADC_1_GPIO_CLOCK_PORT 0
+#define ADC_1_INIT() {}
+#endif
+
+#define ADC_2 ADC1_C2
+#ifdef USE_ADC_2
+#ifndef ADC_2_GPIO_CLOCK_PORT
+#define ADC_2_GPIO_CLOCK_PORT RCC_AHB1ENR_IOPBEN
+#define ADC_2_INIT() gpio_mode_setup(GPIOB, GPIO_MODE_ANALOG, GPIO_PUPD_NONE, GPIO1)
+#endif
+#define USE_AD1_2 1
+#else
+#define ADC_2_GPIO_CLOCK_PORT 0
+#define ADC_2_INIT() {}
+#endif
+
+#define ADC_3 ADC1_C3
+#ifdef USE_ADC_3
+#ifndef ADC_3_GPIO_CLOCK_PORT
+#define ADC_3_GPIO_CLOCK_PORT RCC_AHB1ENR_IOPCEN
+#define ADC_3_INIT() gpio_mode_setup(GPIOC, GPIO_MODE_ANALOG, GPIO_PUPD_NONE, GPIO4)
+#endif
+#define USE_AD1_3 1
+#else
+#define ADC_3_GPIO_CLOCK_PORT 0
+#define ADC_3_INIT() {}
+#endif
+
+#define ADC_4 ADC1_C4
+//#ifdef USE_ADC_4
+#ifndef ADC_4_GPIO_CLOCK_PORT
+#define ADC_4_GPIO_CLOCK_PORT RCC_AHB1ENR_IOPAEN
+#define ADC_4_INIT() gpio_mode_setup(GPIOA, GPIO_MODE_ANALOG, GPIO_PUPD_NONE, GPIO4)
+#endif
+#define USE_AD1_4 1
+//#else
+//#define ADC_4_GPIO_CLOCK_PORT 0
+//#define ADC_4_INIT() {}
+//#endif
+
+/* allow to define ADC_CHANNEL_VSUPPLY in the airframe file*/
+#ifndef ADC_CHANNEL_VSUPPLY
+#define ADC_CHANNEL_VSUPPLY ADC_4
+#endif
+
+#define ADC_GPIO_CLOCK_PORT (ADC_1_GPIO_CLOCK_PORT | ADC_2_GPIO_CLOCK_PORT | ADC_3_GPIO_CLOCK_PORT | ADC_4_GPIO_CLOCK_PORT)
+
+/* GPIO mapping for ADC1 pins, overwrites the default in arch/stm32/mcu_periph/adc_arch.c */
+#ifdef USE_AD1
+#define ADC1_GPIO_INIT(gpio) { \
+ ADC_1_INIT(); \
+ ADC_2_INIT(); \
+ ADC_3_INIT(); \
+ ADC_4_INIT(); \
+}
+#endif // USE_AD1
+
+
+/* I2C mapping */
+#define GPIO_I2C1_SCL GPIO8
+#define GPIO_I2C1_SDA GPIO7
+#define GPIO_I2C2_SCL GPIO10
+#define GPIO_I2C2_SDA GPIO11
+
+
+/* SPI slave pin declaration */
+//#define SPI_SELECT_SLAVE0_PERIPH RCC_APB2ENR_IOPAEN
+//#define SPI_SELECT_SLAVE0_PORT GPIOA
+//#define SPI_SELECT_SLAVE0_PIN GPIO15
+//
+//#define SPI_SELECT_SLAVE1_PERIPH RCC_APB2ENR_IOPAEN
+//#define SPI_SELECT_SLAVE1_PORT GPIOA
+//#define SPI_SELECT_SLAVE1_PIN GPIO4
+
+#define SPI_SELECT_SLAVE2_PERIPH RCC_AHB1ENR_IOPBEN
+#define SPI_SELECT_SLAVE2_PORT GPIOB
+#define SPI_SELECT_SLAVE2_PIN GPIO12
+
+//#define SPI_SELECT_SLAVE3_PERIPH RCC_APB2ENR_IOPCEN
+//#define SPI_SELECT_SLAVE3_PORT GPIOC
+//#define SPI_SELECT_SLAVE3_PIN GPIO13
+//
+//#define SPI_SELECT_SLAVE4_PERIPH RCC_APB2ENR_IOPCEN
+//#define SPI_SELECT_SLAVE4_PORT GPIOC
+//#define SPI_SELECT_SLAVE4_PIN GPIO12
+//
+//#define SPI_SELECT_SLAVE5_PERIPH RCC_APB2ENR_IOPCEN
+//#define SPI_SELECT_SLAVE5_PORT GPIOC
+//#define SPI_SELECT_SLAVE5_PIN GPIO4
+
+/* Activate onboard baro */
+#define BOARD_HAS_BARO 1
+
+/* PWM */
+#define PWM_USE_TIM2 1
+#define PWM_USE_TIM3 1
+
+#define USE_PWM0 1
+#define USE_PWM1 1
+#define USE_PWM2 1
+#define USE_PWM3 1
+#define USE_PWM4 1
+#define USE_PWM5 1
+
+// PWM_SERVO_x is the index of the servo in the actuators_pwm_values array
+#if USE_PWM0
+#define PWM_SERVO_0 0
+#define PWM_SERVO_0_TIMER TIM2
+#define PWM_SERVO_0_RCC_IOP RCC_AHB1ENR_IOPAEN
+#define PWM_SERVO_0_GPIO GPIOA
+#define PWM_SERVO_0_PIN GPIO3
+#define PWM_SERVO_0_AF GPIO_AF1
+#define PWM_SERVO_0_OC TIM_OC4
+#define PWM_SERVO_0_OC_BIT (1<<3)
+#else
+#define PWM_SERVO_0_OC_BIT 0
+#endif
+
+#if USE_PWM1
+#define PWM_SERVO_1 1
+#define PWM_SERVO_1_TIMER TIM2
+#define PWM_SERVO_1_RCC_IOP RCC_AHB1ENR_IOPAEN
+#define PWM_SERVO_1_GPIO GPIOA
+#define PWM_SERVO_1_PIN GPIO2
+#define PWM_SERVO_1_AF GPIO_AF1
+#define PWM_SERVO_1_OC TIM_OC3
+#define PWM_SERVO_1_OC_BIT (1<<2)
+#else
+#define PWM_SERVO_1_OC_BIT 0
+#endif
+
+#if USE_PWM2
+#define PWM_SERVO_2 2
+#define PWM_SERVO_2_TIMER TIM3
+#define PWM_SERVO_2_RCC_IOP RCC_AHB1ENR_IOPBEN
+#define PWM_SERVO_2_GPIO GPIOB
+#define PWM_SERVO_2_PIN GPIO5
+#define PWM_SERVO_2_AF GPIO_AF2
+#define PWM_SERVO_2_OC TIM_OC2
+#define PWM_SERVO_2_OC_BIT (1<<1)
+#else
+#define PWM_SERVO_2_OC_BIT 0
+#endif
+
+#if USE_PWM3
+#define PWM_SERVO_3_IDX 3
+#define PWM_SERVO_3_TIMER TIM3
+#define PWM_SERVO_3_RCC_IOP RCC_AHB1ENR_IOPBEN
+#define PWM_SERVO_3_GPIO GPIOB
+#define PWM_SERVO_3_PIN GPIO4
+#define PWM_SERVO_3_AF GPIO_AF2
+#define PWM_SERVO_3_OC TIM_OC1
+#define PWM_SERVO_3_OC_BIT (1<<0)
+#else
+#define PWM_SERVO_3_OC_BIT 0
+#endif
+
+#if USE_PWM4
+#define PWM_SERVO_4 4
+#define PWM_SERVO_4_TIMER TIM2
+#define PWM_SERVO_4_RCC_IOP RCC_AHB1ENR_IOPBEN
+#define PWM_SERVO_4_GPIO GPIOB
+#define PWM_SERVO_4_PIN GPIO3
+#define PWM_SERVO_4_AF GPIO_AF1
+#define PWM_SERVO_4_OC TIM_OC2
+#define PWM_SERVO_4_OC_BIT (1<<1)
+#else
+#define PWM_SERVO_4_OC_BIT 0
+#endif
+
+#if USE_PWM5
+#define PWM_SERVO_5 5
+#define PWM_SERVO_5_TIMER TIM2
+#define PWM_SERVO_5_RCC_IOP RCC_AHB1ENR_IOPAEN
+#define PWM_SERVO_5_GPIO GPIOA
+#define PWM_SERVO_5_PIN GPIO15
+#define PWM_SERVO_5_AF GPIO_AF1
+#define PWM_SERVO_5_OC TIM_OC1
+#define PWM_SERVO_5_OC_BIT (1<<0)
+#else
+#define PWM_SERVO_5_OC_BIT 0
+#endif
+
+// TODO PWM AUX
+
+#define PWM_TIM2_CHAN_MASK (PWM_SERVO_0_OC_BIT|PWM_SERVO_1_OC_BIT|PWM_SERVO_4_OC_BIT|PWM_SERVO_5_OC_BIT)
+#define PWM_TIM3_CHAN_MASK (PWM_SERVO_2_OC_BIT|PWM_SERVO_3_OC_BIT)
+
+/* PPM */
+
+#define USE_PPM_TIM1 1
+
+#define PPM_CHANNEL TIM_IC1
+#define PPM_TIMER_INPUT TIM_IC_IN_TI1
+#define PPM_IRQ NVIC_TIM1_CC_IRQ
+#define PPM_IRQ2 NVIC_TIM1_UP_TIM10_IRQ
+#define PPM_IRQ_FLAGS TIM_DIER_CC1IE
+#define PPM_IRQ_CCIF TIM_SR_CC1IF
+#define PPM_GPIO_PERIPHERAL RCC_AHB1ENR_IOPAEN
+#define PPM_GPIO_PORT GPIOA
+#define PPM_GPIO_PIN GPIO8
+#define PPM_GPIO_AF GPIO_AF1
+
+/*
+ * Spektrum
+ */
+/* The line that is pulled low at power up to initiate the bind process */
+#define SPEKTRUM_BIND_PIN GPIO8
+#define SPEKTRUM_BIND_PIN_PORT GPIOA
+#define SPEKTRUM_BIND_PIN_RCC_IOP RCC_AHB1ENR_IOPAEN
+
+#endif /* CONFIG_APOGEE_0_99_H */
diff --git a/sw/airborne/boards/krooz_sd.h b/sw/airborne/boards/krooz_sd.h
new file mode 100644
index 0000000000..fcc31fcb07
--- /dev/null
+++ b/sw/airborne/boards/krooz_sd.h
@@ -0,0 +1,393 @@
+#ifndef CONFIG_KROOZ_1_0_H
+#define CONFIG_KROOZ_1_0_H
+
+#define BOARD_KROOZ
+
+/* Krooz/M has a 12MHz external clock and 168MHz internal. */
+#define EXT_CLK 12000000
+#define AHB_CLK 168000000
+
+/*
+ * Onboard LEDs
+ */
+
+/* red, on PA8 */
+#ifndef USE_LED_1
+#define USE_LED_1 1
+#endif
+#define LED_1_GPIO GPIOA
+#define LED_1_GPIO_CLK RCC_AHB1ENR_IOPAEN
+#define LED_1_GPIO_PIN GPIO13
+#define LED_1_GPIO_ON gpio_clear
+#define LED_1_GPIO_OFF gpio_set
+#define LED_1_AFIO_REMAP ((void)0)
+
+/* green, shared with JTAG_TRST */
+#ifndef USE_LED_2
+#define USE_LED_2 1
+#endif
+#define LED_2_GPIO GPIOA
+#define LED_2_GPIO_CLK RCC_AHB1ENR_IOPAEN
+#define LED_2_GPIO_PIN GPIO14
+#define LED_2_GPIO_ON gpio_clear
+#define LED_2_GPIO_OFF gpio_set
+#define LED_2_AFIO_REMAP ((void)0)
+
+/* green, shared with ADC12 (ADC_6 on connector ANALOG2) */
+#ifndef USE_LED_3
+#define USE_LED_3 1
+#endif
+#define LED_3_GPIO GPIOA
+#define LED_3_GPIO_CLK RCC_AHB1ENR_IOPAEN
+#define LED_3_GPIO_PIN GPIO15
+#define LED_3_GPIO_ON gpio_clear
+#define LED_3_GPIO_OFF gpio_set
+#define LED_3_AFIO_REMAP ((void)0)
+
+/*
+ * not actual LEDS, used as GPIOs
+ */
+
+/* PB4, Camera power On/Off */
+#define CAM_SW_GPIO GPIOB
+#define CAM_SW_GPIO_CLK RCC_AHB1ENR_IOPBEN
+#define CAM_SW_GPIO_PIN GPIO4
+#define CAM_SW_AFIO_REMAP ((void)0)
+
+/* PC2, Camera shot */
+#define CAM_SH_GPIO GPIOC
+#define CAM_SH_GPIO_CLK RCC_AHB1ENR_IOPCEN
+#define CAM_SH_GPIO_PIN GPIO2
+#define CAM_SH_AFIO_REMAP ((void)0)
+
+/* PC15, Camera video */
+#define CAM_V_GPIO GPIOC
+#define CAM_V_GPIO_CLK RCC_AHB1ENR_IOPCEN
+#define CAM_V_GPIO_PIN GPIO15
+#define CAM_V_AFIO_REMAP ((void)0)
+
+#define BEEPER_GPIO GPIOC
+#define BEEPER_GPIO_CLK RCC_AHB1ENR_IOPCEN
+#define BEEPER_GPIO_PIN GPIO14
+#define BEEPER_AFIO_REMAP ((void)0)
+
+
+/* Default actuators driver */
+#define DEFAULT_ACTUATORS "subsystems/actuators/actuators_pwm.h"
+#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y)
+#define ActuatorsDefaultInit() ActuatorsPwmInit()
+#define ActuatorsDefaultCommit() ActuatorsPwmCommit()
+
+#define DefaultVoltageOfAdc(adc) (0.008874*adc)
+
+/* UART */
+#define UART1_GPIO_AF GPIO_AF7
+#define UART1_GPIO_PORT_RX GPIOA
+#define UART1_GPIO_RX GPIO10
+#define UART1_GPIO_PORT_TX GPIOA
+#define UART1_GPIO_TX GPIO9
+
+#define UART3_GPIO_AF GPIO_AF7
+#define UART3_GPIO_PORT_RX GPIOC
+#define UART3_GPIO_RX GPIO11
+#define UART3_GPIO_PORT_TX GPIOC
+#define UART3_GPIO_TX GPIO10
+
+#define UART5_GPIO_AF GPIO_AF8
+#define UART5_GPIO_PORT_RX GPIOD
+#define UART5_GPIO_RX GPIO2
+#define UART5_GPIO_PORT_TX GPIOC
+#define UART5_GPIO_TX GPIO12
+
+/* Onboard ADCs */
+#define USE_AD_TIM1 1
+
+#define BOARD_ADC_CHANNEL_1 12
+#define BOARD_ADC_CHANNEL_2 10
+#define BOARD_ADC_CHANNEL_3 11
+#define BOARD_ADC_CHANNEL_4 13
+#define BOARD_ADC_CHANNEL_5 14
+#define BOARD_ADC_CHANNEL_6 15
+
+/* provide defines that can be used to access the ADC_x in the code or airframe file
+ * these directly map to the index number of the 4 adc channels defined above
+ * 4th (index 3) is used for bat monitoring by default
+ */
+/* allow to define ADC_CHANNEL_VSUPPLY in the airframe file*/
+#ifndef ADC_CHANNEL_VSUPPLY
+#define ADC_CHANNEL_VSUPPLY ADC_4
+#endif
+
+#define ADC_CHANNEL_CAM1 ADC_1
+
+#ifndef USE_AD1
+#define USE_AD1 1
+#endif
+/* provide defines that can be used to access the ADC_x in the code or airframe file
+ * these directly map to the index number of the 4 adc channels defined above
+ * 4th (index 3) is used for bat monitoring by default
+ */
+#define ADC_1 ADC1_C1
+#ifdef USE_ADC_1
+#ifndef ADC_1_GPIO_CLOCK_PORT
+#define ADC_1_GPIO_CLOCK_PORT RCC_AHB1ENR_IOPCEN
+#define ADC_1_INIT() gpio_mode_setup(GPIOC, GPIO_MODE_ANALOG, GPIO_PUPD_NONE, GPIO2)
+#endif
+#define USE_AD1_1 1
+#else
+#define ADC_1_GPIO_CLOCK_PORT 0
+#define ADC_1_INIT() {}
+#endif
+
+#define ADC_2 ADC1_C2
+#ifdef USE_ADC_2
+#ifndef ADC_2_GPIO_CLOCK_PORT
+#define ADC_2_GPIO_CLOCK_PORT RCC_AHB1ENR_IOPCEN
+#define ADC_2_INIT() gpio_mode_setup(GPIOC, GPIO_MODE_ANALOG, GPIO_PUPD_NONE, GPIO0)
+#endif
+#define USE_AD1_2 1
+#else
+#define ADC_2_GPIO_CLOCK_PORT 0
+#define ADC_2_INIT() {}
+#endif
+
+#define ADC_3 ADC1_C3
+#ifdef USE_ADC_3
+#ifndef ADC_3_GPIO_CLOCK_PORT
+#define ADC_3_GPIO_CLOCK_PORT RCC_AHB1ENR_IOPCEN
+#define ADC_3_INIT() gpio_mode_setup(GPIOC, GPIO_MODE_ANALOG, GPIO_PUPD_NONE, GPIO1)
+#endif
+#define USE_AD1_3 1
+#else
+#define ADC_3_GPIO_CLOCK_PORT 0
+#define ADC_3_INIT() {}
+#endif
+
+#define ADC_4 ADC1_C4
+//#ifdef USE_ADC_4
+#ifndef ADC_4_GPIO_CLOCK_PORT
+#define ADC_4_GPIO_CLOCK_PORT RCC_AHB1ENR_IOPCEN
+#define ADC_4_INIT() gpio_mode_setup(GPIOC, GPIO_MODE_ANALOG, GPIO_PUPD_NONE, GPIO3)
+#endif
+#define USE_AD1_4 1
+//#else
+//#define ADC_4_GPIO_CLOCK_PORT 0
+//#define ADC_4_INIT() {}
+//#endif
+
+#define ADC_GPIO_CLOCK_PORT (ADC_1_GPIO_CLOCK_PORT | ADC_2_GPIO_CLOCK_PORT | ADC_3_GPIO_CLOCK_PORT | ADC_4_GPIO_CLOCK_PORT)
+
+#ifdef USE_AD1
+#define ADC1_GPIO_INIT(gpio) { \
+ ADC_1_INIT(); \
+ ADC_2_INIT(); \
+ ADC_3_INIT(); \
+ ADC_4_INIT(); \
+ }
+#endif // USE_AD1
+
+
+/* I2C mapping */
+#define GPIO_I2C1_SCL GPIO8
+#define GPIO_I2C1_SDA GPIO9
+#define GPIO_I2C2_SCL GPIO10
+#define GPIO_I2C2_SDA GPIO11
+#define GPIO_I2C3_SCL GPIO8 //PA8
+#define GPIO_I2C3_SDA GPIO9 //PC9
+
+/* Activate onboard baro */
+#define BOARD_HAS_BARO 1
+
+/* PWM */
+#define PWM_USE_TIM2 1
+#define PWM_USE_TIM3 1
+#define PWM_USE_TIM4 1
+#define PWM_USE_TIM5 1
+
+#define USE_PWM0 1
+#define USE_PWM1 1
+#define USE_PWM2 1
+#define USE_PWM3 1
+#define USE_PWM4 1
+#define USE_PWM5 1
+#define USE_PWM6 1
+#define USE_PWM7 1
+#define USE_PWM8 1
+#define USE_PWM9 1
+//#define USE_PWM10 1
+
+#define ACTUATORS_PWM_NB 10
+
+// PWM_SERVO_x is the index of the servo in the actuators_pwm_values array
+#if USE_PWM0
+#define PWM_SERVO_0 0
+#define PWM_SERVO_0_TIMER TIM3
+#define PWM_SERVO_0_RCC_IOP RCC_AHB1ENR_IOPCEN
+#define PWM_SERVO_0_GPIO GPIOC
+#define PWM_SERVO_0_PIN GPIO6
+#define PWM_SERVO_0_AF GPIO_AF1
+#define PWM_SERVO_0_OC TIM_OC1
+#define PWM_SERVO_0_OC_BIT (1<<0)
+#else
+#define PWM_SERVO_0_OC_BIT 0
+#endif
+
+#if USE_PWM1
+#define PWM_SERVO_1 1
+#define PWM_SERVO_1_TIMER TIM3
+#define PWM_SERVO_1_RCC_IOP RCC_AHB1ENR_IOPCEN
+#define PWM_SERVO_1_GPIO GPIOC
+#define PWM_SERVO_1_PIN GPIO7
+#define PWM_SERVO_1_AF GPIO_AF1
+#define PWM_SERVO_1_OC TIM_OC2
+#define PWM_SERVO_1_OC_BIT (1<<1)
+#else
+#define PWM_SERVO_1_OC_BIT 0
+#endif
+
+#if USE_PWM2
+#define PWM_SERVO_2 2
+#define PWM_SERVO_2_TIMER TIM3
+#define PWM_SERVO_2_RCC_IOP RCC_AHB1ENR_IOPCEN
+#define PWM_SERVO_2_GPIO GPIOC
+#define PWM_SERVO_2_PIN GPIO8
+#define PWM_SERVO_2_AF GPIO_AF1
+#define PWM_SERVO_2_OC TIM_OC3
+#define PWM_SERVO_2_OC_BIT (1<<2)
+#else
+#define PWM_SERVO_2_OC_BIT 0
+#endif
+
+#if USE_PWM3
+#define PWM_SERVO_3_IDX 3
+#define PWM_SERVO_3_TIMER TIM3
+#define PWM_SERVO_3_RCC_IOP RCC_AHB1ENR_IOPCEN
+#define PWM_SERVO_3_GPIO GPIOC
+#define PWM_SERVO_3_PIN GPIO9
+#define PWM_SERVO_3_AF GPIO_AF1
+#define PWM_SERVO_3_OC TIM_OC4
+#define PWM_SERVO_3_OC_BIT (1<<3)
+#else
+#define PWM_SERVO_3_OC_BIT 0
+#endif
+
+#if USE_PWM4
+#define PWM_SERVO_4 4
+#define PWM_SERVO_4_TIMER TIM4
+#define PWM_SERVO_4_RCC_IOP RCC_AHB1ENR_IOPBEN
+#define PWM_SERVO_4_GPIO GPIOB
+#define PWM_SERVO_4_PIN GPIO6
+#define PWM_SERVO_4_AF GPIO_AF1
+#define PWM_SERVO_4_OC TIM_OC1
+#define PWM_SERVO_4_OC_BIT (1<<0)
+#else
+#define PWM_SERVO_4_OC_BIT 0
+#endif
+
+#if USE_PWM5
+#define PWM_SERVO_5 5
+#define PWM_SERVO_5_TIMER TIM4
+#define PWM_SERVO_5_RCC_IOP RCC_AHB1ENR_IOPBEN
+#define PWM_SERVO_5_GPIO GPIOB
+#define PWM_SERVO_5_PIN GPIO7
+#define PWM_SERVO_5_AF GPIO_AF1
+#define PWM_SERVO_5_OC TIM_OC2
+#define PWM_SERVO_5_OC_BIT (1<<1)
+#else
+#define PWM_SERVO_5_OC_BIT 0
+#endif
+
+#if USE_PWM6
+#define PWM_SERVO_6 6
+#define PWM_SERVO_6_TIMER TIM5
+#define PWM_SERVO_6_RCC_IOP RCC_AHB1ENR_IOPAEN
+#define PWM_SERVO_6_GPIO GPIOA
+#define PWM_SERVO_6_PIN GPIO0
+#define PWM_SERVO_6_AF GPIO_AF1
+#define PWM_SERVO_6_OC TIM_OC1
+#define PWM_SERVO_6_OC_BIT (1<<0)
+#else
+#define PWM_SERVO_6_OC_BIT 0
+#endif
+
+#if USE_PWM7
+#define PWM_SERVO_7 7
+#define PWM_SERVO_7_TIMER TIM5
+#define PWM_SERVO_7_RCC_IOP RCC_AHB1ENR_IOPAEN
+#define PWM_SERVO_7_GPIO GPIOA
+#define PWM_SERVO_7_PIN GPIO1
+#define PWM_SERVO_7_AF GPIO_AF1
+#define PWM_SERVO_7_OC TIM_OC2
+#define PWM_SERVO_7_OC_BIT (1<<1)
+#else
+#define PWM_SERVO_7_OC_BIT 0
+#endif
+
+#if USE_PWM8
+#define PWM_SERVO_8 8
+#define PWM_SERVO_8_TIMER TIM5
+#define PWM_SERVO_8_RCC_IOP RCC_AHB1ENR_IOPAEN
+#define PWM_SERVO_8_GPIO GPIOA
+#define PWM_SERVO_8_PIN GPIO2
+#define PWM_SERVO_8_AF GPIO_AF1
+#define PWM_SERVO_8_OC TIM_OC3
+#define PWM_SERVO_8_OC_BIT (1<<2)
+#else
+#define PWM_SERVO_8_OC_BIT 0
+#endif
+
+#if USE_PWM9
+#define PWM_SERVO_9 9
+#define PWM_SERVO_9_TIMER TIM5
+#define PWM_SERVO_9_RCC_IOP RCC_AHB1ENR_IOPAEN
+#define PWM_SERVO_9_GPIO GPIOA
+#define PWM_SERVO_9_PIN GPIO3
+#define PWM_SERVO_9_AF GPIO_AF1
+#define PWM_SERVO_9_OC TIM_OC4
+#define PWM_SERVO_9_OC_BIT (1<<3)
+#else
+#define PWM_SERVO_9_OC_BIT 0
+#endif
+
+#if USE_PWM10
+#define PWM_SERVO_10 10
+#define PWM_SERVO_10_TIMER TIM2
+#define PWM_SERVO_10_RCC_IOP RCC_AHB1ENR_IOPBEN
+#define PWM_SERVO_10_GPIO GPIOB
+#define PWM_SERVO_10_PIN GPIO3
+#define PWM_SERVO_10_AF GPIO_AF1
+#define PWM_SERVO_10_OC TIM_OC2
+#define PWM_SERVO_10_OC_BIT (1<<1)
+#else
+#define PWM_SERVO_10_OC_BIT 0
+#endif
+
+#define PWM_TIM2_CHAN_MASK (PWM_SERVO_10_OC_BIT)
+#define PWM_TIM3_CHAN_MASK (PWM_SERVO_0_OC_BIT|PWM_SERVO_1_OC_BIT|PWM_SERVO_2_OC_BIT|PWM_SERVO_3_OC_BIT)
+#define PWM_TIM4_CHAN_MASK (PWM_SERVO_4_OC_BIT|PWM_SERVO_5_OC_BIT)
+#define PWM_TIM5_CHAN_MASK (PWM_SERVO_6_OC_BIT|PWM_SERVO_7_OC_BIT|PWM_SERVO_8_OC_BIT|PWM_SERVO_9_OC_BIT)
+
+/* PPM */
+
+#define USE_PPM_TIM2 1
+
+#define PPM_CHANNEL TIM_IC2
+#define PPM_TIMER_INPUT TIM_IC_IN_TI1
+#define PPM_IRQ NVIC_TIM2_IRQ
+//#define PPM_IRQ2 NVIC_TIM2_UP_TIM10_IRQ
+#define PPM_IRQ_FLAGS TIM_DIER_CC2IE
+#define PPM_IRQ_CCIF TIM_SR_CC2IF
+#define PPM_GPIO_PERIPHERAL RCC_AHB1ENR_IOPBEN
+#define PPM_GPIO_PORT GPIOB
+#define PPM_GPIO_PIN GPIO3
+#define PPM_GPIO_AF GPIO_AF1
+
+/*
+ * Spektrum
+ */
+/* The line that is pulled low at power up to initiate the bind process */
+#define SPEKTRUM_BIND_PIN GPIO8
+#define SPEKTRUM_BIND_PIN_PORT GPIOA
+#define SPEKTRUM_BIND_PIN_RCC_IOP RCC_AHB1ENR_IOPAEN
+
+#endif /* CONFIG_KROOZ_1_0_H */
diff --git a/sw/airborne/boards/lia_1.1.h b/sw/airborne/boards/lia_1.1.h
index 2831fac8ec..3b9c1bf419 100644
--- a/sw/airborne/boards/lia_1.1.h
+++ b/sw/airborne/boards/lia_1.1.h
@@ -1,6 +1,8 @@
#ifndef CONFIG_LIA_1_1_H
#define CONFIG_LIA_1_1_H
+#include "boards/lisa_m_common.h"
+
#define BOARD_LIA
/* Lisa/M has a 12MHz external clock and 72MHz internal. */
@@ -18,8 +20,8 @@
#define LED_1_GPIO GPIOA
#define LED_1_GPIO_CLK RCC_APB2ENR_IOPAEN
#define LED_1_GPIO_PIN GPIO8
-#define LED_1_GPIO_ON GPIO_BRR
-#define LED_1_GPIO_OFF GPIO_BSRR
+#define LED_1_GPIO_ON gpio_clear
+#define LED_1_GPIO_OFF gpio_set
#define LED_1_AFIO_REMAP ((void)0)
/* green, shared with JTAG_TRST */
@@ -29,8 +31,8 @@
#define LED_2_GPIO GPIOB
#define LED_2_GPIO_CLK RCC_APB2ENR_IOPBEN | RCC_APB2ENR_AFIOEN
#define LED_2_GPIO_PIN GPIO4
-#define LED_2_GPIO_ON GPIO_BRR
-#define LED_2_GPIO_OFF GPIO_BSRR
+#define LED_2_GPIO_ON gpio_clear
+#define LED_2_GPIO_OFF gpio_set
#define LED_2_AFIO_REMAP AFIO_MAPR |= AFIO_MAPR_SWJ_CFG_FULL_SWJ_NO_JNTRST
/* green, shared with ADC12 (ADC_6 on connector ANALOG2) */
@@ -40,8 +42,8 @@
#define LED_3_GPIO GPIOC
#define LED_3_GPIO_CLK RCC_APB2ENR_IOPCEN
#define LED_3_GPIO_PIN GPIO2
-#define LED_3_GPIO_ON GPIO_BRR
-#define LED_3_GPIO_OFF GPIO_BSRR
+#define LED_3_GPIO_ON gpio_clear
+#define LED_3_GPIO_OFF gpio_set
#define LED_3_AFIO_REMAP ((void)0)
/* red, shared with ADC15 (ADC_4 on connector ANALOG2) */
@@ -51,8 +53,8 @@
#define LED_4_GPIO GPIOC
#define LED_4_GPIO_CLK RCC_APB2ENR_IOPCEN
#define LED_4_GPIO_PIN GPIO5
-#define LED_4_GPIO_ON GPIO_BRR
-#define LED_4_GPIO_OFF GPIO_BSRR
+#define LED_4_GPIO_ON gpio_clear
+#define LED_4_GPIO_OFF gpio_set
#define LED_4_AFIO_REMAP ((void)0)
/* green, on PC15 */
@@ -62,8 +64,8 @@
#define LED_5_GPIO GPIOC
#define LED_5_GPIO_CLK RCC_APB2ENR_IOPCEN
#define LED_5_GPIO_PIN GPIO15
-#define LED_5_GPIO_ON GPIO_BRR
-#define LED_5_GPIO_OFF GPIO_BSRR
+#define LED_5_GPIO_ON gpio_clear
+#define LED_5_GPIO_OFF gpio_set
#define LED_5_AFIO_REMAP ((void)0)
/*
@@ -73,24 +75,24 @@
#define LED_6_GPIO GPIOC
#define LED_6_GPIO_CLK RCC_APB2ENR_IOPCEN
#define LED_6_GPIO_PIN GPIO3
-#define LED_6_GPIO_ON GPIO_BRR
-#define LED_6_GPIO_OFF GPIO_BSRR
+#define LED_6_GPIO_ON gpio_clear
+#define LED_6_GPIO_OFF gpio_set
#define LED_6_AFIO_REMAP ((void)0)
/* PC0, ADC10 on ADC_2 */
#define LED_7_GPIO GPIOC
#define LED_7_GPIO_CLK RCC_APB2ENR_IOPCEN
#define LED_7_GPIO_PIN GPIO0
-#define LED_7_GPIO_ON GPIO_BRR
-#define LED_7_GPIO_OFF GPIO_BSRR
+#define LED_7_GPIO_ON gpio_clear
+#define LED_7_GPIO_OFF gpio_set
#define LED_7_AFIO_REMAP ((void)0)
/* PC1, ADC11 on ADC_3 */
#define LED_8_GPIO GPIOC
#define LED_8_GPIO_CLK RCC_APB2ENR_IOPCEN
#define LED_8_GPIO_PIN GPIO1
-#define LED_8_GPIO_ON GPIO_BRR
-#define LED_8_GPIO_OFF GPIO_BSRR
+#define LED_8_GPIO_ON gpio_clear
+#define LED_8_GPIO_OFF gpio_set
#define LED_8_AFIO_REMAP ((void)0)
@@ -102,16 +104,16 @@
#define LED_BODY_GPIO GPIOB
#define LED_BODY_GPIO_CLK RCC_APB2ENR_IOPBEN
#define LED_BODY_GPIO_PIN GPIO1
-#define LED_BODY_GPIO_ON GPIO_BSRR
-#define LED_BODY_GPIO_OFF GPIO_BRR
+#define LED_BODY_GPIO_ON gpio_set
+#define LED_BODY_GPIO_OFF gpio_clear
#define LED_BODY_AFIO_REMAP ((void)0)
/* PC12, on GPIO connector*/
#define LED_12_GPIO GPIOC
#define LED_12_GPIO_CLK RCC_APB2ENR_IOPCEN
#define LED_12_GPIO_PIN GPIO12
-#define LED_1_GPIO_ON GPIO_BRR
-#define LED_1_GPIO_OFF GPIO_BSRR
+#define LED_12_GPIO_ON gpio_clear
+#define LED_12_GPIO_OFF gpio_set
#define LED_12_AFIO_REMAP ((void)0)
@@ -166,17 +168,5 @@
// FIXME, using baro_board right now to include the appropriate header
#define BOARD_HAS_BARO 0
-#define PWM_5AND6_TIMER TIM5
-#define PWM_5AND6_RCC RCC_APB1ENR_TIM5EN
-#define PWM5_OC 1
-#define PWM6_OC 2
-#define PWM_5AND6_GPIO GPIOA
-#define PWM5_Pin GPIO0
-#define PWM6_Pin GPIO1
-
-// Remap the servos 5 and 6 to TIM5 CH1 and CH2
-#if !defined REMAP_SERVOS_5AND6
-#define REMAP_SERVOS_5AND6 1
-#endif
#endif /* CONFIG_LIA_1_1_H */
diff --git a/sw/airborne/boards/lisa_l_1.0.h b/sw/airborne/boards/lisa_l_1.0.h
index af45db6abb..2b04367bbb 100644
--- a/sw/airborne/boards/lisa_l_1.0.h
+++ b/sw/airborne/boards/lisa_l_1.0.h
@@ -16,13 +16,90 @@
// FIXME, this is just to make it compile
#define POWER_SWITCH_LED 5
+/* SPI slave mapping */
-/* Default actuators driver */
-#define DEFAULT_ACTUATORS "subsystems/actuators/actuators_pwm.h"
-#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y)
-#define ActuatorsDefaultInit() ActuatorsPwmInit()
-#define ActuatorsDefaultCommit() ActuatorsPwmCommit()
+#define SPI_SELECT_SLAVE0_PERIPH RCC_APB2ENR_IOPAEN
+#define SPI_SELECT_SLAVE0_PORT GPIOA
+#define SPI_SELECT_SLAVE0_PIN GPIO15
+#define SPI_SELECT_SLAVE1_PERIPH RCC_APB2ENR_IOPAEN
+#define SPI_SELECT_SLAVE1_PORT GPIOA
+#define SPI_SELECT_SLAVE1_PIN GPIO4
+
+#define SPI_SELECT_SLAVE2_PERIPH RCC_APB2ENR_IOPBEN
+#define SPI_SELECT_SLAVE2_PORT GPIOB
+#define SPI_SELECT_SLAVE2_PIN GPIO12
+
+#define SPI_SELECT_SLAVE3_PERIPH RCC_APB2ENR_IOPCEN
+#define SPI_SELECT_SLAVE3_PORT GPIOC
+#define SPI_SELECT_SLAVE3_PIN GPIO13
+
+#define SPI_SELECT_SLAVE4_PERIPH RCC_APB2ENR_IOPCEN
+#define SPI_SELECT_SLAVE4_PORT GPIOC
+#define SPI_SELECT_SLAVE4_PIN GPIO12
+
+
+/*
+ * Spektrum
+ */
+/* The line that is pulled low at power up to initiate the bind process */
+#define SPEKTRUM_BIND_PIN GPIO3
+#define SPEKTRUM_BIND_PIN_PORT GPIOC
+#define SPEKTRUM_BIND_PIN_RCC_IOP RCC_APB2ENR_IOPCEN
+
+#define SPEKTRUM_UART1_RCC_GPIO RCC_APB2ENR_IOPAEN
+#define SPEKTRUM_UART1_RCC_REG &RCC_APB2ENR
+#define SPEKTRUM_UART1_RCC_DEV RCC_APB2ENR_USART1EN
+#define SPEKTRUM_UART1_BANK GPIO_BANK_USART1_RX
+#define SPEKTRUM_UART1_PIN GPIO_USART1_RX
+#define SPEKTRUM_UART1_IRQ NVIC_USART1_IRQ
+#define SPEKTRUM_UART1_ISR usart1_isr
+#define SPEKTRUM_UART1_DEV USART1
+#define SPEKTRUM_UART1_REMAP {}
+
+#define SPEKTRUM_UART3_RCC_GPIO RCC_APB2ENR_IOPCEN
+#define SPEKTRUM_UART3_RCC_REG &RCC_APB1ENR
+#define SPEKTRUM_UART3_RCC_DEV RCC_APB1ENR_USART3EN
+#define SPEKTRUM_UART3_BANK GPIO_BANK_USART3_PR_RX
+#define SPEKTRUM_UART3_PIN GPIO_USART3_PR_RX
+#define SPEKTRUM_UART3_IRQ NVIC_USART3_IRQ
+#define SPEKTRUM_UART3_ISR usart3_isr
+#define SPEKTRUM_UART3_DEV USART3
+#define SPEKTRUM_UART3_REMAP {AFIO_MAPR |= AFIO_MAPR_USART3_REMAP_PARTIAL_REMAP;}
+
+#define SPEKTRUM_UART5_RCC_GPIO RCC_APB2ENR_IOPDEN
+#define SPEKTRUM_UART5_RCC_REG &RCC_APB1ENR
+#define SPEKTRUM_UART5_RCC_DEV RCC_APB1ENR_UART5EN
+#define SPEKTRUM_UART5_BANK GPIO_BANK_UART5_RX
+#define SPEKTRUM_UART5_PIN GPIO_UART5_RX
+#define SPEKTRUM_UART5_IRQ NVIC_UART5_IRQ
+#define SPEKTRUM_UART5_ISR uart5_isr
+#define SPEKTRUM_UART5_DEV UART5
+#define SPEKTRUM_UART5_REMAP {}
+
+
+/*
+ * PPM input
+ */
+#define USE_PPM_TIM2 1
+#define PPM_CHANNEL TIM_IC2
+#define PPM_TIMER_INPUT TIM_IC_IN_TI2
+#define PPM_IRQ NVIC_TIM2_IRQ
+#define PPM_IRQ_FLAGS (TIM_DIER_CC2IE | TIM_DIER_UIE)
+#define PPM_GPIO_PERIPHERAL RCC_APB2ENR_IOPAEN
+#define PPM_GPIO_PORT GPIOA
+#define PPM_GPIO_PIN GPIO1
+
+
+/* ADC */
+// active ADC
+#define USE_AD1 1
+#define USE_AD1_1 1
+#define USE_AD1_2 1
+#define USE_AD1_3 1
+#define USE_AD1_4 1
+
+#define USE_AD_TIM1 1
/* PA0 - ADC0 */
/* allow to define ADC_CHANNEL_VSUPPLY in the airframe file*/
@@ -40,4 +117,111 @@
#define BOARD_HAS_BARO 1
+
+/* Default actuators driver */
+#define DEFAULT_ACTUATORS "subsystems/actuators/actuators_pwm.h"
+#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y)
+#define ActuatorsDefaultInit() ActuatorsPwmInit()
+#define ActuatorsDefaultCommit() ActuatorsPwmCommit()
+
+/*
+ * PWM
+ *
+ */
+#define PWM_USE_TIM3 1
+#define PWM_USE_TIM4 1
+
+#define USE_PWM1 1
+#define USE_PWM2 1
+#define USE_PWM3 1
+#define USE_PWM4 1
+#define USE_PWM5 1
+#define USE_PWM6 1
+
+#define ACTUATORS_PWM_NB 6
+
+// PWM_SERVO_x is the index of the servo in the actuators_pwm_values array
+#if USE_PWM1
+#define PWM_SERVO_1 0
+#define PWM_SERVO_1_TIMER TIM3
+#define PWM_SERVO_1_RCC_IOP RCC_APB2ENR_IOPCEN
+#define PWM_SERVO_1_GPIO GPIOC
+#define PWM_SERVO_1_PIN GPIO6
+#define PWM_SERVO_1_AF 0
+#define PWM_SERVO_1_OC TIM_OC1
+#define PWM_SERVO_1_OC_BIT (1<<0)
+#else
+#define PWM_SERVO_1_OC_BIT 0
+#endif
+
+#if USE_PWM2
+#define PWM_SERVO_2 1
+#define PWM_SERVO_2_TIMER TIM3
+#define PWM_SERVO_2_RCC_IOP RCC_APB2ENR_IOPCEN
+#define PWM_SERVO_2_GPIO GPIOC
+#define PWM_SERVO_2_PIN GPIO7
+#define PWM_SERVO_2_AF 0
+#define PWM_SERVO_2_OC TIM_OC2
+#define PWM_SERVO_2_OC_BIT (1<<1)
+#else
+#define PWM_SERVO_2_OC_BIT 0
+#endif
+
+#if USE_PWM3
+#define PWM_SERVO_3 2
+#define PWM_SERVO_3_TIMER TIM3
+#define PWM_SERVO_3_RCC_IOP RCC_APB2ENR_IOPCEN
+#define PWM_SERVO_3_GPIO GPIOC
+#define PWM_SERVO_3_PIN GPIO8
+#define PWM_SERVO_3_AF 0
+#define PWM_SERVO_3_OC TIM_OC3
+#define PWM_SERVO_3_OC_BIT (1<<2)
+#else
+#define PWM_SERVO_3_OC_BIT 0
+#endif
+
+#if USE_PWM4
+#define PWM_SERVO_4 3
+#define PWM_SERVO_4_TIMER TIM3
+#define PWM_SERVO_4_RCC_IOP RCC_APB2ENR_IOPCEN
+#define PWM_SERVO_4_GPIO GPIOC
+#define PWM_SERVO_4_PIN GPIO9
+#define PWM_SERVO_4_AF 0
+#define PWM_SERVO_4_OC TIM_OC4
+#define PWM_SERVO_4_OC_BIT (1<<3)
+#else
+#define PWM_SERVO_4_OC_BIT 0
+#endif
+
+#if USE_PWM5
+#define PWM_SERVO_5 4
+#define PWM_SERVO_5_TIMER TIM4
+#define PWM_SERVO_5_RCC_IOP RCC_APB2ENR_IOPBEN
+#define PWM_SERVO_5_GPIO GPIOB
+#define PWM_SERVO_5_PIN GPIO8
+#define PWM_SERVO_5_AF 0
+#define PWM_SERVO_5_OC TIM_OC3
+#define PWM_SERVO_5_OC_BIT (1<<2)
+#else
+#define PWM_SERVO_5_OC_BIT 0
+#endif
+
+#if USE_PWM6
+#define PWM_SERVO_6 5
+#define PWM_SERVO_6_TIMER TIM4
+#define PWM_SERVO_6_RCC_IOP RCC_APB2ENR_IOPBEN
+#define PWM_SERVO_6_GPIO GPIOB
+#define PWM_SERVO_6_PIN GPIO9
+#define PWM_SERVO_6_AF 0
+#define PWM_SERVO_6_OC TIM_OC4
+#define PWM_SERVO_6_OC_BIT (1<<4)
+#else
+#define PWM_SERVO_6_OC_BIT 0
+#endif
+
+/* servos 1-4 on TIM3 */
+#define PWM_TIM3_CHAN_MASK (PWM_SERVO_1_OC_BIT|PWM_SERVO_2_OC_BIT|PWM_SERVO_3_OC_BIT|PWM_SERVO_4_OC_BIT)
+/* servos 5-6 on TIM4 */
+#define PWM_TIM4_CHAN_MASK (PWM_SERVO_5_OC_BIT|PWM_SERVO_6_OC_BIT)
+
#endif /* CONFIG_LISA_L_1_0_H */
diff --git a/sw/airborne/boards/lisa_m_1.0.h b/sw/airborne/boards/lisa_m_1.0.h
index 1025ff1d2c..a2b1b2e88d 100644
--- a/sw/airborne/boards/lisa_m_1.0.h
+++ b/sw/airborne/boards/lisa_m_1.0.h
@@ -1,6 +1,8 @@
#ifndef CONFIG_LISA_M_1_0_H
#define CONFIG_LISA_M_1_0_H
+#include "boards/lisa_m_common.h"
+
#define BOARD_LISA_M
/* Lisa/M has a 12MHz external clock and 72MHz internal. */
@@ -15,8 +17,8 @@
#define LED_1_GPIO GPIOB
#define LED_1_GPIO_CLK RCC_APB2ENR_IOPBEN | RCC_APB2ENR_AFIOEN
#define LED_1_GPIO_PIN GPIO4
-#define LED_1_GPIO_ON GPIO_BRR
-#define LED_1_GPIO_OFF GPIO_BSRR
+#define LED_1_GPIO_ON gpio_clear
+#define LED_1_GPIO_OFF gpio_set
#define LED_1_AFIO_REMAP AFIO_MAPR |= AFIO_MAPR_SWJ_CFG_FULL_SWJ_NO_JNTRST
/* blue */
@@ -26,8 +28,8 @@
#define LED_2_GPIO GPIOC
#define LED_2_GPIO_CLK RCC_APB2ENR_IOPCEN
#define LED_2_GPIO_PIN GPIO5
-#define LED_2_GPIO_ON GPIO_BRR
-#define LED_2_GPIO_OFF GPIO_BSRR
+#define LED_2_GPIO_ON gpio_clear
+#define LED_2_GPIO_OFF gpio_set
#define LED_2_AFIO_REMAP ((void)0)
/* blue */
@@ -37,8 +39,8 @@
#define LED_3_GPIO GPIOC
#define LED_3_GPIO_CLK RCC_APB2ENR_IOPCEN
#define LED_3_GPIO_PIN GPIO2
-#define LED_3_GPIO_ON GPIO_BRR
-#define LED_3_GPIO_OFF GPIO_BSRR
+#define LED_3_GPIO_ON gpio_clear
+#define LED_3_GPIO_OFF gpio_set
#define LED_3_AFIO_REMAP ((void)0)
// GPIO pins
@@ -48,8 +50,8 @@
#define LED_4_GPIO GPIOC
#define LED_4_GPIO_CLK RCC_APB2ENR_IOPCEN
#define LED_4_GPIO_PIN GPIO12
-#define LED_4_GPIO_ON GPIO_BRR
-#define LED_4_GPIO_OFF GPIO_BSRR
+#define LED_4_GPIO_ON gpio_clear
+#define LED_4_GPIO_OFF gpio_set
#define LED_4_AFIO_REMAP ((void)0)
#ifndef USE_LED_5
@@ -58,16 +60,16 @@
#define LED_5_GPIO GPIOC
#define LED_5_GPIO_CLK RCC_APB2ENR_IOPCEN
#define LED_5_GPIO_PIN GPIO10
-#define LED_5_GPIO_ON GPIO_BRR
-#define LED_5_GPIO_OFF GPIO_BSRR
+#define LED_5_GPIO_ON gpio_clear
+#define LED_5_GPIO_OFF gpio_set
#define LED_5_AFIO_REMAP ((void)0)
/* PB1, DRDY on EXT SPI connector*/
#define LED_BODY_GPIO GPIOB
#define LED_BODY_GPIO_CLK RCC_APB2ENR_IOPBEN
#define LED_BODY_GPIO_PIN GPIO1
-#define LED_BODY_GPIO_ON GPIO_BSRR
-#define LED_BODY_GPIO_OFF GPIO_BRR
+#define LED_BODY_GPIO_ON gpio_set
+#define LED_BODY_GPIO_OFF gpio_clear
#define LED_BODY_AFIO_REMAP ((void)0)
@@ -120,9 +122,4 @@
#define BOARD_HAS_BARO 1
-// Remap the servos 5 and 6 to TIM5 CH1 and CH2
-#if !defined REMAP_SERVOS_5AND6
-#define REMAP_SERVOS_5AND6 1
-#endif
-
#endif /* CONFIG_LISA_M_1_0_H */
diff --git a/sw/airborne/boards/lisa_m_2.0.h b/sw/airborne/boards/lisa_m_2.0.h
index b0d740ec73..3f09b51a8d 100644
--- a/sw/airborne/boards/lisa_m_2.0.h
+++ b/sw/airborne/boards/lisa_m_2.0.h
@@ -1,6 +1,8 @@
#ifndef CONFIG_LISA_M_2_0_H
#define CONFIG_LISA_M_2_0_H
+#include "boards/lisa_m_common.h"
+
#define BOARD_LISA_M
/* Lisa/M has a 12MHz external clock and 72MHz internal. */
@@ -18,8 +20,8 @@
#define LED_1_GPIO GPIOA
#define LED_1_GPIO_CLK RCC_APB2ENR_IOPAEN
#define LED_1_GPIO_PIN GPIO8
-#define LED_1_GPIO_ON GPIO_BRR
-#define LED_1_GPIO_OFF GPIO_BSRR
+#define LED_1_GPIO_ON gpio_clear
+#define LED_1_GPIO_OFF gpio_set
#define LED_1_AFIO_REMAP ((void)0)
/* green, shared with JTAG_TRST */
@@ -29,8 +31,8 @@
#define LED_2_GPIO GPIOB
#define LED_2_GPIO_CLK RCC_APB2ENR_IOPBEN | RCC_APB2ENR_AFIOEN
#define LED_2_GPIO_PIN GPIO4
-#define LED_2_GPIO_ON GPIO_BRR
-#define LED_2_GPIO_OFF GPIO_BSRR
+#define LED_2_GPIO_ON gpio_clear
+#define LED_2_GPIO_OFF gpio_set
#define LED_2_AFIO_REMAP AFIO_MAPR |= AFIO_MAPR_SWJ_CFG_FULL_SWJ_NO_JNTRST
/* green, shared with ADC12 (ADC_6 on connector ANALOG2) */
@@ -40,8 +42,8 @@
#define LED_3_GPIO GPIOC
#define LED_3_GPIO_CLK RCC_APB2ENR_IOPCEN
#define LED_3_GPIO_PIN GPIO2
-#define LED_3_GPIO_ON GPIO_BRR
-#define LED_3_GPIO_OFF GPIO_BSRR
+#define LED_3_GPIO_ON gpio_clear
+#define LED_3_GPIO_OFF gpio_set
#define LED_3_AFIO_REMAP ((void)0)
/* red, shared with ADC15 (ADC_4 on connector ANALOG2) */
@@ -51,8 +53,8 @@
#define LED_4_GPIO GPIOC
#define LED_4_GPIO_CLK RCC_APB2ENR_IOPCEN
#define LED_4_GPIO_PIN GPIO5
-#define LED_4_GPIO_ON GPIO_BRR
-#define LED_4_GPIO_OFF GPIO_BSRR
+#define LED_4_GPIO_ON gpio_clear
+#define LED_4_GPIO_OFF gpio_set
#define LED_4_AFIO_REMAP ((void)0)
/* green, on PC15 */
@@ -62,8 +64,8 @@
#define LED_5_GPIO GPIOC
#define LED_5_GPIO_CLK RCC_APB2ENR_IOPCEN
#define LED_5_GPIO_PIN GPIO15
-#define LED_5_GPIO_ON GPIO_BRR
-#define LED_5_GPIO_OFF GPIO_BSRR
+#define LED_5_GPIO_ON gpio_clear
+#define LED_5_GPIO_OFF gpio_set
#define LED_5_AFIO_REMAP ((void)0)
/*
@@ -73,24 +75,24 @@
#define LED_6_GPIO GPIOC
#define LED_6_GPIO_CLK RCC_APB2ENR_IOPCEN
#define LED_6_GPIO_PIN GPIO3
-#define LED_6_GPIO_ON GPIO_BRR
-#define LED_6_GPIO_OFF GPIO_BSRR
+#define LED_6_GPIO_ON gpio_clear
+#define LED_6_GPIO_OFF gpio_set
#define LED_6_AFIO_REMAP ((void)0)
/* PC0, ADC10 on ADC_2 */
#define LED_7_GPIO GPIOC
#define LED_7_GPIO_CLK RCC_APB2ENR_IOPCEN
#define LED_7_GPIO_PIN GPIO0
-#define LED_7_GPIO_ON GPIO_BRR
-#define LED_7_GPIO_OFF GPIO_BSRR
+#define LED_7_GPIO_ON gpio_clear
+#define LED_7_GPIO_OFF gpio_set
#define LED_7_AFIO_REMAP ((void)0)
/* PC1, ADC11 on ADC_3 */
#define LED_8_GPIO GPIOC
#define LED_8_GPIO_CLK RCC_APB2ENR_IOPCEN
#define LED_8_GPIO_PIN GPIO1
-#define LED_8_GPIO_ON GPIO_BRR
-#define LED_8_GPIO_OFF GPIO_BSRR
+#define LED_8_GPIO_ON gpio_clear
+#define LED_8_GPIO_OFF gpio_set
#define LED_8_AFIO_REMAP ((void)0)
@@ -102,16 +104,16 @@
#define LED_BODY_GPIO GPIOB
#define LED_BODY_GPIO_CLK RCC_APB2ENR_IOPBEN
#define LED_BODY_GPIO_PIN GPIO1
-#define LED_BODY_GPIO_ON GPIO_BSRR
-#define LED_BODY_GPIO_OFF GPIO_BRR
+#define LED_BODY_GPIO_ON gpio_set
+#define LED_BODY_GPIO_OFF gpio_clear
#define LED_BODY_AFIO_REMAP ((void)0)
/* PC12, on GPIO connector*/
#define LED_12_GPIO GPIOC
#define LED_12_GPIO_CLK RCC_APB2ENR_IOPCEN
#define LED_12_GPIO_PIN GPIO12
-#define LED_12_GPIO_ON GPIO_BRR
-#define LED_12_GPIO_OFF GPIO_BSRR
+#define LED_12_GPIO_ON gpio_clear
+#define LED_12_GPIO_OFF gpio_set
#define LED_12_AFIO_REMAP ((void)0)
@@ -153,7 +155,7 @@
#endif
/* GPIO mapping for ADC1 pins, overwrites the default in arch/stm32/mcu_periph/adc_arch.c */
-// FIXME, this is not very nice, is also stm lib specific
+// FIXME, this is not very nice, is also locm3 lib specific
#ifdef USE_AD1
#define ADC1_GPIO_INIT(gpio) { \
gpio_set_mode(GPIOC, GPIO_MODE_INPUT, \
@@ -163,20 +165,6 @@
#endif // USE_AD1
-
#define BOARD_HAS_BARO 1
-#define PWM_5AND6_TIMER TIM5
-#define PWM_5AND6_RCC RCC_APB1ENR_TIM5EN
-#define PWM5_OC 1
-#define PWM6_OC 2
-#define PWM_5AND6_GPIO GPIOA
-#define PWM5_Pin GPIO0
-#define PWM6_Pin GPIO1
-
-// Remap the servos 5 and 6 to TIM5 CH1 and CH2
-#if !defined REMAP_SERVOS_5AND6
-#define REMAP_SERVOS_5AND6 1
-#endif
-
#endif /* CONFIG_LISA_M_2_0_H */
diff --git a/sw/airborne/boards/lisa_m_common.h b/sw/airborne/boards/lisa_m_common.h
new file mode 100644
index 0000000000..d1dc6525d9
--- /dev/null
+++ b/sw/airborne/boards/lisa_m_common.h
@@ -0,0 +1,266 @@
+#ifndef CONFIG_LISA_M_COMMON_H
+#define CONFIG_LISA_M_COMMON_H
+
+/* SPI slave mapping */
+
+#define SPI_SELECT_SLAVE0_PERIPH RCC_APB2ENR_IOPAEN
+#define SPI_SELECT_SLAVE0_PORT GPIOA
+#define SPI_SELECT_SLAVE0_PIN GPIO15
+
+#define SPI_SELECT_SLAVE1_PERIPH RCC_APB2ENR_IOPAEN
+#define SPI_SELECT_SLAVE1_PORT GPIOA
+#define SPI_SELECT_SLAVE1_PIN GPIO4
+
+#define SPI_SELECT_SLAVE2_PERIPH RCC_APB2ENR_IOPBEN
+#define SPI_SELECT_SLAVE2_PORT GPIOB
+#define SPI_SELECT_SLAVE2_PIN GPIO12
+
+#define SPI_SELECT_SLAVE3_PERIPH RCC_APB2ENR_IOPCEN
+#define SPI_SELECT_SLAVE3_PORT GPIOC
+#define SPI_SELECT_SLAVE3_PIN GPIO13
+
+#define SPI_SELECT_SLAVE4_PERIPH RCC_APB2ENR_IOPCEN
+#define SPI_SELECT_SLAVE4_PORT GPIOC
+#define SPI_SELECT_SLAVE4_PIN GPIO12
+
+#define SPI_SELECT_SLAVE5_PERIPH RCC_APB2ENR_IOPCEN
+#define SPI_SELECT_SLAVE5_PORT GPIOC
+#define SPI_SELECT_SLAVE5_PIN GPIO4
+
+
+/*
+ * Spektrum
+ */
+/* The line that is pulled low at power up to initiate the bind process */
+#define SPEKTRUM_BIND_PIN GPIO3
+#define SPEKTRUM_BIND_PIN_PORT GPIOC
+#define SPEKTRUM_BIND_PIN_RCC_IOP RCC_APB2ENR_IOPCEN
+
+#define SPEKTRUM_UART1_RCC_GPIO RCC_APB2ENR_IOPAEN
+#define SPEKTRUM_UART1_RCC_REG &RCC_APB2ENR
+#define SPEKTRUM_UART1_RCC_DEV RCC_APB2ENR_USART1EN
+#define SPEKTRUM_UART1_BANK GPIO_BANK_USART1_RX
+#define SPEKTRUM_UART1_PIN GPIO_USART1_RX
+#define SPEKTRUM_UART1_IRQ NVIC_USART1_IRQ
+#define SPEKTRUM_UART1_ISR usart1_isr
+#define SPEKTRUM_UART1_DEV USART1
+#define SPEKTRUM_UART1_REMAP {}
+
+#define SPEKTRUM_UART3_RCC_GPIO RCC_APB2ENR_IOPCEN
+#define SPEKTRUM_UART3_RCC_REG &RCC_APB1ENR
+#define SPEKTRUM_UART3_RCC_DEV RCC_APB1ENR_USART3EN
+#define SPEKTRUM_UART3_BANK GPIO_BANK_USART3_PR_RX
+#define SPEKTRUM_UART3_PIN GPIO_USART3_PR_RX
+#define SPEKTRUM_UART3_IRQ NVIC_USART3_IRQ
+#define SPEKTRUM_UART3_ISR usart3_isr
+#define SPEKTRUM_UART3_DEV USART3
+#define SPEKTRUM_UART3_REMAP {AFIO_MAPR |= AFIO_MAPR_USART3_REMAP_PARTIAL_REMAP;}
+
+#define SPEKTRUM_UART5_RCC_GPIO RCC_APB2ENR_IOPDEN
+#define SPEKTRUM_UART5_RCC_REG &RCC_APB1ENR
+#define SPEKTRUM_UART5_RCC_DEV RCC_APB1ENR_UART5EN
+#define SPEKTRUM_UART5_BANK GPIO_BANK_UART5_RX
+#define SPEKTRUM_UART5_PIN GPIO_UART5_RX
+#define SPEKTRUM_UART5_IRQ NVIC_UART5_IRQ
+#define SPEKTRUM_UART5_ISR uart5_isr
+#define SPEKTRUM_UART5_DEV UART5
+#define SPEKTRUM_UART5_REMAP {}
+
+
+/* PPM
+ *
+ * Default is PPM config 2, input on GPIO01 (Servo pin 6)
+ */
+
+#ifndef PPM_CONFIG
+#define PPM_CONFIG 2
+#endif
+
+#if PPM_CONFIG == 1
+/* input on PA01 (UART1_RX) */
+#define USE_PPM_TIM1 1
+#define PPM_CHANNEL TIM_IC3
+#define PPM_TIMER_INPUT TIM_IC_IN_TI3
+#define PPM_IRQ NVIC_TIM1_UP_IRQ
+#define PPM_IRQ2 NVIC_TIM1_CC_IRQ
+#define PPM_IRQ_FLAGS (TIM_DIER_CC3IE | TIM_DIER_UIE)
+#define PPM_GPIO_PERIPHERAL RCC_APB2ENR_IOPAEN
+#define PPM_GPIO_PORT GPIOA
+#define PPM_GPIO_PIN GPIO10
+
+#elif PPM_CONFIG == 2
+/* input on PA10 (Servo 6 pin) */
+#define USE_PPM_TIM2 1
+#define PPM_CHANNEL TIM_IC2
+#define PPM_TIMER_INPUT TIM_IC_IN_TI2
+#define PPM_IRQ NVIC_TIM2_IRQ
+#define PPM_IRQ_FLAGS (TIM_DIER_CC2IE | TIM_DIER_UIE)
+#define PPM_GPIO_PERIPHERAL RCC_APB2ENR_IOPAEN
+#define PPM_GPIO_PORT GPIOA
+#define PPM_GPIO_PIN GPIO1
+
+// Move default ADC timer
+#if USE_AD_TIM2
+#undef USE_AD_TIM2
+#endif
+#define USE_AD_TIM1 1
+
+#else
+#error "Unknown PPM config"
+
+#endif // PPM_CONFIG
+
+/* ADC */
+
+// active ADC
+#define USE_AD1 1
+#define USE_AD1_1 1
+#define USE_AD1_2 1
+#define USE_AD1_3 1
+#define USE_AD1_4 1
+
+
+/*
+ * PWM
+ *
+ */
+#define PWM_USE_TIM3 1
+#define PWM_USE_TIM5 1
+
+#define USE_PWM1 1
+#define USE_PWM2 1
+#define USE_PWM3 1
+#define USE_PWM4 1
+#define USE_PWM5 1
+#define USE_PWM6 1
+
+#if USE_SERVOS_7AND8
+ #if USE_I2C1
+ #error "You cannot USE_SERVOS_7AND8 and USE_I2C1 at the same time"
+ #else
+ #define ACTUATORS_PWM_NB 8
+ #define USE_PWM7 1
+ #define USE_PWM8 1
+ #define PWM_USE_TIM4 1
+ #endif
+#else
+ #define ACTUATORS_PWM_NB 6
+#endif
+
+// Servo numbering on LisaM silkscreen/docs starts with 1
+
+// PWM_SERVO_x is the index of the servo in the actuators_pwm_values array
+#if USE_PWM1
+#define PWM_SERVO_1 0
+#define PWM_SERVO_1_TIMER TIM3
+#define PWM_SERVO_1_RCC_IOP RCC_APB2ENR_IOPCEN
+#define PWM_SERVO_1_GPIO GPIOC
+#define PWM_SERVO_1_PIN GPIO6
+#define PWM_SERVO_1_AF 0
+#define PWM_SERVO_1_OC TIM_OC1
+#define PWM_SERVO_1_OC_BIT (1<<0)
+#else
+#define PWM_SERVO_1_OC_BIT 0
+#endif
+
+#if USE_PWM2
+#define PWM_SERVO_2 1
+#define PWM_SERVO_2_TIMER TIM3
+#define PWM_SERVO_2_RCC_IOP RCC_APB2ENR_IOPCEN
+#define PWM_SERVO_2_GPIO GPIOC
+#define PWM_SERVO_2_PIN GPIO7
+#define PWM_SERVO_2_AF 0
+#define PWM_SERVO_2_OC TIM_OC2
+#define PWM_SERVO_2_OC_BIT (1<<1)
+#else
+#define PWM_SERVO_2_OC_BIT 0
+#endif
+
+#if USE_PWM3
+#define PWM_SERVO_3 2
+#define PWM_SERVO_3_TIMER TIM3
+#define PWM_SERVO_3_RCC_IOP RCC_APB2ENR_IOPCEN
+#define PWM_SERVO_3_GPIO GPIOC
+#define PWM_SERVO_3_PIN GPIO8
+#define PWM_SERVO_3_AF 0
+#define PWM_SERVO_3_OC TIM_OC3
+#define PWM_SERVO_3_OC_BIT (1<<2)
+#else
+#define PWM_SERVO_3_OC_BIT 0
+#endif
+
+#if USE_PWM4
+#define PWM_SERVO_4 3
+#define PWM_SERVO_4_TIMER TIM3
+#define PWM_SERVO_4_RCC_IOP RCC_APB2ENR_IOPCEN
+#define PWM_SERVO_4_GPIO GPIOC
+#define PWM_SERVO_4_PIN GPIO9
+#define PWM_SERVO_4_AF 0
+#define PWM_SERVO_4_OC TIM_OC4
+#define PWM_SERVO_4_OC_BIT (1<<3)
+#else
+#define PWM_SERVO_4_OC_BIT 0
+#endif
+
+#if USE_PWM5
+#define PWM_SERVO_5 4
+#define PWM_SERVO_5_TIMER TIM5
+#define PWM_SERVO_5_RCC_IOP RCC_APB2ENR_IOPAEN
+#define PWM_SERVO_5_GPIO GPIOA
+#define PWM_SERVO_5_PIN GPIO0
+#define PWM_SERVO_5_AF 0
+#define PWM_SERVO_5_OC TIM_OC1
+#define PWM_SERVO_5_OC_BIT (1<<0)
+#else
+#define PWM_SERVO_5_OC_BIT 0
+#endif
+
+#if USE_PWM6
+#define PWM_SERVO_6 5
+#define PWM_SERVO_6_TIMER TIM5
+#define PWM_SERVO_6_RCC_IOP RCC_APB2ENR_IOPAEN
+#define PWM_SERVO_6_GPIO GPIOA
+#define PWM_SERVO_6_PIN GPIO1
+#define PWM_SERVO_6_AF 0
+#define PWM_SERVO_6_OC TIM_OC2
+#define PWM_SERVO_6_OC_BIT (1<<1)
+#else
+#define PWM_SERVO_6_OC_BIT 0
+#endif
+
+#if USE_PWM7
+#define PWM_SERVO_7 6
+#define PWM_SERVO_7_TIMER TIM4
+#define PWM_SERVO_7_RCC_IOP RCC_APB2ENR_IOPBEN
+#define PWM_SERVO_7_GPIO GPIOB
+#define PWM_SERVO_7_PIN GPIO6
+#define PWM_SERVO_7_AF 0
+#define PWM_SERVO_7_OC TIM_OC1
+#define PWM_SERVO_7_OC_BIT (1<<0)
+#else
+#define PWM_SERVO_7_OC_BIT 0
+#endif
+
+#if USE_PWM8
+#define PWM_SERVO_8 7
+#define PWM_SERVO_8_TIMER TIM4
+#define PWM_SERVO_8_RCC_IOP RCC_APB2ENR_IOPBEN
+#define PWM_SERVO_8_GPIO GPIOB
+#define PWM_SERVO_8_PIN GPIO7
+#define PWM_SERVO_8_AF 0
+#define PWM_SERVO_8_OC TIM_OC2
+#define PWM_SERVO_8_OC_BIT (1<<1)
+#else
+#define PWM_SERVO_8_OC_BIT 0
+#endif
+
+/* servos 1-4 on TIM3 */
+#define PWM_TIM3_CHAN_MASK (PWM_SERVO_1_OC_BIT|PWM_SERVO_2_OC_BIT|PWM_SERVO_3_OC_BIT|PWM_SERVO_4_OC_BIT)
+/* servos 5-6 on TIM5 */
+#define PWM_TIM5_CHAN_MASK (PWM_SERVO_5_OC_BIT|PWM_SERVO_6_OC_BIT)
+/* servos 7-8 on TIM4 if USE_SERVOS_7AND8 */
+#define PWM_TIM4_CHAN_MASK (PWM_SERVO_7_OC_BIT|PWM_SERVO_8_OC_BIT)
+
+
+#endif
+
diff --git a/sw/airborne/lisa/tunnel_hw.c b/sw/airborne/lisa/tunnel_hw.c
index 5f7ec7365b..9f624f7f0b 100644
--- a/sw/airborne/lisa/tunnel_hw.c
+++ b/sw/airborne/lisa/tunnel_hw.c
@@ -72,7 +72,7 @@ int main(void) {
gpio_set_mode(B_RX_PORT, GPIO_MODE_OUTPUT_50_MHZ,
GPIO_CNF_OUTPUT_PUSHPULL, B_TX_PIN);
- GPIO_BRR(A_TX_PORT) = A_TX_PIN;
+ gpio_clear(A_TX_PORT, A_TX_PIN);
/* */
while (1) {
@@ -104,17 +104,17 @@ static inline void main_event( void ) {
#if 0
if (!(foo%2))
- GPIO_BRR(A_TX_PORT) = A_TX_PIN;
+ gpio_clear(A_TX_PORT, A_TX_PIN);
else
- GPIO_BSRR(A_TX_PORT) = A_TX_PIN;
+ gpio_set(A_TX_PORT, A_TX_PIN);
#endif
#if 1
/* passthrough B_RX to A_TX */
if (GPIO_IDR(B_RX_PORT) & B_RX_PIN)
- GPIO_BSRR(A_TX_PORT) = A_TX_PIN;
+ gpio_set(A_TX_PORT, A_TX_PIN);
else
- GPIO_BRR(A_TX_PORT) = A_TX_PIN;
+ gpio_clear(A_TX_PORT, A_TX_PIN);
#endif
/* passthrough A_RX to B_TX */
if (gpio_get(A_RX_PORT, A_RX_PIN)) {
diff --git a/sw/airborne/mcu.c b/sw/airborne/mcu.c
index 61b8106819..c0472a7d59 100644
--- a/sw/airborne/mcu.c
+++ b/sw/airborne/mcu.c
@@ -38,7 +38,7 @@
#include "subsystems/radio_control.h"
#endif
#endif
-#if defined USE_UART0 || defined USE_UART1 || defined USE_UART2 || defined USE_UART3 || defined USE_UART4 || defined USE_UART5
+#if defined USE_UART0 || defined USE_UART1 || defined USE_UART2 || defined USE_UART3 || defined USE_UART4 || defined USE_UART5 || defined USE_UART6
#include "mcu_periph/uart.h"
#endif
#if defined USE_I2C0 || defined USE_I2C1 || defined USE_I2C2
@@ -89,6 +89,9 @@ void mcu_init(void) {
#ifdef USE_UART5
uart5_init();
#endif
+#ifdef USE_UART6
+ uart6_init();
+#endif
#ifdef USE_I2C0
i2c0_init();
#endif
diff --git a/sw/airborne/mcu_periph/uart.c b/sw/airborne/mcu_periph/uart.c
index af759c9856..49abe70e44 100644
--- a/sw/airborne/mcu_periph/uart.c
+++ b/sw/airborne/mcu_periph/uart.c
@@ -38,10 +38,18 @@ struct uart_periph uart2;
struct uart_periph uart3;
#endif
+#ifdef USE_UART4
+struct uart_periph uart4;
+#endif
+
#ifdef USE_UART5
struct uart_periph uart5;
#endif
+#ifdef USE_UART6
+struct uart_periph uart6;
+#endif
+
void uart_periph_init(struct uart_periph* p) {
p->rx_insert_idx = 0;
p->rx_extract_idx = 0;
diff --git a/sw/airborne/mcu_periph/uart.h b/sw/airborne/mcu_periph/uart.h
index 6c5871773a..1776ae2ca5 100644
--- a/sw/airborne/mcu_periph/uart.h
+++ b/sw/airborne/mcu_periph/uart.h
@@ -74,7 +74,8 @@ struct uart_periph {
extern void uart_periph_init(struct uart_periph* p);
-extern void uart_periph_set_baudrate(struct uart_periph* p, uint32_t baud, bool_t hw_flow_control);
+extern void uart_periph_set_baudrate(struct uart_periph* p, uint32_t baud);
+extern void uart_periph_set_mode(struct uart_periph* p, bool_t tx_enabled, bool_t rx_enabled, bool_t hw_flow_control);
extern void uart_transmit(struct uart_periph* p, uint8_t data);
extern bool_t uart_check_free_space(struct uart_periph* p, uint8_t len);
extern uint8_t uart_getch(struct uart_periph* p);
@@ -95,7 +96,7 @@ extern void uart0_init(void);
#define UART0ChAvailable() uart_char_available(&uart0)
#define UART0Getch() uart_getch(&uart0)
#define UART0TxRunning uart0.tx_running
-#define UART0SetBaudrate(_b) uart_periph_set_baudrate(&uart0, _b, FALSE)
+#define UART0SetBaudrate(_b) uart_periph_set_baudrate(&uart0, _b)
#endif // USE_UART0
@@ -110,11 +111,7 @@ extern void uart1_init(void);
#define UART1ChAvailable() uart_char_available(&uart1)
#define UART1Getch() uart_getch(&uart1)
#define UART1TxRunning uart1.tx_running
-#if UART1_HW_FLOW_CONTROL
-#define UART1SetBaudrate(_b) uart_periph_set_baudrate(&uart1, _b, TRUE)
-#else
-#define UART1SetBaudrate(_b) uart_periph_set_baudrate(&uart1, _b, FALSE)
-#endif
+#define UART1SetBaudrate(_b) uart_periph_set_baudrate(&uart1, _b)
#endif // USE_UART1
@@ -129,7 +126,7 @@ extern void uart2_init(void);
#define UART2ChAvailable() uart_char_available(&uart2)
#define UART2Getch() uart_getch(&uart2)
#define UART2TxRunning uart2.tx_running
-#define UART2SetBaudrate(_b) uart_periph_set_baudrate(&uart2, _b, FALSE)
+#define UART2SetBaudrate(_b) uart_periph_set_baudrate(&uart2, _b)
#endif // USE_UART2
@@ -144,10 +141,25 @@ extern void uart3_init(void);
#define UART3ChAvailable() uart_char_available(&uart3)
#define UART3Getch() uart_getch(&uart3)
#define UART3TxRunning uart3.tx_running
-#define UART3SetBaudrate(_b) uart_periph_set_baudrate(&uart3, _b, FALSE)
+#define UART3SetBaudrate(_b) uart_periph_set_baudrate(&uart3, _b)
#endif // USE_UART3
+#ifdef USE_UART4
+extern struct uart_periph uart4;
+extern void uart4_init(void);
+
+#define UART4Init() uart_periph_init(&uart4)
+#define UART4CheckFreeSpace(_x) uart_check_free_space(&uart4, _x)
+#define UART4Transmit(_x) uart_transmit(&uart4, _x)
+#define UART4SendMessage() {}
+#define UART4ChAvailable() uart_char_available(&uart4)
+#define UART4Getch() uart_getch(&uart4)
+#define UART4TxRunning uart4.tx_running
+#define UART4SetBaudrate(_b) uart_periph_set_baudrate(&uart4, _b)
+
+#endif // USE_UART4
+
#ifdef USE_UART5
extern struct uart_periph uart5;
extern void uart5_init(void);
@@ -159,8 +171,23 @@ extern void uart5_init(void);
#define UART5ChAvailable() uart_char_available(&uart5)
#define UART5Getch() uart_getch(&uart5)
#define UART5TxRunning uart5.tx_running
-#define UART5SetBaudrate(_b) uart_periph_set_baudrate(&uart5, _b, FALSE)
+#define UART5SetBaudrate(_b) uart_periph_set_baudrate(&uart5, _b)
#endif // USE_UART5
+#ifdef USE_UART6
+extern struct uart_periph uart6;
+extern void uart6_init(void);
+
+#define UART6Init() uart_periph_init(&uart6)
+#define UART6CheckFreeSpace(_x) uart_check_free_space(&uart6, _x)
+#define UART6Transmit(_x) uart_transmit(&uart6, _x)
+#define UART6SendMessage() {}
+#define UART6ChAvailable() uart_char_available(&uart6)
+#define UART6Getch() uart_getch(&uart6)
+#define UART6TxRunning uart6.tx_running
+#define UART6SetBaudrate(_b) uart_periph_set_baudrate(&uart6, _b)
+
+#endif // USE_UART6
+
#endif /* MCU_PERIPH_UART_H */
diff --git a/sw/airborne/peripherals/mpu60X0.h b/sw/airborne/peripherals/mpu60X0.h
deleted file mode 100644
index 384b92a726..0000000000
--- a/sw/airborne/peripherals/mpu60X0.h
+++ /dev/null
@@ -1,97 +0,0 @@
-#ifndef MPU60X0
-#define MPU60X0
-
-/* default I2C address */
-#define MPU60X0_ADDR 0xD0
-#define MPU60X0_ADDR_ALT 0xD2
-
-#define MPU60X0_SPI_READ 0x80
-
-// Power and Interface
-#define MPU60X0_REG_AUX_VDDIO 0x01 // Must be set to 0 on MPU6000
-#define MPU60X0_REG_USER_CTRL 0x6A
-#define MPU60X0_REG_PWR_MGMT_1 0x6B
-#define MPU60X0_REG_PWR_MGMT_2 0x6C
-
-// FIFO
-#define MPU60X0_REG_FIFO_EN 0X23
-#define MPU60X0_REG_FIFO_COUNT_H 0x72
-#define MPU60X0_REG_FIFO_COUNT_L 0x73
-#define MPU60X0_REG_FIFO_R_W 0x74
-
-// Measurement Settings
-#define MPU60X0_REG_SMPLRT_DIV 0X19
-#define MPU60X0_REG_CONFIG 0X1A
-#define MPU60X0_REG_GYRO_CONFIG 0X1B
-#define MPU60X0_REG_ACCEL_CONFIG 0X1C
-
-// I2C Slave settings
-#define MPU60X0_REG_I2C_MST_CTRL 0X24
-#define MPU60X0_REG_I2C_MST_STATUS 0X36
-#define MPU60X0_REG_I2C_MST_DELAY 0X67
-// Slave 0
-#define MPU60X0_REG_I2C_SLV0_ADDR 0X25 // i2c addr
-#define MPU60X0_REG_I2C_SLV0_REG 0X26 // slave reg
-#define MPU60X0_REG_I2C_SLV0_CTRL 0X27 // set-bits
-#define MPU60X0_REG_I2C_SLV0_DO 0X63 // DO
-// Slave 1
-#define MPU60X0_REG_I2C_SLV1_ADDR 0X28 // i2c addr
-#define MPU60X0_REG_I2C_SLV1_REG 0X29 // slave reg
-#define MPU60X0_REG_I2C_SLV1_CTRL 0X2A // set-bits
-#define MPU60X0_REG_I2C_SLV1_DO 0X64 // DO
-// Slave 2
-#define MPU60X0_REG_I2C_SLV2_ADDR 0X2B // i2c addr
-#define MPU60X0_REG_I2C_SLV2_REG 0X2C // slave reg
-#define MPU60X0_REG_I2C_SLV2_CTRL 0X2D // set-bits
-#define MPU60X0_REG_I2C_SLV2_DO 0X65 // DO
-// Slave 3
-#define MPU60X0_REG_I2C_SLV3_ADDR 0X2E // i2c addr
-#define MPU60X0_REG_I2C_SLV3_REG 0X2F // slave reg
-#define MPU60X0_REG_I2C_SLV3_CTRL 0X30 // set-bits
-#define MPU60X0_REG_I2C_SLV3_DO 0X66 // DO
-// Slave 4 - special
-#define MPU60X0_REG_I2C_SLV4_ADDR 0X31 // i2c addr
-#define MPU60X0_REG_I2C_SLV4_REG 0X32 // slave reg
-#define MPU60X0_REG_I2C_SLV4_DO 0X33 // DO
-#define MPU60X0_REG_I2C_SLV4_CTRL 0X34 // set-bits
-#define MPU60X0_REG_I2C_SLV4_DI 0X35 // DI
-
-// Interrupt
-#define MPU60X0_REG_INT_PIN_CFG 0X37
-#define MPU60X0_REG_INT_ENABLE 0X38
-#define MPU60X0_REG_INT_STATUS 0X3A
-
-// Accelero
-#define MPU60X0_REG_ACCEL_XOUT_H 0X3B
-#define MPU60X0_REG_ACCEL_XOUT_L 0X3C
-#define MPU60X0_REG_ACCEL_YOUT_H 0X3D
-#define MPU60X0_REG_ACCEL_YOUT_L 0X3E
-#define MPU60X0_REG_ACCEL_ZOUT_H 0X3F
-#define MPU60X0_REG_ACCEL_ZOUT_L 0X40
-
-// Temperature
-#define MPU60X0_REG_TEMP_OUT_H 0X41
-#define MPU60X0_REG_TEMP_OUT_L 0X42
-
-// Gyro
-#define MPU60X0_REG_GYRO_XOUT_H 0X43
-#define MPU60X0_REG_GYRO_XOUT_L 0X44
-#define MPU60X0_REG_GYRO_YOUT_H 0X45
-#define MPU60X0_REG_GYRO_YOUT_L 0X46
-#define MPU60X0_REG_GYRO_ZOUT_H 0X47
-#define MPU60X0_REG_GYRO_ZOUT_L 0X48
-
-// External Sensor Data
-#define MPU60X0_EXT_SENS_DATA 0X49
-#define MPU60X0_EXT_SENS_DATA_SIZE 24
-
-
-/////////////////////////////////////////////////
-// MPU60X0 Definitions
-
-#define MPU60X0_REG_WHO_AM_I 0X75
-#define MPU60X0_WHOAMI_REPLY 0x68
-
-
-
-#endif /* MPU60X0 */
diff --git a/sw/airborne/peripherals/mpu60x0.c b/sw/airborne/peripherals/mpu60x0.c
new file mode 100644
index 0000000000..34598ac5c9
--- /dev/null
+++ b/sw/airborne/peripherals/mpu60x0.c
@@ -0,0 +1,86 @@
+/*
+ * Copyright (C) 2013 Felix Ruess
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file peripherals/mpu60x0.c
+ *
+ * MPU-60X0 driver common functions (I2C and SPI).
+ *
+ * Still needs the either the I2C or SPI specific implementation.
+ */
+
+#include "peripherals/mpu60x0.h"
+
+void mpu60x0_set_default_config(struct Mpu60x0Config *c)
+{
+ c->smplrt_div = MPU60X0_DEFAULT_SMPLRT_DIV;
+ c->dlpf_cfg = MPU60X0_DEFAULT_DLPF_CFG;
+ c->gyro_range = MPU60X0_DEFAULT_FS_SEL;
+ c->accel_range = MPU60X0_DEFAULT_AFS_SEL;
+ c->i2c_bypass = TRUE;
+ c->drdy_int_enable = FALSE;
+ c->clk_sel = MPU60X0_DEFAULT_CLK_SEL;
+}
+
+void mpu60x0_send_config(Mpu60x0ConfigSet mpu_set, void* mpu, struct Mpu60x0Config* config)
+{
+ switch (config->init_status) {
+ case MPU60X0_CONF_PWR:
+ /* switch to gyroX clock by default */
+ mpu_set(mpu, MPU60X0_REG_PWR_MGMT_1, ((config->clk_sel)|(0<<6)));
+ config->init_status++;
+ break;
+ case MPU60X0_CONF_SD:
+ /* configure sample rate divider */
+ mpu_set(mpu, MPU60X0_REG_SMPLRT_DIV, config->smplrt_div);
+ config->init_status++;
+ break;
+ case MPU60X0_CONF_DLPF:
+ /* configure digital low pass filter */
+ mpu_set(mpu, MPU60X0_REG_CONFIG, config->dlpf_cfg);
+ config->init_status++;
+ break;
+ case MPU60X0_CONF_GYRO:
+ /* configure gyro range */
+ mpu_set(mpu, MPU60X0_REG_GYRO_CONFIG, (config->gyro_range<<3));
+ config->init_status++;
+ break;
+ case MPU60X0_CONF_ACCEL:
+ /* configure accelerometer range */
+ mpu_set(mpu, MPU60X0_REG_ACCEL_CONFIG, (config->accel_range<<3));
+ config->init_status++;
+ break;
+ case MPU60X0_CONF_I2C_BYPASS:
+ mpu_set(mpu, MPU60X0_REG_INT_PIN_CFG, (config->i2c_bypass<<1));
+ config->init_status++;
+ break;
+ case MPU60X0_CONF_INT_ENABLE:
+ /* configure data ready interrupt */
+ mpu_set(mpu, MPU60X0_REG_INT_ENABLE, (config->drdy_int_enable<<0));
+ config->init_status++;
+ break;
+ case MPU60X0_CONF_DONE:
+ config->initialized = TRUE;
+ break;
+ default:
+ break;
+ }
+}
diff --git a/sw/airborne/peripherals/mpu60x0.h b/sw/airborne/peripherals/mpu60x0.h
new file mode 100644
index 0000000000..60334d8bd2
--- /dev/null
+++ b/sw/airborne/peripherals/mpu60x0.h
@@ -0,0 +1,81 @@
+/*
+ * Copyright (C) 2013 Gautier Hattenberger
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file peripherals/mpu60x0.h
+ *
+ * MPU-60X0 driver common interface (I2C and SPI).
+ */
+
+#ifndef MPU60X0_H
+#define MPU60X0_H
+
+#include "std.h"
+
+/* Include address and register definition */
+#include "peripherals/mpu60x0_regs.h"
+
+/// Default sample rate divider
+#define MPU60X0_DEFAULT_SMPLRT_DIV 0
+/// Default gyro full scale range +- 2000°/s
+#define MPU60X0_DEFAULT_FS_SEL MPU60X0_GYRO_RANGE_2000
+/// Default accel full scale range +- 16g
+#define MPU60X0_DEFAULT_AFS_SEL MPU60X0_ACCEL_RANGE_16G
+/// Default internal sampling (1kHz, 42Hz LP Bandwidth)
+#define MPU60X0_DEFAULT_DLPF_CFG MPU60X0_DLPF_42HZ
+/// Default interrupt config: DATA_RDY_EN
+#define MPU60X0_DEFAULT_INT_CFG 1
+/// Default clock: PLL with X gyro reference
+#define MPU60X0_DEFAULT_CLK_SEL 1
+
+enum Mpu60x0ConfStatus {
+ MPU60X0_CONF_UNINIT,
+ MPU60X0_CONF_PWR,
+ MPU60X0_CONF_SD,
+ MPU60X0_CONF_DLPF,
+ MPU60X0_CONF_GYRO,
+ MPU60X0_CONF_ACCEL,
+ MPU60X0_CONF_I2C_BYPASS,
+ MPU60X0_CONF_INT_ENABLE,
+ MPU60X0_CONF_DONE
+};
+
+struct Mpu60x0Config {
+ uint8_t smplrt_div; ///< Sample rate divider
+ enum Mpu60x0DLPF dlpf_cfg; ///< Digital Low Pass Filter
+ enum Mpu60x0GyroRanges gyro_range; ///< deg/s Range
+ enum Mpu60x0AccelRanges accel_range; ///< g Range
+ bool_t i2c_bypass; ///< bypass mpu i2c
+ bool_t drdy_int_enable; ///< Enable Data Ready Interrupt
+ uint8_t clk_sel; ///< Clock select
+ enum Mpu60x0ConfStatus init_status; ///< init status
+ bool_t initialized; ///< config done flag
+};
+
+extern void mpu60x0_set_default_config(struct Mpu60x0Config *c);
+
+/// Configuration function prototype
+typedef void (*Mpu60x0ConfigSet)(void* mpu, uint8_t _reg, uint8_t _val);
+
+/// Configuration sequence called once before normal use
+extern void mpu60x0_send_config(Mpu60x0ConfigSet mpu_set, void* mpu, struct Mpu60x0Config* config);
+
+#endif // MPU60X0_H
diff --git a/sw/airborne/peripherals/mpu60x0_i2c.c b/sw/airborne/peripherals/mpu60x0_i2c.c
new file mode 100644
index 0000000000..ada9ee4b55
--- /dev/null
+++ b/sw/airborne/peripherals/mpu60x0_i2c.c
@@ -0,0 +1,114 @@
+/*
+ * Copyright (C) 2013 Gautier Hattenberger
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file peripherals/mpu60x0_i2c.c
+ *
+ * Driver for the MPU-60X0 using I2C.
+ *
+ */
+
+#include "peripherals/mpu60x0_i2c.h"
+
+void mpu60x0_i2c_init(struct Mpu60x0_I2c *mpu, struct i2c_periph *i2c_p, uint8_t addr)
+{
+ /* set i2c_peripheral */
+ mpu->i2c_p = i2c_p;
+
+ /* slave address */
+ mpu->i2c_trans.slave_addr = addr;
+ /* set inital status: Success or Done */
+ mpu->i2c_trans.status = I2CTransDone;
+
+ /* set default MPU60X0 config options */
+ mpu60x0_set_default_config(&(mpu->config));
+
+ mpu->data_available = FALSE;
+ mpu->config.initialized = FALSE;
+ mpu->config.init_status = MPU60X0_CONF_UNINIT;
+}
+
+
+static void mpu60x0_i2c_write_to_reg(void* mpu, uint8_t _reg, uint8_t _val) {
+ struct Mpu60x0_I2c* mpu_i2c = (struct Mpu60x0_I2c*)(mpu);
+ mpu_i2c->i2c_trans.buf[0] = _reg;
+ mpu_i2c->i2c_trans.buf[1] = _val;
+ i2c_transmit(mpu_i2c->i2c_p, &(mpu_i2c->i2c_trans), mpu_i2c->i2c_trans.slave_addr, 2);
+}
+
+// Configuration function called once before normal use
+void mpu60x0_i2c_start_configure(struct Mpu60x0_I2c *mpu)
+{
+ if (mpu->config.init_status == MPU60X0_CONF_UNINIT) {
+ mpu->config.init_status++;
+ if (mpu->i2c_trans.status == I2CTransSuccess || mpu->i2c_trans.status == I2CTransDone) {
+ mpu60x0_send_config(mpu60x0_i2c_write_to_reg, (void*)mpu, &(mpu->config));
+ }
+ }
+}
+
+void mpu60x0_i2c_read(struct Mpu60x0_I2c *mpu)
+{
+ if (mpu->config.initialized && mpu->i2c_trans.status == I2CTransDone) {
+ /* set read bit and multiple byte bit, then address */
+ mpu->i2c_trans.buf[0] = MPU60X0_REG_INT_STATUS;
+ i2c_transceive(mpu->i2c_p, &(mpu->i2c_trans), mpu->i2c_trans.slave_addr, 1, 15);
+ }
+}
+
+#define Int16FromBuf(_buf,_idx) ((int16_t)((_buf[_idx]<<8) | _buf[_idx+1]))
+
+void mpu60x0_i2c_event(struct Mpu60x0_I2c *mpu)
+{
+ if (mpu->config.initialized) {
+ if (mpu->i2c_trans.status == I2CTransFailed) {
+ mpu->i2c_trans.status = I2CTransDone;
+ }
+ else if (mpu->i2c_trans.status == I2CTransSuccess) {
+ // Successfull reading
+ if (bit_is_set(mpu->i2c_trans.buf[0],0)) {
+ // new data
+ mpu->data_accel.vect.x = Int16FromBuf(mpu->i2c_trans.buf,1);
+ mpu->data_accel.vect.y = Int16FromBuf(mpu->i2c_trans.buf,3);
+ mpu->data_accel.vect.z = Int16FromBuf(mpu->i2c_trans.buf,5);
+ mpu->data_rates.rates.p = Int16FromBuf(mpu->i2c_trans.buf,9);
+ mpu->data_rates.rates.q = Int16FromBuf(mpu->i2c_trans.buf,11);
+ mpu->data_rates.rates.r = Int16FromBuf(mpu->i2c_trans.buf,13);
+ mpu->data_available = TRUE;
+ }
+ mpu->i2c_trans.status = I2CTransDone;
+ }
+ }
+ else if (mpu->config.init_status != MPU60X0_CONF_UNINIT) { // Configuring but not yet initialized
+ switch (mpu->i2c_trans.status) {
+ case I2CTransFailed:
+ mpu->config.init_status--; // Retry config (TODO max retry)
+ case I2CTransSuccess:
+ case I2CTransDone:
+ mpu->i2c_trans.status = I2CTransDone;
+ mpu60x0_send_config(mpu60x0_i2c_write_to_reg, (void*)mpu, &(mpu->config));
+ if (mpu->config.initialized) mpu->i2c_trans.status = I2CTransDone;
+ break;
+ default:
+ break;
+ }
+ }
+}
diff --git a/sw/airborne/peripherals/mpu60x0_i2c.h b/sw/airborne/peripherals/mpu60x0_i2c.h
new file mode 100644
index 0000000000..f7d95eff61
--- /dev/null
+++ b/sw/airborne/peripherals/mpu60x0_i2c.h
@@ -0,0 +1,68 @@
+/*
+ * Copyright (C) 2013 Gautier Hattenberger
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file peripherals/mpu60x0_i2c.h
+ *
+ * Driver for the MPU-60X0 using I2C.
+ */
+
+#ifndef MPU60X0_I2C_H
+#define MPU60X0_I2C_H
+
+#include "std.h"
+#include "math/pprz_algebra_int.h"
+#include "mcu_periph/i2c.h"
+
+/* Include common MPU60X0 options and definitions */
+#include "peripherals/mpu60x0.h"
+
+
+struct Mpu60x0_I2c {
+ struct i2c_periph *i2c_p;
+ struct i2c_transaction i2c_trans;
+ volatile bool_t data_available; ///< data ready flag
+ union {
+ struct Int16Vect3 vect; ///< accel data vector in accel coordinate system
+ int16_t value[3]; ///< accel data values accessible by channel index
+ } data_accel;
+ union {
+ struct Int16Rates rates; ///< rates data as angular rates in gyro coordinate system
+ int16_t value[3]; ///< rates data values accessible by channel index
+ } data_rates;
+ struct Mpu60x0Config config;
+};
+
+// Functions
+extern void mpu60x0_i2c_init(struct Mpu60x0_I2c *mpu, struct i2c_periph *i2c_p, uint8_t addr);
+extern void mpu60x0_i2c_start_configure(struct Mpu60x0_I2c *mpu);
+extern void mpu60x0_i2c_read(struct Mpu60x0_I2c *mpu);
+extern void mpu60x0_i2c_event(struct Mpu60x0_I2c *mpu);
+
+/// convenience function: read or start configuration if not already initialized
+static inline void mpu60x0_i2c_periodic(struct Mpu60x0_I2c *mpu) {
+ if (mpu->config.initialized)
+ mpu60x0_i2c_read(mpu);
+ else
+ mpu60x0_i2c_start_configure(mpu);
+}
+
+#endif // MPU60X0_I2C_H
diff --git a/sw/airborne/peripherals/mpu60x0_regs.h b/sw/airborne/peripherals/mpu60x0_regs.h
new file mode 100644
index 0000000000..211d1729f0
--- /dev/null
+++ b/sw/airborne/peripherals/mpu60x0_regs.h
@@ -0,0 +1,160 @@
+/*
+ * Copyright (C) 2010-2013 The Paparazzi Team
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file peripherals/mpu60x0_regs.h
+ *
+ * Register and address definitions for MPU-6000 and MPU-6050.
+ */
+
+#ifndef MPU60X0_REGS_H
+#define MPU60X0_REGS_H
+
+/* default I2C address */
+#define MPU60X0_ADDR 0xD0
+#define MPU60X0_ADDR_ALT 0xD2
+
+#define MPU60X0_SPI_READ 0x80
+
+// Power and Interface
+#define MPU60X0_REG_AUX_VDDIO 0x01 // Must be set to 0 on MPU6000
+#define MPU60X0_REG_USER_CTRL 0x6A
+#define MPU60X0_REG_PWR_MGMT_1 0x6B
+#define MPU60X0_REG_PWR_MGMT_2 0x6C
+
+// FIFO
+#define MPU60X0_REG_FIFO_EN 0X23
+#define MPU60X0_REG_FIFO_COUNT_H 0x72
+#define MPU60X0_REG_FIFO_COUNT_L 0x73
+#define MPU60X0_REG_FIFO_R_W 0x74
+
+// Measurement Settings
+#define MPU60X0_REG_SMPLRT_DIV 0X19
+#define MPU60X0_REG_CONFIG 0X1A
+#define MPU60X0_REG_GYRO_CONFIG 0X1B
+#define MPU60X0_REG_ACCEL_CONFIG 0X1C
+
+// I2C Slave settings
+#define MPU60X0_REG_I2C_MST_CTRL 0X24
+#define MPU60X0_REG_I2C_MST_STATUS 0X36
+#define MPU60X0_REG_I2C_MST_DELAY 0X67
+// Slave 0
+#define MPU60X0_REG_I2C_SLV0_ADDR 0X25 // i2c addr
+#define MPU60X0_REG_I2C_SLV0_REG 0X26 // slave reg
+#define MPU60X0_REG_I2C_SLV0_CTRL 0X27 // set-bits
+#define MPU60X0_REG_I2C_SLV0_DO 0X63 // DO
+// Slave 1
+#define MPU60X0_REG_I2C_SLV1_ADDR 0X28 // i2c addr
+#define MPU60X0_REG_I2C_SLV1_REG 0X29 // slave reg
+#define MPU60X0_REG_I2C_SLV1_CTRL 0X2A // set-bits
+#define MPU60X0_REG_I2C_SLV1_DO 0X64 // DO
+// Slave 2
+#define MPU60X0_REG_I2C_SLV2_ADDR 0X2B // i2c addr
+#define MPU60X0_REG_I2C_SLV2_REG 0X2C // slave reg
+#define MPU60X0_REG_I2C_SLV2_CTRL 0X2D // set-bits
+#define MPU60X0_REG_I2C_SLV2_DO 0X65 // DO
+// Slave 3
+#define MPU60X0_REG_I2C_SLV3_ADDR 0X2E // i2c addr
+#define MPU60X0_REG_I2C_SLV3_REG 0X2F // slave reg
+#define MPU60X0_REG_I2C_SLV3_CTRL 0X30 // set-bits
+#define MPU60X0_REG_I2C_SLV3_DO 0X66 // DO
+// Slave 4 - special
+#define MPU60X0_REG_I2C_SLV4_ADDR 0X31 // i2c addr
+#define MPU60X0_REG_I2C_SLV4_REG 0X32 // slave reg
+#define MPU60X0_REG_I2C_SLV4_DO 0X33 // DO
+#define MPU60X0_REG_I2C_SLV4_CTRL 0X34 // set-bits
+#define MPU60X0_REG_I2C_SLV4_DI 0X35 // DI
+
+// Interrupt
+#define MPU60X0_REG_INT_PIN_CFG 0X37
+#define MPU60X0_REG_INT_ENABLE 0X38
+#define MPU60X0_REG_INT_STATUS 0X3A
+
+// Accelero
+#define MPU60X0_REG_ACCEL_XOUT_H 0X3B
+#define MPU60X0_REG_ACCEL_XOUT_L 0X3C
+#define MPU60X0_REG_ACCEL_YOUT_H 0X3D
+#define MPU60X0_REG_ACCEL_YOUT_L 0X3E
+#define MPU60X0_REG_ACCEL_ZOUT_H 0X3F
+#define MPU60X0_REG_ACCEL_ZOUT_L 0X40
+
+// Temperature
+#define MPU60X0_REG_TEMP_OUT_H 0X41
+#define MPU60X0_REG_TEMP_OUT_L 0X42
+
+// Gyro
+#define MPU60X0_REG_GYRO_XOUT_H 0X43
+#define MPU60X0_REG_GYRO_XOUT_L 0X44
+#define MPU60X0_REG_GYRO_YOUT_H 0X45
+#define MPU60X0_REG_GYRO_YOUT_L 0X46
+#define MPU60X0_REG_GYRO_ZOUT_H 0X47
+#define MPU60X0_REG_GYRO_ZOUT_L 0X48
+
+// External Sensor Data
+#define MPU60X0_EXT_SENS_DATA 0X49
+#define MPU60X0_EXT_SENS_DATA_SIZE 24
+
+
+/////////////////////////////////////////////////
+// MPU60X0 Definitions
+
+#define MPU60X0_REG_WHO_AM_I 0X75
+#define MPU60X0_WHOAMI_REPLY 0x68
+
+// Bit positions
+#define MPU60X0_I2C_BYPASS_EN 1
+#define MPU60X0_I2C_MST_EN 5
+
+/** Digital Low Pass Filter Options
+ * DLFP is affecting both gyro and accels,
+ * with slightly different bandwidth
+ */
+enum Mpu60x0DLPF {
+ MPU60X0_DLPF_256HZ = 0x0, // internal sampling rate 8kHz
+ MPU60X0_DLPF_188HZ = 0x1, // internal sampling rate 1kHz
+ MPU60X0_DLPF_98HZ = 0x2,
+ MPU60X0_DLPF_42HZ = 0x3,
+ MPU60X0_DLPF_20HZ = 0x4,
+ MPU60X0_DLPF_10HZ = 0x5,
+ MPU60X0_DLPF_05HZ = 0x6
+};
+
+/**
+ * Selectable gyro range
+ */
+enum Mpu60x0GyroRanges {
+ MPU60X0_GYRO_RANGE_250 = 0x00,
+ MPU60X0_GYRO_RANGE_500 = 0x01,
+ MPU60X0_GYRO_RANGE_1000 = 0x02,
+ MPU60X0_GYRO_RANGE_2000 = 0x03
+};
+
+/**
+ * Selectable accel range
+ */
+enum Mpu60x0AccelRanges {
+ MPU60X0_ACCEL_RANGE_2G = 0x00,
+ MPU60X0_ACCEL_RANGE_4G = 0x01,
+ MPU60X0_ACCEL_RANGE_8G = 0x02,
+ MPU60X0_ACCEL_RANGE_16G = 0x03
+};
+
+#endif /* MPU60X0_REGS_H */
diff --git a/sw/airborne/peripherals/mpu60x0_spi.c b/sw/airborne/peripherals/mpu60x0_spi.c
new file mode 100644
index 0000000000..97993f322e
--- /dev/null
+++ b/sw/airborne/peripherals/mpu60x0_spi.c
@@ -0,0 +1,132 @@
+/*
+ * Copyright (C) 2013 Gautier Hattenberger
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file peripherals/mpu60x0_spi.c
+ *
+ * Driver for the MPU-60X0 using SPI.
+ *
+ */
+
+#include "peripherals/mpu60x0_spi.h"
+
+void mpu60x0_spi_init(struct Mpu60x0_Spi *mpu, struct spi_periph *spi_p, uint8_t slave_idx)
+{
+ /* set spi_peripheral */
+ mpu->spi_p = spi_p;
+
+ /* configure spi transaction */
+ mpu->spi_trans.cpol = SPICpolIdleHigh;
+ mpu->spi_trans.cpha = SPICphaEdge2;
+ mpu->spi_trans.dss = SPIDss8bit;
+ mpu->spi_trans.bitorder = SPIMSBFirst;
+ mpu->spi_trans.cdiv = SPIDiv64;
+
+ mpu->spi_trans.select = SPISelectUnselect;
+ mpu->spi_trans.slave_idx = slave_idx;
+ mpu->spi_trans.output_length = MPU60X0_BUFFER_LEN; //FIXME
+ mpu->spi_trans.input_length = MPU60X0_BUFFER_LEN;
+ mpu->spi_trans.before_cb = NULL;
+ mpu->spi_trans.after_cb = NULL;
+ mpu->spi_trans.input_buf = &(mpu->rx_buf[0]);
+ mpu->spi_trans.output_buf = &(mpu->tx_buf[0]);
+
+ /* set inital status: Success or Done */
+ mpu->spi_trans.status = SPITransDone;
+
+ /* set default MPU60X0 config options */
+ mpu60x0_set_default_config(&(mpu->config));
+
+ mpu->data_available = FALSE;
+ mpu->config.initialized = FALSE;
+ mpu->config.init_status = MPU60X0_CONF_UNINIT;
+}
+
+
+static void mpu60x0_spi_write_to_reg(void* mpu, uint8_t _reg, uint8_t _val) {
+ struct Mpu60x0_Spi* mpu_spi = (struct Mpu60x0_Spi*)(mpu);
+ mpu_spi->spi_trans.output_length = 2;
+ mpu_spi->spi_trans.input_length = 0;
+ mpu_spi->tx_buf[0] = _reg;
+ mpu_spi->tx_buf[1] = _val;
+ spi_submit(mpu_spi->spi_p, &(mpu_spi->spi_trans));
+}
+
+// Configuration function called once before normal use
+void mpu60x0_spi_start_configure(struct Mpu60x0_Spi *mpu)
+{
+ if (mpu->config.init_status == MPU60X0_CONF_UNINIT) {
+ mpu->config.init_status++;
+ if (mpu->spi_trans.status == SPITransSuccess || mpu->spi_trans.status == SPITransDone) {
+ mpu60x0_send_config(mpu60x0_spi_write_to_reg, (void*)mpu, &(mpu->config));
+ }
+ }
+}
+
+void mpu60x0_spi_read(struct Mpu60x0_Spi *mpu)
+{
+ if (mpu->config.initialized && mpu->spi_trans.status == SPITransDone) {
+ mpu->spi_trans.output_length = 1;
+ mpu->spi_trans.input_length = 16; // FIXME external data
+ /* set read bit and multiple byte bit, then address */
+ mpu->tx_buf[0] = MPU60X0_REG_INT_STATUS + MPU60X0_SPI_READ;
+ spi_submit(mpu->spi_p, &(mpu->spi_trans));
+ }
+}
+
+#define Int16FromBuf(_buf,_idx) ((int16_t)((_buf[_idx]<<8) | _buf[_idx+1]))
+
+void mpu60x0_spi_event(struct Mpu60x0_Spi *mpu)
+{
+ if (mpu->config.initialized) {
+ if (mpu->spi_trans.status == SPITransFailed) {
+ mpu->spi_trans.status = SPITransDone;
+ }
+ else if (mpu->spi_trans.status == SPITransSuccess) {
+ // Successfull reading
+ if (bit_is_set(mpu->rx_buf[1],0)) {
+ // new data
+ mpu->data_accel.vect.x = Int16FromBuf(mpu->rx_buf,2);
+ mpu->data_accel.vect.y = Int16FromBuf(mpu->rx_buf,4);
+ mpu->data_accel.vect.z = Int16FromBuf(mpu->rx_buf,6);
+ mpu->data_rates.rates.p = Int16FromBuf(mpu->rx_buf,10);
+ mpu->data_rates.rates.q = Int16FromBuf(mpu->rx_buf,12);
+ mpu->data_rates.rates.r = Int16FromBuf(mpu->rx_buf,14);
+ mpu->data_available = TRUE;
+ }
+ mpu->spi_trans.status = SPITransDone;
+ }
+ }
+ else if (mpu->config.init_status != MPU60X0_CONF_UNINIT) { // Configuring but not yet initialized
+ switch (mpu->spi_trans.status) {
+ case SPITransFailed:
+ mpu->config.init_status--; // Retry config (TODO max retry)
+ case SPITransSuccess:
+ case SPITransDone:
+ mpu->spi_trans.status = SPITransDone;
+ mpu60x0_send_config(mpu60x0_spi_write_to_reg, (void*)mpu, &(mpu->config));
+ if (mpu->config.initialized) mpu->spi_trans.status = SPITransDone;
+ break;
+ default:
+ break;
+ }
+ }
+}
diff --git a/sw/airborne/peripherals/mpu60x0_spi.h b/sw/airborne/peripherals/mpu60x0_spi.h
new file mode 100644
index 0000000000..6eac22267c
--- /dev/null
+++ b/sw/airborne/peripherals/mpu60x0_spi.h
@@ -0,0 +1,72 @@
+/*
+ * Copyright (C) 2013 Gautier Hattenberger
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file peripherals/mpu60x0_spi.h
+ *
+ * Driver for the MPU-60X0 using SPI.
+ */
+
+#ifndef MPU60X0_SPI_H
+#define MPU60X0_SPI_H
+
+#include "std.h"
+#include "math/pprz_algebra_int.h"
+#include "mcu_periph/spi.h"
+
+/* Include common MPU60X0 options and definitions */
+#include "peripherals/mpu60x0.h"
+
+
+#define MPU60X0_BUFFER_LEN 32
+
+struct Mpu60x0_Spi {
+ struct spi_periph *spi_p;
+ struct spi_transaction spi_trans;
+ volatile uint8_t tx_buf[MPU60X0_BUFFER_LEN]; // FIXME correct length
+ volatile uint8_t rx_buf[MPU60X0_BUFFER_LEN]; // FIXME idem
+ volatile bool_t data_available; ///< data ready flag
+ union {
+ struct Int16Vect3 vect; ///< accel data vector in accel coordinate system
+ int16_t value[3]; ///< accel data values accessible by channel index
+ } data_accel;
+ union {
+ struct Int16Rates rates; ///< rates data as angular rates in gyro coordinate system
+ int16_t value[3]; ///< rates data values accessible by channel index
+ } data_rates;
+ struct Mpu60x0Config config;
+};
+
+// Functions
+extern void mpu60x0_spi_init(struct Mpu60x0_Spi *mpu, struct spi_periph *spi_p, uint8_t addr);
+extern void mpu60x0_spi_start_configure(struct Mpu60x0_Spi *mpu);
+extern void mpu60x0_spi_read(struct Mpu60x0_Spi *mpu);
+extern void mpu60x0_spi_event(struct Mpu60x0_Spi *mpu);
+
+/// convenience function: read or start configuration if not already initialized
+static inline void mpu60x0_spi_periodic(struct Mpu60x0_Spi *mpu) {
+ if (mpu->config.initialized)
+ mpu60x0_spi_read(mpu);
+ else
+ mpu60x0_spi_start_configure(mpu);
+}
+
+#endif // MPU60X0_SPI_H
diff --git a/sw/airborne/subsystems/imu/imu_aspirin2.c b/sw/airborne/subsystems/imu/imu_aspirin2.c
index 6ed8b6096f..b467e1bd35 100644
--- a/sw/airborne/subsystems/imu/imu_aspirin2.c
+++ b/sw/airborne/subsystems/imu/imu_aspirin2.c
@@ -24,7 +24,7 @@
#include "mcu_periph/spi.h"
// Peripherials
-#include "peripherals/mpu60X0.h"
+#include "peripherals/mpu60x0_regs.h"
#include "peripherals/hmc58xx_regs.h"
#include "peripherals/ms5611.h"
diff --git a/sw/airborne/subsystems/imu/imu_aspirin_2.c b/sw/airborne/subsystems/imu/imu_aspirin_2.c
new file mode 100644
index 0000000000..33a444456e
--- /dev/null
+++ b/sw/airborne/subsystems/imu/imu_aspirin_2.c
@@ -0,0 +1,121 @@
+/*
+ * Copyright (C) 2013 Felix Ruess
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file subsystems/imu/imu_aspirin_2.c
+ * Driver for the Aspirin v2.x IMU using SPI for the MPU6000.
+ */
+
+#include "subsystems/imu.h"
+
+#include "mcu_periph/i2c.h"
+#include "mcu_periph/spi.h"
+
+
+/* defaults suitable for Lisa */
+#ifndef ASPIRIN_2_SPI_SLAVE_IDX
+#define ASPIRIN_2_SPI_SLAVE_IDX SPI_SLAVE2
+#endif
+PRINT_CONFIG_VAR(ASPIRIN_2_SPI_SLAVE_IDX)
+
+#ifndef ASPIRIN_2_SPI_DEV
+#define ASPIRIN_2_SPI_DEV spi2
+#endif
+PRINT_CONFIG_VAR(ASPIRIN_2_SPI_DEV)
+
+#ifndef ASPIRIN_2_I2C_DEV
+#define ASPIRIN_2_I2C_DEV i2c2
+#endif
+PRINT_CONFIG_VAR(ASPIRIN_2_I2C_DEV)
+
+
+/* gyro internal lowpass frequency */
+#if !defined ASPIRIN_2_LOWPASS_FILTER && !defined ASPIRIN_2_SMPLRT_DIV
+#define ASPIRIN_2_LOWPASS_FILTER MPU60X0_DLPF_256HZ
+#define ASPIRIN_2_SMPLRT_DIV 1
+PRINT_CONFIG_MSG("Gyro/Accel output rate is 500Hz")
+#endif
+PRINT_CONFIG_VAR(ASPIRIN_2_LOWPASS_FILTER)
+PRINT_CONFIG_VAR(ASPIRIN_2_SMPLRT_DIV)
+
+#ifndef ASPIRIN_2_GYRO_RANGE
+#define ASPIRIN_2_GYRO_RANGE MPU60X0_GYRO_RANGE_2000
+#endif
+PRINT_CONFIG_VAR(ASPIRIN_2_GYRO_RANGE)
+
+#ifndef ASPIRIN_2_ACCEL_RANGE
+#define ASPIRIN_2_ACCEL_RANGE MPU60X0_ACCEL_RANGE_16G
+#endif
+PRINT_CONFIG_VAR(ASPIRIN_2_ACCEL_RANGE)
+
+
+struct ImuAspirin2 imu_aspirin2;
+
+void imu_impl_init(void)
+{
+ imu_aspirin2.accel_valid = FALSE;
+ imu_aspirin2.gyro_valid = FALSE;
+ imu_aspirin2.mag_valid = FALSE;
+
+ mpu60x0_spi_init(&imu_aspirin2.mpu, &(ASPIRIN_2_SPI_DEV), ASPIRIN_2_SPI_SLAVE_IDX);
+ // change the default configuration
+ imu_aspirin2.mpu.config.smplrt_div = ASPIRIN_2_SMPLRT_DIV;
+ imu_aspirin2.mpu.config.dlpf_cfg = ASPIRIN_2_LOWPASS_FILTER;
+ imu_aspirin2.mpu.config.gyro_range = ASPIRIN_2_GYRO_RANGE;
+ imu_aspirin2.mpu.config.accel_range = ASPIRIN_2_ACCEL_RANGE;
+ //imu_aspirin2.mpu.config.i2c_bypass = FALSE;
+ //imu_aspirin2.mpu.config.drdy_int_enable = TRUE;
+
+ //hmc58xx_init(&imu_aspirin2.mag_hmc, &(ASPIRIN_2_I2C_DEV), HMC58XX_ADDR);
+}
+
+
+void imu_periodic(void)
+{
+ mpu60x0_spi_periodic(&imu_aspirin2.mpu);
+
+ // Read HMC58XX at 50Hz (main loop for rotorcraft: 512Hz)
+ //RunOnceEvery(10, hmc58xx_periodic(&imu_aspirin2.mag_hmc));
+}
+
+void imu_aspirin2_event(void)
+{
+ mpu60x0_spi_event(&imu_aspirin2.mpu);
+ if (imu_aspirin2.mpu.data_available) {
+ RATES_COPY(imu.gyro_unscaled, imu_aspirin2.mpu.data_rates.rates);
+ VECT3_COPY(imu.accel_unscaled, imu_aspirin2.mpu.data_accel.vect);
+ imu_aspirin2.mpu.data_available = FALSE;
+ imu_aspirin2.gyro_valid = TRUE;
+ imu_aspirin2.accel_valid = TRUE;
+ }
+
+#if 0
+ /* HMC58XX event task */
+ hmc58xx_event(&imu_aspirin2.mag_hmc);
+ if (imu_aspirin2.mag_hmc.data_available) {
+ imu.mag_unscaled.x = imu_aspirin2.mag_hmc.data.vect.y;
+ imu.mag_unscaled.y = -imu_aspirin2.mag_hmc.data.vect.x;
+ imu.mag_unscaled.z = imu_aspirin2.mag_hmc.data.vect.z;
+ imu_aspirin2.mag_hmc.data_available = FALSE;
+ imu_aspirin2.mag_valid = TRUE;
+ }
+#endif
+}
diff --git a/sw/airborne/subsystems/imu/imu_aspirin_2.h b/sw/airborne/subsystems/imu/imu_aspirin_2.h
new file mode 100644
index 0000000000..33b91686e4
--- /dev/null
+++ b/sw/airborne/subsystems/imu/imu_aspirin_2.h
@@ -0,0 +1,126 @@
+/*
+ * Copyright (C) 2013 Felix Ruess
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file subsystems/imu/imu_aspirin_2.h
+ * Driver for the Aspirin v2.x IMU using SPI for the MPU6000.
+ */
+
+#ifndef IMU_ASPIRIN_2_H
+#define IMU_ASPIRIN_2_H
+
+#include "std.h"
+#include "generated/airframe.h"
+#include "subsystems/imu.h"
+
+#include "peripherals/mpu60x0_spi.h"
+
+#if !defined IMU_MAG_X_SIGN & !defined IMU_MAG_Y_SIGN & !defined IMU_MAG_Z_SIGN
+#define IMU_MAG_X_SIGN 1
+#define IMU_MAG_Y_SIGN 1
+#define IMU_MAG_Z_SIGN 1
+#endif
+
+#if !defined IMU_GYRO_P_SIGN & !defined IMU_GYRO_Q_SIGN & !defined IMU_GYRO_R_SIGN
+#define IMU_GYRO_P_SIGN 1
+#define IMU_GYRO_Q_SIGN 1
+#define IMU_GYRO_R_SIGN 1
+#endif
+#if !defined IMU_ACCEL_X_SIGN & !defined IMU_ACCEL_Y_SIGN & !defined IMU_ACCEL_Z_SIGN
+#define IMU_ACCEL_X_SIGN 1
+#define IMU_ACCEL_Y_SIGN 1
+#define IMU_ACCEL_Z_SIGN 1
+#endif
+
+/** default gyro sensitivy and neutral from the datasheet
+ * MPU60X0 has 16.4 LSB/(deg/s) at 2000deg/s range
+ * sens = 1/16.4 * pi/180 * 2^INT32_RATE_FRAC
+ * sens = 1/16.4 * pi/180 * 4096 = 4.359066229
+ */
+#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
+#define IMU_GYRO_P_SENS 4.359
+#define IMU_GYRO_P_SENS_NUM 4359
+#define IMU_GYRO_P_SENS_DEN 1000
+#define IMU_GYRO_Q_SENS 4.359
+#define IMU_GYRO_Q_SENS_NUM 4359
+#define IMU_GYRO_Q_SENS_DEN 1000
+#define IMU_GYRO_R_SENS 4.359
+#define IMU_GYRO_R_SENS_NUM 4359
+#define IMU_GYRO_R_SENS_DEN 1000
+#endif
+#if !defined IMU_GYRO_P_NEUTRAL & !defined IMU_GYRO_Q_NEUTRAL & !defined IMU_GYRO_R_NEUTRAL
+#define IMU_GYRO_P_NEUTRAL 0
+#define IMU_GYRO_Q_NEUTRAL 0
+#define IMU_GYRO_R_NEUTRAL 0
+#endif
+
+/** default accel sensitivy from the datasheet
+ * MPU60X0 has 2048 LSB/g
+ * fixed point sens: 9.81 [m/s^2] / 2048 [LSB/g] * 2^INT32_ACCEL_FRAC
+ * sens = 9.81 / 2048 * 1024 = 4.905
+ */
+#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS
+#define IMU_ACCEL_X_SENS 4.905
+#define IMU_ACCEL_X_SENS_NUM 4905
+#define IMU_ACCEL_X_SENS_DEN 1000
+#define IMU_ACCEL_Y_SENS 4.905
+#define IMU_ACCEL_Y_SENS_NUM 4905
+#define IMU_ACCEL_Y_SENS_DEN 1000
+#define IMU_ACCEL_Z_SENS 4.905
+#define IMU_ACCEL_Z_SENS_NUM 4905
+#define IMU_ACCEL_Z_SENS_DEN 1000
+#endif
+#if !defined IMU_ACCEL_X_NEUTRAL & !defined IMU_ACCEL_Y_NEUTRAL & !defined IMU_ACCEL_Z_NEUTRAL
+#define IMU_ACCEL_X_NEUTRAL 0
+#define IMU_ACCEL_Y_NEUTRAL 0
+#define IMU_ACCEL_Z_NEUTRAL 0
+#endif
+
+
+struct ImuAspirin2 {
+ volatile bool_t gyro_valid;
+ volatile bool_t accel_valid;
+ volatile bool_t mag_valid;
+ struct Mpu60x0_Spi mpu;
+};
+
+extern struct ImuAspirin2 imu_aspirin2;
+
+extern void imu_aspirin2_event(void);
+
+
+static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void)) {
+ imu_aspirin2_event();
+ if (imu_aspirin2.gyro_valid) {
+ imu_aspirin2.gyro_valid = FALSE;
+ _gyro_handler();
+ }
+ if (imu_aspirin2.accel_valid) {
+ imu_aspirin2.accel_valid = FALSE;
+ _accel_handler();
+ }
+ if (imu_aspirin2.mag_valid) {
+ imu_aspirin2.mag_valid = FALSE;
+ _mag_handler();
+ }
+}
+
+#endif /* IMU_ASPIRIN_2_H */
diff --git a/sw/ext/libopencm3 b/sw/ext/libopencm3
index a12614054c..0bf8324c91 160000
--- a/sw/ext/libopencm3
+++ b/sw/ext/libopencm3
@@ -1 +1 @@
-Subproject commit a12614054c126f36bafd4568eb2bf04316731053
+Subproject commit 0bf8324c9128b146ec6a18d62d8f05a5b61a2530
diff --git a/sw/tools/dfu/stm32_mem.py b/sw/tools/dfu/stm32_mem.py
old mode 100755
new mode 100644
index df2928de5f..9e23af6b02
--- a/sw/tools/dfu/stm32_mem.py
+++ b/sw/tools/dfu/stm32_mem.py
@@ -29,6 +29,7 @@ from optparse import OptionParser
import usb
import dfu
+import time
APP_ADDRESS = 0x08002000
SECTOR_SIZE = 2048
@@ -84,6 +85,8 @@ if __name__ == "__main__":
action="store", default="Lisa/Lia",
help="only upload to device where idProduct contains PRODUCT\n"
"choices: (any, Lisa/Lia), default: Lisa/Lia")
+ parser.add_option("--addr", type="int", action="store", dest="addr", default=APP_ADDRESS,
+ help="Upload start address (default: 0x08002000)")
parser.add_option("-n", "--dry-run", action="store_true",
help="Dry run to check which board is found without actually flashing.")
(options, args) = parser.parse_args()
@@ -99,7 +102,13 @@ if __name__ == "__main__":
if options.verbose:
print_copyright()
- devs = dfu.finddevs()
+ for i in range(1,60):
+ devs = dfu.finddevs()
+ if not devs:
+ print('.', end="")
+ stdout.flush()
+ time.sleep(0.5)
+ print("")
if not devs:
print("No DFU devices found!")
exit(1)
@@ -182,9 +191,13 @@ if __name__ == "__main__":
print("Could not open binary file.")
raise
- addr = APP_ADDRESS
+ #addr = APP_ADDRESS
+ addr = options.addr
+ print ("Programming memory from 0x%08X...\r" % addr)
+
while bin:
- print("Programming memory at 0x%08X\r" % addr)
+# print("Programming memory at 0x%08X\r" % addr),
+ print('#', end="")
stdout.flush()
stm32_erase(target, addr)
stm32_write(target, bin[:SECTOR_SIZE])