diff --git a/.gitmodules b/.gitmodules index 4aed20c866..2d95e748ad 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,9 +1,3 @@ -[submodule "sw/ext/libopencm3"] - path = sw/ext/libopencm3 - url = https://github.com/libopencm3/libopencm3.git -[submodule "sw/ext/luftboot"] - path = sw/ext/luftboot - url = https://github.com/paparazzi/luftboot.git [submodule "sw/ext/fatfs"] path = sw/ext/fatfs url = https://github.com/paparazzi/fatfs.git diff --git a/conf/Makefile.stm32 b/conf/Makefile.stm32 deleted file mode 100644 index 4e63266de3..0000000000 --- a/conf/Makefile.stm32 +++ /dev/null @@ -1,273 +0,0 @@ -# Hey Emacs, this is a -*- makefile -*- -# -# Copyright (C) 2009 Antoine Drouin -# -# 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. -# - -# -# This is the common Makefile for the stm32-target. -# - -SRC_ARCH = arch/stm32 - -# Pretty Printer -# Call with "make Q=''" to get full command display -Q=@ - - -# -# find compiler toolchain -# -include $(PAPARAZZI_SRC)/conf/Makefile.arm-embedded-toolchain - -ifeq ($(ARCH_L),f4) -MCU = cortex-m4 -else -MCU = cortex-m3 -endif -OPT ?= s -# Slightly bigger .elf files but gains the ability to decode macros -DEBUG_FLAGS ?= -ggdb3 - -CSTANDARD ?= -std=gnu99 -CXXSTANDARD ?= -std=c++0x -CINCS = $(INCLUDES) -I$(PAPARAZZI_SRC)/sw/include - -# add syscalls and c++ new/delete operators -$(TARGET).srcs += c++.cpp pprz_syscalls.c -$(TARGET).CXXFLAGS += -fno-sized-deallocation - -# input files -SRCS = $($(TARGET).srcs) -#ASRC = - -# object files -COBJ = $(SRCS:%.c=$(OBJDIR)/%.o) -OBJS = $(COBJ:%.cpp=$(OBJDIR)/%.o) -AOBJ = $(ASRC:%.S=$(OBJDIR)/%.o) - -# linker script : -# if LDSCRIPT is defined in the airframe use that independantly of TARGET -# if not, and a TARGET.LDSCRIPT is defined, use that -# if not, use the default STM32f103re_flash.ld -ifndef LDSCRIPT -ifndef $(TARGET).LDSCRIPT -$(warning Linker script for target "$(TARGET)" on board "$(BOARD)" not defined. Using stm32default.ld.) -LDSCRIPT = $(SRC_ARCH)/stm32default.ld -else -LDSCRIPT = $($(TARGET).LDSCRIPT) -$(info Using "$($(TARGET).LDSCRIPT)" as ldscript for target "$(TARGET)".) -endif -endif - -ifeq ($(ARCH_L), ) -FLOAT_ABI ?= -msoft-float -else ifeq ($(ARCH_L),f4) -ifndef HARD_FLOAT -FLOAT_ABI ?= -msoft-float -else -FLOAT_ABI ?= -mfloat-abi=softfp -mfpu=fpv4-sp-d16 -endif -endif - -ARCH_FLAGS ?= -mcpu=$(MCU) -mthumb - -# flags for warnings (C and C++) -WARN_FLAGS += -Wall -Wextra -Wunused -#WARN_FLAGS += -Wcast-qual -WARN_FLAGS += -Wcast-align -WARN_FLAGS += -Wpointer-arith -WARN_FLAGS += -Wswitch-default -WARN_FLAGS += -Wredundant-decls -Wmissing-declarations -WARN_FLAGS += -Wshadow - - -# common flags -CFLAGS += -O$(OPT) -CFLAGS += $(DEBUG_FLAGS) -CFLAGS += $(FLOAT_ABI) -CFLAGS += $(ARCH_FLAGS) -CFLAGS += $(USER_CFLAGS) -CFLAGS += $(WARN_FLAGS) -CXXFLAGS += -O$(OPT) -CXXFLAGS += $(DEBUG_FLAGS) -CXXFLAGS += $(FLOAT_ABI) -CXXFLAGS += $(ARCH_FLAGS) -CXXFLAGS += $(USER_CFLAGS) -CXXFLAGS += $(WARN_FLAGS) -CXXFLAGS += $(CINCS) -CXXFLAGS += $($(TARGET).CFLAGS) -CXXFLAGS += $(BOARD_CFLAGS) - - -CFLAGS += -I. -I./$(ARCH) -I../ext/libopencm3/include $(INCLUDES) -CFLAGS += -D__thumb2__ -CFLAGS += -Wl,--gc-sections -CFLAGS += -mfix-cortex-m3-ldrd -CFLAGS += $(CSTANDARD) -#CFLAGS += -malignment-traps -CFLAGS += -fno-common -CFLAGS += -ffunction-sections -fdata-sections -CFLAGS += -Wa,-adhlns=$(OBJDIR)/$(notdir $(subst $(suffix $<),.lst,$<)) -CFLAGS += -Wstrict-prototypes -Wmissing-prototypes -CFLAGS += -Wnested-externs -CFLAGS += $($(TARGET).CFLAGS) -CFLAGS += $(BOARD_CFLAGS) - -#CFLAGS += -fno-diagnostics-show-caret - -ifneq ($(ARCH_L), ) -ifeq ($(ARCH_L),f4) -CFLAGS += -DSTM32F4 -CXXFLAGS += -DSTM32F4 -endif -else -CFLAGS += -DSTM32F1 -CXXFLAGS += -DSTM32F1 -endif - -# with cortex-m3 and m4 unaligned data can still be read -CFLAGS += -DPPRZLINK_UNALIGNED_ACCESS=1 - - -# C++ only flags -CXXFLAGS += $(CXXSTANDARD) -CXXFLAGS += -I. -I./$(ARCH) -I../ext/libopencm3/include $(INCLUDES) -CXXFLAGS += -pipe -fshow-column -CXXFLAGS += -ffunction-sections -fdata-sections -CXXFLAGS += $($(TARGET).CXXFLAGS) - - -AFLAGS = -ahls -mapcs-32 -AFLAGS += -mcpu=$(MCU) -mthumb -AFLAGS += -x assembler-with-cpp -Wa,-adhlns=$(OBJDIR)/$(<:.S=.lst) - -LDFLAGS += -L../ext/libopencm3/lib -LDFLAGS += -T$(LDSCRIPT) -nostartfiles -O$(OPT) -mthumb -mcpu=$(MCU) -LDFLAGS += $(BOARD_LDFLAGS) - -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=softfp -mfpu=fpv4-sp-d16 -endif -endif - -LDFLAGS += -Wl,-Map=$(OBJDIR)/$(TARGET).map,--cref,--gc-sections -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 -CPFLAGS_HEX = -Oihex - -ODFLAGS = -S - -# some common informative targets -include $(PAPARAZZI_SRC)/conf/Makefile.arm-embedded-common - -# Default target. -all: printcommands sizebefore build sizeafter - -# depend order only for parallel make -sizebefore: | printcommands -build: | printcommands sizebefore -sizeafter: | build - -build: $(OBJDIR) elf bin hex -# lss sym - -$(OBJDIR): - @echo CREATING object dir $(OBJDIR) - @test -d $(OBJDIR) || mkdir -p $(OBJDIR) - -elf: $(OBJDIR)/$(TARGET).elf -bin: $(OBJDIR)/$(TARGET).bin -hex: $(OBJDIR)/$(TARGET).hex -lss: $(OBJDIR)/$(TARGET).lss -sym: $(OBJDIR)/$(TARGET).sym - -%.bin: %.elf - @echo OBJCB $@ - $(Q)$(CP) $(CPFLAGS) $(CPFLAGS_BIN) $< $@ - -%.hex: %.elf - @echo OBJCH $@ - $(Q)$(CP) $(CPFLAGS) $(CPFLAGS_HEX) $< $@ - -# Create extended listing file from ELF output file. -# testing: option -C -%.lss: %.elf - @echo OBJD $@ - $(Q)$(DMP) -h -S -C $< > $@ - - -# Create a symbol table from ELF output file. -%.sym: %.elf - @echo NM $@ - $(Q)$(NM) -n $< > $@ - - -# Link: create ELF output file from object files. -.SECONDARY : $(OBJDIR)/$(TARGET).elf -.PRECIOUS : $(OBJS) $(AOBJ) -%.elf: $(OBJS) $(AOBJ) | $(OBJDIR) - @echo LD $@ - $(Q)$(LD) $(LDFLAGS) $($(TARGET).LDFLAGS) -o $@ $(OBJS) $(AOBJ) $(LDLIBS) - -# Compile: create object files from C source files. -$(OBJDIR)/%.o : %.c $(OBJDIR)/../Makefile.ac - @echo CC $@ - $(Q)test -d $(dir $@) || mkdir -p $(dir $@) - $(Q)$(CC) -MMD -c $(CFLAGS) $< -o $@ - -# Compile: create object files from C++ source files -$(OBJDIR)/%.o : %.cpp $(OBJDIR)/../Makefile.ac - @echo CXX $@ - $(Q)test -d $(dir $@) || mkdir -p $(dir $@) - $(Q)$(CXX) -MMD -c $(CXXFLAGS) $< -o $@ - -# Assemble: create object files from assembler source files. ARM/Thumb -$(AOBJ) : $(OBJDIR)/%.o : %.S - @echo AS $@ - $(Q)test -d $(dir $@) || mkdir -p $(dir $@) - $(Q)$(CC) -c $(AFLAGS) $< -o $@ - -# Load upload rules -include $(PAPARAZZI_SRC)/conf/Makefile.stm32-upload - -# Listing of phony targets. -.PHONY : all build elf bin lss sym - -# -# Dependencies -# -ifneq ($(MAKECMDGOALS),clean) -DEPS = $(addprefix $(OBJDIR)/,$($(TARGET).srcs:.c=.d)) -DEPS += $(addprefix $(OBJDIR)/,$($(TARGET).srcs:.cpp=.d)) --include $(DEPS) -endif diff --git a/sw/airborne/arch/stm32/TIM_usage_list.txt b/sw/airborne/arch/stm32/TIM_usage_list.txt deleted file mode 100644 index 8e507dd9eb..0000000000 --- a/sw/airborne/arch/stm32/TIM_usage_list.txt +++ /dev/null @@ -1,30 +0,0 @@ - -timer subsystem/type (config options) --------------------------------------------- -(advanced timers using RCC_APB1) -TIM1 adc (if USE_AD_TIM1) - radio_control/ppm (if USE_PPM_TIM1, PPM input on SERVO6 pin) - actuators/pwm (if PWM_USE_TIM1) - -TIM8 radio_control/ppm (if USE_PPM_TIM8) - -(non-advanced timers using RCC_APB2) -TIM2 adc (if USE_AD_TIM2, default) - radio_control/ppm (if USE_PPM_TIM2, PPM input on UART1_RX pin) - actuators/pwm (if PWM_USE_TIM2) - -TIM3 actuators/pwm (if PWM_USE_TIM3, PWM1..4 on LisaM) - -TIM4 adc (if USE_AD_TIM4) - actuators/pwm (PWM6..7 on LisaL) - (PWM7..8) (if USE_SERVOS_7AND8) - -TIM5 actuators/pwm (PWM/Servos 5..6 on LisaM) - -TIM6 radio_control/spektrum - -TIM7 none - -TIM9 actuators/pwm (if PWM_USE_TIM9) - -TIM12 actuators/pwm (if PWM_USE_TIM12) diff --git a/sw/airborne/arch/stm32/apogee.ld b/sw/airborne/arch/stm32/apogee.ld deleted file mode 100644 index 350be841d3..0000000000 --- a/sw/airborne/arch/stm32/apogee.ld +++ /dev/null @@ -1,34 +0,0 @@ -/* - * 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 (STM32F405RGT6, 1024K flash, 192K RAM). */ - -/* Define memory regions. */ -MEMORY -{ - /* only 128K (SRAM1 and SRAM2) are accessible by all AHB masters. */ - ram (rwx) : ORIGIN = 0x20000000, LENGTH = 128K - /* Leaving 128k of space at the end of rom for persistent settings */ - rom (rx) : ORIGIN = 0x08000000, LENGTH = 896K -} - -/* Include the common ld script. */ -INCLUDE cortex-m-generic.ld diff --git a/sw/airborne/arch/stm32/krooz.ld b/sw/airborne/arch/stm32/krooz.ld deleted file mode 100644 index abd47f9975..0000000000 --- a/sw/airborne/arch/stm32/krooz.ld +++ /dev/null @@ -1,36 +0,0 @@ -/* - * 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 -{ - /* Only 128K (SRAM1 and SRAM2) are accessible by all AHB masters. */ - ram (rwx) : ORIGIN = 0x20000000, LENGTH = 128K - /* Leaving 128k of space at the end of rom for persistent settings */ - rom (rx) : ORIGIN = 0x08004000, LENGTH = 896K -} - -/* Include the common ld script. */ -INCLUDE cortex-m-generic.ld diff --git a/sw/airborne/arch/stm32/led_hw.c b/sw/airborne/arch/stm32/led_hw.c deleted file mode 100644 index 49628b5785..0000000000 --- a/sw/airborne/arch/stm32/led_hw.c +++ /dev/null @@ -1,6 +0,0 @@ -#include "led.h" - -#ifdef LED_STP08 -uint8_t led_status[NB_LED]; -#endif - diff --git a/sw/airborne/arch/stm32/led_hw.h b/sw/airborne/arch/stm32/led_hw.h deleted file mode 100644 index 40cf4f7f85..0000000000 --- a/sw/airborne/arch/stm32/led_hw.h +++ /dev/null @@ -1,108 +0,0 @@ -/* - * Copyright (C) 2009 Antoine Drouin - * - * 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. - */ - -#ifndef LED_HW_H -#define LED_HW_H - -#include "mcu_periph/gpio.h" -#include -#include - -#include BOARD_CONFIG - -#include "std.h" - -/* - * - * Regular GPIO driven LEDs - * - */ -#ifndef LED_STP08 - -#define _LED_EVAL(i) i - -#define LED_GPIO(i) _LED_EVAL(LED_ ## i ## _GPIO) -#define LED_GPIO_PIN(i) _LED_EVAL(LED_ ## i ## _GPIO_PIN) -#define LED_GPIO_ON(i) _LED_EVAL(LED_ ## i ## _GPIO_ON) -#define LED_GPIO_OFF(i) _LED_EVAL(LED_ ## i ## _GPIO_OFF) -#define LED_AFIO_REMAP(i) _LED_EVAL(LED_ ## i ## _AFIO_REMAP) - - -#define LED_INIT(i) { \ - gpio_setup_output(LED_GPIO(i), LED_GPIO_PIN(i)); \ - LED_AFIO_REMAP(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_DISABLE(i) gpio_setup_input(LED_GPIO(i), LED_GPIO_PIN(i)) - -#define LED_PERIODIC() {} - - -/* - * - * Shift register driven LEDs - * - */ -#else /* LED_STP08 */ -#define NB_LED 8 -extern uint8_t led_status[NB_LED]; -/* Lisa/L uses a shift register for driving LEDs */ -/* PA8 led_clk */ -/* PC15 led_data */ - -#define LED_INIT(_i) { \ - rcc_periph_clock_enable(RCC_GPIOA); \ - rcc_periph_clock_enable(RCC_GPIOC); \ - gpio_set_mode(GPIOA, \ - GPIO_MODE_OUTPUT_50_MHZ, \ - GPIO_CNF_OUTPUT_PUSHPULL, \ - GPIO8); \ - gpio_set_mode(GPIOC, \ - GPIO_MODE_OUTPUT_50_MHZ, \ - GPIO_CNF_OUTPUT_PUSHPULL, \ - GPIO15); \ - for(uint8_t _cnt=0; _cnt - * - * 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 Lisa-L (STM32F103RET6, 512K flash, 64K RAM). */ - -/* Define memory regions. */ -MEMORY -{ - ram (rwx) : ORIGIN = 0x20000000, LENGTH = 64K - /* Leaving 2k of space at the end of rom for stored settings */ - rom (rx) : ORIGIN = 0x08000000, LENGTH = 510K -} - -/* Include the common ld script. */ -INCLUDE cortex-m-generic.ld diff --git a/sw/airborne/arch/stm32/lisa-m.ld b/sw/airborne/arch/stm32/lisa-m.ld deleted file mode 100644 index fd07adfdde..0000000000 --- a/sw/airborne/arch/stm32/lisa-m.ld +++ /dev/null @@ -1,35 +0,0 @@ -/* - * Hey Emacs, this is a -*- makefile -*- - * - * Copyright (C) 2012 Piotr Esden-Tempski - * - * 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 Lisa-M (STM32F105RCT6, 256K flash, 64K RAM). */ - -/* Define memory regions. */ -MEMORY -{ - ram (rwx) : ORIGIN = 0x20000000, LENGTH = 64K - /* Leaving 2k of space at the end of rom for stored settings */ - rom (rx) : ORIGIN = 0x08000000, LENGTH = 254K -} - -/* Include the common ld script. */ -INCLUDE cortex-m-generic.ld diff --git a/sw/airborne/arch/stm32/lisa-mx.ld b/sw/airborne/arch/stm32/lisa-mx.ld deleted file mode 100644 index f3fc1cdf8c..0000000000 --- a/sw/airborne/arch/stm32/lisa-mx.ld +++ /dev/null @@ -1,35 +0,0 @@ -/* - * 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 Lisa/MX (STM32F415, 1024K flash, 192K RAM). */ - -/* Define memory regions. */ -MEMORY -{ - ram (rwx) : ORIGIN = 0x20000000, LENGTH = 128K - /* Reserving 128kb flash for persistent settings. */ - rom (rx) : ORIGIN = 0x08000000, LENGTH = 896K -} - -/* Include the common ld script. */ -INCLUDE cortex-m-generic.ld diff --git a/sw/airborne/arch/stm32/mcu_arch.c b/sw/airborne/arch/stm32/mcu_arch.c deleted file mode 100644 index 3f1be15fc6..0000000000 --- a/sw/airborne/arch/stm32/mcu_arch.c +++ /dev/null @@ -1,362 +0,0 @@ -/* - * Copyright (C) 2010-2012 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 arch/stm32/mcu_arch.c - * @brief stm32 arch dependant microcontroller initialisation functions. - * @ingroup stm32_arch - */ - -#include "mcu.h" - -#include BOARD_CONFIG - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "std.h" - -#if defined(STM32F4) -/* untested, should go into libopencm3 */ -const struct rcc_clock_scale rcc_hse_24mhz_3v3[RCC_CLOCK_3V3_END] = { - { /* 48MHz */ - .pllm = 24, - .plln = 96, - .pllp = 2, - .pllq = 2, - .hpre = RCC_CFGR_HPRE_DIV_NONE, - .ppre1 = RCC_CFGR_PPRE_DIV_4, - .ppre2 = RCC_CFGR_PPRE_DIV_2, - .voltage_scale = PWR_SCALE1, - .flash_config = FLASH_ACR_ICEN | FLASH_ACR_DCEN | FLASH_ACR_LATENCY_3WS, - .ahb_frequency = 48000000, - .apb1_frequency = 12000000, - .apb2_frequency = 24000000, - }, - { /* 84MHz */ - .pllm = 24, - .plln = 336, - .pllp = 4, - .pllq = 7, - .hpre = RCC_CFGR_HPRE_DIV_NONE, - .ppre1 = RCC_CFGR_PPRE_DIV_2, - .ppre2 = RCC_CFGR_PPRE_DIV_NONE, - .voltage_scale = PWR_SCALE1, - .flash_config = FLASH_ACR_ICEN | FLASH_ACR_DCEN | FLASH_ACR_LATENCY_2WS, - .ahb_frequency = 84000000, - .apb1_frequency = 42000000, - .apb2_frequency = 84000000, - }, - { /* 120MHz */ - .pllm = 24, - .plln = 240, - .pllp = 2, - .pllq = 5, - .hpre = RCC_CFGR_HPRE_DIV_NONE, - .ppre1 = RCC_CFGR_PPRE_DIV_4, - .ppre2 = RCC_CFGR_PPRE_DIV_2, - .voltage_scale = PWR_SCALE1, - .flash_config = FLASH_ACR_ICEN | FLASH_ACR_DCEN | FLASH_ACR_LATENCY_3WS, - .ahb_frequency = 120000000, - .apb1_frequency = 30000000, - .apb2_frequency = 60000000, - }, - { /* 168MHz */ - .pllm = 24, - .plln = 336, - .pllp = 2, - .pllq = 7, - .hpre = RCC_CFGR_HPRE_DIV_NONE, - .ppre1 = RCC_CFGR_PPRE_DIV_4, - .ppre2 = RCC_CFGR_PPRE_DIV_2, - .flash_config = FLASH_ACR_ICEN | FLASH_ACR_DCEN | FLASH_ACR_LATENCY_5WS, - .ahb_frequency = 168000000, - .apb1_frequency = 42000000, - .apb2_frequency = 84000000, - }, -}; -#endif - -#if defined(STM32F1) -/*---------------------------------------------------------------------------*/ -/** @brief RCC Set System Clock HSE at 24MHz from HSE at 24MHz - -*/ -void rcc_clock_setup_in_hse_24mhz_out_24mhz_pprz(void); -void rcc_clock_setup_in_hse_24mhz_out_24mhz_pprz(void) -{ - /* Enable internal high-speed oscillator. */ - rcc_osc_on(RCC_HSI); - rcc_wait_for_osc_ready(RCC_HSI); - - /* Select HSI as SYSCLK source. */ - rcc_set_sysclk_source(RCC_CFGR_SW_SYSCLKSEL_HSICLK); - - /* Enable external high-speed oscillator 24MHz. */ - rcc_osc_on(RCC_HSE); - rcc_wait_for_osc_ready(RCC_HSE); - rcc_set_sysclk_source(RCC_CFGR_SW_SYSCLKSEL_HSECLK); - - /* - * Set prescalers for AHB, ADC, ABP1, ABP2. - * Do this before touching the PLL (TODO: why?). - */ - rcc_set_hpre(RCC_CFGR_HPRE_SYSCLK_NODIV); /* Set. 24MHz Max. 72MHz */ - rcc_set_adcpre(RCC_CFGR_ADCPRE_PCLK2_DIV2); /* Set. 12MHz Max. 14MHz */ - rcc_set_ppre1(RCC_CFGR_PPRE1_HCLK_NODIV); /* Set. 24MHz Max. 36MHz */ - rcc_set_ppre2(RCC_CFGR_PPRE2_HCLK_NODIV); /* Set. 24MHz Max. 72MHz */ - - /* - * Sysclk runs with 24MHz -> 0 waitstates. - * 0WS from 0-24MHz - * 1WS from 24-48MHz - * 2WS from 48-72MHz - */ - flash_set_ws(FLASH_ACR_LATENCY_0WS); - - /* - * Set the PLL multiplication factor to 2. - * 24MHz (external) * 2 (multiplier) / 2 (RCC_CFGR_PLLXTPRE_HSE_CLK_DIV2) = 24MHz - */ - rcc_set_pll_multiplication_factor(RCC_CFGR_PLLMUL_PLL_CLK_MUL2); - - /* Select HSE as PLL source. */ - rcc_set_pll_source(RCC_CFGR_PLLSRC_HSE_CLK); - - /* - * External frequency divide by 2 before entering PLL - * (only valid/needed for HSE). - */ - rcc_set_pllxtpre(RCC_CFGR_PLLXTPRE_HSE_CLK_DIV2); - - rcc_osc_on(RCC_PLL); - rcc_wait_for_osc_ready(RCC_PLL); - - /* Select PLL as SYSCLK source. */ - rcc_set_sysclk_source(RCC_CFGR_SW_SYSCLKSEL_PLLCLK); - - /* Set the peripheral clock frequencies used */ - rcc_ahb_frequency = 24000000; - rcc_apb1_frequency = 24000000; - rcc_apb2_frequency = 24000000; -} -#endif - - -#ifdef SYSTEM_MEMORY_BASE -void reset_to_dfu(void) { - // Request DFU at boot (init_dfu) - pwr_disable_backup_domain_write_protect(); - RTC_BKPXR(0) = 0xFF; - pwr_enable_backup_domain_write_protect(); - // Reset - scb_reset_system(); -} - -typedef void resetHandler_t(void); - -typedef struct isrVector_s { - volatile uint32_t stackEnd; - resetHandler_t *resetHandler; -} isrVector_t; - -static isrVector_t *system_isr_vector_table_base = (isrVector_t *) SYSTEM_MEMORY_BASE; // Find in ST AN2606. Defined in board header. - -static void init_dfu(void) { - /* Reset to DFU if requested */ - rcc_periph_clock_enable(RCC_RTC); - rcc_periph_clock_enable(RCC_PWR); - if (RTC_BKPXR(0) == 0xFF) { - // DFU request - // 0. Reset request - pwr_disable_backup_domain_write_protect(); - RTC_BKPXR(0) = 0x00; - pwr_enable_backup_domain_write_protect(); - // 1. Set MSP to system_isr_vector_table_base.stackEnd - // (betaflight system_stm32f4xx.c::75) - // (betaflight cmsis_armcc.h::226 - register uint32_t __regMainStackPointer __asm("msp") __attribute__((unused)); // Note: declared unused to suppress gcc warning, not actually unused! - __regMainStackPointer = system_isr_vector_table_base->stackEnd; // = topOfMainStack; - // 2. system_isr_vector_table_base.resetHandler() (betaflight system_stm32f4xx.c::76) - system_isr_vector_table_base->resetHandler(); - while (1); - } -} -#endif // SYSTEM_MEMORY_BASE - - -void mcu_arch_init(void) -{ -#ifdef SYSTEM_MEMORY_BASE - init_dfu(); -#endif - -#if LUFTBOOT - PRINT_CONFIG_MSG("We are running luftboot, the interrupt vector is being relocated.") -#if defined STM32F4 - SCB_VTOR = 0x00004000; -#else - SCB_VTOR = 0x00002000; -#endif -#endif -#if EXT_CLK == 8000000 -#if defined(STM32F1) - PRINT_CONFIG_MSG("Using 8MHz external clock to PLL it to 72MHz.") - rcc_clock_setup_pll(&rcc_hse_configs[RCC_CLOCK_HSE8_72MHZ]); -#elif defined(STM32F4) -#if AHB_CLK == 84000000 - PRINT_CONFIG_MSG("Using 8MHz external clock to PLL it to 84MHz.") - rcc_clock_setup_pll(&rcc_hse_8mhz_3v3[RCC_CLOCK_3V3_84MHZ]); -#else - PRINT_CONFIG_MSG("Using 8MHz external clock to PLL it to 168MHz.") - rcc_clock_setup_pll(&rcc_hse_8mhz_3v3[RCC_CLOCK_3V3_168MHZ]); -#endif -#endif -#elif EXT_CLK == 12000000 -#if defined(STM32F1) - PRINT_CONFIG_MSG("Using 12MHz external clock to PLL it to 72MHz.") - rcc_clock_setup_pll(&rcc_hse_configs[RCC_CLOCK_HSE12_72MHZ]); -#elif defined(STM32F4) - PRINT_CONFIG_MSG("Using 12MHz external clock to PLL it to 168MHz.") - rcc_clock_setup_pll(&rcc_hse_12mhz_3v3[RCC_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_pll(&rcc_hse_16mhz_3v3[RCC_CLOCK_3V3_168MHZ]); -#endif -#elif EXT_CLK == 24000000 -#if defined(STM32F4) - PRINT_CONFIG_MSG("Using 24MHz external clock to PLL it to 168MHz.") - rcc_clock_setup_pll(&rcc_hse_24mhz_3v3[RCC_CLOCK_3V3_168MHZ]); -#elif defined(STM32F1) - rcc_clock_setup_in_hse_24mhz_out_24mhz_pprz(); -#endif -#elif EXT_CLK == 25000000 -#if defined(STM32F4) - PRINT_CONFIG_MSG("Using 25MHz external clock to PLL it to 168MHz.") - rcc_clock_setup_pll(&rcc_hse_25mhz_3v3[RCC_CLOCK_3V3_168MHZ]); -#endif -#else -#error EXT_CLK is either set to an unsupported frequency or not defined at all. Please check! -#endif - - /* Configure priority grouping 0 bits for pre-emption priority and 4 bits for sub-priority. - * this was previously in i2c driver - * FIXME is it really needed ? - */ - scb_set_priority_grouping(SCB_AIRCR_PRIGROUP_NOGROUP_SUB16); -} - -#if defined(STM32F1) -#define RCC_CFGR_PPRE2_SHIFT 11 -#define RCC_CFGR_PPRE2 (7 << RCC_CFGR_PPRE2_SHIFT) - -#define RCC_CFGR_PPRE1_SHIFT 8 -#define RCC_CFGR_PPRE1 (7 << RCC_CFGR_PPRE1_SHIFT) - -static inline uint32_t rcc_get_ppre1(void) -{ - return RCC_CFGR & RCC_CFGR_PPRE1; -} - -static inline uint32_t rcc_get_ppre2(void) -{ - return RCC_CFGR & RCC_CFGR_PPRE2; -} -#elif defined(STM32F4) -static inline uint32_t rcc_get_ppre1(void) -{ - return RCC_CFGR & ((1 << 10) | (1 << 11) | (1 << 12)); -} - -static inline uint32_t rcc_get_ppre2(void) -{ - return RCC_CFGR & ((1 << 13) | (1 << 14) | (1 << 15)); -} -#endif - -/** @brief Get Timer clock frequency (before prescaling) - * Only valid if using the internal clock for the timer. - * Currently implemented for STM32F1x and STM32F405xx/407xx STM32F415xx/417xx. - * Not valid for STM32F42xxx and STM32F43xxx. - * @param[in] timer_peripheral Unsigned int32. Timer register address base - * @return Unsigned int32. Timer base frequency - */ -uint32_t timer_get_frequency(uint32_t timer_peripheral) -{ - switch (timer_peripheral) { - // Timers on high speed APB2 - case TIM1: - case TIM8: -#ifdef TIM9 - case TIM9: -#endif -#ifdef TIM10 - case TIM10: -#endif -#ifdef TIM11 - case TIM11: -#endif - if (!rcc_get_ppre2()) { - /* without APB2 prescaler, runs at APB2 freq */ - return rcc_apb2_frequency; - } else { - /* with any ABP2 prescaler, runs at 2 * APB2 freq */ - return rcc_apb2_frequency * 2; - } - - // timers on low speed APB1 - case TIM2: - case TIM3: - case TIM4: - case TIM5: - case TIM6: - case TIM7: -#ifdef TIM12 - case TIM12: -#endif -#ifdef TIM13 - case TIM13: -#endif -#ifdef TIM14 - case TIM14: -#endif - if (!rcc_get_ppre1()) { - /* without APB1 prescaler, runs at APB1 freq */ - return rcc_apb1_frequency; - } else { - /* with any ABP1 prescaler, runs at 2 * APB1 freq */ - return rcc_apb1_frequency * 2; - } - default: - // other timers currently not supported - break; - } - return 0; -} diff --git a/sw/airborne/arch/stm32/mcu_arch.h b/sw/airborne/arch/stm32/mcu_arch.h deleted file mode 100644 index b411dd1d11..0000000000 --- a/sw/airborne/arch/stm32/mcu_arch.h +++ /dev/null @@ -1,56 +0,0 @@ -/* - * Copyright (C) 2010-2012 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 arch/stm32/mcu_arch.h - * @brief stm32 arch dependant microcontroller initialisation functions. - * @addtogroup stm32_arch STM32 architecture - */ - -#ifndef STM32_MCU_ARCH_H -#define STM32_MCU_ARCH_H - -#include "std.h" - -#include BOARD_CONFIG - -extern void mcu_arch_init(void); - -#ifdef SYSTEM_MEMORY_BASE -extern void reset_to_dfu(void); -#endif - - -/* should probably not be here - * a couple of macros to use the rev instruction - */ -#define MyByteSwap16(in, out) { \ - asm volatile ( \ - "rev16 %0, %1\n\t" \ - : "=r" (out) \ - : "r"(in) \ - ); \ - } - -uint32_t timer_get_frequency(uint32_t timer_peripheral); - -#endif /* STM32_MCU_ARCH_H */ diff --git a/sw/airborne/arch/stm32/mcu_periph/adc_arch.c b/sw/airborne/arch/stm32/mcu_periph/adc_arch.c deleted file mode 100644 index 9bef5bec4e..0000000000 --- a/sw/airborne/arch/stm32/mcu_periph/adc_arch.c +++ /dev/null @@ -1,672 +0,0 @@ -/* - * 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 arch/stm32/mcu_periph/adc_arch.c - * @ingroup stm32_arch - * - * Driver for the analog to digital converters on STM32. - * - * Usage: - * Define flags for ADCs to use (can be explicitly disabled by setting to 0): - * - * -DUSE_ADC_1 -DUSE_ADC_3=1 -DUSE_ADC_4=0 - * - * would explicitly enable the ADC_1 and ADC_3 and disable ADC_4. - * - * The mapping of these virtual "board" ADC_x numbers to a concrete AD converter - * and channel is done in the sw/airborne/boards/.h header files. - * Some ADCs are normally already enabled in the board files per default - * (e.g. for battery voltage measurement). - * - */ - -/* - For better understanding of timer and GPIO settings: - - Table of GPIO pins available per ADC: - - ADC1/2: ADC3: - C0 -> PA0 C0 -> PA0 - C1 -> PA1 C1 -> PA1 - C2 -> PA2 C2 -> PA2 - C3 -> PA3 C3 -> PA3 - C4 -> PA4 C4 -> PF6 - C5 -> PA5 C5 -> PF7 - C6 -> PA6 C6 -> PF8 - C7 -> PA7 C7 -> PF9 - C8 -> PB0 C8 -> PF10 - C9 -> PB1 - C10 -> PC0 C10 -> PC0 - C11 -> PC1 C11 -> PC1 - C12 -> PC2 C12 -> PC2 - C13 -> PC3 C13 -> PC3 - C14 -> PC4 - C15 -> PC5 - - Table of timers available per ADC (from libstm/src/stm32_adc.c): - - T1_TRGO: Timer1 TRGO event (ADC1, ADC2 and ADC3) - T1_CC4: Timer1 capture compare4 (ADC1, ADC2 and ADC3) - T2_TRGO: Timer2 TRGO event (ADC1 and ADC2) - T2_CC1: Timer2 capture compare1 (ADC1 and ADC2) - T3_CC4: Timer3 capture compare4 (ADC1 and ADC2) - T4_TRGO: Timer4 TRGO event (ADC1 and ADC2) - TIM8_CC4: External interrupt line 15 or Timer8 capture compare4 event (ADC1 and ADC2) - T4_CC3: Timer4 capture compare3 (ADC3 only) - T8_CC2: Timer8 capture compare2 (ADC3 only) - T8_CC4: Timer8 capture compare4 (ADC3 only) - T5_TRGO: Timer5 TRGO event (ADC3 only) - T5_CC4: Timer5 capture compare4 (ADC3 only) - - By setting ADC_ExternalTrigInjecConv_None, injected conversion - is started by software instead of external trigger for any ADC. - - Table of APB per Timer (from libstm/src/stm32_tim.c): - - RCC_APB1: TIM2, TIM3, TIM4, TIM5, TIM7 (non-advanced timers) - RCC_APB2: TIM1, TIM8 (advanced timers) - -*/ - -#include "mcu_periph/adc.h" - -#include -#include -#include -#include -#include -#include -#include "mcu_periph/gpio.h" -#include "mcu_arch.h" -#include "std.h" -#include BOARD_CONFIG - - -#ifndef NVIC_ADC_IRQ_PRIO -#define NVIC_ADC_IRQ_PRIO 0 -#endif - -#if defined(STM32F1) -#define ADC_SAMPLE_TIME ADC_SMPR_SMP_41DOT5CYC -#elif defined(STM32F4) -#define ADC_SAMPLE_TIME ADC_SMPR_SMP_56CYC -#endif - -// Macros to automatically enable the correct ADC - -#if defined(AD1_1_CHANNEL) || defined(AD1_2_CHANNEL) || defined(AD1_3_CHANNEL) || defined(AD1_4_CHANNEL) -#ifndef USE_AD1 -#define USE_AD1 1 -#endif -#endif - -#if defined(AD2_1_CHANNEL) || defined(AD2_2_CHANNEL) || defined(AD2_3_CHANNEL) || defined(AD2_4_CHANNEL) -#ifndef USE_AD2 -#define USE_AD2 1 -#endif -#endif - -#if defined(STM32F4) - -#if defined(AD3_1_CHANNEL) || defined(AD3_2_CHANNEL) || defined(AD3_3_CHANNEL) || defined(AD3_4_CHANNEL) -#ifndef USE_AD3 -#define USE_AD3 1 -#endif -#endif - -#define RST_ADC1 RST_ADC -#define RST_ADC2 RST_ADC -#define RST_ADC3 RST_ADC - -#else // !STM32F4 -// ADC 3 not supported on STM32F1 -#undef USE_AD3 -#define USE_AD3 0 -#endif - -#if USE_AD1 -PRINT_CONFIG_MSG("Analog to Digital Coverter 1 active") -#endif -#if USE_AD2 -PRINT_CONFIG_MSG("Analog to Digital Coverter 2 active") -#endif -#if USE_AD3 -PRINT_CONFIG_MSG("Analog to Digital Coverter 3 active") -#endif -#if !USE_AD1 && !USE_AD2 && !USE_AD3 && !defined FBW -#warning ALL ADC CONVERTERS INACTIVE -#endif - -#ifndef ADC_TIMER_PERIOD -#define ADC_TIMER_PERIOD 10000 -#endif - -/** Timer frequency for ADC - * Timer will trigger an update event after reaching the period reload value. - * New conversion is triggered on update event. - * ADC measuerement frequency is hence ADC_TIMER_FREQUENCY / ADC_TIMER_PERIOD. - */ -#ifndef ADC_TIMER_FREQUENCY -#define ADC_TIMER_FREQUENCY 2000000 -#endif - -/***************************************/ -/*** STATIC FUNCTION PROTOTYPES ***/ -/***************************************/ - -static inline void adc_init_single(uint32_t adc, uint8_t nb_channels, uint8_t *channel_map); - -static inline void adc_push_sample(struct adc_buf *buf, - uint16_t sample); - -static inline void adc_init_rcc(void); -static inline void adc_init_irq(void); - - -/********************************/ -/*** GLOBAL VARIABLES ***/ -/********************************/ - -/* Only 4 ADC channels may be enabled at the same time - * on each ADC, as there are only 4 injection registers. - * Currently, the enums adc1_channels and adc2_channels only - * serve to resolve the number of channels on each ADC. - * There are 3 separate buffer lists, each holds the addresses of the actual adc buffers - * for the particular adc converter. - */ - -static uint8_t nb_adc1_channels = 0; -static uint8_t nb_adc2_channels = 0; -static uint8_t nb_adc3_channels = 0; - -#if USE_AD1 || USE_AD2 || USE_AD3 -#define ADC_NUM_CHANNELS 4 -#endif - -#if USE_AD1 -/// List of buffers, one for each active channel. -static struct adc_buf *adc1_buffers[ADC_NUM_CHANNELS]; -#endif -#if USE_AD2 -/// List of buffers, one for each active channel. -static struct adc_buf *adc2_buffers[ADC_NUM_CHANNELS]; -#endif -#if USE_AD3 -/// List of buffers, one for each active channel. -static struct adc_buf *adc3_buffers[ADC_NUM_CHANNELS]; -#endif - -#if USE_ADC_WATCHDOG -#include "mcu_periph/sys_time.h" -// watchdog structure with adc bank and callback -static struct { - uint32_t timeStamp; - uint32_t adc; - adc_watchdog_callback cb; -} adc_watchdog; -#endif - -/***************************************/ -/*** PUBLIC FUNCTION DEFINITIONS ***/ -/***************************************/ - -void adc_init(void) -{ -#if USE_AD1 || USE_AD2 || USE_AD3 - uint8_t x = 0; - - // ADC channel mapping - uint8_t adc_channel_map[4]; -#endif - - /* Init GPIO ports for ADC operation - */ -#if USE_ADC_1 - PRINT_CONFIG_MSG("Info: Using ADC_1") - gpio_setup_pin_analog(ADC_1_GPIO_PORT, ADC_1_GPIO_PIN); -#endif -#if USE_ADC_2 - PRINT_CONFIG_MSG("Info: Using ADC_2") - gpio_setup_pin_analog(ADC_2_GPIO_PORT, ADC_2_GPIO_PIN); -#endif -#if USE_ADC_3 - PRINT_CONFIG_MSG("Info: Using ADC_3") - gpio_setup_pin_analog(ADC_3_GPIO_PORT, ADC_3_GPIO_PIN); -#endif -#if USE_ADC_4 - PRINT_CONFIG_MSG("Info: Using ADC_4") - gpio_setup_pin_analog(ADC_4_GPIO_PORT, ADC_4_GPIO_PIN); -#endif -#if USE_ADC_5 - PRINT_CONFIG_MSG("Info: Using ADC_5") - gpio_setup_pin_analog(ADC_5_GPIO_PORT, ADC_5_GPIO_PIN); -#endif -#if USE_ADC_6 - PRINT_CONFIG_MSG("Info: Using ADC_6") - gpio_setup_pin_analog(ADC_6_GPIO_PORT, ADC_6_GPIO_PIN); -#endif -#if USE_ADC_7 - PRINT_CONFIG_MSG("Info: Using ADC_7") - gpio_setup_pin_analog(ADC_7_GPIO_PORT, ADC_7_GPIO_PIN); -#endif -#if USE_ADC_8 - PRINT_CONFIG_MSG("Info: Using ADC_8") - gpio_setup_pin_analog(ADC_8_GPIO_PORT, ADC_8_GPIO_PIN); -#endif -#if USE_ADC_9 - PRINT_CONFIG_MSG("Info: Using ADC_9") - gpio_setup_pin_analog(ADC_9_GPIO_PORT, ADC_9_GPIO_PIN); -#endif - - // Init clock and irq - adc_init_rcc(); - adc_init_irq(); - - /* If fewer than 4 channels are active, say 3, then they are placed in to - * injection slots 2,3 and 4 because the stm32 architecture converts injected - * slots 2,3 and 4 and skips slot 1 instead of logicaly converting slots 1,2 - * and 3 and leave slot 4. - * EXAMPLE OF ADC EXECUTION ORDER WHEN WE HAVE SAY 2 ADC INPUTS USED on ADC1 - * The first board adc channel ADC1_1 is mapped to injected channel 3 and ADC1_2 - * to injected channel 4 and because the conversions start from the lowest - * injection channel used, 3 in our case, injected channel 3 data will be - * located at JDR1 and 4 to JDR2 so JDR1 = ADC1_1 and JDR2 = ADC1_2. - * That's why "adc_channel_map" has this descending order. - */ - - nb_adc1_channels = 0; -#if USE_AD1 -#ifdef AD1_1_CHANNEL - adc_channel_map[AD1_1] = AD1_1_CHANNEL; - nb_adc1_channels++; -#endif -#ifdef AD1_2_CHANNEL - adc_channel_map[AD1_2] = AD1_2_CHANNEL; - nb_adc1_channels++; -#endif -#ifdef AD1_3_CHANNEL - adc_channel_map[AD1_3] = AD1_3_CHANNEL; - nb_adc1_channels++; -#endif -#ifdef AD1_4_CHANNEL - adc_channel_map[AD1_4] = AD1_4_CHANNEL; - nb_adc1_channels++; -#endif - // initialize buffer pointers with 0 (not set). Buffer null pointers will be ignored in interrupt - // handler, which is important as there are no buffers registered at the time the ADC trigger - // interrupt is enabled. - for (x = 0; x < 4; x++) { adc1_buffers[x] = NULL; } - adc_init_single(ADC1, nb_adc1_channels, adc_channel_map); -#endif // USE_AD1 - - - nb_adc2_channels = 0; -#if USE_AD2 -#ifdef AD2_1_CHANNEL - adc_channel_map[AD2_1 - nb_adc1_channels] = AD2_1_CHANNEL; - nb_adc2_channels++; -#endif -#ifdef AD2_2_CHANNEL - adc_channel_map[AD2_2 - nb_adc1_channels] = AD2_2_CHANNEL; - nb_adc2_channels++; -#endif -#ifdef AD2_3_CHANNEL - adc_channel_map[AD2_3 - nb_adc1_channels] = AD2_3_CHANNEL; - nb_adc2_channels++; -#endif -#ifdef AD2_4_CHANNEL - adc_channel_map[AD2_4 - nb_adc1_channels] = AD2_4_CHANNEL; - nb_adc2_channels++; -#endif - // initialize buffer pointers with 0 (not set). Buffer null pointers will be ignored in interrupt - // handler, which is important as there are no buffers registered at the time the ADC trigger - // interrupt is enabled. - for (x = 0; x < 4; x++) { adc2_buffers[x] = NULL; } - adc_init_single(ADC2, nb_adc2_channels, adc_channel_map); -#endif // USE_AD2 - - - nb_adc3_channels = 0; -#if USE_AD3 -#ifdef AD3_1_CHANNEL - adc_channel_map[AD3_1 - nb_adc1_channels - nb_adc2_channels] = AD3_1_CHANNEL; - nb_adc3_channels++; -#endif -#ifdef AD3_2_CHANNEL - adc_channel_map[AD3_2 - nb_adc1_channels - nb_adc2_channels] = AD3_2_CHANNEL; - nb_adc3_channels++; -#endif -#ifdef AD3_3_CHANNEL - adc_channel_map[AD3_3 - nb_adc1_channels - nb_adc2_channels] = AD3_3_CHANNEL; - nb_adc3_channels++; -#endif -#ifdef AD3_4_CHANNEL - adc_channel_map[AD3_4 - nb_adc1_channels - nb_adc2_channels] = AD3_4_CHANNEL; - nb_adc3_channels++; -#endif - // initialize buffer pointers with 0 (not set). Buffer null pointers will be ignored in interrupt - // handler, which is important as there are no buffers registered at the time the ADC trigger - // interrupt is enabled. - for (x = 0; x < 4; x++) { adc3_buffers[x] = NULL; } - adc_init_single(ADC3, nb_adc3_channels, adc_channel_map); -#endif // USE_AD3 - -#if USE_ADC_WATCHDOG - adc_watchdog.cb = NULL; - adc_watchdog.timeStamp = 0; -#endif - -} - -void adc_buf_channel(uint8_t adc_channel, struct adc_buf *s, uint8_t av_nb_sample) -{ - if (adc_channel < nb_adc1_channels) { -#if USE_AD1 - adc1_buffers[adc_channel] = s; -#endif - } else if (adc_channel < (nb_adc1_channels + nb_adc2_channels)) { -#if USE_AD2 - adc2_buffers[adc_channel - nb_adc1_channels] = s; -#endif - } else if (adc_channel < (nb_adc1_channels + nb_adc2_channels + nb_adc3_channels)) { -#if USE_AD3 - adc3_buffers[adc_channel - (nb_adc1_channels + nb_adc2_channels)] = s; -#endif - } - - s->av_nb_sample = av_nb_sample; - -} - -#if USE_ADC_WATCHDOG -void register_adc_watchdog(uint32_t adc, uint8_t chan, uint16_t low, uint16_t high, adc_watchdog_callback cb) -{ - adc_watchdog.adc = adc; - adc_watchdog.cb = cb; - - // activated adc watchdog of a single injected channel with interrupt - adc_set_watchdog_low_threshold(adc, low); - adc_set_watchdog_high_threshold(adc, high); - adc_enable_analog_watchdog_injected(adc); - adc_enable_analog_watchdog_on_selected_channel(adc, chan); - adc_enable_awd_interrupt(adc); -} -#endif - -/**************************************/ -/*** PRIVATE FUNCTION DEFINITIONS ***/ -/**************************************/ - -#if defined(USE_AD_TIM4) -#define TIM_ADC TIM4 -#define RCC_TIM_ADC RCC_TIM4 -#elif defined(USE_AD_TIM1) -#define TIM_ADC TIM1 -#define RCC_TIM_ADC RCC_TIM1 -#else -#define TIM_ADC TIM2 -#define RCC_TIM_ADC RCC_TIM2 -#endif - -/** Configure and enable RCC for peripherals (ADC1, ADC2, Timer) */ -static inline void adc_init_rcc(void) -{ -#if USE_AD1 || USE_AD2 || USE_AD3 - /* Timer peripheral clock enable. */ - rcc_periph_clock_enable(RCC_TIM_ADC); -#if defined(STM32F4) - adc_set_clk_prescale(ADC_CCR_ADCPRE_BY2); -#endif - - /* Enable ADC peripheral clocks. */ -#if USE_AD1 - rcc_periph_clock_enable(RCC_ADC1); - rcc_periph_reset_pulse(RST_ADC1); -#endif -#if USE_AD2 - rcc_periph_clock_enable(RCC_ADC2); - rcc_periph_reset_pulse(RST_ADC2); -#endif -#if USE_AD3 - rcc_periph_clock_enable(RCC_ADC3); - rcc_periph_reset_pulse(RST_ADC3); -#endif - - /* Time Base configuration */ - timer_set_mode(TIM_ADC, TIM_CR1_CKD_CK_INT, - TIM_CR1_CMS_EDGE, TIM_CR1_DIR_UP); - /* timer counts with ADC_TIMER_FREQUENCY */ - uint32_t timer_clk = timer_get_frequency(TIM_ADC); - timer_set_prescaler(TIM_ADC, (timer_clk / ADC_TIMER_FREQUENCY) - 1); - - timer_set_period(TIM_ADC, ADC_TIMER_PERIOD); - /* Generate TRGO on every update (when counter reaches period reload value) */ - timer_set_master_mode(TIM_ADC, TIM_CR2_MMS_UPDATE); - timer_enable_counter(TIM_ADC); - -#endif // USE_AD1 || USE_AD2 || USE_AD3 -} - -/** Configure and enable ADC interrupt */ -static inline void adc_init_irq(void) -{ -#if defined(STM32F1) - nvic_set_priority(NVIC_ADC1_2_IRQ, NVIC_ADC_IRQ_PRIO); - nvic_enable_irq(NVIC_ADC1_2_IRQ); -#elif defined(STM32F4) - nvic_set_priority(NVIC_ADC_IRQ, NVIC_ADC_IRQ_PRIO); - nvic_enable_irq(NVIC_ADC_IRQ); -#endif -} - - -static inline void adc_init_single(uint32_t adc, uint8_t nb_channels, uint8_t *channel_map) -{ - // Paranoia, must be down for 2+ ADC clock cycles before calibration - adc_power_off(adc); - - /* Configure ADC */ - /* Explicitly setting most registers, reset/default values are correct for most */ - /* Set CR1 register. */ - /* Clear AWDEN */ - adc_disable_analog_watchdog_regular(adc); - /* Clear JAWDEN */ - adc_disable_analog_watchdog_injected(adc); - /* Clear DISCEN */ - adc_disable_discontinuous_mode_regular(adc); - /* Clear JDISCEN */ - adc_disable_discontinuous_mode_injected(adc); - /* Clear JAUTO */ - adc_disable_automatic_injected_group_conversion(adc); - /* Set SCAN */ - adc_enable_scan_mode(adc); - /* Enable ADC JEOC interrupt (Set JEOCIE) */ - adc_enable_eoc_interrupt_injected(adc); - /* Clear AWDIE */ - adc_disable_awd_interrupt(adc); - /* Clear EOCIE */ - adc_disable_eoc_interrupt(adc); - - /* Set CR2 register. */ - /* Clear TSVREFE */ - adc_disable_temperature_sensor(); - /* Clear EXTTRIG */ - adc_disable_external_trigger_regular(adc); - /* Clear ALIGN */ - adc_set_right_aligned(adc); - /* Clear DMA */ - adc_disable_dma(adc); - /* Clear CONT */ - adc_set_single_conversion_mode(adc); - - //uint8_t x = 0; - //for (x = 0; x < nb_channels; x++) { - // adc_set_sample_time(adc, channel_map[x], ADC_SAMPLE_TIME); - //} - adc_set_sample_time_on_all_channels(adc, ADC_SAMPLE_TIME); - - adc_set_injected_sequence(adc, nb_channels, channel_map); - -#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) - /* Rest ADC calibaration register and wait until done */ - adc_reset_calibration(adc); - /* Start ADC calibaration and wait until done */ - adc_calibrate(adc); -#endif - - return; -} // adc_init_single - - -static inline void adc_push_sample(struct adc_buf *buf, uint16_t value) -{ - uint8_t new_head = buf->head + 1; - - if (new_head >= buf->av_nb_sample) { - new_head = 0; - } - buf->sum -= buf->values[new_head]; - buf->values[new_head] = value; - buf->sum += value; - buf->head = new_head; -} - -/*********************************/ -/*** ADC INTERRUPT HANDLER ***/ -/*********************************/ - -#if defined(STM32F1) -void adc1_2_isr(void) -#elif defined(STM32F4) -void adc_isr(void) -#endif -{ -#if USE_AD1 || USE_AD2 || USE_AD3 - uint8_t channel = 0; - uint16_t value = 0; - struct adc_buf *buf; -#endif - -#if USE_ADC_WATCHDOG - /* - We need adc sampling fast enough to detect battery plug out, but we did not - need to get actual actual value so fast. So timer fire adc conversion fast, - at least 500 hz, but we inject adc value in sampling buffer only at 50hz - */ - const uint32_t timeStampDiff = get_sys_time_msec() - adc_watchdog.timeStamp; - const bool shouldAccumulateValue = timeStampDiff > 20; - if (shouldAccumulateValue) { - adc_watchdog.timeStamp = get_sys_time_msec(); - } - - if (adc_watchdog.cb != NULL) { - if (adc_awd(adc_watchdog.adc)) { - ADC_SR(adc_watchdog.adc) &= ~ADC_SR_AWD; // clear int flag - adc_watchdog.cb(); - } - } -#endif - -#if USE_AD1 - // Clear Injected End Of Conversion - if (adc_eoc_injected(ADC1)) { - ADC_SR(ADC1) &= ~ADC_SR_JEOC; -#if USE_ADC_WATCHDOG - if (shouldAccumulateValue) { -#endif - for (channel = 0; channel < nb_adc1_channels; channel++) { - buf = adc1_buffers[channel]; - if (buf) { - value = adc_read_injected(ADC1, channel + 1); - adc_push_sample(buf, value); - } - } -#if USE_ADC_WATCHDOG - } -#endif - } -#endif // USE_AD1 - -#if USE_AD2 - if (adc_eoc_injected(ADC2)) { - ADC_SR(ADC2) &= ~ADC_SR_JEOC; -#if USE_ADC_WATCHDOG - if (shouldAccumulateValue) { -#endif - for (channel = 0; channel < nb_adc2_channels; channel++) { - buf = adc2_buffers[channel]; - if (buf) { - value = adc_read_injected(ADC2, channel + 1); - adc_push_sample(buf, value); - } - } -#if USE_ADC_WATCHDOG - } -#endif - } -#endif // USE_AD2 - -#if USE_AD3 - if (adc_eoc_injected(ADC3)) { - ADC_SR(ADC3) &= ~ADC_SR_JEOC; -#if USE_ADC_WATCHDOG - if (shouldAccumulateValue) { -#endif - for (channel = 0; channel < nb_adc3_channels; channel++) { - buf = adc3_buffers[channel]; - if (buf) { - value = adc_read_injected(ADC3, channel + 1); - adc_push_sample(buf, value); - } - } -#if USE_ADC_WATCHDOG - } -#endif - } -#endif // USE_AD3 - - return; -} diff --git a/sw/airborne/arch/stm32/mcu_periph/adc_arch.h b/sw/airborne/arch/stm32/mcu_periph/adc_arch.h deleted file mode 100644 index 5ebed8965b..0000000000 --- a/sw/airborne/arch/stm32/mcu_periph/adc_arch.h +++ /dev/null @@ -1,107 +0,0 @@ -/* - * Copyright (C) 2010-2012 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 arch/stm32/mcu_periph/adc_arch.h - * @ingroup stm32_arch - * - * Driver for the analog to digital converters on STM32. - */ - -#ifndef ADC_ARCH_H -#define ADC_ARCH_H - -#include BOARD_CONFIG - -/* Set the correct ADC resolution */ -#ifndef ADC_RESOLUTION -#define ADC_RESOLUTION 4096 -#endif - -enum adc1_channels { -#ifdef AD1_1_CHANNEL - AD1_1, -#endif -#ifdef AD1_2_CHANNEL - AD1_2, -#endif -#ifdef AD1_3_CHANNEL - AD1_3, -#endif -#ifdef AD1_4_CHANNEL - AD1_4, -#endif - ADC1_END -}; - -enum adc2_channels { - ADC2_BEGIN = ADC1_END-1, -#ifdef AD2_1_CHANNEL - AD2_1, -#endif -#ifdef AD2_2_CHANNEL - AD2_2, -#endif -#ifdef AD2_3_CHANNEL - AD2_3, -#endif -#ifdef AD2_4_CHANNEL - AD2_4, -#endif - ADC2_END -}; - -enum adc3_channels { - ADC3_BEGIN = ADC2_END-1, -#ifdef AD3_1_CHANNEL - AD3_1, -#endif -#ifdef AD3_2_CHANNEL - AD3_2, -#endif -#ifdef AD3_3_CHANNEL - AD3_3, -#endif -#ifdef AD3_4_CHANNEL - AD3_4, -#endif - ADC3_END -}; - -#if USE_ADC_WATCHDOG - -/* Watchdog callback type definition - */ -typedef void (*adc_watchdog_callback)(void); - -/* Watchdog register function - * - * @param adc adc bank to monitor - * @param chan adc channel to monitor - * @param low low threshhold for callback trigger - * @param high high threshhold for callback trigger - */ -extern void register_adc_watchdog(uint32_t adc, uint8_t chan, uint16_t low, uint16_t high, adc_watchdog_callback cb); - -#endif - -#endif /* ADC_ARCH_H */ diff --git a/sw/airborne/arch/stm32/mcu_periph/can_arch.c b/sw/airborne/arch/stm32/mcu_periph/can_arch.c deleted file mode 100644 index 9c370b2cb2..0000000000 --- a/sw/airborne/arch/stm32/mcu_periph/can_arch.c +++ /dev/null @@ -1,259 +0,0 @@ -/* - * Copyright (C) 2012 Piotr Esden-Tempski - * - * 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 arch/stm32/mcu_periph/can_arch.c - * @ingroup stm32_arch - * - * Handling of CAN hardware for STM32. - */ - -#include -#include - -#include "mcu_periph/can_arch.h" -#include "mcu_periph/can.h" - -#include -#include -#include -#include - -#include "led.h" - - -#ifdef RTOS_PRIO -#define NVIC_USB_LP_CAN_RX0_IRQ_PRIO RTOS_PRIO+1 -#else -#define NVIC_USB_LP_CAN_RX0_IRQ_PRIO 1 -#define NVIC_CAN1_RX_IRQ_PRIO 1 -#endif - -void _can_run_rx_callback(uint32_t id, uint8_t *buf, uint8_t len); - -struct can_arch_periph { - uint32_t canport; - bool can_initialized; - struct pprzcan_frame rxframe; - bool new_rxframe; - struct pprzaddr_can addr; -}; - -struct can_arch_periph can1_arch_s = { - .canport = CAN1, - .can_initialized = false, - .addr = {.can_ifindex = 1}, - .rxframe = {0}, - .new_rxframe = false; -}; - - -void can_hw_init(void) -{ - can1.arch_struct = &can1_arch_s; - -#ifdef STM32F1 - /* Enable peripheral clocks. */ - rcc_periph_clock_enable(RCC_AFIO); - rcc_periph_clock_enable(RCC_GPIOB); - rcc_periph_clock_enable(RCC_CAN1); - - /* Remap the gpio pin if necessary. */ - AFIO_MAPR |= AFIO_MAPR_CAN1_REMAP_PORTB; - - /* Configure CAN pin: RX (input pull-up). */ - gpio_set_mode(GPIO_BANK_CAN1_PB_RX, GPIO_MODE_INPUT, - GPIO_CNF_INPUT_PULL_UPDOWN, GPIO_CAN1_PB_RX); - gpio_set(GPIO_BANK_CAN1_PB_RX, GPIO_CAN1_PB_RX); - - /* Configure CAN pin: TX (output push-pull). */ - gpio_set_mode(GPIO_BANK_CAN1_PB_TX, GPIO_MODE_OUTPUT_50_MHZ, - GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, GPIO_CAN1_PB_TX); - - /* NVIC setup. */ - nvic_enable_irq(NVIC_USB_LP_CAN_RX0_IRQ); - nvic_set_priority(NVIC_USB_LP_CAN_RX0_IRQ, NVIC_USB_LP_CAN_RX0_IRQ_PRIO); - -#elif STM32F4 - - /* Enable peripheral clocks. */ - rcc_periph_clock_enable(RCC_GPIOB); - rcc_periph_clock_enable(RCC_CAN1); - - /* set up pins for CAN1TX & CAN1RX alternate function */ - gpio_mode_setup(GPIOB, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO8 | GPIO9); - gpio_set_af(GPIOB, GPIO_AF9, GPIO8 | GPIO9); - - /* enable interrupts on RX0 FIFO */ - nvic_enable_irq(NVIC_CAN1_RX0_IRQ); - nvic_set_priority(NVIC_CAN1_RX0_IRQ, NVIC_CAN1_RX_IRQ_PRIO); - -#endif - /* Reset CAN. */ - can_reset(can1_arch_s.canport); - - /* CAN cell init. - * For time quanta calculation see STM32 reference manual - * section 24.7.7 "Bit timing" page 645 - * - * To talk to CSC using LPC mcu we need a baud rate of 375kHz - * The APB1 runs at 36MHz therefor we select a prescaler of 12 - * resulting in time quanta frequency of 36MHz / 12 = 3MHz - * - * As the Bit time is combined of 1tq for SYNC_SEG, TS1tq for bit - * segment 1 and TS2tq for bit segment 2: - * BITtq = 1tq + TS1tq + TS2tq - * - * We can choose to use TS1 = 3 and TS2 = 4 getting - * 1tq + 3tq + 4tq = 8tq per bit therefor a bit frequency is - * 3MHZ / 8 = 375kHz - * - * Maximum baud rate of CAN is 1MHz so we can choose to use - * prescaler of 2 resulting in a quanta frequency of 36MHz / 2 = 18Mhz - * - * So we need to devide the frequency by 18. This can be accomplished - * using TS1 = 10 and TS2 = 7 resulting in: - * 1tq + 10tq + 7tq = 18tq - * - * NOTE: Although it is out of spec I managed to have CAN run at 2MBit - * Just decrease the prescaler to 1. It worked for me(tm) (esden) - */ - if (can_init(can1_arch_s.canport, - false, /* TTCM: Time triggered comm mode? */ - true, /* ABOM: Automatic bus-off management? */ - false, /* AWUM: Automatic wakeup mode? */ - false, /* NART: No automatic retransmission? */ - false, /* RFLM: Receive FIFO locked mode? */ - false, /* TXFP: Transmit FIFO priority? */ -#ifdef STM32F1 - CAN_BTR_SJW_1TQ, - CAN_BTR_TS1_10TQ, - CAN_BTR_TS2_7TQ, -#elif STM32F4 - CAN_BTR_SJW_1TQ, - CAN_BTR_TS1_14TQ, - CAN_BTR_TS2_6TQ, -#endif - 2, /* BRP+1: Baud rate prescaler */ - false, /* loopback mode */ - false)) { /* silent mode */ - /* TODO we need something somewhere where we can leave a note - * that CAN was unable to initialize. Just like any other - * driver should... - */ - - can_reset(can1_arch_s.canport); - - return; - } - - /* CAN filter 0 init. */ - can_filter_id_mask_32bit_init(0, /* Filter ID */ - 0, /* CAN ID */ - 0, /* CAN ID mask */ - 0, /* FIFO assignment (here: FIFO0) */ - true); /* Enable the filter. */ - - /* Enable CAN RX interrupt. */ - can_enable_irq(can1_arch_s.canport, CAN_IER_FMPIE0); - - /* Remember that we succeeded to initialize. */ - can1_arch_s.can_initialized = true; -} - - -int can_transmit_frame(struct pprzcan_frame* txframe, struct pprzaddr_can* addr) { - if (!can1_arch_s.can_initialized) { - return -2; - } - - if(txframe->len > 8) { - return -1; //does not currently support CANFD - } - - return can_transmit(can1_arch_s.canport, -#ifdef USE_CAN_EXT_ID - txframe->can_id & CAN_EID_MASK, - true, /* IDE: CAN ID extended */ -#else - txframe->can_id & CAN_SID_MASK, - false, /* IDE: CAN ID standard */ -#endif - txframe->can_id & CAN_FRAME_RTR, /* RTR: Request transmit? */ - can_len_to_dlc(txframe->len), /* DLC: Data length */ - (uint8_t *)txframe->data); - -} - -#ifdef STM32F1 -void usb_lp_can_rx0_isr(void) -#elif STM32F4 -void can1_rx0_isr(void) -#else -#error "CAN unsuported on this MCU!" -void __unsupported_isr(void) -#endif -{ - uint32_t id; - uint8_t fmi; - bool ext, rtr; - uint8_t dlc; - struct pprzcan_frame* rxframe = &can1_arch_s.rxframe; - - - can_receive(can1_arch_s.canport, - 0, /* FIFO: 0 */ - false, /* Release */ - &rxframe->can_id, - &ext, - &rtr, - &fmi, - &dlc, - rxframe->data, - &rxframe->timestamp); - - rxframe->len = can_dlc_to_len(dlc); - - if(ext) { - rxframe->can_id |= CAN_FRAME_EFF; - } - - if(rtr) { - rxframe->can_id |= CAN_FRAME_RTR; - } - - can1_arch_s.new_rxframe = true; - - can_fifo_release(can1_arch_s.canport, 0); -} - - -void can_event() { - if(can1_arch_s.new_rxframe) { - for(int i=0; i - * - * 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 arch/stm32/mcu_periph/can_arch.h - * @ingroup stm32_arch - * - * Handling of CAN hardware for STM32. - */ - -#ifndef MCU_PERIPH_STM32_CAN_ARCH_H -#define MCU_PERIPH_STM32_CAN_ARCH_H - -void can_hw_init(void); -void can_event(void); - -#if defined STM32F1 -void usb_lp_can1_rx0_irq_handler(void); -#endif - - -#endif /* MCU_PERIPH_STM32_CAN_ARCH_H */ diff --git a/sw/airborne/arch/stm32/mcu_periph/gpio_arch.c b/sw/airborne/arch/stm32/mcu_periph/gpio_arch.c deleted file mode 100644 index dfe2cd8693..0000000000 --- a/sw/airborne/arch/stm32/mcu_periph/gpio_arch.c +++ /dev/null @@ -1,167 +0,0 @@ -/* - * 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 arch/stm32/mcu_periph/gpio_arch.c - * @ingroup stm32_arch - * - * GPIO helper functions for STM32F1 and STM32F4. - */ - -#include "mcu_periph/gpio.h" - -#include -#include - -void gpio_enable_clock(uint32_t port) -{ - switch (port) { - case GPIOA: - rcc_periph_clock_enable(RCC_GPIOA); - break; - case GPIOB: - rcc_periph_clock_enable(RCC_GPIOB); - break; - case GPIOC: - rcc_periph_clock_enable(RCC_GPIOC); - break; - case GPIOD: - rcc_periph_clock_enable(RCC_GPIOD); - break; -#ifdef GPIOE - case GPIOE: - rcc_periph_clock_enable(RCC_GPIOE); - break; -#endif -#ifdef GPIOF - case GPIOF: - rcc_periph_clock_enable(RCC_GPIOF); - break; -#endif -#ifdef GPIOG - case GPIOG: - rcc_periph_clock_enable(RCC_GPIOG); - break; -#endif -#ifdef GPIOH - case GPIOH: - rcc_periph_clock_enable(RCC_GPIOH); - break; -#endif -#ifdef GPIOI - case GPIOI: - rcc_periph_clock_enable(RCC_GPIOI); - break; -#endif - default: - break; - }; -} - -#ifdef STM32F1 -void gpio_setup_output(uint32_t port, uint16_t gpios) -{ - gpio_enable_clock(port); - gpio_set_mode(port, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_PUSHPULL, gpios); -} - -void gpio_setup_input(uint32_t port, uint16_t gpios) -{ - gpio_enable_clock(port); - gpio_set_mode(port, GPIO_MODE_INPUT, GPIO_CNF_INPUT_FLOAT, gpios); -} - -void gpio_setup_input_pullup(uint32_t port, uint16_t gpios) -{ - gpio_enable_clock(port); - gpio_set(port, gpios); - gpio_set_mode(port, GPIO_MODE_INPUT, GPIO_CNF_INPUT_PULL_UPDOWN, gpios); -} - -void gpio_setup_input_pulldown(uint32_t port, uint16_t gpios) -{ - gpio_enable_clock(port); - gpio_clear(port, gpios); - gpio_set_mode(port, GPIO_MODE_INPUT, GPIO_CNF_INPUT_PULL_UPDOWN, gpios); -} - -void gpio_setup_pin_af(uint32_t port, uint16_t pin, uint32_t af, bool is_output) -{ - gpio_enable_clock(port); - /* remap alternate function if needed */ - if (af) { - rcc_periph_clock_enable(RCC_AFIO); - AFIO_MAPR |= af; - } - if (is_output) { - gpio_set_mode(port, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, pin); - } else { - gpio_set_mode(port, GPIO_MODE_INPUT, GPIO_CNF_INPUT_FLOAT, pin); - } -} - -void gpio_setup_pin_analog(uint32_t port, uint16_t pin) -{ - gpio_enable_clock(port); - gpio_set_mode(port, GPIO_MODE_INPUT, GPIO_CNF_INPUT_ANALOG, pin); -} - -#elif defined STM32F4 - -void gpio_setup_output(uint32_t port, uint16_t gpios) -{ - gpio_enable_clock(port); - gpio_mode_setup(port, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, gpios); -} - -void gpio_setup_input(uint32_t port, uint16_t gpios) -{ - gpio_enable_clock(port); - gpio_mode_setup(port, GPIO_MODE_INPUT, GPIO_PUPD_NONE, gpios); -} - -void gpio_setup_input_pullup(uint32_t port, uint16_t gpios) -{ - gpio_enable_clock(port); - gpio_mode_setup(port, GPIO_MODE_INPUT, GPIO_PUPD_PULLUP, gpios); -} - -void gpio_setup_input_pulldown(uint32_t port, uint16_t gpios) -{ - gpio_enable_clock(port); - gpio_mode_setup(port, GPIO_MODE_INPUT, GPIO_PUPD_PULLDOWN, gpios); -} - -void gpio_setup_pin_af(uint32_t port, uint16_t pin, uint8_t af, bool is_output __attribute__((unused))) -{ - gpio_enable_clock(port); - gpio_set_af(port, af, pin); - gpio_mode_setup(port, GPIO_MODE_AF, GPIO_PUPD_NONE, pin); -} - -void gpio_setup_pin_analog(uint32_t port, uint16_t pin) -{ - gpio_enable_clock(port); - gpio_mode_setup(port, GPIO_MODE_ANALOG, GPIO_PUPD_NONE, pin); -} - -#endif - diff --git a/sw/airborne/arch/stm32/mcu_periph/gpio_arch.h b/sw/airborne/arch/stm32/mcu_periph/gpio_arch.h deleted file mode 100644 index 43957b87d7..0000000000 --- a/sw/airborne/arch/stm32/mcu_periph/gpio_arch.h +++ /dev/null @@ -1,91 +0,0 @@ -/* - * 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 arch/stm32/mcu_periph/gpio_arch.h - * @ingroup stm32_arch - * - * GPIO helper functions for STM32F1 and STM32F4. - * - * The gpio_set and gpio_clear functions are already available from libopencm3. - */ - -#ifndef GPIO_ARCH_H -#define GPIO_ARCH_H - -#include - -/** - * Abstract gpio port type for hardware independent part - */ -typedef uint32_t gpio_port_t; - -/** - * Setup one or more pins of the given GPIO port as outputs. - * @param[in] port - * @param[in] gpios If multiple pins are to be changed, use logical OR '|' to separate them. - */ -extern void gpio_setup_output(uint32_t port, uint16_t gpios); - -/** - * Setup one or more pins of the given GPIO port as inputs. - * @param[in] port - * @param[in] gpios If multiple pins are to be changed, use logical OR '|' to separate them. - */ -extern void gpio_setup_input(uint32_t port, uint16_t gpios); - -/** - * Setup one or more pins of the given GPIO port as inputs with pull up resistor enabled. - * @param[in] port - * @param[in] gpios If multiple pins are to be changed, use logical OR '|' to separate them. - */ -extern void gpio_setup_input_pullup(uint32_t port, uint16_t gpios); - -/** - * Setup one or more pins of the given GPIO port as inputs with pull down resistors enabled. - * @param[in] port - * @param[in] gpios If multiple pins are to be changed, use logical OR '|' to separate them. - */ -extern void gpio_setup_input_pulldown(uint32_t port, uint16_t gpios); - -/** - * Setup a gpio for input or output with alternate function. - * This is an STM32 specific helper funtion and should only be used in stm32 arch code. - */ -#if defined(STM32F1) -extern void gpio_setup_pin_af(uint32_t port, uint16_t pin, uint32_t af, bool is_output); -#else -extern void gpio_setup_pin_af(uint32_t port, uint16_t pin, uint8_t af, bool is_output); -#endif - -/** - * Setup a gpio for analog use. - * This is an STM32 specific helper funtion and should only be used in stm32 arch code. - */ -extern void gpio_setup_pin_analog(uint32_t port, uint16_t pin); - -/** - * Enable the relevant clock. - * This is an STM32 specific helper funtion and should only be used in stm32 arch code. - */ -extern void gpio_enable_clock(uint32_t port); - -#endif /* GPIO_ARCH_H */ diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.c deleted file mode 100644 index 432f0ba5c5..0000000000 --- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.c +++ /dev/null @@ -1,1380 +0,0 @@ -/* - * Copyright (C) 2009-2012 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 arch/stm32/mcu_periph/i2c_arch.c - * @ingroup stm32_arch - * Handling of I2C hardware for STM32. - */ - -#include "mcu_periph/i2c.h" - -#include BOARD_CONFIG - -#include -#include -#include -#include - -#include "mcu_periph/gpio.h" - - -static bool i2c_stm32_idle(struct i2c_periph *periph) __attribute__((unused)); -static bool i2c_stm32_submit(struct i2c_periph *periph, struct i2c_transaction *t) __attribute__((unused)); -static void i2c_stm32_setbitrate(struct i2c_periph *periph, int bitrate) __attribute__((unused)); - - -////////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////////// - - -// Error bit mask -// XXX: consider moving this define into libopencm3 -#define I2C_SR1_ERR_MASK (I2C_SR1_SMBALERT | \ - I2C_SR1_TIMEOUT | \ - I2C_SR1_PECERR | \ - I2C_SR1_OVR | \ - I2C_SR1_AF | \ - I2C_SR1_ARLO | \ - I2C_SR1_BERR) - -// Bit Control - -#define BIT_X_IS_SET_IN_REG(X,REG) (((REG) & (X)) == (X)) - -// disable and enable irq functions are not implemented in libopencm3 defining them here -// XXX: consider moving this definitions into libopencm3 -static inline void __disable_irq(void) { asm volatile("cpsid i"); } -static inline void __enable_irq(void) { asm volatile("cpsie i"); } - -// Critical Zones - -#define __I2C_REG_CRITICAL_ZONE_START __disable_irq(); -#define __I2C_REG_CRITICAL_ZONE_STOP __enable_irq(); - - -#ifndef NVIC_I2C_IRQ_PRIO -#define NVIC_I2C1_IRQ_PRIO 0 -#define NVIC_I2C2_IRQ_PRIO 0 -#define NVIC_I2C3_IRQ_PRIO 0 -#else -#define NVIC_I2C1_IRQ_PRIO NVIC_I2C_IRQ_PRIO -#define NVIC_I2C2_IRQ_PRIO NVIC_I2C_IRQ_PRIO -#define NVIC_I2C3_IRQ_PRIO NVIC_I2C_IRQ_PRIO -#endif - -#if USE_I2C1 || USE_I2C2 || USE_I2C3 -#if defined(STM32F1) -static void i2c_setup_gpio(uint32_t i2c) -{ - switch (i2c) { -#if USE_I2C1 - case I2C1: - gpio_enable_clock(I2C1_GPIO_PORT); - gpio_set_mode(I2C1_GPIO_PORT, GPIO_MODE_OUTPUT_2_MHZ, - GPIO_CNF_OUTPUT_ALTFN_OPENDRAIN, - I2C1_GPIO_SCL | I2C1_GPIO_SDA); - break; -#endif -#if USE_I2C2 - case I2C2: - gpio_enable_clock(I2C2_GPIO_PORT); - gpio_set_mode(I2C2_GPIO_PORT, GPIO_MODE_OUTPUT_2_MHZ, - GPIO_CNF_OUTPUT_ALTFN_OPENDRAIN, - I2C2_GPIO_SCL | I2C2_GPIO_SDA); - break; -#endif - default: - break; - } -} - -#elif defined(STM32F4) -#ifndef I2C1_GPIO_AF -#define I2C1_GPIO_AF GPIO_AF4 -#endif - -#ifndef I2C2_GPIO_AF -#define I2C2_GPIO_AF GPIO_AF4 -#endif - -#ifndef I2C3_GPIO_SCL_AF -#define I2C3_GPIO_SCL_AF GPIO_AF4 -#endif -#ifndef I2C3_GPIO_SDA_AF -#define I2C3_GPIO_SDA_AF GPIO_AF4 -#endif - -static void i2c_setup_gpio(uint32_t i2c) -{ - switch (i2c) { -#if USE_I2C1 - case I2C1: - gpio_enable_clock(I2C1_GPIO_PORT); - gpio_mode_setup(I2C1_GPIO_PORT, GPIO_MODE_AF, GPIO_PUPD_NONE, - I2C1_GPIO_SCL | I2C1_GPIO_SDA); - gpio_set_output_options(GPIOB, GPIO_OTYPE_OD, GPIO_OSPEED_25MHZ, - I2C1_GPIO_SCL | I2C1_GPIO_SDA); - gpio_set_af(I2C1_GPIO_PORT, I2C1_GPIO_AF, I2C1_GPIO_SCL | I2C1_GPIO_SDA); - break; -#endif -#if USE_I2C2 - case I2C2: - gpio_enable_clock(I2C2_GPIO_PORT); - gpio_mode_setup(I2C2_GPIO_PORT, GPIO_MODE_AF, GPIO_PUPD_NONE, - I2C2_GPIO_SCL | I2C2_GPIO_SDA); - gpio_set_output_options(I2C2_GPIO_PORT, GPIO_OTYPE_OD, GPIO_OSPEED_25MHZ, - I2C2_GPIO_SCL | I2C2_GPIO_SDA); - gpio_set_af(I2C2_GPIO_PORT, I2C2_GPIO_AF, I2C2_GPIO_SCL | I2C2_GPIO_SDA); - break; -#endif -#if USE_I2C3 - case I2C3: - gpio_enable_clock(I2C3_GPIO_PORT_SCL); - gpio_mode_setup(I2C3_GPIO_PORT_SCL, GPIO_MODE_AF, GPIO_PUPD_NONE, I2C3_GPIO_SCL); - gpio_set_output_options(I2C3_GPIO_PORT_SCL, GPIO_OTYPE_OD, GPIO_OSPEED_25MHZ, - I2C3_GPIO_SCL); - gpio_set_af(I2C3_GPIO_PORT_SCL, I2C3_GPIO_SCL_AF, I2C3_GPIO_SCL); - - gpio_enable_clock(I2C3_GPIO_PORT_SDA); - gpio_mode_setup(I2C3_GPIO_PORT_SDA, GPIO_MODE_AF, GPIO_PUPD_NONE, I2C3_GPIO_SDA); - gpio_set_output_options(I2C3_GPIO_PORT_SDA, GPIO_OTYPE_OD, GPIO_OSPEED_25MHZ, - I2C3_GPIO_SDA); - gpio_set_af(I2C3_GPIO_PORT_SDA, I2C3_GPIO_SDA_AF, I2C3_GPIO_SDA); - break; -#endif - default: - break; - } -} -#endif -#endif // USE_I2Cx - -static inline void PPRZ_I2C_SEND_STOP(uint32_t i2c) -{ - // Man: p722: Stop generation after the current byte transfer or after the current Start condition is sent. - i2c_send_stop(i2c); -} - -// (RE)START - -static inline void PPRZ_I2C_SEND_START(struct i2c_periph *periph) -{ - uint32_t i2c = (uint32_t) periph->reg_addr; - - // Reset the buffer pointer to the first byte - periph->idx_buf = 0; - periph->watchdog = 0; - - // Enable Error IRQ, Event IRQ but disable Buffer IRQ - i2c_enable_interrupt(i2c, I2C_CR2_ITERREN); - i2c_enable_interrupt(i2c, I2C_CR2_ITEVTEN); - i2c_disable_interrupt(i2c, I2C_CR2_ITBUFEN); - - // Issue a new start - i2c_nack_current(i2c); - i2c_disable_ack(i2c); - i2c_clear_stop(i2c); - i2c_peripheral_enable(i2c); - i2c_send_start(i2c); - periph->status = I2CStartRequested; - -} - -// STOP - -/////////////////////////////////////////////////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////////////////////////////////////////////////// -// -// SUBTRANSACTION SEQUENCES -// -We arrive here every time a ISR is called with no error - -enum STMI2CSubTransactionStatus { - STMI2C_SubTra_Busy, - STMI2C_SubTra_Ready_StopRequested, - STMI2C_SubTra_Ready, - STMI2C_SubTra_Error -}; - -// Doc ID 13902 Rev 11 p 710/1072 -// Transfer Sequence Diagram for Master Transmitter -static inline enum STMI2CSubTransactionStatus stmi2c_send(uint32_t i2c, struct i2c_periph *periph, - struct i2c_transaction *trans) -{ - uint16_t SR1 = I2C_SR1(i2c); - - // Start Condition Was Just Generated - if (BIT_X_IS_SET_IN_REG(I2C_SR1_SB, SR1)) { - // Disable buffer interrupt - i2c_disable_interrupt(i2c, I2C_CR2_ITBUFEN); - // Send Slave address and wait for ADDR interrupt - i2c_send_data(i2c, trans->slave_addr); - // Document the current Status - periph->status = I2CAddrWrSent; - } - // Address Was Sent - else if (BIT_X_IS_SET_IN_REG(I2C_SR1_ADDR, SR1)) { - // Now read SR2 to clear the ADDR status Bit - uint16_t SR2 __attribute__((unused)) = I2C_SR2(i2c); - - // Maybe check we are transmitting (did not loose arbitration for instance) - // if (! BIT_X_IS_SET_IN_REG(I2C_SR2_TRA, SR2)) { } - // update: this should be caught by the ARLO error: so we will not arrive here - - // Send First max 2 bytes - i2c_send_data(i2c, trans->buf[0]); - if (trans->len_w > 1) { - i2c_send_data(i2c, trans->buf[1]); - periph->idx_buf = 2; - } else { - periph->idx_buf = 1; - } - - // Enable buffer-space available interrupt - // only if there is more to send: wait for TXE, no more to send: wait for BTF - if (periph->idx_buf < trans->len_w) { - i2c_enable_interrupt(i2c, I2C_CR2_ITBUFEN); - } - - // Document the current Status - periph->status = I2CSendingByte; - } - // The buffer is not full anymore AND we were not waiting for BTF - else if ((BIT_X_IS_SET_IN_REG(I2C_SR1_TxE, SR1)) && (BIT_X_IS_SET_IN_REG(I2C_CR2_ITBUFEN, I2C_CR2(i2c)))) { - // Send the next byte - i2c_send_data(i2c, trans->buf[periph->idx_buf]); - periph->idx_buf++; - - // All bytes Sent? Then wait for BTF instead - if (periph->idx_buf >= trans->len_w) { - // Not interested anymore to know the buffer has space left - i2c_disable_interrupt(i2c, I2C_CR2_ITBUFEN); - // Next interrupt will be BTF (or error) - } - } - // BTF: means last byte was sent - else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BTF, SR1)) { - if (trans->type == I2CTransTx) { - // Tell the driver we are ready - trans->status = I2CTransSuccess; - } - // Otherwise we still need to do the receiving part - - return STMI2C_SubTra_Ready; - } else { // Event Logic Error - return STMI2C_SubTra_Error; - } - - return STMI2C_SubTra_Busy; -} - -// Doc ID 13902 Rev 11 p 714/1072 -// Transfer Sequence Diagram for Master Receiver for N=1 -static inline enum STMI2CSubTransactionStatus stmi2c_read1(uint32_t i2c, struct i2c_periph *periph, - struct i2c_transaction *trans) -{ - uint16_t SR1 = I2C_SR1(i2c); - - // Start Condition Was Just Generated - if (BIT_X_IS_SET_IN_REG(I2C_SR1_SB, SR1)) { - i2c_disable_interrupt(i2c, I2C_CR2_ITBUFEN); - i2c_send_data(i2c, trans->slave_addr | 0x01); - - // Document the current Status - periph->status = I2CAddrRdSent; - } - // Address Was Sent - else if (BIT_X_IS_SET_IN_REG(I2C_SR1_ADDR, SR1)) { - // First Clear the ACK bit: after the next byte we do not want new bytes - i2c_nack_current(i2c); - i2c_disable_ack(i2c); - - // --- next to steps MUST be executed together to avoid missing the stop - __I2C_REG_CRITICAL_ZONE_START; - - // Only after setting ACK, read SR2 to clear the ADDR (next byte will start arriving) - uint16_t SR2 __attribute__((unused)) = I2C_SR2(i2c); - - // Schedule a Stop - PPRZ_I2C_SEND_STOP(i2c); - - __I2C_REG_CRITICAL_ZONE_STOP; - // --- end of critical zone ----------- - - // Enable the RXNE: it will trigger as soon as the 1 byte is received to get the result - i2c_enable_interrupt(i2c, I2C_CR2_ITBUFEN); - - // Document the current Status - periph->status = I2CReadingLastByte; - } - // As soon as there is 1 byte ready to read, we have our byte - else if (BIT_X_IS_SET_IN_REG(I2C_SR1_RxNE, SR1)) { - i2c_disable_interrupt(i2c, I2C_CR2_ITBUFEN); - trans->buf[0] = I2C_DR(i2c); - - // We got all the results (stop condition might still be in progress but this is the last interrupt) - trans->status = I2CTransSuccess; - - // Document the current Status: - // -the stop was actually already requested in the previous step - periph->status = I2CStopRequested; - - return STMI2C_SubTra_Ready_StopRequested; - } else { // Event Logic Error - return STMI2C_SubTra_Error; - } - - return STMI2C_SubTra_Busy; -} - -// Doc ID 13902 Rev 11 p 713/1072 -// Transfer Sequence Diagram for Master Receiver for N=2 -static inline enum STMI2CSubTransactionStatus stmi2c_read2(uint32_t i2c, struct i2c_periph *periph, - struct i2c_transaction *trans) -{ - uint16_t SR1 = I2C_SR1(i2c); - - // Start Condition Was Just Generated - if (BIT_X_IS_SET_IN_REG(I2C_SR1_SB, SR1)) { - // according to the datasheet: instantly shedule a NAK on the second received byte: - i2c_disable_interrupt(i2c, I2C_CR2_ITBUFEN); - i2c_enable_ack(i2c); - i2c_nack_next(i2c); - i2c_send_data(i2c, trans->slave_addr | 0x01); - - // Document the current Status - periph->status = I2CAddrRdSent; - } - // Address Was Sent - else if (BIT_X_IS_SET_IN_REG(I2C_SR1_ADDR, SR1)) { - // --- make absolutely sure this command is not delayed too much after the previous: - // --- the NAK bits must be set before the first byte arrived: allow other interrupts here - __I2C_REG_CRITICAL_ZONE_START; - - // if transfer of DR was finished already then we will get too many bytes - // BEFORE clearing ACK, read SR2 to clear the ADDR (next byte will start arriving) - // clearing ACK after the byte transfer has already started will NACK the next (2nd) - uint16_t SR2 __attribute__((unused)) = I2C_SR2(i2c); - - // NOT First Clear the ACK bit but only AFTER clearing ADDR - i2c_disable_ack(i2c); - - // Disable the RXNE and wait for BTF - i2c_disable_interrupt(i2c, I2C_CR2_ITBUFEN); - - __I2C_REG_CRITICAL_ZONE_STOP; - // --- end of critical zone ----------- - - // We do not set the RxE but wait for both bytes to arrive using BTF - - // Document the current Status - periph->status = I2CReadingByte; - } - // Receive buffer if full, master is halted: BTF - else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BTF, SR1)) { - // Stop condition MUST be set BEFORE reading the DR - // otherwise since there is new buffer space a new byte will be read - PPRZ_I2C_SEND_STOP(i2c); - - // Document the current Status - periph->status = I2CStopRequested; - - trans->buf[0] = I2C_DR(i2c); - trans->buf[1] = I2C_DR(i2c); - - // We got all the results - trans->status = I2CTransSuccess; - - return STMI2C_SubTra_Ready_StopRequested; - } else { // Event Logic Error - return STMI2C_SubTra_Error; - } - - return STMI2C_SubTra_Busy; -} - -// Doc ID 13902 Rev 11 p 712/1072 -// Transfer Sequence Diagram for Master Receiver for N>2 -static inline enum STMI2CSubTransactionStatus stmi2c_readmany(uint32_t i2c, struct i2c_periph *periph, - struct i2c_transaction *trans) -{ - uint16_t SR1 = I2C_SR1(i2c); - - // Start Condition Was Just Generated - if (BIT_X_IS_SET_IN_REG(I2C_SR1_SB, SR1)) { - i2c_disable_interrupt(i2c, I2C_CR2_ITBUFEN); - // The first data byte will be acked in read many so the slave knows it should send more - i2c_nack_current(i2c); - i2c_enable_ack(i2c); - // Clear the SB flag - i2c_send_data(i2c, trans->slave_addr | 0x01); - - // Document the current Status - periph->status = I2CAddrRdSent; - } - // Address Was Sent - else if (BIT_X_IS_SET_IN_REG(I2C_SR1_ADDR, SR1)) { - periph->idx_buf = 0; - - // Enable RXNE: receive an interrupt any time a byte is available - // only enable if MORE than 3 bytes need to be read - if (periph->idx_buf < (trans->len_r - 3)) { - i2c_enable_interrupt(i2c, I2C_CR2_ITBUFEN); - } - - // ACK is still on to get more DATA - // Read SR2 to clear the ADDR (next byte will start arriving) - uint16_t SR2 __attribute__((unused)) = I2C_SR2(i2c); - - // Document the current Status - periph->status = I2CReadingByte; - } - // one or more bytes are available AND we were interested in Buffer interrupts - else if ((BIT_X_IS_SET_IN_REG(I2C_SR1_RxNE, SR1)) && (BIT_X_IS_SET_IN_REG(I2C_CR2_ITBUFEN, I2C_CR2(i2c)))) { - // read byte until 3 bytes remain to be read (e.g. len_r = 6, -> idx=3 means idx 3,4,5 = 3 remain to be read - if (periph->idx_buf < (trans->len_r - 3)) { - trans->buf[periph->idx_buf] = I2C_DR(i2c); - periph->idx_buf ++; - } - // from : 3bytes -> last byte: do nothing - // - // finally: this was the last byte - else if (periph->idx_buf >= (trans->len_r - 1)) { - i2c_disable_interrupt(i2c, I2C_CR2_ITBUFEN); - - // Last Value - trans->buf[periph->idx_buf] = i2c_get_data(i2c); - periph->idx_buf ++; - - // We got all the results - trans->status = I2CTransSuccess; - - return STMI2C_SubTra_Ready_StopRequested; - } - - // Check for end of transaction: start waiting for BTF instead of RXNE - if (periph->idx_buf < (trans->len_r - 3)) { - i2c_enable_interrupt(i2c, I2C_CR2_ITBUFEN); - } else { // idx >= len-3: there are 3 bytes to be read - // We want to halt I2C to have sufficient time to clear ACK, so: - // Stop listening to RXNE as it will be triggered infinitely since we did not empty the buffer - // on the next (second in buffer) received byte BTF will be set (buffer full and I2C halted) - i2c_disable_interrupt(i2c, I2C_CR2_ITBUFEN); - } - } - // Buffer is full while this was not a RXNE interrupt - else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BTF, SR1)) { - // Now the shift register and data register contain data(n-2) and data(n-1) - // And I2C is halted so we have time - - // --- Make absolutely sure the next 2 I2C actions are performed with no delay - __I2C_REG_CRITICAL_ZONE_START; - - // First we clear the ACK while the SCL is held low by BTF - i2c_disable_ack(i2c); - - // Now that ACK is cleared we read one byte: instantly the last byte is being clocked in... - trans->buf[periph->idx_buf] = i2c_get_data(i2c); - periph->idx_buf ++; - - // Now the last byte is being clocked. Stop in MUST be set BEFORE the transfer of the last byte is complete - PPRZ_I2C_SEND_STOP(i2c); - - __I2C_REG_CRITICAL_ZONE_STOP; - - - // --- end of critical zone ----------- - - // Document the current Status - periph->status = I2CStopRequested; - - // read the byte2 we had in the buffer (BTF means 2 bytes available) - trans->buf[periph->idx_buf] = i2c_get_data(i2c); - periph->idx_buf ++; - - // Ask for an interrupt to read the last byte (which is normally still busy now) - // The last byte will be received with RXNE - i2c_enable_interrupt(i2c, I2C_CR2_ITBUFEN); - } else { // Event Logic Error - return STMI2C_SubTra_Error; - } - - return STMI2C_SubTra_Busy; -} - -//////////////////////////////////////////////// -// Restore bus conditions to normal after errors - -static inline void i2c_error(struct i2c_periph *periph) -{ - periph->errors->er_irq_cnt++; - if ((I2C_SR1((uint32_t)periph->reg_addr) & I2C_SR1_AF) != 0) { /* Acknowledge failure */ - periph->errors->ack_fail_cnt++; - I2C_SR1((uint32_t)periph->reg_addr) &= ~I2C_SR1_AF; - } - if ((I2C_SR1((uint32_t)periph->reg_addr) & I2C_SR1_BERR) != 0) { /* Misplaced Start or Stop condition */ - periph->errors->miss_start_stop_cnt++; - I2C_SR1((uint32_t)periph->reg_addr) &= ~I2C_SR1_BERR; - } - if ((I2C_SR1((uint32_t)periph->reg_addr) & I2C_SR1_ARLO) != 0) { /* Arbitration lost */ - periph->errors->arb_lost_cnt++; - I2C_SR1((uint32_t)periph->reg_addr) &= ~I2C_SR1_ARLO; - } - if ((I2C_SR1((uint32_t)periph->reg_addr) & I2C_SR1_OVR) != 0) { /* Overrun/Underrun */ - periph->errors->over_under_cnt++; - I2C_SR1((uint32_t)periph->reg_addr) &= ~I2C_SR1_OVR; - } - if ((I2C_SR1((uint32_t)periph->reg_addr) & I2C_SR1_PECERR) != 0) { /* PEC Error in reception */ - periph->errors->pec_recep_cnt++; - I2C_SR1((uint32_t)periph->reg_addr) &= ~I2C_SR1_PECERR; - } - if ((I2C_SR1((uint32_t)periph->reg_addr) & I2C_SR1_TIMEOUT) != 0) { /* Timeout or Tlow error */ - periph->errors->timeout_tlow_cnt++; - I2C_SR1((uint32_t)periph->reg_addr) &= ~I2C_SR1_TIMEOUT; - } - if ((I2C_SR1((uint32_t)periph->reg_addr) & I2C_SR1_SMBALERT) != 0) { /* SMBus alert */ - periph->errors->smbus_alert_cnt++; - I2C_SR1((uint32_t)periph->reg_addr) &= ~I2C_SR1_SMBALERT; - } - - return; -} - - -static inline void stmi2c_clear_pending_interrupts(uint32_t i2c) -{ - uint16_t SR1 = I2C_SR1(i2c); - - // Certainly do not wait for buffer interrupts: - // ------------------------------------------- - i2c_disable_interrupt(i2c, I2C_CR2_ITBUFEN); // Disable TXE, RXNE - - // Error interrupts are handled separately: - // --------------------------------------- - - // Clear Event interrupt conditions: - // -------------------------------- - - // Start Condition Was Generated - if (BIT_X_IS_SET_IN_REG(I2C_SR1_SB, SR1)) { - // SB: cleared by software when reading SR1 and writing to DR - i2c_send_data(i2c, 0x00); - } - // Address Was Sent - if (BIT_X_IS_SET_IN_REG(I2C_SR1_ADDR, SR1)) { - // ADDR: Cleared by software when reading SR1 and then SR2 - uint16_t SR2 __attribute__((unused)) = I2C_SR2(i2c); - } - // Byte Transfer Finished - if (BIT_X_IS_SET_IN_REG(I2C_SR1_BTF, SR1)) { - // SB: cleared by software when reading SR1 and reading/writing to DR - uint8_t dummy __attribute__((unused)) = i2c_get_data(i2c); - i2c_send_data(i2c, 0x00); - } - -} - - -//////////////////////////////////////////////// -// Restore bus conditions to normal after errors - -static inline void i2c_irq(struct i2c_periph *periph) -{ - - /* - There are 7 possible event reasons to get here + all errors - - If IT_EV_FEN - ------------------------- - - We are always interested in all IT_EV_FEV: all are required. - - 1) SB // Start Condition Success in Master mode - 2) ADDR // Address sent received Acknowledge - [ADDR10] // -- 10bit address stuff: not used - [STOPF] // -- only for slaves: master has no stop interrupt: not used - 3) BTF // I2C has stopped working (it is waiting for new data, all buffers are tx_empty/rx_full) - - // Beware: using the buffered I2C has some interesting properties: - - in master receive mode: BTF only occurs after the 2nd received byte: after the first byte is received it is - in RD but the I2C can still receive a second byte. Only when the 2nd byte is received while the RxNE is 1 - then a BTF occurs (I2C can not continue receiving bytes or they will get lost). During BTF I2C is halted (SCL held low) - - in master transmit mode: when writing a byte to WD, you instantly get a new TxE interrupt while the first is not - transmitted yet. The byte was pushed to the I2C shift register and the buffer is ready for more. You can already - fill new data in the buffer while the first is still being transmitted for max performance transmission. - - // Beware: besides data buffering you can/must plan several consecutive actions. You can send 2 bytes to the buffer, ask for a stop and - a new start in one go. - - - thanks to / because of this buffering and event sheduling there is not 1 interrupt per start / byte / stop - This also means you must think more in advance and a transaction could be popped from the transaction stack even before it's - stop condition is actually generated. - - // Beware: the order in which Status (and other register) is read determines how flags are cleared. - You should NOT simply read SR1 & SR2 every time - - If IT_EV_FEN AND IT_EV_BUF - -------------------------- - - Buffer event are not always wanted and are typically switched on during longer data transfers. Make sure to turn off in time. - - 4) RxNE - 5) TxE - - -------------------------------------------------------------------------------------------------- - - The STM waits indefinitely (holding SCL low) for user interaction: - a) after a master-start (waiting for address) - b) after an address (waiting for data) - not during data sending when using buffered - c) after the last byte is transmitted (waiting for either stop or restart) - not during data receiving when using buffered - not after the last byte is received - - - The STM I2C stalls indefinitely when a stop condition was attempted that - did not succeed. The BUSY flag remains on. - - There is no STOP interrupt. - - Caution Reading the status: - - Caution: this clears several flags and can start transmissions etc... - - Certain flags like STOP / (N)ACK need to be guaranteed to be set before - the transmission of the byte is finished. At higher clock rates that can be - quite fast: so we allow no other interrupt to be triggered in between - reading the status and setting all needed flags - - */ - - // Here we go ... - - // Apparently we got an I2C interrupt: EVT BUF or ERR - - // Save Some Direct Access to the I2C Registers ... - uint32_t i2c = (uint32_t) periph->reg_addr; - - ///////////////////////////// - // Check if we were ready ... - if (periph->trans_extract_idx == periph->trans_insert_idx) { - // Nothing Left To Do - - // If we still get an interrupt but there are no more things to do - // (which can happen if an event was sheduled just before a bus error occurs) - // (or can happen if both error and event interrupts were called together [the 2nd will then get this error]) - - // since there is nothing more to do: its easy: just stop: clear all interrupt generating bits - - // Count The Errors - i2c_error(periph); - - // Clear Running Events - stmi2c_clear_pending_interrupts(i2c); - - // Mark this as a special error - periph->errors->last_unexpected_event++; - - // Document the current Status - periph->status = I2CIdle; - - // There are no transactions anymore: return - // further-on in this routine we need a transaction pointer: so we are not allowed to continue - return; - } - - // get the I2C transaction we were working on ... - - enum STMI2CSubTransactionStatus ret = 0; - struct i2c_transaction *trans = periph->trans[periph->trans_extract_idx]; - - /////////////////////////// - // If there was an error: - if ((I2C_SR1(i2c) & I2C_SR1_ERR_MASK) != 0x0000) { - - // Notify everyone about the error ... - - // Set result in transaction - trans->status = I2CTransFailed; - - // Document the current Status - periph->status = I2CFailed; - - // Make sure a TxRx does not Restart - trans->type = I2CTransRx; - - // Count The Errors - i2c_error(periph); - - // Clear Running Events - stmi2c_clear_pending_interrupts(i2c); - - // Now continue as if everything was normal from now on - ret = STMI2C_SubTra_Ready; - - } - - /////////////////////////// - // Normal Event: - else { - - /////////////////////////////////////////////////////////////////////////////////////////////////////// - /////////////////////////////////////////////////////////////////////////////////////////////////////// - // - // SUB-TRANSACTION HANDLER - - if (trans->type == I2CTransRx) { // TxRx are converted to Rx after the Tx Part - switch (trans->len_r) { - case 1: - ret = stmi2c_read1(i2c, periph, trans); - break; - case 2: - ret = stmi2c_read2(i2c, periph, trans); - break; - default: - ret = stmi2c_readmany(i2c, periph, trans); - break; - } - } else { // TxRx or Tx - ret = stmi2c_send(i2c, periph, trans); - } - } - - ///////////////////////////////// - // Sub-transaction has finished - if (ret != STMI2C_SubTra_Busy) { - // Ready or SubTraError - // -ready: with or without stop already asked - - // In case of unexpected event condition during subtransaction handling: - if (ret == STMI2C_SubTra_Error) { - // Tell everyone about the subtransaction error: - // this is the previously called SPURRIOUS INTERRUPT - periph->status = I2CFailed; - trans->type = I2CTransRx; // Avoid possible restart - trans->status = I2CTransFailed; // Notify Ready - periph->errors->unexpected_event_cnt++; - - // Clear Running Events - stmi2c_clear_pending_interrupts(i2c); - } - - // RxTx -> Restart and do Rx part - if (trans->type == I2CTransTxRx) { - trans->type = I2CTransRx; - periph->status = I2CStartRequested; - i2c_send_start(i2c); - - // Silent any BTF that would occur before SB - i2c_send_data(i2c, 0x00); - } - // If a restart is not needed: Rx part or Tx-only - else { - // Ready, no stop condition set yet - if (ret == STMI2C_SubTra_Ready) { - - // Program a stop - PPRZ_I2C_SEND_STOP(i2c); - - // Silent any BTF that would occur before STOP is executed - i2c_send_data(i2c, 0x00); - } - - // Jump to the next transaction - periph->trans_extract_idx++; - if (periph->trans_extract_idx >= I2C_TRANSACTION_QUEUE_LEN) { - periph->trans_extract_idx = 0; - } - - // Tell everyone we are ready - periph->status = I2CIdle; - - - // if we have no more transaction to process, stop here - if (periph->trans_extract_idx == periph->trans_insert_idx) { - - periph->watchdog = -1; // stop watchdog - } - // if not, start next transaction - else { - // Restart transaction doing the Rx part now - // --- moved to idle function - PPRZ_I2C_SEND_START(periph); - // ------ - } - } - } - - return; -} - - -/* -// Make sure the bus is free before resetting (p722) -if (regs->SR2 & (I2C_FLAG_BUSY >> 16)) { -// Reset the I2C block -I2C_SoftwareResetCmd(periph->reg_addr, ENABLE); -I2C_SoftwareResetCmd(periph->reg_addr, DISABLE); -} -*/ - - -#if USE_I2C0 -#error "The STM32 doesn't have I2C0, use I2C1 or I2C2" -#endif - -#if USE_I2C1 - -/** default I2C1 clock speed */ -#ifndef I2C1_CLOCK_SPEED -#define I2C1_CLOCK_SPEED 200000 -#endif -PRINT_CONFIG_VAR(I2C1_CLOCK_SPEED) - -struct i2c_errors i2c1_errors; - -void i2c1_hw_init(void) -{ - i2c1.idle = i2c_stm32_idle; - i2c1.submit = i2c_stm32_submit; - i2c1.setbitrate = i2c_stm32_setbitrate; - - i2c1.reg_addr = (void *)I2C1; - i2c1.init_struct = NULL; - i2c1.errors = &i2c1_errors; - i2c1.watchdog = -1; - - /* zeros error counter */ - ZEROS_ERR_COUNTER(i2c1_errors); - - /* reset peripheral to default state ( sometimes not achieved on reset :( ) */ - //rcc_periph_reset_pulse(RST_I2C1); - - /* Configure and enable I2C1 event interrupt --------------------------------*/ - nvic_set_priority(NVIC_I2C1_EV_IRQ, NVIC_I2C1_IRQ_PRIO); - nvic_enable_irq(NVIC_I2C1_EV_IRQ); - - /* Configure and enable I2C1 err interrupt ----------------------------------*/ - nvic_set_priority(NVIC_I2C1_ER_IRQ, NVIC_I2C1_IRQ_PRIO + 1); - nvic_enable_irq(NVIC_I2C1_ER_IRQ); - - /* Enable peripheral clocks -------------------------------------------------*/ - /* Enable I2C1 clock */ - rcc_periph_clock_enable(RCC_I2C1); - /* setup gpio clock and pins */ - i2c_setup_gpio(I2C1); - - rcc_periph_reset_pulse(RST_I2C1); - - // enable peripheral - i2c_peripheral_enable(I2C1); - - i2c_set_own_7bit_slave_address(I2C1, 0); - - // enable error interrupts - i2c_enable_interrupt(I2C1, I2C_CR2_ITERREN); - - i2c_setbitrate(&i2c1, I2C1_CLOCK_SPEED); -} - -void i2c1_ev_isr(void) -{ - uint32_t i2c = (uint32_t) i2c1.reg_addr; - i2c_disable_interrupt(i2c, I2C_CR2_ITERREN); - i2c1.watchdog = 0; // restart watchdog - i2c_irq(&i2c1); - i2c_enable_interrupt(i2c, I2C_CR2_ITERREN); -} - -void i2c1_er_isr(void) -{ - uint32_t i2c = (uint32_t) i2c1.reg_addr; - i2c_disable_interrupt(i2c, I2C_CR2_ITEVTEN); - i2c1.watchdog = 0; // restart watchdog - i2c_irq(&i2c1); - i2c_enable_interrupt(i2c, I2C_CR2_ITEVTEN); -} - -#endif /* USE_I2C1 */ - -#if USE_I2C2 - -/** default I2C2 clock speed */ -#ifndef I2C2_CLOCK_SPEED -#define I2C2_CLOCK_SPEED 300000 -#endif -PRINT_CONFIG_VAR(I2C2_CLOCK_SPEED) - -struct i2c_errors i2c2_errors; - -void i2c2_hw_init(void) -{ - i2c2.idle = i2c_stm32_idle; - i2c2.submit = i2c_stm32_submit; - i2c2.setbitrate = i2c_stm32_setbitrate; - - i2c2.reg_addr = (void *)I2C2; - i2c2.init_struct = NULL; - i2c2.errors = &i2c2_errors; - i2c2.watchdog = -1; - - /* zeros error counter */ - ZEROS_ERR_COUNTER(i2c2_errors); - - /* reset peripheral to default state ( sometimes not achieved on reset :( ) */ - //rcc_periph_reset_pulse(RST_I2C2); - - /* Configure and enable I2C2 event interrupt --------------------------------*/ - nvic_set_priority(NVIC_I2C2_EV_IRQ, NVIC_I2C2_IRQ_PRIO); - nvic_enable_irq(NVIC_I2C2_EV_IRQ); - - /* Configure and enable I2C2 err interrupt ----------------------------------*/ - nvic_set_priority(NVIC_I2C2_ER_IRQ, NVIC_I2C2_IRQ_PRIO + 1); - nvic_enable_irq(NVIC_I2C2_ER_IRQ); - - /* Enable peripheral clocks -------------------------------------------------*/ - /* Enable I2C2 clock */ - rcc_periph_clock_enable(RCC_I2C2); - - /* setup gpio clock and pins */ - i2c_setup_gpio(I2C2); - - rcc_periph_reset_pulse(RST_I2C2); - - // enable peripheral - i2c_peripheral_enable(I2C2); - - i2c_set_own_7bit_slave_address(I2C2, 0); - - // enable error interrupts - i2c_enable_interrupt(I2C2, I2C_CR2_ITERREN); - - i2c_setbitrate(&i2c2, I2C2_CLOCK_SPEED); -} - -void i2c2_ev_isr(void) -{ - uint32_t i2c = (uint32_t) i2c2.reg_addr; - i2c_disable_interrupt(i2c, I2C_CR2_ITERREN); - i2c2.watchdog = 0; // restart watchdog - i2c_irq(&i2c2); - i2c_enable_interrupt(i2c, I2C_CR2_ITERREN); -} - -void i2c2_er_isr(void) -{ - uint32_t i2c = (uint32_t) i2c2.reg_addr; - i2c_disable_interrupt(i2c, I2C_CR2_ITEVTEN); - i2c2.watchdog = 0; // restart watchdog - i2c_irq(&i2c2); - i2c_enable_interrupt(i2c, I2C_CR2_ITEVTEN); -} - -#endif /* USE_I2C2 */ - - -#if USE_I2C3 && defined STM32F4 - -/** default I2C3 clock speed */ -#ifndef I2C3_CLOCK_SPEED -#define I2C3_CLOCK_SPEED 300000 -#endif -PRINT_CONFIG_VAR(I2C3_CLOCK_SPEED) - -struct i2c_errors i2c3_errors; - -void i2c3_hw_init(void) -{ - i2c3.idle = i2c_stm32_idle; - i2c3.submit = i2c_stm32_submit; - i2c3.setbitrate = i2c_stm32_setbitrate; - - i2c3.reg_addr = (void *)I2C3; - i2c3.init_struct = NULL; - i2c3.errors = &i2c3_errors; - i2c3.watchdog = -1; - - /* zeros error counter */ - ZEROS_ERR_COUNTER(i2c3_errors); - - /* reset peripheral to default state ( sometimes not achieved on reset :( ) */ - //rcc_periph_reset_pulse(RST_I2C3); - - /* Configure and enable I2C3 event interrupt --------------------------------*/ - nvic_set_priority(NVIC_I2C3_EV_IRQ, NVIC_I2C3_IRQ_PRIO); - nvic_enable_irq(NVIC_I2C3_EV_IRQ); - - /* Configure and enable I2C3 err interrupt ----------------------------------*/ - nvic_set_priority(NVIC_I2C3_ER_IRQ, NVIC_I2C3_IRQ_PRIO + 1); - nvic_enable_irq(NVIC_I2C3_ER_IRQ); - - /* Enable peripheral clocks -------------------------------------------------*/ - /* Enable I2C3 clock */ - rcc_periph_clock_enable(RCC_I2C3); - - /* setup gpio clock and pins */ - i2c_setup_gpio(I2C3); - - rcc_periph_reset_pulse(RST_I2C3); - - // enable peripheral - i2c_peripheral_enable(I2C3); - - i2c_set_own_7bit_slave_address(I2C3, 0); - - // enable error interrupts - i2c_enable_interrupt(I2C3, I2C_CR2_ITERREN); - - i2c_setbitrate(&i2c3, I2C3_CLOCK_SPEED); -} - -void i2c3_ev_isr(void) -{ - uint32_t i2c = (uint32_t) i2c3.reg_addr; - i2c_disable_interrupt(i2c, I2C_CR2_ITERREN); - i2c3.watchdog = 0; // restart watchdog - i2c_irq(&i2c3); - i2c_enable_interrupt(i2c, I2C_CR2_ITERREN); -} - -void i2c3_er_isr(void) -{ - uint32_t i2c = (uint32_t) i2c3.reg_addr; - i2c_disable_interrupt(i2c, I2C_CR2_ITEVTEN); - i2c3.watchdog = 0; // restart watchdog - i2c_irq(&i2c3); - i2c_enable_interrupt(i2c, I2C_CR2_ITEVTEN); -} - -#endif /* USE_I2C3 */ - -////////////////////////////////////////////////// -// Set Bitrate to Match your application: -// -short wires, low capacitance bus: IMU: high speed -// -long wires with a lot of capacitance: motor controller: put speed as low as possible - -static void i2c_stm32_setbitrate(struct i2c_periph *periph, int bitrate) -{ - // If NOT Busy - if (i2c_idle(periph)) { - volatile int devider; - volatile int risetime; - - uint32_t i2c = (uint32_t) periph->reg_addr; - - /***************************************************** - Bitrate: - - -CR2 + CCR + TRISE registers - -only change when PE=0 - - e.g. - - 10kHz: 36MHz + Standard 0x708 + 0x25 - 70kHz: 36MHz + Standard 0x101 + - 400kHz: 36MHz + Fast 0x1E + 0xb - - // 1) Program peripheral input clock CR2: to get correct timings - // 2) Configure clock control registers - // 3) Configure rise time register - ******************************************************/ - - if (bitrate < 3000) { - bitrate = 3000; - } - - // rcc_apb1_frequency is normally configured to max: 36MHz on F1 and 42MHz on F4 - // in fast mode: 2counts low 1 count high -> / 3: - // in standard mode: 1 count low, 1 count high -> /2: - devider = (rcc_apb1_frequency / 2000) / (bitrate / 1000); - - // never allow faster than 600kbps - if (devider < 20) { - devider = 20; - } - - // no overflow either - if (devider >= 4095) { - devider = 4095; - } - - // risetime can be up to 1/6th of the period - risetime = 1000000 / (bitrate / 1000) / 6 / 28; - - if (risetime < 10) { - risetime = 10; - } - - // more will overflow the register: for more you should lower the FREQ - if (risetime >= 31) { - risetime = 31; - } - - // we do not expect an interrupt as the interface should have been idle, but just in case... - __disable_irq(); // this code is in user space: - - // CCR can only be written when PE is disabled - // p731 note 5 - i2c_peripheral_disable(i2c); - - // 1) -#ifdef STM32F1 - i2c_set_clock_frequency(i2c, 36); -#else // STM32F4 - i2c_set_clock_frequency(i2c, 42); -#endif - // 2) - //i2c_set_fast_mode(i2c); - i2c_set_ccr(i2c, devider); - // 3) - i2c_set_trise(i2c, risetime); - - // Re-Enable - i2c_peripheral_enable(i2c); - - __enable_irq(); - - } -} - -#define WD_DELAY 20 // number of ticks with 2ms - 40ms delay before resetting the bus -#define WD_RECOVERY_TICKS 18 // number of generated SCL clocking pulses - -#if USE_I2C1 || USE_I2C2 || USE_I2C3 -static inline void i2c_scl_set(uint32_t i2c) -{ -#if USE_I2C1 - if (i2c == I2C1) { - gpio_set(I2C1_GPIO_PORT, I2C1_GPIO_SCL); - } -#endif -#if USE_I2C2 - if (i2c == I2C2) { - gpio_set(I2C2_GPIO_PORT, I2C2_GPIO_SCL); - } -#endif -#if USE_I2C3 - if (i2c == I2C3) { - gpio_set(I2C3_GPIO_PORT_SCL, I2C3_GPIO_SCL); - } -#endif -} - -static inline void i2c_scl_toggle(uint32_t i2c) -{ -#if USE_I2C1 - if (i2c == I2C1) { - gpio_toggle(I2C1_GPIO_PORT, I2C1_GPIO_SCL); - } -#endif -#if USE_I2C2 - if (i2c == I2C2) { - gpio_toggle(I2C2_GPIO_PORT, I2C2_GPIO_SCL); - } -#endif -#if USE_I2C3 - if (i2c == I2C3) { - gpio_toggle(I2C3_GPIO_PORT_SCL, I2C3_GPIO_SCL); - } -#endif -} - -static inline bool i2c_sda_get(uint32_t i2c) -{ - bool res = false; -#if USE_I2C1 - if (i2c == I2C1) { - res = gpio_get(I2C1_GPIO_PORT, I2C1_GPIO_SDA); - } -#endif -#if USE_I2C2 - if (i2c == I2C2) { - res = gpio_get(I2C2_GPIO_PORT, I2C2_GPIO_SDA); - } -#endif -#if USE_I2C3 - if (i2c == I2C3) { - res = gpio_get(I2C3_GPIO_PORT_SDA, I2C3_GPIO_SDA); - } -#endif - return res; -} - -static void i2c_wd_check(struct i2c_periph *periph) -{ - uint32_t i2c = (uint32_t) periph->reg_addr; - - if (periph->watchdog > WD_DELAY) { - if (periph->watchdog == WD_DELAY + 1) { - - i2c_disable_interrupt(i2c, I2C_CR2_ITEVTEN); - i2c_disable_interrupt(i2c, I2C_CR2_ITERREN); - - periph->trans_insert_idx = 0; - periph->trans_extract_idx = 0; - periph->status = I2CIdle; - struct i2c_transaction *trans; - for (uint8_t i = 0; i < I2C_TRANSACTION_QUEUE_LEN; i++) { - trans = periph->trans[i]; - trans->status = I2CTransFailed; - } - - i2c_peripheral_disable(i2c); - -#if USE_I2C1 - if (i2c == I2C1) { - gpio_setup_output(I2C1_GPIO_PORT, I2C1_GPIO_SCL); - gpio_setup_input(I2C1_GPIO_PORT, I2C1_GPIO_SDA); - } -#endif -#if USE_I2C2 - if (i2c == I2C2) { - gpio_setup_output(I2C2_GPIO_PORT, I2C2_GPIO_SCL); - gpio_setup_input(I2C2_GPIO_PORT, I2C2_GPIO_SDA); - } -#endif -#if USE_I2C3 - if (i2c == I2C3) { - gpio_setup_output(I2C3_GPIO_PORT_SCL, I2C3_GPIO_SCL); - gpio_setup_input(I2C3_GPIO_PORT_SDA, I2C3_GPIO_SDA); - } -#endif - - i2c_scl_set(i2c); - } else if (periph->watchdog < WD_DELAY + WD_RECOVERY_TICKS) { - if (i2c_sda_get(i2c)) { - periph->watchdog = WD_DELAY + WD_RECOVERY_TICKS; - } - i2c_scl_toggle(i2c); - } else { - i2c_scl_set(i2c); - - /* setup gpios for normal i2c operation again */ - i2c_setup_gpio(i2c); - - i2c_reset(i2c); - - i2c_peripheral_enable(i2c); - i2c_set_own_7bit_slave_address(i2c, 0); - i2c_enable_interrupt(i2c, I2C_CR2_ITERREN); - -#if USE_I2C1 - if (i2c == I2C1) { - i2c_setbitrate(periph, I2C1_CLOCK_SPEED); - } -#endif -#if USE_I2C2 - if (i2c == I2C2) { - i2c_setbitrate(periph, I2C2_CLOCK_SPEED); - } -#endif -#if USE_I2C3 - if (i2c == I2C3) { - i2c_setbitrate(periph, I2C3_CLOCK_SPEED); - } -#endif - - periph->watchdog = 0; // restart watchdog - - periph->errors->wd_reset_cnt++; - - return; - } - } - if (periph->watchdog >= 0) { - periph->watchdog++; - } -} -#endif // USE_I2Cx - -#include "mcu_periph/sys_time.h" - -void i2c_event(void) -{ - static uint32_t i2c_wd_timer; - - if (SysTimeTimer(i2c_wd_timer) > 2000) { // 2ms (500Hz) periodic watchdog check - SysTimeTimerStart(i2c_wd_timer); -#if USE_I2C1 - i2c_wd_check(&i2c1); -#endif - -#if USE_I2C2 - i2c_wd_check(&i2c2); -#endif -#if USE_I2C3 - i2c_wd_check(&i2c3); -#endif - } -} - -///////////////////////////////////////////////////////// -// Implement Interface Functions - -static bool i2c_stm32_submit(struct i2c_periph *periph, struct i2c_transaction *t) -{ - if (periph->watchdog > WD_DELAY) { - return false; - } - - uint8_t temp; - temp = periph->trans_insert_idx + 1; - if (temp >= I2C_TRANSACTION_QUEUE_LEN) { temp = 0; } - if (temp == periph->trans_extract_idx) { - // queue full - periph->errors->queue_full_cnt++; - t->status = I2CTransFailed; - return false; - } - - t->status = I2CTransPending; - - __disable_irq(); - /* put transacation in queue */ - periph->trans[periph->trans_insert_idx] = t; - periph->trans_insert_idx = temp; - - /* if peripheral is idle, start the transaction */ - // if (PPRZ_I2C_IS_IDLE(p)) - if (periph->status == I2CIdle) { - //if (i2c_idle(periph)) - { - PPRZ_I2C_SEND_START(periph); - } - } - /* else it will be started by the interrupt handler when the previous transactions completes */ - __enable_irq(); - - return true; -} - -static bool i2c_stm32_idle(struct i2c_periph *periph) -{ - // This is actually a difficult function: - // -simply reading the status flags can clear bits and corrupt the transaction - - uint32_t i2c = (uint32_t) periph->reg_addr; - - // First we check if the software thinks it is ready - if (periph->status == I2CIdle) { - return !(BIT_X_IS_SET_IN_REG(I2C_SR2_BUSY, I2C_SR2(i2c))); - } else { - return false; - } -} - diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.h b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.h deleted file mode 100644 index 6fbb46684e..0000000000 --- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.h +++ /dev/null @@ -1,61 +0,0 @@ -/* - * Copyright (C) 2009-2012 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 arch/stm32/mcu_periph/i2c_arch.h - * @ingroup stm32_arch - * - * Hardware level I2C handling for the STM32. - */ - -#ifndef I2C_HW_H -#define I2C_HW_H - -#include - -#if USE_I2C1 - -extern void i2c1_hw_init(void); -extern void i2c1_ev_irq_handler(void); -extern void i2c1_er_irq_handler(void); - -#endif /* USE_I2C1 */ - - -#if USE_I2C2 - -extern void i2c2_hw_init(void); -extern void i2c2_ev_irq_handler(void); -extern void i2c2_er_irq_handler(void); - -#endif /* USE_I2C2 */ - - -#if USE_I2C3 && defined STM32F4 - -extern void i2c3_hw_init(void); -extern void i2c3_ev_irq_handler(void); -extern void i2c3_er_irq_handler(void); - -#endif /* USE_I2C3 */ - - -#endif /* I2C_HW_H */ diff --git a/sw/airborne/arch/stm32/mcu_periph/pwm_input_arch.c b/sw/airborne/arch/stm32/mcu_periph/pwm_input_arch.c deleted file mode 100644 index f9a8594271..0000000000 --- a/sw/airborne/arch/stm32/mcu_periph/pwm_input_arch.c +++ /dev/null @@ -1,354 +0,0 @@ -/* - * Copyright (C) 2014 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 arch/stm32/mcu_periph/pwm_input_arch.c - * @ingroup stm32_arch - * - * handling of smt32 PWM input using a timer with capture. - */ -#include "mcu_periph/pwm_input.h" - -#include BOARD_CONFIG -#include "generated/airframe.h" - -#include -#include -#include -#include - -#include "mcu_periph/sys_time.h" -#include "mcu_periph/gpio.h" - -// for timer_get_frequency -#include "mcu_arch.h" - -#define ONE_MHZ_CLK 1000000 -#ifdef NVIC_TIM_IRQ_PRIO -#define PWM_INPUT_IRQ_PRIO NVIC_TIM_IRQ_PRIO -#else -#define PWM_INPUT_IRQ_PRIO 2 -#endif - -static inline void pwm_input_set_timer(uint32_t tim, uint32_t ticks_per_usec) -{ - timer_set_mode(tim, TIM_CR1_CKD_CK_INT, TIM_CR1_CMS_EDGE, TIM_CR1_DIR_UP); - timer_set_period(tim, 0xFFFF); - uint32_t timer_clk = timer_get_frequency(tim); - timer_set_prescaler(tim, (timer_clk / (ticks_per_usec * ONE_MHZ_CLK)) - 1); - timer_enable_counter(tim); -} - -void pwm_input_init(void) -{ - int i; - // initialize the arrays to 0 - for (i = 0; i < PWM_INPUT_NB; i++) { - pwm_input_duty_tics[i] = 0; - pwm_input_duty_valid[i] = 0; - pwm_input_period_tics[i] = 0; - pwm_input_period_valid[i] = 0; - } - - /** Configure timers - * - timer clock enable - * - base configuration - * - enable counter - */ -#if USE_PWM_INPUT_TIM1 - rcc_periph_clock_enable(RCC_TIM1); - rcc_periph_reset_pulse(RST_TIM1); - pwm_input_set_timer(TIM1, TIM1_TICKS_PER_USEC); -#endif -#if USE_PWM_INPUT_TIM2 - rcc_periph_clock_enable(RCC_TIM2); - rcc_periph_reset_pulse(RST_TIM2); - pwm_input_set_timer(TIM2, TIM2_TICKS_PER_USEC); -#endif -#if USE_PWM_INPUT_TIM3 - rcc_periph_clock_enable(RCC_TIM3); - rcc_periph_reset_pulse(RST_TIM3); - pwm_input_set_timer(TIM3, TIM3_TICKS_PER_USEC); -#endif -#if USE_PWM_INPUT_TIM4 - rcc_periph_clock_enable(RCC_TIM4); - rcc_periph_reset_pulse(RST_TIM4); - pwm_input_set_timer(TIM4, TIM4_TICKS_PER_USEC); -#endif -#if USE_PWM_INPUT_TIM5 - rcc_periph_clock_enable(RCC_TIM5); - rcc_periph_reset_pulse(RST_TIM5); - pwm_input_set_timer(TIM5, TIM5_TICKS_PER_USEC); -#endif -#if USE_PWM_INPUT_TIM8 - rcc_periph_clock_enable(RCC_TIM8); - rcc_periph_reset_pulse(RST_TIM8); - pwm_input_set_timer(TIM8, TIM8_TICKS_PER_USEC); -#endif -#if USE_PWM_INPUT_TIM9 - rcc_periph_clock_enable(RCC_TIM9); - rcc_periph_reset_pulse(RST_TIM9); - pwm_input_set_timer(TIM9, TIM9_TICKS_PER_USEC); -#endif - -#ifdef USE_PWM_INPUT1 - /* GPIO configuration as input capture for timer */ - gpio_setup_pin_af(PWM_INPUT1_GPIO_PORT, PWM_INPUT1_GPIO_PIN, PWM_INPUT1_GPIO_AF, FALSE); - - /** TIM configuration: Input Capture mode - * Two IC signals are mapped to the same TI input - */ - timer_ic_set_input(PWM_INPUT1_TIMER, PWM_INPUT1_CHANNEL_PERIOD, PWM_INPUT1_TIMER_INPUT); - timer_ic_set_input(PWM_INPUT1_TIMER, PWM_INPUT1_CHANNEL_DUTY, PWM_INPUT1_TIMER_INPUT); -#if USE_PWM_INPUT1 == PWM_PULSE_TYPE_ACTIVE_LOW - timer_ic_set_polarity(PWM_INPUT1_TIMER, PWM_INPUT1_CHANNEL_PERIOD, TIM_IC_RISING); - timer_ic_set_polarity(PWM_INPUT1_TIMER, PWM_INPUT1_CHANNEL_DUTY, TIM_IC_FALLING); -#elif USE_PWM_INPUT1 == PWM_PULSE_TYPE_ACTIVE_HIGH - timer_ic_set_polarity(PWM_INPUT1_TIMER, PWM_INPUT1_CHANNEL_PERIOD, TIM_IC_FALLING); - timer_ic_set_polarity(PWM_INPUT1_TIMER, PWM_INPUT1_CHANNEL_DUTY, TIM_IC_RISING); -#endif - - /* Select the valid trigger input */ - timer_slave_set_trigger(PWM_INPUT1_TIMER, PWM_INPUT1_SLAVE_TRIG); - /* Configure the slave mode controller in reset mode */ - timer_slave_set_mode(PWM_INPUT1_TIMER, TIM_SMCR_SMS_RM); - - /* Enable timer Interrupt(s). */ - nvic_set_priority(PWM_INPUT1_IRQ, PWM_INPUT_IRQ_PRIO); - nvic_enable_irq(PWM_INPUT1_IRQ); -#ifdef PWM_INPUT1_IRQ2 - nvic_set_priority(PWM_INPUT1_IRQ2, PWM_INPUT_IRQ_PRIO); - nvic_enable_irq(PWM_INPUT1_IRQ2); -#endif - - /* Enable the Capture/Compare and Update interrupt requests. */ - timer_enable_irq(PWM_INPUT1_TIMER, (PWM_INPUT1_CC_IE | TIM_DIER_UIE)); - - /* Enable capture channel. */ - timer_ic_enable(PWM_INPUT1_TIMER, PWM_INPUT1_CHANNEL_PERIOD); - timer_ic_enable(PWM_INPUT1_TIMER, PWM_INPUT1_CHANNEL_DUTY); -#endif - -#ifdef USE_PWM_INPUT2 - /* GPIO configuration as input capture for timer */ - gpio_setup_pin_af(PWM_INPUT2_GPIO_PORT, PWM_INPUT2_GPIO_PIN, PWM_INPUT2_GPIO_AF, FALSE); - - /** TIM configuration: Input Capture mode - * Two IC signals are mapped to the same TI input - */ - timer_ic_set_input(PWM_INPUT2_TIMER, PWM_INPUT2_CHANNEL_PERIOD, PWM_INPUT2_TIMER_INPUT); - timer_ic_set_input(PWM_INPUT2_TIMER, PWM_INPUT2_CHANNEL_DUTY, PWM_INPUT2_TIMER_INPUT); -#if USE_PWM_INPUT2 == PWM_PULSE_TYPE_ACTIVE_LOW - timer_ic_set_polarity(PWM_INPUT2_TIMER, PWM_INPUT2_CHANNEL_PERIOD, TIM_IC_RISING); - timer_ic_set_polarity(PWM_INPUT2_TIMER, PWM_INPUT2_CHANNEL_DUTY, TIM_IC_FALLING); -#elif USE_PWM_INPUT2 == PWM_PULSE_TYPE_ACTIVE_HIGH - timer_ic_set_polarity(PWM_INPUT2_TIMER, PWM_INPUT2_CHANNEL_PERIOD, TIM_IC_FALLING); - timer_ic_set_polarity(PWM_INPUT2_TIMER, PWM_INPUT2_CHANNEL_DUTY, TIM_IC_RISING); -#endif - - /* Select the valid trigger input */ - timer_slave_set_trigger(PWM_INPUT2_TIMER, PWM_INPUT2_SLAVE_TRIG); - /* Configure the slave mode controller in reset mode */ - timer_slave_set_mode(PWM_INPUT2_TIMER, TIM_SMCR_SMS_RM); - - /* Enable timer Interrupt(s). */ - nvic_set_priority(PWM_INPUT2_IRQ, PWM_INPUT_IRQ_PRIO); - nvic_enable_irq(PWM_INPUT2_IRQ); -#ifdef PWM_INPUT2_IRQ2 - nvic_set_priority(PWM_INPUT2_IRQ2, PWM_INPUT_IRQ_PRIO); - nvic_enable_irq(PWM_INPUT2_IRQ2); -#endif - - /* Enable the Capture/Compare and Update interrupt requests. */ - timer_enable_irq(PWM_INPUT2_TIMER, (PWM_INPUT2_CC_IE | TIM_DIER_UIE)); - - /* Enable capture channel. */ - timer_ic_enable(PWM_INPUT2_TIMER, PWM_INPUT2_CHANNEL_PERIOD); - timer_ic_enable(PWM_INPUT2_TIMER, PWM_INPUT2_CHANNEL_DUTY); -#endif - -} - - -#if USE_PWM_INPUT_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_clear_flag(TIM1, TIM_SR_UIF); - // FIXME clear overflow interrupt but what else ? - } -} - -void tim1_cc_isr(void) { - if ((TIM1_SR & TIM1_CC_IF_PERIOD) != 0) { - timer_clear_flag(TIM1, TIM1_CC_IF_PERIOD); - pwm_input_period_tics[TIM1_PWM_INPUT_IDX] = TIM1_CCR_PERIOD; - pwm_input_period_valid[TIM1_PWM_INPUT_IDX] = true; - } - if ((TIM1_SR & TIM1_CC_IF_DUTY) != 0) { - timer_clear_flag(TIM1, TIM1_CC_IF_DUTY); - pwm_input_duty_tics[TIM1_PWM_INPUT_IDX] = TIM1_CCR_DUTY; - pwm_input_duty_valid[TIM1_PWM_INPUT_IDX] = true; - } -} - -#endif - -#if USE_PWM_INPUT_TIM2 - -void tim2_isr(void) { - if ((TIM2_SR & TIM2_CC_IF_PERIOD) != 0) { - timer_clear_flag(TIM2, TIM2_CC_IF_PERIOD); - pwm_input_period_tics[TIM2_PWM_INPUT_IDX] = TIM2_CCR_PERIOD; - pwm_input_period_valid[TIM2_PWM_INPUT_IDX] = true; - } - if ((TIM2_SR & TIM2_CC_IF_DUTY) != 0) { - timer_clear_flag(TIM2, TIM2_CC_IF_DUTY); - pwm_input_duty_tics[TIM2_PWM_INPUT_IDX] = TIM2_CCR_DUTY; - pwm_input_duty_valid[TIM2_PWM_INPUT_IDX] = true; - } - if ((TIM2_SR & TIM_SR_UIF) != 0) { - timer_clear_flag(TIM2, TIM_SR_UIF); - // FIXME clear overflow interrupt but what else ? - } -} - -#endif - -#if USE_PWM_INPUT_TIM3 - -void tim3_isr(void) { - if ((TIM3_SR & TIM3_CC_IF_PERIOD) != 0) { - timer_clear_flag(TIM3, TIM3_CC_IF_PERIOD); - pwm_input_period_tics[TIM3_PWM_INPUT_IDX] = TIM3_CCR_PERIOD; - pwm_input_period_valid[TIM3_PWM_INPUT_IDX] = true; - } - if ((TIM3_SR & TIM3_CC_IF_DUTY) != 0) { - timer_clear_flag(TIM3, TIM3_CC_IF_DUTY); - pwm_input_duty_tics[TIM3_PWM_INPUT_IDX] = TIM3_CCR_DUTY; - pwm_input_duty_valid[TIM3_PWM_INPUT_IDX] = true; - } - if ((TIM3_SR & TIM_SR_UIF) != 0) { - timer_clear_flag(TIM3, TIM_SR_UIF); - // FIXME clear overflow interrupt but what else ? - } -} - -#endif - -#if USE_PWM_INPUT_TIM4 - -void tim4_isr(void) { - if ((TIM4_SR & TIM4_CC_IF_PERIOD) != 0) { - timer_clear_flag(TIM4, TIM4_CC_IF_PERIOD); - pwm_input_period_tics[TIM4_PWM_INPUT_IDX] = TIM4_CCR_PERIOD; - pwm_input_period_valid[TIM4_PWM_INPUT_IDX] = true; - } - if ((TIM4_SR & TIM4_CC_IF_DUTY) != 0) { - timer_clear_flag(TIM4, TIM4_CC_IF_DUTY); - pwm_input_duty_tics[TIM4_PWM_INPUT_IDX] = TIM4_CCR_DUTY; - pwm_input_duty_valid[TIM4_PWM_INPUT_IDX] = true; - } - if ((TIM4_SR & TIM_SR_UIF) != 0) { - timer_clear_flag(TIM4, TIM_SR_UIF); - // FIXME clear overflow interrupt but what else ? - } -} - -#endif - -#if USE_PWM_INPUT_TIM5 - -void tim5_isr(void) { - if ((TIM5_SR & TIM5_CC_IF_PERIOD) != 0) { - timer_clear_flag(TIM5, TIM5_CC_IF_PERIOD); - pwm_input_period_tics[TIM5_PWM_INPUT_IDX] = TIM5_CCR_PERIOD; - pwm_input_period_valid[TIM5_PWM_INPUT_IDX] = true; - } - if ((TIM5_SR & TIM5_CC_IF_DUTY) != 0) { - timer_clear_flag(TIM5, TIM5_CC_IF_DUTY); - pwm_input_duty_tics[TIM5_PWM_INPUT_IDX] = TIM5_CCR_DUTY; - pwm_input_duty_valid[TIM5_PWM_INPUT_IDX] = true; - } - if ((TIM5_SR & TIM_SR_UIF) != 0) { - timer_clear_flag(TIM5, TIM_SR_UIF); - // FIXME clear overflow interrupt but what else ? - } -} - -#endif - -#if USE_PWM_INPUT_TIM8 - -#if defined(STM32F1) -void tim8_up_isr(void) -{ -#elif defined(STM32F4) -void tim8_up_tim13_isr(void) { -#endif - if ((TIM8_SR & TIM_SR_UIF) != 0) { - timer_clear_flag(TIM8, TIM_SR_UIF); - // FIXME clear overflow interrupt but what else ? - } -} - -void tim8_cc_isr(void) { - if ((TIM8_SR & TIM8_CC_IF_PERIOD) != 0) { - timer_clear_flag(TIM8, TIM8_CC_IF_PERIOD); - pwm_input_period_tics[TIM8_PWM_INPUT_IDX] = TIM8_CCR_PERIOD; - pwm_input_period_valid[TIM8_PWM_INPUT_IDX] = true; - } - if ((TIM8_SR & TIM8_CC_IF_DUTY) != 0) { - timer_clear_flag(TIM8, TIM8_CC_IF_DUTY); - pwm_input_duty_tics[TIM8_PWM_INPUT_IDX] = TIM8_CCR_DUTY; - pwm_input_duty_valid[TIM8_PWM_INPUT_IDX] = true; - } -} - -#endif - -#if USE_PWM_INPUT_TIM9 - -// TIM1 break interrupt (which we don't care here) and TIM9 global interrupt -void tim1_brk_tim9_isr(void) { - if ((TIM9_SR & TIM9_CC_IF_PERIOD) != 0) { - timer_clear_flag(TIM9, TIM9_CC_IF_PERIOD); - pwm_input_period_tics[TIM9_PWM_INPUT_IDX] = TIM9_CCR_PERIOD; - pwm_input_period_valid[TIM9_PWM_INPUT_IDX] = true; - } - if ((TIM9_SR & TIM9_CC_IF_DUTY) != 0) { - timer_clear_flag(TIM9, TIM9_CC_IF_DUTY); - pwm_input_duty_tics[TIM9_PWM_INPUT_IDX] = TIM9_CCR_DUTY; - pwm_input_duty_valid[TIM9_PWM_INPUT_IDX] = true; - } - if ((TIM9_SR & TIM_SR_UIF) != 0) { - timer_clear_flag(TIM9, TIM_SR_UIF); - // FIXME clear overflow interrupt but what else ? - } -} - -#endif - diff --git a/sw/airborne/arch/stm32/mcu_periph/pwm_input_arch.h b/sw/airborne/arch/stm32/mcu_periph/pwm_input_arch.h deleted file mode 100644 index 8529d87b38..0000000000 --- a/sw/airborne/arch/stm32/mcu_periph/pwm_input_arch.h +++ /dev/null @@ -1,61 +0,0 @@ -/* - * Copyright (C) 2014 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 arch/stm32/mcu_periph/pwm_input_arch.h - * @ingroup stm32_arch - * - * handling of smt32 PWM input using a timer with capture. - */ - -#ifndef PWM_INPUT_ARCH_H -#define PWM_INPUT_ARCH_H - -#include "std.h" - -enum pwm_input_channels { - PWM_INPUT1, - PWM_INPUT2, - PWM_INPUT_NB -}; - -/** - * The default pwm counter is set-up to have 1/6 us resolution. - * - * The timer clock frequency (before prescaling): - * STM32F1: - * TIM1 -> APB2 = HCLK = 72MHz - * TIM2 -> 2 * APB1 = 2 * 36MHz = 72MHz - * STM32F4: - * TIM1 -> 2 * APB2 = 2 * 84MHz = 168MHz - * TIM2 -> 2 * APB1 = 2 * 42MHz = 84MHz - */ -#ifndef PWM_INPUT1_TICKS_PER_USEC -#define PWM_INPUT1_TICKS_PER_USEC 6 -#endif - -#ifndef PWM_INPUT2_TICKS_PER_USEC -#define PWM_INPUT2_TICKS_PER_USEC 6 -#endif - -#endif /* PWM_INPUT_ARCH_H */ - diff --git a/sw/airborne/arch/stm32/mcu_periph/rng_arch.c b/sw/airborne/arch/stm32/mcu_periph/rng_arch.c deleted file mode 100644 index fec5d45a22..0000000000 --- a/sw/airborne/arch/stm32/mcu_periph/rng_arch.c +++ /dev/null @@ -1,68 +0,0 @@ -/* - * Copyright (C) 2017 Michal Podhradsky - * - * 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 rng_arch.c - * \brief arch specific Random Number Generator API - * - */ -#include "mcu_periph/rng.h" -#include -#include - -uint32_t last; - -void rng_init(void) { - rcc_periph_clock_enable(RCC_RNG); - rng_enable(); - // dont forget to throw away the first generated number - last = rng_wait_and_get(); -} - -void rng_deinit(void) { - rng_disable(); - rcc_periph_clock_disable(RCC_RNG); -} - -// Return true only if we got a new number -// that is different from the previous one -bool rng_get(uint32_t *rand_nr) { - uint32_t tmp = 0; - if (rng_get_random(&tmp) && (tmp != last)) { - last = tmp; - *rand_nr = tmp; - return true; - } else { - return false; - } -} - -// Wait until we get a new number that is different -// from the previous one. We can wait forever here if -// the clocks are not setup properly. -uint32_t rng_wait_and_get(void) { - uint32_t tmp = last; - while (tmp == last) { - tmp = rng_get_random_blocking(); - } - last = tmp; - return tmp; -} diff --git a/sw/airborne/arch/stm32/mcu_periph/spi_arch.c b/sw/airborne/arch/stm32/mcu_periph/spi_arch.c deleted file mode 100644 index 521a4e93da..0000000000 --- a/sw/airborne/arch/stm32/mcu_periph/spi_arch.c +++ /dev/null @@ -1,1738 +0,0 @@ -/* - * Copyright (C) 2005-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 arch/stm32/mcu_periph/spi_arch.c - * @ingroup stm32_arch - * - * Handling of SPI hardware for STM32. - * SPI Master code. - * - * When a transaction is submitted: - * - The transaction is added to the queue if there is space, - * otherwise it returns false - * - The pending state is set - * - SPI Interrupts (in this case the DMA interrupts) are disabled - * to prevent race conditions - * - The slave is selected if required, then the before_cb callback is run - * - The spi and dma registers are set up for the specific transaction - * - Spi, DMA and interrupts are enabled and the transaction starts - * - * Obviously output_length and input_length will never both be 0 at the same time. - * In this case, spi_submit will just return false. - * - * For the DMA and interrupts: - * - If the output_len != input_len, a dummy DMA transfer is triggered for - * the remainder so the same amount of data is moved in and out. - * This simplifies keeping the clock going if output_len is greater and allows - * the rx dma interrupt to represent that the transaction has fully completed. - * - The dummy DMA transfer is initiated at the transaction setup if length is 0, - * otherwise after the first dma interrupt completes in the ISR directly. - * - The rx DMA transfer completed interrupt marks the end of a complete transaction. - * - The after_cb callback happens BEFORE the slave is unselected as configured. - */ - -#include -#include -#include -#include -#include -#include - -#include "mcu_periph/spi.h" -#include "mcu_periph/gpio.h" - -#include BOARD_CONFIG - -#ifdef SPI_MASTER - -#ifndef NVIC_SPI_IRQ_PRIO -#define NVIC_SPI_IRQ_PRIO 0 -#endif - - -/** - * Libopencm3 specifc communication parameters for a SPI peripheral in master mode. - */ -struct locm3_spi_comm { - uint32_t br; ///< baudrate (clock divider) - uint32_t cpol; ///< clock polarity - uint32_t cpha; ///< clock phase - uint32_t dff; ///< data frame format 8/16 bits - uint32_t lsbfirst; ///< frame format lsb/msb first -}; - -/** - * This structure keeps track of specific config for each SPI bus, - * which allows for more code reuse. - */ -struct spi_periph_dma { - uint32_t spi; ///< SPI peripheral identifier - uint32_t spidr; ///< SPI DataRegister address for DMA - uint32_t dma; ///< DMA controller base address (DMA1 or DMA2) - uint32_t rcc_dma; ///< RCC DMA enable clock pin (RCC_DMA1 or RCC_DMA2) - uint8_t rx_chan; ///< receive DMA channel (or stream on F4) number - uint8_t tx_chan; ///< transmit DMA channel (or stream on F4) number - uint32_t rx_chan_sel; ///< F4 only: actual receive DMA channel number - uint32_t tx_chan_sel; ///< F4 only: actual transmit DMA channel number - uint8_t rx_nvic_irq; ///< receive interrupt - uint8_t tx_nvic_irq; ///< transmit interrupt - uint16_t tx_dummy_buf; ///< dummy tx buffer for receive only cases - bool tx_extra_dummy_dma; ///< extra tx dummy dma flag for tx_len < rx_len - uint16_t rx_dummy_buf; ///< dummy rx buffer for receive only cases - bool rx_extra_dummy_dma; ///< extra rx dummy dma flag for tx_len > rx_len - struct locm3_spi_comm comm; ///< current communication parameters - uint8_t comm_sig; ///< comm config signature used to check for changes -}; - - -#if USE_SPI0 -#error "The STM32 doesn't have SPI0" -#endif -#if USE_SPI1 -static struct spi_periph_dma spi1_dma; -#endif -#if USE_SPI2 -static struct spi_periph_dma spi2_dma; -#endif -#if USE_SPI3 -static struct spi_periph_dma spi3_dma; -#endif - -static void spi_start_dma_transaction(struct spi_periph *periph, struct spi_transaction *_trans); -static void spi_next_transaction(struct spi_periph *periph); -static void spi_configure_dma(uint32_t dma, uint32_t rcc_dma, uint8_t chan, uint32_t periph_addr, uint32_t buf_addr, - uint16_t len, enum SPIDataSizeSelect dss, bool increment); -static void __attribute__((unused)) process_rx_dma_interrupt(struct spi_periph *periph); -static void __attribute__((unused)) process_tx_dma_interrupt(struct spi_periph *periph); -static void spi_arch_int_enable(struct spi_periph *spi); -static void spi_arch_int_disable(struct spi_periph *spi); - - -/****************************************************************************** - * - * Handling of Slave Select outputs - * - *****************************************************************************/ - -static inline void SpiSlaveUnselect(uint8_t slave) -{ - switch (slave) { -#if USE_SPI_SLAVE0 - case 0: - gpio_set(SPI_SELECT_SLAVE0_PORT, SPI_SELECT_SLAVE0_PIN); - break; -#endif // USE_SPI_SLAVE0 -#if USE_SPI_SLAVE1 - case 1: - gpio_set(SPI_SELECT_SLAVE1_PORT, SPI_SELECT_SLAVE1_PIN); - break; -#endif //USE_SPI_SLAVE1 -#if USE_SPI_SLAVE2 - case 2: - gpio_set(SPI_SELECT_SLAVE2_PORT, SPI_SELECT_SLAVE2_PIN); - break; -#endif //USE_SPI_SLAVE2 -#if USE_SPI_SLAVE3 - case 3: - gpio_set(SPI_SELECT_SLAVE3_PORT, SPI_SELECT_SLAVE3_PIN); - break; -#endif //USE_SPI_SLAVE3 -#if USE_SPI_SLAVE4 - case 4: - gpio_set(SPI_SELECT_SLAVE4_PORT, SPI_SELECT_SLAVE4_PIN); - break; -#endif //USE_SPI_SLAVE4 -#if USE_SPI_SLAVE5 - case 5: - gpio_set(SPI_SELECT_SLAVE5_PORT, SPI_SELECT_SLAVE5_PIN); - break; -#endif //USE_SPI_SLAVE5 - default: - break; - } -} - -static inline void SpiSlaveSelect(uint8_t slave) -{ - switch (slave) { -#if USE_SPI_SLAVE0 - case 0: - gpio_clear(SPI_SELECT_SLAVE0_PORT, SPI_SELECT_SLAVE0_PIN); - break; -#endif // USE_SPI_SLAVE0 -#if USE_SPI_SLAVE1 - case 1: - gpio_clear(SPI_SELECT_SLAVE1_PORT, SPI_SELECT_SLAVE1_PIN); - break; -#endif //USE_SPI_SLAVE1 -#if USE_SPI_SLAVE2 - case 2: - gpio_clear(SPI_SELECT_SLAVE2_PORT, SPI_SELECT_SLAVE2_PIN); - break; -#endif //USE_SPI_SLAVE2 -#if USE_SPI_SLAVE3 - case 3: - gpio_clear(SPI_SELECT_SLAVE3_PORT, SPI_SELECT_SLAVE3_PIN); - break; -#endif //USE_SPI_SLAVE3 -#if USE_SPI_SLAVE4 - case 4: - gpio_clear(SPI_SELECT_SLAVE4_PORT, SPI_SELECT_SLAVE4_PIN); - break; -#endif //USE_SPI_SLAVE4 -#if USE_SPI_SLAVE5 - case 5: - gpio_clear(SPI_SELECT_SLAVE5_PORT, SPI_SELECT_SLAVE5_PIN); - break; -#endif //USE_SPI_SLAVE5 - default: - break; - } -} - -void spi_slave_select(uint8_t slave) -{ - SpiSlaveSelect(slave); -} - -void spi_slave_unselect(uint8_t slave) -{ - SpiSlaveUnselect(slave); -} - -void spi_init_slaves(void) -{ - -#if USE_SPI_SLAVE0 - gpio_setup_output(SPI_SELECT_SLAVE0_PORT, SPI_SELECT_SLAVE0_PIN); - SpiSlaveUnselect(0); -#endif - -#if USE_SPI_SLAVE1 - gpio_setup_output(SPI_SELECT_SLAVE1_PORT, SPI_SELECT_SLAVE1_PIN); - SpiSlaveUnselect(1); -#endif - -#if USE_SPI_SLAVE2 - gpio_setup_output(SPI_SELECT_SLAVE2_PORT, SPI_SELECT_SLAVE2_PIN); - SpiSlaveUnselect(2); -#endif - -#if USE_SPI_SLAVE3 - gpio_setup_output(SPI_SELECT_SLAVE3_PORT, SPI_SELECT_SLAVE3_PIN); - SpiSlaveUnselect(3); -#endif - -#if USE_SPI_SLAVE4 - gpio_setup_output(SPI_SELECT_SLAVE4_PORT, SPI_SELECT_SLAVE4_PIN); - SpiSlaveUnselect(4); -#endif - -#if USE_SPI_SLAVE5 - gpio_setup_output(SPI_SELECT_SLAVE5_PORT, SPI_SELECT_SLAVE5_PIN); - SpiSlaveUnselect(5); -#endif -} - - -/****************************************************************************** - * - * Implementation of the generic SPI functions - * - *****************************************************************************/ -bool spi_submit(struct spi_periph *p, struct spi_transaction *t) -{ - uint8_t idx; - idx = p->trans_insert_idx + 1; - if (idx >= SPI_TRANSACTION_QUEUE_LEN) { idx = 0; } - if ((idx == p->trans_extract_idx) || ((t->input_length == 0) && (t->output_length == 0))) { - t->status = SPITransFailed; - return false; /* queue full or input_length and output_length both 0 */ - // TODO can't tell why it failed here if it does - } - - t->status = SPITransPending; - - //Disable interrupts to avoid race conflict with end of DMA transfer interrupt - //FIXME - spi_arch_int_disable(p); - - // GT: no copy? There's a queue implying a copy here... - p->trans[p->trans_insert_idx] = t; - p->trans_insert_idx = idx; - - /* if peripheral is idle, start the transaction */ - if (p->status == SPIIdle && !p->suspend) { - spi_start_dma_transaction(p, p->trans[p->trans_extract_idx]); - } - //FIXME - spi_arch_int_enable(p); - return true; -} - -bool spi_lock(struct spi_periph *p, uint8_t slave) -{ - spi_arch_int_disable(p); - if (slave < 254 && p->suspend == 0) { - p->suspend = slave + 1; // 0 is reserved for unlock state - spi_arch_int_enable(p); - return true; - } - spi_arch_int_enable(p); - return false; -} - -bool spi_resume(struct spi_periph *p, uint8_t slave) -{ - spi_arch_int_disable(p); - if (p->suspend == slave + 1) { - // restart fifo - p->suspend = 0; - if (p->trans_extract_idx != p->trans_insert_idx && p->status == SPIIdle) { - spi_start_dma_transaction(p, p->trans[p->trans_extract_idx]); - } - spi_arch_int_enable(p); - return true; - } - spi_arch_int_enable(p); - return false; -} - - -/****************************************************************************** - * - * Transaction configuration helper functions - * - *****************************************************************************/ -static void __attribute__((unused)) set_default_comm_config(struct locm3_spi_comm *c) -{ - c->br = SPI_CR1_BAUDRATE_FPCLK_DIV_64; - c->cpol = SPI_CR1_CPOL_CLK_TO_1_WHEN_IDLE; - c->cpha = SPI_CR1_CPHA_CLK_TRANSITION_2; - c->dff = SPI_CR1_DFF_8BIT; - c->lsbfirst = SPI_CR1_MSBFIRST; -} - -static inline uint8_t get_transaction_signature(struct spi_transaction *t) -{ - return ((t->dss << 6) | (t->cdiv << 3) | (t->bitorder << 2) | - (t->cpha << 1) | (t->cpol)); -} - -static uint8_t __attribute__((unused)) get_comm_signature(struct locm3_spi_comm *c) -{ - uint8_t sig = 0; - if (c->cpol == SPI_CR1_CPOL_CLK_TO_0_WHEN_IDLE) { - sig |= SPICpolIdleLow; - } else { - sig |= SPICpolIdleHigh; - } - if (c->cpha == SPI_CR1_CPHA_CLK_TRANSITION_1) { - sig |= (SPICphaEdge1 << 1); - } else { - sig |= (SPICphaEdge2 << 1); - } - if (c->lsbfirst == SPI_CR1_MSBFIRST) { - sig |= (SPIMSBFirst << 2); - } else { - sig |= (SPILSBFirst << 2); - } - uint8_t cdiv = SPIDiv256; - switch (c->br) { - case SPI_CR1_BAUDRATE_FPCLK_DIV_2: - cdiv = SPIDiv2; - break; - case SPI_CR1_BAUDRATE_FPCLK_DIV_4: - cdiv = SPIDiv4; - break; - case SPI_CR1_BAUDRATE_FPCLK_DIV_8: - cdiv = SPIDiv8; - break; - case SPI_CR1_BAUDRATE_FPCLK_DIV_16: - cdiv = SPIDiv16; - break; - case SPI_CR1_BAUDRATE_FPCLK_DIV_32: - cdiv = SPIDiv32; - break; - case SPI_CR1_BAUDRATE_FPCLK_DIV_64: - cdiv = SPIDiv64; - break; - case SPI_CR1_BAUDRATE_FPCLK_DIV_128: - cdiv = SPIDiv128; - break; - case SPI_CR1_BAUDRATE_FPCLK_DIV_256: - cdiv = SPIDiv256; - break; - default: - break; - } - sig |= (cdiv << 3); - if (c->dff == SPI_CR1_DFF_8BIT) { - sig |= (SPIDss8bit << 6); - } else { - sig |= (SPIDss16bit << 6); - } - return sig; -} - -/** Update SPI communication conf from generic paparazzi SPI transaction */ -static void set_comm_from_transaction(struct locm3_spi_comm *c, struct spi_transaction *t) -{ - if (t->dss == SPIDss8bit) { - c->dff = SPI_CR1_DFF_8BIT; - } else { - c->dff = SPI_CR1_DFF_16BIT; - } - if (t->bitorder == SPIMSBFirst) { - c->lsbfirst = SPI_CR1_MSBFIRST; - } else { - c->lsbfirst = SPI_CR1_LSBFIRST; - } - if (t->cpha == SPICphaEdge1) { - c->cpha = SPI_CR1_CPHA_CLK_TRANSITION_1; - } else { - c->cpha = SPI_CR1_CPHA_CLK_TRANSITION_2; - } - if (t->cpol == SPICpolIdleLow) { - c->cpol = SPI_CR1_CPOL_CLK_TO_0_WHEN_IDLE; - } else { - c->cpol = SPI_CR1_CPOL_CLK_TO_1_WHEN_IDLE; - } - - switch (t->cdiv) { - case SPIDiv2: - c->br = SPI_CR1_BAUDRATE_FPCLK_DIV_2; - break; - case SPIDiv4: - c->br = SPI_CR1_BAUDRATE_FPCLK_DIV_4; - break; - case SPIDiv8: - c->br = SPI_CR1_BAUDRATE_FPCLK_DIV_8; - break; - case SPIDiv16: - c->br = SPI_CR1_BAUDRATE_FPCLK_DIV_16; - break; - case SPIDiv32: - c->br = SPI_CR1_BAUDRATE_FPCLK_DIV_32; - break; - case SPIDiv64: - c->br = SPI_CR1_BAUDRATE_FPCLK_DIV_64; - break; - case SPIDiv128: - c->br = SPI_CR1_BAUDRATE_FPCLK_DIV_128; - break; - case SPIDiv256: - c->br = SPI_CR1_BAUDRATE_FPCLK_DIV_256; - break; - default: - break; - } -} - - -/****************************************************************************** - * - * Helpers for SPI transactions with DMA - * - *****************************************************************************/ -static void spi_configure_dma(uint32_t dma, uint32_t rcc_dma, uint8_t chan, uint32_t periph_addr, uint32_t buf_addr, - uint16_t len, enum SPIDataSizeSelect dss, bool increment) -{ - rcc_periph_clock_enable(rcc_dma); -#ifdef STM32F1 - dma_channel_reset(dma, chan); -#elif defined STM32F4 - dma_stream_reset(dma, chan); -#endif - dma_set_peripheral_address(dma, chan, periph_addr); - dma_set_memory_address(dma, chan, buf_addr); - dma_set_number_of_data(dma, chan, len); - - /* Set the dma transfer size based on SPI transaction DSS */ -#ifdef STM32F1 - if (dss == SPIDss8bit) { - dma_set_peripheral_size(dma, chan, DMA_CCR_PSIZE_8BIT); - dma_set_memory_size(dma, chan, DMA_CCR_MSIZE_8BIT); - } else { - dma_set_peripheral_size(dma, chan, DMA_CCR_PSIZE_16BIT); - dma_set_memory_size(dma, chan, DMA_CCR_MSIZE_16BIT); - } -#elif defined STM32F4 - if (dss == SPIDss8bit) { - dma_set_peripheral_size(dma, chan, DMA_SxCR_PSIZE_8BIT); - dma_set_memory_size(dma, chan, DMA_SxCR_MSIZE_8BIT); - } else { - dma_set_peripheral_size(dma, chan, DMA_SxCR_PSIZE_16BIT); - dma_set_memory_size(dma, chan, DMA_SxCR_MSIZE_16BIT); - } -#endif - - if (increment) { - dma_enable_memory_increment_mode(dma, chan); - } else { - dma_disable_memory_increment_mode(dma, chan); - } -} - -/// Enable DMA channel interrupts -static void spi_arch_int_enable(struct spi_periph *spi) -{ - /// @todo fix priority levels if necessary - // enable receive interrupt - nvic_set_priority(((struct spi_periph_dma *)spi->init_struct)->rx_nvic_irq, NVIC_SPI_IRQ_PRIO); - nvic_enable_irq(((struct spi_periph_dma *)spi->init_struct)->rx_nvic_irq); - // enable transmit interrupt - nvic_set_priority(((struct spi_periph_dma *)spi->init_struct)->tx_nvic_irq, NVIC_SPI_IRQ_PRIO); - nvic_enable_irq(((struct spi_periph_dma *)spi->init_struct)->tx_nvic_irq); -} - -/// Disable DMA channel interrupts -static void spi_arch_int_disable(struct spi_periph *spi) -{ - nvic_disable_irq(((struct spi_periph_dma *)spi->init_struct)->rx_nvic_irq); - nvic_disable_irq(((struct spi_periph_dma *)spi->init_struct)->tx_nvic_irq); -} - -/// start next transaction if there is one in the queue -static void spi_next_transaction(struct spi_periph *periph) -{ - /* Increment the transaction to handle */ - periph->trans_extract_idx++; - - /* wrap read index of circular buffer */ - if (periph->trans_extract_idx >= SPI_TRANSACTION_QUEUE_LEN) { - periph->trans_extract_idx = 0; - } - - /* Check if there is another pending SPI transaction */ - if ((periph->trans_extract_idx == periph->trans_insert_idx) || periph->suspend) { - periph->status = SPIIdle; - } else { - spi_start_dma_transaction(periph, periph->trans[periph->trans_extract_idx]); - } -} - - -/** - * Start a new transaction with DMA. - */ -static void spi_start_dma_transaction(struct spi_periph *periph, struct spi_transaction *trans) -{ - struct spi_periph_dma *dma; - uint8_t sig = 0x00; - - /* Store local copy to notify of the results */ - trans->status = SPITransRunning; - periph->status = SPIRunning; - - dma = periph->init_struct; - - /* - * Check if we need to reconfigure the spi peripheral for this transaction - */ - sig = get_transaction_signature(trans); - if (sig != dma->comm_sig) { - /* A different config is required in this transaction... */ - set_comm_from_transaction(&(dma->comm), trans); - - /* remember the new conf signature */ - dma->comm_sig = sig; - - /* apply the new configuration */ - spi_disable((uint32_t)periph->reg_addr); - spi_init_master((uint32_t)periph->reg_addr, dma->comm.br, dma->comm.cpol, - dma->comm.cpha, dma->comm.dff, dma->comm.lsbfirst); - spi_enable_software_slave_management((uint32_t)periph->reg_addr); - spi_set_nss_high((uint32_t)periph->reg_addr); - spi_enable((uint32_t)periph->reg_addr); - } - - /* - * Select the slave after reconfiguration of the peripheral - */ - if (trans->select == SPISelectUnselect || trans->select == SPISelect) { - SpiSlaveSelect(trans->slave_idx); - } - - /* Run the callback AFTER selecting the slave */ - if (trans->before_cb != 0) { - trans->before_cb(trans); - } - - /* - * Receive DMA channel configuration ---------------------------------------- - * - * We always run the receive DMA until the very end! - * This is done so we can use the transfer complete interrupt - * of the RX DMA to signal the end of the transaction. - * - * If we want to receive less than we transmit, a dummy buffer - * for the rx DMA is used after for the remaining data. - * - * In the transmit only case (input_length == 0), - * the dummy is used right from the start. - */ - if (trans->input_length == 0) { - /* run the dummy rx dma for the complete transaction length */ - spi_configure_dma(dma->dma, dma->rcc_dma, dma->rx_chan, (uint32_t)dma->spidr, - (uint32_t) & (dma->rx_dummy_buf), trans->output_length, trans->dss, FALSE); - } else { - /* run the real rx dma for input_length */ - spi_configure_dma(dma->dma, dma->rcc_dma, dma->rx_chan, (uint32_t)dma->spidr, - (uint32_t)trans->input_buf, trans->input_length, trans->dss, TRUE); - /* use dummy rx dma for the rest */ - if (trans->output_length > trans->input_length) { - /* Enable use of second dma transfer with dummy buffer (cleared in ISR) */ - dma->rx_extra_dummy_dma = true; - } - } -#ifdef STM32F1 - dma_set_read_from_peripheral(dma->dma, dma->rx_chan); - dma_set_priority(dma->dma, dma->rx_chan, DMA_CCR_PL_VERY_HIGH); -#elif defined STM32F4 - dma_channel_select(dma->dma, dma->rx_chan, dma->rx_chan_sel); - dma_set_transfer_mode(dma->dma, dma->rx_chan, DMA_SxCR_DIR_PERIPHERAL_TO_MEM); - dma_set_priority(dma->dma, dma->rx_chan, DMA_SxCR_PL_VERY_HIGH); -#endif - - - /* - * Transmit DMA channel configuration --------------------------------------- - * - * We always run the transmit DMA! - * To receive data, the clock must run, so something has to be transmitted. - * If needed, use a dummy DMA transmitting zeros for the remaining length. - * - * In the receive only case (output_length == 0), - * the dummy is used right from the start. - */ - if (trans->output_length == 0) { - spi_configure_dma(dma->dma, dma->rcc_dma, dma->tx_chan, (uint32_t)dma->spidr, - (uint32_t) & (dma->tx_dummy_buf), trans->input_length, trans->dss, FALSE); - } else { - spi_configure_dma(dma->dma, dma->rcc_dma, dma->tx_chan, (uint32_t)dma->spidr, - (uint32_t)trans->output_buf, trans->output_length, trans->dss, TRUE); - if (trans->input_length > trans->output_length) { - /* Enable use of second dma transfer with dummy buffer (cleared in ISR) */ - dma->tx_extra_dummy_dma = true; - } - } -#ifdef STM32F1 - dma_set_read_from_memory(dma->dma, dma->tx_chan); - dma_set_priority(dma->dma, dma->tx_chan, DMA_CCR_PL_MEDIUM); -#elif defined STM32F4 - dma_channel_select(dma->dma, dma->tx_chan, dma->tx_chan_sel); - dma_set_transfer_mode(dma->dma, dma->tx_chan, DMA_SxCR_DIR_MEM_TO_PERIPHERAL); - dma_set_priority(dma->dma, dma->tx_chan, DMA_SxCR_PL_MEDIUM); -#endif - - /* Enable DMA transfer complete interrupts. */ - dma_enable_transfer_complete_interrupt(dma->dma, dma->rx_chan); - dma_enable_transfer_complete_interrupt(dma->dma, dma->tx_chan); - - /* Enable DMA channels */ -#ifdef STM32F1 - dma_enable_channel(dma->dma, dma->rx_chan); - dma_enable_channel(dma->dma, dma->tx_chan); -#elif defined STM32F4 - dma_enable_stream(dma->dma, dma->rx_chan); - dma_enable_stream(dma->dma, dma->tx_chan); -#endif - - /* Enable SPI transfers via DMA */ - spi_enable_rx_dma((uint32_t)periph->reg_addr); - spi_enable_tx_dma((uint32_t)periph->reg_addr); -} - - - -/****************************************************************************** - * - * Initialization of each SPI peripheral - * - *****************************************************************************/ -#if USE_SPI1 -void spi1_arch_init(void) -{ - - // set dma options - spi1_dma.spidr = (uint32_t)&SPI1_DR; -#ifdef STM32F1 - spi1_dma.dma = DMA1; - spi1_dma.rcc_dma = RCC_DMA1; - spi1_dma.rx_chan = DMA_CHANNEL2; - spi1_dma.tx_chan = DMA_CHANNEL3; - spi1_dma.rx_nvic_irq = NVIC_DMA1_CHANNEL2_IRQ; - spi1_dma.tx_nvic_irq = NVIC_DMA1_CHANNEL3_IRQ; -#elif defined STM32F4 - spi1_dma.dma = DMA2; - spi1_dma.rcc_dma = RCC_DMA2; - // TODO make a macro to configure this from board/airframe file ? - spi1_dma.rx_chan = DMA_STREAM0; - spi1_dma.tx_chan = DMA_STREAM5; - spi1_dma.rx_chan_sel = DMA_SxCR_CHSEL_3; - spi1_dma.tx_chan_sel = DMA_SxCR_CHSEL_3; - spi1_dma.rx_nvic_irq = NVIC_DMA2_STREAM0_IRQ; - spi1_dma.tx_nvic_irq = NVIC_DMA2_STREAM5_IRQ; -#endif - spi1_dma.tx_dummy_buf = 0; - spi1_dma.tx_extra_dummy_dma = false; - spi1_dma.rx_dummy_buf = 0; - spi1_dma.rx_extra_dummy_dma = false; - - // set the default configuration - set_default_comm_config(&spi1_dma.comm); - spi1_dma.comm_sig = get_comm_signature(&spi1_dma.comm); - - // set init struct, indices and status - spi1.reg_addr = (void *)SPI1; - spi1.init_struct = &spi1_dma; - spi1.trans_insert_idx = 0; - spi1.trans_extract_idx = 0; - spi1.status = SPIIdle; - - - // Enable SPI1 Periph and gpio clocks - rcc_periph_clock_enable(RCC_SPI1); - - // Configure GPIOs: SCK, MISO and MOSI -#ifdef STM32F1 - gpio_setup_pin_af(GPIO_BANK_SPI1_MISO, GPIO_SPI1_MISO, 0, FALSE); - gpio_setup_pin_af(GPIO_BANK_SPI1_MOSI, GPIO_SPI1_MOSI, 0, TRUE); - gpio_setup_pin_af(GPIO_BANK_SPI1_SCK, GPIO_SPI1_SCK, 0, TRUE); -#elif defined STM32F4 - gpio_setup_pin_af(SPI1_GPIO_PORT_MISO, SPI1_GPIO_MISO, SPI1_GPIO_AF, FALSE); - gpio_setup_pin_af(SPI1_GPIO_PORT_MOSI, SPI1_GPIO_MOSI, SPI1_GPIO_AF, TRUE); - gpio_setup_pin_af(SPI1_GPIO_PORT_SCK, SPI1_GPIO_SCK, SPI1_GPIO_AF, TRUE); - - gpio_set_output_options(SPI1_GPIO_PORT_MOSI, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, SPI1_GPIO_MOSI); - gpio_set_output_options(SPI1_GPIO_PORT_SCK, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, SPI1_GPIO_SCK); -#endif - - // reset SPI - spi_reset(SPI1); - - // Disable SPI peripheral - spi_disable(SPI1); - - // Force SPI mode over I2S. - SPI1_I2SCFGR = 0; - - // configure master SPI. - spi_init_master(SPI1, spi1_dma.comm.br, spi1_dma.comm.cpol, spi1_dma.comm.cpha, - spi1_dma.comm.dff, spi1_dma.comm.lsbfirst); - /* - * Set NSS management to software. - * - * Note: - * Setting nss high is very important, even if we are controlling the GPIO - * ourselves this bit needs to be at least set to 1, otherwise the spi - * peripheral will not send any data out. - */ - spi_enable_software_slave_management(SPI1); - spi_set_nss_high(SPI1); - - // Enable SPI_1 DMA clock - rcc_periph_clock_enable(spi1_dma.rcc_dma); - - // Enable SPI1 periph. - spi_enable(SPI1); - - spi_arch_int_enable(&spi1); -} -#endif - -#if USE_SPI2 -void spi2_arch_init(void) -{ - - // set dma options - spi2_dma.spidr = (uint32_t)&SPI2_DR; - spi2_dma.dma = DMA1; - spi2_dma.rcc_dma = RCC_DMA1; -#ifdef STM32F1 - spi2_dma.rx_chan = DMA_CHANNEL4; - spi2_dma.tx_chan = DMA_CHANNEL5; - spi2_dma.rx_nvic_irq = NVIC_DMA1_CHANNEL4_IRQ; - spi2_dma.tx_nvic_irq = NVIC_DMA1_CHANNEL5_IRQ; -#elif defined STM32F4 - spi2_dma.rx_chan = DMA_STREAM3; - spi2_dma.tx_chan = DMA_STREAM4; - spi2_dma.rx_chan_sel = DMA_SxCR_CHSEL_0; - spi2_dma.tx_chan_sel = DMA_SxCR_CHSEL_0; - spi2_dma.rx_nvic_irq = NVIC_DMA1_STREAM3_IRQ; - spi2_dma.tx_nvic_irq = NVIC_DMA1_STREAM4_IRQ; -#endif - spi2_dma.tx_dummy_buf = 0; - spi2_dma.tx_extra_dummy_dma = false; - spi2_dma.rx_dummy_buf = 0; - spi2_dma.rx_extra_dummy_dma = false; - - // set the default configuration - set_default_comm_config(&spi2_dma.comm); - spi2_dma.comm_sig = get_comm_signature(&spi2_dma.comm); - - // set init struct, indices and status - spi2.reg_addr = (void *)SPI2; - spi2.init_struct = &spi2_dma; - spi2.trans_insert_idx = 0; - spi2.trans_extract_idx = 0; - spi2.status = SPIIdle; - - - // Enable SPI2 Periph and gpio clocks - rcc_periph_clock_enable(RCC_SPI2); - - // Configure GPIOs: SCK, MISO and MOSI -#ifdef STM32F1 - gpio_setup_pin_af(GPIO_BANK_SPI2_MISO, GPIO_SPI2_MISO, 0, FALSE); - gpio_setup_pin_af(GPIO_BANK_SPI2_MOSI, GPIO_SPI2_MOSI, 0, TRUE); - gpio_setup_pin_af(GPIO_BANK_SPI2_SCK, GPIO_SPI2_SCK, 0, TRUE); -#elif defined STM32F4 - gpio_setup_pin_af(SPI2_GPIO_PORT_MISO, SPI2_GPIO_MISO, SPI2_GPIO_AF, FALSE); - gpio_setup_pin_af(SPI2_GPIO_PORT_MOSI, SPI2_GPIO_MOSI, SPI2_GPIO_AF, TRUE); - gpio_setup_pin_af(SPI2_GPIO_PORT_SCK, SPI2_GPIO_SCK, SPI2_GPIO_AF, TRUE); - - gpio_set_output_options(SPI2_GPIO_PORT_MOSI, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, SPI2_GPIO_MOSI); - gpio_set_output_options(SPI2_GPIO_PORT_SCK, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, SPI2_GPIO_SCK); -#endif - - // reset SPI - spi_reset(SPI2); - - // Disable SPI peripheral - spi_disable(SPI2); - - // Force SPI mode over I2S. - SPI2_I2SCFGR = 0; - - // configure master SPI. - spi_init_master(SPI2, spi2_dma.comm.br, spi2_dma.comm.cpol, spi2_dma.comm.cpha, - spi2_dma.comm.dff, spi2_dma.comm.lsbfirst); - - /* - * Set NSS management to software. - * Setting nss high is very important, even if we are controlling the GPIO - * ourselves this bit needs to be at least set to 1, otherwise the spi - * peripheral will not send any data out. - */ - spi_enable_software_slave_management(SPI2); - spi_set_nss_high(SPI2); - - // Enable SPI_2 DMA clock - rcc_periph_clock_enable(spi2_dma.rcc_dma); - - // Enable SPI2 periph. - spi_enable(SPI2); - - spi_arch_int_enable(&spi2); -} -#endif - -#if USE_SPI3 -void spi3_arch_init(void) -{ - - // set the default configuration - spi3_dma.spidr = (uint32_t)&SPI3_DR; -#ifdef STM32F1 - spi3_dma.dma = DMA2; - spi3_dma.rcc_dma = RCC_DMA2; - spi3_dma.rx_chan = DMA_CHANNEL1; - spi3_dma.tx_chan = DMA_CHANNEL2; - spi3_dma.rx_nvic_irq = NVIC_DMA2_CHANNEL1_IRQ; - spi3_dma.tx_nvic_irq = NVIC_DMA2_CHANNEL2_IRQ; -#elif defined STM32F4 - spi3_dma.dma = DMA1; - spi3_dma.rcc_dma = RCC_DMA1; - spi3_dma.rx_chan = DMA_STREAM0; - spi3_dma.tx_chan = DMA_STREAM5; - spi3_dma.rx_chan_sel = DMA_SxCR_CHSEL_0; - spi3_dma.tx_chan_sel = DMA_SxCR_CHSEL_0; - spi3_dma.rx_nvic_irq = NVIC_DMA1_STREAM0_IRQ; - spi3_dma.tx_nvic_irq = NVIC_DMA1_STREAM5_IRQ; -#endif - spi3_dma.tx_dummy_buf = 0; - spi3_dma.tx_extra_dummy_dma = false; - spi3_dma.rx_dummy_buf = 0; - spi3_dma.rx_extra_dummy_dma = false; - - // set the default configuration - set_default_comm_config(&spi3_dma.comm); - spi3_dma.comm_sig = get_comm_signature(&spi3_dma.comm); - - // set init struct, indices and status - spi3.reg_addr = (void *)SPI3; - spi3.init_struct = &spi3_dma; - spi3.trans_insert_idx = 0; - spi3.trans_extract_idx = 0; - spi3.status = SPIIdle; - - - // Enable SPI3 Periph and gpio clocks - rcc_periph_clock_enable(RCC_SPI3); - - // Configure GPIOs: SCK, MISO and MOSI -#ifdef STM32F1 - gpio_setup_pin_af(GPIO_BANK_SPI3_MISO, GPIO_SPI3_MISO, 0, FALSE); - gpio_setup_pin_af(GPIO_BANK_SPI3_MOSI, GPIO_SPI3_MOSI, 0, TRUE); - gpio_setup_pin_af(GPIO_BANK_SPI3_SCK, GPIO_SPI3_SCK, 0, TRUE); -#elif defined STM32F4 - gpio_setup_pin_af(SPI3_GPIO_PORT_MISO, SPI3_GPIO_MISO, SPI3_GPIO_AF, FALSE); - gpio_setup_pin_af(SPI3_GPIO_PORT_MOSI, SPI3_GPIO_MOSI, SPI3_GPIO_AF, TRUE); - gpio_setup_pin_af(SPI3_GPIO_PORT_SCK, SPI3_GPIO_SCK, SPI3_GPIO_AF, TRUE); - - gpio_set_output_options(SPI3_GPIO_PORT_MOSI, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, SPI3_GPIO_MOSI); - gpio_set_output_options(SPI3_GPIO_PORT_SCK, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, SPI3_GPIO_SCK); -#endif - - /// @todo disable JTAG so the pins can be used? - - // reset SPI - spi_reset(SPI3); - - // Disable SPI peripheral - spi_disable(SPI3); - - // Force SPI mode over I2S. - SPI3_I2SCFGR = 0; - - // configure master SPI. - spi_init_master(SPI3, spi3_dma.comm.br, spi3_dma.comm.cpol, spi3_dma.comm.cpha, - spi3_dma.comm.dff, spi3_dma.comm.lsbfirst); - - /* - * Set NSS management to software. - * Setting nss high is very important, even if we are controlling the GPIO - * ourselves this bit needs to be at least set to 1, otherwise the spi - * peripheral will not send any data out. - */ - spi_enable_software_slave_management(SPI3); - spi_set_nss_high(SPI3); - - // Enable SPI_3 DMA clock - rcc_periph_clock_enable(spi3_dma.rcc_dma); - - // Enable SPI3 periph. - spi_enable(SPI3); - - spi_arch_int_enable(&spi3); -} -#endif - - - - -/****************************************************************************** - * - * DMA Interrupt service routines - * - *****************************************************************************/ -#ifdef USE_SPI1 -/// receive transferred over DMA -#ifdef STM32F1 -void dma1_channel2_isr(void) -{ - if ((DMA1_ISR & DMA_ISR_TCIF2) != 0) { - // clear int pending bit - DMA1_IFCR |= DMA_IFCR_CTCIF2; - } -#elif defined STM32F4 -void dma2_stream0_isr(void) { - if ((DMA2_LISR & DMA_LISR_TCIF0) != 0) { - // clear int pending bit - DMA2_LIFCR |= DMA_LIFCR_CTCIF0; - } -#endif - process_rx_dma_interrupt(&spi1); -} - -/// transmit transferred over DMA -#ifdef STM32F1 -void dma1_channel3_isr(void) { - if ((DMA1_ISR & DMA_ISR_TCIF3) != 0) { - // clear int pending bit - DMA1_IFCR |= DMA_IFCR_CTCIF3; - } -#elif defined STM32F4 -void dma2_stream5_isr(void) { - if ((DMA2_HISR & DMA_HISR_TCIF5) != 0) { - // clear int pending bit - DMA2_HIFCR |= DMA_HIFCR_CTCIF5; - } -#endif - process_tx_dma_interrupt(&spi1); -} - -#endif - -#ifdef USE_SPI2 -/// receive transferred over DMA -#ifdef STM32F1 -void dma1_channel4_isr(void) { - if ((DMA1_ISR & DMA_ISR_TCIF4) != 0) { - // clear int pending bit - DMA1_IFCR |= DMA_IFCR_CTCIF4; - } -#elif defined STM32F4 -void dma1_stream3_isr(void) { - if ((DMA1_LISR & DMA_LISR_TCIF3) != 0) { - // clear int pending bit - DMA1_LIFCR |= DMA_LIFCR_CTCIF3; - } -#endif - process_rx_dma_interrupt(&spi2); -} - -/// transmit transferred over DMA -#ifdef STM32F1 -void dma1_channel5_isr(void) { - if ((DMA1_ISR & DMA_ISR_TCIF5) != 0) { - // clear int pending bit - DMA1_IFCR |= DMA_IFCR_CTCIF5; - } -#elif defined STM32F4 -void dma1_stream4_isr(void) { - if ((DMA1_HISR & DMA_HISR_TCIF4) != 0) { - // clear int pending bit - DMA1_HIFCR |= DMA_HIFCR_CTCIF4; - } -#endif - process_tx_dma_interrupt(&spi2); -} - -#endif - -#if USE_SPI3 -/// receive transferred over DMA -#ifdef STM32F1 -void dma2_channel1_isr(void) { - if ((DMA2_ISR & DMA_ISR_TCIF1) != 0) { - // clear int pending bit - DMA2_IFCR |= DMA_IFCR_CTCIF1; - } -#elif defined STM32F4 -void dma1_stream0_isr(void) { - if ((DMA1_LISR & DMA_LISR_TCIF0) != 0) { - // clear int pending bit - DMA1_LIFCR |= DMA_LIFCR_CTCIF0; - } -#endif - process_rx_dma_interrupt(&spi3); -} - -/// transmit transferred over DMA -#ifdef STM32F1 -void dma2_channel2_isr(void) { - if ((DMA2_ISR & DMA_ISR_TCIF2) != 0) { - // clear int pending bit - DMA2_IFCR |= DMA_IFCR_CTCIF2; - } -#elif defined STM32F4 -void dma1_stream5_isr(void) { - if ((DMA1_HISR & DMA_HISR_TCIF5) != 0) { - // clear int pending bit - DMA1_HIFCR |= DMA_HIFCR_CTCIF5; - } -#endif - process_tx_dma_interrupt(&spi3); -} - -#endif - -/// Processing done after rx completes. -void process_rx_dma_interrupt(struct spi_periph * periph) { - struct spi_periph_dma *dma = periph->init_struct; - struct spi_transaction *trans = periph->trans[periph->trans_extract_idx]; - - /* Disable DMA Channel */ - dma_disable_transfer_complete_interrupt(dma->dma, dma->rx_chan); - - /* Disable SPI Rx request */ - spi_disable_rx_dma((uint32_t)periph->reg_addr); - - /* Disable DMA rx channel */ -#ifdef STM32F1 - dma_disable_channel(dma->dma, dma->rx_chan); -#elif defined STM32F4 - dma_disable_stream(dma->dma, dma->rx_chan); -#endif - - - if (dma->rx_extra_dummy_dma) { - /* - * We are finished the first part of the receive with real data, - * but still need to run the dummy to get a transfer complete interrupt - * after the complete transaction is done. - */ - - /* Reset the flag so this only happens once in a transaction */ - dma->rx_extra_dummy_dma = false; - - /* Use the difference in length between rx and tx */ - uint16_t len_remaining = trans->output_length - trans->input_length; - - spi_configure_dma(dma->dma, dma->rcc_dma, dma->rx_chan, (uint32_t)dma->spidr, - (uint32_t) & (dma->rx_dummy_buf), len_remaining, trans->dss, FALSE); -#ifdef STM32F1 - dma_set_read_from_peripheral(dma->dma, dma->rx_chan); - dma_set_priority(dma->dma, dma->rx_chan, DMA_CCR_PL_HIGH); -#elif defined STM32F4 - dma_channel_select(dma->dma, dma->rx_chan, dma->rx_chan_sel); - dma_set_transfer_mode(dma->dma, dma->rx_chan, DMA_SxCR_DIR_PERIPHERAL_TO_MEM); - dma_set_priority(dma->dma, dma->rx_chan, DMA_SxCR_PL_HIGH); -#endif - - /* Enable DMA transfer complete interrupts. */ - dma_enable_transfer_complete_interrupt(dma->dma, dma->rx_chan); - /* Enable DMA channels */ -#ifdef STM32F1 - dma_enable_channel(dma->dma, dma->rx_chan); -#elif defined STM32F4 - dma_enable_stream(dma->dma, dma->rx_chan); -#endif - /* Enable SPI transfers via DMA */ - spi_enable_rx_dma((uint32_t)periph->reg_addr); - } else { - /* - * Since the receive DMA is always run until the very end - * and this interrupt is triggered after the last data word was read, - * we now know that this transaction is finished. - */ - - /* Run the callback */ - trans->status = SPITransSuccess; - if (trans->after_cb != 0) { - trans->after_cb(trans); - } - - /* AFTER the callback, then unselect the slave if required */ - if (trans->select == SPISelectUnselect || trans->select == SPIUnselect) { - SpiSlaveUnselect(trans->slave_idx); - } - - spi_next_transaction(periph); - } -} - -/// Processing done after tx completes -void process_tx_dma_interrupt(struct spi_periph * periph) { - struct spi_periph_dma *dma = periph->init_struct; - struct spi_transaction *trans = periph->trans[periph->trans_extract_idx]; - - /* Disable DMA Channel */ - dma_disable_transfer_complete_interrupt(dma->dma, dma->tx_chan); - - /* Disable SPI TX request */ - spi_disable_tx_dma((uint32_t)periph->reg_addr); - - /* Disable DMA tx channel */ -#ifdef STM32F1 - dma_disable_channel(dma->dma, dma->tx_chan); -#elif defined STM32F4 - dma_disable_stream(dma->dma, dma->tx_chan); -#endif - - if (dma->tx_extra_dummy_dma) { - /* - * We are finished the first part of the transmit with real data, - * but still need to clock in the rest of the receive data. - * Set up a dummy dma transmit transfer to accomplish this. - */ - - /* Reset the flag so this only happens once in a transaction */ - dma->tx_extra_dummy_dma = false; - - /* Use the difference in length between tx and rx */ - uint16_t len_remaining = trans->input_length - trans->output_length; - - spi_configure_dma(dma->dma, dma->rcc_dma, dma->tx_chan, (uint32_t)dma->spidr, - (uint32_t) & (dma->tx_dummy_buf), len_remaining, trans->dss, FALSE); -#ifdef STM32F1 - dma_set_read_from_memory(dma->dma, dma->tx_chan); - dma_set_priority(dma->dma, dma->tx_chan, DMA_CCR_PL_MEDIUM); -#elif defined STM32F4 - dma_channel_select(dma->dma, dma->tx_chan, dma->tx_chan_sel); - dma_set_transfer_mode(dma->dma, dma->tx_chan, DMA_SxCR_DIR_MEM_TO_PERIPHERAL); - dma_set_priority(dma->dma, dma->tx_chan, DMA_SxCR_PL_MEDIUM); -#endif - - /* Enable DMA transfer complete interrupts. */ - dma_enable_transfer_complete_interrupt(dma->dma, dma->tx_chan); - /* Enable DMA channels */ -#ifdef STM32F1 - dma_enable_channel(dma->dma, dma->tx_chan); -#elif defined STM32F4 - dma_enable_stream(dma->dma, dma->tx_chan); -#endif - /* Enable SPI transfers via DMA */ - spi_enable_tx_dma((uint32_t)periph->reg_addr); - - } -} - -#endif /** SPI_MASTER */ - - -/* - * - * SPI Slave code - * Currently only for F1 - * - */ -#ifdef SPI_SLAVE - -static void process_slave_tx_dma_interrupt(struct spi_periph * periph); -static void process_slave_rx_dma_interrupt(struct spi_periph * periph); - -/****************** Slave 1 ******************/ -#if USE_SPI1_SLAVE -#warning "SPI1 slave: Untested code!" - -#if USE_SPI1 -#error "Using SPI1 as a slave and master at the same time is not possible." -#endif - -static struct spi_periph_dma spi1_dma; - -void spi1_slave_arch_init(void) { - // set dma options - spi1_dma.spidr = (uint32_t)&SPI1_DR; -#ifdef STM32F1 - spi1_dma.dma = DMA1; - spi1_dma.rcc_dma = RCC_DMA1; - spi1_dma.rx_chan = DMA_CHANNEL2; - spi1_dma.tx_chan = DMA_CHANNEL3; - spi1_dma.rx_nvic_irq = NVIC_DMA1_CHANNEL2_IRQ; - spi1_dma.tx_nvic_irq = NVIC_DMA1_CHANNEL3_IRQ; -#elif defined STM32F4 - spi1_dma.dma = DMA2; - spi1_dma.rcc_dma = RCC_DMA2; - spi1_dma.rx_chan = DMA_STREAM0; - spi1_dma.tx_chan = DMA_STREAM5; - spi1_dma.rx_chan_sel = DMA_SxCR_CHSEL_3; - spi1_dma.tx_chan_sel = DMA_SxCR_CHSEL_3; - spi1_dma.rx_nvic_irq = NVIC_DMA2_STREAM0_IRQ; - spi1_dma.tx_nvic_irq = NVIC_DMA2_STREAM5_IRQ; -#endif - spi1_dma.tx_dummy_buf = 0; - spi1_dma.tx_extra_dummy_dma = false; - spi1_dma.rx_dummy_buf = 0; - spi1_dma.rx_extra_dummy_dma = false; - - // set the default configuration - set_default_comm_config(&spi1_dma.comm); - spi1_dma.comm_sig = get_comm_signature(&spi1_dma.comm); - - // set init struct, indices and status - spi1.reg_addr = (void *)SPI1; - spi1.init_struct = &spi1_dma; - spi1.trans_insert_idx = 0; - spi1.trans_extract_idx = 0; - spi1.status = SPIIdle; - - // Enable SPI1 Periph and gpio clocks - rcc_periph_clock_enable(RCC_SPI1); - - // Configure GPIOs: SCK, MISO and MOSI -#if defined STM32F1 - gpio_setup_pin_af(GPIO_BANK_SPI1_MISO, GPIO_SPI1_MISO, 0, TRUE); - gpio_setup_pin_af(GPIO_BANK_SPI1_MOSI, GPIO_SPI1_MOSI, 0, FALSE); - gpio_setup_pin_af(GPIO_BANK_SPI1_SCK, GPIO_SPI1_SCK, 0, FALSE); - gpio_setup_pin_af(GPIO_BANK_SPI1_NSS, GPIO_SPI1_NSS, 0, FALSE); -#elif defined STM32F4 - gpio_setup_pin_af(SPI1_GPIO_PORT_MISO, SPI1_GPIO_MISO, SPI1_GPIO_AF, TRUE); - gpio_setup_pin_af(SPI1_GPIO_PORT_MOSI, SPI1_GPIO_MOSI, SPI1_GPIO_AF, FALSE); - gpio_setup_pin_af(SPI1_GPIO_PORT_SCK, SPI1_GPIO_SCK, SPI1_GPIO_AF, FALSE); - gpio_setup_pin_af(SPI1_GPIO_PORT_NSS, SPI1_GPIO_NSS, SPI1_GPIO_AF, FALSE); - - gpio_set_output_options(SPI1_GPIO_PORT_MISO, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, SPI1_GPIO_MISO); -#endif - - // reset SPI - spi_reset(SPI1); - - // Disable SPI peripheral - spi_disable(SPI1); - - // Force SPI mode over I2S - SPI1_I2SCFGR = 0; - - // configure default spi settings - spi_init_master(SPI1, spi1_dma.comm.br, spi1_dma.comm.cpol, spi1_dma.comm.cpha, - spi1_dma.comm.dff, spi1_dma.comm.lsbfirst); - - // configure slave select management - spi_disable_software_slave_management(SPI1); - - // set slave mode - spi_set_slave_mode(SPI1); - - // Enable SPI_1 DMA clock - rcc_periph_clock_enable(spi1_dma.rcc_dma); - - // Enable SPI1 periph. - spi_enable(SPI1); -} - -/// receive transferred over DMA -#ifdef STM32F1 -void dma1_channel2_isr(void) { - if ((DMA1_ISR & DMA_ISR_TCIF2) != 0) { - // clear int pending bit - DMA1_IFCR |= DMA_IFCR_CTCIF2; - } -#elif defined STM32F4 -void dma2_stream0_isr(void) { - if ((DMA2_LISR & DMA_LISR_TCIF0) != 0) { - // clear int pending bit - DMA2_LIFCR |= DMA_LIFCR_CTCIF0; - } -#endif - process_slave_rx_dma_interrupt(&spi1); -} - -/// transmit transferred over DMA -#ifdef STM32F1 -void dma1_channel3_isr(void) { - if ((DMA1_ISR & DMA_ISR_TCIF3) != 0) { - // clear int pending bit - DMA1_IFCR |= DMA_IFCR_CTCIF3; - } -#elif defined STM32F4 -void dma2_stream5_isr(void) { - if ((DMA2_HISR & DMA_HISR_TCIF5) != 0) { - // clear int pending bit - DMA2_HIFCR |= DMA_HIFCR_CTCIF5; - } -#endif - process_slave_tx_dma_interrupt(&spi1); -} - -#endif /* USE_SPI1_SLAVE */ - -/****************** Slave 2 ******************/ -#if USE_SPI2_SLAVE -#ifndef STM32F1 -#warning "SPI2 slave only tested on STM32F1!" -#endif - -#if USE_SPI2 -#error "Using SPI2 as a slave and master at the same time is not possible." -#endif - -static struct spi_periph_dma spi2_dma; - -void spi2_slave_arch_init(void) { - // set dma options - spi2_dma.spidr = (uint32_t)&SPI2_DR; - spi2_dma.dma = DMA1; - spi2_dma.rcc_dma = RCC_DMA1; -#ifdef STM32F1 - spi2_dma.rx_chan = DMA_CHANNEL4; - spi2_dma.tx_chan = DMA_CHANNEL5; - spi2_dma.rx_nvic_irq = NVIC_DMA1_CHANNEL4_IRQ; - spi2_dma.tx_nvic_irq = NVIC_DMA1_CHANNEL5_IRQ; -#elif defined STM32F4 - spi2_dma.rx_chan = DMA_STREAM3; - spi2_dma.tx_chan = DMA_STREAM4; - spi2_dma.rx_chan_sel = DMA_SxCR_CHSEL_0; - spi2_dma.tx_chan_sel = DMA_SxCR_CHSEL_0; - spi2_dma.rx_nvic_irq = NVIC_DMA1_STREAM3_IRQ; - spi2_dma.tx_nvic_irq = NVIC_DMA1_STREAM4_IRQ; -#endif - spi2_dma.tx_dummy_buf = 0; - spi2_dma.tx_extra_dummy_dma = false; - spi2_dma.rx_dummy_buf = 0; - spi2_dma.rx_extra_dummy_dma = false; - - // set the default configuration - set_default_comm_config(&spi2_dma.comm); - spi2_dma.comm_sig = get_comm_signature(&spi2_dma.comm); - - // set init struct, indices and status - spi2.reg_addr = (void *)SPI2; - spi2.init_struct = &spi2_dma; - spi2.trans_insert_idx = 0; - spi2.trans_extract_idx = 0; - spi2.status = SPIIdle; - - // Enable SPI2 Periph and gpio clocks - rcc_periph_clock_enable(RCC_SPI2); - - // Configure GPIOs: SCK, MISO and MOSI -#ifdef STM32F1 - gpio_setup_pin_af(GPIO_BANK_SPI2_MISO, GPIO_SPI2_MISO, 0, TRUE); - gpio_setup_pin_af(GPIO_BANK_SPI2_MOSI, GPIO_SPI2_MOSI, 0, FALSE); - gpio_setup_pin_af(GPIO_BANK_SPI2_SCK, GPIO_SPI2_SCK, 0, FALSE); - gpio_setup_pin_af(GPIO_BANK_SPI2_NSS, GPIO_SPI2_NSS, 0, FALSE); -#elif defined STM32F4 - gpio_setup_pin_af(SPI2_GPIO_PORT_MISO, SPI2_GPIO_MISO, SPI2_GPIO_AF, TRUE); - gpio_setup_pin_af(SPI2_GPIO_PORT_MOSI, SPI2_GPIO_MOSI, SPI2_GPIO_AF, FALSE); - gpio_setup_pin_af(SPI2_GPIO_PORT_SCK, SPI2_GPIO_SCK, SPI2_GPIO_AF, FALSE); - gpio_setup_pin_af(SPI2_GPIO_PORT_NSS, SPI2_GPIO_NSS, SPI2_GPIO_AF, FALSE); - - gpio_set_output_options(SPI2_GPIO_PORT_MISO, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, SPI2_GPIO_MISO); -#endif - - // reset SPI - spi_reset(SPI2); - - // Disable SPI peripheral - spi_disable(SPI2); - - // Force SPI mode over I2S - SPI2_I2SCFGR = 0; - - // configure default spi settings - spi_init_master(SPI2, spi2_dma.comm.br, spi2_dma.comm.cpol, spi2_dma.comm.cpha, - spi2_dma.comm.dff, spi2_dma.comm.lsbfirst); - - // configure slave select management - spi_disable_software_slave_management(SPI2); - - // set slave mode - spi_set_slave_mode(SPI2); - - // Enable SPI_2 DMA clock - rcc_periph_clock_enable(spi2_dma.rcc_dma); - - // Enable SPI2 periph - spi_enable(SPI2); -} - -/// receive transferred over DMA -#ifdef STM32F1 -void dma1_channel4_isr(void) { - if ((DMA1_ISR & DMA_ISR_TCIF4) != 0) { - // clear int pending bit - DMA1_IFCR |= DMA_IFCR_CTCIF4; - } -#elif defined STM32F4 -void dma1_stream3_isr(void) { - if ((DMA1_LISR & DMA_LISR_TCIF3) != 0) { - // clear int pending bit - DMA1_LIFCR |= DMA_LIFCR_CTCIF3; - } -#endif - process_slave_rx_dma_interrupt(&spi2); -} - -/// transmit transferred over DMA -#ifdef STM32F1 -void dma1_channel5_isr(void) { - if ((DMA1_ISR & DMA_ISR_TCIF5) != 0) { - // clear int pending bit - DMA1_IFCR |= DMA_IFCR_CTCIF5; - } -#elif defined STM32F4 -void dma1_stream4_isr(void) { - if ((DMA1_HISR & DMA_HISR_TCIF4) != 0) { - // clear int pending bit - DMA1_HIFCR |= DMA_HIFCR_CTCIF4; - } -#endif - process_slave_tx_dma_interrupt(&spi2); -} - -#endif /* USE_SPI2_SLAVE */ - - -#if USE_SPI3_SLAVE -#ifndef STM32F4 -#warning "SPI3 slave only tested on STM32F4!" -#endif - -/****************** Slave 3 ******************/ -#if USE_SPI3 -#error "Using SPI3 as a slave and master at the same time is not possible." -#endif - -static struct spi_periph_dma spi3_dma; - -void spi3_slave_arch_init(void) { - // set dma options - spi3_dma.spidr = (uint32_t)&SPI3_DR; -#if defined STM32F1 - spi3_dma.dma = DMA2; - spi3_dma.rcc_dma = RCC_DMA2; - spi3_dma.rx_chan = DMA_CHANNEL1; - spi3_dma.tx_chan = DMA_CHANNEL2; - spi3_dma.rx_nvic_irq = NVIC_DMA2_CHANNEL1_IRQ; - spi3_dma.tx_nvic_irq = NVIC_DMA2_CHANNEL2_IRQ; -#elif defined STM32F4 - spi3_dma.dma = DMA1; - spi3_dma.rcc_dma = RCC_DMA1; - spi3_dma.rx_chan = DMA_STREAM0; - spi3_dma.tx_chan = DMA_STREAM5; - spi3_dma.rx_chan_sel = DMA_SxCR_CHSEL_0; - spi3_dma.tx_chan_sel = DMA_SxCR_CHSEL_0; - spi3_dma.rx_nvic_irq = NVIC_DMA1_STREAM0_IRQ; - spi3_dma.tx_nvic_irq = NVIC_DMA1_STREAM5_IRQ; -#endif - spi3_dma.tx_dummy_buf = 0; - spi3_dma.tx_extra_dummy_dma = false; - spi3_dma.rx_dummy_buf = 0; - spi3_dma.rx_extra_dummy_dma = false; - - // set the default configuration - set_default_comm_config(&spi3_dma.comm); - spi3_dma.comm_sig = get_comm_signature(&spi3_dma.comm); - - // set init struct, indices and status - spi3.reg_addr = (void *)SPI3; - spi3.init_struct = &spi3_dma; - spi3.trans_insert_idx = 0; - spi3.trans_extract_idx = 0; - spi3.status = SPIIdle; - - // Enable SPI3 Periph and gpio clocks - rcc_periph_clock_enable(RCC_SPI3); - - // Configure GPIOs: SCK, MISO and MOSI -#if defined STM32F1 - gpio_setup_pin_af(GPIO_BANK_SPI3_MISO, GPIO_SPI3_MISO, 0, TRUE); - gpio_setup_pin_af(GPIO_BANK_SPI3_MOSI, GPIO_SPI3_MOSI, 0, FALSE); - gpio_setup_pin_af(GPIO_BANK_SPI3_SCK, GPIO_SPI3_SCK, 0, FALSE); - // set NSS as input - gpio_setup_pin_af(GPIO_BANK_SPI3_NSS, GPIO_SPI3_NSS, 0, FALSE); -#elif defined STM32F4 - gpio_setup_pin_af(SPI3_GPIO_PORT_MISO, SPI3_GPIO_MISO, SPI3_GPIO_AF, TRUE); - gpio_setup_pin_af(SPI3_GPIO_PORT_MOSI, SPI3_GPIO_MOSI, SPI3_GPIO_AF, FALSE); - gpio_setup_pin_af(SPI3_GPIO_PORT_SCK, SPI3_GPIO_SCK, SPI3_GPIO_AF, FALSE); - gpio_setup_pin_af(SPI3_GPIO_PORT_NSS, SPI3_GPIO_NSS, SPI3_GPIO_AF, FALSE); - - gpio_set_output_options(SPI3_GPIO_PORT_MISO, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, SPI3_GPIO_MISO); -#endif - - // reset SPI - spi_reset(SPI3); - - // Disable SPI peripheral - spi_disable(SPI3); - - // Force SPI mode over I2S - SPI3_I2SCFGR = 0; - - // configure default spi settings - spi_init_master(SPI3, spi3_dma.comm.br, spi3_dma.comm.cpol, spi3_dma.comm.cpha, - spi3_dma.comm.dff, spi3_dma.comm.lsbfirst); - - // configure slave select management - spi_disable_software_slave_management(SPI3); - - // set slave mode - spi_set_slave_mode(SPI3); - - // Enable SPI_2 DMA clock - rcc_periph_clock_enable(spi3_dma.rcc_dma); - - // Enable SPI3 periph - spi_enable(SPI3); -} - -/// receive transferred over DMA -#ifdef STM32F1 -void dma2_channel1_isr(void) { - if ((DMA2_ISR & DMA_ISR_TCIF1) != 0) { - // clear int pending bit - DMA2_IFCR |= DMA_IFCR_CTCIF1; - } -#elif defined STM32F4 -void dma1_stream0_isr(void) { - if ((DMA1_LISR & DMA_LISR_TCIF0) != 0) { - // clear int pending bit - DMA1_LIFCR |= DMA_LIFCR_CTCIF0; - } -#endif - process_slave_rx_dma_interrupt(&spi3); -} - -/// transmit transferred over DMA -#ifdef STM32F1 -void dma2_channel2_isr(void) { - if ((DMA2_ISR & DMA_ISR_TCIF2) != 0) { - // clear int pending bit - DMA2_IFCR |= DMA_IFCR_CTCIF2; - } -#elif defined STM32F4 -void dma1_stream5_isr(void) { - if ((DMA1_HISR & DMA_HISR_TCIF5) != 0) { - // clear int pending bit - DMA1_HIFCR |= DMA_HIFCR_CTCIF5; - } -#endif - process_slave_tx_dma_interrupt(&spi3); -} - -#endif /* USE_SPI3_SLAVE */ - -/****************** General SPI slave Functions ******************/ -static void spi_slave_set_config(struct spi_periph * periph, struct spi_transaction * trans) { - struct spi_periph_dma *dma; - - dma = periph->init_struct; - - /* set the new configuration */ - set_comm_from_transaction(&(dma->comm), trans); - - /* remember the new conf signature */ - dma->comm_sig = get_transaction_signature(trans); - - /* apply the new configuration */ - spi_disable((uint32_t)periph->reg_addr); - spi_init_master((uint32_t)periph->reg_addr, dma->comm.br, dma->comm.cpol, - dma->comm.cpha, dma->comm.dff, dma->comm.lsbfirst); - - // configure slave select management - spi_disable_software_slave_management((uint32_t)periph->reg_addr); - - // set slave mode - spi_set_slave_mode((uint32_t)periph->reg_addr); - - // enable spi peripheral - spi_enable((uint32_t)periph->reg_addr); -} - -//static void spi_start_slave_dma_transaction(struct spi_periph* periph, struct spi_transaction* trans) - -/* - * Please note: Spi_slave_register will set-up the DMA which will automatically load the first byte of the transmit buffer - * Therefore, to ensure that the first byte of your data will be set, you have to set the transmit buffer before you call - * this function - */ -bool spi_slave_register(struct spi_periph * periph, struct spi_transaction * trans) { - struct spi_periph_dma *dma = periph->init_struct; - - /* Store local copy to notify of the results */ - trans->status = SPITransRunning; - periph->status = SPIRunning; - - periph->trans_insert_idx = 0; - periph->trans[periph->trans_insert_idx] = trans; - - /* update peripheral if changed */ - uint8_t sig = get_transaction_signature(trans); - if (sig != dma->comm_sig) { - spi_slave_set_config(periph, trans); - } - - /* - * Transmit DMA channel configuration --------------------------------------- - */ - spi_configure_dma(dma->dma, dma->rcc_dma, dma->tx_chan, (uint32_t)dma->spidr, - (uint32_t)trans->output_buf, trans->output_length, trans->dss, TRUE); - -#ifdef STM32F1 - dma_set_read_from_memory(dma->dma, dma->tx_chan); - dma_set_priority(dma->dma, dma->tx_chan, DMA_CCR_PL_MEDIUM); -#elif defined STM32F4 - dma_channel_select(dma->dma, dma->tx_chan, dma->tx_chan_sel); - dma_set_transfer_mode(dma->dma, dma->tx_chan, DMA_SxCR_DIR_MEM_TO_PERIPHERAL); - dma_set_priority(dma->dma, dma->tx_chan, DMA_SxCR_PL_MEDIUM); -#endif - - /* - * Receive DMA channel configuration ---------------------------------------- - */ - spi_configure_dma(dma->dma, dma->rcc_dma, dma->rx_chan, (uint32_t)dma->spidr, - (uint32_t)trans->input_buf, trans->input_length, trans->dss, TRUE); - -#ifdef STM32F1 - dma_set_read_from_peripheral(dma->dma, dma->rx_chan); - dma_set_priority(dma->dma, dma->rx_chan, DMA_CCR_PL_VERY_HIGH); -#elif defined STM32F4 - dma_channel_select(dma->dma, dma->rx_chan, dma->rx_chan_sel); - dma_set_transfer_mode(dma->dma, dma->rx_chan, DMA_SxCR_DIR_PERIPHERAL_TO_MEM); - dma_set_priority(dma->dma, dma->rx_chan, DMA_SxCR_PL_VERY_HIGH); -#endif - - /* Enable DMA transfer complete interrupts. */ - dma_enable_transfer_complete_interrupt(dma->dma, dma->tx_chan); - dma_enable_transfer_complete_interrupt(dma->dma, dma->rx_chan); - - /* Enable DMA channels */ -#ifdef STM32F1 - dma_enable_channel(dma->dma, dma->tx_chan); - dma_enable_channel(dma->dma, dma->rx_chan); -#elif defined STM32F4 - dma_enable_stream(dma->dma, dma->tx_chan); - dma_enable_stream(dma->dma, dma->rx_chan); -#endif - - /* Enable SPI transfers via DMA */ - spi_enable_tx_dma((uint32_t)periph->reg_addr); - spi_enable_rx_dma((uint32_t)periph->reg_addr); - - /* enable dma interrupt */ - spi_arch_int_enable(periph); - - return true; -} - -void process_slave_rx_dma_interrupt(struct spi_periph * periph) { - struct spi_periph_dma *dma = periph->init_struct; - struct spi_transaction *trans = periph->trans[periph->trans_extract_idx]; - - /* Disable DMA Channel */ - dma_disable_transfer_complete_interrupt(dma->dma, dma->rx_chan); - - /* disable dma interrupts */ - spi_arch_int_disable(periph); - - /* Disable SPI rx request */ - spi_disable_rx_dma((uint32_t)periph->reg_addr); - - /* Disable DMA rx channel */ -#ifdef STM32F1 - dma_disable_channel(dma->dma, dma->rx_chan); -#elif defined STM32F4 - dma_disable_stream(dma->dma, dma->rx_chan); -#endif - - /* Run the callback */ - trans->status = SPITransSuccess; - - if (trans->after_cb != 0) { - trans->after_cb(trans); - } -} - -void process_slave_tx_dma_interrupt(struct spi_periph * periph) { - struct spi_periph_dma *dma = periph->init_struct; - - /* Disable DMA Channel */ - dma_disable_transfer_complete_interrupt(dma->dma, dma->tx_chan); - - /* Disable SPI tx request */ - spi_disable_tx_dma((uint32_t)periph->reg_addr); - - /* Disable DMA tx channel */ -#ifdef STM32F1 - dma_disable_channel(dma->dma, dma->tx_chan); -#elif defined STM32F4 - dma_disable_stream(dma->dma, dma->tx_chan); -#endif -} - -#endif /* SPI_SLAVE */ diff --git a/sw/airborne/arch/stm32/mcu_periph/spi_arch.h b/sw/airborne/arch/stm32/mcu_periph/spi_arch.h deleted file mode 100644 index 11b7ba79c7..0000000000 --- a/sw/airborne/arch/stm32/mcu_periph/spi_arch.h +++ /dev/null @@ -1,34 +0,0 @@ -/* - * Copyright (C) 2005-2012 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 arch/stm32/mcu_periph/spi_arch.h - * @ingroup stm32_arch - * - * Handling of SPI hardware for STM32. - */ - -#ifndef SPI_ARCH_H -#define SPI_ARCH_H - - -#endif // SPI_ARCH_H diff --git a/sw/airborne/arch/stm32/mcu_periph/sys_time_arch.c b/sw/airborne/arch/stm32/mcu_periph/sys_time_arch.c deleted file mode 100644 index b4ad4bc673..0000000000 --- a/sw/airborne/arch/stm32/mcu_periph/sys_time_arch.c +++ /dev/null @@ -1,160 +0,0 @@ -/* - * Copyright (C) 2009-2011 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 arch/stm32/mcu_periph/sys_time_arch.c - * @ingroup stm32_arch - * - * STM32 timing functions. - * - */ - -#include "mcu_periph/sys_time.h" -#include -#include -#include -#include "std.h" - -#include "libopencm3/cm3/systick.h" - -#ifdef SYS_TIME_LED -#include "led.h" -#endif - -#ifndef USE_OCM3_SYSTICK_INIT -#define USE_OCM3_SYSTICK_INIT 1 -#endif - -void sys_tick_handler(void); - -/** Initialize SysTick. - * Generate SysTick interrupt every sys_time.resolution_cpu_ticks - */ -void sys_time_arch_init(void) -{ - /* run cortex systick timer with 72MHz (FIXME only 72 or does it work with 168MHz???) */ -#if USE_OCM3_SYSTICK_INIT - systick_set_clocksource(STK_CSR_CLKSOURCE_AHB); -#endif - sys_time.cpu_ticks_per_sec = AHB_CLK; - - /* cpu ticks per desired sys_time timer step */ - sys_time.resolution_cpu_ticks = (uint32_t)(sys_time.resolution * sys_time.cpu_ticks_per_sec + 0.5); - -#if USE_OCM3_SYSTICK_INIT - /* The timer interrupt is activated on the transition from 1 to 0, - * therefore it activates every n+1 clock ticks. - */ - systick_set_reload(sys_time.resolution_cpu_ticks - 1); - - systick_interrupt_enable(); - systick_counter_enable(); -#endif -} - - -uint32_t get_sys_time_usec(void) -{ - return sys_time.nb_sec * 1000000 + - usec_of_cpu_ticks(sys_time.nb_sec_rem) + - usec_of_cpu_ticks(systick_get_reload() - systick_get_value()); -} - -uint32_t get_sys_time_usec100(void) -{ - return sys_time.nb_sec * 10000 + - usec_of_cpu_ticks(sys_time.nb_sec_rem)/100 + - usec_of_cpu_ticks(systick_get_reload() - systick_get_value())/100; -} - -uint32_t get_sys_time_msec(void) -{ - return sys_time.nb_sec * 1000 + - msec_of_cpu_ticks(sys_time.nb_sec_rem) + - msec_of_cpu_ticks(systick_get_reload() - systick_get_value()); -} - -// FIXME : nb_tick rollover ??? -// -// 97 days at 512hz -// 12 hours at 100khz -// -void sys_tick_handler(void) -{ - sys_time.nb_tick++; - - sys_time.nb_sec_rem += sys_time.resolution_cpu_ticks; - if (sys_time.nb_sec_rem >= sys_time.cpu_ticks_per_sec) { - sys_time.nb_sec_rem -= sys_time.cpu_ticks_per_sec; - sys_time.nb_sec++; - -#ifdef SYS_TIME_LED - LED_TOGGLE(SYS_TIME_LED); -#endif - } - for (unsigned int i = 0; i < SYS_TIME_NB_TIMER; i++) { - if (sys_time.timer[i].in_use && - sys_time.nb_tick >= sys_time.timer[i].end_time) { - sys_time.timer[i].end_time += sys_time.timer[i].duration; - sys_time.timer[i].elapsed = true; - if (sys_time.timer[i].cb) { - sys_time.timer[i].cb(i); - } - } - } -} - -/** Busy wait in microseconds. - * - * max value is limited by the max number of cycle - * i.e 2^32 * usec_of_cpu_ticks(systick_get_reload()) - */ -void sys_time_usleep(uint32_t us) -{ - // start time - uint32_t start = systick_get_value(); - // max time of one full counter cycle (n + 1 ticks) - uint32_t DT = usec_of_cpu_ticks(systick_get_reload() + 1); - // number of cycles - uint32_t n = us / DT; - // remaining number of cpu ticks - uint32_t rem = cpu_ticks_of_usec(us % DT); - // end time depend on the current value of the counter - uint32_t end; - if (rem < start) { - end = start - rem; - } else { - // one more count flag is required - n++; - end = systick_get_reload() - rem + start; - } - // count number of cycles (when counter reachs 0) - while (n) { - while (!systick_get_countflag()); - n--; - } - // wait remaining ticks - while (systick_get_value() > end); -} - -void sys_time_msleep(uint32_t ms) { - sys_time_usleep(ms*1000); -} diff --git a/sw/airborne/arch/stm32/mcu_periph/uart_arch.c b/sw/airborne/arch/stm32/mcu_periph/uart_arch.c deleted file mode 100644 index 8871e5eb3c..0000000000 --- a/sw/airborne/arch/stm32/mcu_periph/uart_arch.c +++ /dev/null @@ -1,658 +0,0 @@ -/* - * Copyright (C) 2009 Antoine Drouin - * 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 arch/stm32/mcu_periph/uart_arch.c - * @ingroup stm32_arch - * - * Handling of UART hardware for STM32. - */ - -#include "mcu_periph/uart.h" -#include "mcu_periph/gpio.h" - -#include -#include -#include -#include - -#include "std.h" - -#include BOARD_CONFIG - -void uart_periph_set_baudrate(struct uart_periph *p, uint32_t baud) -{ - p->baudrate = baud; - - /* Configure USART baudrate */ - usart_set_baudrate((uint32_t)p->reg_addr, baud); - - /* Disable Idle Line interrupt */ - USART_CR1((uint32_t)p->reg_addr) &= ~USART_CR1_IDLEIE; - - /* Disable LIN break detection interrupt */ - USART_CR2((uint32_t)p->reg_addr) &= ~USART_CR2_LBDIE; - - /* Enable USART1 Receive interrupts */ - USART_CR1((uint32_t)p->reg_addr) |= USART_CR1_RXNEIE; - - /* Enable the USART */ - usart_enable((uint32_t)p->reg_addr); - -} - -void uart_periph_set_bits_stop_parity(struct uart_periph *p, uint8_t bits, uint8_t stop, uint8_t parity) -{ - /* Configure USART parity and data bits */ - if (parity == UPARITY_EVEN) { - usart_set_parity((uint32_t)p->reg_addr, USART_PARITY_EVEN); - if (bits == UBITS_7) { - usart_set_databits((uint32_t)p->reg_addr, 8); - } else { // 8 data bits by default - usart_set_databits((uint32_t)p->reg_addr, 9); - } - } else if (parity == UPARITY_ODD) { - usart_set_parity((uint32_t)p->reg_addr, USART_PARITY_ODD); - if (bits == UBITS_7) { - usart_set_databits((uint32_t)p->reg_addr, 8); - } else { // 8 data bits by default - usart_set_databits((uint32_t)p->reg_addr, 9); - } - } else { // 8 data bist, NO_PARITY by default - usart_set_parity((uint32_t)p->reg_addr, USART_PARITY_NONE); - usart_set_databits((uint32_t)p->reg_addr, 8); // is 7bits without parity possible ? - } - /* Configure USART stop bits */ - if (stop == USTOP_2) { - usart_set_stopbits((uint32_t)p->reg_addr, USART_STOPBITS_2); - } else { // 1 stop bit by default - usart_set_stopbits((uint32_t)p->reg_addr, USART_STOPBITS_1); - } -} - -void uart_periph_set_mode(struct uart_periph *p, bool tx_enabled, bool rx_enabled, bool hw_flow_control) -{ - uint32_t 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((uint32_t)p->reg_addr, mode); - - if (hw_flow_control) { - usart_set_flow_control((uint32_t)p->reg_addr, USART_FLOWCONTROL_RTS_CTS); - } else { - usart_set_flow_control((uint32_t)p->reg_addr, USART_FLOWCONTROL_NONE); - } -} - -void uart_put_byte(struct uart_periph *p, long fd __attribute__((unused)), uint8_t data) -{ - - uint16_t temp = (p->tx_insert_idx + 1) % UART_TX_BUFFER_SIZE; - - if (temp == p->tx_extract_idx) { - return; // no room - } - - USART_CR1((uint32_t)p->reg_addr) &= ~USART_CR1_TXEIE; // Disable TX interrupt - - // check if in process of sending data - if (p->tx_running) { // yes, add to queue - p->tx_buf[p->tx_insert_idx] = data; - p->tx_insert_idx = temp; - } else { // no, set running flag and write to output register - p->tx_running = true; - usart_send((uint32_t)p->reg_addr, data); - } - - USART_CR1((uint32_t)p->reg_addr) |= USART_CR1_TXEIE; // Enable TX interrupt - -} - -static inline void usart_isr(struct uart_periph *p) -{ - - if (((USART_CR1((uint32_t)p->reg_addr) & USART_CR1_TXEIE) != 0) && - ((USART_SR((uint32_t)p->reg_addr) & USART_SR_TXE) != 0)) { - // check if more data to send - if (p->tx_insert_idx != p->tx_extract_idx) { - usart_send((uint32_t)p->reg_addr, p->tx_buf[p->tx_extract_idx]); - p->tx_extract_idx++; - p->tx_extract_idx %= UART_TX_BUFFER_SIZE; - } else { - p->tx_running = false; // clear running flag - USART_CR1((uint32_t)p->reg_addr) &= ~USART_CR1_TXEIE; // Disable TX interrupt - } - } - - if (((USART_CR1((uint32_t)p->reg_addr) & USART_CR1_RXNEIE) != 0) && - ((USART_SR((uint32_t)p->reg_addr) & USART_SR_RXNE) != 0) && - ((USART_SR((uint32_t)p->reg_addr) & USART_SR_ORE) == 0) && - ((USART_SR((uint32_t)p->reg_addr) & USART_SR_NE) == 0) && - ((USART_SR((uint32_t)p->reg_addr) & USART_SR_FE) == 0)) { - uint16_t temp = (p->rx_insert_idx + 1) % UART_RX_BUFFER_SIZE;; - p->rx_buf[p->rx_insert_idx] = usart_recv((uint32_t)p->reg_addr); - // check for more room in queue - if (temp != p->rx_extract_idx) { - p->rx_insert_idx = temp; // update insert index - } - } else { - /* ORE, NE or FE error - read USART_DR reg and log the error */ - if (((USART_CR1((uint32_t)p->reg_addr) & USART_CR1_RXNEIE) != 0) && - ((USART_SR((uint32_t)p->reg_addr) & USART_SR_ORE) != 0)) { - usart_recv((uint32_t)p->reg_addr); - p->ore++; - } - if (((USART_CR1((uint32_t)p->reg_addr) & USART_CR1_RXNEIE) != 0) && - ((USART_SR((uint32_t)p->reg_addr) & USART_SR_NE) != 0)) { - usart_recv((uint32_t)p->reg_addr); - p->ne_err++; - } - if (((USART_CR1((uint32_t)p->reg_addr) & USART_CR1_RXNEIE) != 0) && - ((USART_SR((uint32_t)p->reg_addr) & USART_SR_FE) != 0)) { - usart_recv((uint32_t)p->reg_addr); - p->fe_err++; - } - } -} - -static inline void usart_enable_irq(uint8_t IRQn) -{ - /* Note: - * In libstm32 times the priority of this interrupt was set to - * preemption priority 2 and sub priority 1 - */ - /* Enable USART interrupts */ - nvic_enable_irq(IRQn); -} - - -#if 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 - -#ifndef UART1_BITS -#define UART1_BITS UBITS_8 -#endif - -#ifndef UART1_STOP -#define UART1_STOP USTOP_1 -#endif - -#ifndef UART1_PARITY -#define UART1_PARITY UPARITY_NO -#endif - -void uart1_init(void) -{ - - uart_periph_init(&uart1); - uart1.reg_addr = (void *)USART1; - - /* init RCC and GPIOs */ - rcc_periph_clock_enable(RCC_USART1); - -#if USE_UART1_TX - gpio_setup_pin_af(UART1_GPIO_PORT_TX, UART1_GPIO_TX, UART1_GPIO_AF, TRUE); -#endif -#if USE_UART1_RX - gpio_setup_pin_af(UART1_GPIO_PORT_RX, UART1_GPIO_RX, UART1_GPIO_AF, FALSE); -#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 */ - gpio_setup_pin_af(UART1_GPIO_PORT_CTS, UART1_GPIO_CTS, UART1_GPIO_AF, FALSE); - gpio_setup_pin_af(UART1_GPIO_PORT_RTS, UART1_GPIO_RTS, UART1_GPIO_AF, TRUE); -#endif - - /* Configure USART1, enable hardware flow control*/ - uart_periph_set_mode(&uart1, USE_UART1_TX, USE_UART1_RX, UART1_HW_FLOW_CONTROL); - - /* Set USART1 parameters and enable interrupt */ - uart_periph_set_bits_stop_parity(&uart1, UART1_BITS, UART1_STOP, UART1_PARITY); - uart_periph_set_baudrate(&uart1, UART1_BAUD); -} - -void usart1_isr(void) { usart_isr(&uart1); } - -#endif /* USE_UART1 */ - - -#if 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 - -#ifndef UART2_BITS -#define UART2_BITS UBITS_8 -#endif - -#ifndef UART2_STOP -#define UART2_STOP USTOP_1 -#endif - -#ifndef UART2_PARITY -#define UART2_PARITY UPARITY_NO -#endif - -void uart2_init(void) -{ - - uart_periph_init(&uart2); - uart2.reg_addr = (void *)USART2; - - /* init RCC and GPIOs */ - rcc_periph_clock_enable(RCC_USART2); - -#if USE_UART2_TX - gpio_setup_pin_af(UART2_GPIO_PORT_TX, UART2_GPIO_TX, UART2_GPIO_AF, TRUE); -#endif -#if USE_UART2_RX - gpio_setup_pin_af(UART2_GPIO_PORT_RX, UART2_GPIO_RX, UART2_GPIO_AF, FALSE); -#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 */ - gpio_setup_pin_af(UART2_GPIO_PORT_CTS, UART2_GPIO_CTS, UART2_GPIO_AF, FALSE); - gpio_setup_pin_af(UART2_GPIO_PORT_RTS, UART2_GPIO_RTS, UART2_GPIO_AF, TRUE); -#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_bits_stop_parity(&uart2, UART2_BITS, UART2_STOP, UART2_PARITY); - uart_periph_set_baudrate(&uart2, UART2_BAUD); -} - -void usart2_isr(void) { usart_isr(&uart2); } - -#endif /* USE_UART2 */ - - -#if 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 - -#ifndef UART3_BITS -#define UART3_BITS UBITS_8 -#endif - -#ifndef UART3_STOP -#define UART3_STOP USTOP_1 -#endif - -#ifndef UART3_PARITY -#define UART3_PARITY UPARITY_NO -#endif - -void uart3_init(void) -{ - - uart_periph_init(&uart3); - uart3.reg_addr = (void *)USART3; - - /* init RCC */ - rcc_periph_clock_enable(RCC_USART3); - -#if USE_UART3_TX - gpio_setup_pin_af(UART3_GPIO_PORT_TX, UART3_GPIO_TX, UART3_GPIO_AF, TRUE); -#endif -#if USE_UART3_RX - gpio_setup_pin_af(UART3_GPIO_PORT_RX, UART3_GPIO_RX, UART3_GPIO_AF, FALSE); -#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 */ - gpio_setup_pin_af(UART3_GPIO_PORT_CTS, UART3_GPIO_CTS, UART3_GPIO_AF, FALSE); - gpio_setup_pin_af(UART3_GPIO_PORT_RTS, UART3_GPIO_RTS, UART3_GPIO_AF, TRUE); -#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_bits_stop_parity(&uart3, UART3_BITS, UART3_STOP, UART3_PARITY); - uart_periph_set_baudrate(&uart3, UART3_BAUD); -} - -void usart3_isr(void) { usart_isr(&uart3); } - -#endif /* USE_UART3 */ - - -#if USE_UART4 - -/* 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 - -#ifndef UART4_BITS -#define UART4_BITS UBITS_8 -#endif - -#ifndef UART4_STOP -#define UART4_STOP USTOP_1 -#endif - -#ifndef UART4_PARITY -#define UART4_PARITY UPARITY_NO -#endif - -void uart4_init(void) -{ - - uart_periph_init(&uart4); - uart4.reg_addr = (void *)UART4; - - /* init RCC and GPIOs */ - rcc_periph_clock_enable(RCC_UART4); - -#if USE_UART4_TX - gpio_setup_pin_af(UART4_GPIO_PORT_TX, UART4_GPIO_TX, UART4_GPIO_AF, TRUE); -#endif -#if USE_UART4_RX - gpio_setup_pin_af(UART4_GPIO_PORT_RX, UART4_GPIO_RX, UART4_GPIO_AF, FALSE); -#endif - - /* 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_bits_stop_parity(&uart4, UART4_BITS, UART4_STOP, UART4_PARITY); - uart_periph_set_baudrate(&uart4, UART4_BAUD); -} - -void uart4_isr(void) { usart_isr(&uart4); } - -#endif /* USE_UART4 */ - - -#if 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 - -#ifndef UART5_BITS -#define UART5_BITS UBITS_8 -#endif - -#ifndef UART5_STOP -#define UART5_STOP USTOP_1 -#endif - -#ifndef UART5_PARITY -#define UART5_PARITY UPARITY_NO -#endif - -void uart5_init(void) -{ - - uart_periph_init(&uart5); - uart5.reg_addr = (void *)UART5; - - /* init RCC and GPIOs */ - rcc_periph_clock_enable(RCC_UART5); - -#if USE_UART5_TX - gpio_setup_pin_af(UART5_GPIO_PORT_TX, UART5_GPIO_TX, UART5_GPIO_AF, TRUE); -#endif -#if USE_UART5_RX - gpio_setup_pin_af(UART5_GPIO_PORT_RX, UART5_GPIO_RX, UART5_GPIO_AF, FALSE); -#endif - - /* Enable USART interrupts in the interrupt controller */ - usart_enable_irq(NVIC_UART5_IRQ); - - /* Configure USART */ - uart_periph_set_mode(&uart5, USE_UART5_TX, USE_UART5_RX, FALSE); - uart_periph_set_bits_stop_parity(&uart5, UART5_BITS, UART5_STOP, UART5_PARITY); - uart_periph_set_baudrate(&uart5, UART5_BAUD); -} - -void uart5_isr(void) { usart_isr(&uart5); } - -#endif /* USE_UART5 */ - - -#if 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 - -#ifndef UART6_BITS -#define UART6_BITS UBITS_8 -#endif - -#ifndef UART6_STOP -#define UART6_STOP USTOP_1 -#endif - -#ifndef UART6_PARITY -#define UART6_PARITY UPARITY_NO -#endif - -void uart6_init(void) -{ - - uart_periph_init(&uart6); - uart6.reg_addr = (void *)USART6; - - /* enable uart clock */ - rcc_periph_clock_enable(RCC_USART6); - - /* init RCC and GPIOs */ -#if USE_UART6_TX - gpio_setup_pin_af(UART6_GPIO_PORT_TX, UART6_GPIO_TX, UART6_GPIO_AF, TRUE); -#endif -#if USE_UART6_RX - gpio_setup_pin_af(UART6_GPIO_PORT_RX, UART6_GPIO_RX, UART6_GPIO_AF, FALSE); -#endif - - /* 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 */ - gpio_setup_pin_af(UART6_GPIO_PORT_CTS, UART6_GPIO_CTS, UART6_GPIO_AF, FALSE); - gpio_setup_pin_af(UART6_GPIO_PORT_RTS, UART6_GPIO_RTS, UART6_GPIO_AF, TRUE); -#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_bits_stop_parity(&uart6, UART6_BITS, UART6_STOP, UART6_PARITY); - uart_periph_set_baudrate(&uart6, UART6_BAUD); -} - -void usart6_isr(void) { usart_isr(&uart6); } - -#endif /* USE_UART6 */ - - -#if USE_UART7 - -/* by default enable UART Tx and Rx */ -#ifndef USE_UART7_TX -#define USE_UART7_TX TRUE -#endif -#ifndef USE_UART7_RX -#define USE_UART7_RX TRUE -#endif - -#ifndef UART7_BITS -#define UART7_BITS UBITS_8 -#endif - -#ifndef UART7_STOP -#define UART7_STOP USTOP_1 -#endif - -#ifndef UART7_PARITY -#define UART7_PARITY UPARITY_NO -#endif - -void uart7_init(void) -{ - - uart_periph_init(&uart7); - uart7.reg_addr = (void *)UART7; - - /* init RCC and GPIOs */ - rcc_periph_clock_enable(RCC_UART7); - -#if USE_UART7_TX - gpio_setup_pin_af(UART7_GPIO_PORT_TX, UART7_GPIO_TX, UART7_GPIO_AF, TRUE); -#endif -#if USE_UART7_RX - gpio_setup_pin_af(UART7_GPIO_PORT_RX, UART7_GPIO_RX, UART7_GPIO_AF, FALSE); -#endif - - /* Enable USART interrupts in the interrupt controller */ - usart_enable_irq(NVIC_UART7_IRQ); - - /* Configure USART */ - uart_periph_set_mode(&uart7, USE_UART7_TX, USE_UART7_RX, FALSE); - uart_periph_set_bits_stop_parity(&uart7, UART7_BITS, UART7_STOP, UART7_PARITY); - uart_periph_set_baudrate(&uart7, UART7_BAUD); -} - -void uart7_isr(void) { usart_isr(&uart7); } - -#endif /* USE_UART7 */ - - -#if USE_UART8 - -/* by default enable UART Tx and Rx */ -#ifndef USE_UART8_TX -#define USE_UART8_TX TRUE -#endif -#ifndef USE_UART8_RX -#define USE_UART8_RX TRUE -#endif - -#ifndef UART8_BITS -#define UART8_BITS UBITS_8 -#endif - -#ifndef UART8_STOP -#define UART8_STOP USTOP_1 -#endif - -#ifndef UART8_PARITY -#define UART8_PARITY UPARITY_NO -#endif - -void uart8_init(void) -{ - - uart_periph_init(&uart8); - uart8.reg_addr = (void *)UART8; - - /* init RCC and GPIOs */ - rcc_periph_clock_enable(RCC_UART8); - -#if USE_UART8_TX - gpio_setup_pin_af(UART8_GPIO_PORT_TX, UART8_GPIO_TX, UART8_GPIO_AF, TRUE); -#endif -#if USE_UART8_RX - gpio_setup_pin_af(UART8_GPIO_PORT_RX, UART8_GPIO_RX, UART8_GPIO_AF, FALSE); -#endif - - /* Enable USART interrupts in the interrupt controller */ - usart_enable_irq(NVIC_UART8_IRQ); - - /* Configure USART */ - uart_periph_set_mode(&uart8, USE_UART8_TX, USE_UART8_RX, FALSE); - uart_periph_set_bits_stop_parity(&uart8, UART8_BITS, UART8_STOP, UART8_PARITY); - uart_periph_set_baudrate(&uart8, UART8_BAUD); -} - -void uart8_isr(void) { usart_isr(&uart8); } - -#endif /* USE_UART8 */ diff --git a/sw/airborne/arch/stm32/mcu_periph/uart_arch.h b/sw/airborne/arch/stm32/mcu_periph/uart_arch.h deleted file mode 100644 index 28ee449907..0000000000 --- a/sw/airborne/arch/stm32/mcu_periph/uart_arch.h +++ /dev/null @@ -1,48 +0,0 @@ -/* - * Copyright (C) 2009-2010 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 arch/stm32/mcu_periph/uart_arch.h - * @ingroup stm32_arch - * - * Handling of UART hardware for STM32. - */ - -#ifndef STM32_UART_ARCH_H -#define STM32_UART_ARCH_H - -#define B1200 1200 -#define B2400 2400 -#define B4800 4800 -#define B9600 9600 -#define B19200 19200 -#define B38400 38400 -#define B57600 57600 -#define B100000 100000 -#define B115200 115200 -#define B230400 230400 -#define B460800 460800 -#define B921600 921600 -#define B1500000 1500000 -#define B3000000 3000000 -#define UART_SPEED(_def) _def - -#endif /* STM32_UART_ARCH_H */ diff --git a/sw/airborne/arch/stm32/modules/actuators/actuators_dualpwm_arch.c b/sw/airborne/arch/stm32/modules/actuators/actuators_dualpwm_arch.c deleted file mode 100644 index 18150ed244..0000000000 --- a/sw/airborne/arch/stm32/modules/actuators/actuators_dualpwm_arch.c +++ /dev/null @@ -1,183 +0,0 @@ -/* - * Copyright (C) 2010 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 arch/stm32/modules/actuators/actuators_dualpwm_arch.c - * STM32 dual PWM servos handling. - */ - -//VALID TIMERS IS TIM5 ON THE LISA/M - -#include "modules/actuators/actuators_shared_arch.h" -#include "modules/actuators/actuators_dualpwm_arch.h" -#include "modules/actuators/actuators_dualpwm.h" - -#include -#include -#include -#include - -#include "mcu_periph/gpio_arch.h" - -uint32_t ratio_4ms, ratio_16ms; - - -uint32_t actuators_dualpwm_values[ACTUATORS_DUALPWM_NB]; - -/** PWM arch init called by generic pwm driver - */ -void actuators_dualpwm_arch_init(void) -{ - - /*----------------------------------- - * Configure timer peripheral clocks - *-----------------------------------*/ -#if PWM_USE_TIM1 - rcc_periph_clock_enable(RCC_TIM1); -#endif -#if PWM_USE_TIM2 - rcc_periph_clock_enable(RCC_TIM2); -#endif -#if PWM_USE_TIM3 - rcc_periph_clock_enable(RCC_TIM3); -#endif -#if PWM_USE_TIM4 - rcc_periph_clock_enable(RCC_TIM4); -#endif -#if PWM_USE_TIM5 - rcc_periph_clock_enable(RCC_TIM5); -#endif -#if PWM_USE_TIM8 - rcc_periph_clock_enable(RCC_TIM8); -#endif -#if PWM_USE_TIM9 - rcc_periph_clock_enable(RCC_TIM9); -#endif -#if PWM_USE_TIM12 - rcc_periph_clock_enable(RCC_TIM12); -#endif - - /*---------------- - * Configure GPIO - *----------------*/ -#ifdef DUAL_PWM_SERVO_5 - gpio_setup_pin_af(DUAL_PWM_SERVO_5_GPIO, DUAL_PWM_SERVO_5_PIN, DUAL_PWM_SERVO_5_AF, TRUE); -#endif -#ifdef DUAL_PWM_SERVO_6 - gpio_setup_pin_af(DUAL_PWM_SERVO_6_GPIO, DUAL_PWM_SERVO_6_PIN, DUAL_PWM_SERVO_6_AF, TRUE); -#endif - -#if DUAL_PWM_USE_TIM5 - rcc_periph_reset_pulse(RST_TIM5); - set_servo_timer(TIM5, TIM5_SERVO_HZ, PWM_TIM5_CHAN_MASK); - - nvic_set_priority(NVIC_TIM5_IRQ, 2); - nvic_enable_irq(NVIC_TIM5_IRQ); - timer_enable_irq(TIM5, TIM_DIER_CC1IE); -#endif - - //calculation the values to put into the timer registers to generate pulses every 4ms and 16ms. - ratio_4ms = (ONE_MHZ_CLK / 250) - 1; - ratio_16ms = (ONE_MHZ_CLK / 62.5) - 1; - -} - - -/** Interuption called at the end of the timer. In our case alternatively very 4ms and 16ms (twice every 20ms) - */ -#if DUAL_PWM_USE_TIM5 -void tim5_isr(void) -{ - - dual_pwm_isr(); -} -#endif - - - - -/** Fonction that clears the flag of interuption in order to reactivate the interuption - */ -void clear_timer_flag(void) -{ - -#if DUAL_PWM_USE_TIM5 - timer_clear_flag(TIM5, TIM_SR_CC1IF); -#endif -} - - -void set_dual_pwm_timer_s_period(uint32_t period) -{ - -#if DUAL_PWM_USE_TIM5 - timer_set_period(TIM5, period); -#endif -} - - -void set_dual_pwm_timer_s_oc(uint32_t oc_value, uint32_t oc_value2) -{ - -#if DUAL_PWM_USE_TIM5 - timer_set_oc_value(DUAL_PWM_SERVO_5_TIMER, DUAL_PWM_SERVO_5_OC, oc_value); - timer_set_oc_value(DUAL_PWM_SERVO_6_TIMER, DUAL_PWM_SERVO_6_OC, oc_value2); -#endif -} - - - - -void dual_pwm_isr(void) -{ - - static int num_pulse = 0; //status of the timer. Are we controling the first or the second servo - - clear_timer_flag(); - - if (num_pulse == 1) { - - set_dual_pwm_timer_s_period(ratio_16ms); - set_dual_pwm_timer_s_oc(actuators_dualpwm_values[DUAL_PWM_SERVO_5_P1],actuators_dualpwm_values[DUAL_PWM_SERVO_5_P2]); - - num_pulse = 0; - } else { - - set_dual_pwm_timer_s_period(ratio_4ms); - set_dual_pwm_timer_s_oc(actuators_dualpwm_values[DUAL_PWM_SERVO_6_P1],actuators_dualpwm_values[DUAL_PWM_SERVO_6_P2]); - - num_pulse = 1; - } -} - -void actuators_dualpwm_set(uint8_t idx, int16_t value) -{ - actuators_dualpwm_values[idx] = value; -} - -/** Set pulse widths from actuator values, assumed to be in us - */ -void actuators_dualpwm_commit(void) -{ - - //we don't need to commit the values into this function as far as it's done in the interuption - //(wich is called every 4ms and 16ms alternatively (twice every 20ms)) - -} diff --git a/sw/airborne/arch/stm32/modules/actuators/actuators_dualpwm_arch.h b/sw/airborne/arch/stm32/modules/actuators/actuators_dualpwm_arch.h deleted file mode 100644 index 11c5bf0359..0000000000 --- a/sw/airborne/arch/stm32/modules/actuators/actuators_dualpwm_arch.h +++ /dev/null @@ -1,58 +0,0 @@ -/* - * Copyright (C) 2010 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 arch/stm32/modules/actuators/actuators_dualpwm_arch.h - * STM32 PWM servos handling. - */ - -#ifndef ACTUATORS_dualpwm_ARCH_H -#define ACTUATORS_dualpwm_ARCH_H - -#include "std.h" - -#include BOARD_CONFIG - -// Max 2 dualpwm channels of 2 pulses each -#ifndef ACTUATORS_DUALPWM_NB -#define ACTUATORS_DUALPWM_NB 4 -#endif - - -extern uint32_t actuators_dualpwm_values[ACTUATORS_DUALPWM_NB]; - -extern void actuators_dualpwm_set(uint8_t idx, int16_t value); - -extern void actuators_dualpwm_commit(void); - -extern void dual_pwm_isr(void); - -extern void clear_timer_flag(void); - -extern void set_dual_pwm_timer_s_period(uint32_t period); - -extern void set_dual_pwm_timer_s_oc(uint32_t oc_value, uint32_t oc_value2); - -#define SERVOS_TICS_OF_USEC(_v) (_v) - -#define ActuatorDualpwmSet actuators_dualpwm_set -#define ActuatorsDualpwmCommit actuators_dualpwm_commit - -#endif /* ACTUATORS_dualpwm_ARCH_H */ diff --git a/sw/airborne/arch/stm32/modules/actuators/actuators_pwm_arch.c b/sw/airborne/arch/stm32/modules/actuators/actuators_pwm_arch.c deleted file mode 100644 index 692df593fc..0000000000 --- a/sw/airborne/arch/stm32/modules/actuators/actuators_pwm_arch.c +++ /dev/null @@ -1,195 +0,0 @@ -/* - * Copyright (C) 2010 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 arch/stm32/modules/actuators/actuators_pwm_arch.c - * STM32 PWM servos handling. - */ - -//VALID TIMERS ARE TIM1,2,3,4,5,8,9,12 - -#include "modules/actuators/actuators_shared_arch.h" -#include "modules/actuators/actuators_pwm_arch.h" -#include "modules/actuators/actuators_pwm.h" - -#include -#include -#include - -#include "mcu_periph/gpio_arch.h" - - -/** PWM arch init called by generic pwm driver - */ -void actuators_pwm_arch_init(void) -{ - /*----------------------------------- - * Configure timer peripheral clocks - *-----------------------------------*/ -#if PWM_USE_TIM1 - rcc_periph_clock_enable(RCC_TIM1); -#endif -#if PWM_USE_TIM2 - rcc_periph_clock_enable(RCC_TIM2); -#endif -#if PWM_USE_TIM3 - rcc_periph_clock_enable(RCC_TIM3); -#endif -#if PWM_USE_TIM4 - rcc_periph_clock_enable(RCC_TIM4); -#endif -#if PWM_USE_TIM5 - rcc_periph_clock_enable(RCC_TIM5); -#endif -#if PWM_USE_TIM8 - rcc_periph_clock_enable(RCC_TIM8); -#endif -#if PWM_USE_TIM9 - rcc_periph_clock_enable(RCC_TIM9); -#endif -#if PWM_USE_TIM12 - rcc_periph_clock_enable(RCC_TIM12); -#endif - - /*---------------- - * Configure GPIO - *----------------*/ -#ifdef PWM_SERVO_0 - gpio_setup_pin_af(PWM_SERVO_0_GPIO, PWM_SERVO_0_PIN, PWM_SERVO_0_AF, TRUE); -#endif -#ifdef PWM_SERVO_1 - gpio_setup_pin_af(PWM_SERVO_1_GPIO, PWM_SERVO_1_PIN, PWM_SERVO_1_AF, TRUE); -#endif -#ifdef PWM_SERVO_2 - gpio_setup_pin_af(PWM_SERVO_2_GPIO, PWM_SERVO_2_PIN, PWM_SERVO_2_AF, TRUE); -#endif -#ifdef PWM_SERVO_3 - gpio_setup_pin_af(PWM_SERVO_3_GPIO, PWM_SERVO_3_PIN, PWM_SERVO_3_AF, TRUE); -#endif -#ifdef PWM_SERVO_4 - gpio_setup_pin_af(PWM_SERVO_4_GPIO, PWM_SERVO_4_PIN, PWM_SERVO_4_AF, TRUE); -#endif -#ifdef PWM_SERVO_5 - gpio_setup_pin_af(PWM_SERVO_5_GPIO, PWM_SERVO_5_PIN, PWM_SERVO_5_AF, TRUE); -#endif -#ifdef PWM_SERVO_6 - gpio_setup_pin_af(PWM_SERVO_6_GPIO, PWM_SERVO_6_PIN, PWM_SERVO_6_AF, TRUE); -#endif -#ifdef PWM_SERVO_7 - gpio_setup_pin_af(PWM_SERVO_7_GPIO, PWM_SERVO_7_PIN, PWM_SERVO_7_AF, TRUE); -#endif -#ifdef PWM_SERVO_8 - gpio_setup_pin_af(PWM_SERVO_8_GPIO, PWM_SERVO_8_PIN, PWM_SERVO_8_AF, TRUE); -#endif -#ifdef PWM_SERVO_9 - gpio_setup_pin_af(PWM_SERVO_9_GPIO, PWM_SERVO_9_PIN, PWM_SERVO_9_AF, TRUE); -#endif -#ifdef PWM_SERVO_10 - gpio_setup_pin_af(PWM_SERVO_10_GPIO, PWM_SERVO_10_PIN, PWM_SERVO_10_AF, TRUE); -#endif -#ifdef PWM_SERVO_11 - gpio_setup_pin_af(PWM_SERVO_11_GPIO, PWM_SERVO_11_PIN, PWM_SERVO_11_AF, TRUE); -#endif - -#if PWM_USE_TIM1 - rcc_periph_reset_pulse(RST_TIM1); - set_servo_timer(TIM1, TIM1_SERVO_HZ, PWM_TIM1_CHAN_MASK); -#endif - -#if PWM_USE_TIM2 - rcc_periph_reset_pulse(RST_TIM2); - set_servo_timer(TIM2, TIM2_SERVO_HZ, PWM_TIM2_CHAN_MASK); -#endif - -#if PWM_USE_TIM3 - rcc_periph_reset_pulse(RST_TIM3); - set_servo_timer(TIM3, TIM3_SERVO_HZ, PWM_TIM3_CHAN_MASK); -#endif - -#if PWM_USE_TIM4 - rcc_periph_reset_pulse(RST_TIM4); - set_servo_timer(TIM4, TIM4_SERVO_HZ, PWM_TIM4_CHAN_MASK); -#endif - -#if PWM_USE_TIM5 - rcc_periph_reset_pulse(RST_TIM5); - set_servo_timer(TIM5, TIM5_SERVO_HZ, PWM_TIM5_CHAN_MASK); -#endif - -#if PWM_USE_TIM8 - rcc_periph_reset_pulse(RST_TIM8); - set_servo_timer(TIM8, TIM8_SERVO_HZ, PWM_TIM8_CHAN_MASK); -#endif - -#if PWM_USE_TIM9 - rcc_periph_reset_pulse(RST_TIM9); - set_servo_timer(TIM9, TIM9_SERVO_HZ, PWM_TIM9_CHAN_MASK); -#endif - -#if PWM_USE_TIM12 - rcc_periph_reset_pulse(RST_TIM12); - set_servo_timer(TIM12, TIM12_SERVO_HZ, PWM_TIM12_CHAN_MASK); -#endif - -} - -/** Set pulse widths from actuator values, assumed to be in us - */ -void actuators_pwm_arch_commit(void) -{ -#ifdef PWM_SERVO_0 - timer_set_oc_value(PWM_SERVO_0_TIMER, PWM_SERVO_0_OC, actuators_pwm_values[PWM_SERVO_0]); -#endif -#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 -#ifdef PWM_SERVO_9 - timer_set_oc_value(PWM_SERVO_9_TIMER, PWM_SERVO_9_OC, actuators_pwm_values[PWM_SERVO_9]); -#endif -#ifdef PWM_SERVO_10 - timer_set_oc_value(PWM_SERVO_10_TIMER, PWM_SERVO_10_OC, actuators_pwm_values[PWM_SERVO_10]); -#endif -#ifdef PWM_SERVO_11 - timer_set_oc_value(PWM_SERVO_11_TIMER, PWM_SERVO_11_OC, actuators_pwm_values[PWM_SERVO_11]); -#endif - -} - diff --git a/sw/airborne/arch/stm32/modules/actuators/actuators_pwm_arch.h b/sw/airborne/arch/stm32/modules/actuators/actuators_pwm_arch.h deleted file mode 100644 index fe8271d303..0000000000 --- a/sw/airborne/arch/stm32/modules/actuators/actuators_pwm_arch.h +++ /dev/null @@ -1,35 +0,0 @@ -/* - * Copyright (C) 2010 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 arch/stm32/modules/actuators/actuators_pwm_arch.h - * STM32 PWM servos handling. - */ - -#ifndef ACTUATORS_PWM_ARCH_H -#define ACTUATORS_PWM_ARCH_H - -#include "std.h" - -#include BOARD_CONFIG - -#define SERVOS_TICS_OF_USEC(_v) (_v) - -#endif /* ACTUATORS_PWM_ARCH_H */ diff --git a/sw/airborne/arch/stm32/modules/actuators/actuators_shared_arch.c b/sw/airborne/arch/stm32/modules/actuators/actuators_shared_arch.c deleted file mode 100644 index 767711fee2..0000000000 --- a/sw/airborne/arch/stm32/modules/actuators/actuators_shared_arch.c +++ /dev/null @@ -1,122 +0,0 @@ -/* - * Copyright (C) 2010 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 arch/stm32/modules/actuators/actuators_shared_arch.c - * STM32 PWM and dualPWM servos shared functions. - */ - -#include "arch/stm32/modules/actuators/actuators_shared_arch.h" - -#include -// for timer_get_frequency -#include "arch/stm32/mcu_arch.h" - - -/** Set PWM channel configuration - */ -void actuators_pwm_arch_channel_init(uint32_t timer_peripheral, - enum tim_oc_id oc_id) -{ - - timer_disable_oc_output(timer_peripheral, oc_id); - //There is no such register in TIM9 and 12. - if (timer_peripheral != TIM9 && timer_peripheral != TIM12) { - 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); - timer_set_oc_mode(timer_peripheral, oc_id, TIM_OCM_PWM1); - timer_set_oc_polarity_high(timer_peripheral, oc_id); - timer_enable_oc_output(timer_peripheral, oc_id); - // Used for TIM1 and TIM8, the function does nothing if other timer is specified. - timer_enable_break_main_output(timer_peripheral); -} - - -/** Set Timer configuration - * @param[in] timer Timer register address base - * @param[in] freq PWM frequency in Hz (1 / auto-reload period) - * @param[in] channels_mask output compare channels to enable - */ -void set_servo_timer(uint32_t timer, uint32_t freq, uint8_t channels_mask) -{ - /* Timer global mode: - * - No divider. - * - Alignement edge. - * - Direction up. - */ - if ((timer == TIM9) || (timer == TIM12)) - //There are no EDGE and DIR settings in TIM9 and TIM12 - { - timer_set_mode(timer, TIM_CR1_CKD_CK_INT, 0, 0); - } else { - timer_set_mode(timer, TIM_CR1_CKD_CK_INT, TIM_CR1_CMS_EDGE, TIM_CR1_DIR_UP); - } - - - // By default the PWM_BASE_FREQ is set to 1MHz thus the timer tick period is 1uS - uint32_t timer_clk = timer_get_frequency(timer); - timer_set_prescaler(timer, (timer_clk / PWM_BASE_FREQ) - 1); - - timer_disable_preload(timer); - - timer_continuous_mode(timer); - - timer_set_period(timer, (PWM_BASE_FREQ / freq) - 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); - -} - diff --git a/sw/airborne/arch/stm32/modules/actuators/actuators_shared_arch.h b/sw/airborne/arch/stm32/modules/actuators/actuators_shared_arch.h deleted file mode 100644 index 83c3551c17..0000000000 --- a/sw/airborne/arch/stm32/modules/actuators/actuators_shared_arch.h +++ /dev/null @@ -1,81 +0,0 @@ -/* - * Copyright (C) 2010 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 arch/stm32/modules/actuators/actuators_shared_arch.h - * STM32 PWM and dualPWM servos shared functions. - */ - -#ifndef ACTUATORS_PWM_SHARED_ARCH_H -#define ACTUATORS_PWM_SHARED_ARCH_H - -#include "std.h" - -#include BOARD_CONFIG - -#include -#include -#include -#include -#include "mcu_arch.h" - -#define ONE_MHZ_CLK 1000000 - -/* Default timer base frequency is 1MHz */ -#if ! defined(PWM_BASE_FREQ) -#define PWM_BASE_FREQ ONE_MHZ_CLK -#endif - - -/** Default servo update rate in Hz */ -#ifndef SERVO_HZ -#define SERVO_HZ 40 -#endif - -// 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 -#ifndef TIM8_SERVO_HZ -#define TIM8_SERVO_HZ SERVO_HZ -#endif -#ifndef TIM9_SERVO_HZ -#define TIM9_SERVO_HZ SERVO_HZ -#endif -#ifndef TIM12_SERVO_HZ -#define TIM12_SERVO_HZ SERVO_HZ -#endif - -extern void actuators_pwm_arch_channel_init(uint32_t timer_peripheral, enum tim_oc_id oc_id); -extern void set_servo_timer(uint32_t timer, uint32_t period, uint8_t channels_mask); - -#endif /* ACTUATORS_PWM_SHARED_ARCH_H */ diff --git a/sw/airborne/arch/stm32/modules/core/settings_arch.c b/sw/airborne/arch/stm32/modules/core/settings_arch.c deleted file mode 100644 index 1762c5d447..0000000000 --- a/sw/airborne/arch/stm32/modules/core/settings_arch.c +++ /dev/null @@ -1,320 +0,0 @@ -/* - * Copyright (C) 2011 Martin Mueller - * - * 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 arch/stm32/modules/core/settings_arch.c - * Persistent settings low level flash routines stm32. - * - * data flash_addr - * data_size flash_end - FSIZ - * checksum flash_end - FCHK - * - * STM32: minimum write size 2 bytes, endurance 10k cycles, - * max sector erase time 40ms, max prog time 70us per 2 bytes - */ - -#include "modules/core/settings.h" - -#include -#include -#include - -struct FlashInfo { - uint32_t addr; - uint32_t total_size; - uint32_t page_nr; - uint32_t page_size; -}; - - -static uint32_t pflash_checksum(uint32_t ptr, uint32_t size); -static int32_t flash_detect(struct FlashInfo *flash); -static int32_t pflash_program_bytes(struct FlashInfo *flash, - uint32_t src, - 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 -#define FCHK 4 - - -static uint32_t pflash_checksum(uint32_t ptr, uint32_t size) -{ - uint32_t i; - - /* reset crc */ - CRC_CR = CRC_CR_RESET; - - if (ptr % 4) { - /* calc in 8bit chunks */ - for (i = 0; i < (size & ~3); i += 4) { - CRC_DR = (*(uint8_t *)(ptr + i)) | - (*(uint8_t *)(ptr + i + 1)) << 8 | - (*(uint8_t *)(ptr + i + 2)) << 16 | - (*(uint8_t *)(ptr + i + 3)) << 24; - } - } else { - /* calc in 32bit */ - for (i = 0; i < (size & ~3); i += 4) { - CRC_DR = *(uint32_t *)(ptr + i); - } - } - - /* remaining bytes */ - switch (size % 4) { - case 1: - CRC_DR = *(uint8_t *)(ptr + i); - break; - case 2: - CRC_DR = (*(uint8_t *)(ptr + i)) | - (*(uint8_t *)(ptr + i + 1)) << 8; - break; - case 3: - CRC_DR = (*(uint8_t *)(ptr + i)) | - (*(uint8_t *)(ptr + i + 1)) << 8 | - (*(uint8_t *)(ptr + i + 2)) << 16; - break; - default: - break; - } - - return CRC_DR; -} - -static int32_t flash_detect(struct FlashInfo *flash) -{ - - flash->total_size = FLASH_SIZE_ * 0x400; - -#if defined(STM32F1) - /* FIXME This will not work for connectivity line (needs ID, see below), but - device ID is only readable when freshly loaded through JTAG?! */ - - /* WARNING If you are using this for F4 this only works for memory sizes - * larger than 128kb. Otherwise the first few sectors are either 16kb or - * 64kb. To make those small devices work we would need to know what the page - * we want to put the settings into is. Otherwise we will might be writing - * into a 64kb page that is actually 16kb big. - */ - switch (flash->total_size) { - /* low density */ - case 0x00004000: /* 16 kBytes */ - case 0x00008000: /* 32 kBytes */ - /* medium density, e.g. STM32F103RBT6 (Olimex STM32-H103) */ - case 0x00010000: /* 64 kBytes */ - case 0x00020000: { /* 128 kBytes */ - flash->page_size = 0x400; - break; - } - /* high density, e.g. STM32F103RE (Joby Lisa/M, Lisa/L) */ - case 0x00040000: /* 256 kBytes */ - case 0x00080000: /* 512 kBytes */ - /* XL density */ - case 0x000C0000: /* 768 kBytes */ - case 0x00100000: { /* 1 MByte */ - flash->page_size = 0x800; - break; - } - default: {return -1;} - } - -#elif defined(STM32F4) /* this is the correct way of detecting page sizes but we currently only use it for the F4 because the F1 version is broken. */ - uint32_t device_id; - - /* read device id */ - device_id = DBGMCU_IDCODE & DBGMCU_IDCODE_DEV_ID_MASK; - - switch (device_id) { - /* low density */ - case 0x412: - /* medium density, e.g. STM32F103RB (Olimex STM32-H103) */ - case 0x410: { - flash->page_size = 0x400; - break; - } - /* high density, e.g. STM32F103RE (Joby Lisa/L) */ - case 0x414: - /* XL density */ - case 0x430: - /* connectivity line */ - case 0x418: { - flash->page_size = 0x800; - break; - } - case 0x0413: /* STM32F405xx/07xx and STM32F415xx/17xx) */ - case 0x0419: /* STM32F42xxx and STM32F43xxx */ - case 0x0423: /* STM32F401xB/C */ - case 0x0433: /* STM32F401xD/E */ - case 0x0431: { /* STM32F411xC/E */ - flash->page_size = 0x20000; - break; - } - default: return -1; - } - - switch (flash->total_size) { - case 0x00004000: /* 16 kBytes */ - case 0x00008000: /* 32 kBytes */ - case 0x00010000: /* 64 kBytes */ - case 0x00020000: /* 128 kBytes */ - case 0x00040000: /* 256 kBytes */ - case 0x00080000: /* 512 kBytes */ - case 0x000C0000: /* 768 kBytes */ - case 0x00100000: /* 1 MByte */ - case 0x00200000: /* 2 MByte */ - break; - default: return -1; - } -#else -#error Unknown device -#endif - -#if defined(STM32F1) - flash->page_nr = (flash->total_size / flash->page_size) - 1; - flash->addr = FLASH_BEGIN + flash->page_nr * flash->page_size; -#elif defined(STM32F4) - /* We are assuming all pages are 128kb so we have to compensate for the first - * few pages that are smaller which means we have to skip the first 4. - */ - flash->page_nr = (flash->total_size / flash->page_size) - 1 + 4; - flash->addr = FLASH_BEGIN + (flash->page_nr - 4) * flash->page_size; -#endif - - return 0; -} - - -static int32_t pflash_erase(struct FlashInfo *flash) -{ - uint32_t i; - - /* erase */ - flash_unlock(); -#if defined(STM32F1) - flash_erase_page(flash->addr); -#elif defined(STM32F4) - flash_erase_sector(flash->page_nr, FLASH_CR_PROGRAM_X32); -#endif - flash_lock(); - - /* verify erase */ - for (i = 0; i < flash->page_size; i += 4) { - if ((*(uint32_t *)(flash->addr + i)) != 0xFFFFFFFF) { return -1; } - } - return 0; -} - -// (gdb) p *flash -// $1 = {addr = 134739968, total_size = 524288, page_nr = 255, page_size = 2048} -// 0x807F800 0x80000 -static int32_t pflash_program_bytes(struct FlashInfo *flash, - uint32_t src, - uint32_t size, - uint32_t chksum) -{ - uint32_t i; - - /* erase, return with error if not successful */ - if (pflash_erase(flash)) { return -1; } - - flash_unlock(); - /* write full 16 bit words */ - for (i = 0; i < (size & ~1); i += 2) { - flash_program_half_word(flash->addr + i, - (uint16_t)(*(uint8_t *)(src + i) | (*(uint8_t *)(src + i + 1)) << 8)); - } - /* fill bytes with a zero */ - if (size & 1) { - flash_program_half_word(flash->addr + i, (uint16_t)(*(uint8_t *)(src + i))); - } - /* write size */ - flash_program_half_word(flash->addr + flash->page_size - FSIZ, - (uint16_t)(size & 0xFFFF)); - flash_program_half_word(flash->addr + flash->page_size - FSIZ + 2, - (uint16_t)((size >> 16) & 0xFFFF)); - /* write checksum */ - flash_program_half_word(flash->addr + flash->page_size - FCHK, - (uint16_t)(chksum & 0xFFFF)); - flash_program_half_word(flash->addr + flash->page_size - FCHK + 2, - (uint16_t)((chksum >> 16) & 0xFFFF)); - flash_lock(); - - /* verify data */ - for (i = 0; i < size; i++) { - if ((*(uint8_t *)(flash->addr + i)) != (*(uint8_t *)(src + i))) { return -2; } - } - if (*(uint32_t *)(flash->addr + flash->page_size - FSIZ) != size) { return -3; } - if (*(uint32_t *)(flash->addr + flash->page_size - FCHK) != chksum) { return -4; } - - return 0; -} - -int32_t persistent_write(void *ptr, uint32_t size) -{ - struct FlashInfo flash_info; - if (flash_detect(&flash_info)) { return -1; } - if ((size > flash_info.page_size - FSIZ) || (size == 0)) { return -2; } - - return pflash_program_bytes(&flash_info, - (uint32_t)ptr, - size, - pflash_checksum((uint32_t)ptr, size)); -} - -int32_t persistent_read(void *ptr, uint32_t size) -{ - struct FlashInfo flash; - uint32_t i; - - /* check parameters */ - if (flash_detect(&flash)) { return -1; } - if ((size > flash.page_size - FSIZ) || (size == 0)) { return -2; } - - /* check consistency */ - if (size != *(uint32_t *)(flash.addr + flash.page_size - FSIZ)) { return -3; } - if (pflash_checksum(flash.addr, size) != - *(uint32_t *)(flash.addr + flash.page_size - FCHK)) { - return -4; - } - - /* copy data */ - for (i = 0; i < size; i++) { - *(uint8_t *)((uint32_t)ptr + i) = *(uint8_t *)(flash.addr + i); - } - - return 0; -} - -int32_t persistent_clear(void) -{ - struct FlashInfo flash_info; - if (flash_detect(&flash_info)) { return -1; } - - return pflash_erase(&flash_info); -} diff --git a/sw/airborne/arch/stm32/modules/core/threads_arch.c b/sw/airborne/arch/stm32/modules/core/threads_arch.c deleted file mode 100644 index 1d3829f4e5..0000000000 --- a/sw/airborne/arch/stm32/modules/core/threads_arch.c +++ /dev/null @@ -1,66 +0,0 @@ -/* - * Copyright (C) 2025 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 3 of the License, or - * (at your option) any later version. - * - * See LICENSE file for the full license version, or see http://www.gnu.org/licenses/ - */ -#include "modules/core/threads.h" -#include "modules/core/threads_arch.h" -#include "stdbool.h" -#include "mcu_periph/sys_time.h" - -int pprz_mtx_init(pprz_mutex_t* mtx) { - (void)mtx; - return 0; -} - -int pprz_mtx_lock(pprz_mutex_t* mtx) { - (void)mtx; - return 0; -} - -int pprz_mtx_trylock(pprz_mutex_t* mtx) { - (void)mtx; - (void)mtx; - return 0; -} - -int pprz_mtx_unlock(pprz_mutex_t* mtx) { - (void)mtx; - return 0; -} - -void pprz_bsem_init(pprz_bsem_t* bsem, bool taken) { - bsem->value = taken ? 0: 1; -} - -void pprz_bsem_wait(pprz_bsem_t* bsem) { - while (bsem->value == 0) - { - // active wait - asm("NOP"); - } - bsem->value = 0; -} - -int pprz_bsem_wait_timeout(pprz_bsem_t* bsem, float timeout) { - float time_end = get_sys_time_float() + timeout; - while(get_sys_time_float() - time_end > 0) { - // active wait - if(bsem->value) { - bsem->value = 0; - return 0; - } - } - return -1; -} - -void pprz_bsem_signal(pprz_bsem_t* bsem) { - bsem->value = 1; -} diff --git a/sw/airborne/arch/stm32/modules/core/threads_arch.h b/sw/airborne/arch/stm32/modules/core/threads_arch.h deleted file mode 100644 index ceff0f05d7..0000000000 --- a/sw/airborne/arch/stm32/modules/core/threads_arch.h +++ /dev/null @@ -1,29 +0,0 @@ -/* - * Copyright (C) 2025 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 3 of the License, or - * (at your option) any later version. - * - * See LICENSE file for the full license version, or see http://www.gnu.org/licenses/ - */ - -#pragma once - -#define PPRZ_NORMAL_PRIO NORMALPRIO - -#define THREADS_ATTRIBUTES __attribute__((error("Threads cannot be used in STM32 bare metal ARCH."))) - -struct pprzMutex { - int dummy; // avoid warning: empty declaration -}; - -struct pprzBSem { - volatile int value; -}; - - -typedef int pprz_thread_t; diff --git a/sw/airborne/arch/stm32/modules/imu/imu_aspirin_arch.c b/sw/airborne/arch/stm32/modules/imu/imu_aspirin_arch.c deleted file mode 100644 index 9cd2d1f374..0000000000 --- a/sw/airborne/arch/stm32/modules/imu/imu_aspirin_arch.c +++ /dev/null @@ -1,117 +0,0 @@ -#include "modules/imu/imu.h" - -#include -#include -#include -#include -#include -#include - -#include "mcu_periph/i2c.h" - -#ifndef STM32F1 -#error "imu_aspirin_arch arch currently only implemented for STM32F1" -#endif - -void imu_aspirin_arch_int_enable(void) -{ - -#ifdef ASPIRIN_USE_GYRO_INT - nvic_set_priority(NVIC_EXTI15_10_IRQ, 0x0F); - nvic_enable_irq(NVIC_EXTI15_10_IRQ); -#endif - - nvic_set_priority(NVIC_EXTI2_IRQ, 0x0F); - nvic_enable_irq(NVIC_EXTI2_IRQ); - - // should not be needed anymore, handled by the spi driver -#if 0 - /* Enable DMA1 channel4 IRQ Channel ( SPI RX) */ - nvic_set_priority(NVIC_DMA1_CHANNEL4_IRQ, 0); - nvic_enable_irq(NVIC_DMA1_CHANNEL4_IRQ); -#endif -} - -void imu_aspirin_arch_int_disable(void) -{ - -#ifdef ASPIRIN_USE_GYRO_INT - nvic_disable_irq(NVIC_EXTI15_10_IRQ); -#endif - - nvic_disable_irq(NVIC_EXTI2_IRQ); - - // should not be needed anymore, handled by the spi driver -#if 0 - /* Enable DMA1 channel4 IRQ Channel ( SPI RX) */ - nvic_disable_irq(NVIC_DMA1_CHANNEL4_IRQ); -#endif -} - -void imu_aspirin_arch_init(void) -{ - - // This was needed for Lisa/L???? -#if 0 - /* Set "mag ss" and "mag reset" as floating inputs ------------------------*/ - /* "mag ss" (PC12) is shorted to I2C2 SDA */ - /* "mag reset" (PC13) is shorted to I2C2 SCL */ - rcc_periph_clock_enable(RCC_GPIOC); - gpio_set_mode(GPIOC, GPIO_MODE_INPUT, - GPIO_CNF_INPUT_FLOAT, GPIO12 | GPIO13); -#endif - - /* Gyro --------------------------------------------------------------------*/ - /* configure external interrupt exti15_10 on PC14( gyro int ) */ - rcc_periph_clock_enable(RCC_GPIOC); - rcc_periph_clock_enable(RCC_AFIO); - gpio_set_mode(GPIOC, GPIO_MODE_INPUT, - GPIO_CNF_INPUT_FLOAT, GPIO14); - -#ifdef ASPIRIN_USE_GYRO_INT - exti_select_source(EXTI14, GPIOC); - exti_set_trigger(EXTI14, EXTI_TRIGGER_FALLING); - exti_enable_request(EXTI14); -#endif - - /* configure external interrupt exti2 on PB2( accel int ) */ - rcc_periph_clock_enable(RCC_GPIOB); - gpio_set_mode(GPIOB, GPIO_MODE_INPUT, - GPIO_CNF_INPUT_FLOAT, GPIO2); - exti_select_source(EXTI2, GPIOB); - exti_set_trigger(EXTI2, EXTI_TRIGGER_FALLING); - exti_enable_request(EXTI2); - -} - - -/****** the interrupts should be handled in the peripheral drivers *******/ - -/* - * Gyro data ready - */ -void exti15_10_isr(void) -{ - - /* clear EXTI */ - exti_reset_request(EXTI14); - -#ifdef ASPIRIN_USE_GYRO_INT - imu_aspirin.gyro_eoc = true; - imu_aspirin.status = AspirinStatusReadingGyro; -#endif - -} - -/* - * Accel data ready - */ -void exti2_isr(void) -{ - - /* clear EXTI */ - exti_reset_request(EXTI2); - - //adxl345_start_reading_data(); -} - diff --git a/sw/airborne/arch/stm32/modules/imu/imu_aspirin_arch.h b/sw/airborne/arch/stm32/modules/imu/imu_aspirin_arch.h deleted file mode 100644 index 42722d7e14..0000000000 --- a/sw/airborne/arch/stm32/modules/imu/imu_aspirin_arch.h +++ /dev/null @@ -1,18 +0,0 @@ -#ifndef IMU_ASPIRIN_ARCH_H -#define IMU_ASPIRIN_ARCH_H - -#include "modules/imu/imu.h" -#include - -extern void imu_aspirin_arch_int_enable(void); -extern void imu_aspirin_arch_int_disable(void); - -// gyro eoc -static inline int imu_aspirin_eoc(void) -{ - return (gpio_get(GPIOC, GPIO14) == 0); -} - - - -#endif /* IMU_ASPIRIN_ARCH_H */ diff --git a/sw/airborne/arch/stm32/modules/intermcu/link_mcu_hw.h b/sw/airborne/arch/stm32/modules/intermcu/link_mcu_hw.h deleted file mode 100644 index 122032b0a0..0000000000 --- a/sw/airborne/arch/stm32/modules/intermcu/link_mcu_hw.h +++ /dev/null @@ -1,43 +0,0 @@ -/* $Id: link_mcu_hw.h 2064 2007-11-23 12:35:50Z hecto $ - * - * Copyright (C) 2003-2005 Pascal Brisset, Antoine Drouin - * - * 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. - * - */ - -/** \brief handling of arm7 inter mcu link - * - */ - -#ifndef LINK_MCU_HW_H -#define LINK_MCU_HW_H - -#define CRC_INIT 0x0 -#define CrcLow(x) ((x)&0xff) -#define CrcHigh(x) ((x)>>8) - -static inline uint16_t CrcUpdate(uint16_t crc, uint8_t data) -{ - uint8_t a = ((uint8_t)CrcHigh(crc)) + data; - uint8_t b = ((uint8_t)CrcLow(crc)) + a; - crc = b | a << 8; - return crc; -} - -#endif /* LINK_MCU_HW_H */ diff --git a/sw/airborne/arch/stm32/modules/radio_control/ppm_arch.c b/sw/airborne/arch/stm32/modules/radio_control/ppm_arch.c deleted file mode 100644 index c9b9480383..0000000000 --- a/sw/airborne/arch/stm32/modules/radio_control/ppm_arch.c +++ /dev/null @@ -1,312 +0,0 @@ -/* - * Copyright (C) 2010-2014 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, see - * . - */ - -/** - * @file arch/stm32/modules/radio_control/ppm_arch.c - * @ingroup stm32_arch - * - * STM32 ppm decoder. - * - * Input signal either on: - * - PA1 TIM2/CH2 (uart1 trig on Lisa/L) (Servo 6 on Lisa/M) - * - PA10 TIM1/CH3 (uart1 trig on Lisa/L) (uart1 rx on Lisa/M) - * - */ - -#include "modules/radio_control/radio_control.h" -#include "modules/radio_control/ppm.h" - -#include BOARD_CONFIG - -#include -#include -#include -#include - -#include "mcu_periph/gpio.h" - -// for timer_get_frequency -#include "mcu_arch.h" - -#define ONE_MHZ_CLK 1000000 -#ifdef NVIC_TIM_IRQ_PRIO -#define PPM_IRQ_PRIO NVIC_TIM_IRQ_PRIO -#else -#define PPM_IRQ_PRIO 2 -#endif - - -static uint32_t timer_rollover_cnt; - - -#if USE_PPM_TIM1 - -PRINT_CONFIG_MSG("Using TIM1 for PPM input.") -#define PPM_TIMER TIM1 -#define RCC_TIM_PPM RCC_TIM1 -#define RST_TIM_PPM RST_TIM1 - -#elif USE_PPM_TIM2 - -PRINT_CONFIG_MSG("Using TIM2 for PPM input.") -#define PPM_TIMER TIM2 -#define RCC_TIM_PPM RCC_TIM2 -#define RST_TIM_PPM RST_TIM2 - -#elif USE_PPM_TIM3 - -PRINT_CONFIG_MSG("Using TIM3 for PPM input.") -#define PPM_TIMER TIM3 -#define RCC_TIM_PPM RCC_TIM3 -#define RST_TIM_PPM RST_TIM3 - -#elif USE_PPM_TIM4 - -PRINT_CONFIG_MSG("Using TIM4 for PPM input.") -#define PPM_TIMER TIM4 -#define RCC_TIM_PPM RCC_TIM4 -#define RST_TIM_PPM RST_TIM4 - -#elif USE_PPM_TIM5 - -PRINT_CONFIG_MSG("Using TIM5 for PPM input.") -#define PPM_TIMER TIM5 -#define RCC_TIM_PPM RCC_TIM5 -#define RST_TIM_PPM RST_TIM5 - -#elif USE_PPM_TIM8 - -PRINT_CONFIG_MSG("Using TIM8 for PPM input.") -#define PPM_TIMER TIM8 -#define RCC_TIM_PPM RCC_TIM8 -#define RST_TIM_PPM RST_TIM8 - -#elif USE_PPM_TIM9 - -PRINT_CONFIG_MSG("Using TIM9 for PPM input.") -#define PPM_TIMER TIM9 -#define RCC_TIM_PPM RCC_TIM9 -#define RST_TIM_PPM RST_TIM9 - -#elif USE_PPM_TIM12 - -PRINT_CONFIG_MSG("Using TIM12 for PPM input.") -#define PPM_TIMER TIM12 -#define RCC_TIM_PPM RCC_TIM12 -#define RST_TIM_PPM RST_TIM12 - -#else -#error Unknown PPM input timer configuration. -#endif - -void ppm_arch_init(void) -{ - /* timer clock enable */ - rcc_periph_clock_enable(RCC_TIM_PPM); - - /* GPIO configuration as input capture for timer */ - gpio_setup_pin_af(PPM_GPIO_PORT, PPM_GPIO_PIN, PPM_GPIO_AF, FALSE); - - /* Time Base configuration */ - rcc_periph_reset_pulse(RST_TIM_PPM); - timer_set_mode(PPM_TIMER, TIM_CR1_CKD_CK_INT, - TIM_CR1_CMS_EDGE, TIM_CR1_DIR_UP); - timer_set_period(PPM_TIMER, 0xFFFF); - uint32_t timer_clk = timer_get_frequency(PPM_TIMER); - timer_set_prescaler(PPM_TIMER, (timer_clk / (RC_PPM_TICKS_PER_USEC * ONE_MHZ_CLK)) - 1); - - /* TIM configuration: Input Capture mode --------------------- - 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); -#elif defined PPM_PULSE_TYPE && PPM_PULSE_TYPE == PPM_PULSE_TYPE_NEGATIVE - timer_ic_set_polarity(PPM_TIMER, PPM_CHANNEL, TIM_IC_FALLING); -#else -#error "Unknown PPM_PULSE_TYPE" -#endif - timer_ic_set_input(PPM_TIMER, PPM_CHANNEL, PPM_TIMER_INPUT); - timer_ic_set_prescaler(PPM_TIMER, PPM_CHANNEL, TIM_IC_PSC_OFF); - timer_ic_set_filter(PPM_TIMER, PPM_CHANNEL, TIM_IC_OFF); - - /* Enable timer Interrupt(s). */ - nvic_set_priority(PPM_IRQ, PPM_IRQ_PRIO); - nvic_enable_irq(PPM_IRQ); - -#ifdef PPM_IRQ2 - nvic_set_priority(PPM_IRQ2, PPM_IRQ_PRIO); - nvic_enable_irq(PPM_IRQ2); -#endif - - /* Enable the Capture/Compare and Update interrupt requests. */ - timer_enable_irq(PPM_TIMER, (PPM_CC_IE | TIM_DIER_UIE)); - - /* Enable capture channel. */ - timer_ic_enable(PPM_TIMER, PPM_CHANNEL); - - /* TIM enable counter */ - timer_enable_counter(PPM_TIMER); - - timer_rollover_cnt = 0; -} - -#if USE_PPM_TIM2 - -void tim2_isr(void) -{ - if ((TIM2_SR & PPM_CC_IF) != 0) { - timer_clear_flag(TIM2, PPM_CC_IF); - - uint32_t now = timer_get_counter(TIM2) + timer_rollover_cnt; - ppm_decode_frame(now); - } else if ((TIM2_SR & TIM_SR_UIF) != 0) { - timer_rollover_cnt += (1 << 16); - timer_clear_flag(TIM2, TIM_SR_UIF); - } -} - - -#elif USE_PPM_TIM3 - -void tim3_isr(void) -{ - if ((TIM3_SR & PPM_CC_IF) != 0) { - timer_clear_flag(TIM3, PPM_CC_IF); - - uint32_t now = timer_get_counter(TIM3) + timer_rollover_cnt; - ppm_decode_frame(now); - } else if ((TIM3_SR & TIM_SR_UIF) != 0) { - timer_rollover_cnt += (1 << 16); - timer_clear_flag(TIM3, TIM_SR_UIF); - } -} - -#elif USE_PPM_TIM4 - -void tim4_isr(void) -{ - if ((TIM4_SR & PPM_CC_IF) != 0) { - timer_clear_flag(TIM4, PPM_CC_IF); - - uint32_t now = timer_get_counter(TIM4) + timer_rollover_cnt; - ppm_decode_frame(now); - } else if ((TIM4_SR & TIM_SR_UIF) != 0) { - timer_rollover_cnt += (1 << 16); - timer_clear_flag(TIM4, TIM_SR_UIF); - } -} - -#elif USE_PPM_TIM5 - -void tim5_isr(void) -{ - if ((TIM5_SR & PPM_CC_IF) != 0) { - timer_clear_flag(TIM5, PPM_CC_IF); - - uint32_t now = timer_get_counter(TIM5) + timer_rollover_cnt; - ppm_decode_frame(now); - } else if ((TIM5_SR & TIM_SR_UIF) != 0) { - timer_rollover_cnt += (1 << 16); - timer_clear_flag(TIM5, TIM_SR_UIF); - } -} - - -#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); - } -} - -void tim1_cc_isr(void) { - if ((TIM1_SR & PPM_CC_IF) != 0) { - timer_clear_flag(TIM1, PPM_CC_IF); - - uint32_t now = timer_get_counter(TIM1) + timer_rollover_cnt; - ppm_decode_frame(now); - } -} - -#elif USE_PPM_TIM8 - -#if defined(STM32F1) -void tim8_up_isr(void) { -#elif defined(STM32F4) -void tim8_up_tim13_isr(void) { -#endif - if ((TIM8_SR & TIM_SR_UIF) != 0) { - timer_rollover_cnt += (1 << 16); - timer_clear_flag(TIM8, TIM_SR_UIF); - } -} - -void tim8_cc_isr(void) { - if ((TIM8_SR & PPM_CC_IF) != 0) { - timer_clear_flag(TIM8, PPM_CC_IF); - - uint32_t now = timer_get_counter(TIM8) + timer_rollover_cnt; - ppm_decode_frame(now); - } -} - -#elif USE_PPM_TIM9 && defined(STM32F4) - -void tim1_brk_tim9_isr(void) -{ - if ((TIM9_SR & PPM_CC_IF) != 0) { - timer_clear_flag(TIM9, PPM_CC_IF); - - uint32_t now = timer_get_counter(TIM9) + timer_rollover_cnt; - ppm_decode_frame(now); - - } else if ((TIM9_SR & TIM_SR_UIF) != 0) { - timer_rollover_cnt += (1 << 16); - timer_clear_flag(TIM9, TIM_SR_UIF); - } -} - -#elif USE_PPM_TIM12 && defined(STM32F4) - -void tim8_brk_tim12_isr(void) -{ - if ((TIM12_SR & PPM_CC_IF) != 0) { - timer_clear_flag(TIM12, PPM_CC_IF); - - uint32_t now = timer_get_counter(TIM12) + timer_rollover_cnt; - ppm_decode_frame(now); - - } else if ((TIM12_SR & TIM_SR_UIF) != 0) { - timer_rollover_cnt += (1 << 16); - timer_clear_flag(TIM12, TIM_SR_UIF); - } -} - - - -#endif /* USE_PPM_TIM1 */ - diff --git a/sw/airborne/arch/stm32/modules/radio_control/ppm_arch.h b/sw/airborne/arch/stm32/modules/radio_control/ppm_arch.h deleted file mode 100644 index 507d2cc124..0000000000 --- a/sw/airborne/arch/stm32/modules/radio_control/ppm_arch.h +++ /dev/null @@ -1,49 +0,0 @@ -/* - * Copyright (C) 2010-2014 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, see - * . - */ - -/** - * @file arch/stm32/modules/radio_control/ppm_arch.h - * @ingroup stm32_arch - * - * STM32 ppm decoder. - * - */ - -#ifndef PPM_ARCH_H -#define PPM_ARCH_H - -/** - * The ppm counter is set-up to have 1/6 us resolution. - * - * The timer clock frequency (before prescaling): - * STM32F1: - * TIM1 -> APB2 = HCLK = 72MHz - * TIM2 -> 2 * APB1 = 2 * 36MHz = 72MHz - * STM32F4: - * TIM1 -> 2 * APB2 = 2 * 84MHz = 168MHz - * TIM2 -> 2 * APB1 = 2 * 42MHz = 84MHz - */ -#define RC_PPM_TICKS_PER_USEC 6 - -#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) - -#endif /* PPM_ARCH_H */ diff --git a/sw/airborne/arch/stm32/my_debug_servo.h b/sw/airborne/arch/stm32/my_debug_servo.h deleted file mode 100644 index 19f76db83c..0000000000 --- a/sw/airborne/arch/stm32/my_debug_servo.h +++ /dev/null @@ -1,63 +0,0 @@ -#ifndef MY_DEBUG_SERVO_H -#define MY_DEBUG_SERVO_H - -#include -#include - -/* using servo 2 connector as debug */ - -#define DEBUG_S1_TOGGLE() { GPIOC_ODR ^= GPIO6; } -#define DEBUG_S1_ON() { GPIOC_BSRR = GPIO6; } -#define DEBUG_S1_OFF() { GPIOC_BRR = GPIO6; } - -#define DEBUG_S2_TOGGLE() { GPIOC_ODR ^= GPIO7; } -#define DEBUG_S2_ON() { GPIOC_BSRR = GPIO7; } -#define DEBUG_S2_OFF() { GPIOC_BRR = GPIO7; } - -#define DEBUG_S3_TOGGLE() { GPIOC_ODR ^= GPIO8; } -#define DEBUG_S3_ON() { GPIOC_BSRR = GPIO8; } -#define DEBUG_S3_OFF() { GPIOC_BRR = GPIO8; } - -#define DEBUG_S4_TOGGLE() { GPIOC_ODR ^= GPIO9; } -#define DEBUG_S4_ON() { GPIOC_BSRR = GPIO9; } -#define DEBUG_S4_OFF() { GPIOC_BRR = GPIO9; } - -#define DEBUG_S5_TOGGLE() { GPIOB_ODR ^= GPIO8; } -#define DEBUG_S5_ON() { GPIOB_BSRR = GPIO8; } -#define DEBUG_S5_OFF() { GPIOB_BRR = GPIO8; } - -#define DEBUG_S6_TOGGLE() { GPIOB_ODR ^= GPIO9; } -#define DEBUG_S6_ON() { GPIOB_BSRR = GPIO9; } -#define DEBUG_S6_OFF() { GPIOB_BRR = GPIO9; } - - - -#define DEBUG_SERVO1_INIT() { \ - /* S1: PC6 S2: PC7 S3: PC8 */ \ - GPIOC_BSRR = GPIO6 | GPIO7 | GPIO8 ; \ - rcc_periph_clock_enable(RCC_GPIOC); \ - gpio_set_mode(GPIOC, GPIO_MODE_OUTPUT_50_MHZ, \ - GPIO_CNF_OUTPUT_PUSHPULL, GPIO6 | GPIO7 | GPIO8); \ - DEBUG_S1_OFF(); \ - DEBUG_S2_OFF(); \ - DEBUG_S3_OFF(); \ - } - -#define DEBUG_SERVO2_INIT() { \ - /* S4: PC9 */ \ - GPIOC_BSRR = GPIO9; \ - rcc_periph_clock_enable(RCC_GPIOC); \ - gpio_set_mode(GPIOC, GPIO_MODE_OUTPUT_50_MHZ, \ - GPIO_CNF_OUTPUT_PUSHPULL, GPIO9); \ - /* S5: PB8 and S6: PB9 */ \ - GPIOB_BSRR = GPIO8 | GPIO9; \ - rcc_periph_clock_enable(RCC_GPIOB); \ - gpio_set_mode(GPIOB, GPIO_MODE_OUTPUT_50_MHZ, \ - GPIO_CNF_OUTPUT_PUSHPULL, GPIO8 | GPIO9); \ - DEBUG_S4_OFF(); \ - DEBUG_S5_OFF(); \ - DEBUG_S6_OFF(); \ - } - - -#endif /* MY_DEBUG_SERVO_H */ diff --git a/sw/airborne/arch/stm32/navstik.ld b/sw/airborne/arch/stm32/navstik.ld deleted file mode 100644 index cd9c9c99bb..0000000000 --- a/sw/airborne/arch/stm32/navstik.ld +++ /dev/null @@ -1,36 +0,0 @@ -/* - * Hey Emacs, this is a -*- makefile -*- - * - * Copyright (C) 2014 Freek van Tienen - * - * 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 Navstik (STM32F415, 1024K flash, 192K RAM). */ - -/* Define memory regions. */ -MEMORY -{ - /* only 128K (SRAM1 and SRAM2) are accessible by all AHB masters. */ - ram (rwx) : ORIGIN = 0x20000000, LENGTH = 128K - /* Reserving 128kb flash for persistent settings. */ - rom (rx) : ORIGIN = 0x08000000, LENGTH = 896K -} - -/* Include the common ld script. */ -INCLUDE cortex-m-generic.ld diff --git a/sw/airborne/arch/stm32/peripherals/hmc5843_arch.c b/sw/airborne/arch/stm32/peripherals/hmc5843_arch.c deleted file mode 100644 index 3332e476de..0000000000 --- a/sw/airborne/arch/stm32/peripherals/hmc5843_arch.c +++ /dev/null @@ -1,62 +0,0 @@ -/* - * Copyright (C) 2010 Antoine Drouin - * - * 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. - */ -#include "peripherals/hmc5843.h" - -#include -#include -#include -#include -#include - -#include "mcu_periph/i2c.h" - -#ifndef STM32F1 -#error "HMC5843 arch currently only implemented for STM32F1" -#endif - -void hmc5843_arch_init(void) -{ - /* configure external interrupt exti5 on PB5( mag int ) */ - rcc_periph_clock_enable(RCC_GPIOB); - rcc_periph_clock_enable(RCC_AFIO); - gpio_set_mode(GPIOB, GPIO_MODE_INPUT, - GPIO_CNF_INPUT_FLOAT, GPIO5); - -#ifdef HMC5843_USE_INT - exti_select_source(EXTI5, GPIOB); - exti_set_trigger(EXTI5, EXTI_TRIGGER_FALLING); - exti_enable_request(EXTI5); - - nvic_set_priority(NVIC_EXTI9_5_IRQ, 0x0f); - nvic_enable_irq(NVIC_EXTI9_5_IRQ); -#endif -} - -void hmc5843_arch_reset(void) -{ - i2c2_er_irq_handler(); -} - -void exti9_5_isr(void) -{ - - exti_reset_request(EXTI5); -} diff --git a/sw/airborne/arch/stm32/peripherals/hmc5843_arch.h b/sw/airborne/arch/stm32/peripherals/hmc5843_arch.h deleted file mode 100644 index ba3f9611b1..0000000000 --- a/sw/airborne/arch/stm32/peripherals/hmc5843_arch.h +++ /dev/null @@ -1,33 +0,0 @@ -/* - * Copyright (C) 2010 Antoine Drouin - * - * 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. - */ - -#ifndef HMC5843_ARCH_H -#define HMC5843_ARCH_H - -#include - -/* returns true if conversion done */ -static inline int mag_eoc(void) -{ - return (gpio_get(GPIOB, GPIO5) != 0); -} - -#endif /* HMC5843_ARCH_H */ diff --git a/sw/airborne/arch/stm32/peripherals/max1168_arch.c b/sw/airborne/arch/stm32/peripherals/max1168_arch.c deleted file mode 100644 index d77019ce0c..0000000000 --- a/sw/airborne/arch/stm32/peripherals/max1168_arch.c +++ /dev/null @@ -1,60 +0,0 @@ -/* - * Copyright (C) 2010 Antoine Drouin - * - * 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. - */ -#include "peripherals/max1168.h" - -#include "libopencm3/stm32/rcc.h" -#include "libopencm3/stm32/gpio.h" -#include "libopencm3/stm32/exti.h" -#include - -#ifndef STM32F1 -#error "HMC5843 arch currently only implemented for STM32F1" -#endif - -void max1168_arch_init(void) -{ - - /* configure external interrupt exti2 on PD2( data ready ) v1.0*/ - /* PB2( data ready ) v1.1*/ - rcc_periph_clock_enable(RCC_GPIOB); - rcc_periph_clock_enable(RCC_AFIO); - gpio_set_mode(GPIOB, GPIO_MODE_INPUT, - GPIO_CNF_INPUT_FLOAT, GPIO2); - - exti_select_source(EXTI2, GPIOB); - exti_set_trigger(EXTI2, EXTI_TRIGGER_FALLING); - exti_enable_request(EXTI2); - - nvic_set_priority(NVIC_EXTI2_IRQ, 0xF); - nvic_enable_irq(NVIC_EXTI2_IRQ); - -} - -void exti2_isr(void) -{ - - /* clear EXTI */ - exti_reset_request(EXTI2); - - max1168_status = MAX1168_GOT_EOC; - -} - diff --git a/sw/airborne/arch/stm32/peripherals/max1168_arch.h b/sw/airborne/arch/stm32/peripherals/max1168_arch.h deleted file mode 100644 index 9d78937078..0000000000 --- a/sw/airborne/arch/stm32/peripherals/max1168_arch.h +++ /dev/null @@ -1,26 +0,0 @@ -/* - * Copyright (C) 2010 Antoine Drouin - * - * 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. - */ - -#ifndef MAX1168_ARCH_H -#define MAX1168_ARCH_H - - -#endif /* MAX1168_ARCH_H */ diff --git a/sw/airborne/arch/stm32/peripherals/ms2100_arch.c b/sw/airborne/arch/stm32/peripherals/ms2100_arch.c deleted file mode 100644 index 16f1293009..0000000000 --- a/sw/airborne/arch/stm32/peripherals/ms2100_arch.c +++ /dev/null @@ -1,85 +0,0 @@ -/* - * Copyright (C) 2010 Antoine Drouin - * - * 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 arch/stm32/peripherals/ms2100_arch.c - * - * STM32 specific functions for the ms2100 magnetic sensor from PNI. - */ - -#include "peripherals/ms2100.h" -#include "mcu_periph/sys_time.h" - -#include -#include -#include -#include - -#ifndef STM32F1 -#error "MS2100 arch currently only implemented for STM32F1" -#endif - -void ms2100_arch_init(void) -{ - - /* set mag reset as output (reset on PC13) ----*/ - rcc_periph_clock_enable(RCC_GPIOC); - rcc_periph_clock_enable(RCC_AFIO); - gpio_set(GPIOC, GPIO13); - gpio_set_mode(GPIOC, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_PUSHPULL, GPIO13); - Ms2100Reset(); - - /* configure data ready input on PB5 */ - rcc_periph_clock_enable(RCC_GPIOB); - gpio_set_mode(GPIOB, GPIO_MODE_INPUT, GPIO_CNF_INPUT_FLOAT, GPIO5); - - /* external interrupt for drdy pin */ - exti_select_source(EXTI5, GPIOB); - exti_set_trigger(EXTI5, EXTI_TRIGGER_RISING); - exti_enable_request(EXTI5); - - nvic_set_priority(NVIC_EXTI9_5_IRQ, 0x0f); - nvic_enable_irq(NVIC_EXTI9_5_IRQ); -} - -void ms2100_reset_cb(struct spi_transaction *t __attribute__((unused))) -{ - // set RESET pin high for at least 100 nsec - // busy wait should not harm - Ms2100Set(); - - // FIXME, make nanosleep funcion - uint32_t dt_ticks = cpu_ticks_of_nsec(110); - int32_t end_cpu_ticks = systick_get_value() - dt_ticks; - if (end_cpu_ticks < 0) { - end_cpu_ticks += systick_get_reload(); - } - while (systick_get_value() > (uint32_t)end_cpu_ticks) - ; - - Ms2100Reset(); -} - -void exti9_5_isr(void) -{ - ms2100.status = MS2100_GOT_EOC; - exti_reset_request(EXTI5); -} diff --git a/sw/airborne/arch/stm32/peripherals/ms2100_arch.h b/sw/airborne/arch/stm32/peripherals/ms2100_arch.h deleted file mode 100644 index 0037b3c97b..0000000000 --- a/sw/airborne/arch/stm32/peripherals/ms2100_arch.h +++ /dev/null @@ -1,59 +0,0 @@ -/* - * Copyright (C) 2010 Antoine Drouin - * - * 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 arch/stm32/peripherals/ms2100_arch.h - * - * STM32 specific functions for the ms2100 magnetic sensor from PNI. - */ - -#ifndef MS2100_ARCH_H -#define MS2100_ARCH_H - -#include -#include "mcu_periph/spi.h" - -/** - * Here Reset indicates the Ms2100 is in normal state, i.e. - * the reset line is driven low (i.e. the GPIO is "reset") - */ -static inline void Ms2100Reset(void) -{ - GPIOC_BRR = GPIO13; -} - -/** - * Here Set indicates the Ms2100 is in reset state, i.e. - * the reset line is driven high (i.e. the GPIO is "set") - */ -static inline void Ms2100Set(void) -{ - GPIOC_BSRR = GPIO13; -} - -#define Ms2100HasEOC() (gpio_get(GPIOB, GPIO5) != 0) - -/** Reset callback. - * called before spi transaction and after slave select - */ -extern void ms2100_reset_cb(struct spi_transaction *t); - -#endif /* MS2100_ARCH_H */ diff --git a/sw/airborne/arch/stm32/peripherals/sc18is600_arch.c b/sw/airborne/arch/stm32/peripherals/sc18is600_arch.c deleted file mode 100644 index f19c07fd73..0000000000 --- a/sw/airborne/arch/stm32/peripherals/sc18is600_arch.c +++ /dev/null @@ -1,290 +0,0 @@ -#include "peripherals/sc18is600.h" - -#include -#include -#include -#include -#include -#include - -/* commands definition */ -#define Sc18Is600_Cmd_Write 0x00 -#define Sc18Is600_Cmd_Read 0x01 -#define Sc18Is600_Cmd_Read_After_Write 0x02 -#define Sc18Is600_Cmd_Write_After_Write 0x03 -#define Sc18Is600_Cmd_Read_Buffer 0x06 -#define Sc18Is600_Cmd_Write_To_Reg 0x20 -#define Sc18Is600_Cmd_Read_From_Reg 0x21 -#define Sc18Is600_Cmd_Power_Down 0x30 - -extern void exti2_irq_handler(void); -extern void dma1_c4_irq_handler(void); - -static inline void sc18is600_setup_SPI_DMA(uint8_t _len); - -void sc18is600_arch_init(void) -{ - - /* set slave select as output and assert it ( on PB12) */ - Sc18Is600Unselect(); - RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); - GPIO_InitTypeDef GPIO_InitStructure; - GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12; - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; - GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; - GPIO_Init(GPIOB, &GPIO_InitStructure); - - /* configure external interrupt exti2 on PD2( data ready ) */ - RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOD | RCC_APB2Periph_AFIO, ENABLE); - GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2; - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU; - GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; - GPIO_Init(GPIOD, &GPIO_InitStructure); - - EXTI_InitTypeDef EXTI_InitStructure; - GPIO_EXTILineConfig(GPIO_PortSourceGPIOD, GPIO_PinSource2); - EXTI_InitStructure.EXTI_Line = EXTI_Line2; - EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt; - EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Falling; - EXTI_InitStructure.EXTI_LineCmd = ENABLE; - EXTI_Init(&EXTI_InitStructure); - - NVIC_InitTypeDef NVIC_InitStructure; - NVIC_InitStructure.NVIC_IRQChannel = EXTI2_IRQn; - NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x0F; - NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x0F; - NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; - - NVIC_Init(&NVIC_InitStructure); - - /* Enable DMA1 channel4 IRQ Channel */ - NVIC_InitTypeDef NVIC_init_struct = { - .NVIC_IRQChannel = DMA1_Channel4_IRQn, - .NVIC_IRQChannelPreemptionPriority = 0, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE - }; - NVIC_Init(&NVIC_init_struct); - /* Enable SPI2 Periph clock -------------------------------------------------*/ - RCC_APB1PeriphClockCmd(RCC_APB1Periph_SPI2, ENABLE); - - /* Configure GPIOs: SCK, MISO and MOSI --------------------------------*/ - GPIO_InitStructure.GPIO_Pin = GPIO_Pin_13 | GPIO_Pin_14 | GPIO_Pin_15; - GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; - GPIO_Init(GPIOB, &GPIO_InitStructure); - - RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO , ENABLE); - - - /* configure SPI */ - SPI_InitTypeDef SPI_InitStructure; - SPI_InitStructure.SPI_Direction = SPI_Direction_2Lines_FullDuplex; - SPI_InitStructure.SPI_Mode = SPI_Mode_Master; - SPI_InitStructure.SPI_DataSize = SPI_DataSize_8b; - SPI_InitStructure.SPI_CPOL = SPI_CPOL_High; - SPI_InitStructure.SPI_CPHA = SPI_CPHA_2Edge; - SPI_InitStructure.SPI_NSS = SPI_NSS_Soft; - SPI_InitStructure.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_256; - SPI_InitStructure.SPI_FirstBit = SPI_FirstBit_MSB; - SPI_InitStructure.SPI_CRCPolynomial = 7; - SPI_Init(SPI2, &SPI_InitStructure); - - /* Enable SPI */ - SPI_Cmd(SPI2, ENABLE); - - /* Enable SPI_2 DMA clock ---------------------------------------------------*/ - RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE); - -} - -static inline void sc18is600_setup_SPI_DMA(uint8_t _len) -{ - /* SPI2_Rx_DMA_Channel configuration ------------------------------------*/ - DMA_DeInit(DMA1_Channel4); - DMA_InitTypeDef DMA_initStructure_4 = { - .DMA_PeripheralBaseAddr = (uint32_t)(SPI2_BASE + 0x0C), - .DMA_MemoryBaseAddr = (uint32_t)sc18is600.priv_rx_buf, - .DMA_DIR = DMA_DIR_PeripheralSRC, - .DMA_BufferSize = _len, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_VeryHigh, - .DMA_M2M = DMA_M2M_Disable - }; - DMA_Init(DMA1_Channel4, &DMA_initStructure_4); - /* SPI2_Tx_DMA_Channel configuration ------------------------------------*/ - DMA_DeInit(DMA1_Channel5); - DMA_InitTypeDef DMA_initStructure_5 = { - .DMA_PeripheralBaseAddr = (uint32_t)(SPI2_BASE + 0x0C), - .DMA_MemoryBaseAddr = (uint32_t)sc18is600.priv_tx_buf, - .DMA_DIR = DMA_DIR_PeripheralDST, - .DMA_BufferSize = _len, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_Medium, - .DMA_M2M = DMA_M2M_Disable - }; - DMA_Init(DMA1_Channel5, &DMA_initStructure_5); - - /* Enable SPI_2 Rx request */ - SPI_I2S_DMACmd(SPI2, SPI_I2S_DMAReq_Rx, ENABLE); - /* Enable DMA1 Channel4 */ - DMA_Cmd(DMA1_Channel4, ENABLE); - - /* Enable SPI_2 Tx request */ - SPI_I2S_DMACmd(SPI2, SPI_I2S_DMAReq_Tx, ENABLE); - /* Enable DMA1 Channel5 */ - DMA_Cmd(DMA1_Channel5, ENABLE); - - /* Enable DMA1 Channel4 Transfer Complete interrupt */ - DMA_ITConfig(DMA1_Channel4, DMA_IT_TC, ENABLE); -} - - -void sc18is600_transmit(uint8_t addr, uint8_t len) -{ - - sc18is600.transaction = Sc18Is600Transmit; - sc18is600.status = Sc18Is600SendingRequest; - sc18is600.priv_tx_buf[0] = Sc18Is600_Cmd_Write; // write command - sc18is600.priv_tx_buf[1] = len; - sc18is600.priv_tx_buf[2] = addr; - Sc18Is600Select(); - sc18is600_setup_SPI_DMA(len + 3); - -} - -void sc18is600_receive(uint8_t addr, uint8_t len) -{ - -} - -void sc18is600_tranceive(uint8_t addr, uint8_t len_tx, uint8_t len_rx) -{ - sc18is600.transaction = Sc18Is600Transcieve; - sc18is600.status = Sc18Is600SendingRequest; - sc18is600.rx_len = len_rx; - sc18is600.priv_tx_buf[0] = Sc18Is600_Cmd_Read_After_Write; // read after write command - sc18is600.priv_tx_buf[1] = len_tx; - sc18is600.priv_tx_buf[2] = len_rx; - sc18is600.priv_tx_buf[3] = addr; - sc18is600.priv_tx_buf[4 + len_tx] = addr; - Sc18Is600Select(); - sc18is600_setup_SPI_DMA(len_tx + 5); -} - -void sc18is600_write_to_register(uint8_t addr, uint8_t value) -{ - sc18is600.transaction = Sc18Is600WriteRegister; - sc18is600.status = Sc18Is600SendingRequest; - sc18is600.priv_tx_buf[0] = Sc18Is600_Cmd_Write_To_Reg; // write to register - sc18is600.priv_tx_buf[1] = addr; - sc18is600.priv_tx_buf[2] = value; - Sc18Is600Select(); - sc18is600_setup_SPI_DMA(3); -} - - -void sc18is600_read_from_register(uint8_t addr) -{ - sc18is600.transaction = Sc18Is600ReadRegister; - sc18is600.status = Sc18Is600SendingRequest; - sc18is600.priv_tx_buf[0] = Sc18Is600_Cmd_Read_From_Reg; // read from register - sc18is600.priv_tx_buf[1] = addr; - sc18is600.priv_tx_buf[2] = 0; - Sc18Is600Select(); - sc18is600_setup_SPI_DMA(3); -} - -#define ReadI2CStatReg() { \ - sc18is600.priv_tx_buf[0] = Sc18Is600_Cmd_Read_From_Reg; \ - sc18is600.priv_tx_buf[1] = Sc18Is600_I2CStat; \ - sc18is600.priv_tx_buf[2] = 0; \ - Sc18Is600Select(); \ - sc18is600_setup_SPI_DMA(3); \ - } - - -void exti2_irq_handler(void) -{ - /* clear EXTI */ - if (EXTI_GetITStatus(EXTI_Line2) != RESET) { - EXTI_ClearITPendingBit(EXTI_Line2); - } - switch (sc18is600.transaction) { - case Sc18Is600Receive: - case Sc18Is600Transmit: - case Sc18Is600Transcieve: - if (sc18is600.status == Sc18Is600WaitingForI2C) { - sc18is600.status = Sc18Is600ReadingI2CStat; - ReadI2CStatReg(); - } - break; - case Sc18Is600ReadRegister: - case Sc18Is600WriteRegister: - // should not happen - break; - default: - break; - } - -} - - - -void dma1_c4_irq_handler(void) -{ - - DMA_ITConfig(DMA1_Channel4, DMA_IT_TC, DISABLE); - /* Disable SPI_2 Rx and TX request */ - SPI_I2S_DMACmd(SPI2, SPI_I2S_DMAReq_Rx, DISABLE); - SPI_I2S_DMACmd(SPI2, SPI_I2S_DMAReq_Tx, DISABLE); - /* Disable DMA1 Channel4 and 5 */ - DMA_Cmd(DMA1_Channel4, DISABLE); - DMA_Cmd(DMA1_Channel5, DISABLE); - - switch (sc18is600.transaction) { - case Sc18Is600ReadRegister: - case Sc18Is600WriteRegister: - sc18is600.status = Sc18Is600TransactionComplete; - Sc18Is600Unselect(); - break; - case Sc18Is600Transmit: - if (sc18is600.status == Sc18Is600SendingRequest) { - sc18is600.status = Sc18Is600WaitingForI2C; - Sc18Is600Unselect(); - } else if (sc18is600.status == Sc18Is600ReadingI2CStat) { - sc18is600.i2c_status = sc18is600.priv_rx_buf[2]; - sc18is600.status = Sc18Is600TransactionComplete; - Sc18Is600Unselect(); - } - break; - case Sc18Is600Receive: - case Sc18Is600Transcieve: - if (sc18is600.status == Sc18Is600SendingRequest) { - sc18is600.status = Sc18Is600WaitingForI2C; - Sc18Is600Unselect(); - } else if (sc18is600.status == Sc18Is600ReadingI2CStat) { - sc18is600.status = Sc18Is600ReadingBuffer; - sc18is600.priv_tx_buf[0] = Sc18Is600_Cmd_Read_Buffer; - // debug - for (int i = 1; i < sc18is600.rx_len + 1; i++) { sc18is600.priv_tx_buf[i] = 0; } - Sc18Is600Select(); - sc18is600_setup_SPI_DMA(sc18is600.rx_len + 1); - } else if (sc18is600.status == Sc18Is600ReadingBuffer) { - sc18is600.status = Sc18Is600TransactionComplete; - Sc18Is600Unselect(); - } - break; - default: - break; - } - -} diff --git a/sw/airborne/arch/stm32/peripherals/sc18is600_arch.h b/sw/airborne/arch/stm32/peripherals/sc18is600_arch.h deleted file mode 100644 index 327b33b143..0000000000 --- a/sw/airborne/arch/stm32/peripherals/sc18is600_arch.h +++ /dev/null @@ -1,9 +0,0 @@ -#ifndef SC18IS600_ARCH_H -#define SC18IS600_ARCH_H - -#define Sc18Is600Unselect() GPIOB->BSRR = GPIO_Pin_12 -#define Sc18Is600Select() GPIOB->BRR = GPIO_Pin_12 - - - -#endif /* SC18IS600_ARCH_H */ diff --git a/sw/airborne/arch/stm32/stm32default.ld b/sw/airborne/arch/stm32/stm32default.ld deleted file mode 100644 index 0abd0338cd..0000000000 --- a/sw/airborne/arch/stm32/stm32default.ld +++ /dev/null @@ -1,37 +0,0 @@ -/* - * Hey Emacs, this is a -*- makefile -*- - * - * Copyright (C) 2012 Piotr Esden-Tempski - * - * 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 "default" stm32 architecture */ -/* Assuming the least mean denominator to be 128K flash and 20K ram. */ - -/* Define memory regions. */ -MEMORY -{ - ram (rwx) : ORIGIN = 0x20000000, LENGTH = 64K - /* Leaving 1k of space at the end of rom for stored settings */ - rom (rx) : ORIGIN = 0x08000000, LENGTH = 127K -} - -/* Include the common ld script. */ -INCLUDE cortex-m-generic.ld - diff --git a/sw/airborne/arch/stm32/test_bswap.c b/sw/airborne/arch/stm32/test_bswap.c deleted file mode 100644 index 63aecfefab..0000000000 --- a/sw/airborne/arch/stm32/test_bswap.c +++ /dev/null @@ -1,57 +0,0 @@ - -#include -#include "mcu.h" - - -#define MyByteSwap16(n) \ - ( ((((uint16_t) n) << 8) & 0xFF00) | \ - ((((uint16_t) n) >> 8) & 0x00FF) ) - -#define MyByteSwap32(n) \ - ( ((((uint32_t) n) << 24) & 0xFF000000) | \ - ((((uint32_t) n) << 8) & 0x00FF0000) | \ - ((((uint32_t) n) >> 8) & 0x0000FF00) | \ - ((((uint32_t) n) >> 24) & 0x000000FF) ) - -#define MyByteSwap32_1(in, out) \ - asm volatile ( \ - "rev %0, %1\n\t" \ - : "=r" (out) \ - : "r"(in) \ - ); - - -#define MyByteSwap16_1(in, out) \ - asm volatile ( \ - "rev16 %0, %1\n\t" \ - : "=r" (out) \ - : "r"(in) \ - ); - - - -int main(void) -{ - - // uint32_t foo = 0; - // uint32_t bar = __builtin_bswap32(*(uint32_t*)&foo); - - // uint16_t foo = 12345; - // uint16_t bar = foo >> 8 | foo << 8; - - // uint16_t foo = 12345; - // uint16_t bar = ByteSwap(foo); - - uint16_t foo = 12345; - uint16_t bar = MyByteSwap16(foo); - MyByteSwap16_1(foo, bar); - bar = __REV16(foo); - - - - uint32_t a = 23456; - uint32_t b = MyByteSwap32(a); - MyByteSwap32_1(a, b); - - return 0; -} diff --git a/sw/airborne/arch/stm32/uart_tunnel.c b/sw/airborne/arch/stm32/uart_tunnel.c deleted file mode 100644 index d40a199d31..0000000000 --- a/sw/airborne/arch/stm32/uart_tunnel.c +++ /dev/null @@ -1,125 +0,0 @@ -/* - * Copyright (C) 2010 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. - */ - -#include BOARD_CONFIG -#include "mcu.h" -#include "mcu_periph/sys_time.h" -#include "mcu_periph/gpio.h" -#include "led.h" - -/* UART1 */ -#define A_PORT GPIOA -#define A_RX_PIN GPIO10 -#define A_RX_PORT A_PORT -#define A_TX_PIN GPIO9 -#define A_TX_PORT A_PORT - -/* UART2 */ -#define B_PORT GPIOA -#define B_RX_PIN GPIO3 -#define B_RX_PORT B_PORT -#define B_TX_PIN GPIO2 -#define B_TX_PORT B_PORT - -static inline void main_periodic(void); -static inline void main_event(void); -void Delay(volatile uint32_t nCount); - -void Delay(volatile uint32_t nCount) -{ - for (; nCount != 0; nCount--); -} - -int main(void) -{ - - mcu_init(); - sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL); - - /* Init GPIO for rx pins */ - gpio_setup_input_pullup(A_RX_PORT, A_RX_PIN); - gpio_setup_input_pullup(B_RX_PORT, B_RX_PIN); - - /* Init GPIO for tx pins */ - gpio_setup_output(A_TX_PORT, A_TX_PIN); - gpio_setup_output(B_TX_PORT, B_TX_PIN); - - gpio_clear(A_TX_PORT, A_TX_PIN); - - /* */ - while (1) { - if (sys_time_check_and_ack_timer(0)) { - main_periodic(); - } - main_event(); - } - - return 0; -} - - - -static inline void main_periodic(void) -{ - LED_PERIODIC(); -} - -static inline void main_event(void) -{ - // Delay(2000); - static uint8_t foo = 0; - foo++; - -#if 0 - if (!(foo % 2)) { - gpio_set(B_TX_PORT, B_TX_PIN); - } else { - gpio_clear(B_TX_PORT, B_TX_PIN); - } -#endif - -#if 0 - if (!(foo % 2)) { - gpio_clear(A_TX_PORT, A_TX_PIN); - } else { - 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_set(A_TX_PORT, A_TX_PIN); - } else { - gpio_clear(A_TX_PORT, A_TX_PIN); - } -#endif - /* passthrough A_RX to B_TX */ - if (gpio_get(A_RX_PORT, A_RX_PIN)) { - gpio_set(B_TX_PORT, B_TX_PIN); - LED_ON(2); - } else { - gpio_clear(B_TX_PORT, B_TX_PIN); - LED_OFF(2); - } - - -} diff --git a/sw/airborne/arch/stm32/usb_ser_hw.c b/sw/airborne/arch/stm32/usb_ser_hw.c deleted file mode 100644 index 0559fac015..0000000000 --- a/sw/airborne/arch/stm32/usb_ser_hw.c +++ /dev/null @@ -1,610 +0,0 @@ -/* - * Copyright (C) 2014 Michal Podhradsky, - * based on example from libopencm3 - * - * 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 arch/stm32/usb_ser_hw.c - * CDC USB device driver for STM32 architecture (STM32F1, STM32F4) - */ - -/* This version derived from libopencm3 example */ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "mcu_periph/usb_serial.h" - -#include "mcu_periph/sys_time.h" - -/* Max packet size for USB transfer */ -#define MAX_PACKET_SIZE 64 -/* Max fifo size for storing data */ -#define VCOM_FIFO_SIZE 256 - -#define TX_TIMEOUT_CNT 20 //TODO, make dynamic with event period - -typedef struct { - int head; - int tail; - uint8_t *buf; -} fifo_t; - -static uint8_t txdata[VCOM_FIFO_SIZE]; -static uint8_t rxdata[VCOM_FIFO_SIZE]; - -static fifo_t txfifo; -static fifo_t rxfifo; - -void fifo_init(fifo_t *fifo, uint8_t *buf); -bool fifo_put(fifo_t *fifo, uint8_t c); -bool fifo_get(fifo_t *fifo, uint8_t *pc); -bool fifo_peek(fifo_t *fifo, uint8_t *pc, uint8_t ofs); -int fifo_avail(fifo_t *fifo); -int fifo_free(fifo_t *fifo); -int tx_timeout; // tmp work around for usbd_ep_stall_get from, this function does not always seem to work - -usbd_device *my_usbd_dev; - -static const struct usb_device_descriptor dev = { - .bLength = USB_DT_DEVICE_SIZE, - .bDescriptorType = USB_DT_DEVICE, - .bcdUSB = 0x0200, - .bDeviceClass = USB_CLASS_CDC, - .bDeviceSubClass = 0, - .bDeviceProtocol = 0, - .bMaxPacketSize0 = MAX_PACKET_SIZE, - .idVendor = 0x1d50, // OpenMoko, Inc. - .idProduct = 0x603d, // Paparazzi LPC(STM32)USB Serial - .bcdDevice = 0x0200, - .iManufacturer = 1, - .iProduct = 2, - .iSerialNumber = 3, - .bNumConfigurations = 1, -}; - -/* - * This notification endpoint isn't implemented. According to CDC spec it's - * optional, but its absence causes a NULL pointer dereference in the - * Linux cdc_acm driver. - */ -static const struct usb_endpoint_descriptor comm_endp[] = {{ - .bLength = USB_DT_ENDPOINT_SIZE, - .bDescriptorType = USB_DT_ENDPOINT, - .bEndpointAddress = 0x83, - .bmAttributes = USB_ENDPOINT_ATTR_INTERRUPT, - .wMaxPacketSize = 16, - .bInterval = 255, - } -}; - -static const struct usb_endpoint_descriptor data_endp[] = {{ - .bLength = USB_DT_ENDPOINT_SIZE, - .bDescriptorType = USB_DT_ENDPOINT, - .bEndpointAddress = 0x01, - .bmAttributes = USB_ENDPOINT_ATTR_BULK, - .wMaxPacketSize = MAX_PACKET_SIZE, - .bInterval = 1, - }, { - .bLength = USB_DT_ENDPOINT_SIZE, - .bDescriptorType = USB_DT_ENDPOINT, - .bEndpointAddress = 0x82, - .bmAttributes = USB_ENDPOINT_ATTR_BULK, - .wMaxPacketSize = MAX_PACKET_SIZE, - .bInterval = 1, - } -}; - -static const struct { - struct usb_cdc_header_descriptor header; - struct usb_cdc_call_management_descriptor call_mgmt; - struct usb_cdc_acm_descriptor acm; - struct usb_cdc_union_descriptor cdc_union; -} __attribute__((packed)) cdcacm_functional_descriptors = { - .header = { - .bFunctionLength = sizeof(struct usb_cdc_header_descriptor), - .bDescriptorType = CS_INTERFACE, - .bDescriptorSubtype = USB_CDC_TYPE_HEADER, - .bcdCDC = 0x0110, - }, - .call_mgmt = { - .bFunctionLength = - sizeof(struct usb_cdc_call_management_descriptor), - .bDescriptorType = CS_INTERFACE, - .bDescriptorSubtype = USB_CDC_TYPE_CALL_MANAGEMENT, - .bmCapabilities = 0, - .bDataInterface = 1, - }, - .acm = { - .bFunctionLength = sizeof(struct usb_cdc_acm_descriptor), - .bDescriptorType = CS_INTERFACE, - .bDescriptorSubtype = USB_CDC_TYPE_ACM, - .bmCapabilities = 0, - }, - .cdc_union = { - .bFunctionLength = sizeof(struct usb_cdc_union_descriptor), - .bDescriptorType = CS_INTERFACE, - .bDescriptorSubtype = USB_CDC_TYPE_UNION, - .bControlInterface = 0, - .bSubordinateInterface0 = 1, - } -}; - -static const struct usb_interface_descriptor comm_iface[] = {{ - .bLength = USB_DT_INTERFACE_SIZE, - .bDescriptorType = USB_DT_INTERFACE, - .bInterfaceNumber = 0, - .bAlternateSetting = 0, - .bNumEndpoints = 1, - .bInterfaceClass = USB_CLASS_CDC, - .bInterfaceSubClass = USB_CDC_SUBCLASS_ACM, - .bInterfaceProtocol = USB_CDC_PROTOCOL_AT, - .iInterface = 0, - - .endpoint = comm_endp, - - .extra = &cdcacm_functional_descriptors, - .extralen = sizeof(cdcacm_functional_descriptors) - } -}; - -static const struct usb_interface_descriptor data_iface[] = {{ - .bLength = USB_DT_INTERFACE_SIZE, - .bDescriptorType = USB_DT_INTERFACE, - .bInterfaceNumber = 1, - .bAlternateSetting = 0, - .bNumEndpoints = 2, - .bInterfaceClass = USB_CLASS_DATA, - .bInterfaceSubClass = 0, - .bInterfaceProtocol = 0, - .iInterface = 0, - - .endpoint = data_endp, - } -}; - -static const struct usb_interface ifaces[] = {{ - .num_altsetting = 1, - .altsetting = comm_iface, - }, { - .num_altsetting = 1, - .altsetting = data_iface, - } -}; - -static const struct usb_config_descriptor config = { - .bLength = USB_DT_CONFIGURATION_SIZE, - .bDescriptorType = USB_DT_CONFIGURATION, - .wTotalLength = 0, - .bNumInterfaces = 2, - .bConfigurationValue = 1, - .iConfiguration = 0, - .bmAttributes = 0x80, - .bMaxPower = 0x32, - - .interface = ifaces, -}; - -static char serial_no[25]; - -/* Description of the device as it appears after enumeration */ -static const char *usb_strings[] = { - "Paparazzi UAV", - "CDC Serial STM32", - serial_no, -}; - - -/* - * Buffer to be used for control requests. - * (from libopencm3 examples) - */ -uint8_t usbd_control_buffer[128]; - -/** - * CDC device control request - * (from libopencm3 examples) - */ -static enum usbd_request_return_codes cdcacm_control_request(usbd_device *usbd_dev, struct usb_setup_data *req, uint8_t **buf, - uint16_t *len, void (**complete)(usbd_device *usbd_dev, struct usb_setup_data *req)) -{ - (void)complete; - (void)buf; - (void)usbd_dev; - - switch (req->bRequest) { - case USB_CDC_REQ_SET_CONTROL_LINE_STATE: { - /* - * This Linux cdc_acm driver requires this to be implemented - * even though it's optional in the CDC spec, and we don't - * advertise it in the ACM functional descriptor. - */ - char local_buf[10]; - struct usb_cdc_notification *notif = (void *)local_buf; - - /* We echo signals back to host as notification. */ - notif->bmRequestType = 0xA1; - notif->bNotification = USB_CDC_NOTIFY_SERIAL_STATE; - notif->wValue = 0; - notif->wIndex = 0; - notif->wLength = 2; - local_buf[8] = req->wValue & 3; - local_buf[9] = 0; - usbd_ep_write_packet(usbd_dev, 0x83, local_buf, 10); - return USBD_REQ_HANDLED; - } - case USB_CDC_REQ_SET_LINE_CODING: - if (*len < sizeof(struct usb_cdc_line_coding)) { - return USBD_REQ_NOTSUPP; - } - - return USBD_REQ_HANDLED; - default: - return USBD_REQ_NOTSUPP; - } -} - -/** - * RX callback for CDC device - * (from libopencm3 examples) - */ -static void cdcacm_data_rx_cb(usbd_device *usbd_dev, uint8_t ep) -{ - (void)ep; - uint8_t buf[MAX_PACKET_SIZE]; - static int len = 0; - // read packet - len = usbd_ep_read_packet(usbd_dev, 0x01, buf, MAX_PACKET_SIZE); - - // write to fifo - for (int i = 0; i < len; i++) { - fifo_put(&rxfifo, buf[i]); - } -} - -// store USB connection status -static bool usb_connected; - -// use suspend callback to detect disconnect -static void suspend_cb(void) -{ - usb_connected = false; -} - -/** - * Set configuration and control callbacks for CDC device - * (from libopencm3 examples) - */ -static void cdcacm_set_config(usbd_device *usbd_dev, uint16_t wValue) -{ - (void)wValue; - - usbd_ep_setup(usbd_dev, 0x01, USB_ENDPOINT_ATTR_BULK, MAX_PACKET_SIZE, cdcacm_data_rx_cb); - usbd_ep_setup(usbd_dev, 0x82, USB_ENDPOINT_ATTR_BULK, MAX_PACKET_SIZE, NULL); - usbd_ep_setup(usbd_dev, 0x83, USB_ENDPOINT_ATTR_INTERRUPT, 16, NULL); - - usbd_register_control_callback(usbd_dev, - USB_REQ_TYPE_CLASS | USB_REQ_TYPE_INTERFACE, - USB_REQ_TYPE_TYPE | USB_REQ_TYPE_RECIPIENT, - cdcacm_control_request); - - // use config and suspend callback to detect connect - usb_connected = true; - usbd_register_suspend_callback(usbd_dev, suspend_cb); -} - - -void fifo_init(fifo_t *fifo, uint8_t *buf) -{ - fifo->head = 0; - fifo->tail = 0; - fifo->buf = buf; -} - - - -bool fifo_put(fifo_t *fifo, uint8_t c) -{ - int next; - - // check if FIFO has room - next = (fifo->head + 1) % VCOM_FIFO_SIZE; - if (next == fifo->tail) { - // full - return false; - } - - fifo->buf[fifo->head] = c; - fifo->head = next; - - return true; -} - - -bool fifo_get(fifo_t *fifo, uint8_t *pc) -{ - int next; - - // check if FIFO has data - if (fifo->head == fifo->tail) { - return false; - } - - next = (fifo->tail + 1) % VCOM_FIFO_SIZE; - - *pc = fifo->buf[fifo->tail]; - fifo->tail = next; - - return true; -} - - -int fifo_avail(fifo_t *fifo) -{ - return (VCOM_FIFO_SIZE + fifo->head - fifo->tail) % VCOM_FIFO_SIZE; -} - - -int fifo_free(fifo_t *fifo) -{ - return (VCOM_FIFO_SIZE - 1 - fifo_avail(fifo)); -} - -bool fifo_peek(fifo_t *fifo, uint8_t *pc, uint8_t ofs) { - if (fifo_avail(fifo) < ofs + 1) { - return false; - } - int index = (fifo->tail + ofs) % VCOM_FIFO_SIZE; - *pc = fifo->buf[index]; - return true; -} - - -/** - * Writes one character to VCOM port fifo. - * - * Since we don't really have an instant feeedback from USB driver if - * the character was written, we always return c if we are connected. - * - * @param [in] c character to write - * @returns character to be written, -1 if not usb is not connected - */ -int VCOM_putchar(int c) -{ - if (usb_connected) { - // check if there are at least two more bytes left in queue - if (VCOM_check_free_space(2)) { - // if yes, add char - fifo_put(&txfifo, c); - /*c is not send until VCOM_send_message is called. This only happens in three cases: - * i) after a timeout (giving the chance to add more data to the fifo before sending) - * ii) if the fifo is filled, at which point the data is send immidiately - * iii) VCOM_send_message is called externally - */ - tx_timeout = TX_TIMEOUT_CNT; // set timeout - } else { - // less than 2 bytes available, add byte and send data now - fifo_put(&txfifo, c); - sys_time_usleep(10); //far from optimal, increase fifo size to prevent this problem - VCOM_send_message(); - } - return c; - } - return -1; -} - -/** - * Reads one character from VCOM port - * @returns character read, or -1 if character could not be read - */ -int VCOM_getchar(void) -{ - static int result = 0; - uint8_t c; - result = fifo_get(&rxfifo, &c) ? c : -1; - return result; -} - -/** - * Reads one character from VCOM port without removing it from the queue - * @param ofs position to read - * @returns character read, or -1 if character could not be read - */ -int VCOM_peekchar(int ofs) -{ - static int result = 0; - uint8_t c; - result = fifo_peek(&rxfifo, &c, ofs) ? c : -1; - return result; -} - -/** - * Checks if buffer free in VCOM buffer - * @returns TRUE if len bytes are free - */ -bool VCOM_check_free_space(uint16_t len) -{ - return (fifo_free(&txfifo) >= len ? TRUE : FALSE); -} - -/** - * Checks if data available in VCOM buffer. - * @returns nonzero if char is available in the queue, zero otherwise - */ -int VCOM_check_available(void) -{ - return (fifo_avail(&rxfifo)); -} - -/** - * Poll usb (required by libopencm3). - * VCOM_event() should be called from main/module event function - */ -void VCOM_event(void) -{ - if (tx_timeout == 1) { // send any remaining bytes that still hang arround in the tx fifo, after a timeout - if (fifo_avail(&txfifo)) { - VCOM_send_message(); - } - } - if (tx_timeout > 0) { - tx_timeout--; - } - - usbd_poll(my_usbd_dev); - -} - -/** - * Send data from fifo right now. - * Only if usb is connected. - */ -void VCOM_send_message(void) -{ - if (usb_connected) { - - uint8_t buf[MAX_PACKET_SIZE]; - uint8_t i; - for (i = 0; i < MAX_PACKET_SIZE; i++) { - if (!fifo_get(&txfifo, &buf[i])) { - break; - } - } - - // wait until the line is free to write - // this however seems buggy, sometimes data gets lost even for the stall to clear - // so do not call this function continously without additional safe guards - while (usbd_ep_stall_get(my_usbd_dev, 0x82)) {}; - - // send the data over usb - usbd_ep_write_packet(my_usbd_dev, 0x82, buf, i); - - } -} - - -/* - * USE_USB_LINE_CODING is not used in case of example1, example2 and telemetry - */ -#ifdef USE_USB_LINE_CODING -void VCOM_allow_linecoding(uint8_t mode __attribute__((unused))) {} -void VCOM_set_linecoding(uint8_t mode __attribute__((unused))) {} -#endif - -/* - * For USB telemetry & generic device API - */ -// Periph with generic device API -struct usb_serial_periph usb_serial; - -// Functions for the generic device API -static int usb_serial_check_free_space(struct usb_serial_periph *p __attribute__((unused)), - long *fd __attribute__((unused)), - uint16_t len) -{ - return (int)VCOM_check_free_space(len); -} - -static void usb_serial_transmit(struct usb_serial_periph *p __attribute__((unused)), - long fd __attribute__((unused)), - uint8_t byte) -{ - VCOM_putchar(byte); -} - -static void usb_serial_transmit_buffer(struct usb_serial_periph *p __attribute__((unused)), - long fd __attribute__((unused)), - uint8_t *data, uint16_t len) -{ - int i; - for (i = 0; i < len; i++) { - VCOM_putchar(data[i]); - } -} - -static void usb_serial_send(struct usb_serial_periph *p __attribute__((unused)), long fd __attribute__((unused))) -{ - VCOM_send_message(); -} - -static int usb_serial_char_available(struct usb_serial_periph *p __attribute__((unused))) -{ - return VCOM_check_available(); -} - -static uint8_t usb_serial_getch(struct usb_serial_periph *p __attribute__((unused))) -{ - return (uint8_t)(VCOM_getchar()); -} - -void VCOM_init(void) -{ - // initialise fifos - fifo_init(&txfifo, txdata); - fifo_init(&rxfifo, rxdata); - - /* set up GPIO pins */ -#if defined STM32F4 - rcc_periph_clock_enable(RCC_GPIOA); - gpio_mode_setup(GPIOA, GPIO_MODE_AF, GPIO_PUPD_NONE, - GPIO11 | GPIO12); - gpio_set_af(GPIOA, GPIO_AF10, GPIO11 | GPIO12); -#endif - - /* USB clock */ - rcc_periph_clock_enable(RCC_OTGFS); - - /* Get serial number */ - desig_get_unique_id_as_string(serial_no, 25); - - /* usb driver init*/ - my_usbd_dev = usbd_init(&otgfs_usb_driver, &dev, &config, - usb_strings, 3, - usbd_control_buffer, sizeof(usbd_control_buffer)); - - usbd_register_set_config_callback(my_usbd_dev, cdcacm_set_config); - - // disable VBUS monitoring - OTG_FS_GCCFG = 0; - OTG_FS_GCCFG |= OTG_GCCFG_NOVBUSSENS | OTG_GCCFG_PWRDWN; - - // disconnected by default - usb_connected = false; - - // Configure generic device - usb_serial.device.periph = (void *)(&usb_serial); - usb_serial.device.check_free_space = (check_free_space_t) usb_serial_check_free_space; - usb_serial.device.put_byte = (put_byte_t) usb_serial_transmit; - usb_serial.device.put_buffer = (put_buffer_t) usb_serial_transmit_buffer; - usb_serial.device.send_message = (send_message_t) usb_serial_send; - usb_serial.device.char_available = (char_available_t) usb_serial_char_available; - usb_serial.device.get_byte = (get_byte_t) usb_serial_getch; - - tx_timeout = 0; -} - diff --git a/sw/airborne/modules/com/usb_serial_stm32.h b/sw/airborne/modules/com/usb_serial_stm32.h deleted file mode 100644 index affd513743..0000000000 --- a/sw/airborne/modules/com/usb_serial_stm32.h +++ /dev/null @@ -1,39 +0,0 @@ -/* - * Copyright (C) 2014 Michal Podhradsky - * - * 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 modules/com/usb_serial_stm32.h - * header for serial over USB modules - */ - -#ifndef USB_SERIAL_STM32_H -#define USB_SERIAL_STM32_H - -#include "mcu_periph/usb_serial.h" - -void init_usb_serial(void); -void periodic_usb_serial(void); -void event_usb_serial(void); - -void usb_serial_parse_packet(int c); - -#endif // USB_SERIAL_STM32_H diff --git a/sw/airborne/modules/com/usb_serial_stm32_example1.c b/sw/airborne/modules/com/usb_serial_stm32_example1.c deleted file mode 100644 index 1675afa0da..0000000000 --- a/sw/airborne/modules/com/usb_serial_stm32_example1.c +++ /dev/null @@ -1,136 +0,0 @@ -/* - * Copyright (C) 2014 Michal Podhradsky - * - * 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 modules/com/usb_serial_stm32_example1.c - * - * USB_SERIAL_STM32 example 1 - a template for a console to autopilot - * - */ - -#include "modules/com/usb_serial_stm32.h" -#include - -void send_command(void); -void cmd_execute(void); - -char cmd_buf[64]; -uint8_t cmd_idx; -bool cmd_avail; -uint8_t prompt = '$'; - -/** - * Init module, call VCOM_init() from here - */ -void init_usb_serial(void) -{ - VCOM_init(); - cmd_idx = 0; - cmd_avail = false; -} - - -/** - * Parse data from buffer - * Note that the function receives int, not char - * Because we want to be able to catch -1 in case no - * more data were available - */ -void usb_serial_parse_packet(int data) -{ - if (data == -1) { return; } - char c = (char)data; - - if (c == '\r' || c == '\n') { - // command complete - cmd_avail = true; - // add termination characters and the prompt into buffer - VCOM_putchar('\r'); - VCOM_putchar('\n'); - VCOM_putchar(prompt); - } else { - // buffer command - cmd_buf[cmd_idx++] = c; - // echo char back and transmit immediately - VCOM_putchar((uint8_t)c); - VCOM_send_message(); - } -} - -/** - * Helper function - */ -static inline void ReadUsbBuffer(void) -{ - while (VCOM_check_available() && !cmd_avail) { - usb_serial_parse_packet(VCOM_getchar()); - } -} - -/** - * Execute command from user - * use strncmp - */ -void cmd_execute(void) -{ - // copy command into tx buffer for user feedback - for (int i = 0; i < cmd_idx; i++) { - VCOM_putchar(cmd_buf[i]); - } - - if (strncmp("help", cmd_buf, cmd_idx) == 0) { - uint8_t response[] = " - user asked for help"; - for (uint16_t i = 0; i < sizeof(response); i++) { - VCOM_putchar(response[i]); - } - } else { - uint8_t response[] = " Command not recognized"; - for (uint16_t i = 0; i < sizeof(response); i++) { - VCOM_putchar(response[i]); - } - } - - // add termination characters and the prompt into buffer - VCOM_putchar('\r'); - VCOM_putchar('\n'); - VCOM_putchar(prompt); - - // reset counter - cmd_idx = 0; - // send complete message - VCOM_send_message(); -} - -/** - * Call VCOM_poll() from module event function - */ -void event_usb_serial(void) -{ - VCOM_event(); - if (VCOM_check_available()) { - ReadUsbBuffer(); - } - if (cmd_avail) { - cmd_execute(); - cmd_avail = false; - } -} diff --git a/sw/airborne/modules/com/usb_serial_stm32_example2.c b/sw/airborne/modules/com/usb_serial_stm32_example2.c deleted file mode 100644 index f210509397..0000000000 --- a/sw/airborne/modules/com/usb_serial_stm32_example2.c +++ /dev/null @@ -1,97 +0,0 @@ -/* - * Copyright (C) 2014 Michal Podhradsky - * - * 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 modules/com/usb_serial_stm32_example2.c - * - * USB_SERIAL_STM32 example 2 - sends lot of data through serial port. User can control - * the flow by pressing "S" for stop and "R" for run. - */ - -#include "modules/com/usb_serial_stm32.h" - -void send_command(void); - -uint8_t run; -uint8_t prompt = '$'; - -/* A lot of text */ -uint8_t big_buffer[] = - " ASCII stands for American Standard Code for Information Interchange. Computers can only understand numbers, so an ASCII code is the numerical representation of a character such as 'a' or '@' or an action of some sort. ASCII was developed a long time ago and now the non-printing characters are rarely used for their original purpose. Below is the ASCII character table and this includes descriptions of the first 32 non-printing characters. ASCII was actually designed for use with teletypes and so the descriptions are somewhat obscure. If someone says they want your CV however in ASCII format, all this means is they want 'plain' text with no formatting such as tabs, bold or underscoring - the raw format that any computer can understand. This is usually so they can easily import the file into their own applications without issues. Notepad.exe creates ASCII text, or in MS Word you can save a file as 'text only' "; - -/** - * Init module, call VCOM_init() from here - */ -void init_usb_serial(void) -{ - VCOM_init(); - run = false; -} - -/** - * Periodic function in case you needed to send data periodically - * like telemetry - * Note that the data are sent once the buffer is full, not immediately - */ -void periodic_usb_serial(void) -{ - if (run) { - for (uint16_t i = 0; i < sizeof(big_buffer); i++) { - VCOM_putchar(big_buffer[i]); - } - } -} - -/** - * Parse data from buffer - * Note that the function receives int, not char - * Because we want to be able to catch -1 in case no - * more data were available - */ -void usb_serial_parse_packet(int data) -{ - if (data == -1) { return; } - uint8_t c = (uint8_t)data; - VCOM_putchar(prompt); - VCOM_putchar(data); - VCOM_putchar('\r'); - VCOM_putchar('\n'); - - if (c == 'S') { - run = false; - } - if (c == 'R') { - run = true; - } - VCOM_send_message(); -} - -/** - * Call VCOM_poll() from module event function - */ -void event_usb_serial(void) -{ - VCOM_event(); - if (VCOM_check_available()) { - usb_serial_parse_packet(VCOM_getchar()); - } -} diff --git a/sw/ext/Makefile b/sw/ext/Makefile index 855882b0f7..ab2ea81dfc 100644 --- a/sw/ext/Makefile +++ b/sw/ext/Makefile @@ -36,7 +36,7 @@ MY_PYTHON := $(shell echo `which python3`) MY_MAVLINKTOOLS := $(shell $(MY_PYTHON) -c 'import imp; import future' 2>&1) MY_DRONECANTOOLS := $(shell $(MY_PYTHON) -c 'import em' 2>&1) -all: libopencm3 luftboot chibios fatfs libsbp TRICAL hacl-c key_generator rustlink ecl matrix mavlink dronecan +all: chibios fatfs libsbp TRICAL hacl-c key_generator rustlink ecl matrix mavlink dronecan # update (and init if needed) all submodules update_submodules: @@ -56,24 +56,6 @@ update_submodules: cd $(PAPARAZZI_SRC) && git submodule sync --recursive sw/ext/$*; \ fi -# only build currentl checkout of libopencm3 -libopencm3.build: - $(Q)$(MAKE) -C libopencm3 lib PREFIX=$(PREFIX) TARGETS="stm32/f1" - $(Q)$(MAKE) -C libopencm3 lib PREFIX=$(PREFIX) FP_FLAGS="-mfloat-abi=softfp -mfpu=fpv4-sp-d16" TARGETS="stm32/f4" - -# update and then build libopencm3 -libopencm3: libopencm3.update libopencm3.build - -# only build current checkout of luftboot -luftboot.build: libopencm3.build - $(Q)$(MAKE) -C luftboot/src all LIBOPENCM3=../../libopencm3 PREFIX=$(PREFIX) - -# update libopencm3 and luftboot, then build it -luftboot: libopencm3.update luftboot.update luftboot.build - -luftboot_flash: luftboot.build - $(Q)$(MAKE) -C luftboot/src flash BMP_PORT?=/dev/ttyACM0 LIBOPENCM3=../../libopencm3 PREFIX=$(PREFIX) - hacl-c: hacl-c.update key_generator: key_generator.update @@ -121,20 +103,15 @@ else endif clean: - $(Q)if [ -f libopencm3/Makefile ]; then \ - $(MAKE) -C $(EXT_DIR)/libopencm3 clean; \ - fi - $(Q)if [ -f luftboot/src/Makefile ]; then \ - $(MAKE) -C luftboot/src clean; \ - fi + clean_opencv_bebop: $(Q)if [ -f opencv_bebop/Makefile ]; then \ $(MAKE) -C opencv_bebop clean; \ fi -.NOTPARALLEL: libopencm3 luftboot -.PHONY: all clean update_submodules libopencm3 luftboot \ - chibios fatfs luftboot_flash libopencm3.build luftboot.build \ +.NOTPARALLEL: +.PHONY: all clean update_submodules \ + chibios fatfs \ mavlink.build libsbp pprzlink pprzlink.build opencv_bebop \ opencv_bebop.build clean_opencv_bebop TRICAL dronecan diff --git a/sw/ext/libopencm3 b/sw/ext/libopencm3 deleted file mode 160000 index 5980c58f9b..0000000000 --- a/sw/ext/libopencm3 +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 5980c58f9b72bf302f64da76bdf105a85b9d1903 diff --git a/sw/ext/luftboot b/sw/ext/luftboot deleted file mode 160000 index 836f16c0ab..0000000000 --- a/sw/ext/luftboot +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 836f16c0abc74617c87fe073fd4b06462c3db1ad