├── .gitignore ├── .gitmodules ├── .travis.yml ├── .vscode └── launch.json ├── CMakeLists.txt ├── LICENSE.md ├── README.md ├── src ├── example │ ├── audio-beep │ │ └── main.cpp │ ├── retired │ │ ├── example-audio-wav.cpp │ │ ├── example-beep.cpp │ │ ├── example-bemf.cpp │ │ ├── example-closed-loop.cpp │ │ ├── example-commutate.cpp │ │ ├── example-detect-input-protocol.cpp │ │ ├── example-dshot-output.cpp │ │ ├── example-input-dshot.cpp │ │ └── example-spwm-complementary.cpp │ ├── usart │ │ └── main.cpp │ └── wav │ │ ├── blue.h │ │ ├── ridin-dirty.h │ │ └── smoke-weed.h ├── hal │ ├── inc │ │ ├── adc.h │ │ ├── bridge.h │ │ ├── comparator.h │ │ ├── global.h │ │ ├── pwm-input.h │ │ ├── system.h │ │ ├── usart.h │ │ └── watchdog.h │ └── src │ │ └── stm32 │ │ └── common │ │ ├── adc.c │ │ ├── bridge.c │ │ ├── comparator-external.c │ │ ├── comparator-internal.c │ │ ├── global.c │ │ ├── pwm-input.c │ │ ├── system.c │ │ ├── usart.c │ │ └── watchdog.c ├── main.cpp ├── main │ ├── inc │ │ ├── audio.h │ │ ├── bemf.h │ │ ├── commutation.h │ │ ├── console.h │ │ ├── debug-pins.h │ │ ├── io.h │ │ ├── isr.h │ │ ├── led.h │ │ └── overcurrent-watchdog.h │ └── src │ │ ├── audio.cpp │ │ ├── bemf.c │ │ ├── commutation.c │ │ ├── comparator-isr.c │ │ ├── console.c │ │ ├── debug-pins.c │ │ ├── io.cpp │ │ ├── led-gpio.c │ │ └── overcurrent-watchdog.c ├── target │ ├── b-g431b-esc1 │ │ ├── pinout.png │ │ ├── target-mcu.cmake │ │ ├── target.c │ │ └── target.h │ ├── gsc │ │ ├── target-mcu.cmake │ │ ├── target.c │ │ └── target.h │ ├── inc │ │ └── stm32 │ │ │ ├── common │ │ │ └── target.h │ │ │ ├── f0 │ │ │ ├── isr.c │ │ │ └── target.h │ │ │ ├── f3 │ │ │ ├── isr.c │ │ │ └── target.h │ │ │ └── g4 │ │ │ ├── isr.c │ │ │ └── target.h │ ├── nucleo-f042 │ │ ├── target-mcu.cmake │ │ ├── target.c │ │ └── target.h │ ├── nucleo-f072 │ │ ├── target-mcu.cmake │ │ ├── target.c │ │ └── target.h │ ├── nucleo-f334 │ │ ├── pinout.png │ │ ├── target-mcu.cmake │ │ ├── target.c │ │ └── target.h │ ├── nucleo-g431 │ │ ├── pinout.png │ │ ├── target-mcu.cmake │ │ ├── target.c │ │ └── target.h │ ├── steval-esc002v1 │ │ ├── target-mcu.cmake │ │ ├── target.c │ │ └── target.h │ ├── target.cmake │ └── wraith32 │ │ ├── target-mcu.cmake │ │ ├── target.c │ │ └── target.h └── util │ ├── genlookup.py │ ├── openesc.json │ └── sine-lookup.h └── tools ├── build-all.sh ├── generate-nvic-all.sh ├── install-arm-toolchain.sh ├── travis-ci-functions.sh └── travis-ci-script.sh /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode/.cortex-debug* 2 | .vscode/settings.json 3 | build/ 4 | 5 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "lib/stm32-boilerplate"] 2 | path = lib/stm32-boilerplate 3 | url = https://github.com/jaxxzer/stm32-boilerplate 4 | [submodule "lib/ping-cpp"] 5 | path = lib/ping-cpp 6 | url = https://github.com/jaxxzer/ping-cpp 7 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | dist: xenial 2 | 3 | language: minimal 4 | 5 | addons: 6 | apt: 7 | packages: 8 | - python3-pip 9 | 10 | install: 11 | - pip3 install --user jinja2; 12 | 13 | script: 14 | - tools/travis-ci-script.sh || travis_terminate 1 15 | 16 | before_deploy: 17 | - git tag continuous-deployment -f 18 | - git remote set-url origin https://${GITHUB_TOKEN}@github.com/jaxxzer/open-esc-firmware 19 | - git push origin continuous-deployment -f 20 | 21 | deploy: 22 | provider: releases 23 | api_key: ${GITHUB_TOKEN} 24 | file_glob: true 25 | file: build/bin/*.hex 26 | overwrite: true 27 | skip_cleanup: true 28 | on: 29 | branch: master 30 | -------------------------------------------------------------------------------- /.vscode/launch.json: -------------------------------------------------------------------------------- 1 | { 2 | // Use IntelliSense to learn about possible attributes. 3 | // Hover to view descriptions of existing attributes. 4 | // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 5 | "version": "0.2.0", 6 | "configurations": [ 7 | { 8 | "name": "F030", 9 | "cwd": "${workspaceRoot}", 10 | "executable": "build/debug.elf", 11 | "request": "launch", 12 | "type": "cortex-debug", 13 | "servertype": "openocd", 14 | "configFiles": [ 15 | "lib/stm32-boilerplate/lib/openocd/f0.cfg" 16 | ], 17 | "svdFile": "lib/stm32-boilerplate/lib/svd/stm32f0/STM32F0x0.svd", 18 | "device": "STM32F030" 19 | }, 20 | { 21 | "name": "F042-nucleo", 22 | "cwd": "${workspaceRoot}", 23 | "executable": "build/debug.elf", 24 | "request": "launch", 25 | "type": "cortex-debug", 26 | "servertype": "openocd", 27 | "configFiles": [ 28 | "lib/stm32-boilerplate/lib/openocd/f0-nucleo.cfg" 29 | ], 30 | "svdFile": "lib/stm32-boilerplate/lib/svd/stm32f0/STM32F0x2.svd", 31 | "device": "STM32F042" 32 | }, 33 | { 34 | "name": "F051", 35 | "cwd": "${workspaceRoot}", 36 | "executable": "build/debug.elf", 37 | "request": "launch", 38 | "type": "cortex-debug", 39 | "servertype": "openocd", 40 | "configFiles": [ 41 | "lib/stm32-boilerplate/lib/openocd/f0.cfg" 42 | ], 43 | "svdFile": "lib/stm32-boilerplate/lib/svd/stm32f0/STM32F0x1.svd", 44 | "device": "STM32F051" 45 | }, 46 | { 47 | "name": "F103", 48 | "cwd": "${workspaceRoot}", 49 | "executable": "build/debug.elf", 50 | "request": "launch", 51 | "type": "cortex-debug", 52 | "servertype": "openocd", 53 | "configFiles": [ 54 | "lib/stm32-boilerplate/lib/openocd/f1.cfg" 55 | ], 56 | "svdFile": "lib/stm32-boilerplate/lib/svd/stm32f1/STM32F103.svd", 57 | "device": "STM32F103" 58 | }, 59 | { 60 | "name": "F303", 61 | "cwd": "${workspaceRoot}", 62 | "executable": "build/debug.elf", 63 | "request": "launch", 64 | "type": "cortex-debug", 65 | "servertype": "openocd", 66 | "configFiles": [ 67 | "lib/stm32-boilerplate/lib/openocd/f3.cfg" 68 | ], 69 | "svdFile": "lib/stm32-boilerplate/lib/svd/stm32f3/STM32F303.svd", 70 | "device": "STM32F303" 71 | }, 72 | { 73 | "name": "F072-nucleo", 74 | "cwd": "${workspaceRoot}", 75 | "executable": "build/debug.elf", 76 | "request": "launch", 77 | "type": "cortex-debug", 78 | "servertype": "openocd", 79 | "configFiles": [ 80 | "lib/stm32-boilerplate/lib/openocd/f0-nucleo.cfg" 81 | ], 82 | "svdFile": "lib/stm32-boilerplate/lib/svd/stm32f0/STM32F0x2.svd", 83 | "device": "STM32F072" 84 | }, 85 | { 86 | "name": "F103-nucleo", 87 | "cwd": "${workspaceRoot}", 88 | "executable": "build/debug.elf", 89 | "request": "launch", 90 | "type": "cortex-debug", 91 | "servertype": "openocd", 92 | "configFiles": [ 93 | "lib/stm32-boilerplate/lib/openocd/f1-nucleo.cfg" 94 | ], 95 | "svdFile": "lib/stm32-boilerplate/lib/svd/stm32f1/STM32F103.svd", 96 | "device": "STM32F103" 97 | }, 98 | { 99 | "name": "F303-nucleo", 100 | "cwd": "${workspaceRoot}", 101 | "executable": "build/debug.elf", 102 | "request": "launch", 103 | "type": "cortex-debug", 104 | "servertype": "openocd", 105 | "configFiles": [ 106 | "lib/stm32-boilerplate/lib/openocd/f3-nucleo.cfg" 107 | ] 108 | }, 109 | { 110 | "name": "F334-nucleo", 111 | "cwd": "${workspaceRoot}", 112 | "executable": "build/debug.elf", 113 | "request": "launch", 114 | "type": "cortex-debug", 115 | "servertype": "openocd", 116 | "configFiles": [ 117 | "lib/stm32-boilerplate/lib/openocd/f3-nucleo.cfg" 118 | ], 119 | "svdFile": "lib/stm32-boilerplate/lib/svd/stm32f3/STM32F3x4.svd", 120 | "device": "STM32F334" 121 | }, 122 | { 123 | "name": "G431-nucleo", 124 | "cwd": "${workspaceRoot}", 125 | "executable": "build/debug.elf", 126 | "request": "launch", 127 | "type": "cortex-debug", 128 | "servertype": "openocd", 129 | "configFiles": [ 130 | "lib/stm32-boilerplate/lib/openocd/g4-nucleo.cfg" 131 | ], 132 | "svdFile": "lib/stm32-boilerplate/lib/svd/stm32g4/STM32G431xx.svd" 133 | } 134 | ] 135 | } 136 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.12) 2 | project(stm32-boilerplate-example) 3 | 4 | set(CMAKE_C_COMPILER "arm-none-eabi-gcc") 5 | set(CMAKE_CXX_COMPILER "arm-none-eabi-g++") 6 | 7 | set(BOILERPLATE ${PROJECT_SOURCE_DIR}/lib/stm32-boilerplate) 8 | set(LIBOPENCM3 ${BOILERPLATE}/lib/libopencm3) 9 | set(LIBOPENCM3_SRC ${LIBOPENCM3}/lib/stm32) 10 | 11 | set(COMPILER_CXX_FLAGS "-fno-use-cxa-atexit") 12 | set(COMPILER_FLAGS "-fdata-sections -ffunction-sections -O0 -DNDEBUG -Werror") 13 | set(LINKER_FLAGS "-nostartfiles -specs=nano.specs -specs=nosys.specs --static -ggdb3 -Wl,--gc-sections -Wl,--start-group -lc -lgcc -lnosys -Wl,--end-group -Wl,-Map=output.map -T ${BOILERPLATE}/src/link/stm32-mem.ld -L ${BOILERPLATE}/lib/libopencm3/lib") 14 | set(CMAKE_BUILD_TYPE Debug) 15 | set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/bin) 16 | 17 | include(src/target/target.cmake) 18 | 19 | include_directories(${BOILERPLATE}/lib/libopencm3/include) 20 | include_directories(${PROJECT_SOURCE_DIR}/lib/ping-cpp/src/message) 21 | 22 | set(CMAKE_C_FLAGS "${COMPILER_FLAGS} ${ARCH_FLAGS}") 23 | set(CMAKE_CXX_FLAGS "${COMPILER_CXX_FLAGS} ${COMPILER_FLAGS} ${ARCH_FLAGS}") 24 | set(CMAKE_EXE_LINKER_FLAGS "${LINKER_FLAGS} ${ARCH_FLAGS}") 25 | 26 | include(${BOILERPLATE}/src/target/f/f.cmake) 27 | 28 | set(TARGET_SRC 29 | ${ADC_SRC} 30 | ${COMP_SRC} 31 | ${DMA_SRC} 32 | ${GPIO_SRC} 33 | ${ISR_SRC} 34 | ${RCC_SRC} 35 | ${TARGET_SRC} 36 | ${TIMER_SRC} 37 | ${USART_SRC} 38 | ${COMPARATOR_SRC} 39 | ) 40 | 41 | include_directories( 42 | ${PROJECT_SOURCE_DIR}/src/hal/inc 43 | ${PROJECT_SOURCE_DIR}/src/main/inc 44 | ) 45 | 46 | set(PINGCPP_SRC ${PROJECT_SOURCE_DIR}/lib/ping-cpp/src) 47 | set(PINGPROTOCOL_SRC ${PROJECT_SOURCE_DIR}/lib/ping-cpp/lib/ping-protocol/src) 48 | 49 | set(COMMON_SRC 50 | ${BOILERPLATE}/lib/libopencm3/lib/cm3/nvic.c 51 | ${BOILERPLATE}/lib/libopencm3/lib/cm3/vector.c 52 | ${BOILERPLATE}/lib/libopencm3/lib/stm32/common/exti_common_all.c 53 | ${BOILERPLATE}/lib/libopencm3/lib/stm32/common/iwdg_common_all.c 54 | ${PROJECT_SOURCE_DIR}/src/hal/src/stm32/common/adc.c 55 | ${PROJECT_SOURCE_DIR}/src/hal/src/stm32/common/bridge.c 56 | ${PROJECT_SOURCE_DIR}/src/hal/src/stm32/common/global.c 57 | ${PROJECT_SOURCE_DIR}/src/hal/src/stm32/common/pwm-input.c 58 | ${PROJECT_SOURCE_DIR}/src/hal/src/stm32/common/system.c 59 | ${PROJECT_SOURCE_DIR}/src/hal/src/stm32/common/usart.c 60 | ${PROJECT_SOURCE_DIR}/src/hal/src/stm32/common/watchdog.c 61 | ${PROJECT_SOURCE_DIR}/src/main/src/audio.cpp 62 | ${PROJECT_SOURCE_DIR}/src/main/src/bemf.c 63 | ${PROJECT_SOURCE_DIR}/src/main/src/commutation.c 64 | ${PROJECT_SOURCE_DIR}/src/main/src/comparator-isr.c 65 | ${PROJECT_SOURCE_DIR}/src/main/src/console.c 66 | ${PROJECT_SOURCE_DIR}/src/main/src/debug-pins.c 67 | ${PROJECT_SOURCE_DIR}/src/main/src/io.cpp 68 | ${PROJECT_SOURCE_DIR}/src/main/src/led-gpio.c 69 | ${PROJECT_SOURCE_DIR}/src/main/src/overcurrent-watchdog.c 70 | ${PINGCPP_SRC}/message/ping-message-common.h 71 | ${PINGCPP_SRC}/message/ping-message-openesc.h 72 | ) 73 | 74 | if(DEFINED EXAMPLE) 75 | set(EXE_NAME open-esc-example-${EXAMPLE}) 76 | add_executable(${EXE_NAME} ${TARGET_SRC} ${COMMON_SRC} ${PROJECT_SOURCE_DIR}/src/example/${EXAMPLE}/main.cpp) 77 | else() 78 | set(EXE_NAME open-esc-${TARGET_BOARD}) 79 | add_executable(${EXE_NAME} ${TARGET_SRC} ${COMMON_SRC} src/main.cpp) 80 | endif() 81 | 82 | 83 | # TODO how to make clean? 84 | add_custom_command(TARGET ${EXE_NAME} POST_BUILD 85 | COMMAND arm-none-eabi-objcopy -O ihex ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}/${EXE_NAME} ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}/${EXE_NAME}.hex 86 | COMMAND arm-none-eabi-size ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}/${EXE_NAME} 87 | COMMAND cp ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}/${EXE_NAME} debug.elf 88 | ) 89 | 90 | add_custom_command(TARGET ${EXE_NAME} PRE_LINK 91 | COMMAND python ${BOILERPLATE}/src/link/generate-ldscript.py -p ${TARGET_MCU} > ${BOILERPLATE}/src/link/stm32-mem.ld 92 | ) 93 | 94 | # 'make flash' to upload to board after compiling 95 | add_custom_target(flash 96 | # try stlink v2 first, then stlink v2-1 if that fails 97 | openocd -f interface/stlink-v2.cfg -f ${OPENOCD_TARGET} -c "program debug.elf verify reset exit" || 98 | openocd -f interface/stlink-v2-1.cfg -f ${OPENOCD_TARGET} -c "program debug.elf verify reset exit" 99 | DEPENDS ${EXE_NAME} 100 | ) 101 | 102 | 103 | add_custom_command(OUTPUT ${PINGCPP_SRC}/message/ping-message-common.h 104 | # generate ping-cpp message files 105 | COMMAND ${PINGCPP_SRC}/generate/generate-message.py --output-dir ${PINGCPP_SRC}/message 106 | COMMENT "generating ping-cpp messages" 107 | ) 108 | 109 | add_custom_command(OUTPUT ${PINGCPP_SRC}/message/ping-message-openesc.h 110 | # generate ping-cpp message files 111 | COMMAND ${PINGPROTOCOL_SRC}/generator.py --definition ${PROJECT_SOURCE_DIR}/src/util/openesc.json --template ${PINGCPP_SRC}/generate/templates/ping-message-.h.in > ${PINGCPP_SRC}/message/ping-message-openesc.h 112 | COMMENT "generating openesc messages" 113 | ) 114 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | Welcome to open-esc: the open esc! 2 | 3 | The aim of this project is to produce an ESC that is very high performing, widely applicable, and attractive to manufacture. 4 | 5 | GSC will run on these target microcontrollers: 6 | 7 | - Giga Device GD32 F350 series (in development) 8 | - STM32 F0 series (working!) 9 | - STM32 F3 series (in development) 10 | - STM32 G0 series (in development) 11 | - STM32 G4 series (in development) 12 | - STSPIN series (in development) 13 | - Active Semi PAC5523 (in development) 14 | 15 | [open-esc-hardware](https://github.com/jaxxzer/open-esc-hardware) is a repository of hardware designs supported by this firmware. 16 | 17 | To add support for a new hardware, copy one of the exisiting folders in the [target](target) directory and change the pin defitions. 18 | 19 | Current functionality: 20 | - automatic input signal detection (pwm, oneshot125/42, multishot) 21 | - audio 22 | - open-loop sine-modulated pwm 23 | - closed-loop comparator based commutation 24 | 25 | Project roadmap: 26 | - dshot, proshot 27 | - Closed-loop adc based commutation 28 | - Support for dshot commands 29 | - Save/store settings 30 | - Sensored Field Oriented Control 31 | - Sensorless Field Oriented Control 32 | - Support for common communication interfaces (uart, i2c, spi, can, usb) 33 | - GUI configuration 34 | - Bipolar pwm (active braking) 35 | - Support for PAC5523 36 | - Switch from stm32-lib to STM LL, libopencm3 or bare-metal 37 | 38 | To build the main application for the selected [target board](src/target) (the default board is `wraith32`): 39 | ```sh 40 | mkdir -p build 41 | cd build 42 | cmake --configure -DTARGET_BOARD=wraith32 .. 43 | make 44 | ``` 45 | 46 | To flash the program after building (with openocd and st-link programmer), use the `flash` make target: 47 | ```sh 48 | make flash 49 | ``` 50 | 51 | This project's [`launch.json`](.vscode/launch.json) will allow you to debug the program in vscode with the cortex debug extension using an st-link programmer and openocd. 52 | 53 | **Be Advised:** I am developing my understanding of motor control and embedded programming as I work on this project. I am not an expert (yet). Many cool open source projects already exist to control brushless motors. Some day, I might choose to retire this project in favor of developing further one or more of these: 54 | 55 | - [f051bldc](https://github.com/conuthead/f051bldc) (I see you, @conuthead) 56 | - [phobia](https://bitbucket.org/amaora/phobia) (stm32, foc) 57 | - [blheli_s](https://github.com/bitdump/BLHeli/tree/master/BLHeli_S%20SiLabs) (assembly, avr, silabs) 58 | - [tgy](https://github.com/sim-/tgy) (assembly, avr, silabs) 59 | - [openmotordrive](https://github.com/OpenMotorDrive) (stm32) 60 | - [vesc](https://github.com/vedderb/bldc) (stm32, foc) 61 | - [odrive](https://github.com/madcowswe/ODrive) (stm32) 62 | - [betaflight-esc](https://github.com/betaflight/betaflight-esc) (stm32, not mature) 63 | - [sapog](https://github.com/PX4/sapog) (stm32) 64 | -------------------------------------------------------------------------------- /src/example/audio-beep/main.cpp: -------------------------------------------------------------------------------- 1 | extern "C" { 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | } 9 | 10 | // hpp 11 | #include 12 | 13 | #include 14 | 15 | // declared/documented in global.h 16 | // todo remove these irrelevant definitions 17 | // build requires them for now 18 | const uint16_t slow_run_zc_confirmations_required = 14; 19 | uint16_t run_zc_confirmations_required = slow_run_zc_confirmations_required; 20 | volatile bool starting; 21 | const uint16_t startup_commutation_period_ticks = 12000; 22 | const uint16_t startup_zc_confirmations_required = 15; 23 | volatile uint32_t zc_counter; 24 | uint32_t zc_confirmations_required = startup_zc_confirmations_required; 25 | 26 | int main(void) { 27 | system_initialize(); 28 | 29 | // safety stuff 30 | adc_initialize(); 31 | overcurrent_watchdog_initialize(); 32 | adc_start(); 33 | watchdog_start(10); // 10ms watchdog timeout 34 | 35 | // enable bridge 36 | bridge_initialize(); 37 | 38 | // high c 39 | audio_play_note_blocking(1046, 0x04, 1000000); 40 | 41 | // disable bridge 42 | bridge_disable(); 43 | 44 | while (1) 45 | { 46 | watchdog_reset(); 47 | } 48 | } 49 | -------------------------------------------------------------------------------- /src/example/retired/example-audio-wav.cpp: -------------------------------------------------------------------------------- 1 | // Audio example 2 | // If this program is difficult to understand, start with the beep example 3 | // 4 | // This program plays an 8 bit 8khz pcm wav audio file 5 | // The file is not parsed, the header data is played like the rest of the file (so brief, it's not noticable) 6 | // 7 | // The program is configured to produce this result on the motor phase timer: 8 | // 9 | // |------------------------------------------------------------------| 10 | // timer overflow | | | | | | | | | 11 | // ccr1 output |‾‾‾‾‾|___________|‾‾‾|_____________|‾‾‾‾‾|___________|‾‾|_________| 12 | // ccr2 output |________|‾|_______________|‾|_______________|‾|_______________|‾|_| 13 | // ccr3 output |__________________________________________________________________| 14 | // 15 | // Here, the audio pcm data is loaded into CCR1. CCR2 is loaded with a constant (small) 16 | // value. The phases are activated alternately in order to make the rotor move back and 17 | // forth. The amplitude of the vibration is dynamically adjusted by changing the CCR1 value 18 | // The timer overflow frequency must be more than 4x the maximum frequency in the audio 19 | // to prevent ailiasing. (Because the pwm is alternated between phases, the effective 20 | // pwm frequency is half the timer overflow frequency.) 21 | 22 | #include "stm32lib-conf.h" 23 | 24 | // pick any header from the wav directory 25 | #include "wav/blue.h" 26 | 27 | Timer &timerMotor = PHASE_OUTPUT_TIMER; 28 | Gpio gpio_m1{ GPIO_PORT_HIGH_U, GPIO_PIN_HIGH_U }; 29 | Gpio gpio_m1n{ GPIO_PORT_LOW_U, GPIO_PIN_LOW_U }; 30 | Gpio gpio_m2{ GPIO_PORT_HIGH_V, GPIO_PIN_HIGH_V }; 31 | Gpio gpio_m2n{ GPIO_PORT_LOW_V, GPIO_PIN_LOW_V }; 32 | Gpio gpio_m3{ GPIO_PORT_HIGH_W, GPIO_PIN_HIGH_W }; 33 | Gpio gpio_m3n{ GPIO_PORT_LOW_W, GPIO_PIN_LOW_W }; 34 | 35 | TimerChannelOutput tco1{&timerMotor, TIM_Channel_1}; 36 | TimerChannelOutput tco2{&timerMotor, TIM_Channel_2}; 37 | TimerChannelOutput tco3{&timerMotor, TIM_Channel_3}; 38 | 39 | // This timer updates the amplitude at 8kHz 40 | Timer &timerAudio = timer15; 41 | 42 | // Buffer to hold CCR1, CCR2 values in two states 43 | // index 1 is updated with sequential audio samples at 8kHz 44 | uint16_t pulses[] = { 0, 0, 200, 0 }; 45 | 46 | int main() { 47 | configureClocks(1000); 48 | 49 | // Configure gpio 50 | gpio_m1.init(GPIO_Mode_AF); 51 | gpio_m1n.init(GPIO_Mode_AF); 52 | gpio_m2.init(GPIO_Mode_AF); 53 | gpio_m2n.init(GPIO_Mode_AF); 54 | gpio_m3.init(GPIO_Mode_AF); 55 | gpio_m3n.init(GPIO_Mode_AF); 56 | 57 | gpio_m1.configAF(GPIO_AF_TIMER_PHASE_OUTPUT); 58 | gpio_m1n.configAF(GPIO_AF_TIMER_PHASE_OUTPUT); 59 | gpio_m2.configAF(GPIO_AF_TIMER_PHASE_OUTPUT); 60 | gpio_m2n.configAF(GPIO_AF_TIMER_PHASE_OUTPUT); 61 | gpio_m3.configAF(GPIO_AF_TIMER_PHASE_OUTPUT); 62 | gpio_m3n.configAF(GPIO_AF_TIMER_PHASE_OUTPUT); 63 | 64 | // Configure dma channel to transfer pulse lengths from `pulses` array to timer CCR1 and CCR2 65 | // This is done via the 'dma burst' feature of the timer (DMAR) 66 | Dma dmaChannel = Dma(DMA1_Channel4); 67 | dmaChannel.init((uint32_t) & (timerMotor.peripheral()->DMAR), (uint32_t)&pulses[0], 4, DMA_DIR_PeripheralDST, 68 | DMA_PeripheralDataSize_HalfWord, DMA_MemoryDataSize_HalfWord, DMA_Mode_Circular, DMA_Priority_Medium, 69 | DMA_MemoryInc_Enable, DMA_PeripheralInc_Disable, DMA_M2M_Disable); 70 | dmaChannel.setEnabled(ENABLE); 71 | 72 | // phase output timer configuration 73 | // PSC = 0, ARR = 1000, pwm frequency is 72kHz, allowing max audio frequency of 18kHz 74 | // reduce ARR (minimum 255) to increase duty to the motor during audio playback 75 | // this will increase volume, but also heating! 76 | timerMotor.init(0, 1000); 77 | timerMotor.setDTG(0x18); // ~333ns deadtime insertion (this also sets OSSR) 78 | 79 | // the dma request may be an unused CC (set CCR to 0 for equivalent to update request) or Update (pick what's available) 80 | TIM_DMACmd(timerMotor.peripheral(), TIM_DMA_CC4, ENABLE); 81 | TIM_DMAConfig(timerMotor.peripheral(), TIM_DMABase_CCR1, TIM_DMABurstLength_2Transfers); 82 | 83 | tco1.init(TIM_OCMode_PWM1, 0, TIM_OutputState_Enable, TIM_OutputNState_Enable); 84 | tco2.init(TIM_OCMode_PWM1, 0, TIM_OutputState_Enable, TIM_OutputNState_Enable); 85 | tco3.init(TIM_OCMode_PWM1, 0, TIM_OutputState_Enable, TIM_OutputNState_Enable); 86 | 87 | timerMotor.setMOE(ENABLE); 88 | timerMotor.setEnabled(ENABLE); 89 | 90 | // This dma channel moves samples from the audio buffer to the 'pulses' array used by the other dma channel 91 | // The samples are then moved into the timer CCR1 register by the other dma channel 92 | Dma dmaChannelAudio = Dma(DMA1_Channel5); 93 | dmaChannelAudio.init((uint32_t) & pulses[1], (uint32_t)&wav[0], sizeof(wav), DMA_DIR_PeripheralDST, 94 | DMA_PeripheralDataSize_HalfWord, DMA_MemoryDataSize_Byte, DMA_Mode_Normal, DMA_Priority_Medium, 95 | DMA_MemoryInc_Enable, DMA_PeripheralInc_Disable, DMA_M2M_Disable); 96 | dmaChannelAudio.setEnabled(ENABLE); 97 | 98 | // this timer triggers dmaChannelAudio to transfer samples from the audio buffer at 8kHz 99 | timerAudio.initFreq(8000); 100 | 101 | // make dma request on update event 102 | TIM_DMACmd(timerAudio.peripheral(), TIM_DMA_Update, ENABLE); 103 | 104 | // begin transferring audio samples 105 | timerAudio.setEnabled(ENABLE); 106 | 107 | // wait for audio to complete 108 | while (!DMA_GetFlagStatus(DMA1_FLAG_TC5)); 109 | 110 | timerAudio.setEnabled(DISABLE); 111 | 112 | // freewheel 113 | tco1.setEnabledN(DISABLE); 114 | tco2.setEnabledN(DISABLE); 115 | tco3.setEnabledN(DISABLE); 116 | timerMotor.setEnabled(DISABLE); 117 | 118 | while(1); 119 | return 0; 120 | } 121 | -------------------------------------------------------------------------------- /src/example/retired/example-beep.cpp: -------------------------------------------------------------------------------- 1 | // Beep example 2 | // Use a dma channel to update timer CCR1 and CCR2 on update events 3 | // Pulse them alternately to make the rotor waggle back and forth 4 | // OC preload enabled! 5 | // Timer makes dma request on each overflow and dma updates CCR1 6 | // and CCR2 (preloaded, so the new values are used on the subsequent overflow) 7 | 8 | #include "stm32lib-conf.h" 9 | 10 | Timer &timer = PHASE_OUTPUT_TIMER; 11 | Gpio gpio_m1{ GPIO_PORT_HIGH_U, GPIO_PIN_HIGH_U }; 12 | Gpio gpio_m1n{ GPIO_PORT_LOW_U, GPIO_PIN_LOW_U }; 13 | Gpio gpio_m2{ GPIO_PORT_HIGH_V, GPIO_PIN_HIGH_V }; 14 | Gpio gpio_m2n{ GPIO_PORT_LOW_V, GPIO_PIN_LOW_V }; 15 | Gpio gpio_m3{ GPIO_PORT_HIGH_W, GPIO_PIN_HIGH_W }; 16 | Gpio gpio_m3n{ GPIO_PORT_LOW_W, GPIO_PIN_LOW_W }; 17 | 18 | TimerChannelOutput tco1{&timer, TIM_Channel_1}; 19 | TimerChannelOutput tco2{&timer, TIM_Channel_2}; 20 | TimerChannelOutput tco3{&timer, TIM_Channel_3}; 21 | 22 | // If dma transfer length (CNDTR) is 4, then alternate CCR1 and CCR2 values (400, 0) 23 | // If dma transfer length is 8 or 12, then a consistent pulse length is not used (500, 100 also) 24 | // This is to modulate some small noise in the waveform for pleasant effect 25 | uint16_t pulses[] = { 0, 400, 400, 0, 0, 500, 500, 0, 0, 100, 100, 0 }; 26 | uint16_t notes[] = { 9000, 10000, 6000, 6500, 8000 }; 27 | 28 | int main() { 29 | configureClocks(1000); 30 | 31 | #if defined(STM32F0) 32 | #error 33 | #elif defined(STM32F3) // should work with f0 too 34 | gpio_m1.init(GPIO_Mode_AF); 35 | gpio_m1n.init(GPIO_Mode_AF); 36 | gpio_m2.init(GPIO_Mode_AF); 37 | gpio_m2n.init(GPIO_Mode_AF); 38 | gpio_m3.init(GPIO_Mode_AF); 39 | gpio_m3n.init(GPIO_Mode_AF); 40 | 41 | gpio_m1.configAF(GPIO_AF_TIMER_PHASE_OUTPUT); 42 | gpio_m1n.configAF(GPIO_AF_TIMER_PHASE_OUTPUT); 43 | gpio_m2.configAF(GPIO_AF_TIMER_PHASE_OUTPUT); 44 | gpio_m2n.configAF(GPIO_AF_TIMER_PHASE_OUTPUT); 45 | gpio_m3.configAF(GPIO_AF_TIMER_PHASE_OUTPUT); 46 | gpio_m3n.configAF(GPIO_AF_TIMER_PHASE_OUTPUT); 47 | #else 48 | #error 49 | #endif 50 | 51 | // Configure dma channel to transfer pulse lengths from `pulses` array to timer CCR1 and CCR2 52 | // This is done via the 'dma burst' feature of the timer (DMAR) 53 | Dma dmaChannel = Dma(DMA1_Channel4); 54 | 55 | // move values from pulses array to timer DMAR register 56 | dmaChannel.init((uint32_t) & (timer.peripheral()->DMAR), (uint32_t)&pulses[0], 12, DMA_DIR_PeripheralDST, 57 | DMA_PeripheralDataSize_HalfWord, DMA_MemoryDataSize_HalfWord, DMA_Mode_Circular, DMA_Priority_Medium, 58 | DMA_MemoryInc_Enable, DMA_PeripheralInc_Disable, DMA_M2M_Disable); 59 | 60 | dmaChannel.setEnabled(ENABLE); 61 | 62 | // Timer configuration 63 | timer.initFreq(notes[0]); 64 | timer.setDTG(0x18); // ~333ns deadtime insertion 65 | 66 | // the dma request may be an unused CC (set CCR to 0 for equivalent to update request) 67 | // or Update (pick what's available) 68 | TIM_DMACmd(timer.peripheral(), TIM_DMA_CC4, ENABLE); 69 | 70 | // timer generates two DMA requests, CCR1 and CCR2 are updated 71 | TIM_DMAConfig(timer.peripheral(), TIM_DMABase_CCR1, TIM_DMABurstLength_2Transfers); 72 | 73 | // enable CCR preload 74 | TIM_OC1PreloadConfig(TIM1, TIM_OCPreload_Enable); 75 | TIM_OC2PreloadConfig(TIM1, TIM_OCPreload_Enable); 76 | 77 | tco1.init(TIM_OCMode_PWM1, 0, TIM_OutputState_Enable, TIM_OutputNState_Enable); 78 | tco2.init(TIM_OCMode_PWM1, 0, TIM_OutputState_Enable, TIM_OutputNState_Enable); 79 | tco3.init(TIM_OCMode_PWM1, 0, TIM_OutputState_Enable, TIM_OutputNState_Enable); 80 | 81 | timer.setEnabled(ENABLE); 82 | timer.setMOE(ENABLE); 83 | 84 | // play beep sequence 85 | for (uint8_t n = 0; n < 5; n++) { 86 | timer.setFrequency(notes[n]); 87 | mDelay(75); 88 | } 89 | 90 | // freewheeling 91 | tco1.setEnabledN(DISABLE); 92 | tco2.setEnabledN(DISABLE); 93 | tco3.setEnabledN(DISABLE); 94 | timer.setEnabled(DISABLE); 95 | 96 | while(1); 97 | 98 | return 0; 99 | } 100 | -------------------------------------------------------------------------------- /src/example/retired/example-bemf.cpp: -------------------------------------------------------------------------------- 1 | // six-step comparator bemf detection example 2 | // the comparator state is polled in this example, and at each zero-cross event, 3 | // the input pin is reconfigured for the next undriven phase 4 | // demonstrates comparator states during six step commutation 5 | // spin a motor by hand and a gpio is pulsed at each zero cross 6 | #include "stm32lib-conf.h" 7 | 8 | Gpio gpioComparator0 { GPIO_PORT_BEMF, GPIO_PIN_BEMF_U }; 9 | Gpio gpioComparator1 { GPIO_PORT_BEMF, GPIO_PIN_BEMF_V }; 10 | Gpio gpioComparator2 { GPIO_PORT_BEMF, GPIO_PIN_BEMF_W }; 11 | Gpio gpioComparatorN { GPIO_PORT_BEMF, GPIO_PIN_BEMF_NEUTRAL }; 12 | 13 | // this pin is used for output, it is toggled on each comparator match (zero cross) 14 | Gpio gpioTest1 { GPIO_USART1_TX_PORT, GPIO_USART1_TX_PIN }; 15 | 16 | // this array holds the desired values of the comparator register for each commutation step 17 | // NOTE: the pin selection should cycle through each phase, as in (A-B-C-A-B-C) 18 | // however, this does not work as expected on GD32, and the correct sequence appears to be instead (A-A-B-B-C-C) 19 | // my best explanation moment for why this works is a silicon error. the issue appears to be only with the falling 20 | // edge configurations. 21 | static const uint32_t compRegs[6] = { 22 | 0x00000041, // rising edge 23 | 0x00000861, // invert comparator output (falling edge) 24 | 0x00000061, // rising edge 25 | 0x00000851, // invert comparator output (falling edge) 26 | 0x00000051, // rising edge 27 | 0x00000841, // invert comparator output (falling edge) 28 | }; 29 | 30 | // This is what the correct comparator configuration order should be according to the register documentation 31 | // static const uint32_t compRegs[6] = { 32 | // 0x00000041, // rising edge 33 | // 0x00000851, // invert comparator output (falling edge) 34 | // 0x00000061, // rising edge 35 | // 0x00000841, // invert comparator output (falling edge) 36 | // 0x00000051, // rising edge 37 | // 0x00000861, // invert comparator output (falling edge) 38 | // }; 39 | 40 | // the current commutation step 41 | static uint8_t comStep = 0; 42 | 43 | // get comparator output 44 | bool comparatorGetOuptut(uint32_t baseAddress); 45 | 46 | // advance to the next commutation step configuration 47 | void nextCompare(); 48 | 49 | int main() { 50 | configureClocks(1000); 51 | 52 | gpioComparator0.init(GPIO_Mode_AN); 53 | gpioComparator1.init(GPIO_Mode_AN); 54 | gpioComparator2.init(GPIO_Mode_AN); 55 | gpioComparatorN.init(GPIO_Mode_AN); 56 | 57 | gpioTest1.init(GPIO_Mode_OUT); 58 | 59 | // Initialize the comparator 60 | RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE); 61 | *(volatile uint32_t *) COMP_BASE = compRegs[comStep]; 62 | 63 | while (1) { 64 | // wait for zero cross 65 | while (comparatorGetOuptut(COMP_BASE)); 66 | 67 | // set up to catch the next zero cross in sequence 68 | // configure comparator for the next step 69 | // the proper direction of rotation for the rotor is hard coded here 70 | // turn the rotor both ways and note the difference in the output 71 | // replace this line with comStep-- to reverse the operation 72 | comStep++; 73 | comStep = comStep % 6; 74 | *(volatile uint32_t *) COMP_BASE = compRegs[comStep]; 75 | 76 | // toggle gpio, hook up a logic analyzer to see 77 | gpioTest1.toggle(); 78 | } 79 | 80 | return 0; 81 | } 82 | 83 | #define COMP_MASK_OUTPUT (1 << 14) 84 | bool comparatorGetOuptut(uint32_t baseAddress) { 85 | return *(volatile uint32_t *) baseAddress & COMP_MASK_OUTPUT; 86 | } 87 | -------------------------------------------------------------------------------- /src/example/retired/example-closed-loop.cpp: -------------------------------------------------------------------------------- 1 | #include "stm32lib-conf.h" 2 | 3 | // Example of closed-loop 6-step commutation 4 | // wait 5 seconds, then the duty cycles between 3/20 ~ 5/20 power forever 5 | 6 | typedef struct { 7 | uint32_t CCER; // CCER register value 8 | volatile uint32_t * pwmChannel; // ccr register address to write zero 9 | volatile uint32_t * zeroDutyChannel1; // ccr register address to write zero 10 | volatile uint32_t * zeroDutyChannel2; // ccr register address to write zero 11 | uint16_t comparatorState; // COMP register value 12 | } commutation_state_t; 13 | 14 | volatile commutation_state_t commutationStates[6] = { 15 | { 0x00000155, &TIM1->CCR1, &TIM1->CCR2, &TIM1->CCR3, 0x0041 }, 16 | { 0x00000515, &TIM1->CCR1, &TIM1->CCR2, &TIM1->CCR3, 0x0861 }, 17 | { 0x00000551, &TIM1->CCR2, &TIM1->CCR1, &TIM1->CCR3, 0x0061 }, 18 | { 0x00000155, &TIM1->CCR2, &TIM1->CCR1, &TIM1->CCR3, 0x0851 }, 19 | { 0x00000515, &TIM1->CCR3, &TIM1->CCR1, &TIM1->CCR2, 0x0051 }, 20 | { 0x00000551, &TIM1->CCR3, &TIM1->CCR1, &TIM1->CCR2, 0x0841 }, 21 | }; 22 | 23 | static uint8_t commutationState = 0; 24 | 25 | Timer &commutationTimer = timer15; 26 | Timer &zeroCrossTimer = timer3; 27 | 28 | Timer &timer = PHASE_OUTPUT_TIMER; 29 | Gpio gpio_m1{ GPIO_PORT_HIGH_U, GPIO_PIN_HIGH_U }; 30 | Gpio gpio_m1n{ GPIO_PORT_LOW_U, GPIO_PIN_LOW_U }; 31 | Gpio gpio_m2{ GPIO_PORT_HIGH_V, GPIO_PIN_HIGH_V }; 32 | Gpio gpio_m2n{ GPIO_PORT_LOW_V, GPIO_PIN_LOW_V }; 33 | Gpio gpio_m3{ GPIO_PORT_HIGH_W, GPIO_PIN_HIGH_W }; 34 | Gpio gpio_m3n{ GPIO_PORT_LOW_W, GPIO_PIN_LOW_W }; 35 | 36 | TimerChannelOutput tco1{&timer, TIM_Channel_1}; 37 | TimerChannelOutput tco2{&timer, TIM_Channel_2}; 38 | TimerChannelOutput tco3{&timer, TIM_Channel_3}; 39 | 40 | Gpio gpioComparator0 { GPIO_PORT_BEMF, GPIO_PIN_BEMF_U }; 41 | Gpio gpioComparator1 { GPIO_PORT_BEMF, GPIO_PIN_BEMF_V }; 42 | Gpio gpioComparator2 { GPIO_PORT_BEMF, GPIO_PIN_BEMF_W }; 43 | Gpio gpioComparatorN { GPIO_PORT_BEMF, GPIO_PIN_BEMF_NEUTRAL }; 44 | 45 | Gpio gpioTest1 { GPIO_USART1_TX_PORT, GPIO_USART1_TX_PIN }; 46 | Gpio gpioTest2 { GPIO_USART1_RX_PORT, GPIO_USART1_RX_PIN }; 47 | 48 | #define COMP_MASK_OUTPUT (1 << 14) 49 | volatile uint8_t validCross = 0; 50 | 51 | // schedule speed adjustment for demonstration 52 | uint32_t nextSpeedAdjustTime = microseconds + 1000000; 53 | 54 | // adjust the duty by this amount when 55 | int8_t inc = 1; 56 | 57 | // flag to indicate we are just getting going and need another 58 | // zero cross to get the inter-cross period measurement 59 | bool firstcross = true; 60 | 61 | // comparator checks required to be considered a valid zero cross 62 | uint8_t validCrosses = 3; 63 | 64 | // divide the inter cross measurement by this amount, then schedule the next commutation by the result 65 | // should be 1 if zero cross is detected on every step (rising and falling edges) 66 | // should be 2 if zero cross is detected on every other step (rising edges only) 67 | static const uint8_t divisor = 1; 68 | 69 | void detectZeroCross(); 70 | void commutate(); 71 | void freewheel(); 72 | 73 | extern "C" { 74 | void TIM15_IRQHandler(void) { 75 | commutate(); 76 | TIM15->SR &= ~1; 77 | validCross = 0; 78 | } 79 | } 80 | 81 | // commutation timer ticks from stop 82 | static const uint16_t baseTiming = 0xaff; 83 | static const uint16_t minDuty = 300; 84 | static const uint16_t maxDuty = 500; 85 | static uint16_t duty = minDuty; 86 | 87 | 88 | int main() { 89 | configureClocks(1000); 90 | 91 | mDelay(5000); 92 | 93 | // initialize gpio 94 | gpio_m1.init(GPIO_Mode_AF); 95 | gpio_m1n.init(GPIO_Mode_AF); 96 | gpio_m2.init(GPIO_Mode_AF); 97 | gpio_m2n.init(GPIO_Mode_AF); 98 | gpio_m3.init(GPIO_Mode_AF); 99 | gpio_m3n.init(GPIO_Mode_AF); 100 | 101 | gpio_m1.configAF(GPIO_AF_TIMER_PHASE_OUTPUT); 102 | gpio_m1n.configAF(GPIO_AF_TIMER_PHASE_OUTPUT); 103 | gpio_m2.configAF(GPIO_AF_TIMER_PHASE_OUTPUT); 104 | gpio_m2n.configAF(GPIO_AF_TIMER_PHASE_OUTPUT); 105 | gpio_m3.configAF(GPIO_AF_TIMER_PHASE_OUTPUT); 106 | gpio_m3n.configAF(GPIO_AF_TIMER_PHASE_OUTPUT); 107 | 108 | gpioComparator0.init(GPIO_Mode_AN); 109 | gpioComparator1.init(GPIO_Mode_AN); 110 | gpioComparator2.init(GPIO_Mode_AN); 111 | gpioComparatorN.init(GPIO_Mode_AN); 112 | 113 | gpioTest1.init(GPIO_Mode_OUT); 114 | gpioTest2.init(GPIO_Mode_OUT); 115 | 116 | // Initialize the comparator 117 | RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE); 118 | *(volatile uint32_t *) COMP_BASE = commutationStates[commutationState].comparatorState; 119 | 120 | // Timer configuration 121 | // pwm frequency is system clock / 2000 = 72MHz/2000 = 36kHz 122 | timer.init(0, 2000); 123 | timer.setDTG(0x18); // ~333ns deadtime insertion (this is also configuring OSSR) 124 | 125 | // Initialize pwm channels disabled, 0 duty 126 | tco1.init(TIM_OCMode_PWM1, 0); 127 | tco2.init(TIM_OCMode_PWM1, 0); 128 | tco3.init(TIM_OCMode_PWM1, 0); 129 | 130 | // set bit 1, enable CCPC for com events 131 | timer.peripheral()->CR2 |= 0x1; 132 | timer.setEnabled(ENABLE); 133 | timer.setMOE(ENABLE); 134 | 135 | // commutate on timer update 136 | commutationTimer.init(29, baseTiming); 137 | commutationTimer.setupUpCallback(&commutate); 138 | commutationTimer.interruptConfig(TIM_IT_Update, ENABLE); 139 | nvic_config(TIM15_IRQn, 0, 0, ENABLE); 140 | commutationTimer.setEnabled(ENABLE); 141 | 142 | // 72MHz / 30 = 2.4M 143 | // we want 0xffff * timer period to be larger than any zero-cross interval we could expect 144 | // 0xffff / 2.4M = 27 milliseconds 145 | zeroCrossTimer.init(29, 0xffff); 146 | zeroCrossTimer.setEnabled(ENABLE); 147 | 148 | while(1) { 149 | 150 | detectZeroCross(); 151 | 152 | // wait for next commutation (interrupt) 153 | while(validCross == validCrosses); 154 | 155 | // smoothly cycle duty up and down 156 | if (microseconds > nextSpeedAdjustTime) { 157 | duty += inc; 158 | nextSpeedAdjustTime = microseconds + 1000; 159 | if (duty >= maxDuty) { 160 | inc = -1; 161 | } else if (duty <= minDuty) { 162 | inc = 1; 163 | } 164 | } 165 | }; 166 | 167 | return 0; 168 | } 169 | 170 | void detectZeroCross() 171 | { 172 | if (commutationTimer.peripheral()->CNT > 0xff // commutation blanking, don't measure immediately after commutation 173 | && TIM1->CNT > 400 // pwm blanking, don't measure right after pwm on 174 | && ( *(volatile uint32_t *) COMP_BASE & (1<<14) ) ) // comparator state 175 | { 176 | // this qualifies as zero cross 177 | validCross++; 178 | gpioTest1.toggle(); 179 | } 180 | 181 | // we have detected enough zero crosses to be sure of it, time to commutate 182 | if (validCross == validCrosses) { 183 | // zc-zc period in timer counts 184 | uint16_t period = zeroCrossTimer.peripheral()->CNT / divisor; 185 | // reset zero cross timer 186 | zeroCrossTimer.peripheral()->CNT = 0; 187 | 188 | // skip the first cross, so that we have accurately timed one period start-to-finish 189 | if (firstcross) { 190 | firstcross = false; 191 | validCrosses = 4; 192 | } else { 193 | // if the zero cross period is exceptionally short, it's probably de-sync 194 | // start over 195 | if (period < commutationTimer.peripheral()->ARR / 2) { 196 | firstcross = true; 197 | validCrosses = 8; 198 | validCross = 0; 199 | 200 | commutationTimer.peripheral()->ARR = baseTiming; 201 | duty = minDuty; 202 | } else { 203 | // Schedule next commutation 204 | commutationTimer.peripheral()->ARR = period; 205 | commutationTimer.peripheral()->CNT = period/2; 206 | // pulse pin 207 | gpioTest2.toggle(); 208 | gpioTest2.toggle(); 209 | 210 | } 211 | } 212 | } 213 | } 214 | 215 | void commutate() 216 | { 217 | // preload next commutation state 218 | commutationState++; 219 | commutationState %= 6; 220 | TIM1->CCER = commutationStates[commutationState].CCER; 221 | *(commutationStates[commutationState].zeroDutyChannel1) = 0; 222 | *(commutationStates[commutationState].zeroDutyChannel2) = 0; 223 | *(commutationStates[commutationState].pwmChannel) = duty; 224 | TIM1->EGR |= 0x20; // set bit 5 in event generation register, generate commutate event 225 | *(__IO uint32_t *) COMP_BASE = commutationStates[commutationState].comparatorState; 226 | 227 | gpioTest2.toggle(); 228 | } 229 | 230 | void freewheel() 231 | { 232 | commutationTimer.setEnabled(DISABLE); 233 | tco1.setCompare(0); 234 | tco2.setCompare(0); 235 | tco3.setCompare(0); 236 | tco1.setEnabled(ENABLE); 237 | tco2.setEnabled(ENABLE); 238 | tco3.setEnabled(ENABLE); 239 | tco1.setEnabledN(DISABLE); 240 | tco2.setEnabledN(DISABLE); 241 | tco3.setEnabledN(DISABLE); 242 | timer.setEnabled(DISABLE); 243 | } 244 | -------------------------------------------------------------------------------- /src/example/retired/example-commutate.cpp: -------------------------------------------------------------------------------- 1 | #include "stm32lib-conf.h" 2 | 3 | typedef struct { 4 | uint32_t CCER; // which channel/nchannels to activate for this step 5 | volatile uint32_t * pwmChannel; // ccr register address to write duty cycle 6 | volatile uint32_t * zeroDutyChannel1; // ccr register address to write zero 7 | volatile uint32_t * zeroDutyChannel2; // ccr register address to write zero 8 | } commutation_state_t; 9 | 10 | volatile commutation_state_t commutationStates[6] = { 11 | { 0x00000155, &TIM1->CCR1, &TIM1->CCR2, &TIM1->CCR3 }, // set CCER bits 8, 6, 4, 2, 0 12 | { 0x00000515, &TIM1->CCR1, &TIM1->CCR2, &TIM1->CCR3 }, // set CCER bits 10, 8, 4, 2, 0 13 | { 0x00000551, &TIM1->CCR2, &TIM1->CCR1, &TIM1->CCR3 }, // set CCER bits 10, 8, 6, 4, 0 14 | { 0x00000155, &TIM1->CCR2, &TIM1->CCR1, &TIM1->CCR3 }, // set CCER bits 8, 6, 4, 2, 0 15 | { 0x00000515, &TIM1->CCR3, &TIM1->CCR1, &TIM1->CCR2 }, // set CCER bits 10, 8, 4, 2, 0 16 | { 0x00000551, &TIM1->CCR3, &TIM1->CCR1, &TIM1->CCR2 }, // set CCER bits 10, 8, 6, 4, 0 17 | }; 18 | 19 | static uint8_t commutationState = 0; 20 | static uint16_t duty = 100; 21 | 22 | // TODO OCxM bits 23 | 24 | // ----------------------------------------------- 25 | // | Step0 | Step1 | Step2 | Step3 | Step4 | Step5 | 26 | //---------------------------------------------------------- 27 | //|Channel1 | 1 | 1 | 1 | 1 | 1 | 1 | 28 | //---------------------------------------------------------- 29 | //|Channel1N | 1 | 1 | 0 | 1 | 1 | 0 | 30 | //---------------------------------------------------------- 31 | //|Duty | ? | ? | 0 | 0 | 0 | 0 | 32 | //---------------------------------------------------------- 33 | //|Channel2 | 1 | 1 | 1 | 1 | 1 | 1 | 34 | //---------------------------------------------------------- 35 | //|Channel2N | 1 | 0 | 1 | 1 | 0 | 1 | 36 | //---------------------------------------------------------- 37 | //|Duty | 0 | 0 | ? | ? | 0 | 0 | 38 | //---------------------------------------------------------- 39 | //|Channel3 | 1 | 1 | 1 | 1 | 1 | 1 | 40 | //---------------------------------------------------------- 41 | //|Channel3N | 0 | 1 | 1 | 0 | 1 | 1 | 42 | //---------------------------------------------------------- 43 | //|Duty | 0 | 0 | 0 | 0 | ? | ? | 44 | 45 | // x = ZC ----------------------------------------------- 46 | // | Step0 | Step1 | Step2 | Step3 | Step4 | Step5 | 47 | //----------------------------------------------------------- 48 | //|Channel1 |‾|_|‾|_|‾|_|‾|_________________________________| 49 | //-------------------------------x-----------------------x--- 50 | //|Channel1N |_|‾|_|‾|_|‾|_|‾|_______|‾‾‾‾‾‾‾‾‾‾‾‾‾‾‾|_______| 51 | //----------------------------------------------------------- 52 | //|Channel2 |_______________|‾|_|‾|_|‾|_|‾|_________________| 53 | //-----------------------x-----------------------x------------ 54 | //|Channel2N |‾‾‾‾‾‾‾|_________|‾|_|‾|_|‾|_|‾|_______|‾‾‾‾‾‾‾| 55 | //----------------------------------------------------------- 56 | //|Channel3 |_______________________________|‾|_|‾|_|‾|_|‾|_| 57 | //---------------x-----------------------x------------------- 58 | //|Channel3N |_______|‾‾‾‾‾‾‾‾‾‾‾‾‾‾‾|_________|‾|_|‾|_|‾|_|‾| 59 | 60 | Timer &timer = PHASE_OUTPUT_TIMER; 61 | Gpio gpio_m1{ GPIO_PORT_HIGH_U, GPIO_PIN_HIGH_U }; 62 | Gpio gpio_m1n{ GPIO_PORT_LOW_U, GPIO_PIN_LOW_U }; 63 | Gpio gpio_m2{ GPIO_PORT_HIGH_V, GPIO_PIN_HIGH_V }; 64 | Gpio gpio_m2n{ GPIO_PORT_LOW_V, GPIO_PIN_LOW_V }; 65 | Gpio gpio_m3{ GPIO_PORT_HIGH_W, GPIO_PIN_HIGH_W }; 66 | Gpio gpio_m3n{ GPIO_PORT_LOW_W, GPIO_PIN_LOW_W }; 67 | 68 | TimerChannelOutput tco1{&timer, TIM_Channel_1}; 69 | TimerChannelOutput tco2{&timer, TIM_Channel_2}; 70 | TimerChannelOutput tco3{&timer, TIM_Channel_3}; 71 | 72 | void commutate() { 73 | // increment and wrap commutation step 74 | commutationState++; 75 | commutationState %= 6; 76 | 77 | TIM1->CCER = commutationStates[commutationState].CCER; 78 | *(commutationStates[commutationState].zeroDutyChannel1) = 0; 79 | *(commutationStates[commutationState].zeroDutyChannel2) = 0; 80 | *(commutationStates[commutationState].pwmChannel) = 47; 81 | 82 | // set bit 5 in event generation register, generate commutate event 83 | // TODO move to top of call, and preload next commutation state after generating the event 84 | TIM1->EGR |= 0x20; 85 | } 86 | 87 | int main() { 88 | configureClocks(1000); 89 | 90 | gpio_m1.init(GPIO_Mode_AF); 91 | gpio_m1n.init(GPIO_Mode_AF); 92 | gpio_m2.init(GPIO_Mode_AF); 93 | gpio_m2n.init(GPIO_Mode_AF); 94 | gpio_m3.init(GPIO_Mode_AF); 95 | gpio_m3n.init(GPIO_Mode_AF); 96 | 97 | gpio_m1.configAF(GPIO_AF_TIMER_PHASE_OUTPUT); 98 | gpio_m1n.configAF(GPIO_AF_TIMER_PHASE_OUTPUT); 99 | gpio_m2.configAF(GPIO_AF_TIMER_PHASE_OUTPUT); 100 | gpio_m2n.configAF(GPIO_AF_TIMER_PHASE_OUTPUT); 101 | gpio_m3.configAF(GPIO_AF_TIMER_PHASE_OUTPUT); 102 | gpio_m3n.configAF(GPIO_AF_TIMER_PHASE_OUTPUT); 103 | 104 | // Timer configuration 105 | timer.init(0, 2000); 106 | timer.setDTG(0x18); // ~333ns deadtime insertion (this also sets ossr) 107 | 108 | tco1.init(TIM_OCMode_PWM1, 0); 109 | tco2.init(TIM_OCMode_PWM1, 0); 110 | tco3.init(TIM_OCMode_PWM1, 0); 111 | 112 | // set bit 1, enable CCPC for com events 113 | TIM1->CR2 |= 0x1; 114 | timer.setEnabled(ENABLE); 115 | timer.setMOE(ENABLE); 116 | 117 | while(1) { 118 | mDelay(10); 119 | commutate(); 120 | }; 121 | 122 | return 0; 123 | } 124 | -------------------------------------------------------------------------------- /src/example/retired/example-detect-input-protocol.cpp: -------------------------------------------------------------------------------- 1 | // This example measures the pulse width and frequency on an input pin 2 | // Channel 1 gives the frequency (rise -> rise time) 3 | // Channel 2 gives the pulse width (rise -> fall time) 4 | // the timer must have a slave controller to use this example! 5 | 6 | #include "stm32lib-conf.h" 7 | 8 | #define USART_BAUDRATE 115200 9 | #define GPIO_CAPTURE_AF 2 10 | #define CAPTURE_TIMER timer2 11 | #define GPIO_CAPTURE_PORT GPIOA 12 | #define GPIO_CAPTURE_PIN 15 13 | 14 | const char* inputProtocol(); 15 | void initUsart1(void); 16 | 17 | Gpio gpioLed{ GPIO_PORT_LED_RED, GPIO_PIN_LED_RED }; 18 | 19 | Timer &timerCapture = CAPTURE_TIMER; 20 | Gpio gpioCapture{ GPIO_CAPTURE_PORT, GPIO_CAPTURE_PIN }; 21 | TimerChannelInput tciRising{ &timerCapture, TIM_Channel_1 }; 22 | TimerChannelInput tciFalling{ &timerCapture, TIM_Channel_2 }; 23 | 24 | int main() { 25 | configureClocks(1000); 26 | initUsart1(); 27 | #if defined(STM32F1) 28 | gpioLed.init(GPIO_Mode_AF_PP); 29 | gpioCapture.init(GPIO_Mode_IN_FLOATING); 30 | gpioLed.remapConfig(GPIO_LED1_REMAP, ENABLE); 31 | gpioCapture.remapConfig(GPIO_CAPTURE_REMAP, ENABLE); 32 | #elif defined(STM32F0) || defined(STM32F3) 33 | gpioLed.init(GPIO_Mode_OUT); 34 | gpioCapture.init(GPIO_Mode_AF); 35 | gpioCapture.configAF(GPIO_CAPTURE_AF); 36 | #else 37 | #error 38 | #endif 39 | 40 | TIM_SelectInputTrigger(timerCapture.peripheral(), TIM_TS_TI1FP1); 41 | TIM_SelectSlaveMode(timerCapture.peripheral(), TIM_SlaveMode_Reset); 42 | timerCapture.init(); 43 | 44 | // Note CCxS bits only writable when CCxE is 0 (channel is disabled) 45 | tciRising.init(TIM_ICPolarity_Rising, 0x0); 46 | tciFalling.init(TIM_ICPolarity_Falling, 0x0, TIM_ICPSC_DIV1, TIM_ICSelection_IndirectTI); 47 | 48 | timerCapture.setEnabled(ENABLE); 49 | 50 | print_clocks(); 51 | 52 | while (1) { 53 | mDelay(100); 54 | printf("period: %d\tduty: %d\r\n", TIM2->CCR1, TIM2->CCR2); 55 | printf("input protocol: %s\r\n", inputProtocol()); 56 | gpioLed.toggle(); 57 | } 58 | 59 | return 0; 60 | } 61 | 62 | // @72MHz... 63 | // proshot is 290, 70~200 64 | // dshot1200 is 60, 20/40 65 | // dshot600 is 120, 40/80 66 | // dshot300 is 240, 80/160 67 | // multishot is 360~1800 68 | // oneshot42 is 3020~6040 69 | // oneshot125 is 8000~16000 70 | // classical pwm is shorter 1000~2000 microseconds 2500us pulse is 180000 71 | const char* inputProtocol() 72 | { 73 | uint32_t period = timerCapture.peripheral()->CCR1; 74 | uint32_t duty = timerCapture.peripheral()->CCR2; 75 | if (period < 2000) { // high frequency/digital protocol 76 | // TODO use number of symbols/frame to differentiate between dshot and proshot 77 | if ( (duty > 65 && duty < 75) || duty > 180 ) { 78 | return "proshot"; 79 | } 80 | return "dshot"; 81 | } else { // analog protocol 82 | if (duty < 2400) { 83 | return "multishot"; 84 | } else if (duty < 7000) { 85 | return "oneshot42"; 86 | } else if (duty < 25000) { 87 | return "oneshot125"; 88 | } else if (duty < 180000) { 89 | return "classic"; 90 | } else { 91 | return "noise/unknown"; 92 | } 93 | } 94 | } 95 | 96 | void initUsart1(void) { 97 | 98 | Gpio gpioUsart1Tx{GPIO_USART1_TX_PORT, GPIO_USART1_TX_PIN}; 99 | Gpio gpioUsart1Rx{GPIO_USART1_RX_PORT, GPIO_USART1_RX_PIN}; 100 | 101 | #if defined(STM32F0) 102 | nvic_config(USART1_IRQn, 0, ENABLE); 103 | #elif defined(STM32F1) || defined(STM32F3) 104 | nvic_config(USART1_IRQn, 0, 0, ENABLE); 105 | #else 106 | #error 107 | #endif 108 | 109 | #if defined(STM32F0) || defined(STM32F3) 110 | gpioUsart1Rx.init(GPIO_Mode_AF, GPIO_PuPd_UP); 111 | gpioUsart1Tx.init(GPIO_Mode_AF, GPIO_PuPd_UP); 112 | gpioUsart1Rx.configAF(GPIO_USART1_AF); 113 | gpioUsart1Tx.configAF(GPIO_USART1_AF); 114 | #elif defined(STM32F1) 115 | gpioUsart1Rx.init(GPIO_Mode_AF_PP); 116 | gpioUsart1Tx.init(GPIO_Mode_AF_PP); 117 | Gpio::remapConfig(GPIO_USART1_REMAP, ENABLE); 118 | #else 119 | #error 120 | #endif 121 | 122 | uart1.init(USART_BAUDRATE); 123 | uart1.ITConfig(USART_IT_RXNE, ENABLE); 124 | uart1.setEnabled(ENABLE); 125 | uart1.cls(); 126 | } 127 | -------------------------------------------------------------------------------- /src/example/retired/example-dshot-output.cpp: -------------------------------------------------------------------------------- 1 | // This example transmits a dshot symbol using dma 2 | // The timer period is set to the duration of the dshot frame 3 | // 4 | // The bit pulse widths are written to an array, then dma transfers 5 | // each bit to the compare register sequentially on each timer update event 6 | // 7 | // The array must be null-terminated in order to set 0 duty on the signal 8 | // line at the end of a frame 9 | 10 | #include "stm32lib-conf.h" 11 | 12 | #define THROTTLE_BITS 11 13 | #define FRAME_BITS 16 14 | #define DSHOT_TIMER timer2 15 | #define DSHOT_TIMER_CHANNEL TIM_Channel_1 16 | #define DSHOT_DMA_CHANNEL DMA1_Channel2 17 | #define DSHOT_GPIO_PORT GPIOA 18 | #define DSHOT_GPIO_PIN 15 19 | #define DSHOT_GPIO_AF 2 20 | 21 | Timer &dshotTimer = DSHOT_TIMER ; 22 | TimerChannelOutput tco { &dshotTimer, DSHOT_TIMER_CHANNEL }; 23 | Dma dshotDma { DSHOT_DMA_CHANNEL }; 24 | Gpio dshotGpio { DSHOT_GPIO_PORT, DSHOT_GPIO_PIN }; 25 | 26 | // pulse widths, in timer counts 27 | static const uint16_t dshot_0 = 20000; 28 | static const uint16_t dshot_1 = 2 * dshot_0; 29 | static const uint16_t dshot_period = 3 * dshot_0; // symbol duration 30 | 31 | // arbitrary data (for now) 32 | uint16_t pulses[FRAME_BITS]; 33 | 34 | void updatePulses(); 35 | uint16_t prepareDshotPacket(uint16_t throttle); 36 | 37 | int main() { 38 | configureClocks(1000); 39 | 40 | dshotGpio.init(GPIO_Mode_AF); 41 | dshotGpio.configAF(DSHOT_GPIO_AF); 42 | 43 | dshotTimer.init(2, dshot_period); 44 | tco.init(TIM_OCMode_PWM1, 0, TIM_OutputState_Enable); 45 | tco.preloadConfig(ENABLE); 46 | 47 | dshotDma.init((uint32_t) & (dshotTimer.peripheral()->CCR1), (uint32_t)&pulses[0], FRAME_BITS, DMA_DIR_PeripheralDST, 48 | DMA_PeripheralDataSize_Word, DMA_MemoryDataSize_HalfWord, DMA_Mode_Normal, DMA_Priority_Medium, 49 | DMA_MemoryInc_Enable, DMA_PeripheralInc_Disable, DMA_M2M_Disable); 50 | 51 | dshotDma.setEnabled(ENABLE); 52 | 53 | 54 | TIM_DMACmd(dshotTimer.peripheral(), TIM_DMA_Update, ENABLE); 55 | 56 | dshotTimer.setEnabled(ENABLE); 57 | dshotTimer.setMOE(ENABLE); 58 | 59 | // try it out if this example does not work as you expect 60 | // print_clocks(); 61 | 62 | while (1) { 63 | mDelay(100); 64 | updatePulses(); 65 | 66 | // reload dma number of data to transfer 67 | // it will go out on next timer update 68 | dshotDma.setEnabled(DISABLE); 69 | DMA1_Channel2->CNDTR = sizeof(pulses) / 2; 70 | dshotDma.setEnabled(ENABLE); 71 | } 72 | 73 | return 0; 74 | } 75 | 76 | void updatePulses() 77 | { 78 | static uint8_t throttle; 79 | static uint8_t bitPosition; 80 | throttle++; 81 | throttle %= 1 << THROTTLE_BITS; 82 | 83 | uint16_t packet = prepareDshotPacket(throttle); 84 | for (bitPosition = 0; bitPosition < THROTTLE_BITS; bitPosition++) 85 | { 86 | if (packet & (1 << bitPosition)) { 87 | pulses[bitPosition] = dshot_1; 88 | } else { 89 | pulses[bitPosition] = dshot_0; 90 | } 91 | } 92 | 93 | } 94 | 95 | // checksum calculation 96 | // thanks a lot, BETAFLIGHT 97 | uint16_t prepareDshotPacket(uint16_t throttle) 98 | { 99 | static const bool telemetry = false; 100 | uint16_t packet = (throttle << 1) | telemetry; 101 | 102 | // compute checksum 103 | int csum = 0; 104 | int csum_data = packet; 105 | for (int i = 0; i < 3; i++) { 106 | csum ^= csum_data; // xor data by nibbles 107 | csum_data >>= 4; 108 | } 109 | csum &= 0xf; 110 | // append checksum 111 | packet = (packet << 4) | csum; 112 | 113 | return packet; 114 | } 115 | -------------------------------------------------------------------------------- /src/example/retired/example-input-dshot.cpp: -------------------------------------------------------------------------------- 1 | // dshot decoding 2 | // this is capable of decoding dshot frames at at least 8kHz with minimal cpu overhead 3 | // cc3 interrupt fires at the end of a packet 4 | // this code autobauds, it does not discriminate between dshot300, 600, 1200 etc. 5 | // the single limiting factor is DSHOT timeout counts, increase this number to increase the MINIMUM acceptable dshot bitrate 6 | // the maximum dshot bitrate is determined by the timer clock frequency, this might be equivalent to 7 | // ~dshot5000 with a 72MHz clock in tame (low noise) electrical environments 8 | // the default of 400 is fitting for dshot 300 and frame frequencies (1/looptimes) of up to 8kHz 9 | 10 | #include "stm32lib-conf.h" 11 | 12 | #define DSHOT_TIMEOUT_COUNTS 400 13 | #define USART_BAUDRATE 115200 14 | #define GPIO_CAPTURE_AF 2 15 | #define CAPTURE_TIMER timer2 16 | #define GPIO_CAPTURE_PORT GPIOA 17 | #define GPIO_CAPTURE_PIN 15 18 | #define TIMER_DMA_CHANNEL DMA1_Channel3 19 | 20 | Dma dmaChannel = Dma( TIMER_DMA_CHANNEL ); 21 | 22 | Gpio gpioLed { GPIO_PORT_LED_RED, GPIO_PIN_LED_RED }; 23 | 24 | Timer &timerCapture = CAPTURE_TIMER; 25 | Gpio gpioCapture{ GPIO_CAPTURE_PORT, GPIO_CAPTURE_PIN }; 26 | TimerChannelInput tciRising{&timerCapture, TIM_Channel_1}; 27 | TimerChannelInput tciFalling{&timerCapture, TIM_Channel_2}; 28 | TimerChannelOutput tcoFraming{&timerCapture, TIM_Channel_3}; 29 | 30 | const uint8_t maxFrameLength = 50; 31 | volatile uint16_t fallCaptures[maxFrameLength]; 32 | volatile uint8_t frameLength = 0; // number of bits in last dshot frame received 33 | volatile uint16_t dshotThreshold = 0; 34 | volatile bool gotCapture = false; 35 | volatile uint32_t decodedPackets = 0; 36 | 37 | // Triggered on start of frame 38 | void cc2Callback(void); 39 | 40 | // triggered on timeout waiting for next bit in frame (end of frame) 41 | void cc3Callback(void); 42 | 43 | void initUsart1(void); 44 | 45 | // ch1 -> rising counter is captured into ccr1 on rising edges tells you the frequency of pulse (ccr == time since last 46 | // rising edge) ch2 -> falling timer counter is captured into ccr2 on falling edges ch3 -> reset and trigger timer on 47 | // ccr match (make this less than or equal to the inter-frame period) Dma transfer is restarted (the buffer must be 48 | // copied by application code before the next transfer request from the timer (next pulse)) 49 | int main() { 50 | configureClocks(1000); 51 | initUsart1(); 52 | print_clocks(); 53 | 54 | #if defined(STM32F1) 55 | gpioLed.init(GPIO_Mode_AF_PP); 56 | gpioCapture.init(GPIO_Mode_AF_OD); 57 | gpioLed.configRemap(GPIO_LED1_REMAP, ENABLE); 58 | gpioLed.configRemap(GPIO_CAPTURE_REMAP, ENABLE); 59 | #elif defined(STM32F0) || defined(STM32F3) 60 | gpioLed.init(GPIO_Mode_OUT); 61 | gpioCapture.init(GPIO_Mode_AF); 62 | gpioCapture.configAF(GPIO_CAPTURE_AF); 63 | #else 64 | #error 65 | #endif 66 | 67 | TIM_SelectInputTrigger(timerCapture.peripheral(), TIM_TS_TI1FP1); 68 | TIM_SelectSlaveMode(timerCapture.peripheral(), TIM_SlaveMode_Reset); 69 | 70 | dmaChannel.init((uint32_t) & (timerCapture.peripheral()->CCR2), (uint32_t)&fallCaptures[0], maxFrameLength, DMA_DIR_PeripheralSRC, 71 | DMA_PeripheralDataSize_HalfWord, DMA_MemoryDataSize_HalfWord, DMA_Mode_Normal, DMA_Priority_Medium, 72 | DMA_MemoryInc_Enable, DMA_PeripheralInc_Disable, DMA_M2M_Disable); 73 | 74 | TIM_DMACmd(timerCapture.peripheral(), TIM_DMA_CC2, ENABLE); 75 | 76 | timerCapture.setupCc2Callback(&cc2Callback); 77 | timerCapture.setupCc3Callback(&cc3Callback); 78 | timerCapture.init(); 79 | 80 | timerCapture.interruptConfig(TIM_IT_CC2, ENABLE); 81 | 82 | // Note CCxS bits only writable when CCxE is 0 (channel is disabled) 83 | tcoFraming.init(TIM_OCMode_PWM1, DSHOT_TIMEOUT_COUNTS, TIM_OutputState_Enable); 84 | tciRising.init(TIM_ICPolarity_Rising, 0x0); 85 | tciFalling.init(TIM_ICPolarity_Falling, 0x0, TIM_ICPSC_DIV1, TIM_ICSelection_IndirectTI); 86 | 87 | dmaChannel.setEnabled(ENABLE); 88 | timerCapture.setEnabled(ENABLE); 89 | 90 | #if defined(STM32F0) 91 | nvic_config(TIM2_BRK_UP_TRG_COM_IRQn, 0, ENABLE); 92 | #elif defined(STM32F1) 93 | nvic_config(TIM2_UP_IRQn, 0, 0, ENABLE); 94 | #elif defined(STM32F3) 95 | nvic_config(TIM2_IRQn, 0, 0, ENABLE); 96 | #endif 97 | 98 | while (1) { 99 | gpioLed.toggle(); 100 | mDelay(100); 101 | 102 | if (gotCapture) { 103 | gotCapture = false; 104 | uint16_t packet = 0; 105 | uint16_t throttle = 0; 106 | uint8_t telemRequest = 0; 107 | uint8_t csum = 0; 108 | for (uint8_t i = 0; i < frameLength; i++) { 109 | bool bit = fallCaptures[i] > dshotThreshold; 110 | packet = packet << 1; 111 | packet = packet | bit; 112 | } 113 | 114 | csum = packet & 0xf; 115 | packet = packet >> 4; 116 | telemRequest = packet & 0x1; 117 | throttle = packet >> 1; 118 | 119 | uint16_t csumCheck = 0; 120 | for (uint8_t i = 0; i < 3; i++) { 121 | csumCheck ^= packet; 122 | packet >>= 4; 123 | } 124 | 125 | csumCheck &= 0xf; 126 | 127 | printf("throttle command: %d telem request: %d checksum: %d calculated: %d\r\n", throttle, telemRequest, csum, csumCheck); 128 | 129 | // get statistics on packets/second 130 | //printf(" %d %d %d\r\n", packet, decodedPackets, microseconds); 131 | } 132 | } 133 | 134 | return 0; 135 | } 136 | 137 | // Triggered on start of frame 138 | void cc2Callback(void) { 139 | // enable framing interrupt to indicate end of frame on CC3 match 140 | timerCapture.interruptConfig(TIM_IT_CC3, ENABLE); 141 | // disable this interrupt until we are done with this frame 142 | timerCapture.interruptConfig(TIM_IT_CC2, DISABLE); 143 | } 144 | 145 | // triggered on timeout waiting for next bit in frame (end of frame) 146 | void cc3Callback(void) { 147 | // On STM32F3, you don't have to use CC2 interrupt at the beginning of each frame 148 | // (acheiving one interrupt/frame instead of two interrupts/frame) 149 | // This can be accomplished by: 150 | // 1. leaving CC3 interrupt enabled (always) 151 | // 2. disabling the CC2 interrupt (always) 152 | // 3. using the TIM_SlaveMode_Combined_ResetTrigger instead of TIM_SlaveMode_Reset 153 | // 4. disabling the timer counter on each cc3 interrupt 154 | 155 | // setup interupt to re-enable framing interupt on the next rising edge (start of next frame) 156 | timerCapture.interruptConfig(TIM_IT_CC2, ENABLE); 157 | // Disable framing interrupt until we begin a new frame, 158 | // so that we don't keep firing while waiting for the next frame 159 | timerCapture.interruptConfig(TIM_IT_CC3, DISABLE); 160 | 161 | frameLength = maxFrameLength - DMA1_Channel3->CNDTR; 162 | // choose pulse-width of half the period for threshold of 0/1 decoding 163 | dshotThreshold = timerCapture.peripheral()->CCR1 / 2; 164 | 165 | dmaChannel.setEnabled(DISABLE); 166 | dmaChannel.peripheral()->CNDTR = maxFrameLength; 167 | dmaChannel.setEnabled(ENABLE); 168 | 169 | decodedPackets++; 170 | gotCapture = true; 171 | } 172 | 173 | void initUsart1(void) { 174 | 175 | Gpio gpioUsart1Tx{GPIO_USART1_TX_PORT, GPIO_USART1_TX_PIN}; 176 | Gpio gpioUsart1Rx{GPIO_USART1_RX_PORT, GPIO_USART1_RX_PIN}; 177 | 178 | #if defined(STM32F0) 179 | nvic_config(USART1_IRQn, 0, ENABLE); 180 | #elif defined(STM32F1) || defined(STM32F3) 181 | nvic_config(USART1_IRQn, 0, 0, ENABLE); 182 | #else 183 | #error 184 | #endif 185 | 186 | #if defined(STM32F0) || defined(STM32F3) 187 | gpioUsart1Rx.init(GPIO_Mode_AF); 188 | gpioUsart1Tx.init(GPIO_Mode_AF); 189 | gpioUsart1Rx.configAF(GPIO_USART1_AF); 190 | gpioUsart1Tx.configAF(GPIO_USART1_AF); 191 | #elif defined(STM32F1) 192 | gpioUsart1Rx.init(GPIO_Mode_AF_PP); 193 | gpioUsart1Tx.init(GPIO_Mode_AF_PP); 194 | Gpio::remapConfig(GPIO_USART1_REMAP, ENABLE); 195 | #else 196 | #error 197 | #endif 198 | 199 | uart1.init(USART_BAUDRATE); 200 | uart1.ITConfig(USART_IT_RXNE, ENABLE); 201 | uart1.setEnabled(ENABLE); 202 | uart1.cls(); 203 | } 204 | -------------------------------------------------------------------------------- /src/example/retired/example-spwm-complementary.cpp: -------------------------------------------------------------------------------- 1 | #include "../sine-lookup.h" 2 | #include "stm32lib-conf.h" 3 | #include 4 | #include "arm_math.h" 5 | 6 | // open loop sine wave modulated pwm (3 phase ac) 7 | // duty and resolution/speed are set by genlookup.py (sine-lookup.h) 8 | 9 | #define RESOLUTION 300 10 | #define RESOLUTION1 100 11 | #define RESOLUTION2 200 12 | 13 | Timer &timer = PHASE_OUTPUT_TIMER; 14 | Gpio gpio_m1{ GPIO_PORT_HIGH_U, GPIO_PIN_HIGH_U }; 15 | Gpio gpio_m1n{ GPIO_PORT_LOW_U, GPIO_PIN_LOW_U }; 16 | Gpio gpio_m2{ GPIO_PORT_HIGH_V, GPIO_PIN_HIGH_V }; 17 | Gpio gpio_m2n{ GPIO_PORT_LOW_V, GPIO_PIN_LOW_V }; 18 | Gpio gpio_m3{ GPIO_PORT_HIGH_W, GPIO_PIN_HIGH_W }; 19 | Gpio gpio_m3n{ GPIO_PORT_LOW_W, GPIO_PIN_LOW_W }; 20 | 21 | TimerChannelOutput tco1{&timer, TIM_Channel_1}; 22 | TimerChannelOutput tco2{&timer, TIM_Channel_2}; 23 | TimerChannelOutput tco3{&timer, TIM_Channel_3}; 24 | 25 | uint16_t _field_angle = 0; 26 | 27 | void setFieldAngle(float angle); 28 | 29 | // rotate the field by 360/resolution 30 | // to slow down, increase the resolution 31 | void updateSPWM(void); 32 | 33 | int main() { 34 | configureClocks(1000); 35 | 36 | // Thank you so much: 37 | // https://www.silabs.com/community/mcu/32-bit/knowledge-base.entry.html/2014/04/16/how_to_enable_hardwa-vM9u 38 | // Set floating point coprosessor access mode. */ 39 | SCB->CPACR |= ((3UL << 10*2) | /* set CP10 Full Access */ 40 | 3UL << 11*2) ); /* set CP11 Full Access */ 41 | 42 | mDelay(2000); 43 | 44 | #if defined(STM32F0) 45 | #error 46 | #elif defined(STM32F3) // should work with f0 too 47 | gpio_m1.init(GPIO_Mode_AF); 48 | gpio_m1n.init(GPIO_Mode_AF); 49 | gpio_m2.init(GPIO_Mode_AF); 50 | gpio_m2n.init(GPIO_Mode_AF); 51 | gpio_m3.init(GPIO_Mode_AF); 52 | gpio_m3n.init(GPIO_Mode_AF); 53 | 54 | gpio_m1.configAF(GPIO_AF_TIMER_PHASE_OUTPUT); 55 | gpio_m1n.configAF(GPIO_AF_TIMER_PHASE_OUTPUT); 56 | gpio_m2.configAF(GPIO_AF_TIMER_PHASE_OUTPUT); 57 | gpio_m2n.configAF(GPIO_AF_TIMER_PHASE_OUTPUT); 58 | gpio_m3.configAF(GPIO_AF_TIMER_PHASE_OUTPUT); 59 | gpio_m3n.configAF(GPIO_AF_TIMER_PHASE_OUTPUT); 60 | #else 61 | #error 62 | #endif 63 | 64 | timer.initFreq(48e3); // 48kHz pwm frequency 65 | timer.setDTG(0x18); // ~333ns deadtime insertion 66 | 67 | tco1.init(TIM_OCMode_PWM1, 0, TIM_OutputState_Enable, TIM_OutputNState_Enable); 68 | tco2.init(TIM_OCMode_PWM1, 0, TIM_OutputState_Enable, TIM_OutputNState_Enable); 69 | tco3.init(TIM_OCMode_PWM1, 0, TIM_OutputState_Enable, TIM_OutputNState_Enable); 70 | 71 | timer.setEnabled(ENABLE); 72 | 73 | timer.setMOE(ENABLE); 74 | 75 | while (microseconds < 5000000) { 76 | mDelay(1); 77 | updateSPWM(); 78 | } 79 | 80 | tco1.setEnabledN(DISABLE); 81 | tco2.setEnabledN(DISABLE); 82 | tco3.setEnabledN(DISABLE); 83 | timer.setEnabled(DISABLE); 84 | 85 | return 0; 86 | } 87 | 88 | void setFieldAngle(float angle) { 89 | while (angle > 2 * M_PI) { 90 | angle -= 2 * M_PI; 91 | if (angle < 0) 92 | angle = 0; 93 | } 94 | 95 | _field_angle = angle; 96 | } 97 | 98 | void updateSPWM(void) { 99 | _field_angle += 5; // to speed up, increase the increment 100 | _field_angle = _field_angle % RESOLUTION; 101 | tco1.setDuty(lookup[(_field_angle + RESOLUTION1) % RESOLUTION]); 102 | tco2.setDuty(lookup[_field_angle]); 103 | tco3.setDuty(lookup[(_field_angle + RESOLUTION2) % RESOLUTION]); 104 | } 105 | -------------------------------------------------------------------------------- /src/example/usart/main.cpp: -------------------------------------------------------------------------------- 1 | extern "C" { 2 | #include 3 | #include 4 | #include 5 | } 6 | 7 | #include 8 | 9 | // declared/documented in global.h 10 | // todo remove these irrelevant definitions 11 | // build requires them for now 12 | const uint16_t slow_run_zc_confirmations_required = 14; 13 | uint16_t run_zc_confirmations_required = slow_run_zc_confirmations_required; 14 | volatile bool starting; 15 | const uint16_t startup_commutation_period_ticks = 12000; 16 | const uint16_t startup_zc_confirmations_required = 15; 17 | volatile uint32_t zc_counter; 18 | uint32_t zc_confirmations_required = startup_zc_confirmations_required; 19 | 20 | int main(void) { 21 | system_initialize(); 22 | console_initialize(); 23 | while (1) { 24 | console_write("hello world\r\n"); 25 | } 26 | } 27 | -------------------------------------------------------------------------------- /src/hal/inc/adc.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | void adc_initialize(); 6 | void adc_start(); 7 | volatile uint16_t adc_read_channel(uint8_t channel); 8 | -------------------------------------------------------------------------------- /src/hal/inc/bridge.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | typedef enum { 6 | BRIDGE_STATE_DISABLED, // aka freewheel 7 | BRIDGE_STATE_AUDIO, // arr is adjusted for audio frequency 8 | BRIDGE_STATE_RUN, // arr is fixed at 2048 for throttle resolution 9 | } bridge_state_e; 10 | 11 | typedef enum { 12 | BRIDGE_COMM_STEP0, 13 | BRIDGE_COMM_STEP1, 14 | BRIDGE_COMM_STEP2, 15 | BRIDGE_COMM_STEP3, 16 | BRIDGE_COMM_STEP4, 17 | BRIDGE_COMM_STEP5, 18 | } bridge_comm_step_e; 19 | 20 | extern volatile bridge_state_e g_bridge_state; 21 | extern volatile bridge_comm_step_e g_bridge_comm_step; 22 | 23 | extern volatile uint16_t g_bridge_run_duty; // run mode duty 24 | extern volatile uint8_t g_bridge_audio_duty; // audio mode duty 25 | 26 | void bridge_initialize(); 27 | void bridge_set_state(bridge_state_e state); 28 | 29 | void bridge_enable(); 30 | void bridge_enable_adc_trigger(); 31 | void bridge_disable(); 32 | 33 | // always 4 bit 34 | void bridge_set_audio_duty(uint8_t duty); 35 | 36 | void bridge_set_audio_frequency(uint16_t frequency); 37 | 38 | // always 12 bit (results in ~21kHz pwm @ 48Mhz) 39 | void bridge_set_run_duty(uint16_t duty); 40 | 41 | // setup adc to trigger when TIM1_CNT == ticks 42 | void bridge_setup_adc_trigger(uint16_t ticks); 43 | 44 | void bridge_commutate(); 45 | -------------------------------------------------------------------------------- /src/hal/inc/comparator.h: -------------------------------------------------------------------------------- 1 | // single comparator driver 2 | 3 | #pragma once 4 | 5 | #include 6 | #include 7 | 8 | typedef enum { 9 | COMP_STATE0, 10 | COMP_STATE1, 11 | COMP_STATE2, 12 | COMP_STATE3, 13 | COMP_STATE4, 14 | COMP_STATE5, 15 | } comp_state_e; 16 | 17 | extern volatile comp_state_e g_comparator_state; 18 | 19 | void comparator_initialize(); 20 | 21 | void comparator_set_state(comp_state_e new_state); 22 | 23 | bool comparator_get_output(); 24 | 25 | void comparator_zc_isr_enable(); 26 | void comparator_zc_isr_disable(); 27 | 28 | // comparator bemf zero cross isr, normally you will commutate 29 | void comparator_zc_isr(); 30 | 31 | void comparator_blank(uint32_t nanoseconds); 32 | -------------------------------------------------------------------------------- /src/hal/inc/global.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // todo move this file out of hal and into application src 4 | 5 | #include 6 | #include 7 | 8 | #define ADC_NUM_CHANNELS 8 9 | typedef struct { 10 | uint16_t adc_buffer[ADC_NUM_CHANNELS]; 11 | uint16_t throttle; 12 | uint8_t direction; 13 | uint8_t direction_mode; 14 | uint16_t startup_throttle; 15 | // if pwm signal is present at boot, it will be selected as the throttle signal 16 | // source and throttle commands from the serial interface will be ignored 17 | // if only serial is present, it will be selected and pwm input will be disabled. 18 | bool throttle_valid; 19 | } global_t; 20 | 21 | extern global_t g; 22 | 23 | // the zero crosses required in closed loop mode, this is variable depending on current speed 24 | extern uint16_t run_zc_confirmations_required; 25 | 26 | // if true, we are in open-loop 27 | // we wait for the first zero cross period (2 sequential valid zero crosses) 28 | // timing, then we will enter closed loop 29 | extern volatile bool starting; 30 | 31 | extern const uint16_t slow_run_zc_confirmations_required; 32 | 33 | // open loop startup commutation timer ARR value 34 | // TODO do this in human-readable time (microseconds) 35 | extern const uint16_t startup_commutation_period_ticks; 36 | 37 | // the comparator output must hold for this many checks in a row before we consider it a valid zero-cross 38 | // this can be descreased as rpm increases 39 | extern const uint16_t startup_zc_confirmations_required; 40 | 41 | // REMAINING zero cross checks before we pass 42 | extern volatile uint32_t zc_counter; 43 | 44 | // the zero cross confirmations currently required to pass a zero cross check 45 | extern uint32_t zc_confirmations_required; 46 | -------------------------------------------------------------------------------- /src/hal/inc/pwm-input.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #define PERIOD_MAX_NS 22000000 // 50Hz - 10% 7 | 8 | // dshot150 is 150kbps 9 | #define DIGITAL_PERIOD_MAX_NS 28409 // ie 'bits' incoming faster than 32kHz + 10% 10 | 11 | #define MULTISHOT_DUTY_MAX_NS 27500// 25us + 10% 12 | #define ONESHOT42_DUTY_MAX_NS 92400 // 84us + 10% 13 | #define ONESHOT125_DUTY_MAX_NS 275000 // 250us + 10% 14 | #define CONVENTIONAL_DUTY_MAX_NS 2200000 // 2000us + 10% 15 | 16 | typedef enum { 17 | PWM_INPUT_TYPE_NONE, 18 | PWM_INPUT_TYPE_CONVENTIONAL, 19 | PWM_INPUT_TYPE_ONESHOT125, 20 | PWM_INPUT_TYPE_ONESHOT42, 21 | PWM_INPUT_TYPE_MULTISHOT, 22 | PWM_INPUT_TYPE_UNKNOWN, 23 | } pwm_input_type_t; 24 | 25 | extern pwm_input_type_t g_pwm_type; 26 | 27 | void pwm_input_initialize(); 28 | void pwm_input_deinitialize(); 29 | 30 | // set the timeout in microseconds from last valid signal 31 | void pwm_input_set_timeout(uint32_t timeout); 32 | uint32_t pwm_input_get_timeout(); 33 | 34 | pwm_input_type_t pwm_input_detect_type(); 35 | 36 | uint32_t pwm_input_tick_period_ns(); 37 | 38 | // period in ticks 39 | uint32_t pwm_input_get_period(); 40 | uint32_t pwm_input_get_period_ns(); 41 | 42 | // duty in ticks 43 | uint32_t pwm_input_get_duty(); 44 | uint32_t pwm_input_get_duty_ns(); 45 | 46 | void pwm_input_set_type(pwm_input_type_t input_type); 47 | 48 | // 16-bit throttle 49 | uint16_t pwm_input_get_throttle(); 50 | 51 | // still valid? 52 | // valid if: 53 | // - the signal is in the expected range for the selecte type 54 | // - the timeout period has not elapsed since the last valid signal 55 | bool pwm_input_valid(); 56 | -------------------------------------------------------------------------------- /src/hal/inc/system.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | void system_clock_initialize(); 4 | void system_initialize(); 5 | -------------------------------------------------------------------------------- /src/hal/inc/usart.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | void usart_initialize(uint32_t usart, uint32_t baudrate); 6 | uint16_t usart_read(uint32_t usart, char* byte, uint16_t length); 7 | uint16_t usart_rx_waiting(); 8 | void usart_write(uint32_t usart, const char* data, uint16_t length); 9 | void usart_write_string(uint32_t usart, const char* string); 10 | void usart_write_int(uint32_t usart, uint32_t i); 11 | -------------------------------------------------------------------------------- /src/hal/inc/watchdog.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | void watchdog_start(uint32_t period_ms); 6 | void watchdog_reset(); 7 | -------------------------------------------------------------------------------- /src/hal/src/stm32/common/adc.c: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #if defined(STM32G0) || defined(STM32G4) 9 | #include 10 | #endif 11 | 12 | #include 13 | #include 14 | 15 | void adc_initialize() 16 | { 17 | #if defined(ADC_GPIOA_PINS) 18 | rcc_periph_clock_enable(RCC_GPIOA); 19 | gpio_mode_setup(GPIOA, GPIO_MODE_ANALOG, GPIO_PUPD_NONE, ADC_GPIOA_PINS); 20 | #endif 21 | 22 | #if defined(ADC_GPIOB_PINS) 23 | rcc_periph_clock_enable(RCC_GPIOB); 24 | gpio_mode_setup(GPIOB, GPIO_MODE_ANALOG, GPIO_PUPD_NONE, ADC_GPIOB_PINS); 25 | #endif 26 | 27 | #if defined(ADC_GPIOC_PINS) 28 | rcc_periph_clock_enable(RCC_GPIOC); 29 | gpio_mode_setup(GPIOC, GPIO_MODE_ANALOG, GPIO_PUPD_NONE, ADC_GPIOC_PINS); 30 | #endif 31 | 32 | rcc_periph_clock_enable(RCC_DMA1); 33 | dma_set_peripheral_address(DMA1, DMA_CHANNEL1, (uint32_t)&ADC_DR(ADC_PERIPH)); // set CPAR 34 | dma_set_memory_address(DMA1, DMA_CHANNEL1, (uint32_t)&g.adc_buffer); // set CMAR 35 | dma_set_read_from_peripheral(DMA1, DMA_CHANNEL1); // set DIR 36 | dma_set_memory_size(DMA1, DMA_CHANNEL1, DMA_CCR_MSIZE_16BIT); // set MSIZE 37 | dma_set_peripheral_size(DMA1, DMA_CHANNEL1, DMA_CCR_PSIZE_16BIT); // set PSIZE 38 | dma_enable_memory_increment_mode(DMA1, DMA_CHANNEL1); // set MINC 39 | dma_set_number_of_data(DMA1, DMA_CHANNEL1, sizeof(adc_channels)); // set CNDTR 40 | dma_enable_circular_mode(DMA1, DMA_CHANNEL1); // set CIRC 41 | dma_set_priority(DMA1, DMA_CHANNEL1, DMA_CCR_PL_VERY_HIGH); 42 | dma_enable_channel(DMA1, DMA_CHANNEL1); // set EN 43 | 44 | #if defined(STM32G0) || defined(STM32G4) 45 | DMAMUX_CxCR(DMAMUX1, DMA_CHANNEL1) = ADC_DMAMUX_REQID; 46 | #endif 47 | 48 | rcc_periph_clock_enable(ADC_RCC); 49 | 50 | adc_enable_temperature_sensor(); 51 | #if defined(STM32F3) || defined(STM32G4) 52 | adc_enable_regulator(ADC_PERIPH); 53 | for (long i = 0; i < 100000; i++) 54 | asm("nop"); 55 | // TODO should be ADC_PERIPH 56 | adc_set_clk_prescale(ADC1, ADC_CCR_CKMODE_DIV1); 57 | #endif 58 | adc_set_regular_sequence(ADC_PERIPH, sizeof(adc_channels), adc_channels); 59 | adc_calibrate(ADC_PERIPH); 60 | 61 | // TODO unify/de-duplicate libopencm3 here 62 | // the reading the internal temperature sensor requires a minimum sampling time 63 | #if defined(STM32F0) 64 | adc_set_sample_time_on_all_channels(ADC_PERIPH, ADC_SMPTIME_071DOT5); 65 | #else 66 | // TODO handle ADC overrun, longer sample times mitigate this for now. 67 | // todo increase dma channel priority 68 | adc_set_sample_time_on_all_channels(ADC_PERIPH, ADC_SMPR_SMP_4DOT5CYC); 69 | // TODO restore/report temperature 70 | // adc_set_sample_time(ADC_PERIPH, ADC_CHANNEL_TEMPERATURE, ADC_SMPR_SMP_61DOT5CYC); 71 | #endif 72 | 73 | adc_enable_dma(ADC_PERIPH); 74 | adc_enable_dma_circular_mode(ADC_PERIPH); 75 | adc_power_on(ADC_PERIPH); 76 | } 77 | 78 | void adc_start() 79 | { 80 | adc_start_conversion_regular(ADC_PERIPH); 81 | } 82 | 83 | volatile uint16_t adc_read_channel(uint8_t channel) 84 | { 85 | return g.adc_buffer[channel]; 86 | } 87 | -------------------------------------------------------------------------------- /src/hal/src/stm32/common/bridge.c: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | volatile bridge_state_e g_bridge_state; 12 | volatile bridge_comm_step_e g_bridge_comm_step; 13 | 14 | volatile uint16_t g_bridge_run_duty; // run mode duty 15 | volatile uint8_t g_bridge_audio_duty; // audio mode duty 16 | 17 | typedef struct { 18 | uint32_t CCER; // CCER register value 19 | volatile uint32_t* pwmChannel; // ccr register address to write zero 20 | volatile uint32_t* zeroDutyChannel1; // ccr register address to write zero 21 | volatile uint32_t* zeroDutyChannel2; // ccr register address to write zero 22 | } commutation_state_t; 23 | 24 | #define TIM1_CCR1_ADDR (volatile uint32_t *)(TIM1 + 0x34) 25 | #define TIM1_CCR2_ADDR (volatile uint32_t *)(TIM1 + 0x38) 26 | #define TIM1_CCR3_ADDR (volatile uint32_t *)(TIM1 + 0x3C) 27 | 28 | volatile commutation_state_t bridge_comm_states[6] = { 29 | { 0x00000155, TIM1_CCR1_ADDR, TIM1_CCR2_ADDR, TIM1_CCR3_ADDR }, 30 | { 0x00000515, TIM1_CCR1_ADDR, TIM1_CCR2_ADDR, TIM1_CCR3_ADDR }, 31 | { 0x00000551, TIM1_CCR2_ADDR, TIM1_CCR1_ADDR, TIM1_CCR3_ADDR }, 32 | { 0x00000155, TIM1_CCR2_ADDR, TIM1_CCR1_ADDR, TIM1_CCR3_ADDR }, 33 | { 0x00000515, TIM1_CCR3_ADDR, TIM1_CCR1_ADDR, TIM1_CCR2_ADDR }, 34 | { 0x00000551, TIM1_CCR3_ADDR, TIM1_CCR1_ADDR, TIM1_CCR2_ADDR }, 35 | }; 36 | 37 | void bridge_gpio_setup( 38 | enum rcc_periph_clken rcc, 39 | uint32_t port, 40 | uint16_t pin, 41 | uint8_t af) 42 | { 43 | rcc_periph_clock_enable(rcc); 44 | gpio_mode_setup(port, GPIO_MODE_AF, GPIO_PUPD_NONE, pin); 45 | gpio_set_af(port, af, pin); 46 | } 47 | 48 | void bridge_initialize() 49 | { 50 | #if defined(STSPIN) 51 | // initalize bridge for stspin 52 | rcc_periph_clock_enable(RCC_GPIOF); 53 | gpio_mode_setup(GPIOF, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO6 | GPIO7); 54 | gpio_set(GPIOF, GPIO6 | GPIO7); 55 | #endif 56 | 57 | // TODO optimize 58 | bridge_gpio_setup( 59 | BRIDGE_HI_A_GPIO_RCC, 60 | BRIDGE_HI_A_GPIO_PORT, 61 | BRIDGE_HI_A_GPIO_PIN, 62 | BRIDGE_HI_A_GPIO_AF); 63 | 64 | bridge_gpio_setup( 65 | BRIDGE_HI_B_GPIO_RCC, 66 | BRIDGE_HI_B_GPIO_PORT, 67 | BRIDGE_HI_B_GPIO_PIN, 68 | BRIDGE_HI_B_GPIO_AF); 69 | 70 | bridge_gpio_setup( 71 | BRIDGE_HI_C_GPIO_RCC, 72 | BRIDGE_HI_C_GPIO_PORT, 73 | BRIDGE_HI_C_GPIO_PIN, 74 | BRIDGE_HI_C_GPIO_AF); 75 | 76 | bridge_gpio_setup( 77 | BRIDGE_LO_A_GPIO_RCC, 78 | BRIDGE_LO_A_GPIO_PORT, 79 | BRIDGE_LO_A_GPIO_PIN, 80 | BRIDGE_LO_A_GPIO_AF); 81 | 82 | bridge_gpio_setup( 83 | BRIDGE_LO_B_GPIO_RCC, 84 | BRIDGE_LO_B_GPIO_PORT, 85 | BRIDGE_LO_B_GPIO_PIN, 86 | BRIDGE_LO_B_GPIO_AF); 87 | 88 | bridge_gpio_setup( 89 | BRIDGE_LO_C_GPIO_RCC, 90 | BRIDGE_LO_C_GPIO_PORT, 91 | BRIDGE_LO_C_GPIO_PIN, 92 | BRIDGE_LO_C_GPIO_AF); 93 | 94 | // initialize timer 95 | rcc_periph_clock_enable(RCC_TIM1); 96 | 97 | timer_set_prescaler(TIM1, 0); 98 | timer_set_period(TIM1, 2048); 99 | TIM1_CCR4 = 1024; 100 | timer_set_oc_value(TIM1, TIM_OC1, 0); 101 | timer_set_oc_value(TIM1, TIM_OC2, 0); 102 | timer_set_oc_value(TIM1, TIM_OC3, 0); 103 | timer_set_oc_mode(TIM1, TIM_OC1, TIM_OCM_PWM1); 104 | timer_set_oc_mode(TIM1, TIM_OC2, TIM_OCM_PWM1); 105 | timer_set_oc_mode(TIM1, TIM_OC3, TIM_OCM_PWM1); 106 | timer_enable_oc_preload(TIM1, TIM_OC1); 107 | timer_enable_oc_preload(TIM1, TIM_OC2); 108 | timer_enable_oc_preload(TIM1, TIM_OC3); 109 | timer_enable_oc_preload(TIM1, TIM_OC4); 110 | timer_enable_oc_output(TIM1, TIM_OC1); 111 | timer_enable_oc_output(TIM1, TIM_OC1N); 112 | timer_enable_oc_output(TIM1, TIM_OC2); 113 | timer_enable_oc_output(TIM1, TIM_OC2N); 114 | timer_enable_oc_output(TIM1, TIM_OC3); 115 | timer_enable_oc_output(TIM1, TIM_OC3N); 116 | 117 | timer_set_deadtime(TIM1, 0x40); 118 | 119 | timer_set_enabled_off_state_in_idle_mode(TIM1); // get this bit wrong and blow your bridge 120 | timer_set_enabled_off_state_in_run_mode(TIM1); 121 | 122 | timer_enable_counter(TIM1); 123 | 124 | // TODO lock registers 125 | } 126 | 127 | void bridge_set_state(bridge_state_e new_state) 128 | { 129 | cm3_assert(g_bridge_state != new_state); 130 | 131 | switch (new_state) { 132 | case BRIDGE_STATE_DISABLED: 133 | bridge_disable(); 134 | break; 135 | case BRIDGE_STATE_AUDIO: 136 | bridge_set_audio_duty(0); 137 | TIM1_ARR = 0xff; 138 | TIM1_CCR4 = 0x80; // synchronize adc between pwms 139 | g_bridge_state = BRIDGE_STATE_AUDIO; 140 | break; 141 | case BRIDGE_STATE_RUN: 142 | bridge_set_run_duty(0); 143 | TIM1_PSC = 1; 144 | TIM1_ARR = 2047; 145 | TIM1_CCR4 = 1500; 146 | g_bridge_state = BRIDGE_STATE_RUN; 147 | break; 148 | default: 149 | cm3_assert_not_reached(); 150 | break; 151 | } 152 | } 153 | 154 | void bridge_enable() { 155 | cm3_assert(g_bridge_state == BRIDGE_STATE_DISABLED); 156 | timer_enable_break_main_output(TIM1); // shouldn't do/be part of f0 aapi? 157 | } 158 | 159 | void bridge_disable() { 160 | timer_disable_break_main_output(TIM1); // shouldn't do/be part of f0 aapi? 161 | bridge_set_run_duty(0); 162 | bridge_set_audio_duty(0); 163 | g_bridge_state = BRIDGE_STATE_DISABLED; 164 | } 165 | 166 | // for use in audio mode - duty is always 4 bit (0-16) 167 | // duty may never exceed 12.5% 168 | void bridge_set_audio_duty(uint8_t duty) { 169 | cm3_assert(g_bridge_state == BRIDGE_STATE_AUDIO); 170 | 171 | g_bridge_audio_duty = duty & 0xf; 172 | TIM1_CCR1 = g_bridge_audio_duty; 173 | } 174 | 175 | void bridge_set_audio_frequency(uint16_t frequency) { 176 | cm3_assert(g_bridge_state == BRIDGE_STATE_AUDIO); 177 | 178 | // frequency = ahb clock / prescaler / arr 179 | // psc = ahb clock / arr / frequency 180 | uint16_t psc = rcc_ahb_frequency / TIM1_ARR / frequency; 181 | TIM1_PSC = psc; 182 | } 183 | 184 | // for use in run mode - duty is always 12bit (0-2047) 185 | void bridge_set_run_duty(uint16_t duty) 186 | { 187 | cm3_assert(g_bridge_state == BRIDGE_STATE_RUN); 188 | 189 | if (duty > 512) { 190 | duty = 512; // clamp duty to ~25% max for testing 191 | } 192 | 193 | // set adc sample trigger to 75% period when duty < 50% and 194 | // 25% period when duty > 50% 195 | if (duty > 0x400) { 196 | bridge_setup_adc_trigger(0x200); 197 | } else { 198 | bridge_setup_adc_trigger(0x600); 199 | } 200 | 201 | g_bridge_run_duty = duty; 202 | // *(bridge_comm_states[g_bridge_comm_step].pwmChannel) = g_bridge_run_duty; 203 | } 204 | 205 | void bridge_commutate() { 206 | cm3_assert(state == BRIDGE_STATE_RUN); 207 | 208 | TIM_EGR(TIM1) |= 0x20; // set bit 5 in event generation register, generate commutate event 209 | 210 | // wait for the bit to be cleared by hardware 211 | // while (TIM_EGR(TIM1) & 0x20); 212 | 213 | // preload next commutation state 214 | g_bridge_comm_step++; 215 | g_bridge_comm_step %= 6; 216 | 217 | *(bridge_comm_states[g_bridge_comm_step].zeroDutyChannel1) = 0; 218 | *(bridge_comm_states[g_bridge_comm_step].zeroDutyChannel2) = 0; 219 | *(bridge_comm_states[g_bridge_comm_step].pwmChannel) = g_bridge_run_duty; 220 | TIM_CCER(TIM1) = bridge_comm_states[g_bridge_comm_step].CCER; 221 | } 222 | 223 | void bridge_enable_adc_trigger() 224 | { 225 | // setup adc synchronization 226 | nvic_enable_irq(NVIC_TIM1_CC_IRQ); 227 | TIM_DIER(TIM1) |= TIM_DIER_CC4IE; 228 | } 229 | 230 | void bridge_setup_adc_trigger(uint16_t ticks) 231 | { 232 | TIM1_CCR4 = ticks; 233 | } 234 | -------------------------------------------------------------------------------- /src/hal/src/stm32/common/comparator-external.c: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | 13 | volatile comp_state_e g_comparator_state; 14 | 15 | typedef struct 16 | { 17 | uint32_t port; // gpio port 18 | uint16_t pin; // gpio pin 19 | uint32_t exti; // exti line 20 | enum exti_trigger_type trigger; // exti trigger edge (rising or falling) 21 | uint8_t nvic; // nvic irq 22 | 23 | } comparator_state_t; 24 | 25 | volatile comparator_state_t comparator_states[6] = { 26 | GPIOF, GPIO1, EXTI1, EXTI_TRIGGER_FALLING, NVIC_EXTI0_1_IRQ, 27 | GPIOB, GPIO1, EXTI1, EXTI_TRIGGER_RISING, NVIC_EXTI0_1_IRQ, 28 | GPIOF, GPIO0, EXTI0, EXTI_TRIGGER_FALLING, NVIC_EXTI0_1_IRQ, 29 | GPIOF, GPIO1, EXTI1, EXTI_TRIGGER_RISING, NVIC_EXTI0_1_IRQ, 30 | GPIOB, GPIO1, EXTI1, EXTI_TRIGGER_FALLING, NVIC_EXTI0_1_IRQ, 31 | GPIOF, GPIO0, EXTI0, EXTI_TRIGGER_RISING, NVIC_EXTI0_1_IRQ, 32 | }; 33 | 34 | uint32_t comparator_blank_tick_period_ns; 35 | 36 | // TODO move nvic isrs to target directory 37 | 38 | // linked for f0 39 | void exti0_1_isr() { 40 | comparator_zc_isr(); // user code 41 | exti_reset_request(comparator_states[g_comparator_state].exti); 42 | } 43 | 44 | void comparator_blank_complete_isr() 45 | { 46 | comparator_zc_isr_enable(); 47 | } 48 | 49 | void comparator_initialize() 50 | { 51 | #if defined(STM32F3) 52 | rcc_periph_clock_enable(RCC_SYSCFG); 53 | #elif defined(STM32F0) 54 | rcc_periph_clock_enable(RCC_SYSCFG_COMP); 55 | #else 56 | # error 57 | #endif 58 | 59 | rcc_periph_clock_enable(COMPARATOR_A_GPIO_RCC); 60 | rcc_periph_clock_enable(COMPARATOR_B_GPIO_RCC); 61 | rcc_periph_clock_enable(COMPARATOR_C_GPIO_RCC); 62 | 63 | // gpio_mode_setup(COMPARATOR_A_GPIO_PORT, GPIO_MODE_INPUT, GPIO_PUPD_NONE, COMPARATOR_A_GPIO_PIN); 64 | // gpio_mode_setup(COMPARATOR_B_GPIO_PORT, GPIO_MODE_INPUT, GPIO_PUPD_NONE, COMPARATOR_B_GPIO_PIN); 65 | // gpio_mode_setup(COMPARATOR_C_GPIO_PORT, GPIO_MODE_INPUT, GPIO_PUPD_NONE, COMPARATOR_C_GPIO_PIN); 66 | gpio_mode_setup(COMPARATOR_A_GPIO_PORT, GPIO_MODE_AF, GPIO_PUPD_NONE, COMPARATOR_A_GPIO_PIN); 67 | gpio_mode_setup(COMPARATOR_B_GPIO_PORT, GPIO_MODE_AF, GPIO_PUPD_NONE, COMPARATOR_B_GPIO_PIN); 68 | gpio_mode_setup(COMPARATOR_C_GPIO_PORT, GPIO_MODE_AF, GPIO_PUPD_NONE, COMPARATOR_C_GPIO_PIN); 69 | 70 | // enable comparator (there is a startup delay) 71 | comparator_set_state(COMP_STATE0); 72 | 73 | // setup comparator blanking timer, this is not used in normal operation 74 | // testing purposes only 75 | rcc_periph_clock_enable(COMPARATOR_BLANK_TIMER_RCC); 76 | nvic_enable_irq(COMPARATOR_BLANK_IRQ); 77 | timer_one_shot_mode(COMPARATOR_BLANK_TIMER); 78 | timer_enable_irq(COMPARATOR_BLANK_TIMER, TIM_DIER_UIE); 79 | comparator_blank_tick_period_ns = 1000000000 / (rcc_ahb_frequency / (TIM_PSC(COMPARATOR_BLANK_TIMER) + 1)); 80 | } 81 | 82 | void comparator_set_state(comp_state_e new_state) 83 | { 84 | // de-initialize current state 85 | //comparator_zc_isr_disable(); 86 | 87 | g_comparator_state = new_state%6; 88 | exti_select_source(comparator_states[g_comparator_state].exti, comparator_states[g_comparator_state].port); 89 | exti_set_trigger(comparator_states[g_comparator_state].exti, comparator_states[g_comparator_state].trigger); 90 | } 91 | 92 | void comparator_zc_isr_enable() 93 | { 94 | exti_reset_request(comparator_states[g_comparator_state].exti); 95 | exti_enable_request(comparator_states[g_comparator_state].exti); 96 | nvic_enable_irq(comparator_states[g_comparator_state].nvic); 97 | } 98 | 99 | void comparator_zc_isr_disable() 100 | { 101 | nvic_disable_irq(comparator_states[g_comparator_state].nvic); 102 | nvic_clear_pending_irq(comparator_states[g_comparator_state].nvic); 103 | exti_disable_request(comparator_states[g_comparator_state].exti); 104 | exti_reset_request(comparator_states[g_comparator_state].exti); 105 | } 106 | 107 | void comparator_blank(uint32_t nanoseconds) 108 | { 109 | comparator_zc_isr_disable(); 110 | timer_set_period(COMPARATOR_BLANK_TIMER, nanoseconds / comparator_blank_tick_period_ns); 111 | timer_enable_counter(COMPARATOR_BLANK_TIMER); 112 | } 113 | 114 | bool comparator_get_output() 115 | { 116 | if (comparator_states[g_comparator_state].trigger == EXTI_TRIGGER_RISING) { 117 | return gpio_get(comparator_states[g_comparator_state].port, comparator_states[g_comparator_state].pin); 118 | } else { 119 | return !gpio_get(comparator_states[g_comparator_state].port, comparator_states[g_comparator_state].pin); 120 | } 121 | } 122 | 123 | void comparator_unblank_isr() 124 | { 125 | comparator_zc_isr_enable(); 126 | } 127 | -------------------------------------------------------------------------------- /src/hal/src/stm32/common/comparator-internal.c: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | 14 | #if defined(STM32F0) 15 | #define COMP_HYST COMP_CSR_HYST_NO 16 | #elif defined(STM32F3) 17 | #define COMP_HYST COMP_CSR_HYST_NONE 18 | #endif 19 | 20 | volatile comp_state_e g_comparator_state; 21 | 22 | volatile uint16_t comparator_states[6] = { 23 | COMP_HYST | 0x041, 24 | COMP_HYST | 0x851, 25 | COMP_HYST | 0x061, 26 | COMP_HYST | 0x841, 27 | COMP_HYST | 0x051, 28 | COMP_HYST | 0x861, 29 | }; 30 | 31 | uint32_t comparator_blank_tick_period_ns; 32 | 33 | void comparator_blank_complete_isr() 34 | { 35 | comparator_zc_isr_enable(); 36 | } 37 | 38 | void comparator_initialize() 39 | { 40 | rcc_periph_clock_enable(RCC_GPIOA); 41 | 42 | gpio_mode_setup(GPIOA, GPIO_MODE_ANALOG, GPIO_PUPD_NONE, GPIO0 | GPIO1 | GPIO4 | GPIO5); 43 | 44 | #if defined(STM32F3) 45 | rcc_periph_clock_enable(RCC_SYSCFG); 46 | #elif defined(STM32F0) 47 | rcc_periph_clock_enable(RCC_SYSCFG_COMP); 48 | #else 49 | #error 50 | #endif 51 | 52 | // enable comparator (there is a startup delay) 53 | comparator_set_state(COMP_STATE1); 54 | 55 | // setup comparator edge interrupt (zero cross) 56 | exti_set_trigger(EXTI21, EXTI_TRIGGER_BOTH); 57 | 58 | nvic_enable_irq(COMPARATOR_ZC_IRQ); 59 | 60 | // setup comparator blanking timer, this is not used in normal operation 61 | // testing purposes only 62 | rcc_periph_clock_enable(COMPARATOR_BLANK_TIMER_RCC); 63 | nvic_enable_irq(COMPARATOR_BLANK_IRQ); 64 | timer_one_shot_mode(COMPARATOR_BLANK_TIMER); 65 | timer_enable_irq(COMPARATOR_BLANK_TIMER, TIM_DIER_UIE); 66 | comparator_blank_tick_period_ns = 1000000000 / (rcc_ahb_frequency / (TIM_PSC(COMPARATOR_BLANK_TIMER) + 1)); 67 | } 68 | 69 | void comparator_set_state(comp_state_e new_state) 70 | { 71 | g_comparator_state = new_state%6; 72 | COMP_CSR(COMP1) = comparator_states[g_comparator_state]; 73 | } 74 | 75 | void comparator_zc_isr_enable() 76 | { 77 | exti_reset_request(EXTI21); 78 | exti_enable_request(EXTI21); 79 | } 80 | 81 | void comparator_zc_isr_disable() 82 | { 83 | exti_disable_request(EXTI21); 84 | } 85 | 86 | void comparator_blank(uint32_t nanoseconds) 87 | { 88 | comparator_zc_isr_disable(); 89 | timer_set_period(COMPARATOR_BLANK_TIMER, nanoseconds / comparator_blank_tick_period_ns); 90 | timer_enable_counter(COMPARATOR_BLANK_TIMER); 91 | } 92 | 93 | bool comparator_get_output() 94 | { 95 | return COMP_CSR(COMP1) & COMP_CSR_OUT; 96 | } 97 | 98 | void comparator_unblank_isr() 99 | { 100 | comparator_zc_isr_enable(); 101 | } 102 | -------------------------------------------------------------------------------- /src/hal/src/stm32/common/global.c: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | global_t g; 4 | -------------------------------------------------------------------------------- /src/hal/src/stm32/common/pwm-input.c: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | pwm_input_type_t g_pwm_type; 12 | 13 | static uint16_t psc = 0; 14 | static uint32_t arr = 0; 15 | 16 | void pwm_input_set_counter_frequency(uint32_t new_frequency) 17 | { 18 | psc = (rcc_ahb_frequency / new_frequency) - 1; 19 | timer_set_prescaler(PWM_INPUT_TIMER, psc); 20 | } 21 | 22 | // timeout is measured from the rising edge (beginning) of the last valid signal 23 | // counter frequency must be configured first 24 | void pwm_input_set_timeout_period_ns(uint32_t new_period) 25 | { 26 | TIM_ARR(PWM_INPUT_TIMER) = new_period / pwm_input_tick_period_ns(); 27 | } 28 | 29 | void pwm_input_initialize() 30 | { 31 | rcc_periph_clock_enable(PWM_INPUT_GPIO_RCC); 32 | gpio_mode_setup(PWM_INPUT_GPIO_PORT, GPIO_MODE_AF, GPIO_PUPD_NONE, PWM_INPUT_GPIO_PIN); 33 | gpio_set_af(PWM_INPUT_GPIO_PORT, PWM_INPUT_GPIO_AF, PWM_INPUT_GPIO_PIN); 34 | 35 | rcc_periph_clock_enable(PWM_INPUT_TIMER_RCC); // enable timer clock 36 | 37 | // set timer prescaler so that timer counter runs at 2.5MHz 38 | // this provides a ~30ms period 39 | pwm_input_set_counter_frequency(2000000); 40 | 41 | // arr = period / tick time 42 | // TODO setup for update event to invalidate data due to timeout 43 | // arr = PERIOD_MAX_NS / pwm_input_tick_period_ns(); 44 | arr = 0xffff; 45 | 46 | // trigger input 1 will: 47 | // 1. capture the signal period in CCR1 (rising edge - rising edge time) 48 | // 2. reset the counter (the timer keeps running) 49 | // stretch clock with larger dividers in order to time longer signals without overruns 50 | timer_set_period(PWM_INPUT_TIMER, arr); // set ARR 51 | timer_ic_set_input(PWM_INPUT_TIMER, PWM_INPUT_TIMER_IC_ID_RISE, PWM_INPUT_TIMER_IC_TRIGGER); // set both input channels to trigger input 1 52 | timer_ic_set_input(PWM_INPUT_TIMER, PWM_INPUT_TIMER_IC_ID_FALL, PWM_INPUT_TIMER_IC_TRIGGER); 53 | // set second input channel trigger polarity to falling (rising is default, first input channel is rising) 54 | // input/output configurations are on the same register (so we use set oc_polarity..) 55 | timer_set_oc_polarity_low(PWM_INPUT_TIMER, PWM_INPUT_TIMER_OC_ID_FALL); 56 | 57 | timer_ic_enable(PWM_INPUT_TIMER, PWM_INPUT_TIMER_IC_ID_RISE); 58 | timer_ic_enable(PWM_INPUT_TIMER, PWM_INPUT_TIMER_IC_ID_FALL); 59 | 60 | // set slave mode, reset counter on trigger 61 | //timer_slave_set_mode(PWM_INPUT_TIMER, PWM_INPUT_TIMER_SLAVE_MODE); 62 | 63 | // set slave mode trigger to trigger input 1 64 | timer_slave_set_trigger(PWM_INPUT_TIMER, PWM_INPUT_TIMER_SLAVE_TRIGGER); 65 | 66 | // disable slave trigger event from setting UIF 67 | TIM_CR1(PWM_INPUT_TIMER) |= TIM_CR1_URS; 68 | 69 | // update event indiates throttle timeout 70 | nvic_enable_irq(PWM_INPUT_TIMER_IRQ); 71 | TIM_DIER(PWM_INPUT_TIMER) |= TIM_DIER_UIE; 72 | 73 | timer_enable_counter(PWM_INPUT_TIMER); // set CEN 74 | 75 | } 76 | 77 | void pwm_input_set_type(pwm_input_type_t new_type) 78 | { 79 | switch (new_type) { 80 | case PWM_INPUT_TYPE_NONE: 81 | case PWM_INPUT_TYPE_CONVENTIONAL: 82 | pwm_input_set_counter_frequency(2000000); 83 | pwm_input_set_timeout_period_ns(25000000); 84 | break; 85 | default: 86 | pwm_input_set_counter_frequency(48000000); 87 | pwm_input_set_timeout_period_ns(3000000); 88 | break; 89 | } 90 | g_pwm_type = new_type; 91 | } 92 | 93 | pwm_input_type_t pwm_input_detect_type() 94 | { 95 | uint32_t duty = pwm_input_get_duty_ns(); 96 | uint32_t period = pwm_input_get_period_ns(); 97 | 98 | // if the signal frequency is > 32kHz, it's a digital protocol 99 | // if the signal period is < 1/32kHz = 31250 nanoseconds, it's a digital protocol 100 | if (period == 0) { 101 | return PWM_INPUT_TYPE_NONE; 102 | } else if (period < DIGITAL_PERIOD_MAX_NS) { 103 | return PWM_INPUT_TYPE_UNKNOWN; // no support for dshot or multishot yet 104 | } else { 105 | // otherwise, it's an 'analog' pwm protocol 106 | // we can determine thdutye type now by looking at the duty 107 | if (duty == 0) { 108 | return PWM_INPUT_TYPE_NONE; 109 | } else if (duty < MULTISHOT_DUTY_MAX_NS) { 110 | return PWM_INPUT_TYPE_MULTISHOT; 111 | } else if (duty < ONESHOT42_DUTY_MAX_NS) { 112 | return PWM_INPUT_TYPE_ONESHOT42; 113 | } else if (duty < ONESHOT125_DUTY_MAX_NS) { 114 | return PWM_INPUT_TYPE_ONESHOT125; 115 | } else if (duty < CONVENTIONAL_DUTY_MAX_NS) { 116 | return PWM_INPUT_TYPE_CONVENTIONAL; 117 | } else { 118 | return PWM_INPUT_TYPE_UNKNOWN; 119 | } 120 | } 121 | } 122 | 123 | uint32_t pwm_input_tick_period_ns() 124 | { 125 | // 1e9 / clock frequency 126 | return 1000000000 / (rcc_ahb_frequency / (psc + 1)); 127 | } 128 | 129 | uint32_t pwm_input_get_period() 130 | { 131 | return PWM_INPUT_PERIOD_CCR(PWM_INPUT_TIMER); 132 | } 133 | 134 | uint32_t pwm_input_get_period_ns() 135 | { 136 | // tick counts * tick period in nanoseconds 137 | return pwm_input_get_period() * pwm_input_tick_period_ns(); 138 | } 139 | 140 | uint32_t pwm_input_get_duty() 141 | { 142 | return PWM_INPUT_DUTY_CCR(PWM_INPUT_TIMER); 143 | } 144 | 145 | uint32_t pwm_input_get_duty_ns() 146 | { 147 | // tick counts * tick period in nanoseconds 148 | return pwm_input_get_duty() * pwm_input_tick_period_ns(); 149 | } 150 | 151 | uint16_t pwm_input_throttle_scale_ns(uint32_t duty_min_ns, uint32_t duty_max_ns) 152 | { 153 | uint32_t duty = pwm_input_get_duty_ns(); 154 | 155 | if (duty < duty_min_ns) { 156 | return 0; 157 | } 158 | 159 | if (duty > duty_max_ns) { 160 | return 0x800; 161 | } 162 | 163 | return ((0x800 * (uint64_t)(duty - duty_min_ns))) / ((duty_max_ns - duty_min_ns)); 164 | } 165 | 166 | uint16_t pwm_input_get_throttle() 167 | { 168 | switch(g_pwm_type) { 169 | case PWM_INPUT_TYPE_CONVENTIONAL: 170 | return pwm_input_throttle_scale_ns(1000000, 2000000); 171 | case PWM_INPUT_TYPE_ONESHOT125: 172 | return pwm_input_throttle_scale_ns(125000, 250000); 173 | case PWM_INPUT_TYPE_ONESHOT42: 174 | return pwm_input_throttle_scale_ns(42000, 84000); 175 | case PWM_INPUT_TYPE_MULTISHOT: 176 | return pwm_input_throttle_scale_ns(5000, 25000); 177 | case PWM_INPUT_TYPE_NONE: 178 | case PWM_INPUT_TYPE_UNKNOWN: 179 | default: 180 | return 0; 181 | } 182 | } 183 | 184 | // TODO handle noise + unexpected signals 185 | // maybe try enabling internal pulldowns 186 | bool pwm_input_valid() 187 | { 188 | if (TIM_SR(PWM_INPUT_TIMER) & TIM_SR_UIF) { 189 | TIM_SR(PWM_INPUT_TIMER) &= ~TIM_SR_UIF; 190 | return false; 191 | } 192 | return true; 193 | } 194 | -------------------------------------------------------------------------------- /src/hal/src/stm32/common/system.c: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | void system_initialize() 6 | { 7 | system_clock_initialize(); 8 | } 9 | 10 | void system_clock_initialize() 11 | { 12 | #if defined(STM32F0) 13 | rcc_clock_setup_in_hsi_out_48mhz(); 14 | #elif defined(STM32F3) 15 | rcc_clock_setup_pll(&rcc_hsi_configs[1]); 16 | #elif defined(STM32G4) 17 | rcc_clock_setup(&rcc_clock_config[RCC_CLOCK_CONFIG_HSI_PLL_170MHZ]); 18 | // enable rcc peripheral clock for dmamux 19 | *(uint32_t*)0x40021048 |= 0b100; 20 | #else 21 | #error "system not defined" 22 | #endif 23 | } 24 | -------------------------------------------------------------------------------- /src/hal/src/stm32/common/usart.c: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #if defined(STM32G4) 10 | #include 11 | #endif 12 | 13 | #include 14 | 15 | #include 16 | 17 | // circular buffer 18 | // TODO per instance buffers 19 | // do not change, implementation takes advantage of integer overflow 20 | const uint16_t _tx_buffer_size = 256; 21 | static uint8_t _tx_buffer[256]; 22 | volatile uint8_t _tx_head; 23 | volatile uint8_t _tx_tail; 24 | volatile uint8_t _dma_transfer_count; 25 | 26 | const uint16_t _rx_buffer_size = 256; 27 | static uint8_t _rx_buffer[256]; 28 | volatile uint8_t _rx_head; 29 | volatile uint8_t _rx_tail; 30 | 31 | void usart_setup_dma_rx(uint32_t usart) 32 | { 33 | rcc_periph_clock_enable(CONSOLE_RX_DMA_RCC); // enable dma clock 34 | DMA_CPAR(CONSOLE_RX_DMA, CONSOLE_RX_DMA_CHANNEL) = (uint32_t)&USART_RDR(usart); // set CPAR 35 | dma_set_read_from_peripheral(CONSOLE_RX_DMA, CONSOLE_RX_DMA_CHANNEL); // set DIR 36 | dma_set_memory_size(CONSOLE_RX_DMA, CONSOLE_RX_DMA_CHANNEL, DMA_CCR_MSIZE_8BIT); // set MSIZE 37 | dma_set_peripheral_size(CONSOLE_RX_DMA, CONSOLE_RX_DMA_CHANNEL, DMA_CCR_PSIZE_16BIT); // set PSIZE 38 | dma_enable_memory_increment_mode(CONSOLE_RX_DMA, CONSOLE_RX_DMA_CHANNEL); // set MINC 39 | dma_set_priority(DMA1, DMA_CHANNEL1, DMA_CCR_PL_VERY_HIGH); 40 | 41 | DMA_CCR(CONSOLE_RX_DMA, CONSOLE_RX_DMA_CHANNEL) |= DMA_CCR_CIRC; 42 | DMA_CNDTR(CONSOLE_RX_DMA, CONSOLE_RX_DMA_CHANNEL) = 256; 43 | DMA_CMAR(CONSOLE_RX_DMA, CONSOLE_RX_DMA_CHANNEL) = (uint32_t)(_rx_buffer); 44 | USART_CR3(usart) |= USART_CR3_DMAR; 45 | DMA_CCR(CONSOLE_RX_DMA, CONSOLE_RX_DMA_CHANNEL) |= DMA_CCR_EN; 46 | 47 | #if defined(STM32G4) 48 | DMAMUX_CxCR(DMAMUX1, CONSOLE_RX_DMA_CHANNEL) = CONSOLE_RX_DMAMUX_REQID; 49 | #endif 50 | } 51 | 52 | void usart_setup_dma_tx(uint32_t usart) 53 | { 54 | rcc_periph_clock_enable(CONSOLE_TX_DMA_RCC); // enable dma clock 55 | DMA_CPAR(CONSOLE_TX_DMA, CONSOLE_TX_DMA_CHANNEL) = (uint32_t)&USART_TDR(usart); // set CPAR 56 | dma_set_read_from_memory(CONSOLE_TX_DMA, CONSOLE_TX_DMA_CHANNEL); // set DIR 57 | dma_set_memory_size(CONSOLE_TX_DMA, CONSOLE_TX_DMA_CHANNEL, DMA_CCR_MSIZE_8BIT); // set MSIZE 58 | dma_set_peripheral_size(CONSOLE_TX_DMA, CONSOLE_TX_DMA_CHANNEL, DMA_CCR_PSIZE_16BIT); // set PSIZE 59 | dma_enable_memory_increment_mode(CONSOLE_TX_DMA, CONSOLE_TX_DMA_CHANNEL); // set MINC 60 | dma_set_priority(DMA1, DMA_CHANNEL1, DMA_CCR_PL_LOW); 61 | 62 | USART_CR3(usart) |= USART_CR3_DMAT; 63 | DMA_CCR(CONSOLE_TX_DMA, CONSOLE_TX_DMA_CHANNEL) |= DMA_CCR_TCIE; 64 | 65 | // lowest priority 66 | nvic_set_priority(CONSOLE_TX_DMA_IRQ, 0b11000000); 67 | nvic_enable_irq(CONSOLE_TX_DMA_IRQ); 68 | 69 | #if defined(STM32G4) 70 | DMAMUX_CxCR(DMAMUX1, CONSOLE_TX_DMA_CHANNEL) = CONSOLE_TX_DMAMUX_REQID; 71 | #endif 72 | } 73 | 74 | void usart_initialize(uint32_t usart, uint32_t baudrate) 75 | { 76 | // enable transmitter + receiver 77 | USART_CR1(usart) |= USART_CR1_TE | USART_CR1_RE; 78 | usart_set_baudrate(usart, baudrate); 79 | usart_enable(usart); 80 | 81 | usart_setup_dma_tx(usart); 82 | usart_setup_dma_rx(usart); 83 | } 84 | 85 | uint16_t usart_read(uint32_t usart, char* byte, uint16_t length) { 86 | uint16_t i = 0; 87 | 88 | uint16_t read_bytes = usart_rx_waiting(); 89 | 90 | if (length < read_bytes) { 91 | read_bytes = length; 92 | } 93 | 94 | while(i++ < read_bytes) { 95 | *byte++ = _rx_buffer[_rx_head++]; 96 | } 97 | 98 | return read_bytes; 99 | } 100 | 101 | uint16_t usart_rx_waiting() { 102 | return 256 - DMA_CNDTR(CONSOLE_RX_DMA, CONSOLE_RX_DMA_CHANNEL) - _rx_head; 103 | } 104 | 105 | // how many bytes are waiting to be shifted out 106 | uint8_t usart_tx_waiting() { 107 | return _tx_head - _tx_tail; 108 | } 109 | 110 | // number of bytes waiting or number of bytes till the end of the buffer 111 | // whichever is smallest 112 | uint8_t usart_tx_dma_waiting() { 113 | if (_tx_head >= _tx_tail) { 114 | return _tx_head - _tx_tail; 115 | } else { 116 | return _tx_buffer_size - _tx_tail; 117 | } 118 | } 119 | 120 | // how many bytes available to write to buffer 121 | uint8_t usart_tx_available() { 122 | return _tx_buffer_size - usart_tx_waiting() - 1; // must subtract one for tracking empty/patrial[255]/full states 123 | } 124 | 125 | void usart_start_tx_dma_transfer(uint32_t usart) 126 | { 127 | if (DMA_CCR(CONSOLE_TX_DMA, CONSOLE_TX_DMA_CHANNEL) & DMA_CCR_EN) { 128 | // dma busy doing transfer 129 | return; 130 | } 131 | 132 | _dma_transfer_count = usart_tx_dma_waiting(usart); 133 | if (_dma_transfer_count) { 134 | // we must clear the flag first or we will hang sporadically 135 | DMA_IFCR(CONSOLE_TX_DMA) |= DMA_IFCR_CTCIF(CONSOLE_TX_DMA_CHANNEL); 136 | DMA_CNDTR(CONSOLE_TX_DMA, CONSOLE_TX_DMA_CHANNEL) = _dma_transfer_count; 137 | DMA_CMAR(CONSOLE_TX_DMA, CONSOLE_TX_DMA_CHANNEL) = (uint32_t)(_tx_buffer + _tx_tail); 138 | USART_ICR(CONSOLE_USART) |= USART_ICR_TCCF; 139 | DMA_CCR(CONSOLE_TX_DMA, CONSOLE_TX_DMA_CHANNEL) |= DMA_CCR_EN; 140 | } 141 | } 142 | 143 | void usart_dma_transfer_complete_isr(uint32_t usart) 144 | { 145 | // disable dma transfer 146 | DMA_CCR(CONSOLE_TX_DMA, CONSOLE_TX_DMA_CHANNEL) &= ~DMA_CCR_EN; 147 | 148 | // free available space, move head forward 149 | _tx_tail += _dma_transfer_count; 150 | 151 | // start transferring fresh data 152 | usart_start_tx_dma_transfer(usart); 153 | } 154 | 155 | void usart_write(uint32_t usart, const char* data, uint16_t length) 156 | { 157 | // no blocking 158 | if (usart_tx_available() < length) { 159 | return; 160 | } 161 | 162 | // copy data to tx buffer 163 | for (uint16_t i = 0; i < length; i++) { 164 | _tx_buffer[_tx_head++] = data[i]; 165 | } 166 | 167 | // start transferring the data 168 | usart_start_tx_dma_transfer(usart); 169 | } 170 | 171 | void usart_write_string(uint32_t usart, const char* string) 172 | { 173 | usart_write(usart, string, strlen(string)); 174 | } 175 | 176 | void usart_write_int(uint32_t usart, uint32_t i) 177 | { 178 | // output character buffer 179 | char c[10]; 180 | // maximum character length for uint32_t 181 | uint8_t len = 10; 182 | 183 | // first character 184 | uint8_t p = i % 10; 185 | c[--len] = p + '0'; 186 | 187 | while (i > 9) { 188 | i /= 10; 189 | p = i % 10; 190 | c[--len] = p + '0'; 191 | } 192 | 193 | usart_write(usart, &c[len], 10 - len); 194 | } 195 | -------------------------------------------------------------------------------- /src/hal/src/stm32/common/watchdog.c: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | void watchdog_start(uint32_t period_ms) 6 | { 7 | iwdg_set_period_ms(period_ms); 8 | iwdg_start(); 9 | } 10 | 11 | void watchdog_reset() 12 | { 13 | iwdg_reset(); 14 | } 15 | -------------------------------------------------------------------------------- /src/main.cpp: -------------------------------------------------------------------------------- 1 | 2 | extern "C" { 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | } 18 | 19 | // hpp 20 | #include 21 | #include 22 | 23 | #include 24 | 25 | // use this throttle in open loop startup 26 | const uint16_t startup_throttle = 75; 27 | 28 | // declared/documented in global.h 29 | uint16_t run_zc_confirmations_required = slow_run_zc_confirmations_required; 30 | const uint16_t slow_run_zc_confirmations_required = 14; 31 | volatile bool starting; 32 | const uint16_t startup_commutation_period_ticks = 12000; 33 | const uint16_t startup_zc_confirmations_required = 15; 34 | volatile uint32_t zc_counter; 35 | uint32_t zc_confirmations_required = startup_zc_confirmations_required; 36 | 37 | // for test 38 | void test_spin() 39 | { 40 | for (uint32_t i = 0; i < 4000; i++) { io_write_state(); watchdog_reset(); } 41 | bridge_enable(); 42 | bridge_set_state(BRIDGE_STATE_RUN); 43 | bridge_set_run_duty(200); 44 | watchdog_reset(); 45 | start_motor(); 46 | for (uint32_t i = 200; i < 500; i++) { 47 | for (uint32_t j = 0; j < 100; j++) { 48 | watchdog_reset(); 49 | bridge_set_run_duty(i); 50 | io_write_state(); 51 | } 52 | } 53 | 54 | bridge_disable(); 55 | stop_motor(); 56 | 57 | while(1) {watchdog_reset();} 58 | } 59 | 60 | int main() 61 | { 62 | system_initialize(); 63 | 64 | io_initialize(); 65 | 66 | led_initialize(); 67 | 68 | debug_pins_initialize(); 69 | 70 | console_initialize(); 71 | console_write("welcome to open-esc!\r\n"); 72 | 73 | for (uint32_t j = 0; j < 1000000; j++) { float a = 0.6*9; } 74 | 75 | adc_initialize(); 76 | overcurrent_watchdog_initialize(); 77 | adc_start(); 78 | 79 | watchdog_start(10); // 10ms watchdog timeout 80 | 81 | bridge_initialize(); 82 | bridge_enable_adc_trigger(); 83 | 84 | // startup beep 85 | audio_play_note_blocking(1000, 0x04, 40000); 86 | audio_play_note_blocking(1200, 0x04, 40000); 87 | audio_play_note_blocking(1600, 0x04, 40000); 88 | for (int i = 0; i < 9999; i++) { watchdog_reset(); io_process_input(); } 89 | 90 | #if defined(FEEDBACK_COMPARATOR) 91 | // initialize comparator 92 | comparator_initialize(); 93 | #elif defined(FEEDBACK_BEMF) 94 | // initialize bemf 95 | bemf_initialize(); 96 | #else 97 | #error "no feedback defined" 98 | #endif 99 | 100 | // initialize motor run timers 101 | commutation_timer_initialize(); 102 | zc_timer_initialize(); 103 | 104 | // detect pwm input type 105 | pwm_input_initialize(); 106 | // successive pwm input type detection checks passed 107 | uint8_t succeded = 0; 108 | // successive pwm input type detection checks needed 109 | const uint8_t succeded_needed = 3; 110 | pwm_input_type_t pwm_type_check; 111 | 112 | // setup input signal timer with 25ms timeout 113 | // todo: rework pwm_input, throttle, and io interfaces for some sense 114 | pwm_input_set_type(PWM_INPUT_TYPE_CONVENTIONAL); 115 | while (!g.throttle_valid) 116 | { 117 | watchdog_reset(); 118 | led_toggle(); 119 | io_process_input(); 120 | // todo beep once in a while 121 | } 122 | 123 | // pwm input type valid confirmation beep 124 | audio_play_note_blocking(1000, 0x04, 90000); 125 | 126 | // wait for low throttle 127 | while (g.throttle > 0) { 128 | watchdog_reset(); 129 | io_process_input(); 130 | } 131 | 132 | // low throttle armed beep 133 | audio_play_note_blocking(1600, 0x04, 90000); 134 | 135 | // prepare the motor for run mode (armed) 136 | bridge_enable(); 137 | bridge_set_state(BRIDGE_STATE_RUN); 138 | 139 | // we are armed 140 | 141 | // initialize commutation and zc timers for open loop 142 | // motor won't move until throttle is applied 143 | start_motor(); 144 | 145 | while(1) { 146 | watchdog_reset(); 147 | led_toggle(); 148 | io_process_input(); 149 | if (g.throttle_valid && g.throttle > startup_throttle) { 150 | bridge_enable(); 151 | bridge_set_run_duty(g_bridge_run_duty/2 + g.throttle/2); 152 | } else { 153 | bridge_disable(); 154 | } 155 | } 156 | 157 | // disable the motor output 158 | bridge_disable(); 159 | // stop commutation and zc timers 160 | stop_motor(); 161 | 162 | // independent watchdog will issue a system reset 163 | while (1) {}; 164 | } 165 | -------------------------------------------------------------------------------- /src/main/inc/audio.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | void audio_play_note_blocking(uint16_t frequency, uint8_t duty, uint32_t cycles); 6 | -------------------------------------------------------------------------------- /src/main/inc/bemf.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | typedef enum { 6 | BEMF_PHASE_A, 7 | BEMF_PHASE_B, 8 | BEMF_PHASE_C, 9 | BEMF_PHASE_N, 10 | } bemf_phase_e; 11 | 12 | typedef enum { 13 | BEMF_STEP0, 14 | BEMF_STEP1, 15 | BEMF_STEP2, 16 | BEMF_STEP3, 17 | BEMF_STEP4, 18 | BEMF_STEP5, 19 | } bemf_step_e; 20 | 21 | volatile bemf_step_e g_bemf_step; 22 | 23 | void bemf_disable_divider(); 24 | void bemf_enable_divider(); 25 | 26 | // return voltage in millivolts (~65V 6s or so max for now) 27 | uint16_t bemf_get_phase_voltage(bemf_phase_e phase); 28 | uint16_t bemf_get_phase_adc_raw(bemf_phase_e phase); 29 | 30 | void bemf_set_step(bemf_step_e new_step); 31 | void bemf_blank(uint32_t nanoseconds); 32 | void bemf_unblank_isr(); 33 | void bemf_initialize(); 34 | void bemf_zc_isr(); 35 | void bemf_zc_isr_enable(); 36 | void bemf_zc_isr_disable(); 37 | -------------------------------------------------------------------------------- /src/main/inc/commutation.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | void commutation_timer_initialize(); 4 | void commutation_timer_enable_interrupts(); 5 | void commutation_timer_disable_interrupts(); 6 | 7 | void zc_timer_initialize(); 8 | void zc_timer_enable_interrupts(); 9 | void zc_timer_disable_interrupts(); 10 | 11 | void stop_motor(); 12 | void start_motor(); 13 | -------------------------------------------------------------------------------- /src/main/inc/console.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | void console_initialize(); 6 | void console_write(const char* string); 7 | void console_write_int(uint32_t i); 8 | 9 | void console_write_pwm_info(); 10 | void console_write_adc_info(); 11 | -------------------------------------------------------------------------------- /src/main/inc/debug-pins.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // toggle gpio for debugging with logic analyzer 4 | void debug_pins_initialize(); 5 | void debug_pins_toggle0(); 6 | void debug_pins_toggle1(); 7 | void debug_pins_toggle2(); 8 | -------------------------------------------------------------------------------- /src/main/inc/io.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | void io_control_timeout_reset(); 7 | void io_initialize(); 8 | void io_handle_message(ping_message* message); 9 | void io_parse_byte(uint8_t b); 10 | void io_parse_reset(); 11 | void io_process_input(); 12 | void io_write_message(ping_message* message); 13 | void io_write_state(); 14 | -------------------------------------------------------------------------------- /src/main/inc/isr.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | void io_control_timeout_isr(); 6 | void commutation_isr(); 7 | void comparator_unblank_isr(); 8 | void comparator_zc_isr(); 9 | void comparator_zc_timeout_isr(); 10 | void comparator_blank_complete_isr(); 11 | void usart_dma_transfer_complete_isr(uint32_t usart); 12 | void overcurrent_watchdog_isr(); 13 | -------------------------------------------------------------------------------- /src/main/inc/led.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | void led_initialize(); 6 | void led_on(); 7 | void led_off(); 8 | void led_toggle(); 9 | void led_write(uint8_t r, uint8_t g, uint8_t b); 10 | -------------------------------------------------------------------------------- /src/main/inc/overcurrent-watchdog.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | void overcurrent_watchdog_initialize(); 4 | void overcurrent_watchdog_isr(); 5 | -------------------------------------------------------------------------------- /src/main/src/audio.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | extern "C" 4 | { 5 | #include 6 | #include 7 | } 8 | 9 | #include 10 | 11 | void audio_play_note_blocking(uint16_t frequency, uint8_t duty, uint32_t cycles) 12 | { 13 | bridge_enable(); 14 | bridge_set_state(BRIDGE_STATE_AUDIO); 15 | bridge_set_audio_duty(duty); 16 | bridge_set_audio_frequency(frequency); 17 | for (uint32_t i = 0; i < cycles; i++) { watchdog_reset(); io_process_input(); } 18 | bridge_disable(); 19 | } 20 | -------------------------------------------------------------------------------- /src/main/src/bemf.c: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #if defined(FEEDBACK_BEMF) 10 | // stub 11 | void bemf_initialize() 12 | { 13 | rcc_periph_clock_enable(BEMF_DIVIDER_ENABLE_GPIO_RCC); 14 | gpio_mode_setup(BEMF_DIVIDER_ENABLE_GPIO, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, BEMF_DIVIDER_ENABLE_GPIO_PIN); 15 | gpio_set(BEMF_DIVIDER_ENABLE_GPIO, BEMF_DIVIDER_ENABLE_GPIO_PIN); 16 | } 17 | 18 | // stub 19 | uint16_t bemf_get_phase_voltage(bemf_phase_e phase) 20 | { 21 | return bemf_get_phase_adc_raw(phase); 22 | } 23 | 24 | uint16_t bemf_get_phase_adc_raw(bemf_phase_e phase) 25 | { 26 | // todo move compile switch to cmake build system 27 | switch (phase) { 28 | case (BEMF_PHASE_A): 29 | return g.adc_buffer[ADC_CHANNEL_PHASEA_VOLTAGE]; 30 | case (BEMF_PHASE_B): 31 | return g.adc_buffer[ADC_CHANNEL_PHASEB_VOLTAGE]; 32 | case (BEMF_PHASE_C): 33 | return g.adc_buffer[ADC_CHANNEL_PHASEC_VOLTAGE]; 34 | case (BEMF_PHASE_N): 35 | #if defined(ADC_CHANNEL_NEUTRAL_VOLTAGE) 36 | return g.adc_buffer[ADC_CHANNEL_NEUTRAL_VOLTAGE]; 37 | #else 38 | return 0; 39 | #endif 40 | } 41 | } 42 | #endif 43 | -------------------------------------------------------------------------------- /src/main/src/commutation.c: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | // commutation and zero cross timers must have the same frequency 15 | const uint32_t commutation_zc_timer_frequency = 2000000; 16 | 17 | void commutation_timer_enable_interrupts() 18 | { 19 | nvic_enable_irq(COMMUTATION_TIMER_IRQ); 20 | TIM_DIER(COMMUTATION_TIMER) |= TIM_DIER_CC1IE | TIM_DIER_UIE; 21 | } 22 | 23 | void zc_timer_enable_interrupts() 24 | { 25 | nvic_enable_irq(ZC_TIMER_IRQ); 26 | TIM_DIER(ZC_TIMER) |= TIM_DIER_UIE; 27 | } 28 | 29 | void commutation_timer_disable_interrupts() 30 | { 31 | TIM_DIER(COMMUTATION_TIMER) &= ~(TIM_DIER_CC1IE | TIM_DIER_UIE); 32 | nvic_disable_irq(COMMUTATION_TIMER_IRQ); 33 | } 34 | 35 | void zc_timer_disable_interrupts() 36 | { 37 | TIM_DIER(ZC_TIMER) &= ~(TIM_DIER_UIE); 38 | nvic_disable_irq(ZC_TIMER_IRQ); 39 | } 40 | 41 | void stop_motor() 42 | { 43 | commutation_timer_disable_interrupts(); 44 | #if defined(FEEDBACK_COMPARATOR) 45 | comparator_zc_isr_disable(); 46 | zc_timer_disable_interrupts(); 47 | #endif 48 | } 49 | 50 | void start_motor() 51 | { 52 | starting = true; 53 | TIM_CR1(COMMUTATION_TIMER) &= ~TIM_CR1_CEN; // disable counter 54 | TIM_CNT(COMMUTATION_TIMER) = 1; // set counter to zero 55 | TIM_ARR(COMMUTATION_TIMER) = startup_commutation_period_ticks; 56 | TIM_CCR1(COMMUTATION_TIMER) = TIM_ARR(COMMUTATION_TIMER)/16; 57 | zc_counter = zc_confirmations_required; 58 | 59 | g_bridge_comm_step = BRIDGE_COMM_STEP0; 60 | 61 | #if defined(FEEDBACK_COMPARATOR) 62 | comparator_set_state((comp_state_e)g_bridge_comm_step); 63 | #endif 64 | 65 | commutation_timer_enable_interrupts(); 66 | zc_timer_enable_interrupts(); 67 | //bridge_set_run_duty(startup_throttle); 68 | TIM_CR1(COMMUTATION_TIMER) |= TIM_CR1_CEN; // enable counter 69 | TIM_CNT(ZC_TIMER) = TIM_ARR(COMMUTATION_TIMER)/2; 70 | TIM_CR1(ZC_TIMER) |= TIM_CR1_CEN; // enable counter 71 | } 72 | 73 | void commutation_timer_initialize() 74 | { 75 | rcc_periph_clock_enable(COMMUTATION_TIMER_RCC); 76 | TIM_ARR(COMMUTATION_TIMER) = 0xffff; 77 | TIM_PSC(COMMUTATION_TIMER) = (rcc_ahb_frequency / commutation_zc_timer_frequency) - 1; 78 | // prescaler is not applied until update event 79 | TIM_EGR(COMMUTATION_TIMER) |= TIM_EGR_UG; 80 | TIM_SR(COMMUTATION_TIMER) = 0; 81 | } 82 | 83 | void zc_timer_initialize() 84 | { 85 | rcc_periph_clock_enable(ZC_TIMER_RCC); 86 | TIM_ARR(ZC_TIMER) = 0xffff; 87 | TIM_PSC(ZC_TIMER) = (rcc_ahb_frequency / commutation_zc_timer_frequency) - 1; 88 | // prescaler is not applied until update event 89 | TIM_EGR(ZC_TIMER) |= TIM_EGR_UG; 90 | TIM_SR(ZC_TIMER) = 0; 91 | 92 | // commutation timer takes priority over comparator 93 | nvic_set_priority(ZC_TIMER_IRQ, 0x40); 94 | } 95 | 96 | void commutation_isr() 97 | { 98 | bridge_commutate(); 99 | 100 | #if defined(FEEDBACK_COMPARATOR) 101 | comparator_zc_isr_disable(); 102 | #endif 103 | 104 | zc_counter = zc_confirmations_required; // remove 105 | 106 | #if defined(FEEDBACK_COMPARATOR) 107 | // TODO rotate table to get this right 108 | comparator_set_state((comp_state_e)(g_bridge_comm_step + 2)); 109 | #endif 110 | 111 | debug_pins_toggle2(); 112 | } 113 | -------------------------------------------------------------------------------- /src/main/src/comparator-isr.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | // these comparator isrs are defined by the application 9 | 10 | // zc timer isr 11 | void comparator_zc_timeout_isr() { 12 | // timeout waiting for zero-cross, go to open loop 13 | // TODO should load commutation timer default values 14 | starting = true; 15 | debug_pins_toggle2(); 16 | debug_pins_toggle2(); 17 | } 18 | 19 | void comparator_zc_isr() 20 | { 21 | while (zc_counter--) { 22 | if (!comparator_get_output()) { 23 | zc_counter = zc_confirmations_required; 24 | comparator_blank(20000); // give main loop a gasp of air 25 | return; 26 | } 27 | debug_pins_toggle0(); 28 | } 29 | 30 | volatile uint16_t cnt = TIM_CNT(ZC_TIMER); 31 | // TODO does zero cause update event? 32 | TIM_CNT(ZC_TIMER) = 0; 33 | 34 | 35 | if (cnt < 300) { 36 | starting = true; 37 | } 38 | if (cnt < TIM_ARR(COMMUTATION_TIMER)/2) { 39 | if (starting) { 40 | zc_confirmations_required = startup_zc_confirmations_required; 41 | 42 | zc_counter = zc_confirmations_required; 43 | TIM_ARR(COMMUTATION_TIMER) = startup_commutation_period_ticks; 44 | debug_pins_toggle1(); 45 | debug_pins_toggle1(); 46 | } 47 | } else if (cnt > TIM_ARR(COMMUTATION_TIMER) + TIM_ARR(COMMUTATION_TIMER)/2 + TIM_ARR(COMMUTATION_TIMER)/4) { 48 | // we missed a cross 49 | // do nothing 50 | } else { 51 | if (starting) { 52 | starting = false; 53 | debug_pins_toggle1(); 54 | } else { 55 | if (cnt < 6000) { 56 | if (run_zc_confirmations_required > 6) { 57 | run_zc_confirmations_required--; 58 | } 59 | } else if (cnt < 8000) { 60 | if (run_zc_confirmations_required > 8) { 61 | run_zc_confirmations_required--; 62 | } 63 | } else if (cnt < 10000) { 64 | if (run_zc_confirmations_required > 10) { 65 | run_zc_confirmations_required--; 66 | } 67 | } else { 68 | if (run_zc_confirmations_required < slow_run_zc_confirmations_required) { 69 | run_zc_confirmations_required++; 70 | } 71 | } 72 | zc_confirmations_required = run_zc_confirmations_required; 73 | TIM_ARR(COMMUTATION_TIMER) = TIM_ARR(COMMUTATION_TIMER)/2 + TIM_ARR(COMMUTATION_TIMER)/4 + cnt/4; 74 | // TIM_ARR(COMMUTATION_TIMER) = TIM_ARR(COMMUTATION_TIMER)/2 + cnt/2; 75 | // schedule commutation (+ timing advance) 76 | TIM_CNT(COMMUTATION_TIMER) = TIM_ARR(COMMUTATION_TIMER)/2; 77 | debug_pins_toggle1(); 78 | debug_pins_toggle1(); 79 | debug_pins_toggle1(); 80 | } 81 | } 82 | 83 | // blanking period 84 | TIM_CCR1(COMMUTATION_TIMER) = TIM_ARR(COMMUTATION_TIMER)/8; 85 | 86 | comparator_zc_isr_disable(); 87 | TIM_SR(COMMUTATION_TIMER) = 0; 88 | } 89 | -------------------------------------------------------------------------------- /src/main/src/console.c: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | 15 | void console_initialize() { 16 | rcc_periph_clock_enable(CONSOLE_TX_GPIO_RCC); 17 | 18 | gpio_mode_setup(CONSOLE_TX_GPIO_PORT, GPIO_MODE_AF, GPIO_PUPD_NONE, CONSOLE_TX_GPIO_PIN); 19 | 20 | gpio_set_af(CONSOLE_TX_GPIO_PORT, CONSOLE_TX_GPIO_AF, CONSOLE_TX_GPIO_PIN); 21 | 22 | rcc_periph_clock_enable(CONSOLE_RX_GPIO_RCC); 23 | 24 | gpio_mode_setup(CONSOLE_RX_GPIO_PORT, GPIO_MODE_AF, GPIO_PUPD_NONE, CONSOLE_RX_GPIO_PIN); 25 | 26 | gpio_set_af(CONSOLE_RX_GPIO_PORT, CONSOLE_RX_GPIO_AF, CONSOLE_RX_GPIO_PIN); 27 | 28 | rcc_periph_clock_enable(CONSOLE_USART_RCC); 29 | 30 | usart_initialize(CONSOLE_USART, 1000000); 31 | } 32 | 33 | void console_write(const char* string) 34 | { 35 | usart_write_string(CONSOLE_USART, string); 36 | } 37 | 38 | void console_write_int(const uint32_t i) 39 | { 40 | usart_write_int(CONSOLE_USART, i); 41 | } 42 | 43 | const char* pwm_type_names[6] = 44 | { 45 | [PWM_INPUT_TYPE_NONE] = "none", 46 | [PWM_INPUT_TYPE_CONVENTIONAL] = "conventional", 47 | [PWM_INPUT_TYPE_ONESHOT125] = "oneshot125", 48 | [PWM_INPUT_TYPE_ONESHOT42] = "oneshot42", 49 | [PWM_INPUT_TYPE_MULTISHOT] = "multishot", 50 | [PWM_INPUT_TYPE_UNKNOWN] = "unknown", 51 | }; 52 | 53 | void console_write_pwm_info() 54 | { 55 | char buffer[100]; 56 | uint8_t size = snprintf(buffer, 100, "pwm type: %s\tthrottle: %d\tduty (ns): %d\tperiod (ns): %d\r\n", 57 | pwm_type_names[g_pwm_type], 58 | pwm_input_get_throttle(), 59 | pwm_input_get_duty_ns(), 60 | pwm_input_get_period_ns()); 61 | usart_write(CONSOLE_USART, buffer, size); 62 | } 63 | 64 | void console_write_adc_info() 65 | { 66 | char buffer[120]; 67 | uint8_t size = snprintf(buffer, 120, "temperature: %d\tbus voltage: %d\tbus current: %d\tA: %d\tB: %d\tC: %d\tN: %d\r\n", 68 | # 69 | #if defined(ADC_IDX_TEMPERATURE) 70 | adc_read_channel(ADC_IDX_TEMPERATURE), 71 | #else 72 | 0, 73 | #endif 74 | 75 | #if defined(ADC_IDX_BUS_VOLTAGE) 76 | adc_read_channel(ADC_IDX_BUS_VOLTAGE), 77 | #else 78 | 0, 79 | #endif 80 | 81 | #if defined(ADC_IDX_BUS_CURRENT) 82 | adc_read_channel(ADC_IDX_BUS_CURRENT), 83 | #else 84 | 0, 85 | #endif 86 | 87 | #if defined(ADC_IDX_PHASEA_VOLTAGE) 88 | adc_read_channel(ADC_IDX_PHASEA_VOLTAGE), 89 | #else 90 | 0, 91 | #endif 92 | 93 | #if defined(ADC_IDX_PHASEB_VOLTAGE) 94 | adc_read_channel(ADC_IDX_PHASEB_VOLTAGE), 95 | #else 96 | 0, 97 | #endif 98 | 99 | #if defined(ADC_IDX_PHASEC_VOLTAGE) 100 | adc_read_channel(ADC_IDX_PHASEC_VOLTAGE), 101 | #else 102 | 0, 103 | #endif 104 | 105 | #if defined(ADC_IDX_NEUTRAL_VOLTAGE) 106 | adc_read_channel(ADC_IDX_NEUTRAL_VOLTAGE) 107 | #else 108 | 0 109 | #endif 110 | ); 111 | usart_write(CONSOLE_USART, buffer, size); 112 | } 113 | -------------------------------------------------------------------------------- /src/main/src/debug-pins.c: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | // #define DEBUG_PINS 9 | 10 | void debug_pins_initialize() 11 | { 12 | #if defined(DEBUG_PINS) 13 | rcc_periph_clock_enable(DBG0_GPIO_RCC); 14 | rcc_periph_clock_enable(DBG1_GPIO_RCC); 15 | rcc_periph_clock_enable(DBG2_GPIO_RCC); 16 | for (uint32_t i = 0; i < 10000; i++) { float a = 0.6*9; } 17 | 18 | gpio_mode_setup(DBG0_GPIO_PORT, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, DBG0_GPIO_PIN); 19 | gpio_mode_setup(DBG1_GPIO_PORT, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, DBG1_GPIO_PIN); 20 | gpio_mode_setup(DBG2_GPIO_PORT, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, DBG2_GPIO_PIN); 21 | for (uint32_t i = 0; i < 10000; i++) { float a = 0.6*9; } 22 | 23 | debug_pins_toggle0(); 24 | debug_pins_toggle0(); 25 | for (uint32_t i = 0; i < 10000; i++) { float a = 0.6*9; } 26 | 27 | debug_pins_toggle1(); 28 | debug_pins_toggle1(); 29 | debug_pins_toggle1(); 30 | debug_pins_toggle1(); 31 | for (uint32_t i = 0; i < 10000; i++) { float a = 0.6*9; } 32 | 33 | debug_pins_toggle2(); 34 | debug_pins_toggle2(); 35 | debug_pins_toggle2(); 36 | debug_pins_toggle2(); 37 | debug_pins_toggle2(); 38 | debug_pins_toggle2(); 39 | for (uint32_t i = 0; i < 90000; i++) { float a = 0.6*9; } 40 | #endif 41 | } 42 | 43 | void debug_pins_toggle0() 44 | { 45 | #if defined(DEBUG_PINS) 46 | gpio_toggle(DBG0_GPIO_PORT, DBG0_GPIO_PIN); 47 | #endif 48 | } 49 | 50 | void debug_pins_toggle1() 51 | { 52 | #if defined(DEBUG_PINS) 53 | gpio_toggle(DBG1_GPIO_PORT, DBG1_GPIO_PIN); 54 | #endif 55 | } 56 | 57 | void debug_pins_toggle2() 58 | { 59 | #if defined(DEBUG_PINS) 60 | gpio_toggle(DBG2_GPIO_PORT, DBG2_GPIO_PIN); 61 | #endif 62 | } 63 | -------------------------------------------------------------------------------- /src/main/src/io.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | extern "C" { 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | } 15 | 16 | #include 17 | #include 18 | 19 | #include 20 | #include 21 | 22 | PingParser parser; 23 | 24 | common_ack ack; 25 | common_nack nack(32); 26 | 27 | #define NUM_REGS 32 28 | uint32_t registers[NUM_REGS]; 29 | 30 | extern "C" 31 | void io_control_timeout_isr() 32 | { 33 | g.throttle_valid = false; 34 | } 35 | 36 | // this is used only when pwm signal is not present 37 | // pwm input driver manages the timeout itself 38 | void io_control_timeout_reset() 39 | { 40 | TIM_CNT(PWM_INPUT_TIMER) = 1; 41 | } 42 | 43 | void io_initialize() 44 | { 45 | ack.updateChecksum(); 46 | nack.updateChecksum(); 47 | registers[0] = 1; 48 | registers[1] = 2; 49 | registers[2] = 3; 50 | } 51 | 52 | void io_handle_message(ping_message* message) 53 | { 54 | switch (message->message_id()) { 55 | case (CommonId::GENERAL_REQUEST): 56 | { 57 | common_general_request* m = (common_general_request*)message; 58 | switch(m->requested_id()) { 59 | case (CommonId::DEVICE_INFORMATION): 60 | { 61 | common_device_information response; 62 | response.set_device_type(3); 63 | response.set_device_revision(0); 64 | response.set_firmware_version_major(0); 65 | response.set_firmware_version_minor(0); 66 | response.set_firmware_version_patch(1); 67 | response.updateChecksum(); 68 | io_write_message(&response); 69 | } 70 | break; 71 | case (CommonId::PROTOCOL_VERSION): 72 | { 73 | common_protocol_version response; 74 | response.set_version_major(1); 75 | response.set_version_minor(0); 76 | response.set_version_patch(0); 77 | response.updateChecksum(); 78 | io_write_message(&response); 79 | } 80 | 81 | break; 82 | 83 | default: 84 | break; 85 | } 86 | } 87 | break; 88 | case (OpenescId::SET_THROTTLE): 89 | { 90 | openesc_set_throttle* m = (openesc_set_throttle*)message; 91 | if (m->throttle_signal() > 0xfff) { 92 | g.throttle_valid = false; 93 | io_write_message(&nack); 94 | } else { 95 | g.throttle_valid = true; 96 | g.throttle = m->throttle_signal(); 97 | io_control_timeout_reset(); 98 | io_write_state(); 99 | } 100 | } 101 | break; 102 | default: 103 | break; 104 | } 105 | } 106 | 107 | void io_process_input() 108 | { 109 | char b; 110 | uint8_t nbytes = usart_rx_waiting(); 111 | for (uint8_t i = 0; i < nbytes; i++) { 112 | usart_read(CONSOLE_USART, &b, 1); 113 | io_parse_byte(b); 114 | } 115 | } 116 | 117 | void io_parse_byte(uint8_t b) 118 | { 119 | if (parser.parseByte(b) == PingParser::NEW_MESSAGE) { 120 | io_handle_message(&parser.rxMessage); 121 | } 122 | } 123 | 124 | void io_parse_reset() 125 | { 126 | parser.reset(); 127 | } 128 | 129 | void io_write_message(ping_message* message) 130 | { 131 | usart_write(CONSOLE_USART, (char*)message->msgData, message->msgDataLength()); 132 | } 133 | 134 | void io_write_state() 135 | { 136 | static openesc_state message; 137 | #if defined(ADC_IDX_PHASEA_VOLTAGE) 138 | message.set_phaseA(g.adc_buffer[ADC_IDX_PHASEA_VOLTAGE]); 139 | #else 140 | message.set_phaseA(0); 141 | #endif 142 | #if defined(ADC_IDX_PHASEB_VOLTAGE) 143 | message.set_phaseB(g.adc_buffer[ADC_IDX_PHASEB_VOLTAGE]); 144 | #else 145 | message.set_phaseB(0); 146 | #endif 147 | #if defined(ADC_IDX_PHASEC_VOLTAGE) 148 | message.set_phaseC(g.adc_buffer[ADC_IDX_PHASEC_VOLTAGE]); 149 | #else 150 | message.set_phaseC(0); 151 | #endif 152 | #if defined(ADC_IDX_NEUTRAL_VOLTAGE) 153 | message.set_neutral(g.adc_buffer[ADC_IDX_NEUTRAL_VOLTAGE]); 154 | #else 155 | message.set_neutral(0); 156 | #endif 157 | #if defined(ADC_IDX_BUS_CURRENT) 158 | message.set_current(g.adc_buffer[ADC_IDX_BUS_CURRENT]); 159 | #else 160 | message.set_current(0); 161 | #endif 162 | #if defined(ADC_IDX_BUS_VOLTAGE) 163 | message.set_voltage(g.adc_buffer[ADC_IDX_BUS_VOLTAGE]); 164 | #else 165 | message.set_voltage(0); 166 | #endif 167 | message.set_throttle(g.throttle); 168 | message.set_commutation_period(TIM_ARR(COMMUTATION_TIMER)); 169 | #if defined(FEEDBACK_COMPARATOR) 170 | message.set_flags(comparator_get_output()); 171 | message.set_comparator_step(g_comparator_state); 172 | #endif 173 | message.set_bridge_step(g_bridge_comm_step); 174 | message.updateChecksum(); 175 | io_write_message(&message); 176 | } 177 | -------------------------------------------------------------------------------- /src/main/src/led-gpio.c: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | void led_initialize() 9 | { 10 | rcc_periph_clock_enable(LED_GPIO_RCC); 11 | gpio_mode_setup(LED_GPIO_PORT, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, LED_GPIO_PIN); 12 | for (uint8_t i = 0; i < 10; i++) { 13 | led_toggle(); 14 | for (uint32_t j = 0; j < 100000; j++) { float a = 0.6*9; } 15 | } 16 | } 17 | 18 | void led_on() 19 | { 20 | 21 | } 22 | 23 | void led_off() 24 | { 25 | 26 | } 27 | 28 | void led_toggle() 29 | { 30 | gpio_toggle(LED_GPIO_PORT, LED_GPIO_PIN); 31 | } 32 | 33 | void led_write(uint8_t r, uint8_t g, uint8_t b) 34 | { 35 | 36 | } 37 | -------------------------------------------------------------------------------- /src/main/src/overcurrent-watchdog.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | void overcurrent_watchdog_initialize() 11 | { 12 | #if defined(ADC_CHANNEL_BUS_CURRENT) 13 | // set AWD high threshold 14 | ADC_TR1(ADC1) = (ADC_WWDG_CURRENT_MAX << 16) & 0x0fff0000; 15 | 16 | // enable watchdog on selected channel 17 | ADC_CFGR1(ADC1) = (ADC_CFGR1(ADC1) & ~ADC_CFGR1_AWD1CH) | ADC_CFGR1_AWD1CH_VAL(ADC_CHANNEL_BUS_CURRENT); 18 | ADC_CFGR1(ADC1) |= ADC_CFGR1_AWD1EN | ADC_CFGR1_AWD1SGL; 19 | 20 | // enable watchdog interrupt 21 | ADC_IER(ADC1) |= ADC_IER_AWD1IE; 22 | nvic_enable_irq(OVERCURRENT_WATCHDOG_IRQ); 23 | #endif 24 | } 25 | 26 | void overcurrent_watchdog_isr() 27 | { 28 | bridge_disable(); 29 | stop_motor(); 30 | while (1) { watchdog_reset(); } 31 | } 32 | -------------------------------------------------------------------------------- /src/target/b-g431b-esc1/pinout.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jaxxzer/open-esc-firmware/351193749e93441343dd0239c148da52145f7e74/src/target/b-g431b-esc1/pinout.png -------------------------------------------------------------------------------- /src/target/b-g431b-esc1/target-mcu.cmake: -------------------------------------------------------------------------------- 1 | set(TARGET_MCU "STM32G431CB") 2 | -------------------------------------------------------------------------------- /src/target/b-g431b-esc1/target.c: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | uint8_t adc_channels[4] = { 4 | ADC_CHANNEL_BUS_VOLTAGE, 5 | ADC_CHANNEL_PHASEB_VOLTAGE, 6 | ADC_CHANNEL_PHASEC_VOLTAGE, 7 | ADC_CHANNEL_PHASEA_VOLTAGE, 8 | }; 9 | -------------------------------------------------------------------------------- /src/target/b-g431b-esc1/target.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "../inc/stm32/g4/target.h" 4 | #include 5 | 6 | #define FEEDBACK_BEMF 7 | 8 | #define LED_GPIO_RCC RCC_GPIOC 9 | #define LED_GPIO_PORT GPIOC 10 | #define LED_GPIO_PIN GPIO6 11 | 12 | #define DBG0_GPIO_RCC RCC_GPIOB 13 | #define DBG0_GPIO_PORT GPIOB 14 | #define DBG0_GPIO_PIN GPIO5 15 | 16 | #define DBG1_GPIO_RCC RCC_GPIOB 17 | #define DBG1_GPIO_PORT GPIOB 18 | #define DBG1_GPIO_PIN GPIO6 19 | 20 | #define DBG2_GPIO_RCC RCC_GPIOB 21 | #define DBG2_GPIO_PORT GPIOB 22 | #define DBG2_GPIO_PIN GPIO7 23 | 24 | #define CONSOLE_USART USART2 25 | #define CONSOLE_USART_RCC RCC_USART2 26 | #define CONSOLE_TX_GPIO_AF GPIO_AF7 27 | #define CONSOLE_TX_GPIO_RCC RCC_GPIOB 28 | #define CONSOLE_TX_GPIO_PORT GPIOB 29 | #define CONSOLE_TX_GPIO_PIN GPIO3 30 | #define CONSOLE_TX_DMA_RCC RCC_DMA1 31 | #define CONSOLE_TX_DMA DMA1 32 | #define CONSOLE_TX_DMA_CHANNEL DMA_CHANNEL5 33 | #define CONSOLE_TX_DMA_IRQ NVIC_DMA1_CHANNEL5_IRQ 34 | #define CONSOLE_TX_DMAMUX_REQID 27 35 | 36 | #define CONSOLE_RX_GPIO_AF GPIO_AF7 37 | #define CONSOLE_RX_GPIO_RCC RCC_GPIOB 38 | #define CONSOLE_RX_GPIO_PORT GPIOB 39 | #define CONSOLE_RX_GPIO_PIN GPIO4 40 | #define CONSOLE_RX_DMA_RCC RCC_DMA1 41 | #define CONSOLE_RX_DMA DMA1 42 | #define CONSOLE_RX_DMA_CHANNEL DMA_CHANNEL6 43 | #define CONSOLE_RX_DMA_IRQ NVIC_DMA1_CHANNEL6_IRQ 44 | #define CONSOLE_RX_DMAMUX_REQID 26 45 | 46 | #define PWM_INPUT_GPIO_RCC RCC_GPIOA 47 | #define PWM_INPUT_GPIO_PORT GPIOA 48 | #define PWM_INPUT_GPIO_PIN GPIO15 49 | #define PWM_INPUT_GPIO_AF GPIO_AF1 50 | 51 | #define PWM_INPUT_TIMER_RCC RCC_TIM2 52 | #define PWM_INPUT_TIMER TIM2 53 | #define PWM_INPUT_TIMER_IRQ NVIC_TIM2_IRQ 54 | #define PWM_INPUT_TIMER_IC_ID_RISE TIM_IC1 55 | #define PWM_INPUT_TIMER_IC_ID_FALL TIM_IC2 56 | #define PWM_INPUT_TIMER_IC_TRIGGER TIM_IC_IN_TI1 57 | #define PWM_INPUT_TIMER_OC_ID_FALL TIM_OC2 58 | #define PWM_INPUT_TIMER_SLAVE_MODE TIM_SMCR_SMS_RM 59 | #define PWM_INPUT_TIMER_SLAVE_TRIGGER TIM_SMCR_TS_TI1FP1 60 | #define PWM_INPUT_DUTY_CCR TIM_CCR2 61 | #define PWM_INPUT_PERIOD_CCR TIM_CCR1 62 | 63 | #define BRIDGE_HI_A_GPIO_RCC RCC_GPIOA 64 | #define BRIDGE_HI_A_GPIO_PORT GPIOA 65 | #define BRIDGE_HI_A_GPIO_PIN GPIO8 66 | #define BRIDGE_HI_A_GPIO_AF GPIO_AF6 67 | 68 | #define BRIDGE_HI_B_GPIO_RCC RCC_GPIOA 69 | #define BRIDGE_HI_B_GPIO_PORT GPIOA 70 | #define BRIDGE_HI_B_GPIO_PIN GPIO9 71 | #define BRIDGE_HI_B_GPIO_AF GPIO_AF6 72 | 73 | #define BRIDGE_HI_C_GPIO_RCC RCC_GPIOA 74 | #define BRIDGE_HI_C_GPIO_PORT GPIOA 75 | #define BRIDGE_HI_C_GPIO_PIN GPIO10 76 | #define BRIDGE_HI_C_GPIO_AF GPIO_AF6 77 | 78 | #define BRIDGE_LO_A_GPIO_RCC RCC_GPIOC 79 | #define BRIDGE_LO_A_GPIO_PORT GPIOC 80 | #define BRIDGE_LO_A_GPIO_PIN GPIO13 81 | #define BRIDGE_LO_A_GPIO_AF GPIO_AF4 82 | 83 | #define BRIDGE_LO_B_GPIO_RCC RCC_GPIOA 84 | #define BRIDGE_LO_B_GPIO_PORT GPIOA 85 | #define BRIDGE_LO_B_GPIO_PIN GPIO12 86 | #define BRIDGE_LO_B_GPIO_AF GPIO_AF6 87 | 88 | #define BRIDGE_LO_C_GPIO_RCC RCC_GPIOB 89 | #define BRIDGE_LO_C_GPIO_PORT GPIOB 90 | #define BRIDGE_LO_C_GPIO_PIN GPIO15 91 | #define BRIDGE_LO_C_GPIO_AF GPIO_AF4 92 | 93 | #define COMPARATOR_ZC_IRQ NVIC_COMP123_IRQ 94 | #define COMPARATOR_BLANK_TIMER_RCC RCC_TIM17 95 | #define COMPARATOR_BLANK_TIMER TIM17 96 | #define COMPARATOR_BLANK_IRQ NVIC_TIM1_TRG_COM_TIM17_IRQ 97 | 98 | #define ADC_PERIPH ADC2 99 | #define ADC_RCC RCC_ADC12 100 | 101 | #define ADC_CHANNEL_BUS_VOLTAGE 1 102 | #define ADC_CHANNEL_PHASEB_VOLTAGE 5 103 | #define ADC_CHANNEL_PHASEC_VOLTAGE 14 104 | #define ADC_CHANNEL_PHASEA_VOLTAGE 17 105 | 106 | // pb14 bus temperature : ~1:5 107 | // pb12 pot ~1:11 108 | // pa0 bus voltage ~12:1 109 | // pc4 bemf2 ~2:5 110 | // pb11 bemf3 ~12:14 111 | // pa4 bemf1 ~2:17 112 | 113 | #define ADC_IDX_BUS_VOLTAGE 0 114 | #define ADC_IDX_PHASEB_VOLTAGE 1 115 | #define ADC_IDX_PHASEC_VOLTAGE 2 116 | #define ADC_IDX_PHASEA_VOLTAGE 3 117 | 118 | #define PHASEC_VOLTAGE_GPIO_PIN GPIO11 // b11 119 | #define PHASEA_VOLTAGE_GPIO_PIN GPIO4 // a4 120 | #define PHASEB_VOLTAGE_GPIO_PIN GPIO4 // c4 121 | #define BUS_VOLTAGE_GPIO_PIN GPIO0 // a0 122 | 123 | #define ADC_GPIOA_PINS PHASEA_VOLTAGE_GPIO_PIN | \ 124 | BUS_VOLTAGE_GPIO_PIN 125 | 126 | #define ADC_GPIOB_PINS PHASEC_VOLTAGE_GPIO_PIN 127 | 128 | #define ADC_GPIOC_PINS PHASEB_VOLTAGE_GPIO_PIN 129 | 130 | // adc threshold for current channel 131 | #define ADC_WWDG_CURRENT_MAX 400 132 | 133 | extern uint8_t adc_channels[4]; 134 | 135 | #define ADC_DMAMUX_REQID 36 136 | 137 | #define BEMF_DIVIDER_ENABLE_GPIO GPIOB 138 | #define BEMF_DIVIDER_ENABLE_GPIO_RCC RCC_GPIOB 139 | #define BEMF_DIVIDER_ENABLE_GPIO_PIN GPIO5 140 | -------------------------------------------------------------------------------- /src/target/gsc/target-mcu.cmake: -------------------------------------------------------------------------------- 1 | set(TARGET_MCU "STM32F350G8") 2 | 3 | set(COMPARATOR_SRC ${PROJECT_SOURCE_DIR}/src/hal/src/stm32/common/comparator-internal.c) 4 | -------------------------------------------------------------------------------- /src/target/gsc/target.c: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | uint8_t adc_channels[7] = { 4 | ADC_CHANNEL_PHASEC_VOLTAGE, 5 | ADC_CHANNEL_NEUTRAL_VOLTAGE, 6 | ADC_CHANNEL_BUS_VOLTAGE, 7 | ADC_CHANNEL_PHASEA_VOLTAGE, 8 | ADC_CHANNEL_PHASEB_VOLTAGE, 9 | ADC_CHANNEL_BUS_CURRENT, 10 | ADC_CHANNEL_TEMPERATURE, 11 | }; 12 | -------------------------------------------------------------------------------- /src/target/gsc/target.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #define FEEDBACK_COMPARATOR 6 | 7 | #define LED_GPIO_RCC RCC_GPIOA 8 | #define LED_GPIO_PORT GPIOA 9 | #define LED_GPIO_PIN GPIO2 10 | 11 | #define DBG0_GPIO_RCC RCC_GPIOB 12 | #define DBG0_GPIO_PORT GPIOB 13 | #define DBG0_GPIO_PIN GPIO5 14 | 15 | #define DBG1_GPIO_RCC RCC_GPIOB 16 | #define DBG1_GPIO_PORT GPIOB 17 | #define DBG1_GPIO_PIN GPIO6 // conflicts with usart 18 | 19 | #define DBG2_GPIO_RCC RCC_GPIOB 20 | #define DBG2_GPIO_PORT GPIOB 21 | #define DBG2_GPIO_PIN GPIO7 // conflicts with usart 22 | 23 | #define CONSOLE_USART USART1 24 | #define CONSOLE_USART_RCC RCC_USART1 25 | #define CONSOLE_TX_GPIO_AF GPIO_AF0 26 | #define CONSOLE_TX_GPIO_RCC RCC_GPIOB 27 | #define CONSOLE_TX_GPIO_PORT GPIOB 28 | #define CONSOLE_TX_GPIO_PIN GPIO6 29 | #define CONSOLE_TX_DMA_RCC RCC_DMA1 30 | #define CONSOLE_TX_DMA DMA1 31 | #define CONSOLE_TX_DMA_CHANNEL DMA_CHANNEL2 32 | #define CONSOLE_TX_DMA_IRQ NVIC_DMA1_CHANNEL2_3_DMA2_CHANNEL1_2_IRQ 33 | 34 | #define PWM_INPUT_GPIO_RCC RCC_GPIOA 35 | #define PWM_INPUT_GPIO_PORT GPIOA 36 | #define PWM_INPUT_GPIO_PIN GPIO15 37 | #define PWM_INPUT_GPIO_AF GPIO_AF2 38 | 39 | #define PWM_INPUT_TIMER_RCC RCC_TIM2 40 | #define PWM_INPUT_TIMER TIM2 41 | #define PWM_INPUT_TIMER_IC_ID_RISE TIM_IC1 42 | #define PWM_INPUT_TIMER_IC_ID_FALL TIM_IC2 43 | #define PWM_INPUT_TIMER_IC_TRIGGER TIM_IC_IN_TI1 44 | #define PWM_INPUT_TIMER_OC_ID_FALL TIM_OC2 45 | #define PWM_INPUT_TIMER_SLAVE_MODE TIM_SMCR_SMS_RM 46 | #define PWM_INPUT_TIMER_SLAVE_TRIGGER TIM_SMCR_TS_TI1FP1 47 | #define PWM_INPUT_DUTY_CCR TIM_CCR2 48 | #define PWM_INPUT_PERIOD_CCR TIM_CCR1 49 | 50 | #define BRIDGE_GPIO_AF GPIO_AF2 51 | 52 | #define BRIDGE_HI_A_GPIO_RCC RCC_GPIOA 53 | #define BRIDGE_HI_A_GPIO_PORT GPIOA 54 | #define BRIDGE_HI_A_GPIO_PIN GPIO8 55 | 56 | #define BRIDGE_HI_B_GPIO_RCC RCC_GPIOA 57 | #define BRIDGE_HI_B_GPIO_PORT GPIOA 58 | #define BRIDGE_HI_B_GPIO_PIN GPIO9 59 | 60 | #define BRIDGE_HI_C_GPIO_RCC RCC_GPIOA 61 | #define BRIDGE_HI_C_GPIO_PORT GPIOA 62 | #define BRIDGE_HI_C_GPIO_PIN GPIO10 63 | 64 | #define BRIDGE_LO_A_GPIO_RCC RCC_GPIOA 65 | #define BRIDGE_LO_A_GPIO_PORT GPIOA 66 | #define BRIDGE_LO_A_GPIO_PIN GPIO7 67 | 68 | #define BRIDGE_LO_B_GPIO_RCC RCC_GPIOB 69 | #define BRIDGE_LO_B_GPIO_PORT GPIOB 70 | #define BRIDGE_LO_B_GPIO_PIN GPIO0 71 | 72 | #define BRIDGE_LO_C_GPIO_RCC RCC_GPIOB 73 | #define BRIDGE_LO_C_GPIO_PORT GPIOB 74 | #define BRIDGE_LO_C_GPIO_PIN GPIO1 75 | 76 | #define COMPARATOR_ZC_IRQ NVIC_ADC_COMP_IRQ 77 | #define COMPARATOR_BLANK_TIMER_RCC RCC_TIM17 78 | #define COMPARATOR_BLANK_TIMER TIM17 79 | #define COMPARATOR_BLANK_IRQ NVIC_TIM17_IRQ 80 | 81 | #define ADC_RCC RCC_ADC 82 | 83 | #define ADC_CHANNEL_PHASEC_VOLTAGE 0 84 | #define ADC_CHANNEL_NEUTRAL_VOLTAGE 1 85 | #define ADC_CHANNEL_BUS_VOLTAGE 3 86 | #define ADC_CHANNEL_PHASEA_VOLTAGE 4 87 | #define ADC_CHANNEL_PHASEB_VOLTAGE 5 88 | #define ADC_CHANNEL_BUS_CURRENT 6 89 | #define ADC_CHANNEL_TEMPERATURE 16 90 | 91 | #define ADC_IDX_PHASEC_VOLTAGE 0 92 | #define ADC_IDX_NEUTRAL_VOLTAGE 1 93 | #define ADC_IDX_BUS_VOLTAGE 2 94 | #define ADC_IDX_PHASEA_VOLTAGE 3 95 | #define ADC_IDX_PHASEB_VOLTAGE 4 96 | #define ADC_IDX_BUS_CURRENT 5 97 | #define ADC_IDX_TEMPERATURE 6 98 | 99 | #define PHASEC_VOLTAGE_GPIO_PIN GPIO0 100 | #define NEUTRAL_VOLTAGE_GPIO_PIN GPIO1 101 | #define BUS_VOLTAGE_GPIO_PIN GPIO3 102 | #define PHASEA_VOLTAGE_GPIO_PIN GPIO4 103 | #define PHASEB_VOLTAGE_GPIO_PIN GPIO5 104 | #define BUS_CURRENT_GPIO_PIN GPIO6 105 | 106 | #define ADC_GPIOA_PINS PHASEC_VOLTAGE_GPIO_PIN | \ 107 | NEUTRAL_VOLTAGE_GPIO_PIN | \ 108 | BUS_VOLTAGE_GPIO_PIN | \ 109 | PHASEA_VOLTAGE_GPIO_PIN | \ 110 | PHASEB_VOLTAGE_GPIO_PIN | \ 111 | BUS_CURRENT_GPIO_PIN 112 | 113 | // adc threshold for current channel 114 | #define ADC_WWDG_CURRENT_MAX 400 115 | 116 | extern uint8_t adc_channels[7]; 117 | -------------------------------------------------------------------------------- /src/target/inc/stm32/common/target.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // requires at least one compare channel for comparator blanking 4 | #define ZC_TIMER TIM14 5 | #define ZC_TIMER_RCC RCC_TIM14 6 | #define ZC_TIMER_IRQ NVIC_TIM14_IRQ 7 | -------------------------------------------------------------------------------- /src/target/inc/stm32/f0/isr.c: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | 13 | void tim14_isr() 14 | { 15 | // timeout waiting for zero-cross, go to open loop 16 | // TODO should load commutation timer default values 17 | if (timer_get_flag(ZC_TIMER, TIM_SR_UIF)) { 18 | comparator_zc_timeout_isr(); 19 | timer_clear_flag(ZC_TIMER, TIM_SR_UIF); 20 | } 21 | } 22 | 23 | void adc_comp_isr() 24 | { 25 | if (exti_get_flag_status(EXTI21)) { 26 | comparator_zc_isr(); // user code 27 | exti_reset_request(EXTI21); 28 | } 29 | if (adc_get_watchdog_flag(ADC1)) { 30 | overcurrent_watchdog_isr(); 31 | adc_clear_watchdog_flag(ADC1); 32 | } 33 | } 34 | 35 | void tim16_isr() 36 | { 37 | if (timer_get_flag(COMMUTATION_TIMER, TIM_SR_UIF)) { 38 | commutation_isr(); 39 | timer_clear_flag(COMMUTATION_TIMER, TIM_SR_UIF); 40 | } 41 | 42 | // check cc1 interrupt 43 | // ccr1 = comparator blanking period 44 | if (timer_get_flag(COMMUTATION_TIMER, TIM_SR_CC1IF)) { 45 | comparator_unblank_isr(); 46 | timer_clear_flag(COMMUTATION_TIMER, TIM_SR_CC1IF); 47 | } 48 | } 49 | 50 | void tim17_isr() { 51 | comparator_blank_complete_isr(); 52 | timer_clear_flag(TIM17, TIM_SR_UIF); 53 | } 54 | 55 | void tim1_cc_isr() { 56 | if (timer_get_flag(TIM1, TIM_SR_CC4IF)) { 57 | adc_start(); 58 | timer_clear_flag(TIM1, TIM_SR_CC4IF); 59 | } 60 | } 61 | 62 | void dma1_channel4_7_dma2_channel3_5_isr() { 63 | usart_dma_transfer_complete_isr(CONSOLE_USART); 64 | // clear isr flag 65 | DMA_IFCR(CONSOLE_TX_DMA) |= DMA_IFCR_CTCIF(CONSOLE_TX_DMA_CHANNEL); 66 | } 67 | 68 | void dma1_channel2_3_dma2_channel1_2_isr() { 69 | usart_dma_transfer_complete_isr(CONSOLE_USART); 70 | // clear isr flag 71 | DMA_IFCR(CONSOLE_TX_DMA) |= DMA_IFCR_CTCIF(CONSOLE_TX_DMA_CHANNEL); 72 | } 73 | 74 | void tim2_isr() { 75 | io_control_timeout_isr(); 76 | timer_clear_flag(TIM2, TIM_SR_UIF); 77 | } 78 | 79 | void tim15_isr() { 80 | io_control_timeout_isr(); 81 | timer_clear_flag(TIM15, TIM_SR_UIF); 82 | } 83 | 84 | void tim3_isr() { 85 | io_control_timeout_isr(); 86 | timer_clear_flag(TIM3, TIM_SR_UIF); 87 | } 88 | -------------------------------------------------------------------------------- /src/target/inc/stm32/f0/target.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "../common/target.h" 4 | 5 | #define COMMUTATION_TIMER TIM16 6 | #define COMMUTATION_TIMER_RCC RCC_TIM16 7 | #define COMMUTATION_TIMER_IRQ NVIC_TIM16_IRQ 8 | 9 | #define OVERCURRENT_WATCHDOG_IRQ NVIC_ADC_COMP_IRQ 10 | -------------------------------------------------------------------------------- /src/target/inc/stm32/f3/isr.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | void comp123_isr() 13 | { 14 | if (exti_get_flag_status(EXTI21)) { 15 | comparator_zc_isr(); // user code 16 | exti_reset_request(EXTI21); 17 | } 18 | } 19 | 20 | void tim6_dac_isr() 21 | { 22 | // timeout waiting for zero-cross, go to open loop 23 | // TODO should load commutation timer default values 24 | if (timer_get_flag(ZC_TIMER, TIM_SR_UIF)) { 25 | comparator_zc_timeout_isr(); 26 | timer_clear_flag(ZC_TIMER, TIM_SR_UIF); 27 | } 28 | } 29 | 30 | void tim1_up_tim16_isr() 31 | { 32 | if (timer_get_flag(COMMUTATION_TIMER, TIM_SR_UIF)) { 33 | commutation_isr(); 34 | timer_clear_flag(COMMUTATION_TIMER, TIM_SR_UIF); 35 | } 36 | 37 | // check cc1 interrupt 38 | // ccr1 = comparator blanking period 39 | if (timer_get_flag(COMMUTATION_TIMER, TIM_SR_CC1IF)) { 40 | comparator_unblank_isr(); 41 | timer_clear_flag(COMMUTATION_TIMER, TIM_SR_CC1IF); 42 | } 43 | } 44 | 45 | void tim1_trg_com_tim17_isr() { 46 | comparator_blank_complete_isr(); 47 | timer_clear_flag(TIM17, TIM_SR_UIF); 48 | } 49 | 50 | void dma1_channel7_isr() { 51 | if (DMA_ISR(CONSOLE_TX_DMA) & DMA_ISR_TCIF(CONSOLE_TX_DMA_CHANNEL)) { 52 | usart_dma_transfer_complete_isr(CONSOLE_USART); 53 | // clear isr flag 54 | DMA_IFCR(CONSOLE_TX_DMA) |= DMA_IFCR_CTCIF(CONSOLE_TX_DMA_CHANNEL); 55 | } 56 | } 57 | 58 | void adc1_2_isr() { 59 | if (ADC_ISR(ADC1) & ADC_ISR_AWD1) { 60 | overcurrent_watchdog_isr(); 61 | ADC_ISR(ADC1) = ADC_ISR_AWD1; 62 | } 63 | } 64 | 65 | void tim1_cc_isr() { 66 | if (timer_get_flag(TIM1, TIM_SR_CC4IF)) { 67 | adc_start(); 68 | timer_clear_flag(TIM1, TIM_SR_CC4IF); 69 | } 70 | } 71 | 72 | void tim2_isr() { 73 | io_control_timeout_isr(); 74 | timer_clear_flag(TIM2, TIM_SR_UIF); 75 | } 76 | -------------------------------------------------------------------------------- /src/target/inc/stm32/f3/target.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #define ZC_TIMER TIM6 4 | #define ZC_TIMER_RCC RCC_TIM6 5 | #define ZC_TIMER_IRQ NVIC_TIM6_DAC_IRQ 6 | 7 | #define COMMUTATION_TIMER TIM16 8 | #define COMMUTATION_TIMER_RCC RCC_TIM16 9 | #define COMMUTATION_TIMER_IRQ NVIC_TIM1_UP_TIM16_IRQ 10 | 11 | #define OVERCURRENT_WATCHDOG_IRQ NVIC_ADC1_2_IRQ 12 | -------------------------------------------------------------------------------- /src/target/inc/stm32/g4/isr.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | 14 | void comp123_isr() 15 | { 16 | if (exti_get_flag_status(EXTI21)) { 17 | comparator_zc_isr(); // user code 18 | exti_reset_request(EXTI21); 19 | } 20 | } 21 | 22 | void tim6_dac13_underrun_isr() 23 | { 24 | // timeout waiting for zero-cross, go to open loop 25 | // TODO should load commutation timer default values 26 | if (timer_get_flag(ZC_TIMER, TIM_SR_UIF)) { 27 | //comparator_zc_timeout_isr(); 28 | timer_clear_flag(ZC_TIMER, TIM_SR_UIF); 29 | } 30 | } 31 | 32 | void tim1_up_tim16_isr() 33 | { 34 | if (timer_get_flag(COMMUTATION_TIMER, TIM_SR_UIF)) { 35 | commutation_isr(); 36 | timer_clear_flag(COMMUTATION_TIMER, TIM_SR_UIF); 37 | } 38 | 39 | // check cc1 interrupt 40 | // ccr1 = comparator blanking period 41 | if (timer_get_flag(COMMUTATION_TIMER, TIM_SR_CC1IF)) { 42 | //comparator_unblank_isr(); 43 | timer_clear_flag(COMMUTATION_TIMER, TIM_SR_CC1IF); 44 | } 45 | } 46 | 47 | void tim1_trg_com_tim17_isr() { 48 | comparator_blank_complete_isr(); 49 | timer_clear_flag(TIM17, TIM_SR_UIF); 50 | } 51 | 52 | void dma1_channel5_isr() { 53 | if (DMA_ISR(CONSOLE_TX_DMA) & DMA_ISR_TCIF(CONSOLE_TX_DMA_CHANNEL)) { 54 | usart_dma_transfer_complete_isr(CONSOLE_USART); 55 | // clear isr flag 56 | DMA_IFCR(CONSOLE_TX_DMA) |= DMA_IFCR_CTCIF(CONSOLE_TX_DMA_CHANNEL); 57 | } 58 | } 59 | 60 | void adc1_2_isr() { 61 | if (ADC_ISR(ADC1) & ADC_ISR_AWD1) { 62 | overcurrent_watchdog_isr(); 63 | ADC_ISR(ADC1) = ADC_ISR_AWD1; 64 | } 65 | } 66 | 67 | void tim1_cc_isr() { 68 | if (timer_get_flag(TIM1, TIM_SR_CC4IF)) { 69 | adc_start(); 70 | timer_clear_flag(TIM1, TIM_SR_CC4IF); 71 | } 72 | } 73 | 74 | void tim2_isr() { 75 | io_control_timeout_isr(); 76 | timer_clear_flag(TIM2, TIM_SR_UIF); 77 | } 78 | -------------------------------------------------------------------------------- /src/target/inc/stm32/g4/target.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #define ZC_TIMER TIM6 4 | #define ZC_TIMER_RCC RCC_TIM6 5 | #define ZC_TIMER_IRQ NVIC_TIM6_DAC13_UNDERRUN_IRQ 6 | 7 | #define COMMUTATION_TIMER TIM16 8 | #define COMMUTATION_TIMER_RCC RCC_TIM16 9 | #define COMMUTATION_TIMER_IRQ NVIC_TIM1_UP_TIM16_IRQ 10 | 11 | #define OVERCURRENT_WATCHDOG_IRQ NVIC_ADC1_2_IRQ 12 | -------------------------------------------------------------------------------- /src/target/nucleo-f042/target-mcu.cmake: -------------------------------------------------------------------------------- 1 | set(TARGET_MCU "STM32F042K6") 2 | 3 | set(COMPARATOR_SRC ${PROJECT_SOURCE_DIR}/src/hal/src/stm32/common/comparator-internal.c) 4 | -------------------------------------------------------------------------------- /src/target/nucleo-f042/target.c: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | uint8_t adc_channels[7] = { 4 | ADC_CHANNEL_PHASEC_VOLTAGE, 5 | ADC_CHANNEL_NEUTRAL_VOLTAGE, 6 | ADC_CHANNEL_BUS_VOLTAGE, 7 | ADC_CHANNEL_PHASEA_VOLTAGE, 8 | ADC_CHANNEL_PHASEB_VOLTAGE, 9 | ADC_CHANNEL_BUS_CURRENT, 10 | ADC_CHANNEL_TEMPERATURE, 11 | }; 12 | -------------------------------------------------------------------------------- /src/target/nucleo-f042/target.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "../inc/stm32/f0/target.h" 4 | #include 5 | 6 | #define FEEDBACK_COMPARATOR 7 | 8 | #define LED_GPIO_RCC RCC_GPIOB 9 | #define LED_GPIO_PORT GPIOB 10 | #define LED_GPIO_PIN GPIO3 11 | 12 | #define DBG0_GPIO_RCC RCC_GPIOB 13 | #define DBG0_GPIO_PORT GPIOB 14 | #define DBG0_GPIO_PIN GPIO5 15 | 16 | #define DBG1_GPIO_RCC RCC_GPIOB 17 | #define DBG1_GPIO_PORT GPIOB 18 | #define DBG1_GPIO_PIN GPIO6 19 | 20 | #define DBG2_GPIO_RCC RCC_GPIOB 21 | #define DBG2_GPIO_PORT GPIOB 22 | #define DBG2_GPIO_PIN GPIO7 23 | 24 | #define CONSOLE_USART USART2 25 | #define CONSOLE_USART_RCC RCC_USART2 26 | #define CONSOLE_TX_GPIO_AF GPIO_AF1 27 | #define CONSOLE_TX_GPIO_RCC RCC_GPIOA 28 | #define CONSOLE_TX_GPIO_PORT GPIOA 29 | #define CONSOLE_TX_GPIO_PIN GPIO2 30 | #define CONSOLE_TX_DMA_RCC RCC_DMA1 31 | #define CONSOLE_TX_DMA DMA1 32 | #define CONSOLE_TX_DMA_CHANNEL DMA_CHANNEL4 33 | #define CONSOLE_TX_DMA_IRQ NVIC_DMA1_CHANNEL4_7_DMA2_CHANNEL3_5_IRQ 34 | 35 | #define CONSOLE_RX_GPIO_AF GPIO_AF1 36 | #define CONSOLE_RX_GPIO_RCC RCC_GPIOA 37 | #define CONSOLE_RX_GPIO_PORT GPIOA 38 | #define CONSOLE_RX_GPIO_PIN GPIO15 39 | #define CONSOLE_RX_DMA_RCC RCC_DMA1 40 | #define CONSOLE_RX_DMA DMA1 41 | #define CONSOLE_RX_DMA_CHANNEL DMA_CHANNEL5 42 | #define CONSOLE_RX_DMA_IRQ NVIC_DMA1_CHANNEL4_7_DMA2_CHANNEL3_5_IRQ 43 | 44 | #define PWM_INPUT_GPIO_RCC RCC_GPIOA 45 | #define PWM_INPUT_GPIO_PORT GPIOA 46 | #define PWM_INPUT_GPIO_PIN GPIO5 47 | #define PWM_INPUT_GPIO_AF GPIO_AF2 48 | 49 | #define PWM_INPUT_TIMER_RCC RCC_TIM2 50 | #define PWM_INPUT_TIMER TIM2 51 | #define PWM_INPUT_TIMER_IRQ NVIC_TIM2_IRQ 52 | #define PWM_INPUT_TIMER_IC_ID_RISE TIM_IC1 53 | #define PWM_INPUT_TIMER_IC_ID_FALL TIM_IC2 54 | #define PWM_INPUT_TIMER_IC_TRIGGER TIM_IC_IN_TI1 55 | #define PWM_INPUT_TIMER_OC_ID_FALL TIM_OC2 56 | #define PWM_INPUT_TIMER_SLAVE_MODE TIM_SMCR_SMS_RM 57 | #define PWM_INPUT_TIMER_SLAVE_TRIGGER TIM_SMCR_TS_TI1FP1 58 | #define PWM_INPUT_DUTY_CCR TIM_CCR2 59 | #define PWM_INPUT_PERIOD_CCR TIM_CCR1 60 | 61 | #define BRIDGE_HI_A_GPIO_RCC RCC_GPIOA 62 | #define BRIDGE_HI_A_GPIO_PORT GPIOA 63 | #define BRIDGE_HI_A_GPIO_PIN GPIO8 64 | #define BRIDGE_HI_A_GPIO_AF GPIO_AF2 65 | 66 | #define BRIDGE_HI_B_GPIO_RCC RCC_GPIOA 67 | #define BRIDGE_HI_B_GPIO_PORT GPIOA 68 | #define BRIDGE_HI_B_GPIO_PIN GPIO9 69 | #define BRIDGE_HI_B_GPIO_AF GPIO_AF2 70 | 71 | #define BRIDGE_HI_C_GPIO_RCC RCC_GPIOA 72 | #define BRIDGE_HI_C_GPIO_PORT GPIOA 73 | #define BRIDGE_HI_C_GPIO_PIN GPIO10 74 | #define BRIDGE_HI_C_GPIO_AF GPIO_AF2 75 | 76 | #define BRIDGE_LO_A_GPIO_RCC RCC_GPIOA 77 | #define BRIDGE_LO_A_GPIO_PORT GPIOA 78 | #define BRIDGE_LO_A_GPIO_PIN GPIO7 79 | #define BRIDGE_LO_A_GPIO_AF GPIO_AF2 80 | 81 | #define BRIDGE_LO_B_GPIO_RCC RCC_GPIOB 82 | #define BRIDGE_LO_B_GPIO_PORT GPIOB 83 | #define BRIDGE_LO_B_GPIO_PIN GPIO0 84 | #define BRIDGE_LO_B_GPIO_AF GPIO_AF2 85 | 86 | #define BRIDGE_LO_C_GPIO_RCC RCC_GPIOB 87 | #define BRIDGE_LO_C_GPIO_PORT GPIOB 88 | #define BRIDGE_LO_C_GPIO_PIN GPIO1 89 | #define BRIDGE_LO_C_GPIO_AF GPIO_AF2 90 | 91 | #define COMPARATOR_ZC_IRQ NVIC_ADC_COMP_IRQ 92 | #define COMPARATOR_BLANK_TIMER_RCC RCC_TIM17 93 | #define COMPARATOR_BLANK_TIMER TIM17 94 | #define COMPARATOR_BLANK_IRQ NVIC_TIM17_IRQ 95 | 96 | #define ADC_PERIPH ADC1 97 | #define ADC_RCC RCC_ADC1 98 | 99 | #define ADC_CHANNEL_PHASEC_VOLTAGE 0 100 | #define ADC_CHANNEL_NEUTRAL_VOLTAGE 1 101 | #define ADC_CHANNEL_BUS_VOLTAGE 3 102 | #define ADC_CHANNEL_PHASEA_VOLTAGE 4 103 | #define ADC_CHANNEL_PHASEB_VOLTAGE 5 104 | #define ADC_CHANNEL_BUS_CURRENT 6 105 | #define ADC_CHANNEL_TEMPERATURE 16 106 | 107 | #define ADC_IDX_PHASEC_VOLTAGE 0 108 | #define ADC_IDX_NEUTRAL_VOLTAGE 1 109 | #define ADC_IDX_BUS_VOLTAGE 2 110 | #define ADC_IDX_PHASEA_VOLTAGE 3 111 | #define ADC_IDX_PHASEB_VOLTAGE 4 112 | #define ADC_IDX_BUS_CURRENT 5 113 | #define ADC_IDX_TEMPERATURE 6 114 | 115 | #define PHASEC_VOLTAGE_GPIO_PIN GPIO0 116 | #define NEUTRAL_VOLTAGE_GPIO_PIN GPIO1 117 | #define BUS_VOLTAGE_GPIO_PIN GPIO3 118 | #define PHASEA_VOLTAGE_GPIO_PIN GPIO4 119 | #define PHASEB_VOLTAGE_GPIO_PIN GPIO5 120 | #define BUS_CURRENT_GPIO_PIN GPIO6 121 | 122 | #define ADC_GPIOA_PINS PHASEC_VOLTAGE_GPIO_PIN | \ 123 | NEUTRAL_VOLTAGE_GPIO_PIN | \ 124 | BUS_VOLTAGE_GPIO_PIN | \ 125 | PHASEA_VOLTAGE_GPIO_PIN | \ 126 | PHASEB_VOLTAGE_GPIO_PIN | \ 127 | BUS_CURRENT_GPIO_PIN 128 | 129 | // adc threshold for current channel 130 | #define ADC_WWDG_CURRENT_MAX 400 131 | 132 | extern uint8_t adc_channels[7]; 133 | -------------------------------------------------------------------------------- /src/target/nucleo-f072/target-mcu.cmake: -------------------------------------------------------------------------------- 1 | set(TARGET_MCU "STM32F072RB") 2 | 3 | set(COMPARATOR_SRC ${PROJECT_SOURCE_DIR}/src/hal/src/stm32/common/comparator-internal.c) 4 | -------------------------------------------------------------------------------- /src/target/nucleo-f072/target.c: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | uint8_t adc_channels[7] = { 4 | ADC_CHANNEL_PHASEC_VOLTAGE, 5 | ADC_CHANNEL_NEUTRAL_VOLTAGE, 6 | ADC_CHANNEL_PHASEA_VOLTAGE, 7 | ADC_CHANNEL_PHASEB_VOLTAGE, 8 | ADC_CHANNEL_BUS_CURRENT, 9 | ADC_CHANNEL_BUS_VOLTAGE, 10 | ADC_CHANNEL_TEMPERATURE, 11 | }; 12 | -------------------------------------------------------------------------------- /src/target/nucleo-f072/target.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "../inc/stm32/f0/target.h" 4 | #include 5 | 6 | #define FEEDBACK_COMPARATOR 7 | 8 | #define LED_GPIO_RCC RCC_GPIOA 9 | #define LED_GPIO_PORT GPIOA 10 | #define LED_GPIO_PIN GPIO5 // CONFLICTS WITH COMPARATOR 11 | 12 | #define DBG0_GPIO_RCC RCC_GPIOB 13 | #define DBG0_GPIO_PORT GPIOB 14 | #define DBG0_GPIO_PIN GPIO5 15 | 16 | #define DBG1_GPIO_RCC RCC_GPIOB 17 | #define DBG1_GPIO_PORT GPIOB 18 | #define DBG1_GPIO_PIN GPIO6 19 | 20 | #define DBG2_GPIO_RCC RCC_GPIOB 21 | #define DBG2_GPIO_PORT GPIOB 22 | #define DBG2_GPIO_PIN GPIO7 23 | 24 | #define CONSOLE_USART USART2 25 | #define CONSOLE_USART_RCC RCC_USART2 26 | #define CONSOLE_TX_GPIO_AF GPIO_AF1 27 | #define CONSOLE_TX_GPIO_RCC RCC_GPIOA 28 | #define CONSOLE_TX_GPIO_PORT GPIOA 29 | #define CONSOLE_TX_GPIO_PIN GPIO2 30 | #define CONSOLE_TX_DMA_RCC RCC_DMA1 31 | #define CONSOLE_TX_DMA DMA1 32 | #define CONSOLE_TX_DMA_CHANNEL DMA_CHANNEL4 33 | #define CONSOLE_TX_DMA_IRQ NVIC_DMA1_CHANNEL4_7_DMA2_CHANNEL3_5_IRQ 34 | 35 | #define CONSOLE_RX_GPIO_AF GPIO_AF1 36 | #define CONSOLE_RX_GPIO_RCC RCC_GPIOA 37 | #define CONSOLE_RX_GPIO_PORT GPIOA 38 | #define CONSOLE_RX_GPIO_PIN GPIO3 39 | #define CONSOLE_RX_DMA_RCC RCC_DMA1 40 | #define CONSOLE_RX_DMA DMA1 41 | #define CONSOLE_RX_DMA_CHANNEL DMA_CHANNEL5 42 | #define CONSOLE_RX_DMA_IRQ NVIC_DMA1_CHANNEL4_7_DMA2_CHANNEL3_5_IRQ 43 | 44 | #define PWM_INPUT_GPIO_RCC RCC_GPIOA 45 | #define PWM_INPUT_GPIO_PORT GPIOA 46 | #define PWM_INPUT_GPIO_PIN GPIO15 47 | #define PWM_INPUT_GPIO_AF GPIO_AF2 48 | 49 | #define PWM_INPUT_TIMER_RCC RCC_TIM2 50 | #define PWM_INPUT_TIMER TIM2 51 | #define PWM_INPUT_TIMER_IRQ NVIC_TIM2_IRQ 52 | #define PWM_INPUT_TIMER_IC_ID_RISE TIM_IC1 53 | #define PWM_INPUT_TIMER_IC_ID_FALL TIM_IC2 54 | #define PWM_INPUT_TIMER_IC_TRIGGER TIM_IC_IN_TI1 55 | #define PWM_INPUT_TIMER_OC_ID_FALL TIM_OC2 56 | #define PWM_INPUT_TIMER_SLAVE_MODE TIM_SMCR_SMS_RM 57 | #define PWM_INPUT_TIMER_SLAVE_TRIGGER TIM_SMCR_TS_TI1FP1 58 | #define PWM_INPUT_DUTY_CCR TIM_CCR2 59 | #define PWM_INPUT_PERIOD_CCR TIM_CCR1 60 | 61 | #define BRIDGE_HI_A_GPIO_RCC RCC_GPIOA 62 | #define BRIDGE_HI_A_GPIO_PORT GPIOA 63 | #define BRIDGE_HI_A_GPIO_PIN GPIO8 64 | #define BRIDGE_HI_A_GPIO_AF GPIO_AF2 65 | 66 | #define BRIDGE_HI_B_GPIO_RCC RCC_GPIOA 67 | #define BRIDGE_HI_B_GPIO_PORT GPIOA 68 | #define BRIDGE_HI_B_GPIO_PIN GPIO9 69 | #define BRIDGE_HI_B_GPIO_AF GPIO_AF2 70 | 71 | #define BRIDGE_HI_C_GPIO_RCC RCC_GPIOA 72 | #define BRIDGE_HI_C_GPIO_PORT GPIOA 73 | #define BRIDGE_HI_C_GPIO_PIN GPIO10 74 | #define BRIDGE_HI_C_GPIO_AF GPIO_AF2 75 | 76 | #define BRIDGE_LO_A_GPIO_RCC RCC_GPIOA 77 | #define BRIDGE_LO_A_GPIO_PORT GPIOA 78 | #define BRIDGE_LO_A_GPIO_PIN GPIO7 79 | #define BRIDGE_LO_A_GPIO_AF GPIO_AF2 80 | 81 | #define BRIDGE_LO_B_GPIO_RCC RCC_GPIOB 82 | #define BRIDGE_LO_B_GPIO_PORT GPIOB 83 | #define BRIDGE_LO_B_GPIO_PIN GPIO0 84 | #define BRIDGE_LO_B_GPIO_AF GPIO_AF2 85 | 86 | #define BRIDGE_LO_C_GPIO_RCC RCC_GPIOB 87 | #define BRIDGE_LO_C_GPIO_PORT GPIOB 88 | #define BRIDGE_LO_C_GPIO_PIN GPIO1 89 | #define BRIDGE_LO_C_GPIO_AF GPIO_AF2 90 | 91 | #define COMPARATOR_ZC_IRQ NVIC_ADC_COMP_IRQ 92 | #define COMPARATOR_BLANK_TIMER_RCC RCC_TIM17 93 | #define COMPARATOR_BLANK_TIMER TIM17 94 | #define COMPARATOR_BLANK_IRQ NVIC_TIM17_IRQ 95 | 96 | #define ADC_PERIPH ADC1 97 | #define ADC_RCC RCC_ADC1 98 | 99 | #define ADC_CHANNEL_PHASEC_VOLTAGE 0 100 | #define ADC_CHANNEL_NEUTRAL_VOLTAGE 1 101 | #define ADC_CHANNEL_PHASEA_VOLTAGE 4 102 | #define ADC_CHANNEL_PHASEB_VOLTAGE 5 103 | #define ADC_CHANNEL_BUS_CURRENT 6 104 | #define ADC_CHANNEL_BUS_VOLTAGE 10 105 | #define ADC_CHANNEL_TEMPERATURE 16 106 | 107 | #define ADC_IDX_PHASEC_VOLTAGE 0 108 | #define ADC_IDX_NEUTRAL_VOLTAGE 1 109 | #define ADC_IDX_PHASEA_VOLTAGE 2 110 | #define ADC_IDX_PHASEB_VOLTAGE 3 111 | #define ADC_IDX_BUS_CURRENT 4 112 | #define ADC_IDX_BUS_VOLTAGE 5 113 | #define ADC_IDX_TEMPERATURE 6 114 | 115 | #define PHASEC_VOLTAGE_GPIO_PIN GPIO0 116 | #define NEUTRAL_VOLTAGE_GPIO_PIN GPIO1 117 | #define PHASEA_VOLTAGE_GPIO_PIN GPIO4 118 | #define PHASEB_VOLTAGE_GPIO_PIN GPIO5 119 | #define BUS_CURRENT_GPIO_PIN GPIO6 120 | #define BUS_VOLTAGE_GPIO_PIN GPIO0 121 | 122 | #define ADC_GPIOA_PINS PHASEC_VOLTAGE_GPIO_PIN | \ 123 | NEUTRAL_VOLTAGE_GPIO_PIN | \ 124 | PHASEA_VOLTAGE_GPIO_PIN | \ 125 | PHASEB_VOLTAGE_GPIO_PIN | \ 126 | BUS_CURRENT_GPIO_PIN 127 | #define ADC_GPIOC_PINS BUS_VOLTAGE_GPIO_PIN 128 | 129 | // adc threshold for current channel 130 | #define ADC_WWDG_CURRENT_MAX 400 131 | 132 | extern uint8_t adc_channels[7]; 133 | -------------------------------------------------------------------------------- /src/target/nucleo-f334/pinout.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jaxxzer/open-esc-firmware/351193749e93441343dd0239c148da52145f7e74/src/target/nucleo-f334/pinout.png -------------------------------------------------------------------------------- /src/target/nucleo-f334/target-mcu.cmake: -------------------------------------------------------------------------------- 1 | set(TARGET_MCU "STM32F334RE") 2 | 3 | set(COMPARATOR_SRC ${PROJECT_SOURCE_DIR}/src/hal/src/stm32/common/comparator-internal.c) 4 | -------------------------------------------------------------------------------- /src/target/nucleo-f334/target.c: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | uint8_t adc_channels[7] = { 4 | ADC_CHANNEL_PHASEC_VOLTAGE, 5 | ADC_CHANNEL_NEUTRAL_VOLTAGE, 6 | ADC_CHANNEL_BUS_VOLTAGE, 7 | ADC_CHANNEL_PHASEA_VOLTAGE, 8 | ADC_CHANNEL_PHASEB_VOLTAGE, 9 | ADC_CHANNEL_BUS_CURRENT, 10 | ADC_CHANNEL_TEMPERATURE, 11 | }; 12 | -------------------------------------------------------------------------------- /src/target/nucleo-f334/target.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "../inc/stm32/f3/target.h" 4 | #include 5 | 6 | #define FEEDBACK_COMPARATOR 7 | 8 | #define LED_GPIO_RCC RCC_GPIOA 9 | #define LED_GPIO_PORT GPIOA 10 | #define LED_GPIO_PIN GPIO5 11 | 12 | #define DBG0_GPIO_RCC RCC_GPIOB 13 | #define DBG0_GPIO_PORT GPIOB 14 | #define DBG0_GPIO_PIN GPIO5 15 | 16 | #define DBG1_GPIO_RCC RCC_GPIOB 17 | #define DBG1_GPIO_PORT GPIOB 18 | #define DBG1_GPIO_PIN GPIO6 19 | 20 | #define DBG2_GPIO_RCC RCC_GPIOB 21 | #define DBG2_GPIO_PORT GPIOB 22 | #define DBG2_GPIO_PIN GPIO7 23 | 24 | #define CONSOLE_USART USART2 25 | #define CONSOLE_USART_RCC RCC_USART2 26 | #define CONSOLE_TX_GPIO_AF GPIO_AF7 27 | #define CONSOLE_TX_GPIO_RCC RCC_GPIOA 28 | #define CONSOLE_TX_GPIO_PORT GPIOA 29 | #define CONSOLE_TX_GPIO_PIN GPIO2 30 | #define CONSOLE_TX_DMA_RCC RCC_DMA1 31 | #define CONSOLE_TX_DMA DMA1 32 | #define CONSOLE_TX_DMA_CHANNEL DMA_CHANNEL7 33 | #define CONSOLE_TX_DMA_IRQ NVIC_DMA1_CHANNEL7_IRQ 34 | 35 | #define CONSOLE_RX_GPIO_AF GPIO_AF1 36 | #define CONSOLE_RX_GPIO_RCC RCC_GPIOA 37 | #define CONSOLE_RX_GPIO_PORT GPIOA 38 | #define CONSOLE_RX_GPIO_PIN GPIO3 39 | #define CONSOLE_RX_DMA_RCC RCC_DMA1 40 | #define CONSOLE_RX_DMA DMA1 41 | #define CONSOLE_RX_DMA_CHANNEL DMA_CHANNEL6 42 | #define CONSOLE_RX_DMA_IRQ NVIC_DMA1_CHANNEL6_IRQ 43 | 44 | #define PWM_INPUT_GPIO_RCC RCC_GPIOA 45 | #define PWM_INPUT_GPIO_PORT GPIOA 46 | #define PWM_INPUT_GPIO_PIN GPIO15 47 | #define PWM_INPUT_GPIO_AF GPIO_AF1 48 | 49 | #define PWM_INPUT_TIMER_RCC RCC_TIM2 50 | #define PWM_INPUT_TIMER TIM2 51 | #define PWM_INPUT_TIMER_IRQ NVIC_TIM2_IRQ 52 | #define PWM_INPUT_TIMER_IC_ID_RISE TIM_IC1 53 | #define PWM_INPUT_TIMER_IC_ID_FALL TIM_IC2 54 | #define PWM_INPUT_TIMER_IC_TRIGGER TIM_IC_IN_TI1 55 | #define PWM_INPUT_TIMER_OC_ID_FALL TIM_OC2 56 | #define PWM_INPUT_TIMER_SLAVE_MODE TIM_SMCR_SMS_RM 57 | #define PWM_INPUT_TIMER_SLAVE_TRIGGER TIM_SMCR_TS_TI1FP1 58 | #define PWM_INPUT_DUTY_CCR TIM_CCR2 59 | #define PWM_INPUT_PERIOD_CCR TIM_CCR1 60 | 61 | #define BRIDGE_HI_A_GPIO_RCC RCC_GPIOA 62 | #define BRIDGE_HI_A_GPIO_PORT GPIOA 63 | #define BRIDGE_HI_A_GPIO_PIN GPIO8 64 | #define BRIDGE_HI_A_GPIO_AF GPIO_AF6 65 | 66 | #define BRIDGE_HI_B_GPIO_RCC RCC_GPIOA 67 | #define BRIDGE_HI_B_GPIO_PORT GPIOA 68 | #define BRIDGE_HI_B_GPIO_PIN GPIO9 69 | #define BRIDGE_HI_B_GPIO_AF GPIO_AF6 70 | 71 | #define BRIDGE_HI_C_GPIO_RCC RCC_GPIOA 72 | #define BRIDGE_HI_C_GPIO_PORT GPIOA 73 | #define BRIDGE_HI_C_GPIO_PIN GPIO10 74 | #define BRIDGE_HI_C_GPIO_AF GPIO_AF6 75 | 76 | #define BRIDGE_LO_A_GPIO_RCC RCC_GPIOA 77 | #define BRIDGE_LO_A_GPIO_PORT GPIOA 78 | #define BRIDGE_LO_A_GPIO_PIN GPIO7 79 | #define BRIDGE_LO_A_GPIO_AF GPIO_AF6 80 | 81 | #define BRIDGE_LO_B_GPIO_RCC RCC_GPIOB 82 | #define BRIDGE_LO_B_GPIO_PORT GPIOB 83 | #define BRIDGE_LO_B_GPIO_PIN GPIO0 84 | #define BRIDGE_LO_B_GPIO_AF GPIO_AF6 85 | 86 | #define BRIDGE_LO_C_GPIO_RCC RCC_GPIOB 87 | #define BRIDGE_LO_C_GPIO_PORT GPIOB 88 | #define BRIDGE_LO_C_GPIO_PIN GPIO1 89 | #define BRIDGE_LO_C_GPIO_AF GPIO_AF6 90 | 91 | #define COMPARATOR_ZC_IRQ NVIC_COMP123_IRQ 92 | #define COMPARATOR_BLANK_TIMER_RCC RCC_TIM17 93 | #define COMPARATOR_BLANK_TIMER TIM17 94 | #define COMPARATOR_BLANK_IRQ NVIC_TIM1_TRG_COM_TIM17_IRQ 95 | 96 | #define ADC_PERIPH ADC1 97 | #define ADC_RCC RCC_ADC12 98 | 99 | #define ADC_CHANNEL_PHASEC_VOLTAGE 1 100 | #define ADC_CHANNEL_NEUTRAL_VOLTAGE 2 101 | #define ADC_CHANNEL_PHASEA_VOLTAGE 5 102 | #define ADC_CHANNEL_PHASEB_VOLTAGE 6 103 | #define ADC_CHANNEL_BUS_CURRENT 7 // note this is connected to adc2 (no support yet) 104 | #define ADC_CHANNEL_BUS_VOLTAGE 11 105 | #define ADC_CHANNEL_TEMPERATURE 16 106 | 107 | #define ADC_IDX_PHASEC_VOLTAGE 0 108 | #define ADC_IDX_NEUTRAL_VOLTAGE 1 109 | #define ADC_IDX_PHASEA_VOLTAGE 2 110 | #define ADC_IDX_PHASEB_VOLTAGE 3 111 | #define ADC_IDX_BUS_CURRENT 4 112 | #define ADC_IDX_BUS_VOLTAGE 5 113 | #define ADC_IDX_TEMPERATURE 6 114 | 115 | #define PHASEC_VOLTAGE_GPIO_PIN GPIO0 116 | #define NEUTRAL_VOLTAGE_GPIO_PIN GPIO1 117 | #define PHASEA_VOLTAGE_GPIO_PIN GPIO4 118 | #define PHASEB_VOLTAGE_GPIO_PIN GPIO5 119 | #define BUS_CURRENT_GPIO_PIN GPIO6 120 | #define BUS_VOLTAGE_GPIO_PIN GPIO0 121 | 122 | #define ADC_GPIOA_PINS PHASEC_VOLTAGE_GPIO_PIN | \ 123 | NEUTRAL_VOLTAGE_GPIO_PIN | \ 124 | PHASEA_VOLTAGE_GPIO_PIN | \ 125 | PHASEB_VOLTAGE_GPIO_PIN | \ 126 | BUS_CURRENT_GPIO_PIN 127 | #define ADC_GPIOC_PINS BUS_VOLTAGE_GPIO_PIN 128 | 129 | // adc threshold for current channel 130 | #define ADC_WWDG_CURRENT_MAX 400 131 | 132 | extern uint8_t adc_channels[7]; 133 | -------------------------------------------------------------------------------- /src/target/nucleo-g431/pinout.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jaxxzer/open-esc-firmware/351193749e93441343dd0239c148da52145f7e74/src/target/nucleo-g431/pinout.png -------------------------------------------------------------------------------- /src/target/nucleo-g431/target-mcu.cmake: -------------------------------------------------------------------------------- 1 | set(TARGET_MCU "STM32G431CB") 2 | -------------------------------------------------------------------------------- /src/target/nucleo-g431/target.c: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | uint8_t adc_channels[7] = { 4 | ADC_CHANNEL_BUS_VOLTAGE, 5 | ADC_CHANNEL_PHASEB_VOLTAGE, 6 | ADC_CHANNEL_PHASEC_VOLTAGE, 7 | ADC_CHANNEL_PHASEA_VOLTAGE, 8 | }; 9 | -------------------------------------------------------------------------------- /src/target/nucleo-g431/target.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "../inc/stm32/g4/target.h" 4 | #include 5 | 6 | #define FEEDBACK_BEMF 7 | 8 | #define LED_GPIO_RCC RCC_GPIOC 9 | #define LED_GPIO_PORT GPIOC 10 | #define LED_GPIO_PIN GPIO6 11 | 12 | #define DBG0_GPIO_RCC RCC_GPIOB 13 | #define DBG0_GPIO_PORT GPIOB 14 | #define DBG0_GPIO_PIN GPIO5 15 | 16 | #define DBG1_GPIO_RCC RCC_GPIOB 17 | #define DBG1_GPIO_PORT GPIOB 18 | #define DBG1_GPIO_PIN GPIO6 19 | 20 | #define DBG2_GPIO_RCC RCC_GPIOB 21 | #define DBG2_GPIO_PORT GPIOB 22 | #define DBG2_GPIO_PIN GPIO7 23 | 24 | #define CONSOLE_USART USART2 25 | #define CONSOLE_USART_RCC RCC_USART2 26 | #define CONSOLE_TX_GPIO_AF GPIO_AF7 27 | #define CONSOLE_TX_GPIO_RCC RCC_GPIOA 28 | #define CONSOLE_TX_GPIO_PORT GPIOA 29 | #define CONSOLE_TX_GPIO_PIN GPIO2 30 | #define CONSOLE_TX_DMA_RCC RCC_DMA1 31 | #define CONSOLE_TX_DMA DMA1 32 | #define CONSOLE_TX_DMA_CHANNEL DMA_CHANNEL5 33 | #define CONSOLE_TX_DMA_IRQ NVIC_DMA1_CHANNEL5_IRQ 34 | #define CONSOLE_TX_DMAMUX_REQID 27 35 | 36 | #define CONSOLE_RX_GPIO_AF GPIO_AF7 37 | #define CONSOLE_RX_GPIO_RCC RCC_GPIOA 38 | #define CONSOLE_RX_GPIO_PORT GPIOA 39 | #define CONSOLE_RX_GPIO_PIN GPIO3 40 | #define CONSOLE_RX_DMA_RCC RCC_DMA1 41 | #define CONSOLE_RX_DMA DMA1 42 | #define CONSOLE_RX_DMA_CHANNEL DMA_CHANNEL6 43 | #define CONSOLE_RX_DMA_IRQ NVIC_DMA1_CHANNEL6_IRQ 44 | #define CONSOLE_RX_DMAMUX_REQID 26 45 | 46 | #define PWM_INPUT_GPIO_RCC RCC_GPIOA 47 | #define PWM_INPUT_GPIO_PORT GPIOA 48 | #define PWM_INPUT_GPIO_PIN GPIO15 49 | #define PWM_INPUT_GPIO_AF GPIO_AF1 50 | 51 | #define PWM_INPUT_TIMER_RCC RCC_TIM2 52 | #define PWM_INPUT_TIMER TIM2 53 | #define PWM_INPUT_TIMER_IRQ NVIC_TIM2_IRQ 54 | #define PWM_INPUT_TIMER_IC_ID_RISE TIM_IC1 55 | #define PWM_INPUT_TIMER_IC_ID_FALL TIM_IC2 56 | #define PWM_INPUT_TIMER_IC_TRIGGER TIM_IC_IN_TI1 57 | #define PWM_INPUT_TIMER_OC_ID_FALL TIM_OC2 58 | #define PWM_INPUT_TIMER_SLAVE_MODE TIM_SMCR_SMS_RM 59 | #define PWM_INPUT_TIMER_SLAVE_TRIGGER TIM_SMCR_TS_TI1FP1 60 | #define PWM_INPUT_DUTY_CCR TIM_CCR2 61 | #define PWM_INPUT_PERIOD_CCR TIM_CCR1 62 | 63 | #define BRIDGE_HI_A_GPIO_RCC RCC_GPIOA 64 | #define BRIDGE_HI_A_GPIO_PORT GPIOA 65 | #define BRIDGE_HI_A_GPIO_PIN GPIO8 66 | #define BRIDGE_HI_A_GPIO_AF GPIO_AF6 67 | 68 | #define BRIDGE_HI_B_GPIO_RCC RCC_GPIOA 69 | #define BRIDGE_HI_B_GPIO_PORT GPIOA 70 | #define BRIDGE_HI_B_GPIO_PIN GPIO9 71 | #define BRIDGE_HI_B_GPIO_AF GPIO_AF6 72 | 73 | #define BRIDGE_HI_C_GPIO_RCC RCC_GPIOA 74 | #define BRIDGE_HI_C_GPIO_PORT GPIOA 75 | #define BRIDGE_HI_C_GPIO_PIN GPIO10 76 | #define BRIDGE_HI_C_GPIO_AF GPIO_AF6 77 | 78 | #define BRIDGE_LO_A_GPIO_RCC RCC_GPIOC 79 | #define BRIDGE_LO_A_GPIO_PORT GPIOC 80 | #define BRIDGE_LO_A_GPIO_PIN GPIO13 81 | #define BRIDGE_LO_A_GPIO_AF GPIO_AF4 82 | 83 | #define BRIDGE_LO_B_GPIO_RCC RCC_GPIOA 84 | #define BRIDGE_LO_B_GPIO_PORT GPIOA 85 | #define BRIDGE_LO_B_GPIO_PIN GPIO12 86 | #define BRIDGE_LO_B_GPIO_AF GPIO_AF6 87 | 88 | #define BRIDGE_LO_C_GPIO_RCC RCC_GPIOB 89 | #define BRIDGE_LO_C_GPIO_PORT GPIOB 90 | #define BRIDGE_LO_C_GPIO_PIN GPIO15 91 | #define BRIDGE_LO_C_GPIO_AF GPIO_AF4 92 | 93 | #define COMPARATOR_ZC_IRQ NVIC_COMP123_IRQ 94 | #define COMPARATOR_BLANK_TIMER_RCC RCC_TIM17 95 | #define COMPARATOR_BLANK_TIMER TIM17 96 | #define COMPARATOR_BLANK_IRQ NVIC_TIM1_TRG_COM_TIM17_IRQ 97 | 98 | #define ADC_PERIPH ADC2 99 | #define ADC_RCC RCC_ADC12 100 | 101 | #define ADC_CHANNEL_BUS_VOLTAGE 1 102 | #define ADC_CHANNEL_PHASEB_VOLTAGE 5 103 | #define ADC_CHANNEL_PHASEC_VOLTAGE 14 104 | #define ADC_CHANNEL_PHASEA_VOLTAGE 17 105 | 106 | // pb14 bus temperature : ~1:5 107 | // pb12 pot ~1:11 108 | // pa0 bus voltage ~12:1 109 | // pc4 bemf2 ~2:5 110 | // pb11 bemf3 ~12:14 111 | // pa4 bemf1 ~2:17 112 | 113 | #define ADC_IDX_BUS_VOLTAGE 0 114 | #define ADC_IDX_PHASEB_VOLTAGE 1 115 | #define ADC_IDX_PHASEC_VOLTAGE 2 116 | #define ADC_IDX_PHASEA_VOLTAGE 3 117 | 118 | #define PHASEC_VOLTAGE_GPIO_PIN GPIO11 // b11 119 | #define PHASEA_VOLTAGE_GPIO_PIN GPIO4 // a4 120 | #define PHASEB_VOLTAGE_GPIO_PIN GPIO4 // c4 121 | #define BUS_VOLTAGE_GPIO_PIN GPIO0 // a0 122 | 123 | #define ADC_GPIOA_PINS PHASEA_VOLTAGE_GPIO_PIN | \ 124 | BUS_VOLTAGE_GPIO_PIN 125 | 126 | #define ADC_GPIOB_PINS PHASEC_VOLTAGE_GPIO_PIN 127 | 128 | #define ADC_GPIOC_PINS PHASEB_VOLTAGE_GPIO_PIN 129 | 130 | // adc threshold for current channel 131 | #define ADC_WWDG_CURRENT_MAX 400 132 | 133 | extern uint8_t adc_channels[7]; 134 | 135 | #define ADC_DMAMUX_REQID 36 136 | 137 | #define BEMF_DIVIDER_ENABLE_GPIO GPIOB 138 | #define BEMF_DIVIDER_ENABLE_GPIO_RCC RCC_GPIOB 139 | #define BEMF_DIVIDER_ENABLE_GPIO_PIN GPIO5 140 | -------------------------------------------------------------------------------- /src/target/steval-esc002v1/target-mcu.cmake: -------------------------------------------------------------------------------- 1 | set(TARGET_MCU "STM32F031C6") 2 | 3 | set(COMPARATOR_SRC ${PROJECT_SOURCE_DIR}/src/hal/src/stm32/common/comparator-external.c) 4 | -------------------------------------------------------------------------------- /src/target/steval-esc002v1/target.c: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | uint8_t adc_channels[3] = { 4 | ADC_CHANNEL_BUS_VOLTAGE, 5 | ADC_CHANNEL_BUS_CURRENT, 6 | ADC_CHANNEL_TEMPERATURE, 7 | }; 8 | -------------------------------------------------------------------------------- /src/target/steval-esc002v1/target.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | /* 4 | * timer allocation 5 | * TIM1 bridge 6 | * TIM2 7 | * TIM3 pwm-input 8 | * TIM14 zero cross 9 | * TIM16 commutation 10 | * TIM17 11 | */ 12 | 13 | #include "../inc/stm32/f0/target.h" 14 | #include 15 | 16 | #define FEEDBACK_COMPARATOR 17 | 18 | #define LED_GPIO_RCC RCC_GPIOA 19 | #define LED_GPIO_PORT GPIOA 20 | #define LED_GPIO_PIN GPIO0 21 | 22 | #define DBG0_GPIO_RCC RCC_GPIOB 23 | #define DBG0_GPIO_PORT GPIOB 24 | #define DBG0_GPIO_PIN GPIO6 25 | 26 | #define DBG1_GPIO_RCC RCC_GPIOA 27 | #define DBG1_GPIO_PORT GPIOA 28 | #define DBG1_GPIO_PIN GPIO15 29 | 30 | #define DBG2_GPIO_RCC RCC_GPIOB 31 | #define DBG2_GPIO_PORT GPIOB 32 | #define DBG2_GPIO_PIN GPIO7 // conflicts with usart rx 33 | 34 | #define CONSOLE_USART USART1 35 | #define CONSOLE_USART_RCC RCC_USART1 36 | #define CONSOLE_TX_GPIO_AF GPIO_AF0 37 | #define CONSOLE_TX_GPIO_RCC RCC_GPIOB 38 | #define CONSOLE_TX_GPIO_PORT GPIOB 39 | #define CONSOLE_TX_GPIO_PIN GPIO6 40 | #define CONSOLE_TX_DMA_RCC RCC_DMA1 41 | #define CONSOLE_TX_DMA DMA1 42 | #define CONSOLE_TX_DMA_CHANNEL DMA_CHANNEL2 43 | #define CONSOLE_TX_DMA_IRQ NVIC_DMA1_CHANNEL2_3_DMA2_CHANNEL1_2_IRQ 44 | 45 | #define CONSOLE_RX_GPIO_AF GPIO_AF0 46 | #define CONSOLE_RX_GPIO_RCC RCC_GPIOB 47 | #define CONSOLE_RX_GPIO_PORT GPIOB 48 | #define CONSOLE_RX_GPIO_PIN GPIO7 49 | #define CONSOLE_RX_DMA_RCC RCC_DMA1 50 | #define CONSOLE_RX_DMA DMA1 51 | #define CONSOLE_RX_DMA_CHANNEL DMA_CHANNEL3 52 | #define CONSOLE_RX_DMA_IRQ NVIC_DMA1_CHANNEL2_3_DMA2_CHANNEL1_2_IRQ 53 | 54 | #define PWM_INPUT_GPIO_RCC RCC_GPIOA 55 | #define PWM_INPUT_GPIO_PORT GPIOA 56 | #define PWM_INPUT_GPIO_PIN GPIO6 57 | #define PWM_INPUT_GPIO_AF GPIO_AF1 58 | 59 | #define PWM_INPUT_TIMER_RCC RCC_TIM3 60 | #define PWM_INPUT_TIMER TIM3 61 | #define PWM_INPUT_TIMER_IRQ NVIC_TIM3_IRQ 62 | #define PWM_INPUT_TIMER_IC_ID_RISE TIM_IC1 63 | #define PWM_INPUT_TIMER_IC_ID_FALL TIM_IC2 64 | #define PWM_INPUT_TIMER_IC_TRIGGER TIM_IC_IN_TI1 65 | #define PWM_INPUT_TIMER_OC_ID_FALL TIM_OC2 66 | #define PWM_INPUT_TIMER_SLAVE_MODE TIM_SMCR_SMS_RM 67 | #define PWM_INPUT_TIMER_SLAVE_TRIGGER TIM_SMCR_TS_TI1FP1 68 | #define PWM_INPUT_DUTY_CCR TIM_CCR2 69 | #define PWM_INPUT_PERIOD_CCR TIM_CCR1 70 | 71 | #define BRIDGE_HI_A_GPIO_RCC RCC_GPIOA 72 | #define BRIDGE_HI_A_GPIO_PORT GPIOA 73 | #define BRIDGE_HI_A_GPIO_PIN GPIO8 74 | #define BRIDGE_HI_A_GPIO_AF GPIO_AF2 75 | 76 | #define BRIDGE_HI_B_GPIO_RCC RCC_GPIOA 77 | #define BRIDGE_HI_B_GPIO_PORT GPIOA 78 | #define BRIDGE_HI_B_GPIO_PIN GPIO9 79 | #define BRIDGE_HI_B_GPIO_AF GPIO_AF2 80 | 81 | #define BRIDGE_HI_C_GPIO_RCC RCC_GPIOA 82 | #define BRIDGE_HI_C_GPIO_PORT GPIOA 83 | #define BRIDGE_HI_C_GPIO_PIN GPIO10 84 | #define BRIDGE_HI_C_GPIO_AF GPIO_AF2 85 | 86 | #define BRIDGE_LO_A_GPIO_RCC RCC_GPIOB 87 | #define BRIDGE_LO_A_GPIO_PORT GPIOB 88 | #define BRIDGE_LO_A_GPIO_PIN GPIO13 89 | #define BRIDGE_LO_A_GPIO_AF GPIO_AF2 90 | 91 | #define BRIDGE_LO_B_GPIO_RCC RCC_GPIOB 92 | #define BRIDGE_LO_B_GPIO_PORT GPIOB 93 | #define BRIDGE_LO_B_GPIO_PIN GPIO14 94 | #define BRIDGE_LO_B_GPIO_AF GPIO_AF2 95 | 96 | #define BRIDGE_LO_C_GPIO_RCC RCC_GPIOB 97 | #define BRIDGE_LO_C_GPIO_PORT GPIOB 98 | #define BRIDGE_LO_C_GPIO_PIN GPIO15 99 | #define BRIDGE_LO_C_GPIO_AF GPIO_AF2 100 | 101 | #define COMPARATOR_A_GPIO_RCC RCC_GPIOF 102 | #define COMPARATOR_A_GPIO_PORT GPIOF 103 | #define COMPARATOR_A_GPIO_PIN GPIO1 104 | #define COMPARATOR_B_GPIO_RCC RCC_GPIOF 105 | #define COMPARATOR_B_GPIO_PORT GPIOF 106 | #define COMPARATOR_B_GPIO_PIN GPIO0 107 | #define COMPARATOR_C_GPIO_RCC RCC_GPIOB 108 | #define COMPARATOR_C_GPIO_PORT GPIOB 109 | #define COMPARATOR_C_GPIO_PIN GPIO1 110 | 111 | #define COMPARATOR_ZC_IRQ NVIC_ADC_COMP_IRQ 112 | #define COMPARATOR_BLANK_TIMER_RCC RCC_TIM17 113 | #define COMPARATOR_BLANK_TIMER TIM17 114 | #define COMPARATOR_BLANK_IRQ NVIC_TIM17_IRQ 115 | 116 | #define ADC_PERIPH ADC1 117 | #define ADC_RCC RCC_ADC1 118 | 119 | #define ADC_CHANNEL_BUS_VOLTAGE 3 120 | #define ADC_CHANNEL_BUS_CURRENT 4 121 | #define ADC_CHANNEL_TEMPERATURE 16 122 | 123 | #define ADC_IDX_BUS_VOLTAGE 0 124 | #define ADC_IDX_BUS_CURRENT 1 125 | #define ADC_IDX_TEMPERATURE 2 126 | 127 | #define BUS_VOLTAGE_GPIO_PIN GPIO3 128 | #define BUS_CURRENT_GPIO_PIN GPIO4 129 | 130 | #define ADC_GPIOA_PINS BUS_VOLTAGE_GPIO_PIN | \ 131 | BUS_CURRENT_GPIO_PIN 132 | 133 | // TODO from the F0A datasheet: each unused gpio should be programed to output mode low by software after startup 134 | 135 | #define ADC_WWDG_CURRENT_MAX 250 136 | 137 | extern uint8_t adc_channels[3]; 138 | 139 | // trigger platform-specific bridge initialization 140 | // (set gpiof 6 and 7 in order to enable bridge) 141 | // TODO abstract this 142 | #define STSPIN 143 | -------------------------------------------------------------------------------- /src/target/target.cmake: -------------------------------------------------------------------------------- 1 | # target board 2 | if(NOT DEFINED TARGET_BOARD) 3 | set(TARGET_BOARD "wraith32") 4 | message("no TARGET_BOARD defined, using ${TARGET_BOARD}") 5 | endif() 6 | 7 | # target board directory 8 | set(TARGET_DIR "${PROJECT_SOURCE_DIR}/src/target") 9 | 10 | set(TARGET_SRC ${TARGET_DIR}/${TARGET_BOARD}/target.c) 11 | 12 | # include target board CMakeLists 13 | include(${TARGET_DIR}/${TARGET_BOARD}/target-mcu.cmake) 14 | 15 | # target part 16 | # TODO fail if not set 17 | message("TARGET_MCU: ${TARGET_MCU}") 18 | 19 | include(${BOILERPLATE}/src/target-mcu.cmake) 20 | 21 | # target architecture 22 | # TODO fail if not set 23 | message("TARGET_CPU: ${TARGET_CPU}") 24 | 25 | # include target board defintions 26 | include_directories(${TARGET_DIR}/${TARGET_BOARD}) 27 | 28 | if(TARGET_MCU MATCHES "STM32.*") 29 | include_directories(${TARGET_DIR}/inc/stm32/common) 30 | if(TARGET_MCU MATCHES "STM32F0.*") 31 | include_directories(${TARGET_DIR}/inc/stm32/f0) 32 | set(ISR_SRC ${TARGET_DIR}/inc/stm32/f0/isr.c) 33 | elseif(TARGET_MCU MATCHES "STM32F3.*") 34 | include_directories(${TARGET_DIR}/inc/stm32/f3) 35 | set(ISR_SRC ${TARGET_DIR}/inc/stm32/f3/isr.c) 36 | elseif(TARGET_MCU MATCHES "STM32G4.*") 37 | include_directories(${TARGET_DIR}/inc/stm32/g4) 38 | set(ISR_SRC ${TARGET_DIR}/inc/stm32/g4/isr.c) 39 | else() 40 | message("unsupported TARGET_MCU: ${TARGET_MCU}") 41 | endif() 42 | else() 43 | message("unsupported TARGET_MCU: ${TARGET_MCU}") 44 | endif() 45 | -------------------------------------------------------------------------------- /src/target/wraith32/target-mcu.cmake: -------------------------------------------------------------------------------- 1 | set(TARGET_MCU "STM32F051K8") 2 | 3 | set(COMPARATOR_SRC ${PROJECT_SOURCE_DIR}/src/hal/src/stm32/common/comparator-internal.c) 4 | -------------------------------------------------------------------------------- /src/target/wraith32/target.c: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | uint8_t adc_channels[7] = { 4 | ADC_CHANNEL_PHASEC_VOLTAGE, 5 | ADC_CHANNEL_NEUTRAL_VOLTAGE, 6 | ADC_CHANNEL_BUS_VOLTAGE, 7 | ADC_CHANNEL_PHASEA_VOLTAGE, 8 | ADC_CHANNEL_PHASEB_VOLTAGE, 9 | ADC_CHANNEL_BUS_CURRENT, 10 | ADC_CHANNEL_TEMPERATURE, 11 | }; 12 | -------------------------------------------------------------------------------- /src/target/wraith32/target.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | /* 3 | * timer allocations (required channels) 4 | * tim1: bridge pwm (4-5) (adc sync, comparator blanking) 5 | * tim6: zero-cross (0) 6 | * tim15: pwm input (2) 7 | * tim16: commutation (0) 8 | * tim17: arbitrary comparator blanking (0) 9 | */ 10 | 11 | #include "../inc/stm32/f0/target.h" 12 | #include 13 | 14 | #define FEEDBACK_COMPARATOR 15 | 16 | #define LED_GPIO_RCC RCC_GPIOA 17 | #define LED_GPIO_PORT GPIOA 18 | #define LED_GPIO_PIN GPIO2 19 | 20 | #define DBG0_GPIO_RCC RCC_GPIOB 21 | #define DBG0_GPIO_PORT GPIOB 22 | #define DBG0_GPIO_PIN GPIO5 23 | 24 | #define DBG1_GPIO_RCC RCC_GPIOB 25 | #define DBG1_GPIO_PORT GPIOB 26 | #define DBG1_GPIO_PIN GPIO6 // conflicts with usart 27 | 28 | #define DBG2_GPIO_RCC RCC_GPIOB 29 | #define DBG2_GPIO_PORT GPIOB 30 | #define DBG2_GPIO_PIN GPIO7 // conflicts with usart 31 | 32 | #define CONSOLE_USART USART1 33 | #define CONSOLE_USART_RCC RCC_USART1 34 | #define CONSOLE_TX_GPIO_AF GPIO_AF0 35 | #define CONSOLE_TX_GPIO_RCC RCC_GPIOB 36 | #define CONSOLE_TX_GPIO_PORT GPIOB 37 | #define CONSOLE_TX_GPIO_PIN GPIO6 38 | #define CONSOLE_TX_DMA_RCC RCC_DMA1 39 | #define CONSOLE_TX_DMA DMA1 40 | #define CONSOLE_TX_DMA_CHANNEL DMA_CHANNEL2 41 | #define CONSOLE_TX_DMA_IRQ NVIC_DMA1_CHANNEL2_3_DMA2_CHANNEL1_2_IRQ 42 | 43 | #define CONSOLE_RX_GPIO_AF GPIO_AF0 44 | #define CONSOLE_RX_GPIO_RCC RCC_GPIOB 45 | #define CONSOLE_RX_GPIO_PORT GPIOB 46 | #define CONSOLE_RX_GPIO_PIN GPIO7 47 | #define CONSOLE_RX_DMA_RCC RCC_DMA1 48 | #define CONSOLE_RX_DMA DMA1 49 | #define CONSOLE_RX_DMA_CHANNEL DMA_CHANNEL3 50 | #define CONSOLE_RX_DMA_IRQ NVIC_DMA1_CHANNEL2_3_DMA2_CHANNEL1_2_IRQ 51 | 52 | #define PWM_INPUT_GPIO_RCC RCC_GPIOA 53 | #define PWM_INPUT_GPIO_PORT GPIOA 54 | #define PWM_INPUT_GPIO_PIN GPIO2 55 | #define PWM_INPUT_GPIO_AF GPIO_AF0 56 | 57 | #define PWM_INPUT_TIMER_RCC RCC_TIM15 58 | #define PWM_INPUT_TIMER TIM15 59 | #define PWM_INPUT_TIMER_IRQ NVIC_TIM15_IRQ 60 | #define PWM_INPUT_TIMER_IC_ID_RISE TIM_IC1 61 | #define PWM_INPUT_TIMER_IC_ID_FALL TIM_IC2 62 | #define PWM_INPUT_TIMER_IC_TRIGGER TIM_IC_IN_TI1 63 | #define PWM_INPUT_TIMER_OC_ID_FALL TIM_OC2 64 | #define PWM_INPUT_TIMER_SLAVE_MODE TIM_SMCR_SMS_RM 65 | #define PWM_INPUT_TIMER_SLAVE_TRIGGER TIM_SMCR_TS_TI1FP1 66 | #define PWM_INPUT_DUTY_CCR TIM_CCR2 67 | #define PWM_INPUT_PERIOD_CCR TIM_CCR1 68 | 69 | #define BRIDGE_HI_A_GPIO_RCC RCC_GPIOA 70 | #define BRIDGE_HI_A_GPIO_PORT GPIOA 71 | #define BRIDGE_HI_A_GPIO_PIN GPIO8 72 | #define BRIDGE_HI_A_GPIO_AF GPIO_AF2 73 | 74 | #define BRIDGE_HI_B_GPIO_RCC RCC_GPIOA 75 | #define BRIDGE_HI_B_GPIO_PORT GPIOA 76 | #define BRIDGE_HI_B_GPIO_PIN GPIO9 77 | #define BRIDGE_HI_B_GPIO_AF GPIO_AF2 78 | 79 | #define BRIDGE_HI_C_GPIO_RCC RCC_GPIOA 80 | #define BRIDGE_HI_C_GPIO_PORT GPIOA 81 | #define BRIDGE_HI_C_GPIO_PIN GPIO10 82 | #define BRIDGE_HI_C_GPIO_AF GPIO_AF2 83 | 84 | #define BRIDGE_LO_A_GPIO_RCC RCC_GPIOA 85 | #define BRIDGE_LO_A_GPIO_PORT GPIOA 86 | #define BRIDGE_LO_A_GPIO_PIN GPIO7 87 | #define BRIDGE_LO_A_GPIO_AF GPIO_AF2 88 | 89 | #define BRIDGE_LO_B_GPIO_RCC RCC_GPIOB 90 | #define BRIDGE_LO_B_GPIO_PORT GPIOB 91 | #define BRIDGE_LO_B_GPIO_PIN GPIO0 92 | #define BRIDGE_LO_B_GPIO_AF GPIO_AF2 93 | 94 | #define BRIDGE_LO_C_GPIO_RCC RCC_GPIOB 95 | #define BRIDGE_LO_C_GPIO_PORT GPIOB 96 | #define BRIDGE_LO_C_GPIO_PIN GPIO1 97 | #define BRIDGE_LO_C_GPIO_AF GPIO_AF2 98 | 99 | #define COMPARATOR_ZC_IRQ NVIC_ADC_COMP_IRQ 100 | #define COMPARATOR_BLANK_TIMER_RCC RCC_TIM17 101 | #define COMPARATOR_BLANK_TIMER TIM17 102 | #define COMPARATOR_BLANK_IRQ NVIC_TIM17_IRQ 103 | 104 | #define ADC_PERIPH ADC1 105 | #define ADC_RCC RCC_ADC1 106 | 107 | #define ADC_CHANNEL_PHASEC_VOLTAGE 0 108 | #define ADC_CHANNEL_NEUTRAL_VOLTAGE 1 109 | #define ADC_CHANNEL_BUS_VOLTAGE 3 110 | #define ADC_CHANNEL_PHASEA_VOLTAGE 4 111 | #define ADC_CHANNEL_PHASEB_VOLTAGE 5 112 | #define ADC_CHANNEL_BUS_CURRENT 6 113 | #define ADC_CHANNEL_TEMPERATURE 16 114 | 115 | #define ADC_IDX_PHASEC_VOLTAGE 0 116 | #define ADC_IDX_NEUTRAL_VOLTAGE 1 117 | #define ADC_IDX_BUS_VOLTAGE 2 118 | #define ADC_IDX_PHASEA_VOLTAGE 3 119 | #define ADC_IDX_PHASEB_VOLTAGE 4 120 | #define ADC_IDX_BUS_CURRENT 5 121 | #define ADC_IDX_TEMPERATURE 6 122 | 123 | #define PHASEC_VOLTAGE_GPIO_PIN GPIO0 124 | #define NEUTRAL_VOLTAGE_GPIO_PIN GPIO1 125 | #define BUS_VOLTAGE_GPIO_PIN GPIO3 126 | #define PHASEA_VOLTAGE_GPIO_PIN GPIO4 127 | #define PHASEB_VOLTAGE_GPIO_PIN GPIO5 128 | #define BUS_CURRENT_GPIO_PIN GPIO6 129 | 130 | #define ADC_GPIOA_PINS PHASEC_VOLTAGE_GPIO_PIN | \ 131 | NEUTRAL_VOLTAGE_GPIO_PIN | \ 132 | BUS_VOLTAGE_GPIO_PIN | \ 133 | PHASEA_VOLTAGE_GPIO_PIN | \ 134 | PHASEB_VOLTAGE_GPIO_PIN | \ 135 | BUS_CURRENT_GPIO_PIN 136 | 137 | // adc threshold for current channel 138 | #define ADC_WWDG_CURRENT_MAX 200 139 | 140 | extern uint8_t adc_channels[7]; 141 | -------------------------------------------------------------------------------- /src/util/genlookup.py: -------------------------------------------------------------------------------- 1 | import math 2 | 3 | resolution = 300 4 | maxDuty = 10000 5 | 6 | print("#include \nuint16_t lookup[] = {"), 7 | for i in range(0,resolution): 8 | print("%d%s" % (int(maxDuty*(math.sin(2 * math.pi * i / resolution) + 1)/2), ",")) 9 | print("};") 10 | -------------------------------------------------------------------------------- /src/util/openesc.json: -------------------------------------------------------------------------------- 1 | { 2 | "definition": "openesc", 3 | "messages": { 4 | "set": { 5 | "set_device_id": { 6 | "id": "100", 7 | "description": "Set the device ID.", 8 | "payload": [ 9 | { 10 | "name": "device_id", 11 | "type": "u8", 12 | "description": "Device ID (0-254). 255 is reserved for broadcast messages." 13 | } 14 | ] 15 | }, 16 | "set_throttle": { 17 | "id": "101", 18 | "description": "set the throttle signal (12bit)", 19 | "payload": [ 20 | { 21 | "name": "throttle_signal", 22 | "type": "u16", 23 | "description": "the throttle signal! the throttle controls the duty (torque)" 24 | } 25 | ] 26 | } 27 | }, 28 | "get": { 29 | "state": { 30 | "id": "110", 31 | "description": "esc state information", 32 | "payload": [ 33 | { 34 | "name": "phaseA", 35 | "type": "u16", 36 | "description": "phase A adc reading" 37 | }, 38 | { 39 | "name": "phaseB", 40 | "type": "u16", 41 | "description": "phase B adc reading" 42 | }, 43 | { 44 | "name": "phaseC", 45 | "type": "u16", 46 | "description": "phase C adc reading" 47 | }, 48 | { 49 | "name": "neutral", 50 | "type": "u16", 51 | "description": "virtual neutral point adc reading" 52 | }, 53 | { 54 | "name": "voltage", 55 | "type": "u16", 56 | "description": "voltage adc reading" 57 | }, 58 | { 59 | "name": "current", 60 | "type": "u16", 61 | "description": "current adc reading" 62 | }, 63 | { 64 | "name": "throttle", 65 | "type": "u16", 66 | "description": "throttle signal" 67 | }, 68 | { 69 | "name": "commutation_period", 70 | "type": "u16", 71 | "description": "commutation timer period (auto-reload ARR register value)" 72 | }, 73 | { 74 | "name": "flags", 75 | "type": "u16", 76 | "description": "development flags; bit0: comparator state" 77 | }, 78 | { 79 | "name": "comparator_step", 80 | "type": "u8", 81 | "description": "comparator step" 82 | }, 83 | { 84 | "name": "bridge_step", 85 | "type": "u8", 86 | "description": "bridge step" 87 | } 88 | ] 89 | } 90 | }, 91 | "control": {} 92 | } 93 | } 94 | -------------------------------------------------------------------------------- /src/util/sine-lookup.h: -------------------------------------------------------------------------------- 1 | #include 2 | uint16_t lookup[] = { 3 | 5000, 4 | 5104, 5 | 5209, 6 | 5313, 7 | 5418, 8 | 5522, 9 | 5626, 10 | 5730, 11 | 5833, 12 | 5936, 13 | 6039, 14 | 6141, 15 | 6243, 16 | 6344, 17 | 6445, 18 | 6545, 19 | 6644, 20 | 6742, 21 | 6840, 22 | 6937, 23 | 7033, 24 | 7128, 25 | 7223, 26 | 7316, 27 | 7408, 28 | 7500, 29 | 7590, 30 | 7679, 31 | 7766, 32 | 7853, 33 | 7938, 34 | 8022, 35 | 8105, 36 | 8187, 37 | 8267, 38 | 8345, 39 | 8422, 40 | 8498, 41 | 8572, 42 | 8644, 43 | 8715, 44 | 8784, 45 | 8852, 46 | 8918, 47 | 8982, 48 | 9045, 49 | 9105, 50 | 9164, 51 | 9221, 52 | 9276, 53 | 9330, 54 | 9381, 55 | 9431, 56 | 9478, 57 | 9524, 58 | 9567, 59 | 9609, 60 | 9648, 61 | 9686, 62 | 9721, 63 | 9755, 64 | 9786, 65 | 9815, 66 | 9842, 67 | 9867, 68 | 9890, 69 | 9911, 70 | 9929, 71 | 9946, 72 | 9960, 73 | 9972, 74 | 9982, 75 | 9990, 76 | 9995, 77 | 9998, 78 | 10000, 79 | 9998, 80 | 9995, 81 | 9990, 82 | 9982, 83 | 9972, 84 | 9960, 85 | 9946, 86 | 9929, 87 | 9911, 88 | 9890, 89 | 9867, 90 | 9842, 91 | 9815, 92 | 9786, 93 | 9755, 94 | 9721, 95 | 9686, 96 | 9648, 97 | 9609, 98 | 9567, 99 | 9524, 100 | 9478, 101 | 9431, 102 | 9381, 103 | 9330, 104 | 9276, 105 | 9221, 106 | 9164, 107 | 9105, 108 | 9045, 109 | 8982, 110 | 8918, 111 | 8852, 112 | 8784, 113 | 8715, 114 | 8644, 115 | 8572, 116 | 8498, 117 | 8422, 118 | 8345, 119 | 8267, 120 | 8187, 121 | 8105, 122 | 8022, 123 | 7938, 124 | 7853, 125 | 7766, 126 | 7679, 127 | 7590, 128 | 7500, 129 | 7408, 130 | 7316, 131 | 7223, 132 | 7128, 133 | 7033, 134 | 6937, 135 | 6840, 136 | 6742, 137 | 6644, 138 | 6545, 139 | 6445, 140 | 6344, 141 | 6243, 142 | 6141, 143 | 6039, 144 | 5936, 145 | 5833, 146 | 5730, 147 | 5626, 148 | 5522, 149 | 5418, 150 | 5313, 151 | 5209, 152 | 5104, 153 | 5000, 154 | 4895, 155 | 4790, 156 | 4686, 157 | 4581, 158 | 4477, 159 | 4373, 160 | 4269, 161 | 4166, 162 | 4063, 163 | 3960, 164 | 3858, 165 | 3756, 166 | 3655, 167 | 3554, 168 | 3454, 169 | 3355, 170 | 3257, 171 | 3159, 172 | 3062, 173 | 2966, 174 | 2871, 175 | 2776, 176 | 2683, 177 | 2591, 178 | 2500, 179 | 2409, 180 | 2320, 181 | 2233, 182 | 2146, 183 | 2061, 184 | 1977, 185 | 1894, 186 | 1812, 187 | 1732, 188 | 1654, 189 | 1577, 190 | 1501, 191 | 1427, 192 | 1355, 193 | 1284, 194 | 1215, 195 | 1147, 196 | 1081, 197 | 1017, 198 | 954, 199 | 894, 200 | 835, 201 | 778, 202 | 723, 203 | 669, 204 | 618, 205 | 568, 206 | 521, 207 | 475, 208 | 432, 209 | 390, 210 | 351, 211 | 313, 212 | 278, 213 | 244, 214 | 213, 215 | 184, 216 | 157, 217 | 132, 218 | 109, 219 | 88, 220 | 70, 221 | 53, 222 | 39, 223 | 27, 224 | 17, 225 | 9, 226 | 4, 227 | 1, 228 | 0, 229 | 1, 230 | 4, 231 | 9, 232 | 17, 233 | 27, 234 | 39, 235 | 53, 236 | 70, 237 | 88, 238 | 109, 239 | 132, 240 | 157, 241 | 184, 242 | 213, 243 | 244, 244 | 278, 245 | 313, 246 | 351, 247 | 390, 248 | 432, 249 | 475, 250 | 521, 251 | 568, 252 | 618, 253 | 669, 254 | 723, 255 | 778, 256 | 835, 257 | 894, 258 | 954, 259 | 1017, 260 | 1081, 261 | 1147, 262 | 1215, 263 | 1284, 264 | 1355, 265 | 1427, 266 | 1501, 267 | 1577, 268 | 1654, 269 | 1732, 270 | 1812, 271 | 1894, 272 | 1977, 273 | 2061, 274 | 2146, 275 | 2233, 276 | 2320, 277 | 2409, 278 | 2500, 279 | 2591, 280 | 2683, 281 | 2776, 282 | 2871, 283 | 2966, 284 | 3062, 285 | 3159, 286 | 3257, 287 | 3355, 288 | 3454, 289 | 3554, 290 | 3655, 291 | 3756, 292 | 3858, 293 | 3960, 294 | 4063, 295 | 4166, 296 | 4269, 297 | 4373, 298 | 4477, 299 | 4581, 300 | 4686, 301 | 4790, 302 | 4895, 303 | }; 304 | -------------------------------------------------------------------------------- /tools/build-all.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | source tools/travis-ci-functions.sh 4 | 5 | test tools/generate-nvic-all.sh 6 | 7 | test export PATH=$(pwd)/arm9/gcc-arm-none-eabi-9-2019-q4-major/bin:$PATH 8 | 9 | mkdir -p build 10 | pushd build 11 | 12 | # we are excluding gsc until we add GD32F3 support to libopencm3 13 | for TARGET_BOARD in $(ls ../src/target -I inc -I gsc -I target.cmake) 14 | do 15 | echob "selecting TARGET_BOARD: ${TARGET_BOARD}" 16 | rm -f CMakeCache.txt 17 | test cmake -DTARGET_BOARD=${TARGET_BOARD} .. 18 | test make -j$(nproc) 19 | test cmake -DEXAMPLE=audio-beep .. 20 | test make -j$(nproc) 21 | test cmake -DEXAMPLE=usart .. 22 | test make -j$(nproc) 23 | done 24 | popd 25 | -------------------------------------------------------------------------------- /tools/generate-nvic-all.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | pushd lib/stm32-boilerplate/lib/libopencm3 4 | 5 | for PLATFORM in f0 f1 f3 g0 g4 6 | do 7 | ./scripts/irq2nvic_h ./include/libopencm3/stm32/${PLATFORM}/irq.json 8 | done 9 | 10 | popd 11 | -------------------------------------------------------------------------------- /tools/install-arm-toolchain.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | source tools/travis-ci-functions.sh 4 | 5 | [ ! -d arm9 ] || exit 0 6 | mkdir -p arm9 7 | pushd arm9 8 | test wget https://developer.arm.com/-/media/Files/downloads/gnu-rm/9-2019q4/RC2.1/gcc-arm-none-eabi-9-2019-q4-major-x86_64-linux.tar.bz2 9 | test tar -xf gcc-arm-none-eabi-9-2019-q4-major-x86_64-linux.tar.bz2 10 | popd 11 | test export PATH=$(pwd)/arm9/gcc-arm-none-eabi-9-2019-q4-major/bin:$PATH 12 | -------------------------------------------------------------------------------- /tools/travis-ci-functions.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | # print control for echob() 4 | bold=$(tput bold) 5 | normal=$(tput sgr0) 6 | 7 | # counter for travis_fold: https://github.com/travis-ci/travis-ci/issues/2285 8 | if [ -z $testN ]; then testN=0; fi 9 | 10 | # echo with bold text 11 | echob() { 12 | echo "${bold}${@}${normal}" 13 | } 14 | 15 | # run command helper for ci scripts 16 | test() { 17 | echo -en "travis_fold:start:$testN\r \r" 18 | echob "$@" 19 | time "$@" 20 | exitcode=$? 21 | echo -en "travis_fold:end:$testN\r \r" 22 | echob "$@ exited with $exitcode" 23 | if [ $exitcode -ne 0 ]; then exit $exitcode; fi 24 | testN=$(($testN+1)) 25 | } 26 | -------------------------------------------------------------------------------- /tools/travis-ci-script.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | source tools/travis-ci-functions.sh 4 | 5 | test tools/install-arm-toolchain.sh 6 | test tools/build-all.sh 7 | --------------------------------------------------------------------------------