diff --git a/Makefile b/Makefile new file mode 100644 index 000000000..3f6eaf4b2 --- /dev/null +++ b/Makefile @@ -0,0 +1,256 @@ +############################################################################## +# Build global options +# NOTE: Can be overridden externally. +# + +# Compiler options here. +ifeq ($(USE_OPT),) + USE_OPT = -O2 -ggdb -fomit-frame-pointer -falign-functions=16 -std=gnu99 + USE_OPT += -DSTM32F40_41xxx +endif + +# C specific options here (added to USE_OPT). +ifeq ($(USE_COPT),) + USE_COPT = +endif + +# C++ specific options here (added to USE_OPT). +ifeq ($(USE_CPPOPT),) + USE_CPPOPT = -fno-rtti +endif + +# Enable this if you want the linker to remove unused code and data +ifeq ($(USE_LINK_GC),) + USE_LINK_GC = yes +endif + +# If enabled, this option allows to compile the application in THUMB mode. +ifeq ($(USE_THUMB),) + USE_THUMB = yes +endif + +# Enable this if you want to see the full log while compiling. +ifeq ($(USE_VERBOSE_COMPILE),) + USE_VERBOSE_COMPILE = yes +endif + +# +# Build global options +############################################################################## + +############################################################################## +# Architecture or project specific options +# + +# Enables the use of FPU on Cortex-M4. +ifeq ($(USE_FPU),) + USE_FPU = yes +endif + +# Enable this if you really want to use the STM FWLib. +ifeq ($(USE_FWLIB),) + USE_FWLIB = yes +endif + +# +# Architecture or project specific options +############################################################################## + +############################################################################## +# Project, sources and paths +# + +# Define project name here +PROJECT = BLDC_4_ChibiOS + +# Imported source files and paths +CHIBIOS = ../../ChibiOS-RT-master +include $(CHIBIOS)/boards/ST_STM32F4_DISCOVERY/board.mk +include $(CHIBIOS)/os/hal/platforms/STM32F4xx/platform.mk +include $(CHIBIOS)/os/hal/hal.mk +include $(CHIBIOS)/os/ports/GCC/ARMCMx/STM32F4xx/port.mk +include $(CHIBIOS)/os/kernel/kernel.mk + +# Define linker script file here +LDSCRIPT= $(PORTLD)/STM32F407xG.ld +#LDSCRIPT= $(PORTLD)/STM32F407xG_CCM.ld + +# C sources that can be compiled in ARM or THUMB mode depending on the global +# setting. +CSRC = $(PORTSRC) \ + $(KERNSRC) \ + $(TESTSRC) \ + $(HALSRC) \ + $(PLATFORMSRC) \ + $(BOARDSRC) \ + $(CHIBIOS)/os/various/chprintf.c \ + $(CHIBIOS)/os/various/shell.c \ + $(CHIBIOS)/os/various/syscalls.c \ + main.c \ + myUSB.c \ + irq_handlers.c \ + buffer.c \ + comm.c \ + crc.c \ + digital_filter.c \ + ledpwm.c \ + mcpwm.c \ + servo_dec.c \ + utils.c \ + servo.c \ + packet.c \ + terminal.c + +# C++ sources that can be compiled in ARM or THUMB mode depending on the global +# setting. +CPPSRC = + +# C sources to be compiled in ARM mode regardless of the global setting. +# NOTE: Mixing ARM and THUMB mode enables the -mthumb-interwork compiler +# option that results in lower performance and larger code size. +ACSRC = + +# C++ sources to be compiled in ARM mode regardless of the global setting. +# NOTE: Mixing ARM and THUMB mode enables the -mthumb-interwork compiler +# option that results in lower performance and larger code size. +ACPPSRC = + +# C sources to be compiled in THUMB mode regardless of the global setting. +# NOTE: Mixing ARM and THUMB mode enables the -mthumb-interwork compiler +# option that results in lower performance and larger code size. +TCSRC = + +# C sources to be compiled in THUMB mode regardless of the global setting. +# NOTE: Mixing ARM and THUMB mode enables the -mthumb-interwork compiler +# option that results in lower performance and larger code size. +TCPPSRC = + +# List ASM source files here +ASMSRC = $(PORTASM) + +INCDIR = $(PORTINC) $(KERNINC) $(TESTINC) \ + $(HALINC) $(PLATFORMINC) $(BOARDINC) \ + $(CHIBIOS)/os/various/devices_lib/accel \ + $(CHIBIOS)/os/various \ + $(CHIBIOS)/os/various $(CC2520INC) + +# +# Project, sources and paths +############################################################################## + +############################################################################## +# Compiler settings +# + +MCU = cortex-m4 + +#TRGT = arm-elf- +#TRGT = ~/sat/bin/arm-none-eabi- +TRGT = arm-none-eabi- +CC = $(TRGT)gcc +CPPC = $(TRGT)g++ +# Enable loading with g++ only if you need C++ runtime support. +# NOTE: You can use C++ even without C++ support if you are careful. C++ +# runtime support makes code size explode. +LD = $(TRGT)gcc +#LD = $(TRGT)g++ +CP = $(TRGT)objcopy +AS = $(TRGT)gcc -x assembler-with-cpp +OD = $(TRGT)objdump +SIZE = $(TRGT)size +HEX = $(CP) -O ihex +BIN = $(CP) -O binary + +# ARM-specific options here +AOPT = + +# THUMB-specific options here +TOPT = -mthumb -DTHUMB + +# Define C warning options here +CWARN = -Wall -Wextra -Wstrict-prototypes + +# Define C++ warning options here +CPPWARN = -Wall -Wextra + +# +# Compiler settings +############################################################################## + +############################################################################## +# Start of default section +# + +# List all default C defines here, like -D_DEBUG=1 +DDEFS = + +# List all default ASM defines here, like -D_DEBUG=1 +DADEFS = + +# List all default directories to look for include files here +DINCDIR = + +# List the default directory to look for the libraries here +DLIBDIR = + +# List all default libraries here +DLIBS = + +# +# End of default section +############################################################################## + +############################################################################## +# Start of user section +# + +# List all user C define here, like -D_DEBUG=1 +UDEFS = + +# Define ASM defines here +UADEFS = + +# List all user directories here +UINCDIR = + +# List the user directory to look for the libraries here +ULIBDIR = + +# List all user libraries here +ULIBS = -lm + +# +# End of user defines +############################################################################## + +ifeq ($(USE_FPU),yes) + USE_OPT += -mfloat-abi=softfp -mfpu=fpv4-sp-d16 -fsingle-precision-constant -Wdouble-promotion + DDEFS += -DCORTEX_USE_FPU=TRUE +else + DDEFS += -DCORTEX_USE_FPU=FALSE +endif + +ifeq ($(USE_FWLIB),yes) + include $(CHIBIOS)/ext/stdperiph_stm32f4/stm32lib.mk + CSRC += $(STM32SRC) + INCDIR += $(STM32INC) + USE_OPT += -DUSE_STDPERIPH_DRIVER +endif + +include $(CHIBIOS)/os/ports/GCC/ARMCMx/rules.mk + +# Print size +all: + $(SIZE) build/$(PROJECT).elf + +build/$(PROJECT).bin: build/$(PROJECT).elf + $(BIN) build/$(PROJECT).elf build/$(PROJECT).bin + +# Program +upload: build/$(PROJECT).bin + qstlink2 --cli --erase --write build/$(PROJECT).bin + +debug-start: + openocd -f stm32-bv_openocd.cfg + + \ No newline at end of file diff --git a/README.md b/README.md new file mode 100644 index 000000000..16152b31b --- /dev/null +++ b/README.md @@ -0,0 +1 @@ +This is the source code for my custom BLDC controller. I will post a link to a complete tutorial on my homepage soon. diff --git a/buffer.c b/buffer.c new file mode 100644 index 000000000..e69069fba --- /dev/null +++ b/buffer.c @@ -0,0 +1,81 @@ +/* + Copyright 2012-2014 Benjamin Vedder benjamin@vedder.se + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +/* + * buffer.c + * + * Created on: 13 maj 2013 + * Author: benjamin + */ + +#include "buffer.h" + +void buffer_append_int32(uint8_t* buffer, int32_t number, int32_t *index) { + buffer[(*index)++] = number >> 24; + buffer[(*index)++] = number >> 16; + buffer[(*index)++] = number >> 8; + buffer[(*index)++] = number; +} + +void buffer_append_uint32(uint8_t* buffer, uint32_t number, int32_t *index) { + buffer[(*index)++] = number >> 24; + buffer[(*index)++] = number >> 16; + buffer[(*index)++] = number >> 8; + buffer[(*index)++] = number; +} + +void buffer_append_int16(uint8_t* buffer, int16_t number, int32_t *index) { + buffer[(*index)++] = number >> 8; + buffer[(*index)++] = number; +} + +void buffer_append_uint16(uint8_t* buffer, uint16_t number, int32_t *index) { + buffer[(*index)++] = number >> 8; + buffer[(*index)++] = number; +} + +int16_t buffer_get_int16(const uint8_t *buffer, int32_t *index) { + int16_t res = ((uint16_t) buffer[*index]) << 8 | + ((uint16_t) buffer[*index + 1]); + *index += 2; + return res; +} + +uint16_t buffer_get_uint16(const uint8_t *buffer, int32_t *index) { + uint16_t res = ((uint16_t) buffer[*index]) << 8 | + ((uint16_t) buffer[*index + 1]); + *index += 2; + return res; +} + +int32_t buffer_get_int32(const uint8_t *buffer, int32_t *index) { + int32_t res = ((uint32_t) buffer[*index]) << 24 | + ((uint32_t) buffer[*index]) << 16 | + ((uint32_t) buffer[*index]) << 8 | + ((uint32_t) buffer[*index + 1]); + *index += 4; + return res; +} + +uint32_t buffer_get_uint32(const uint8_t *buffer, int32_t *index) { + uint32_t res = ((uint32_t) buffer[*index]) << 24 | + ((uint32_t) buffer[*index]) << 16 | + ((uint32_t) buffer[*index]) << 8 | + ((uint32_t) buffer[*index + 1]); + *index += 4; + return res; +} diff --git a/buffer.h b/buffer.h new file mode 100644 index 000000000..4d5569f7b --- /dev/null +++ b/buffer.h @@ -0,0 +1,22 @@ +/* + * buffer.h + * + * Created on: 13 maj 2013 + * Author: benjamin + */ + +#ifndef BUFFER_H_ +#define BUFFER_H_ + +#include + +void buffer_append_int32(uint8_t* buffer, int32_t number, int32_t *index); +void buffer_append_uint32(uint8_t* buffer, uint32_t number, int32_t *index); +void buffer_append_int16(uint8_t* buffer, int16_t number, int32_t *index); +void buffer_append_uint16(uint8_t* buffer, uint16_t number, int32_t *index); +int16_t buffer_get_int16(const uint8_t *buffer, int32_t *index); +uint16_t buffer_get_uint16(const uint8_t *buffer, int32_t *index); +int32_t buffer_get_int32(const uint8_t *buffer, int32_t *index); +uint32_t buffer_get_uint32(const uint8_t *buffer, int32_t *index); + +#endif /* BUFFER_H_ */ diff --git a/chconf.h b/chconf.h new file mode 100644 index 000000000..5a94d1c3a --- /dev/null +++ b/chconf.h @@ -0,0 +1,538 @@ +/* + ChibiOS/RT - Copyright (C) 2006,2007,2008,2009,2010, + 2011,2012 Giovanni Di Sirio. + + This file is part of ChibiOS/RT. + + ChibiOS/RT is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 3 of the License, or + (at your option) any later version. + + ChibiOS/RT is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ + +/** + * @file templates/chconf.h + * @brief Configuration file template. + * @details A copy of this file must be placed in each project directory, it + * contains the application specific kernel settings. + * + * @addtogroup config + * @details Kernel related settings and hooks. + * @{ + */ + +#ifndef _CHCONF_H_ +#define _CHCONF_H_ + +#define CHPRINTF_USE_FLOAT TRUE +#define CORTEX_SIMPLIFIED_PRIORITY TRUE + +/*===========================================================================*/ +/** + * @name Kernel parameters and options + * @{ + */ +/*===========================================================================*/ + +/** + * @brief System tick frequency. + * @details Frequency of the system timer that drives the system ticks. This + * setting also defines the system tick time unit. + */ +#if !defined(CH_FREQUENCY) || defined(__DOXYGEN__) +#define CH_FREQUENCY 10000 +#endif + +/** + * @brief Round robin interval. + * @details This constant is the number of system ticks allowed for the + * threads before preemption occurs. Setting this value to zero + * disables the preemption for threads with equal priority and the + * round robin becomes cooperative. Note that higher priority + * threads can still preempt, the kernel is always preemptive. + * + * @note Disabling the round robin preemption makes the kernel more compact + * and generally faster. + */ +#if !defined(CH_TIME_QUANTUM) || defined(__DOXYGEN__) +#define CH_TIME_QUANTUM 20 +#endif + +/** + * @brief Managed RAM size. + * @details Size of the RAM area to be managed by the OS. If set to zero + * then the whole available RAM is used. The core memory is made + * available to the heap allocator and/or can be used directly through + * the simplified core memory allocator. + * + * @note In order to let the OS manage the whole RAM the linker script must + * provide the @p __heap_base__ and @p __heap_end__ symbols. + * @note Requires @p CH_USE_MEMCORE. + */ +#if !defined(CH_MEMCORE_SIZE) || defined(__DOXYGEN__) +#define CH_MEMCORE_SIZE 0 +#endif + +/** + * @brief Idle thread automatic spawn suppression. + * @details When this option is activated the function @p chSysInit() + * does not spawn the idle thread automatically. The application has + * then the responsibility to do one of the following: + * - Spawn a custom idle thread at priority @p IDLEPRIO. + * - Change the main() thread priority to @p IDLEPRIO then enter + * an endless loop. In this scenario the @p main() thread acts as + * the idle thread. + * . + * @note Unless an idle thread is spawned the @p main() thread must not + * enter a sleep state. + */ +#if !defined(CH_NO_IDLE_THREAD) || defined(__DOXYGEN__) +#define CH_NO_IDLE_THREAD FALSE +#endif + +/** @} */ + +/*===========================================================================*/ +/** + * @name Performance options + * @{ + */ +/*===========================================================================*/ + +/** + * @brief OS optimization. + * @details If enabled then time efficient rather than space efficient code + * is used when two possible implementations exist. + * + * @note This is not related to the compiler optimization options. + * @note The default is @p TRUE. + */ +#if !defined(CH_OPTIMIZE_SPEED) || defined(__DOXYGEN__) +#define CH_OPTIMIZE_SPEED TRUE +#endif + +/** @} */ + +/*===========================================================================*/ +/** + * @name Subsystem options + * @{ + */ +/*===========================================================================*/ + +/** + * @brief Threads registry APIs. + * @details If enabled then the registry APIs are included in the kernel. + * + * @note The default is @p TRUE. + */ +#if !defined(CH_USE_REGISTRY) || defined(__DOXYGEN__) +#define CH_USE_REGISTRY TRUE +#endif + +/** + * @brief Threads synchronization APIs. + * @details If enabled then the @p chThdWait() function is included in + * the kernel. + * + * @note The default is @p TRUE. + */ +#if !defined(CH_USE_WAITEXIT) || defined(__DOXYGEN__) +#define CH_USE_WAITEXIT TRUE +#endif + +/** + * @brief Semaphores APIs. + * @details If enabled then the Semaphores APIs are included in the kernel. + * + * @note The default is @p TRUE. + */ +#if !defined(CH_USE_SEMAPHORES) || defined(__DOXYGEN__) +#define CH_USE_SEMAPHORES TRUE +#endif + +/** + * @brief Semaphores queuing mode. + * @details If enabled then the threads are enqueued on semaphores by + * priority rather than in FIFO order. + * + * @note The default is @p FALSE. Enable this if you have special requirements. + * @note Requires @p CH_USE_SEMAPHORES. + */ +#if !defined(CH_USE_SEMAPHORES_PRIORITY) || defined(__DOXYGEN__) +#define CH_USE_SEMAPHORES_PRIORITY FALSE +#endif + +/** + * @brief Atomic semaphore API. + * @details If enabled then the semaphores the @p chSemSignalWait() API + * is included in the kernel. + * + * @note The default is @p TRUE. + * @note Requires @p CH_USE_SEMAPHORES. + */ +#if !defined(CH_USE_SEMSW) || defined(__DOXYGEN__) +#define CH_USE_SEMSW TRUE +#endif + +/** + * @brief Mutexes APIs. + * @details If enabled then the mutexes APIs are included in the kernel. + * + * @note The default is @p TRUE. + */ +#if !defined(CH_USE_MUTEXES) || defined(__DOXYGEN__) +#define CH_USE_MUTEXES TRUE +#endif + +/** + * @brief Conditional Variables APIs. + * @details If enabled then the conditional variables APIs are included + * in the kernel. + * + * @note The default is @p TRUE. + * @note Requires @p CH_USE_MUTEXES. + */ +#if !defined(CH_USE_CONDVARS) || defined(__DOXYGEN__) +#define CH_USE_CONDVARS TRUE +#endif + +/** + * @brief Conditional Variables APIs with timeout. + * @details If enabled then the conditional variables APIs with timeout + * specification are included in the kernel. + * + * @note The default is @p TRUE. + * @note Requires @p CH_USE_CONDVARS. + */ +#if !defined(CH_USE_CONDVARS_TIMEOUT) || defined(__DOXYGEN__) +#define CH_USE_CONDVARS_TIMEOUT TRUE +#endif + +/** + * @brief Events Flags APIs. + * @details If enabled then the event flags APIs are included in the kernel. + * + * @note The default is @p TRUE. + */ +#if !defined(CH_USE_EVENTS) || defined(__DOXYGEN__) +#define CH_USE_EVENTS TRUE +#endif + +/** + * @brief Events Flags APIs with timeout. + * @details If enabled then the events APIs with timeout specification + * are included in the kernel. + * + * @note The default is @p TRUE. + * @note Requires @p CH_USE_EVENTS. + */ +#if !defined(CH_USE_EVENTS_TIMEOUT) || defined(__DOXYGEN__) +#define CH_USE_EVENTS_TIMEOUT TRUE +#endif + +/** + * @brief Synchronous Messages APIs. + * @details If enabled then the synchronous messages APIs are included + * in the kernel. + * + * @note The default is @p TRUE. + */ +#if !defined(CH_USE_MESSAGES) || defined(__DOXYGEN__) +#define CH_USE_MESSAGES TRUE +#endif + +/** + * @brief Synchronous Messages queuing mode. + * @details If enabled then messages are served by priority rather than in + * FIFO order. + * + * @note The default is @p FALSE. Enable this if you have special requirements. + * @note Requires @p CH_USE_MESSAGES. + */ +#if !defined(CH_USE_MESSAGES_PRIORITY) || defined(__DOXYGEN__) +#define CH_USE_MESSAGES_PRIORITY FALSE +#endif + +/** + * @brief Mailboxes APIs. + * @details If enabled then the asynchronous messages (mailboxes) APIs are + * included in the kernel. + * + * @note The default is @p TRUE. + * @note Requires @p CH_USE_SEMAPHORES. + */ +#if !defined(CH_USE_MAILBOXES) || defined(__DOXYGEN__) +#define CH_USE_MAILBOXES TRUE +#endif + +/** + * @brief I/O Queues APIs. + * @details If enabled then the I/O queues APIs are included in the kernel. + * + * @note The default is @p TRUE. + */ +#if !defined(CH_USE_QUEUES) || defined(__DOXYGEN__) +#define CH_USE_QUEUES TRUE +#endif + +/** + * @brief Core Memory Manager APIs. + * @details If enabled then the core memory manager APIs are included + * in the kernel. + * + * @note The default is @p TRUE. + */ +#if !defined(CH_USE_MEMCORE) || defined(__DOXYGEN__) +#define CH_USE_MEMCORE TRUE +#endif + +/** + * @brief Heap Allocator APIs. + * @details If enabled then the memory heap allocator APIs are included + * in the kernel. + * + * @note The default is @p TRUE. + * @note Requires @p CH_USE_MEMCORE and either @p CH_USE_MUTEXES or + * @p CH_USE_SEMAPHORES. + * @note Mutexes are recommended. + */ +#if !defined(CH_USE_HEAP) || defined(__DOXYGEN__) +#define CH_USE_HEAP TRUE +#endif + +/** + * @brief C-runtime allocator. + * @details If enabled the the heap allocator APIs just wrap the C-runtime + * @p malloc() and @p free() functions. + * + * @note The default is @p FALSE. + * @note Requires @p CH_USE_HEAP. + * @note The C-runtime may or may not require @p CH_USE_MEMCORE, see the + * appropriate documentation. + */ +#if !defined(CH_USE_MALLOC_HEAP) || defined(__DOXYGEN__) +#define CH_USE_MALLOC_HEAP FALSE +#endif + +/** + * @brief Memory Pools Allocator APIs. + * @details If enabled then the memory pools allocator APIs are included + * in the kernel. + * + * @note The default is @p TRUE. + */ +#if !defined(CH_USE_MEMPOOLS) || defined(__DOXYGEN__) +#define CH_USE_MEMPOOLS TRUE +#endif + +/** + * @brief Dynamic Threads APIs. + * @details If enabled then the dynamic threads creation APIs are included + * in the kernel. + * + * @note The default is @p TRUE. + * @note Requires @p CH_USE_WAITEXIT. + * @note Requires @p CH_USE_HEAP and/or @p CH_USE_MEMPOOLS. + */ +#if !defined(CH_USE_DYNAMIC) || defined(__DOXYGEN__) +#define CH_USE_DYNAMIC TRUE +#endif + +/** @} */ + +/*===========================================================================*/ +/** + * @name Debug options + * @{ + */ +/*===========================================================================*/ + +/** + * @brief Debug option, system state check. + * @details If enabled the correct call protocol for system APIs is checked + * at runtime. + * + * @note The default is @p FALSE. + */ +#if !defined(CH_DBG_SYSTEM_STATE_CHECK) || defined(__DOXYGEN__) +#define CH_DBG_SYSTEM_STATE_CHECK FALSE +#endif + +/** + * @brief Debug option, parameters checks. + * @details If enabled then the checks on the API functions input + * parameters are activated. + * + * @note The default is @p FALSE. + */ +#if !defined(CH_DBG_ENABLE_CHECKS) || defined(__DOXYGEN__) +#define CH_DBG_ENABLE_CHECKS FALSE +#endif + +/** + * @brief Debug option, consistency checks. + * @details If enabled then all the assertions in the kernel code are + * activated. This includes consistency checks inside the kernel, + * runtime anomalies and port-defined checks. + * + * @note The default is @p FALSE. + */ +#if !defined(CH_DBG_ENABLE_ASSERTS) || defined(__DOXYGEN__) +#define CH_DBG_ENABLE_ASSERTS FALSE +#endif + +/** + * @brief Debug option, trace buffer. + * @details If enabled then the context switch circular trace buffer is + * activated. + * + * @note The default is @p FALSE. + */ +#if !defined(CH_DBG_ENABLE_TRACE) || defined(__DOXYGEN__) +#define CH_DBG_ENABLE_TRACE FALSE +#endif + +/** + * @brief Debug option, stack checks. + * @details If enabled then a runtime stack check is performed. + * + * @note The default is @p FALSE. + * @note The stack check is performed in a architecture/port dependent way. + * It may not be implemented or some ports. + * @note The default failure mode is to halt the system with the global + * @p panic_msg variable set to @p NULL. + */ +#if !defined(CH_DBG_ENABLE_STACK_CHECK) || defined(__DOXYGEN__) +#define CH_DBG_ENABLE_STACK_CHECK FALSE +#endif + +/** + * @brief Debug option, stacks initialization. + * @details If enabled then the threads working area is filled with a byte + * value when a thread is created. This can be useful for the + * runtime measurement of the used stack. + * + * @note The default is @p FALSE. + */ +#if !defined(CH_DBG_FILL_THREADS) || defined(__DOXYGEN__) +#define CH_DBG_FILL_THREADS FALSE +#endif + +/** + * @brief Debug option, threads profiling. + * @details If enabled then a field is added to the @p Thread structure that + * counts the system ticks occurred while executing the thread. + * + * @note The default is @p TRUE. + * @note This debug option is defaulted to TRUE because it is required by + * some test cases into the test suite. + */ +#if !defined(CH_DBG_THREADS_PROFILING) || defined(__DOXYGEN__) +#define CH_DBG_THREADS_PROFILING TRUE +#endif + +/** @} */ + +/*===========================================================================*/ +/** + * @name Kernel hooks + * @{ + */ +/*===========================================================================*/ + +/** + * @brief Threads descriptor structure extension. + * @details User fields added to the end of the @p Thread structure. + */ +#if !defined(THREAD_EXT_FIELDS) || defined(__DOXYGEN__) +#define THREAD_EXT_FIELDS \ + /* Add threads custom fields here.*/ +#endif + +/** + * @brief Threads initialization hook. + * @details User initialization code added to the @p chThdInit() API. + * + * @note It is invoked from within @p chThdInit() and implicitly from all + * the threads creation APIs. + */ +#if !defined(THREAD_EXT_INIT_HOOK) || defined(__DOXYGEN__) +#define THREAD_EXT_INIT_HOOK(tp) { \ + /* Add threads initialization code here.*/ \ +} +#endif + +/** + * @brief Threads finalization hook. + * @details User finalization code added to the @p chThdExit() API. + * + * @note It is inserted into lock zone. + * @note It is also invoked when the threads simply return in order to + * terminate. + */ +#if !defined(THREAD_EXT_EXIT_HOOK) || defined(__DOXYGEN__) +#define THREAD_EXT_EXIT_HOOK(tp) { \ + /* Add threads finalization code here.*/ \ +} +#endif + +/** + * @brief Context switch hook. + * @details This hook is invoked just before switching between threads. + */ +#if !defined(THREAD_CONTEXT_SWITCH_HOOK) || defined(__DOXYGEN__) +#define THREAD_CONTEXT_SWITCH_HOOK(ntp, otp) { \ + /* System halt code here.*/ \ +} +#endif + +/** + * @brief Idle Loop hook. + * @details This hook is continuously invoked by the idle thread loop. + */ +#if !defined(IDLE_LOOP_HOOK) || defined(__DOXYGEN__) +#define IDLE_LOOP_HOOK() { \ + /* Idle loop code here.*/ \ +} +#endif + +/** + * @brief System tick event hook. + * @details This hook is invoked in the system tick handler immediately + * after processing the virtual timers queue. + */ +#if !defined(SYSTEM_TICK_EVENT_HOOK) || defined(__DOXYGEN__) +#define SYSTEM_TICK_EVENT_HOOK() { \ + /* System tick event code here.*/ \ +} +#endif + +/** + * @brief System halt hook. + * @details This hook is invoked in case to a system halting error before + * the system is halted. + */ +#if !defined(SYSTEM_HALT_HOOK) || defined(__DOXYGEN__) +#define SYSTEM_HALT_HOOK() { \ + /* System halt code here.*/ \ +} +#endif + +/** @} */ + +/*===========================================================================*/ +/* Port-specific settings (override port settings defaulted in chcore.h). */ +/*===========================================================================*/ + +#endif /* _CHCONF_H_ */ + +/** @} */ diff --git a/comm.c b/comm.c new file mode 100644 index 000000000..526edaf2a --- /dev/null +++ b/comm.c @@ -0,0 +1,328 @@ +/* + Copyright 2012-2014 Benjamin Vedder benjamin@vedder.se + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +/* + * comm.c + * + * Created on: 22 nov 2012 + * Author: benjamin + */ + +#include "ch.h" +#include "hal.h" +#include "comm.h" +#include "main.h" +#include "stm32f4xx_conf.h" +#include "mcpwm.h" +#include "servo.h" +#include "buffer.h" +#include "packet.h" +#include "myUSB.h" +#include "terminal.h" + +#include +#include + +// Settings +#define PACKET_BUFFER_LEN 30 +#define PRINT_BUFFER_LEN 10 +#define PRINT_MAXLEN 240 + +// Private variables +#define SERIAL_RX_BUFFER_SIZE 4096 +static uint8_t serial_rx_buffer[SERIAL_RX_BUFFER_SIZE]; +static int serial_rx_read_pos = 0; +static int serial_rx_write_pos = 0; +static WORKING_AREA(serial_read_thread_wa, 1024); +static WORKING_AREA(serial_process_thread_wa, 2048); +static WORKING_AREA(timer_thread_wa, 128); +static Mutex send_mutex; +static Thread *process_tp; + +// Private functions +static void send_ping(void); +static void handle_res_packet(unsigned char *data, unsigned char len); +static void handle_nores_packet(unsigned char *data, unsigned char len); +static void process_packet(unsigned char *buffer, unsigned char len); +static void send_packet(unsigned char *buffer, unsigned char len); + +static msg_t serial_read_thread(void *arg) { + (void)arg; + + chRegSetThreadName("Serial read"); + + uint8_t buffer[128]; + int i; + int len; + int had_data = 0; + + for(;;) { + len = chSequentialStreamRead(&SDU1, (uint8_t*) buffer, 1); + + for (i = 0;i < len;i++) { + serial_rx_buffer[serial_rx_write_pos++] = buffer[i]; + + if (serial_rx_write_pos == SERIAL_RX_BUFFER_SIZE) { + serial_rx_write_pos = 0; + } + + had_data = 1; + } + + if (had_data) { + chEvtSignal(process_tp, (eventmask_t) 1); + had_data = 0; + } + } + + return 0; +} + +static msg_t serial_process_thread(void *arg) { + (void)arg; + + chRegSetThreadName("Serial process"); + + process_tp = chThdSelf(); + + for(;;) { + chEvtWaitAny((eventmask_t) 1); + + while (serial_rx_read_pos != serial_rx_write_pos) { + packet_process_byte(serial_rx_buffer[serial_rx_read_pos++]); + + if (serial_rx_read_pos == SERIAL_RX_BUFFER_SIZE) { + serial_rx_read_pos = 0; + } + } + } + + return 0; +} + +static msg_t timer_thread(void *arg) { + (void)arg; + + chRegSetThreadName("Serial timer"); + + for(;;) { + packet_timerfunc(); + chThdSleepMilliseconds(1); + } + + return 0; +} + +static void process_packet(unsigned char *buffer, unsigned char len) { + if (!len) { + return; + } + + switch (buffer[0]) { + case 0: + // Send the received data to the main loop + main_process_packet(buffer + 1, len - 1); + break; + + case 1: + /* + * Packet that expects response + */ + handle_res_packet(buffer + 1, len - 1); + break; + + case 2: + /* + * Packet that expects no response + */ + handle_nores_packet(buffer + 1, len - 1); + break; + + default: + break; + } +} + +static void send_packet(unsigned char *buffer, unsigned char len) { + chMtxLock(&send_mutex); + chSequentialStreamWrite(&SDU1, buffer, len); + chMtxUnlock(); +} + +void comm_init(void) { + myUSBinit(); + packet_init(send_packet, process_packet); + + chMtxInit(&send_mutex); + + // Threads + chThdCreateStatic(serial_read_thread_wa, sizeof(serial_read_thread_wa), NORMALPRIO, serial_read_thread, NULL); + chThdCreateStatic(serial_process_thread_wa, sizeof(serial_process_thread_wa), NORMALPRIO, serial_process_thread, NULL); + chThdCreateStatic(timer_thread_wa, sizeof(timer_thread_wa), NORMALPRIO, timer_thread, NULL); +} + +static void send_ping(void) { + unsigned char byte = CAR_PACKET_PING; + packet_send_packet(&byte, 1); +} + +static void handle_res_packet(unsigned char *data, unsigned char len) { + CAR_RES_PACKET_ID car_res_packet; + uint8_t buffer2[256]; + int32_t index; + + (void)len; + + car_res_packet = data[0]; + data++; + len--; + + switch (car_res_packet) { + case CAR_PACKET_READ_VALUES: + index = 0; + buffer2[index++] = CAR_PACKET_READ_VALUES; + buffer_append_int16(buffer2, (int16_t)(37.0 * 10.0), &index); + buffer_append_int16(buffer2, (int16_t)(38.0 * 10.0), &index); + buffer_append_int16(buffer2, (int16_t)(39.0 * 10.0), &index); + buffer_append_int16(buffer2, (int16_t)(40.0 * 10.0), &index); + buffer_append_int16(buffer2, (int16_t)(41.0 * 10.0), &index); + buffer_append_int16(buffer2, (int16_t)(42.0 * 10.0), &index); + buffer_append_int16(buffer2, (int16_t)(43.0 * 10.0), &index); + buffer_append_int32(buffer2, (int32_t)(mcpwm_read_reset_avg_motor_current() * 100.0), &index); + buffer_append_int32(buffer2, (int32_t)(mcpwm_read_reset_avg_input_current() * 100.0), &index); + buffer_append_int16(buffer2, (int16_t)(mcpwm_get_dutycycle_now() * 1000.0), &index); + buffer_append_int32(buffer2, (int32_t)mcpwm_get_rpm(), &index); + buffer_append_int16(buffer2, (int16_t)(GET_INPUT_VOLTAGE * 10.0), &index); + packet_send_packet(buffer2, index); + break; + + case CAR_PACKET_READ_POS: + /* + * Read dead reckoning position. + */ + + break; + + case CAR_PACKET_READ_SENS_ULTRA: + + break; + + case CAR_PACKET_READ_SENS_IR: + + break; + + case CAR_PACKET_READ_TRAVEL_COUNTER: + + break; + + case CAR_PACKET_PING: + send_ping(); + break; + + default: + break; + } +} + +static void handle_nores_packet(unsigned char *data, unsigned char len) { + CAR_NORES_PACKET_ID car_nores_packet; + + (void)len; + + car_nores_packet = data[0]; + data++; + len--; + + switch (car_nores_packet) { + case CAR_PACKET_SET_POWER_SERVO: + + break; + + case CAR_PACKET_WRITE_POS: + + break; + + case CAR_PACKET_ADD_POINT: + + break; + + case CAR_PACKET_AP_RUN: + + break; + + case CAR_PACKET_AP_CLEAR: + + break; + + case CAR_PACKET_RESET_TRAVEL_CNT: + + break; + + case CAR_PACKET_SET_LIMITED: + + break; + + case CAR_PACKET_FULL_BRAKE: + mcpwm_full_brake(); + break; + + case CAR_PACKET_SERVO_OFFSET: + servos[0].offset = data[0]; + break; + + case CAR_PACKET_CAN_TEST: + + break; + + case CAR_PACKET_TERMINAL_CMD: + data[len] = '\0'; + terminal_process_string((char*)data); + break; + + default: + break; + } +} + +void comm_print(char* str) { + static char print_buffer[255]; + int i; + print_buffer[0] = CAR_PACKET_PRINT; + + for (i = 0;i < 255;i++) { + if (str[i] == '\0') { + break; + } + print_buffer[i + 1] = str[i]; + } + + packet_send_packet((unsigned char*)print_buffer, i + 1); + return; +} + +void comm_send_samples(uint8_t *data, int len) { + uint8_t buffer[len + 1]; + int index = 0; + + buffer[index++] = CAR_PACKET_SEND_SAMPLES; + + for (int i = 0;i < len;i++) { + buffer[index++] = data[i]; + } + + packet_send_packet(buffer, index); +} diff --git a/comm.h b/comm.h new file mode 100644 index 000000000..153a76cce --- /dev/null +++ b/comm.h @@ -0,0 +1,62 @@ +/* + Copyright 2012 Benjamin Vedder benjamin@vedder.se + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +/* + * comm.h + * + * Created on: 22 nov 2012 + * Author: benjamin + */ + +#ifndef COMM_H_ +#define COMM_H_ + +#include + +// Packets that expect response +typedef enum { + CAR_PACKET_READ_VALUES = 0, + CAR_PACKET_READ_POS, + CAR_PACKET_READ_SENS_ULTRA, + CAR_PACKET_READ_SENS_IR, + CAR_PACKET_READ_TRAVEL_COUNTER, + CAR_PACKET_PRINT, + CAR_PACKET_SEND_SAMPLES, + CAR_PACKET_PING +} CAR_RES_PACKET_ID; + +// Packets that only expect ack +typedef enum { + CAR_PACKET_SET_POWER_SERVO = 0, + CAR_PACKET_WRITE_POS, + CAR_PACKET_ADD_POINT, + CAR_PACKET_AP_RUN, + CAR_PACKET_AP_CLEAR, + CAR_PACKET_RESET_TRAVEL_CNT, + CAR_PACKET_SET_LIMITED, + CAR_PACKET_FULL_BRAKE, + CAR_PACKET_SERVO_OFFSET, + CAR_PACKET_CAN_TEST, + CAR_PACKET_TERMINAL_CMD +} CAR_NORES_PACKET_ID; + +// Functions +void comm_init(void); +void comm_print(char* str); +void comm_send_samples(uint8_t *data, int len); + +#endif /* COMM_INTERFACE_H_ */ diff --git a/crc.c b/crc.c new file mode 100644 index 000000000..9768b847e --- /dev/null +++ b/crc.c @@ -0,0 +1,64 @@ +/* + Copyright 2012-2014 Benjamin Vedder benjamin@vedder.se + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +/* + * crc.c + * + * Created on: 26 feb 2012 + * Author: benjamin + */ +#include "crc.h" + +// CRC Table +const unsigned short crc16_tab[] = { 0x0000, 0x1021, 0x2042, 0x3063, 0x4084, + 0x50a5, 0x60c6, 0x70e7, 0x8108, 0x9129, 0xa14a, 0xb16b, 0xc18c, 0xd1ad, + 0xe1ce, 0xf1ef, 0x1231, 0x0210, 0x3273, 0x2252, 0x52b5, 0x4294, 0x72f7, + 0x62d6, 0x9339, 0x8318, 0xb37b, 0xa35a, 0xd3bd, 0xc39c, 0xf3ff, 0xe3de, + 0x2462, 0x3443, 0x0420, 0x1401, 0x64e6, 0x74c7, 0x44a4, 0x5485, 0xa56a, + 0xb54b, 0x8528, 0x9509, 0xe5ee, 0xf5cf, 0xc5ac, 0xd58d, 0x3653, 0x2672, + 0x1611, 0x0630, 0x76d7, 0x66f6, 0x5695, 0x46b4, 0xb75b, 0xa77a, 0x9719, + 0x8738, 0xf7df, 0xe7fe, 0xd79d, 0xc7bc, 0x48c4, 0x58e5, 0x6886, 0x78a7, + 0x0840, 0x1861, 0x2802, 0x3823, 0xc9cc, 0xd9ed, 0xe98e, 0xf9af, 0x8948, + 0x9969, 0xa90a, 0xb92b, 0x5af5, 0x4ad4, 0x7ab7, 0x6a96, 0x1a71, 0x0a50, + 0x3a33, 0x2a12, 0xdbfd, 0xcbdc, 0xfbbf, 0xeb9e, 0x9b79, 0x8b58, 0xbb3b, + 0xab1a, 0x6ca6, 0x7c87, 0x4ce4, 0x5cc5, 0x2c22, 0x3c03, 0x0c60, 0x1c41, + 0xedae, 0xfd8f, 0xcdec, 0xddcd, 0xad2a, 0xbd0b, 0x8d68, 0x9d49, 0x7e97, + 0x6eb6, 0x5ed5, 0x4ef4, 0x3e13, 0x2e32, 0x1e51, 0x0e70, 0xff9f, 0xefbe, + 0xdfdd, 0xcffc, 0xbf1b, 0xaf3a, 0x9f59, 0x8f78, 0x9188, 0x81a9, 0xb1ca, + 0xa1eb, 0xd10c, 0xc12d, 0xf14e, 0xe16f, 0x1080, 0x00a1, 0x30c2, 0x20e3, + 0x5004, 0x4025, 0x7046, 0x6067, 0x83b9, 0x9398, 0xa3fb, 0xb3da, 0xc33d, + 0xd31c, 0xe37f, 0xf35e, 0x02b1, 0x1290, 0x22f3, 0x32d2, 0x4235, 0x5214, + 0x6277, 0x7256, 0xb5ea, 0xa5cb, 0x95a8, 0x8589, 0xf56e, 0xe54f, 0xd52c, + 0xc50d, 0x34e2, 0x24c3, 0x14a0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405, + 0xa7db, 0xb7fa, 0x8799, 0x97b8, 0xe75f, 0xf77e, 0xc71d, 0xd73c, 0x26d3, + 0x36f2, 0x0691, 0x16b0, 0x6657, 0x7676, 0x4615, 0x5634, 0xd94c, 0xc96d, + 0xf90e, 0xe92f, 0x99c8, 0x89e9, 0xb98a, 0xa9ab, 0x5844, 0x4865, 0x7806, + 0x6827, 0x18c0, 0x08e1, 0x3882, 0x28a3, 0xcb7d, 0xdb5c, 0xeb3f, 0xfb1e, + 0x8bf9, 0x9bd8, 0xabbb, 0xbb9a, 0x4a75, 0x5a54, 0x6a37, 0x7a16, 0x0af1, + 0x1ad0, 0x2ab3, 0x3a92, 0xfd2e, 0xed0f, 0xdd6c, 0xcd4d, 0xbdaa, 0xad8b, + 0x9de8, 0x8dc9, 0x7c26, 0x6c07, 0x5c64, 0x4c45, 0x3ca2, 0x2c83, 0x1ce0, + 0x0cc1, 0xef1f, 0xff3e, 0xcf5d, 0xdf7c, 0xaf9b, 0xbfba, 0x8fd9, 0x9ff8, + 0x6e17, 0x7e36, 0x4e55, 0x5e74, 0x2e93, 0x3eb2, 0x0ed1, 0x1ef0 }; + +unsigned short crc16(unsigned char *buf, unsigned int len) { + unsigned int i; + unsigned short cksum = 0; + for (i = 0; i < len; i++) { + cksum = crc16_tab[(((cksum >> 8) ^ *buf++) & 0xFF)] ^ (cksum << 8); + } + return cksum; +} diff --git a/crc.h b/crc.h new file mode 100644 index 000000000..0ef263635 --- /dev/null +++ b/crc.h @@ -0,0 +1,33 @@ +/* + Copyright 2012 Benjamin Vedder benjamin@vedder.se + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +/* + * crc.h + * + * Created on: 26 feb 2012 + * Author: benjamin + */ + +#ifndef CRC_H_ +#define CRC_H_ + +/* + * Functions + */ +unsigned short crc16(unsigned char *buf, unsigned int len); + +#endif /* CRC_H_ */ diff --git a/digital_filter.c b/digital_filter.c new file mode 100644 index 000000000..723b667a3 --- /dev/null +++ b/digital_filter.c @@ -0,0 +1,239 @@ +/* + Copyright 2012-2014 Benjamin Vedder benjamin@vedder.se + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +/* + * digital_filter.c + * + * Created on: 24 nov 2012 + * Author: benjamin + */ + +#include "digital_filter.h" +#include +#include + +// Found at http://paulbourke.net/miscellaneous//dft/ +void filter_fft(int dir, int m, float *real, float *imag) { + long n,i,i1,j,k,i2,l,l1,l2; + float c1,c2,tx,ty,t1,t2,u1,u2,z; + + // Calculate the number of points + n = 1 << m; + + // Do the bit reversal + i2 = n >> 1; + j = 0; + for (i=0;i>= 1; + } + j += k; + } + + // Compute the FFT + c1 = -1.0; + c2 = 0.0; + l2 = 1; + for (l=0;l> (32 - bits); + + for (int i = 0;i < size;i++) { + result += filter[i] * vector[offset]; + offset++; + offset &= cnt_mask; + } + + return result; +} + +/** + * Add sample to buffer + * @param buffer + * The buffer to add the sample to + * @param sample + * The sample to add + * @param bits + * The length of the buffer in bits + * @param offset + * Pointer to the current offset in the buffer. Will be updated in this call + * and wrapped at the length of this buffer. + */ +void filter_add_sample(float *buffer, float sample, int bits, uint32_t *offset) { + uint32_t cnt_mask = 0xFFFFFFFF >> (32 - bits); + buffer[*offset] = sample; + *offset += 1; + *offset &= cnt_mask; +} diff --git a/digital_filter.h b/digital_filter.h new file mode 100644 index 000000000..5678f690f --- /dev/null +++ b/digital_filter.h @@ -0,0 +1,40 @@ +/* + Copyright 2012 Benjamin Vedder benjamin@vedder.se + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +/* + * digital_filter.h + * + * Created on: 24 nov 2012 + * Author: benjamin + */ + +#ifndef DIGITAL_FILTER_H_ +#define DIGITAL_FILTER_H_ + +#include + +// Functions +void filter_fft(int dir, int m, float *real, float *imag); +void filter_dft(int dir, int len, float *real, float *imag); +void filter_fftshift(float *data, int len); +void filter_hamming(float *data, int len); +void filter_zeroPad(float *data, float *result, int dataLen, int resultLen); +void filter_create_fir(float *filter_vector, float f_break, int bits, int use_hamming); +float filter_run_fir_iteration(float *vector, float *filter, int bits, uint32_t offset); +void filter_add_sample(float *buffer, float sample, int bits, uint32_t *offset); + +#endif /* DIGITAL_FILTER_H_ */ diff --git a/halconf.h b/halconf.h new file mode 100644 index 000000000..3941f191e --- /dev/null +++ b/halconf.h @@ -0,0 +1,349 @@ +/* + ChibiOS/RT - Copyright (C) 2006,2007,2008,2009,2010, + 2011,2012 Giovanni Di Sirio. + + This file is part of ChibiOS/RT. + + ChibiOS/RT is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 3 of the License, or + (at your option) any later version. + + ChibiOS/RT is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + + --- + + A special exception to the GPL can be applied should you wish to distribute + a combined work that includes ChibiOS/RT, without being obliged to provide + the source code for any proprietary components. See the file exception.txt + for full details of how and when the exception can be applied. +*/ + +/** + * @file templates/halconf.h + * @brief HAL configuration header. + * @details HAL configuration file, this file allows to enable or disable the + * various device drivers from your application. You may also use + * this file in order to override the device drivers default settings. + * + * @addtogroup HAL_CONF + * @{ + */ + +#ifndef _HALCONF_H_ +#define _HALCONF_H_ + +#include "mcuconf.h" + +/** + * @brief Enables the TM subsystem. + */ +#if !defined(HAL_USE_TM) || defined(__DOXYGEN__) +#define HAL_USE_TM FALSE +#endif + +/** + * @brief Enables the PAL subsystem. + */ +#if !defined(HAL_USE_PAL) || defined(__DOXYGEN__) +#define HAL_USE_PAL TRUE +#endif + +/** + * @brief Enables the ADC subsystem. + */ +#if !defined(HAL_USE_ADC) || defined(__DOXYGEN__) +#define HAL_USE_ADC FALSE +#endif + +/** + * @brief Enables the CAN subsystem. + */ +#if !defined(HAL_USE_CAN) || defined(__DOXYGEN__) +#define HAL_USE_CAN FALSE +#endif + +/** + * @brief Enables the EXT subsystem. + */ +#if !defined(HAL_USE_EXT) || defined(__DOXYGEN__) +#define HAL_USE_EXT FALSE +#endif + +/** + * @brief Enables the GPT subsystem. + */ +#if !defined(HAL_USE_GPT) || defined(__DOXYGEN__) +#define HAL_USE_GPT FALSE +#endif + +/** + * @brief Enables the I2C subsystem. + */ +#if !defined(HAL_USE_I2C) || defined(__DOXYGEN__) +#define HAL_USE_I2C FALSE +#endif + +/** + * @brief Enables the ICU subsystem. + */ +#if !defined(HAL_USE_ICU) || defined(__DOXYGEN__) +#define HAL_USE_ICU FALSE +#endif + +/** + * @brief Enables the MAC subsystem. + */ +#if !defined(HAL_USE_MAC) || defined(__DOXYGEN__) +#define HAL_USE_MAC FALSE +#endif + +/** + * @brief Enables the MMC_SPI subsystem. + */ +#if !defined(HAL_USE_MMC_SPI) || defined(__DOXYGEN__) +#define HAL_USE_MMC_SPI FALSE +#endif + +/** + * @brief Enables the PWM subsystem. + */ +#if !defined(HAL_USE_PWM) || defined(__DOXYGEN__) +#define HAL_USE_PWM FALSE +#endif + +/** + * @brief Enables the RTC subsystem. + */ +#if !defined(HAL_USE_RTC) || defined(__DOXYGEN__) +#define HAL_USE_RTC FALSE +#endif + +/** + * @brief Enables the SDC subsystem. + */ +#if !defined(HAL_USE_SDC) || defined(__DOXYGEN__) +#define HAL_USE_SDC FALSE +#endif + +/** + * @brief Enables the SERIAL subsystem. + */ +#if !defined(HAL_USE_SERIAL) || defined(__DOXYGEN__) +#define HAL_USE_SERIAL FALSE +#endif + +/** + * @brief Enables the SERIAL over USB subsystem. + */ +#if !defined(HAL_USE_SERIAL_USB) || defined(__DOXYGEN__) +#define HAL_USE_SERIAL_USB TRUE +#endif + +/** + * @brief Enables the SPI subsystem. + */ +#if !defined(HAL_USE_SPI) || defined(__DOXYGEN__) +#define HAL_USE_SPI FALSE +#endif + +/** + * @brief Enables the UART subsystem. + */ +#if !defined(HAL_USE_UART) || defined(__DOXYGEN__) +#define HAL_USE_UART TRUE +#endif + +/** + * @brief Enables the USB subsystem. + */ +#if !defined(HAL_USE_USB) || defined(__DOXYGEN__) +#define HAL_USE_USB TRUE +#endif + +/*===========================================================================*/ +/* ADC driver related settings. */ +/*===========================================================================*/ + +/** + * @brief Enables synchronous APIs. + * @note Disabling this option saves both code and data space. + */ +#if !defined(ADC_USE_WAIT) || defined(__DOXYGEN__) +#define ADC_USE_WAIT TRUE +#endif + +/** + * @brief Enables the @p adcAcquireBus() and @p adcReleaseBus() APIs. + * @note Disabling this option saves both code and data space. + */ +#if !defined(ADC_USE_MUTUAL_EXCLUSION) || defined(__DOXYGEN__) +#define ADC_USE_MUTUAL_EXCLUSION TRUE +#endif + +/*===========================================================================*/ +/* CAN driver related settings. */ +/*===========================================================================*/ + +/** + * @brief Sleep mode related APIs inclusion switch. + */ +#if !defined(CAN_USE_SLEEP_MODE) || defined(__DOXYGEN__) +#define CAN_USE_SLEEP_MODE TRUE +#endif + +/*===========================================================================*/ +/* I2C driver related settings. */ +/*===========================================================================*/ + +/** + * @brief Enables the mutual exclusion APIs on the I2C bus. + */ +#if !defined(I2C_USE_MUTUAL_EXCLUSION) || defined(__DOXYGEN__) +#define I2C_USE_MUTUAL_EXCLUSION TRUE +#endif + +/*===========================================================================*/ +/* MAC driver related settings. */ +/*===========================================================================*/ + +/** + * @brief Enables an event sources for incoming packets. + */ +#if !defined(MAC_USE_EVENTS) || defined(__DOXYGEN__) +#define MAC_USE_EVENTS TRUE +#endif + +/*===========================================================================*/ +/* MMC_SPI driver related settings. */ +/*===========================================================================*/ + +/** + * @brief Block size for MMC transfers. + */ +#if !defined(MMC_SECTOR_SIZE) || defined(__DOXYGEN__) +#define MMC_SECTOR_SIZE 512 +#endif + +/** + * @brief Delays insertions. + * @details If enabled this options inserts delays into the MMC waiting + * routines releasing some extra CPU time for the threads with + * lower priority, this may slow down the driver a bit however. + * This option is recommended also if the SPI driver does not + * use a DMA channel and heavily loads the CPU. + */ +#if !defined(MMC_NICE_WAITING) || defined(__DOXYGEN__) +#define MMC_NICE_WAITING TRUE +#endif + +/** + * @brief Number of positive insertion queries before generating the + * insertion event. + */ +#if !defined(MMC_POLLING_INTERVAL) || defined(__DOXYGEN__) +#define MMC_POLLING_INTERVAL 10 +#endif + +/** + * @brief Interval, in milliseconds, between insertion queries. + */ +#if !defined(MMC_POLLING_DELAY) || defined(__DOXYGEN__) +#define MMC_POLLING_DELAY 10 +#endif + +/** + * @brief Uses the SPI polled API for small data transfers. + * @details Polled transfers usually improve performance because it + * saves two context switches and interrupt servicing. Note + * that this option has no effect on large transfers which + * are always performed using DMAs/IRQs. + */ +#if !defined(MMC_USE_SPI_POLLING) || defined(__DOXYGEN__) +#define MMC_USE_SPI_POLLING TRUE +#endif + +/*===========================================================================*/ +/* SDC driver related settings. */ +/*===========================================================================*/ + +/** + * @brief Number of initialization attempts before rejecting the card. + * @note Attempts are performed at 10mS intervals. + */ +#if !defined(SDC_INIT_RETRY) || defined(__DOXYGEN__) +#define SDC_INIT_RETRY 100 +#endif + +/** + * @brief Include support for MMC cards. + * @note MMC support is not yet implemented so this option must be kept + * at @p FALSE. + */ +#if !defined(SDC_MMC_SUPPORT) || defined(__DOXYGEN__) +#define SDC_MMC_SUPPORT FALSE +#endif + +/** + * @brief Delays insertions. + * @details If enabled this options inserts delays into the MMC waiting + * routines releasing some extra CPU time for the threads with + * lower priority, this may slow down the driver a bit however. + */ +#if !defined(SDC_NICE_WAITING) || defined(__DOXYGEN__) +#define SDC_NICE_WAITING TRUE +#endif + +/*===========================================================================*/ +/* SERIAL driver related settings. */ +/*===========================================================================*/ + +/** + * @brief Default bit rate. + * @details Configuration parameter, this is the baud rate selected for the + * default configuration. + */ +#if !defined(SERIAL_DEFAULT_BITRATE) || defined(__DOXYGEN__) +#define SERIAL_DEFAULT_BITRATE 38400 +#endif + +/** + * @brief Serial buffers size. + * @details Configuration parameter, you can change the depth of the queue + * buffers depending on the requirements of your application. + * @note The default is 64 bytes for both the transmission and receive + * buffers. + */ +#if !defined(SERIAL_BUFFERS_SIZE) || defined(__DOXYGEN__) +#define SERIAL_BUFFERS_SIZE 16 +#endif + +/*===========================================================================*/ +/* SPI driver related settings. */ +/*===========================================================================*/ + +/** + * @brief Enables synchronous APIs. + * @note Disabling this option saves both code and data space. + */ +#if !defined(SPI_USE_WAIT) || defined(__DOXYGEN__) +#define SPI_USE_WAIT TRUE +#endif + +/** + * @brief Enables the @p spiAcquireBus() and @p spiReleaseBus() APIs. + * @note Disabling this option saves both code and data space. + */ +#if !defined(SPI_USE_MUTUAL_EXCLUSION) || defined(__DOXYGEN__) +#define SPI_USE_MUTUAL_EXCLUSION TRUE +#endif + +#endif /* _HALCONF_H_ */ + +/** @} */ diff --git a/irq_handlers.c b/irq_handlers.c new file mode 100644 index 000000000..cd6c60010 --- /dev/null +++ b/irq_handlers.c @@ -0,0 +1,60 @@ +/* + Copyright 2012-2014 Benjamin Vedder benjamin@vedder.se + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#include "ch.h" +#include "hal.h" +#include +#include "stm32f4xx_conf.h" +#include "main.h" +#include "mcpwm.h" +#include "servo.h" +#include "comm.h" +#include "servo_dec.h" + +CH_IRQ_HANDLER(TIM7_IRQHandler) { + TIM_ClearITPendingBit(TIM7, TIM_IT_Update); + servo_irq(); +} + +CH_IRQ_HANDLER(TIM1_TRG_COM_IRQHandler) { + TIM_ClearITPendingBit(TIM1, TIM_IT_COM ); + mcpwm_comm_int_handler(); +} + +CH_IRQ_HANDLER(ADC1_2_3_IRQHandler) { + ADC_ClearITPendingBit(ADC1, ADC_IT_JEOC); + mcpwm_adc_inj_int_handler(); +} + +CH_IRQ_HANDLER(EXTI3_IRQHandler) { + if (EXTI_GetITStatus(EXTI_Line3) != RESET) { + EXTI_ClearITPendingBit(EXTI_Line3); + servodec_int_handler(); + } +} + +CH_IRQ_HANDLER(EXTI15_10_IRQHandler) { + if (EXTI_GetITStatus(EXTI_Line13) != RESET) { + EXTI_ClearITPendingBit(EXTI_Line13); + servodec_int_handler(); + } + + if (EXTI_GetITStatus(EXTI_Line14) != RESET) { + EXTI_ClearITPendingBit(EXTI_Line14); + servodec_int_handler(); + } +} diff --git a/ledpwm.c b/ledpwm.c new file mode 100644 index 000000000..3d6139d83 --- /dev/null +++ b/ledpwm.c @@ -0,0 +1,110 @@ +/* + Copyright 2012-2014 Benjamin Vedder benjamin@vedder.se + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +/* + * ledpwm.c + * + * Created on: 3 nov 2012 + * Author: benjamin + */ + +#include "ledpwm.h" +#include "ch.h" +#include "hal.h" +#include +#include + +// Macros +#define LED1_ON() palSetPad(GPIOC, 4) +#define LED1_OFF() palClearPad(GPIOC, 4) +#define LED2_ON() palSetPad(GPIOA, 7) +#define LED2_OFF() palClearPad(GPIOA, 7) + +// Private variables +static volatile int led_values[LEDPWM_LED_NUM]; + +void ledpwm_init(void) { + memset((int*)led_values, 0, sizeof(led_values)); + + palSetPadMode(GPIOC, 4, + PAL_MODE_OUTPUT_PUSHPULL | + PAL_STM32_OSPEED_HIGHEST); + palSetPadMode(GPIOA, 7, + PAL_MODE_OUTPUT_PUSHPULL | + PAL_STM32_OSPEED_HIGHEST); +} + +/* + * Set the intensity for one led. The intensity value is mapped to a PWM value + * according to human luminance perception. + * + * Intensity range is 0.0 to 1.0 + */ +void ledpwm_set_intensity(unsigned int led, float intensity) { + if (led >= LEDPWM_LED_NUM) { + return; + } + + if (intensity < 0.0) { + intensity = 0.0; + } + + if (intensity > 1.0) { + intensity = 1.0; + } + + led_values[led] = (int)roundf(powf(intensity, 2.5) * ((float)LEDPWM_CNT_TOP - 1.0)); +} + +void ledpwm_led_on(int led) { + if (led >= LEDPWM_LED_NUM) { + return; + } + + led_values[led] = LEDPWM_CNT_TOP; +} + +void ledpwm_led_off(int led) { + if (led >= LEDPWM_LED_NUM) { + return; + } + + led_values[led] = 0; +} + +/* + * Call this function as fast as possible, with a deterministic rate. + */ +void ledpwm_update_pwm(void) { + static int cnt = 0; + cnt++; + if (cnt == LEDPWM_CNT_TOP) { + cnt = 0; + } + + if (cnt >= led_values[0]) { + LED1_OFF(); + } else { + LED1_ON(); + } + + if (cnt >= led_values[1]) { + LED2_OFF(); + } else { + LED2_ON(); + } +} diff --git a/ledpwm.h b/ledpwm.h new file mode 100644 index 000000000..148057593 --- /dev/null +++ b/ledpwm.h @@ -0,0 +1,42 @@ +/* + Copyright 2012 Benjamin Vedder benjamin@vedder.se + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +/* + * ledpwm.h + * + * Created on: 3 nov 2012 + * Author: benjamin + */ + +#ifndef LEDPWM_H_ +#define LEDPWM_H_ + +// Settings +#define LEDPWM_LED_NUM 2 +#define LEDPWM_CNT_TOP 200 + +#define LED_GREEN 0 +#define LED_RED 1 + +// Functions +void ledpwm_init(void); +void ledpwm_set_intensity(unsigned int led, float intensity); +void ledpwm_led_on(int led); +void ledpwm_led_off(int led); +void ledpwm_update_pwm(void); + +#endif /* LEDPWM_H_ */ diff --git a/main.c b/main.c new file mode 100644 index 000000000..e302f5949 --- /dev/null +++ b/main.c @@ -0,0 +1,322 @@ +/* + Copyright 2012-2014 Benjamin Vedder benjamin@vedder.se + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#include "ch.h" +#include "hal.h" +#include "stm32f4xx_conf.h" + +#include +#include +#include +#include + +#include "main.h" +#include "mcpwm.h" +#include "ledpwm.h" +#include "servo.h" +#include "servo_dec.h" +#include "comm.h" +#include "servo.h" +#include "ledpwm.h" +#include "terminal.h" + +/* + * Timers used: + * TIM7: servo + * TIM1: mcpwm + * TIM2: mcpwm + * TIM4: mcpwm + * TIM8: mcpwm + * TIM3: servo_dec + */ + +/* + * Notes: + * + * Disable USB VBUS sensing: + * ChibiOS-RT-master/os/hal/platforms/STM32/OTGv1/usb_lld.c + * + * change + * otgp->GCCFG = GCCFG_VBUSASEN | GCCFG_VBUSBSEN | GCCFG_PWRDWN; + * to + * otgp->GCCFG = GCCFG_NOVBUSSENS | GCCFG_PWRDWN; + * + */ + +// Settings +#define USE_SERVO_INPUT 0 +#define USE_THROTTLE_ADC 0 + +// Macros +#define IS_FAULT() (!palReadPad(GPIOC, 12)) + +// Private variables +#define ADC_SAMPLE_MAX_LEN 4000 +static volatile uint16_t curr0_samples[ADC_SAMPLE_MAX_LEN]; +static volatile uint16_t curr1_samples[ADC_SAMPLE_MAX_LEN]; +static volatile uint16_t ph1_samples[ADC_SAMPLE_MAX_LEN]; +static volatile uint16_t ph2_samples[ADC_SAMPLE_MAX_LEN]; +static volatile uint16_t ph3_samples[ADC_SAMPLE_MAX_LEN]; +static volatile uint16_t vzero_samples[ADC_SAMPLE_MAX_LEN]; +static volatile uint8_t status_samples[ADC_SAMPLE_MAX_LEN]; +static volatile int16_t curr_fir_samples[ADC_SAMPLE_MAX_LEN]; + +static volatile int sample_len = 1000; +static volatile int sample_int = 1; +static volatile int sample_ready = 1; +static volatile int sample_now = 0; +static volatile int sample_at_start = 0; +static volatile int was_start_wample = 0; +static volatile float main_last_adc_duration = 0.0; + +static WORKING_AREA(periodic_thread_wa, 1024); +static WORKING_AREA(sample_send_thread_wa, 1024); + +static Thread *sample_send_tp; + +static msg_t periodic_thread(void *arg) { + (void)arg; + + chRegSetThreadName("Main periodic"); + + for(;;) { + if (mcpwm_get_state() == MC_STATE_RUNNING) { + ledpwm_set_intensity(LED_GREEN, 1.0); + } else { + ledpwm_set_intensity(LED_GREEN, 0.2); + } + + if (IS_FAULT()) { + ledpwm_set_intensity(LED_RED, 0.5); + } else { + ledpwm_set_intensity(LED_RED, 0.0); + } + +#if USE_SERVO_INPUT + // Use decoded servo inputs +#endif + + // Gurgalof bicycle-throttle +#if USE_THROTTLE_ADC +#define MIN_PWR 0.24 + float pwr = (float)ADC_Value[ADC_IND_EXT]; + pwr /= 4095.0; + + if (pwr < MIN_PWR) { + mcpwm_use_pid(0); + mcpwm_set_duty(0); + } else { + mcpwm_use_pid(0); + mcpwm_set_duty(pwr); + } +#endif + chThdSleepMilliseconds(1); + } + + return 0; +} + +static msg_t sample_send_thread(void *arg) { + (void)arg; + + chRegSetThreadName("Main sample"); + + sample_send_tp = chThdSelf(); + + for(;;) { + chEvtWaitAny((eventmask_t) 1); + + for (int i = 0;i < sample_len;i++) { + uint8_t buffer[20]; + int index = 0; + + buffer[index++] = curr0_samples[i] >> 8; + buffer[index++] = curr0_samples[i]; + buffer[index++] = curr1_samples[i] >> 8; + buffer[index++] = curr1_samples[i]; + buffer[index++] = ph1_samples[i] >> 8; + buffer[index++] = ph1_samples[i]; + buffer[index++] = ph2_samples[i] >> 8; + buffer[index++] = ph2_samples[i]; + buffer[index++] = ph3_samples[i] >> 8; + buffer[index++] = ph3_samples[i]; + buffer[index++] = vzero_samples[i] >> 8; + buffer[index++] = vzero_samples[i]; + buffer[index++] = status_samples[i]; + buffer[index++] = curr_fir_samples[i] >> 8; + buffer[index++] = curr_fir_samples[i]; + + comm_send_samples(buffer, index); + + // TODO: wait?? + chThdSleep(2); + } + } + + return 0; +} + +/* + * Called every time new ADC values are available. Note that + * the ADC is initialized from mcpwm.c + */ +void main_dma_adc_handler(void) { + ledpwm_update_pwm(); + + if (sample_at_start && mcpwm_get_state() == MC_STATE_STARTING) { + sample_now = 0; + sample_ready = 0; + was_start_wample = 1; + sample_at_start = 0; + } + + static int a = 0; + if (!sample_ready) { + a++; + if (a >= sample_int) { + a = 0; + curr0_samples[sample_now] = ADC_curr_norm_value[0]; + curr1_samples[sample_now] = ADC_curr_norm_value[1]; + ph1_samples[sample_now] = ADC_V_L1; + ph2_samples[sample_now] = ADC_V_L2; + ph3_samples[sample_now] = ADC_V_L3; + vzero_samples[sample_now] = ADC_V_ZERO * MCPWM_VZERO_FACT; + + if (mcpwm_get_state() == MC_STATE_DETECTING && 0) { + status_samples[sample_now] = mcpwm_get_detect_top(); + } else { + uint8_t tmp; + + if (was_start_wample) { + if (mcpwm_get_state() == MC_STATE_STARTING) { + tmp = 1; + } else if (mcpwm_get_state() == MC_STATE_RUNNING) { + tmp = 2; + } else { + tmp = 3; + } + } else { + tmp = mcpwm_read_hall_phase(); + } + + status_samples[sample_now] = mcpwm_get_comm_step() | (tmp << 3); + } + + curr_fir_samples[sample_now] = (int16_t)(mcpwm_get_tot_current_filtered() * 100); + + sample_now++; + + if (sample_now == sample_len) { + sample_ready = 1; + sample_now = 0; + was_start_wample = 0; + chSysLockFromIsr(); + chEvtSignalI(sample_send_tp, (eventmask_t) 1); + chSysUnlockFromIsr(); + } + + main_last_adc_duration = mcpwm_get_last_adc_isr_duration(); + } + } +} + +float main_get_last_adc_isr_duration(void) { + return main_last_adc_duration; +} + +void main_process_packet(unsigned char *data, unsigned char len) { + if (!len) { + return; + } + + int16_t value16; + int32_t value32; + + switch (data[0]) { + case 1: + // Sample and print data + value16 = (int)data[1] << 8 | (int)data[2]; + if (value16 > ADC_SAMPLE_MAX_LEN) { + value16 = ADC_SAMPLE_MAX_LEN; + } + sample_len = value16; + sample_int = data[3]; + sample_now = 0; + sample_ready = 0; + break; + + case 2: + // Duty Control + value16 = (int)data[1] << 8 | (int)data[2]; + mcpwm_use_pid(0); + mcpwm_set_duty((float)value16 / 1000.0); + break; + + case 3: + // PID Control + value32 = (int)data[1] << 24 | (int)data[2] << 16 | (int)data[3] << 8 | (int)data[4]; + mcpwm_use_pid(1); + mcpwm_set_pid_speed((float)value32); + break; + + case 4: + // Detect + mcpwm_set_detect(); + break; + + case 5: + // Reset Fault + // TODO + break; + + case 6: + // Sample and print data at start + value16 = (int)data[1] << 8 | (int)data[2]; + if (value16 > ADC_SAMPLE_MAX_LEN) { + value16 = ADC_SAMPLE_MAX_LEN; + } + + sample_len = value16; + sample_int = data[3]; + sample_at_start = 1; + break; + + default: + break; + } +} + +int main(void) { + halInit(); + chSysInit(); + ledpwm_init(); + mcpwm_init(); + comm_init(); + servo_init(); + +#if USE_SERVO_INPUT + servodec_init(); +#endif + + // Threads + chThdCreateStatic(periodic_thread_wa, sizeof(periodic_thread_wa), NORMALPRIO, periodic_thread, NULL); + chThdCreateStatic(sample_send_thread_wa, sizeof(sample_send_thread_wa), NORMALPRIO, sample_send_thread, NULL); + + for(;;) { + chThdSleepMilliseconds(100); + } +} diff --git a/main.h b/main.h new file mode 100644 index 000000000..5a4546fac --- /dev/null +++ b/main.h @@ -0,0 +1,86 @@ +/* + Copyright 2012 Benjamin Vedder benjamin@vedder.se + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +/* + * main.h + * + * Created on: 10 jul 2012 + * Author: BenjaminVe + */ + +#ifndef MAIN_H_ +#define MAIN_H_ + +#include + +// Component parameters +#define V_REG 3.3 +#define VIN_R1 22000.0 +#define VIN_R2 2200.0 +#define HV_R1 33000.0 +#define HV_R2 2200.0 + +// Input voltage +#define GET_INPUT_VOLTAGE ((V_REG / 4095.0) * (float)ADC_Value[ADC_IND_VIN_SENS] * ((VIN_R1 + VIN_R2) / VIN_R2)) +#define GET_BRIDGE_VOLTAGE ((V_REG / 4095.0) * (float)ADC_Value[ADC_IND_VIN_SENS] * ((HV_R1 + HV_R2) / HV_R2)) + +// Voltage on ADC channel +#define ADC_VOLTS(ch) ((float)ADC_Value[ch] / 4096.0 * 3.3) + +// Sharp sensors + +// EXternal Variables +extern volatile uint16_t ADC_Value[]; + +/* + * ADC Vector + * + * 0: IN0 SENS3 + * 1: IN1 SENS2 + * 2: IN2 SENS1 + * 3: IN5 CURR1 + * 4: IN6 CURR2 + * 5: IN3 NC + * 6: IN10 TEMP_MOTOR + * 7: IN11 NC + * 8: IN12 AN_IN + * 9: IN13 NC + * 10: IN15 ADC_EXT + * 11: IN3 NC + */ + +// ADC Indexes +#define ADC_IND_SENS1 2 +#define ADC_IND_SENS2 1 +#define ADC_IND_SENS3 0 +#define ADC_IND_CURR1 3 +#define ADC_IND_CURR2 4 +#define ADC_IND_VIN_SENS 8 +#define ADC_IND_EXT 10 + +// Measurement macros +#define ADC_V_L1 ADC_Value[ADC_IND_SENS1] +#define ADC_V_L2 ADC_Value[ADC_IND_SENS2] +#define ADC_V_L3 ADC_Value[ADC_IND_SENS3] +#define ADC_V_ZERO (ADC_Value[ADC_IND_VIN_SENS] / 2) + +// Function prototypes +void main_dma_adc_handler(void); +float main_get_last_adc_isr_duration(void); +void main_process_packet(unsigned char *data, unsigned char len); + +#endif /* MAIN_H_ */ diff --git a/mcpwm.c b/mcpwm.c new file mode 100644 index 000000000..80368ca9e --- /dev/null +++ b/mcpwm.c @@ -0,0 +1,1774 @@ +/* + Copyright 2012-2014 Benjamin Vedder benjamin@vedder.se + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +/* + * mcpwm.c + * + * Created on: 13 okt 2012 + * Author: benjamin + */ + +#include "ch.h" +#include "hal.h" +#include "stm32f4xx_conf.h" +#include +#include +#include +#include "main.h" +#include "mcpwm.h" +#include "digital_filter.h" +#include "utils.h" +#include "ledpwm.h" +#include "comm.h" + +// Private variables +static volatile int comm_step; +static volatile int direction; +static volatile int last_comm_time; +static volatile float dutycycle_set; +static volatile float dutycycle_now; +static volatile float rpm_now; +static volatile int is_using_pid; +static volatile float pid_set_rpm; +static volatile int tachometer; +static volatile int pwm_adc_cycles; +static volatile int curr0_sum; +static volatile int curr1_sum; +static volatile int curr_start_samples; +static volatile int curr0_offset; +static volatile int curr1_offset; +static volatile mc_state state; +static volatile float detect_currents[6]; +static volatile int detect_steps; +static volatile int detect_now; +static volatile int detect_inc; +static volatile int detect_do_step; +static volatile mc_pwm_mode pwm_mode; +static volatile float last_current_sample; +static volatile float motor_current_sum; +static volatile float input_current_sum; +static volatile float motor_current_iterations; +static volatile float input_current_iterations; +static volatile float static_torque_current; + +#if MCPWM_IS_SENSORLESS +static volatile int start_pulses; +static volatile int closed_cycles; +static volatile float cycle_integrator; +static volatile int start_time_ms_now; +#endif + +// KV FIR filter +#define KV_FIR_TAPS_BITS 7 +#define KV_FIR_LEN (1 << KV_FIR_TAPS_BITS) +#define KV_FIR_FCUT 0.02 +static volatile float kv_fir_coeffs[KV_FIR_LEN]; +static volatile float kv_fir_samples[KV_FIR_LEN]; +static volatile int kv_fir_index = 0; + +// Current FIR filter +#define CURR_FIR_TAPS_BITS 4 +#define CURR_FIR_LEN (1 << CURR_FIR_TAPS_BITS) +#define CURR_FIR_FCUT 0.15 +static volatile float current_fir_coeffs[CURR_FIR_LEN]; +static volatile float current_fir_samples[CURR_FIR_LEN]; +static volatile int current_fir_index = 0; + +// Sine table (to be generated in init) +#define SINE_TABLE_LEN 3600 +static volatile float sine_table[SINE_TABLE_LEN]; + +// Hall sensor shift table +const unsigned int mc_shift_table[] = { + // 0 + 0b000, // 000 + 0b001, // 001 + 0b010, // 010 + 0b011, // 011 + 0b100, // 100 + 0b101, // 101 + 0b110, // 110 + 0b111, // 111 + + // 1 + 0b000, // 000 + 0b001, // 001 + 0b100, // 010 + 0b101, // 011 + 0b010, // 100 + 0b011, // 101 + 0b110, // 110 + 0b111, // 111 + + // 2 + 0b000, // 000 + 0b010, // 001 + 0b001, // 010 + 0b011, // 011 + 0b100, // 100 + 0b110, // 101 + 0b101, // 110 + 0b111, // 111 + + // 3 + 0b000, // 000 + 0b100, // 001 + 0b010, // 010 + 0b110, // 011 + 0b001, // 100 + 0b101, // 101 + 0b011, // 110 + 0b111, // 111 + + // 4 + 0b000, // 000 + 0b010, // 001 + 0b100, // 010 + 0b110, // 011 + 0b001, // 100 + 0b011, // 101 + 0b101, // 110 + 0b111, // 111 + + // 5 + 0b000, // 000 + 0b100, // 001 + 0b001, // 010 + 0b101, // 011 + 0b010, // 100 + 0b110, // 101 + 0b011, // 110 + 0b111 // 111 +}; + +static volatile unsigned int hall_sensor_order; +static volatile float last_adc_isr_duration; +static volatile float last_inj_adc_isr_duration; + +// Global variables +volatile uint16_t ADC_Value[MCPWM_ADC_CHANNELS]; +volatile int ADC_curr_norm_value[3]; + +// Private functions +static void set_duty_cycle(float dutyCycle); +static void stop_pwm(void); +static void run_pid_controller(void); +static void set_next_comm_step(int next_step); +static void update_rpm_tacho(void); +static void update_adc_sample_pos(void); +#if MCPWM_IS_SENSORLESS +static float get_start_duty(void); +#endif + +#if MCPWM_IS_SENSORLESS +static void set_open_loop(void); +static int integrate_cycle(float v_diff); +#endif + +// Defines +#define ADC_CDR_ADDRESS ((uint32_t)0x40012308) +#define ENABLE_GATE() palSetPad(GPIOC, 10) +#define DISABLE_GATE() palClearPad(GPIOC, 10) +#define DCCAL_ON() palSetPad(GPIOB, 12) +#define DCCAL_OFF() palClearPad(GPIOB, 12) +#define IS_FAULT() (!palReadPad(GPIOC, 12)) +#define CYCLE_INT_START (0) + +// Threads +static WORKING_AREA(timer_thread_wa, 1024); +static msg_t timer_thread(void *arg); + +void mcpwm_init(void) { + TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; + TIM_OCInitTypeDef TIM_OCInitStructure; + TIM_BDTRInitTypeDef TIM_BDTRInitStructure; + NVIC_InitTypeDef NVIC_InitStructure; + + // Initialize variables + comm_step = 1; + direction = 1; + rpm_now = 0; + last_comm_time = 0; + dutycycle_set = 0; + dutycycle_now = 0; + is_using_pid = 0; + pid_set_rpm = 0.0; + tachometer = 0; + pwm_adc_cycles = 0; + state = MC_STATE_OFF; + detect_steps = 0; + detect_now = 0; + detect_inc = 0; + detect_do_step = 0; + hall_sensor_order = MCPWM_HALL_SENSOR_ORDER; + pwm_mode = MCPWM_PWM_MODE; + last_current_sample = 0.0; + motor_current_sum = 0.0; + input_current_sum = 0.0; + motor_current_iterations = 0.0; + input_current_iterations = 0.0; + static_torque_current = 0.0; + +#if MCPWM_IS_SENSORLESS + start_pulses = 0; + closed_cycles = 0; + cycle_integrator = CYCLE_INT_START; + start_time_ms_now = MCPWM_START_COMM_TIME_MS_L; +#endif + + // Create KV FIR filter + filter_create_fir((float*)kv_fir_coeffs, KV_FIR_FCUT, KV_FIR_TAPS_BITS, 1); + + // Create current FIR filter + filter_create_fir((float*)current_fir_coeffs, CURR_FIR_FCUT, CURR_FIR_TAPS_BITS, 1); + + // Generate sine table (Range: 0.0 - 1.0) + for (int i = 0;i < SINE_TABLE_LEN;i++) { + sine_table[i] = (sinf((2.0 * M_PI * (float)i) / SINE_TABLE_LEN) + 1.0) / 2.0; + } + + // Start the timer thread + chThdCreateStatic(timer_thread_wa, sizeof(timer_thread_wa), NORMALPRIO, timer_thread, NULL); + + // GPIO clock enable + RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE); + RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE); + RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE); + + // GPIOC (ENABLE_GATE) + palSetPadMode(GPIOC, 10, + PAL_MODE_OUTPUT_PUSHPULL | + PAL_STM32_OSPEED_HIGHEST); + DISABLE_GATE(); + + // GPIOB (DCCAL) + palSetPadMode(GPIOB, 12, + PAL_MODE_OUTPUT_PUSHPULL | + PAL_STM32_OSPEED_HIGHEST); + + // GPIOB (hall sensors) + palSetPadMode(GPIOB, 6, PAL_MODE_INPUT_PULLUP); + palSetPadMode(GPIOB, 7, PAL_MODE_INPUT_PULLUP); + palSetPadMode(GPIOB, 8, PAL_MODE_INPUT_PULLUP); + + // Fault pin + palSetPadMode(GPIOC, 12, PAL_MODE_INPUT_PULLUP); + + // TIM1 clock enable + RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE); + + // GPIOA Configuration: Channel 1 to 3 as alternate function push-pull + palSetPadMode(GPIOA, 8, PAL_MODE_ALTERNATE(GPIO_AF_TIM1) | + PAL_STM32_OSPEED_HIGHEST | + PAL_STM32_PUDR_FLOATING); + palSetPadMode(GPIOA, 9, PAL_MODE_ALTERNATE(GPIO_AF_TIM1) | + PAL_STM32_OSPEED_HIGHEST | + PAL_STM32_PUDR_FLOATING); + palSetPadMode(GPIOA, 10, PAL_MODE_ALTERNATE(GPIO_AF_TIM1) | + PAL_STM32_OSPEED_HIGHEST | + PAL_STM32_PUDR_FLOATING); + + palSetPadMode(GPIOB, 13, PAL_MODE_ALTERNATE(GPIO_AF_TIM1) | + PAL_STM32_OSPEED_HIGHEST | + PAL_STM32_PUDR_FLOATING); + palSetPadMode(GPIOB, 14, PAL_MODE_ALTERNATE(GPIO_AF_TIM1) | + PAL_STM32_OSPEED_HIGHEST | + PAL_STM32_PUDR_FLOATING); + palSetPadMode(GPIOB, 15, PAL_MODE_ALTERNATE(GPIO_AF_TIM1) | + PAL_STM32_OSPEED_HIGHEST | + PAL_STM32_PUDR_FLOATING); + + // Enable the TIM1 Trigger and commutation interrupt + NVIC_InitStructure.NVIC_IRQChannel = TIM1_TRG_COM_TIM11_IRQn; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2; + NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2; + NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; + NVIC_Init(&NVIC_InitStructure); + + // Time Base configuration + TIM_TimeBaseStructure.TIM_Prescaler = 0; + TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; + TIM_TimeBaseStructure.TIM_Period = 168000000 / MCPWM_SWITCH_FREQUENCY; + TIM_TimeBaseStructure.TIM_ClockDivision = 0; + TIM_TimeBaseStructure.TIM_RepetitionCounter = 0; + + TIM_TimeBaseInit(TIM1, &TIM_TimeBaseStructure); + + // Channel 1, 2 and 3 Configuration in PWM mode + TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; + TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; + TIM_OCInitStructure.TIM_OutputNState = TIM_OutputState_Enable; + TIM_OCInitStructure.TIM_Pulse = 0; + TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High; + TIM_OCInitStructure.TIM_OCNPolarity = TIM_OCNPolarity_High; + TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set; + TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Set; + + TIM_OC1Init(TIM1, &TIM_OCInitStructure); + TIM_OC2Init(TIM1, &TIM_OCInitStructure); + TIM_OC3Init(TIM1, &TIM_OCInitStructure); + TIM_OC4Init(TIM1, &TIM_OCInitStructure); + + TIM_OC1PreloadConfig(TIM1, TIM_OCPreload_Disable); + TIM_OC2PreloadConfig(TIM1, TIM_OCPreload_Disable); + TIM_OC3PreloadConfig(TIM1, TIM_OCPreload_Disable); + TIM_OC4PreloadConfig(TIM1, TIM_OCPreload_Disable); + + // Automatic Output enable, Break, dead time and lock configuration + TIM_BDTRInitStructure.TIM_OSSRState = TIM_OSSRState_Enable; + TIM_BDTRInitStructure.TIM_OSSIState = TIM_OSSRState_Enable; + TIM_BDTRInitStructure.TIM_LOCKLevel = TIM_LOCKLevel_OFF; + TIM_BDTRInitStructure.TIM_DeadTime = MCPWM_DEAD_TIME_CYCLES; + TIM_BDTRInitStructure.TIM_Break = TIM_Break_Disable; + TIM_BDTRInitStructure.TIM_BreakPolarity = TIM_BreakPolarity_High; + TIM_BDTRInitStructure.TIM_AutomaticOutput = TIM_AutomaticOutput_Enable; + + TIM_BDTRConfig(TIM1, &TIM_BDTRInitStructure); + TIM_CCPreloadControl(TIM1, ENABLE); + TIM_ITConfig(TIM1, TIM_IT_COM, ENABLE); + + /* + * ADC! + */ + ADC_CommonInitTypeDef ADC_CommonInitStructure; + DMA_InitTypeDef DMA_InitStructure; + ADC_InitTypeDef ADC_InitStructure; + + // Clock + RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_DMA2 | RCC_AHB1Periph_GPIOA | RCC_AHB1Periph_GPIOC, ENABLE); + RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1 | RCC_APB2Periph_ADC2 | RCC_APB2Periph_ADC3, ENABLE); + + dmaStreamAllocate(STM32_DMA_STREAM(STM32_DMA_STREAM_ID(2, 4)), + 2, + (stm32_dmaisr_t)mcpwm_adc_int_handler, + (void *)0); + + // DMA + DMA_InitStructure.DMA_Channel = DMA_Channel_0; + DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)&ADC_Value; + DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)ADC_CDR_ADDRESS; + DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralToMemory; + DMA_InitStructure.DMA_BufferSize = MCPWM_ADC_CHANNELS; + DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; + DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; + DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord; + DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord; + DMA_InitStructure.DMA_Mode = DMA_Mode_Circular; + DMA_InitStructure.DMA_Priority = DMA_Priority_High; + DMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable; + DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_1QuarterFull; + DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single; + DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single; + DMA_Init(DMA2_Stream4, &DMA_InitStructure); + + // DMA2_Stream0 enable + DMA_Cmd(DMA2_Stream4, ENABLE); + + // Enable transfer complete interrupt + DMA_ITConfig(DMA2_Stream4, DMA_IT_TC, ENABLE); + + palSetPadMode(GPIOA, 0, PAL_MODE_INPUT_ANALOG); + palSetPadMode(GPIOA, 1, PAL_MODE_INPUT_ANALOG); + palSetPadMode(GPIOA, 2, PAL_MODE_INPUT_ANALOG); + palSetPadMode(GPIOA, 3, PAL_MODE_INPUT_ANALOG); + palSetPadMode(GPIOA, 4, PAL_MODE_INPUT_ANALOG); + palSetPadMode(GPIOA, 5, PAL_MODE_INPUT_ANALOG); + palSetPadMode(GPIOA, 6, PAL_MODE_INPUT_ANALOG); + + palSetPadMode(GPIOB, 0, PAL_MODE_INPUT_ANALOG); + palSetPadMode(GPIOB, 1, PAL_MODE_INPUT_ANALOG); + + palSetPadMode(GPIOC, 0, PAL_MODE_INPUT_ANALOG); + palSetPadMode(GPIOC, 1, PAL_MODE_INPUT_ANALOG); + palSetPadMode(GPIOC, 2, PAL_MODE_INPUT_ANALOG); + palSetPadMode(GPIOC, 3, PAL_MODE_INPUT_ANALOG); + + // ADC Common Init + ADC_CommonInitStructure.ADC_Mode = ADC_TripleMode_RegSimult; + ADC_CommonInitStructure.ADC_Prescaler = ADC_Prescaler_Div2; + ADC_CommonInitStructure.ADC_DMAAccessMode = ADC_DMAAccessMode_1; + ADC_CommonInitStructure.ADC_TwoSamplingDelay = ADC_TwoSamplingDelay_5Cycles; + ADC_CommonInit(&ADC_CommonInitStructure); + + // Channel-specific settings + ADC_InitStructure.ADC_Resolution = ADC_Resolution_12b; + ADC_InitStructure.ADC_ScanConvMode = ENABLE; + ADC_InitStructure.ADC_ContinuousConvMode = DISABLE; + ADC_InitStructure.ADC_ExternalTrigConvEdge = ADC_ExternalTrigConvEdge_Falling; + ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_T8_CC1; + ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right; + ADC_InitStructure.ADC_NbrOfConversion = 4; + + ADC_Init(ADC1, &ADC_InitStructure); + ADC_InitStructure.ADC_ExternalTrigConvEdge = ADC_ExternalTrigConvEdge_None; + ADC_InitStructure.ADC_ExternalTrigConv = 0; + ADC_Init(ADC2, &ADC_InitStructure); + ADC_Init(ADC3, &ADC_InitStructure); + + // ADC1 regular channels 0, 5, 10, 13 + ADC_RegularChannelConfig(ADC1, ADC_Channel_0, 1, ADC_SampleTime_3Cycles); + ADC_RegularChannelConfig(ADC1, ADC_Channel_5, 2, ADC_SampleTime_3Cycles); + ADC_RegularChannelConfig(ADC1, ADC_Channel_10, 3, ADC_SampleTime_3Cycles); + ADC_RegularChannelConfig(ADC1, ADC_Channel_13, 4, ADC_SampleTime_3Cycles); + + // ADC2 regular channels 1, 6, 11, 15 + ADC_RegularChannelConfig(ADC2, ADC_Channel_1, 1, ADC_SampleTime_3Cycles); + ADC_RegularChannelConfig(ADC2, ADC_Channel_6, 2, ADC_SampleTime_3Cycles); + ADC_RegularChannelConfig(ADC2, ADC_Channel_11, 3, ADC_SampleTime_3Cycles); + ADC_RegularChannelConfig(ADC2, ADC_Channel_15, 4, ADC_SampleTime_3Cycles); + + // ADC3 regular channels 2, 3, 12, 3 + ADC_RegularChannelConfig(ADC3, ADC_Channel_2, 1, ADC_SampleTime_3Cycles); + ADC_RegularChannelConfig(ADC3, ADC_Channel_3, 2, ADC_SampleTime_3Cycles); + ADC_RegularChannelConfig(ADC3, ADC_Channel_12, 3, ADC_SampleTime_3Cycles); + ADC_RegularChannelConfig(ADC3, ADC_Channel_3, 4, ADC_SampleTime_3Cycles); + + // Enable DMA request after last transfer (Multi-ADC mode) + ADC_MultiModeDMARequestAfterLastTransferCmd(ENABLE); + + // Injected channels for current measurement at end of cycle + ADC_ExternalTrigInjectedConvConfig(ADC1, ADC_ExternalTrigInjecConv_T1_CC4); + ADC_ExternalTrigInjectedConvConfig(ADC2, ADC_ExternalTrigInjecConv_T8_CC2); + ADC_ExternalTrigInjectedConvEdgeConfig(ADC1, ADC_ExternalTrigInjecConvEdge_Falling); + ADC_ExternalTrigInjectedConvEdgeConfig(ADC2, ADC_ExternalTrigInjecConvEdge_Falling); + ADC_InjectedSequencerLengthConfig(ADC1, 1); + ADC_InjectedSequencerLengthConfig(ADC2, 1); + ADC_InjectedChannelConfig(ADC1, ADC_Channel_6, 1, ADC_SampleTime_3Cycles); + ADC_InjectedChannelConfig(ADC2, ADC_Channel_5, 1, ADC_SampleTime_3Cycles); + + // Interrupt + ADC_ITConfig(ADC1, ADC_IT_JEOC, ENABLE); + NVIC_InitStructure.NVIC_IRQChannel = ADC_IRQn; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2; + NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2; + NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; + NVIC_Init(&NVIC_InitStructure); + + // Enable ADC1 + ADC_Cmd(ADC1, ENABLE); + + // Enable ADC2 + ADC_Cmd(ADC2, ENABLE); + + // Enable ADC3 + ADC_Cmd(ADC3, ENABLE); + + // ------------- Timer8 for ADC sampling ------------- // + // Time Base configuration + RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8, ENABLE); + + TIM_TimeBaseStructure.TIM_Prescaler = 0; + TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; + TIM_TimeBaseStructure.TIM_Period = 168000000 / MCPWM_SWITCH_FREQUENCY; + TIM_TimeBaseStructure.TIM_ClockDivision = 0; + TIM_TimeBaseStructure.TIM_RepetitionCounter = 0; + TIM_TimeBaseInit(TIM8, &TIM_TimeBaseStructure); + + TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; + TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; + TIM_OCInitStructure.TIM_Pulse = 100; + TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High; + TIM_OCInitStructure.TIM_OCNPolarity = TIM_OCNPolarity_High; + TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set; + TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Set; + TIM_OC1Init(TIM8, &TIM_OCInitStructure); + TIM_OC1PreloadConfig(TIM8, TIM_OCPreload_Disable); + TIM_OC2Init(TIM8, &TIM_OCInitStructure); + TIM_OC2PreloadConfig(TIM8, TIM_OCPreload_Disable); + + // PWM outputs have to be enabled in order to trigger ADC on CCx + TIM_CtrlPWMOutputs(TIM8, ENABLE); + + // TIM1 Master and TIM8 slave + TIM_SelectOutputTrigger(TIM1, TIM_TRGOSource_Enable); + TIM_SelectMasterSlaveMode(TIM1, TIM_MasterSlaveMode_Enable); + TIM_SelectMasterSlaveMode(TIM8, TIM_MasterSlaveMode_Enable); + TIM_SelectInputTrigger(TIM8, TIM_TS_ITR0); + TIM_SelectSlaveMode(TIM8, TIM_SlaveMode_Gated); + + // Enable TIM8 first to make sure timers are in sync + TIM_Cmd(TIM8, ENABLE); + TIM_Cmd(TIM1, ENABLE); + + // Main Output Enable + TIM_CtrlPWMOutputs(TIM1, ENABLE); + + // 32-bit timer for RPM measurement + RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE); + uint16_t PrescalerValue = (uint16_t) ((168000000 / 2) / 1000000) - 1; + + // Time base configuration + TIM_TimeBaseStructure.TIM_Period = 0xFFFFFFFF; + TIM_TimeBaseStructure.TIM_Prescaler = PrescalerValue; + TIM_TimeBaseStructure.TIM_ClockDivision = 0; + TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; + TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure); + + // TIM2 enable counter + TIM_Cmd(TIM2, ENABLE); + + // Sample injected channels at end of PWM cycle + TIM1->CCR4 = TIM1->ARR - 300; + TIM8->CCR2 = TIM1->ARR - 300; + + // Calibrate current offset + ENABLE_GATE(); + chThdSleepMilliseconds(100); + DCCAL_ON(); + curr0_sum = 0; + curr1_sum = 0; + curr_start_samples = 0; + while(curr_start_samples < 5000) {}; + curr0_offset = curr0_sum / curr_start_samples; + curr1_offset = curr1_sum / curr_start_samples; + DCCAL_OFF(); + + // Various time measurements + RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE); + PrescalerValue = (uint16_t) ((168000000 / 2) / 10000000) - 1; + + // Time base configuration + TIM_TimeBaseStructure.TIM_Period = 0xFFFFFFFF; + TIM_TimeBaseStructure.TIM_Prescaler = PrescalerValue; + TIM_TimeBaseStructure.TIM_ClockDivision = 0; + TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; + TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure); + + // TIM3 enable counter + TIM_Cmd(TIM4, ENABLE); +} + +void mcpwm_set_duty(float dutyCycle) { + if (dutyCycle > MCPWM_MIN_DUTY_CYCLE) { + direction = 1; + } else if (dutyCycle < -MCPWM_MIN_DUTY_CYCLE) { + dutyCycle = -dutyCycle; + direction = 0; + } + + if (dutyCycle < MCPWM_MIN_DUTY_CYCLE) { + switch (state) { + case MC_STATE_STARTING: + state = MC_STATE_OFF; + if (MCPWM_FULL_BRAKE_AT_STOP) { + mcpwm_full_brake(); + } else { + stop_pwm(); + } + break; + + case MC_STATE_DETECTING: + state = MC_STATE_OFF; + stop_pwm(); // TODO: Full break? + break; + + case MC_STATE_RUNNING: + if (!MCPWM_FULL_BRAKE_AT_STOP) { + state = MC_STATE_OFF; + stop_pwm(); + } + break; + + default: + break; + } + + dutycycle_set = dutyCycle; + return; + } else if (dutyCycle > MCPWM_MAX_DUTY_CYCLE) { + dutyCycle = MCPWM_MAX_DUTY_CYCLE; + } + +#if MCPWM_IS_SENSORLESS + if (state != MC_STATE_RUNNING && state != MC_STATE_STARTING) { + set_open_loop(); + } +#else + if (state != MC_STATE_RUNNING) { + state = MC_STATE_RUNNING; + set_next_comm_step(mcpwm_read_hall_phase()); + TIM_GenerateEvent(TIM1, TIM_EventSource_COM); + } +#endif + + dutycycle_set = dutyCycle; +} + +void mcpwm_use_pid(int use_pid) { + is_using_pid = use_pid; +} + +void mcpwm_set_pid_speed(float rpm) { + pid_set_rpm = rpm; +} + +int mcpwm_get_comm_step(void) { + return comm_step; +} + +float mcpwm_get_duty_cycle(void) { + return dutycycle_set; +} + +float mcpwm_get_rpm(void) { + return direction ? rpm_now : -rpm_now; +} + +float mcpwm_get_kv(void) { + return rpm_now / (GET_INPUT_VOLTAGE * mcpwm_get_duty_cycle()); +} + +mc_state mcpwm_get_state(void) { + return state; +} + +float mcpwm_get_kv_filtered(void) { + float value = filter_run_fir_iteration((float*)kv_fir_samples, + (float*)kv_fir_coeffs, KV_FIR_TAPS_BITS, kv_fir_index); + + return value; +} + +float mcpwm_get_tot_current_filtered(void) { + float value = filter_run_fir_iteration((float*)current_fir_samples, + (float*)current_fir_coeffs, CURR_FIR_TAPS_BITS, current_fir_index); + + value *= (3.3 / 4095.0) / (0.001 * 10.0); + return value; +} + +float mcpwm_get_tot_current(void) { + return last_current_sample * (3.3 / 4095.0) / (0.001 * 10.0); +} + +float mcpwm_get_tot_current_in(void) { + return mcpwm_get_tot_current() * dutycycle_now; +} + +int mcpwm_get_tachometer_value(int reset) { + int val = tachometer; + + if (reset) { + tachometer = 0; + } + + return val; +} + +void mcpwm_set_detect(void) { + detect_steps = 0; + is_using_pid = 0; + stop_pwm(); + + for(int i = 0;i < 6;i++) { + detect_currents[i] = 0; + } + + state = MC_STATE_DETECTING; +} + +int mcpwm_get_detect_top(void) { + float max = 0; + int pos = 1; + for(int i = 0;i < 6;i++) { + if (detect_currents[i] > max) { + max = detect_currents[i]; + pos = i + 1; + } + } + + return pos; +} + +static void stop_pwm(void) { + dutycycle_set = 0; + dutycycle_now = 0; + + TIM_SelectOCxM(TIM1, TIM_Channel_1, TIM_ForcedAction_InActive); + TIM_CCxCmd(TIM1, TIM_Channel_1, TIM_CCx_Enable); + TIM_CCxNCmd(TIM1, TIM_Channel_1, TIM_CCxN_Disable); + + TIM_SelectOCxM(TIM1, TIM_Channel_2, TIM_ForcedAction_InActive); + TIM_CCxCmd(TIM1, TIM_Channel_2, TIM_CCx_Enable); + TIM_CCxNCmd(TIM1, TIM_Channel_2, TIM_CCxN_Disable); + + TIM_SelectOCxM(TIM1, TIM_Channel_3, TIM_ForcedAction_InActive); + TIM_CCxCmd(TIM1, TIM_Channel_3, TIM_CCx_Enable); + TIM_CCxNCmd(TIM1, TIM_Channel_3, TIM_CCxN_Disable); + + TIM_GenerateEvent(TIM1, TIM_EventSource_COM); +} + +void mcpwm_full_brake(void) { + state = MC_STATE_FULL_BRAKE; + + dutycycle_set = 0; + dutycycle_now = 0; + + TIM_SelectOCxM(TIM1, TIM_Channel_1, TIM_ForcedAction_InActive); + TIM_CCxCmd(TIM1, TIM_Channel_1, TIM_CCx_Enable); + TIM_CCxNCmd(TIM1, TIM_Channel_1, TIM_CCxN_Enable); + + TIM_SelectOCxM(TIM1, TIM_Channel_2, TIM_ForcedAction_InActive); + TIM_CCxCmd(TIM1, TIM_Channel_2, TIM_CCx_Enable); + TIM_CCxNCmd(TIM1, TIM_Channel_2, TIM_CCxN_Enable); + + TIM_SelectOCxM(TIM1, TIM_Channel_3, TIM_ForcedAction_InActive); + TIM_CCxCmd(TIM1, TIM_Channel_3, TIM_CCx_Enable); + TIM_CCxNCmd(TIM1, TIM_Channel_3, TIM_CCxN_Enable); + + TIM_GenerateEvent(TIM1, TIM_EventSource_COM); +} + +#if MCPWM_IS_SENSORLESS +static float get_start_duty(void) { + float ret = 0.0; + float ratio = calc_ratio(MCPWM_START_COMM_TIME_MS_L, + MCPWM_START_COMM_TIME_MS_H, start_time_ms_now); + + if (direction) { + ret = MCPWM_START_DUTY_CYCLE_L * (1 - ratio) + + MCPWM_START_DUTY_CYCLE_H * ratio; + } else { + ret = MCPWM_START_DUTY_CYCLE_REV_L * (1 - ratio) + MCPWM_START_DUTY_CYCLE_REV_H * ratio; + } + + ret *= 20.0 / GET_BRIDGE_VOLTAGE; + + return ret; +} + +static void set_open_loop(void) { + start_pulses = 0; + state = MC_STATE_STARTING; + closed_cycles = 0; + cycle_integrator = CYCLE_INT_START; + start_time_ms_now = MCPWM_START_COMM_TIME_MS_L; + + dutycycle_now = get_start_duty(); + set_duty_cycle(dutycycle_now); +} +#endif + +static void set_duty_cycle(float dutyCycle) { + if (dutyCycle < MCPWM_MIN_DUTY_CYCLE) { + dutyCycle = MCPWM_MIN_DUTY_CYCLE; + } else if (dutyCycle > MCPWM_MAX_DUTY_CYCLE) { + dutyCycle = MCPWM_MAX_DUTY_CYCLE; + } + + uint16_t period; + if (pwm_mode == PWM_MODE_BIPOLAR) { + period = (uint16_t)(((float)TIM1->ARR / 2.0) * dutyCycle + ((float)TIM1->ARR / 2.0)); + } else { + period = (uint16_t)((float)TIM1->ARR * dutyCycle); + } + + TIM1->CCR1 = period; + TIM1->CCR2 = period; + TIM1->CCR3 = period; + + update_adc_sample_pos(); +} + +static void run_pid_controller(void) { + static float i_term = 0; + static float prev_error = 0; + float p_term; + float d_term; + + // PID is off. Return. + if (!is_using_pid) { + i_term = 0; + prev_error = 0; + return; + } + + // Too low RPM set. Stop and return. + if (fabsf(pid_set_rpm) < MCPWM_PID_MIN_RPM) { + i_term = 0; + prev_error = 0; + mcpwm_set_duty(0.0); + return; + } + +#if MCPWM_IS_SENSORLESS + // Start sequence running. Return. + if (state == MC_STATE_STARTING || closed_cycles < MCPWM_CLOSED_STARTPWM_COMMS) { + i_term = 0; + prev_error = 0; + + mcpwm_set_duty(pid_set_rpm > 0 ? get_start_duty() : -get_start_duty()); + return; + } +#endif + + // Compensation for supply voltage variations + float scale = 1.0 / GET_INPUT_VOLTAGE; + + // Compute error + float error = pid_set_rpm - mcpwm_get_rpm(); + + // Compute parameters + p_term = error * MCPWM_PID_KP * scale; + i_term += error * (MCPWM_PID_KI * MCPWM_PID_TIME_K) * scale; + d_term = (error - prev_error) * (MCPWM_PID_KD / MCPWM_PID_TIME_K) * scale; + + // I-term wind-up protection + if (i_term > 1.0) { + i_term = 1.0; + } else if (i_term < -1.0) { + i_term = -1.0; + } + + // Store previous error + prev_error = error; + + // Calculate output + float output = p_term + i_term + d_term; + + // Make sure that at least minimum output is used + if (fabsf(output) < MCPWM_MIN_DUTY_CYCLE) { + if (output > 0.0) { + output = MCPWM_MIN_DUTY_CYCLE; + } else { + output = -MCPWM_MIN_DUTY_CYCLE; + } + } + + // Do not output in reverse direction to oppose too high rpm + if (pid_set_rpm > 0.0 && output < 0.0) { + output = MCPWM_MIN_DUTY_CYCLE; + i_term = 0.0; + } else if (pid_set_rpm < 0.0 && output > 0.0) { + output = -MCPWM_MIN_DUTY_CYCLE; + i_term = 0.0; + } + + mcpwm_set_duty(output); +} + +static msg_t timer_thread(void *arg) { + (void)arg; + + chRegSetThreadName("mcpwm timer"); + + for(;;) { + // Update RPM in case it has slowed down + uint32_t tim_val = TIM2->CNT; + uint32_t tim_diff = tim_val - last_comm_time; + +#if MCPWM_IS_SENSORLESS + static int start_time_cnt = 0; +#endif + + if (tim_diff > 0) { + float rpm_tmp = ((float)MCPWM_AVG_COM_RPM * 1000000.0 * 60.0) / + ((float)tim_diff * (float)MCPWM_NUM_POLES * 3.0); + + // Re-calculate RPM between commutations + // This will end up being used when slowing down + if (rpm_tmp < rpm_now) { + rpm_now = rpm_tmp; + } + +#if MCPWM_IS_SENSORLESS + if (rpm_now < MCPWM_MIN_CLOSED_RPM && state == MC_STATE_RUNNING && closed_cycles > 40) { + set_open_loop(); + } +#endif + } + +#if MCPWM_IS_SENSORLESS + // Duty-cycle, detect and startup + static int start_time = 0; + + if (state != MC_STATE_STARTING) { + if (state == MC_STATE_RUNNING) { + if (closed_cycles >= MCPWM_CLOSED_STARTPWM_COMMS) { + start_time_ms_now = MCPWM_START_COMM_TIME_MS_L; + } + } else { + start_time_ms_now = MCPWM_START_COMM_TIME_MS_L; + } + } +#endif + + switch (state) { + case MC_STATE_OFF: + stop_pwm(); + break; + + case MC_STATE_DETECTING: + detect_do_step = 1; + break; + +#if MCPWM_IS_SENSORLESS + case MC_STATE_STARTING: + start_time++; + + if (start_time >= start_time_ms_now) { + start_time = 0; + start_pulses++; + TIM_GenerateEvent(TIM1, TIM_EventSource_COM); + + start_time_cnt++; + if (start_time_cnt > 6) { + start_time_cnt = 0; + start_time_ms_now++; + if (start_time_ms_now > MCPWM_START_COMM_TIME_MS_H) { + start_time_ms_now = MCPWM_START_COMM_TIME_MS_L; + } + + dutycycle_now = get_start_duty(); + set_duty_cycle(dutycycle_now); + } + } + break; +#endif + + case MC_STATE_RUNNING: +#if MCPWM_IS_SENSORLESS + start_time = 0; +#endif + break; + + case MC_STATE_FULL_BRAKE: + + break; + + default: + break; + } + + run_pid_controller(); + + // Fill KV filter vector at 100Hz + static int cnt_tmp = 0; + cnt_tmp++; + if (cnt_tmp >= 10) { + cnt_tmp = 0; + if (state == MC_STATE_RUNNING) { + filter_add_sample((float*)kv_fir_samples, mcpwm_get_kv(), + KV_FIR_TAPS_BITS, (uint32_t*)&kv_fir_index); + } + } + + chThdSleepMilliseconds(1); + } + + return 0; +} + +void mcpwm_adc_inj_int_handler(void) { + TIM4->CNT = 0; + + int curr0 = ADC_GetInjectedConversionValue(ADC1, ADC_InjectedChannel_1); + int curr1 = ADC_GetInjectedConversionValue(ADC2, ADC_InjectedChannel_1); + + curr0_sum += curr0; + curr1_sum += curr1; + curr_start_samples++; + + ADC_curr_norm_value[0] = curr0 - curr0_offset; + ADC_curr_norm_value[1] = curr1 - curr1_offset; + ADC_curr_norm_value[2] = -(ADC_curr_norm_value[0] + ADC_curr_norm_value[1]); + + // Run current FIR filter + float curr_tot_sample = 0; + + switch (comm_step) { + case 1: + case 6: + if (direction) { + curr_tot_sample = (float)ADC_curr_norm_value[2]; + } else { + curr_tot_sample = (float)ADC_curr_norm_value[1]; + } + break; + + case 2: + case 3: + curr_tot_sample = (float)ADC_curr_norm_value[0]; + break; + + case 4: + case 5: + if (direction) { + curr_tot_sample = (float)ADC_curr_norm_value[1]; + } else { + curr_tot_sample = (float)ADC_curr_norm_value[2]; + } + break; + } + + if (detect_now == 1) { + float a = fabsf(ADC_curr_norm_value[0]); + float b = fabsf(ADC_curr_norm_value[1]); + + if (a > b) { + detect_currents[comm_step] = a; + } else { + detect_currents[comm_step] = b; + } + + stop_pwm(); + detect_steps++; + } + + if (detect_now) { + detect_now--; + } + + if (detect_do_step) { + detect_steps++; + detect_now = 2; + + set_duty_cycle(0.5); + + direction = 1; + comm_step++; + if (comm_step > 6) { + comm_step = 1; + } + + set_next_comm_step(comm_step); + TIM_GenerateEvent(TIM1, TIM_EventSource_COM); + detect_do_step = 0; + } + + last_current_sample = curr_tot_sample; + + filter_add_sample((float*)current_fir_samples, curr_tot_sample, + CURR_FIR_TAPS_BITS, (uint32_t*)¤t_fir_index); + + last_inj_adc_isr_duration = (float)TIM4->CNT / 10000000; +} + +/* + * New ADC samples ready. Do commutation! + */ +void mcpwm_adc_int_handler(void *p, uint32_t flags) { + (void)p; + (void)flags; + + TIM4->CNT = 0; + +#if MCPWM_IS_SENSORLESS + // See if current RPM is large enough to consider it updated, + // otherwise use low enough RPM value + float div_rpm = rpm_now; + + if (div_rpm < (float)MCPWM_MIN_CLOSED_RPM) { + div_rpm = (float)MCPWM_MIN_CLOSED_RPM; + } + + // Compute the theoretical commutation time at the current RPM + float comm_time = ((float)MCPWM_SWITCH_FREQUENCY) / + ((div_rpm / 60.0) * (float)MCPWM_NUM_POLES * 3.0); + + if (pwm_adc_cycles >= (int)(comm_time * (1.0 / MCPWM_COMM_RPM_FACTOR))) { + if (state == MC_STATE_RUNNING) { + TIM_GenerateEvent(TIM1, TIM_EventSource_COM); + closed_cycles++; + cycle_integrator = CYCLE_INT_START; + } + } + + if (pwm_mode == PWM_MODE_BIPOLAR) { + comm_time = 3.0 / MCPWM_COMM_RPM_FACTOR; + } + + if (pwm_adc_cycles >= (int)(comm_time * MCPWM_COMM_RPM_FACTOR)) { + int inc_step = 0; + int curr_tres = ADC_V_ZERO * MCPWM_VZERO_FACT; + int ph1, ph2, ph3; + int v_diff = 0; + + if (direction) { + ph1 = ADC_V_L1; + ph2 = ADC_V_L2; + ph3 = ADC_V_L3; + } else { + ph1 = ADC_V_L1; + ph2 = ADC_V_L3; + ph3 = ADC_V_L2; + } + + switch (comm_step) { + case 1: + if (ph1 > curr_tres) { + inc_step = 1; + } + v_diff = (ph1 - curr_tres); + break; + + case 2: + if (ph2 < curr_tres) { + inc_step = 1; + } + v_diff = -(ph2 - curr_tres); + break; + + case 3: + if (ph3 > curr_tres) { + inc_step = 1; + } + v_diff = (ph3 - curr_tres); + break; + + case 4: + if (ph1 < curr_tres) { + inc_step = 1; + } + v_diff = -(ph1 - curr_tres); + break; + + case 5: + if (ph2 > curr_tres) { + inc_step = 1; + } + v_diff = (ph2 - curr_tres); + break; + + case 6: + if (ph3 < curr_tres) { + inc_step = 1; + } + v_diff = -(ph3 - curr_tres); + break; + + default: + break; + } + + if (inc_step) { + const int ratio = (100 * v_diff) / curr_tres; + + if (state == MC_STATE_STARTING && ratio < MCPWM_MAX_COMM_START_DIFF + && start_pulses > MCPWM_MIN_START_STEPS) { + // We think we are running in closed loop. Stop start sequence! + state = MC_STATE_RUNNING; + cycle_integrator = CYCLE_INT_START; + } else if (state == MC_STATE_RUNNING) { + integrate_cycle((float)v_diff); + } + } else { + cycle_integrator = CYCLE_INT_START; + } + } + + pwm_adc_cycles++; +#else + int hall_phase = mcpwm_read_hall_phase(); + if (comm_step != hall_phase) { + comm_step = hall_phase; + + update_rpm_tacho(); + + if (state == MC_STATE_RUNNING) { + set_next_comm_step(hall_phase); + TIM_GenerateEvent(TIM1, TIM_EventSource_COM); + } + } +#endif + + if (state == MC_STATE_RUNNING) { + const float ramp_step = MCPWM_RAMP_STEP / (MCPWM_SWITCH_FREQUENCY / 1000.0); + const float current = mcpwm_get_tot_current(); + const float current_in = mcpwm_get_tot_current_in(); + + float dutycycle_set_tmp = dutycycle_set; + +#if MCPWM_IS_SENSORLESS + if (closed_cycles < MCPWM_CLOSED_STARTPWM_COMMS) { + dutycycle_set_tmp = get_start_duty(); + } +#endif + + motor_current_sum += current; + input_current_sum += current_in; + motor_current_iterations++; + input_current_iterations++; + + if (current > MCPWM_CURRENT_MAX) { + step_towards((float*) &dutycycle_now, 0.0, + ramp_step * fabsf(current - MCPWM_CURRENT_MAX)); + } else if (current < MCPWM_CURRENT_MIN) { + step_towards((float*) &dutycycle_now, MCPWM_MAX_DUTY_CYCLE, ramp_step * 0.5); + } else if (fabsf(current_in) > MCPWM_IN_CURRENT_LIMIT) { + step_towards((float*) &dutycycle_now, 0.0, + ramp_step * fabsf(current_in - MCPWM_IN_CURRENT_LIMIT)); + } else { + step_towards((float*)&dutycycle_now, dutycycle_set_tmp, ramp_step); + } + + set_duty_cycle(dutycycle_now); + + if (state == MC_STATE_RUNNING && + dutycycle_set < MCPWM_MIN_DUTY_CYCLE && + dutycycle_now <= MCPWM_MIN_DUTY_CYCLE + 0.03) { + state = MC_STATE_OFF; + dutycycle_now = 0.0; + dutycycle_set = 0.0; + if (MCPWM_FULL_BRAKE_AT_STOP) { + mcpwm_full_brake(); + } else { + stop_pwm(); + } + } + } + + main_dma_adc_handler(); + + last_adc_isr_duration = (float)TIM4->CNT / 10000000; +} + +float mcpwm_read_reset_avg_motor_current(void) { + float res = motor_current_sum / motor_current_iterations; + motor_current_sum = 0; + motor_current_iterations = 0; + return res; +} + +float mcpwm_read_reset_avg_input_current(void) { + float res = input_current_sum / input_current_iterations; + input_current_sum = 0; + input_current_iterations = 0; + return res; +} + +float mcpwm_get_dutycycle_now(void) { + return dutycycle_now; +} + +float mcpwm_get_last_adc_isr_duration(void) { + return last_adc_isr_duration; +} + +float mcpwm_get_last_inj_adc_isr_duration(void) { + return last_inj_adc_isr_duration; +} + +#if MCPWM_IS_SENSORLESS +static int integrate_cycle(float v_diff) { + cycle_integrator += v_diff; + static const float limit = (MCPWM_CYCLE_INT_LIMIT * 800000.0) / (float)MCPWM_SWITCH_FREQUENCY; + + if (cycle_integrator >= limit) { + TIM_GenerateEvent(TIM1, TIM_EventSource_COM); + closed_cycles++; + cycle_integrator = CYCLE_INT_START; + return 1; + } + + return 0; +} +#endif + +/** + * Read the current phase of the motor using hall effect sensors + * @return + * The phase read. + */ +signed int mcpwm_read_hall_phase(void) { + int hall = READ_HALL1() | (READ_HALL2() << 1) | (READ_HALL3() << 2); + + signed int tmp_phase = -1; + int shift = mc_shift_table[hall + (hall_sensor_order << 3)]; + + switch (shift) { + case 0b101: + tmp_phase = 1; + break; + + case 0b001: + tmp_phase = 2; + break; + + case 0b011: + tmp_phase = 3; + break; + + case 0b010: + tmp_phase = 4; + break; + + case 0b110: + tmp_phase = 5; + break; + + case 0b100: + tmp_phase = 6; + break; + case 0b000: + case 0b111: + tmp_phase = -1; + break; + } + + // TODO: Gurgalof-fix + tmp_phase--; + if (tmp_phase == 0) { + tmp_phase = 6; + } + + // This is NOT a proper way to solve this... + if (!direction && tmp_phase >= 0) { + signed int p_tmp = tmp_phase; + p_tmp += 4; + if (p_tmp < 0) { + p_tmp += 6; + } else if (p_tmp > 5) { + p_tmp -= 6; + } + tmp_phase = 6 - p_tmp; + } + + return tmp_phase; +} + +/* + * Commutation Steps FORWARDS + * STEP BR1 BR2 BR3 + * 1 0 + - + * 2 + 0 - + * 3 + - 0 + * 4 0 - + + * 5 - 0 + + * 6 - + 0 + * + * Commutation Steps REVERSE (switch phase 2 and 3) + * STEP BR1 BR2 BR3 + * 1 0 - + + * 2 + - 0 + * 3 + 0 - + * 4 0 + - + * 5 - + 0 + * 6 - 0 + + */ + +static void update_adc_sample_pos(void) { + uint32_t period = TIM1->CCR1; + uint32_t samp_neg = period / 2; + uint32_t samp_pos = (TIM1->ARR - period) / 2 + period; + uint32_t samp_zero = TIM1->ARR - 2; + + // Sample the ADC at an appropriate time during the pwm cycle + if (pwm_mode == PWM_MODE_BIPOLAR) { + TIM8->CCR1 = samp_neg - 500; // ?? + + switch (comm_step) { + case 1: + if (direction) { + TIM1->CCR4 = samp_zero; + TIM8->CCR2 = samp_neg; + } else { + TIM1->CCR4 = samp_zero; + TIM8->CCR2 = samp_pos; + } + break; + + case 2: + if (direction) { + TIM1->CCR4 = samp_pos; + TIM8->CCR2 = samp_neg; + } else { + TIM1->CCR4 = samp_pos; + TIM8->CCR2 = samp_zero; + } + break; + + case 3: + if (direction) { + TIM1->CCR4 = samp_pos; + TIM8->CCR2 = samp_zero; + } else { + TIM1->CCR4 = samp_pos; + TIM8->CCR2 = samp_neg; + } + break; + + case 4: + if (direction) { + TIM1->CCR4 = samp_zero; + TIM8->CCR2 = samp_pos; + } else { + TIM1->CCR4 = samp_zero; + TIM8->CCR2 = samp_neg; + } + break; + + case 5: + if (direction) { + TIM1->CCR4 = samp_neg; + TIM8->CCR2 = samp_pos; + } else { + TIM1->CCR4 = samp_neg; + TIM8->CCR2 = samp_zero; + } + break; + + case 6: + if (direction) { + TIM1->CCR4 = samp_neg; + TIM8->CCR2 = samp_zero; + } else { + TIM1->CCR4 = samp_neg; + TIM8->CCR2 = samp_pos; + } + break; + } + } else { +// TIM8->CCR1 = period / 2; + + // TODO: WTF?? + const uint32_t low_samp = 215; + const uint32_t norm_samp = period / 2; + const uint32_t low = TIM1->ARR / 12; + const uint32_t high = TIM1->ARR / 8; + + if (period <= low) { + TIM8->CCR1 = low_samp; + } else if (period < high) { + float ratio = calc_ratio(low, high, period); + TIM8->CCR1 = (uint32_t) ((float) low_samp * (1.0 - ratio) + + (float) norm_samp * ratio); + } else { + TIM8->CCR1 = norm_samp; + } + + // Current samples + TIM1->CCR4 = (TIM1->ARR - period) / 2 + period; + TIM8->CCR2 = (TIM1->ARR - period) / 2 + period; + } +} + +static void update_rpm_tacho(void) { + pwm_adc_cycles = 0; + + static uint32_t comm_counter = 0; + comm_counter++; + + if (comm_counter == MCPWM_AVG_COM_RPM) { + comm_counter = 0; + uint32_t tim_val = TIM2->CNT; + uint32_t tim_diff = tim_val - last_comm_time; + last_comm_time = tim_val; + + if (tim_diff > 0) { + rpm_now = ((float)MCPWM_AVG_COM_RPM * 1000000.0 * 60.0) / + ((float)tim_diff * (float)MCPWM_NUM_POLES * 3.0); + } + } + + static int last_step = 0; + int tacho_diff = 0; + + if (comm_step == 1 && last_step == 6) { + tacho_diff++; + } else if (comm_step == 6 && last_step == 1) { + tacho_diff--; + } else { + tacho_diff += comm_step - last_step; + } + + last_step = comm_step; + + // Tachometer + if (direction) { + tachometer += tacho_diff; + } else { + tachometer -= tacho_diff; + } +} + +void mcpwm_comm_int_handler(void) { +#if MCPWM_IS_SENSORLESS + // PWM commutation in advance for next step. + + if (!(state == MC_STATE_STARTING || state == MC_STATE_RUNNING)) { + return; + } + + update_rpm_tacho(); + + comm_step++; + if (comm_step > 6) { + comm_step = 1; + } + + int next_step = comm_step + 1; + if (next_step > 6) { + next_step = 1; + } + + set_next_comm_step(next_step); +#endif +} + +static void set_next_comm_step(int next_step) { + uint16_t positive_oc_mode = TIM_OCMode_PWM1; + uint16_t negative_oc_mode = TIM_OCMode_Inactive; + + uint16_t positive_highside = TIM_CCx_Enable; + uint16_t positive_lowside = TIM_CCxN_Enable; + + uint16_t negative_highside = TIM_CCx_Enable; + uint16_t negative_lowside = TIM_CCxN_Enable; + + switch (pwm_mode) { + case PWM_MODE_NONSYNCHRONOUS_LOSW: + positive_oc_mode = TIM_OCMode_Active; + negative_oc_mode = TIM_OCMode_PWM2; + negative_highside = TIM_CCx_Disable; + break; + + case PWM_MODE_NONSYNCHRONOUS_HISW: + positive_lowside = TIM_CCxN_Disable; + break; + + case PWM_MODE_SYNCHRONOUS: + break; + + case PWM_MODE_BIPOLAR: + negative_oc_mode = TIM_OCMode_PWM2; + break; + } + + if (next_step == 1) { + if (direction) { + // 0 + TIM_SelectOCxM(TIM1, TIM_Channel_1, TIM_OCMode_Inactive); + TIM_CCxCmd(TIM1, TIM_Channel_1, TIM_CCx_Enable); + TIM_CCxNCmd(TIM1, TIM_Channel_1, TIM_CCxN_Disable); + + // + + TIM_SelectOCxM(TIM1, TIM_Channel_2, positive_oc_mode); + TIM_CCxCmd(TIM1, TIM_Channel_2, positive_highside); + TIM_CCxNCmd(TIM1, TIM_Channel_2, positive_lowside); + + // - + TIM_SelectOCxM(TIM1, TIM_Channel_3, negative_oc_mode); + TIM_CCxCmd(TIM1, TIM_Channel_3, negative_highside); + TIM_CCxNCmd(TIM1, TIM_Channel_3, negative_lowside); + } else { + // 0 + TIM_SelectOCxM(TIM1, TIM_Channel_1, TIM_OCMode_Inactive); + TIM_CCxCmd(TIM1, TIM_Channel_1, TIM_CCx_Enable); + TIM_CCxNCmd(TIM1, TIM_Channel_1, TIM_CCxN_Disable); + + // + + TIM_SelectOCxM(TIM1, TIM_Channel_3, positive_oc_mode); + TIM_CCxCmd(TIM1, TIM_Channel_3, positive_highside); + TIM_CCxNCmd(TIM1, TIM_Channel_3, positive_lowside); + + // - + TIM_SelectOCxM(TIM1, TIM_Channel_2, negative_oc_mode); + TIM_CCxCmd(TIM1, TIM_Channel_2, negative_highside); + TIM_CCxNCmd(TIM1, TIM_Channel_2, negative_lowside); + } + } else if (next_step == 2) { + if (direction) { + // 0 + TIM_SelectOCxM(TIM1, TIM_Channel_2, TIM_OCMode_Inactive); + TIM_CCxCmd(TIM1, TIM_Channel_2, TIM_CCx_Enable); + TIM_CCxNCmd(TIM1, TIM_Channel_2, TIM_CCxN_Disable); + + // + + TIM_SelectOCxM(TIM1, TIM_Channel_1, positive_oc_mode); + TIM_CCxCmd(TIM1, TIM_Channel_1, positive_highside); + TIM_CCxNCmd(TIM1, TIM_Channel_1, positive_lowside); + + // - + TIM_SelectOCxM(TIM1, TIM_Channel_3, negative_oc_mode); + TIM_CCxCmd(TIM1, TIM_Channel_3, negative_highside); + TIM_CCxNCmd(TIM1, TIM_Channel_3, negative_lowside); + } else { + // 0 + TIM_SelectOCxM(TIM1, TIM_Channel_3, TIM_OCMode_Inactive); + TIM_CCxCmd(TIM1, TIM_Channel_3, TIM_CCx_Enable); + TIM_CCxNCmd(TIM1, TIM_Channel_3, TIM_CCxN_Disable); + + // + + TIM_SelectOCxM(TIM1, TIM_Channel_1, positive_oc_mode); + TIM_CCxCmd(TIM1, TIM_Channel_1, positive_highside); + TIM_CCxNCmd(TIM1, TIM_Channel_1, positive_lowside); + + // - + TIM_SelectOCxM(TIM1, TIM_Channel_2, negative_oc_mode); + TIM_CCxCmd(TIM1, TIM_Channel_2, negative_highside); + TIM_CCxNCmd(TIM1, TIM_Channel_2, negative_lowside); + } + } else if (next_step == 3) { + if (direction) { + // 0 + TIM_SelectOCxM(TIM1, TIM_Channel_3, TIM_OCMode_Inactive); + TIM_CCxCmd(TIM1, TIM_Channel_3, TIM_CCx_Enable); + TIM_CCxNCmd(TIM1, TIM_Channel_3, TIM_CCxN_Disable); + + // + + TIM_SelectOCxM(TIM1, TIM_Channel_1, positive_oc_mode); + TIM_CCxCmd(TIM1, TIM_Channel_1, positive_highside); + TIM_CCxNCmd(TIM1, TIM_Channel_1, positive_lowside); + + // - + TIM_SelectOCxM(TIM1, TIM_Channel_2, negative_oc_mode); + TIM_CCxCmd(TIM1, TIM_Channel_2, negative_highside); + TIM_CCxNCmd(TIM1, TIM_Channel_2, negative_lowside); + } else { + // 0 + TIM_SelectOCxM(TIM1, TIM_Channel_2, TIM_OCMode_Inactive); + TIM_CCxCmd(TIM1, TIM_Channel_2, TIM_CCx_Enable); + TIM_CCxNCmd(TIM1, TIM_Channel_2, TIM_CCxN_Disable); + + // + + TIM_SelectOCxM(TIM1, TIM_Channel_1, positive_oc_mode); + TIM_CCxCmd(TIM1, TIM_Channel_1, positive_highside); + TIM_CCxNCmd(TIM1, TIM_Channel_1, positive_lowside); + + // - + TIM_SelectOCxM(TIM1, TIM_Channel_3, negative_oc_mode); + TIM_CCxCmd(TIM1, TIM_Channel_3, negative_highside); + TIM_CCxNCmd(TIM1, TIM_Channel_3, negative_lowside); + } + } else if (next_step == 4) { + if (direction) { + // 0 + TIM_SelectOCxM(TIM1, TIM_Channel_1, TIM_OCMode_Inactive); + TIM_CCxCmd(TIM1, TIM_Channel_1, TIM_CCx_Enable); + TIM_CCxNCmd(TIM1, TIM_Channel_1, TIM_CCxN_Disable); + + // + + TIM_SelectOCxM(TIM1, TIM_Channel_3, positive_oc_mode); + TIM_CCxCmd(TIM1, TIM_Channel_3, positive_highside); + TIM_CCxNCmd(TIM1, TIM_Channel_3, positive_lowside); + + // - + TIM_SelectOCxM(TIM1, TIM_Channel_2, negative_oc_mode); + TIM_CCxCmd(TIM1, TIM_Channel_2, negative_highside); + TIM_CCxNCmd(TIM1, TIM_Channel_2, negative_lowside); + } else { + // 0 + TIM_SelectOCxM(TIM1, TIM_Channel_1, TIM_OCMode_Inactive); + TIM_CCxCmd(TIM1, TIM_Channel_1, TIM_CCx_Enable); + TIM_CCxNCmd(TIM1, TIM_Channel_1, TIM_CCxN_Disable); + + // + + TIM_SelectOCxM(TIM1, TIM_Channel_2, positive_oc_mode); + TIM_CCxCmd(TIM1, TIM_Channel_2, positive_highside); + TIM_CCxNCmd(TIM1, TIM_Channel_2, positive_lowside); + + // - + TIM_SelectOCxM(TIM1, TIM_Channel_3, negative_oc_mode); + TIM_CCxCmd(TIM1, TIM_Channel_3, negative_highside); + TIM_CCxNCmd(TIM1, TIM_Channel_3, negative_lowside); + } + } else if (next_step == 5) { + if (direction) { + // 0 + TIM_SelectOCxM(TIM1, TIM_Channel_2, TIM_OCMode_Inactive); + TIM_CCxCmd(TIM1, TIM_Channel_2, TIM_CCx_Enable); + TIM_CCxNCmd(TIM1, TIM_Channel_2, TIM_CCxN_Disable); + + // + + TIM_SelectOCxM(TIM1, TIM_Channel_3, positive_oc_mode); + TIM_CCxCmd(TIM1, TIM_Channel_3, positive_highside); + TIM_CCxNCmd(TIM1, TIM_Channel_3, positive_lowside); + + // - + TIM_SelectOCxM(TIM1, TIM_Channel_1, negative_oc_mode); + TIM_CCxCmd(TIM1, TIM_Channel_1, negative_highside); + TIM_CCxNCmd(TIM1, TIM_Channel_1, negative_lowside); + } else { + // 0 + TIM_SelectOCxM(TIM1, TIM_Channel_3, TIM_OCMode_Inactive); + TIM_CCxCmd(TIM1, TIM_Channel_3, TIM_CCx_Enable); + TIM_CCxNCmd(TIM1, TIM_Channel_3, TIM_CCxN_Disable); + + // + + TIM_SelectOCxM(TIM1, TIM_Channel_2, positive_oc_mode); + TIM_CCxCmd(TIM1, TIM_Channel_2, positive_highside); + TIM_CCxNCmd(TIM1, TIM_Channel_2, positive_lowside); + + // - + TIM_SelectOCxM(TIM1, TIM_Channel_1, negative_oc_mode); + TIM_CCxCmd(TIM1, TIM_Channel_1, negative_highside); + TIM_CCxNCmd(TIM1, TIM_Channel_1, negative_lowside); + } + } else if (next_step == 6) { + if (direction) { + // 0 + TIM_SelectOCxM(TIM1, TIM_Channel_3, TIM_OCMode_Inactive); + TIM_CCxCmd(TIM1, TIM_Channel_3, TIM_CCx_Enable); + TIM_CCxNCmd(TIM1, TIM_Channel_3, TIM_CCxN_Disable); + + // + + TIM_SelectOCxM(TIM1, TIM_Channel_2, positive_oc_mode); + TIM_CCxCmd(TIM1, TIM_Channel_2, positive_highside); + TIM_CCxNCmd(TIM1, TIM_Channel_2, positive_lowside); + + // - + TIM_SelectOCxM(TIM1, TIM_Channel_1, negative_oc_mode); + TIM_CCxCmd(TIM1, TIM_Channel_1, negative_highside); + TIM_CCxNCmd(TIM1, TIM_Channel_1, negative_lowside); + } else { + // 0 + TIM_SelectOCxM(TIM1, TIM_Channel_2, TIM_OCMode_Inactive); + TIM_CCxCmd(TIM1, TIM_Channel_2, TIM_CCx_Enable); + TIM_CCxNCmd(TIM1, TIM_Channel_2, TIM_CCxN_Disable); + + // + + TIM_SelectOCxM(TIM1, TIM_Channel_3, positive_oc_mode); + TIM_CCxCmd(TIM1, TIM_Channel_3, positive_highside); + TIM_CCxNCmd(TIM1, TIM_Channel_3, positive_lowside); + + // - + TIM_SelectOCxM(TIM1, TIM_Channel_1, negative_oc_mode); + TIM_CCxCmd(TIM1, TIM_Channel_1, negative_highside); + TIM_CCxNCmd(TIM1, TIM_Channel_1, negative_lowside); + } + } else if (next_step == 32) { + // NOTE: This means we are going to use sine modulation. Switch on all phases! + TIM_SelectOCxM(TIM1, TIM_Channel_1, TIM_OCMode_PWM1); + TIM_CCxCmd(TIM1, TIM_Channel_1, TIM_CCx_Enable); + TIM_CCxNCmd(TIM1, TIM_Channel_1, TIM_CCxN_Enable); + + TIM_SelectOCxM(TIM1, TIM_Channel_2, TIM_OCMode_PWM1); + TIM_CCxCmd(TIM1, TIM_Channel_2, TIM_CCx_Enable); + TIM_CCxNCmd(TIM1, TIM_Channel_2, TIM_CCxN_Enable); + + TIM_SelectOCxM(TIM1, TIM_Channel_3, TIM_OCMode_PWM1); + TIM_CCxCmd(TIM1, TIM_Channel_3, TIM_CCx_Enable); + TIM_CCxNCmd(TIM1, TIM_Channel_3, TIM_CCxN_Enable); + } else { + // Invalid phase.. stop PWM! + TIM_SelectOCxM(TIM1, TIM_Channel_1, TIM_ForcedAction_InActive); + TIM_CCxCmd(TIM1, TIM_Channel_1, TIM_CCx_Enable); + TIM_CCxNCmd(TIM1, TIM_Channel_1, TIM_CCxN_Disable); + + TIM_SelectOCxM(TIM1, TIM_Channel_2, TIM_ForcedAction_InActive); + TIM_CCxCmd(TIM1, TIM_Channel_2, TIM_CCx_Enable); + TIM_CCxNCmd(TIM1, TIM_Channel_2, TIM_CCxN_Disable); + + TIM_SelectOCxM(TIM1, TIM_Channel_3, TIM_ForcedAction_InActive); + TIM_CCxCmd(TIM1, TIM_Channel_3, TIM_CCx_Enable); + TIM_CCxNCmd(TIM1, TIM_Channel_3, TIM_CCxN_Disable); + } + + update_adc_sample_pos(); +} diff --git a/mcpwm.h b/mcpwm.h new file mode 100644 index 000000000..f5b0359ba --- /dev/null +++ b/mcpwm.h @@ -0,0 +1,188 @@ +/* + Copyright 2012 Benjamin Vedder benjamin@vedder.se + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +/* + * mc_pwm.h + * + * Created on: 13 okt 2012 + * Author: benjamin + */ + +#ifndef MCPWM_H_ +#define MCPWM_H_ + +typedef enum { + MC_STATE_OFF = 0, + MC_STATE_DETECTING, + MC_STATE_STARTING, + MC_STATE_RUNNING, + MC_STATE_FULL_BRAKE, + MC_STATE_STATIC_VECTOR +} mc_state; + +typedef enum { + PWM_MODE_NONSYNCHRONOUS_LOSW = 0, // Note: Does not work! + PWM_MODE_NONSYNCHRONOUS_HISW, + PWM_MODE_SYNCHRONOUS, + PWM_MODE_BIPOLAR +} mc_pwm_mode; + +// Functions +void mcpwm_init(void); +void mcpwm_set_duty(float dutyCycle); +int mcpwm_get_comm_step(void); +float mcpwm_get_duty_cycle(void); +float mcpwm_get_rpm(void); +void mcpwm_use_pid(int use_pid); +void mcpwm_set_pid_speed(float rpm); +float mcpwm_get_kv(void); +float mcpwm_get_kv_filtered(void); +int mcpwm_get_tachometer_value(int reset); +float mcpwm_get_tot_current_filtered(void); +float mcpwm_get_tot_current(void); +float mcpwm_get_tot_current_in(void); +void mcpwm_set_detect(void); +int mcpwm_get_detect_top(void); +mc_state mcpwm_get_state(void); +signed int mcpwm_read_hall_phase(void); +void mcpwm_full_brake(void); +float mcpwm_read_reset_avg_motor_current(void); +float mcpwm_read_reset_avg_input_current(void); +float mcpwm_get_dutycycle_now(void); +float mcpwm_get_last_adc_isr_duration(void); +float mcpwm_get_last_inj_adc_isr_duration(void); + +// Interrupt handlers +void mcpwm_time_int_handler(void); +void mcpwm_comm_int_handler(void); +void mcpwm_adc_inj_int_handler(void); +void mcpwm_adc_int_handler(void *p, uint32_t flags); + +// External variables +extern volatile uint16_t ADC_Value[]; +extern volatile int ADC_curr_norm_value[]; + +// Macros +#define READ_HALL1() palReadPad(GPIOB, 6) +#define READ_HALL2() palReadPad(GPIOB, 7) +#define READ_HALL3() palReadPad(GPIOB, 8) + +/* + * Parameters + */ +#define MCPWM_SWITCH_FREQUENCY 40000 // Switching frequency in HZ +#define MCPWM_DEAD_TIME_CYCLES 80 // Dead time +#define MCPWM_PWM_MODE PWM_MODE_BIPOLAR // Default PWM mode +#define MCPWM_MIN_DUTY_CYCLE 0.01 // Minimum duty cycle +#define MCPWM_MAX_DUTY_CYCLE 0.95 // Maximum duty cycle +#define MCPWM_AVG_COM_RPM 6 // Number of commutations to average RPM over +#define MCPWM_NUM_POLES 2 // Motor pole number (for RPM calculation) +#define MCPWM_HALL_SENSOR_ORDER 5 // Order in which hall sensors are connected +#define MCPWM_RAMP_STEP 0.005 // Ramping step (1000 times/sec) +#define MCPWM_CURRENT_MAX 30.0 // Current limit in Amperes +#define MCPWM_CURRENT_MIN -10.0 // Current limit in Amperes +#define MCPWM_IN_CURRENT_LIMIT 20.0 // Input current limit in Amperes +#define MCPWM_FULL_BRAKE_AT_STOP 0 // Brake the motor when the power is set to stopped + +// Sensorless settings +#define MCPWM_IS_SENSORLESS 1 // Use sensorless commutation +#define MCPWM_MAX_COMM_START_DIFF 2 // The lower the number, the more picky the the closed loop detector +#define MCPWM_MIN_CLOSED_RPM 300 // Switch to open loop below this RPM +#define MCPWM_START_COMM_TIME_MS_L 7 // Commutation time during startup in msecs LOW +#define MCPWM_START_COMM_TIME_MS_H 20 // Commutation time during startup in msecs HIGH +#define MCPWM_START_DUTY_CYCLE_L 0.06 // Startup duty cycle LOW @ 20V +#define MCPWM_START_DUTY_CYCLE_H 0.2 // Startup duty cycle HIGH @ 20V +#define MCPWM_START_DUTY_CYCLE_REV_L 0.06 // Startup duty cycle LOW @ 20V +#define MCPWM_START_DUTY_CYCLE_REV_H 0.2 // Startup duty cycle HIGH @ 20V +#define MCPWM_MIN_START_STEPS 1 // Minimum steps to run in open loop +#define MCPWM_CLOSED_STARTPWM_COMMS 1 // Run at least this many commutations in closed loop with start duty cycle +#define MCPWM_CYCLE_INT_LIMIT 150.0 // Flux integrator limit +#define MCPWM_VZERO_FACT 1.0 // Virtual zero adjustment +#define MCPWM_COMM_RPM_FACTOR 0.4 // at least run one commutation for the expected times times this factor + +// PID parameters +#define MCPWM_PID_TIME_K 0.001 // Pid controller sample time in seconds +#define MCPWM_PID_KP 0.0001 // Proportional gain +#define MCPWM_PID_KI 0.002 // Integral gain +#define MCPWM_PID_KD 0.0 // Derivative gain +#define MCPWM_PID_MIN_RPM 2000.0 // Minimum allowed RPM + +// Misc settings +#define MCPWM_ADC_CHANNELS 12 + +/* + * Turnigy big + * MCPWM_USE_BIPOLAR_PWM: 0 + * MCPWM_MAX_COMM_START_DIFF: 20 + * MCPWM_START_DUTY_CYCLE_L: 0.05 + * MCPWM_START_DUTY_CYCLE_H: 0.12 + * MCPWM_START_DUTY_CYCLE_REV_L: 0.05 + * MCPWM_START_DUTY_CYCLE_REV_H. 0.12 + * MCPWM_CYCLE_INT_LIMIT: 150 + * MCPWM_PID_KP: 0.0001 + * MCPWM_PID_KI: 0.002 + * MCPWM_VZERO_FACT: 1.0 + * MCPWM_COMM_RPM_FACTOR: ?? + */ + +/* + * Gurgalof + * MCPWM_USE_BIPOLAR_PWM: 0 + * MCPWM_MAX_COMM_START_DIFF: 20 + * MCPWM_START_DUTY_CYCLE_L: 0.2 + * MCPWM_START_DUTY_CYCLE_H: 0.5 + * MCPWM_START_DUTY_CYCLE_REV_L: 0.2 + * MCPWM_START_DUTY_CYCLE_REV_H. 0.5 + * MCPWM_CYCLE_INT_LIMIT: 350 + * MCPWM_PID_KP: 0.0001 + * MCPWM_PID_KI: 0.002 + * MCPWM_VZERO_FACT: 1.0 + * MCPWM_COMM_RPM_FACTOR: ?? + */ + +/* + * Selden parameters (black motor): + * MCPWM_USE_BIPOLAR_PWM: 0 + * MCPWM_MAX_COMM_START_DIFF: 20 + * MCPWM_START_DUTY_CYCLE_L: 0.1 + * MCPWM_START_DUTY_CYCLE_H: 0.3 + * MCPWM_START_DUTY_CYCLE_REV_L: 0.1 + * MCPWM_START_DUTY_CYCLE_REV_H. 0.18 + * MCPWM_CYCLE_INT_LIMIT: 110 + * MCPWM_PID_KP: 0.0001 + * MCPWM_PID_KI: 0.002 + * MCPWM_VZERO_FACT: 1.0 + * MCPWM_COMM_RPM_FACTOR: ?? + */ + +/* + * Selden parameters (orange motor): + * MCPWM_USE_BIPOLAR_PWM: 0 + * MCPWM_MAX_COMM_START_DIFF: 10 + * MCPWM_START_DUTY_CYCLE_L: 0.15 + * MCPWM_START_DUTY_CYCLE_H: 0.4 + * MCPWM_START_DUTY_CYCLE_REV_L: 0.15 + * MCPWM_START_DUTY_CYCLE_REV_H. 0.2 + * MCPWM_CYCLE_INT_LIMIT: 110 + * MCPWM_PID_KP: 0.0001 + * MCPWM_PID_KI: 0.002 + * MCPWM_VZERO_FACT: 1.0 + * MCPWM_COMM_RPM_FACTOR: 0.4 + */ + + +#endif /* MC_PWM_H_ */ diff --git a/mcuconf.h b/mcuconf.h new file mode 100644 index 000000000..36f0f01b3 --- /dev/null +++ b/mcuconf.h @@ -0,0 +1,257 @@ +/* + ChibiOS/RT - Copyright (C) 2006,2007,2008,2009,2010, + 2011,2012 Giovanni Di Sirio. + + This file is part of ChibiOS/RT. + + ChibiOS/RT is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 3 of the License, or + (at your option) any later version. + + ChibiOS/RT is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ + +/* + * STM32F4xx drivers configuration. + * The following settings override the default settings present in + * the various device driver implementation headers. + * Note that the settings for each driver only have effect if the whole + * driver is enabled in halconf.h. + * + * IRQ priorities: + * 15...0 Lowest...Highest. + * + * DMA priorities: + * 0...3 Lowest...Highest. + */ + +#define STM32F4xx_MCUCONF + +/* + * HAL driver system settings. + */ +#define STM32_NO_INIT FALSE +#define STM32_HSI_ENABLED TRUE +#define STM32_LSI_ENABLED TRUE +#define STM32_HSE_ENABLED TRUE +#define STM32_LSE_ENABLED FALSE +#define STM32_CLOCK48_REQUIRED TRUE +#define STM32_SW STM32_SW_PLL +#define STM32_PLLSRC STM32_PLLSRC_HSE +#define STM32_PLLM_VALUE 8 +#define STM32_PLLN_VALUE 336 +#define STM32_PLLP_VALUE 2 +#define STM32_PLLQ_VALUE 7 +#define STM32_HPRE STM32_HPRE_DIV1 +#define STM32_PPRE1 STM32_PPRE1_DIV4 +#define STM32_PPRE2 STM32_PPRE2_DIV2 +#define STM32_RTCSEL STM32_RTCSEL_LSI +#define STM32_RTCPRE_VALUE 8 +#define STM32_MCO1SEL STM32_MCO1SEL_HSI +#define STM32_MCO1PRE STM32_MCO1PRE_DIV1 +#define STM32_MCO2SEL STM32_MCO2SEL_SYSCLK +#define STM32_MCO2PRE STM32_MCO2PRE_DIV5 +#define STM32_I2SSRC STM32_I2SSRC_CKIN +#define STM32_PLLI2SN_VALUE 192 +#define STM32_PLLI2SR_VALUE 5 +#define STM32_VOS STM32_VOS_HIGH +#define STM32_PVD_ENABLE FALSE +#define STM32_PLS STM32_PLS_LEV0 + +/* + * ADC driver system settings. + */ +#define STM32_ADC_ADCPRE ADC_CCR_ADCPRE_DIV4 +#define STM32_ADC_USE_ADC1 FALSE +#define STM32_ADC_USE_ADC2 FALSE +#define STM32_ADC_USE_ADC3 FALSE +#define STM32_ADC_ADC1_DMA_STREAM STM32_DMA_STREAM_ID(2, 4) +#define STM32_ADC_ADC2_DMA_STREAM STM32_DMA_STREAM_ID(2, 2) +#define STM32_ADC_ADC3_DMA_STREAM STM32_DMA_STREAM_ID(2, 1) +#define STM32_ADC_ADC1_DMA_PRIORITY 2 +#define STM32_ADC_ADC2_DMA_PRIORITY 2 +#define STM32_ADC_ADC3_DMA_PRIORITY 2 +#define STM32_ADC_IRQ_PRIORITY 6 +#define STM32_ADC_ADC1_DMA_IRQ_PRIORITY 6 +#define STM32_ADC_ADC2_DMA_IRQ_PRIORITY 6 +#define STM32_ADC_ADC3_DMA_IRQ_PRIORITY 6 + +/* + * CAN driver system settings. + */ +#define STM32_CAN_USE_CAN1 FALSE +#define STM32_CAN_USE_CAN2 FALSE +#define STM32_CAN_CAN1_IRQ_PRIORITY 11 +#define STM32_CAN_CAN2_IRQ_PRIORITY 11 + +/* + * EXT driver system settings. + */ +#define STM32_EXT_EXTI0_IRQ_PRIORITY 6 +#define STM32_EXT_EXTI1_IRQ_PRIORITY 6 +#define STM32_EXT_EXTI2_IRQ_PRIORITY 6 +#define STM32_EXT_EXTI3_IRQ_PRIORITY 6 +#define STM32_EXT_EXTI4_IRQ_PRIORITY 6 +#define STM32_EXT_EXTI5_9_IRQ_PRIORITY 6 +#define STM32_EXT_EXTI10_15_IRQ_PRIORITY 6 +#define STM32_EXT_EXTI16_IRQ_PRIORITY 6 +#define STM32_EXT_EXTI17_IRQ_PRIORITY 15 +#define STM32_EXT_EXTI18_IRQ_PRIORITY 6 +#define STM32_EXT_EXTI19_IRQ_PRIORITY 6 +#define STM32_EXT_EXTI20_IRQ_PRIORITY 6 +#define STM32_EXT_EXTI21_IRQ_PRIORITY 15 +#define STM32_EXT_EXTI22_IRQ_PRIORITY 15 + +/* + * GPT driver system settings. + */ +#define STM32_GPT_USE_TIM1 FALSE +#define STM32_GPT_USE_TIM2 FALSE +#define STM32_GPT_USE_TIM3 FALSE +#define STM32_GPT_USE_TIM4 FALSE +#define STM32_GPT_USE_TIM5 FALSE +#define STM32_GPT_USE_TIM8 FALSE +#define STM32_GPT_TIM1_IRQ_PRIORITY 7 +#define STM32_GPT_TIM2_IRQ_PRIORITY 7 +#define STM32_GPT_TIM3_IRQ_PRIORITY 7 +#define STM32_GPT_TIM4_IRQ_PRIORITY 7 +#define STM32_GPT_TIM5_IRQ_PRIORITY 7 +#define STM32_GPT_TIM8_IRQ_PRIORITY 7 + +/* + * I2C driver system settings. + */ +#define STM32_I2C_USE_I2C1 FALSE +#define STM32_I2C_USE_I2C2 FALSE +#define STM32_I2C_USE_I2C3 FALSE +#define STM32_I2C_I2C1_RX_DMA_STREAM STM32_DMA_STREAM_ID(1, 0) +#define STM32_I2C_I2C1_TX_DMA_STREAM STM32_DMA_STREAM_ID(1, 6) +#define STM32_I2C_I2C2_RX_DMA_STREAM STM32_DMA_STREAM_ID(1, 2) +#define STM32_I2C_I2C2_TX_DMA_STREAM STM32_DMA_STREAM_ID(1, 7) +#define STM32_I2C_I2C3_RX_DMA_STREAM STM32_DMA_STREAM_ID(1, 2) +#define STM32_I2C_I2C3_TX_DMA_STREAM STM32_DMA_STREAM_ID(1, 4) +#define STM32_I2C_I2C1_IRQ_PRIORITY 5 +#define STM32_I2C_I2C2_IRQ_PRIORITY 5 +#define STM32_I2C_I2C3_IRQ_PRIORITY 5 +#define STM32_I2C_I2C1_DMA_PRIORITY 3 +#define STM32_I2C_I2C2_DMA_PRIORITY 3 +#define STM32_I2C_I2C3_DMA_PRIORITY 3 +#define STM32_I2C_I2C1_DMA_ERROR_HOOK() chSysHalt() +#define STM32_I2C_I2C2_DMA_ERROR_HOOK() chSysHalt() +#define STM32_I2C_I2C3_DMA_ERROR_HOOK() chSysHalt() + +/* + * ICU driver system settings. + */ +#define STM32_ICU_USE_TIM1 FALSE +#define STM32_ICU_USE_TIM2 FALSE +#define STM32_ICU_USE_TIM3 FALSE +#define STM32_ICU_USE_TIM4 FALSE +#define STM32_ICU_USE_TIM5 FALSE +#define STM32_ICU_USE_TIM8 FALSE +#define STM32_ICU_TIM1_IRQ_PRIORITY 7 +#define STM32_ICU_TIM2_IRQ_PRIORITY 7 +#define STM32_ICU_TIM3_IRQ_PRIORITY 7 +#define STM32_ICU_TIM4_IRQ_PRIORITY 7 +#define STM32_ICU_TIM5_IRQ_PRIORITY 7 +#define STM32_ICU_TIM8_IRQ_PRIORITY 7 + +/* + * PWM driver system settings. + */ +#define STM32_PWM_USE_ADVANCED FALSE +#define STM32_PWM_USE_TIM1 FALSE +#define STM32_PWM_USE_TIM2 FALSE +#define STM32_PWM_USE_TIM3 FALSE +#define STM32_PWM_USE_TIM4 FALSE +#define STM32_PWM_USE_TIM5 FALSE +#define STM32_PWM_USE_TIM8 FALSE +#define STM32_PWM_TIM1_IRQ_PRIORITY 7 +#define STM32_PWM_TIM2_IRQ_PRIORITY 7 +#define STM32_PWM_TIM3_IRQ_PRIORITY 7 +#define STM32_PWM_TIM4_IRQ_PRIORITY 7 +#define STM32_PWM_TIM5_IRQ_PRIORITY 7 +#define STM32_PWM_TIM8_IRQ_PRIORITY 7 + +/* + * SERIAL driver system settings. + */ +#define STM32_SERIAL_USE_USART1 FALSE +#define STM32_SERIAL_USE_USART2 TRUE +#define STM32_SERIAL_USE_USART3 FALSE +#define STM32_SERIAL_USE_UART4 FALSE +#define STM32_SERIAL_USE_UART5 FALSE +#define STM32_SERIAL_USE_USART6 FALSE +#define STM32_SERIAL_USART1_PRIORITY 12 +#define STM32_SERIAL_USART2_PRIORITY 12 +#define STM32_SERIAL_USART3_PRIORITY 12 +#define STM32_SERIAL_UART4_PRIORITY 12 +#define STM32_SERIAL_UART5_PRIORITY 12 +#define STM32_SERIAL_USART6_PRIORITY 12 + +/* + * SPI driver system settings. + */ +#define STM32_SPI_USE_SPI1 FALSE +#define STM32_SPI_USE_SPI2 FALSE +#define STM32_SPI_USE_SPI3 FALSE +#define STM32_SPI_SPI1_RX_DMA_STREAM STM32_DMA_STREAM_ID(2, 0) +#define STM32_SPI_SPI1_TX_DMA_STREAM STM32_DMA_STREAM_ID(2, 3) +#define STM32_SPI_SPI2_RX_DMA_STREAM STM32_DMA_STREAM_ID(1, 3) +#define STM32_SPI_SPI2_TX_DMA_STREAM STM32_DMA_STREAM_ID(1, 4) +#define STM32_SPI_SPI3_RX_DMA_STREAM STM32_DMA_STREAM_ID(1, 0) +#define STM32_SPI_SPI3_TX_DMA_STREAM STM32_DMA_STREAM_ID(1, 7) +#define STM32_SPI_SPI1_DMA_PRIORITY 1 +#define STM32_SPI_SPI2_DMA_PRIORITY 1 +#define STM32_SPI_SPI3_DMA_PRIORITY 1 +#define STM32_SPI_SPI1_IRQ_PRIORITY 10 +#define STM32_SPI_SPI2_IRQ_PRIORITY 10 +#define STM32_SPI_SPI3_IRQ_PRIORITY 10 +#define STM32_SPI_DMA_ERROR_HOOK(spip) chSysHalt() + +/* + * UART driver system settings. + */ +#define STM32_UART_USE_USART1 FALSE +#define STM32_UART_USE_USART2 TRUE +#define STM32_UART_USE_USART3 FALSE +#define STM32_UART_USE_USART6 FALSE +#define STM32_UART_USART1_RX_DMA_STREAM STM32_DMA_STREAM_ID(2, 5) +#define STM32_UART_USART1_TX_DMA_STREAM STM32_DMA_STREAM_ID(2, 7) +#define STM32_UART_USART2_RX_DMA_STREAM STM32_DMA_STREAM_ID(1, 5) +#define STM32_UART_USART2_TX_DMA_STREAM STM32_DMA_STREAM_ID(1, 6) +#define STM32_UART_USART3_RX_DMA_STREAM STM32_DMA_STREAM_ID(1, 1) +#define STM32_UART_USART3_TX_DMA_STREAM STM32_DMA_STREAM_ID(1, 3) +#define STM32_UART_USART6_RX_DMA_STREAM STM32_DMA_STREAM_ID(2, 2) +#define STM32_UART_USART6_TX_DMA_STREAM STM32_DMA_STREAM_ID(2, 7) +#define STM32_UART_USART1_IRQ_PRIORITY 12 +#define STM32_UART_USART2_IRQ_PRIORITY 12 +#define STM32_UART_USART3_IRQ_PRIORITY 12 +#define STM32_UART_USART6_IRQ_PRIORITY 12 +#define STM32_UART_USART1_DMA_PRIORITY 0 +#define STM32_UART_USART2_DMA_PRIORITY 0 +#define STM32_UART_USART3_DMA_PRIORITY 0 +#define STM32_UART_USART6_DMA_PRIORITY 0 +#define STM32_UART_DMA_ERROR_HOOK(uartp) chSysHalt() + +/* + * USB driver system settings. + */ +#define STM32_USB_USE_OTG1 TRUE +#define STM32_USB_USE_OTG2 FALSE +#define STM32_USB_OTG1_IRQ_PRIORITY 14 +#define STM32_USB_OTG2_IRQ_PRIORITY 14 +#define STM32_USB_OTG1_RX_FIFO_SIZE 512 +#define STM32_USB_OTG2_RX_FIFO_SIZE 1024 +#define STM32_USB_OTG_THREAD_PRIO LOWPRIO +#define STM32_USB_OTG_THREAD_STACK_SIZE 128 +#define STM32_USB_OTGFIFO_FILL_BASEPRI 0 + + diff --git a/myUSB.c b/myUSB.c new file mode 100644 index 000000000..fab2781ad --- /dev/null +++ b/myUSB.c @@ -0,0 +1,180 @@ +#include "ch.h" +#include "hal.h" +#include "shell.h" + +#include "myUSB.h" +#include "usbdescriptor.h" +/* + * Don't ask me, I have no idea what is done here... + * I think most things are either self explanatory, well documented or not to be touched + */ + +/* + * USB Driver structure. + */ +SerialUSBDriver SDU1; + + +/*===========================================================================*/ +/* USB related stuff. */ +/*===========================================================================*/ + +/* + * Endpoints to be used for USBD1. + */ +#define USBD1_DATA_REQUEST_EP 1 +#define USBD1_DATA_AVAILABLE_EP 1 +#define USBD1_INTERRUPT_REQUEST_EP 2 + +/* + * Handles the GET_DESCRIPTOR callback. All required descriptors must be + * handled here. + */ +static const USBDescriptor *get_descriptor(USBDriver *usbp, + uint8_t dtype, + uint8_t dindex, + uint16_t lang) { + + (void)usbp; + (void)lang; + switch (dtype) { + case USB_DESCRIPTOR_DEVICE: + return &vcom_device_descriptor; + case USB_DESCRIPTOR_CONFIGURATION: + return &vcom_configuration_descriptor; + case USB_DESCRIPTOR_STRING: + if (dindex < 4) + return &vcom_strings[dindex]; + } + return NULL; +} + +/** + * @brief IN EP1 state. + */ +static USBInEndpointState ep1instate; + +/** + * @brief OUT EP1 state. + */ +static USBOutEndpointState ep1outstate; + +/** + * @brief EP1 initialization structure (both IN and OUT). + */ +static const USBEndpointConfig ep1config = { + USB_EP_MODE_TYPE_BULK, + NULL, + sduDataTransmitted, + sduDataReceived, + 0x0040, + 0x0040, + &ep1instate, + &ep1outstate, + 2, + NULL +}; + +/** + * @brief IN EP2 state. + */ +static USBInEndpointState ep2instate; + +/** + * @brief EP2 initialization structure (IN only). + */ +static const USBEndpointConfig ep2config = { + USB_EP_MODE_TYPE_INTR, + NULL, + sduInterruptTransmitted, + NULL, + 0x0010, + 0x0000, + &ep2instate, + NULL, + 1, + NULL +}; + +/* + * Handles the USB driver global events. + */ +static void usb_event(USBDriver *usbp, usbevent_t event) { + extern SerialUSBDriver SDU1; + + switch (event) { + case USB_EVENT_RESET: + return; + case USB_EVENT_ADDRESS: + return; + case USB_EVENT_CONFIGURED: + chSysLockFromIsr(); + + /* Enables the endpoints specified into the configuration. +Note, this callback is invoked from an ISR so I-Class functions +must be used.*/ + usbInitEndpointI(usbp, USBD1_DATA_REQUEST_EP, &ep1config); + usbInitEndpointI(usbp, USBD1_INTERRUPT_REQUEST_EP, &ep2config); + + /* Resetting the state of the CDC subsystem.*/ + sduConfigureHookI(&SDU1); + + chSysUnlockFromIsr(); + return; + case USB_EVENT_SUSPEND: + return; + case USB_EVENT_WAKEUP: + return; + case USB_EVENT_STALLED: + return; + } + return; +} + +/* + * USB driver configuration. + */ +const USBConfig usbcfg = { + usb_event, + get_descriptor, + sduRequestsHook, + NULL +}; + +/* + * Serial over USB driver configuration. + */ +const SerialUSBConfig serusbcfg = { + &USBD1, + USBD1_DATA_REQUEST_EP, + USBD1_DATA_AVAILABLE_EP, + USBD1_INTERRUPT_REQUEST_EP +}; + +int isUsbActive(void){ + return SDU1.config->usbp->state == USB_ACTIVE; +} + +void myUSBinit(void){ + + /* + * Shell manager initialization. + */ + shellInit(); + + /* + * Initializes a serial-over-USB CDC driver. + */ + sduObjectInit(&SDU1); + sduStart(&SDU1, &serusbcfg); + + /* + * Activates the USB driver and then the USB bus pull-up on D+. + * Note, a delay is inserted in order to not have to disconnect the cable + * after a reset. + */ + usbDisconnectBus(serusbcfg.usbp); + chThdSleepMilliseconds(1000); + usbStart(serusbcfg.usbp, &usbcfg); + usbConnectBus(serusbcfg.usbp); +} diff --git a/myUSB.h b/myUSB.h new file mode 100644 index 000000000..aad165cf3 --- /dev/null +++ b/myUSB.h @@ -0,0 +1,8 @@ +#ifndef MYUSB_H_INCLUDED +#define MYUSB_H_INCLUDED +extern SerialUSBDriver SDU1; + +void myUSBinit(void); +int isUsbActive(void); + +#endif // MYUSB_H_INCLUDED diff --git a/packet.c b/packet.c new file mode 100644 index 000000000..e9fc613cf --- /dev/null +++ b/packet.c @@ -0,0 +1,143 @@ +/* + Copyright 2012-2014 Benjamin Vedder benjamin@vedder.se + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +/* + * packet.c + * + * Created on: 21 mar 2013 + * Author: benjamin + */ + +#include +#include "packet.h" +#include "crc.h" + +// Settings +#define RX_TIMEOUT 2 + +// Packet variables +static volatile unsigned char rx_state = 0; +static volatile unsigned char rx_timeout = 0; + +// Function pointers +static void(*send_func)(unsigned char *data, unsigned char len) = 0; +static void(*process_func)(unsigned char *data, unsigned char len) = 0; + +void packet_init(void (*s_func)(unsigned char *data, unsigned char len), + void (*p_func)(unsigned char *data, unsigned char len)) { + send_func = s_func; + process_func = p_func; +} + +void packet_send_packet(unsigned char *data, unsigned char len) { + static uint8_t data_buffer[261]; + int b_ind = 0; + + data_buffer[b_ind++] = 2; + data_buffer[b_ind++] = len; + + for(int i = 0;i < len;i++) { + data_buffer[b_ind++] = data[i]; + } + + unsigned short crc = crc16(data, len); + data_buffer[b_ind++] = (uint8_t)(crc >> 8); + data_buffer[b_ind++] = (uint8_t)(crc & 0xFF); + data_buffer[b_ind++] = 3; + + if (send_func) { + send_func(data_buffer, len + 5); + } +} + +void packet_timerfunc(void) { + if (rx_timeout) { + rx_timeout--; + } else { + rx_state = 0; + } +} + +void packet_process_byte(uint8_t rx_data) { + static unsigned char payload_length = 0; + static unsigned char rx_buffer[256]; + static unsigned char rx_data_ptr = 0; + static unsigned char crc_low = 0; + static unsigned char crc_high = 0; + + switch (rx_state) { + case 0: + if (rx_data == 2) { + rx_state++; + rx_timeout = RX_TIMEOUT + ; + rx_data_ptr = 0; + } else { + rx_state = 0; + } + break; + + case 1: + payload_length = rx_data; + rx_state++; + rx_timeout = RX_TIMEOUT + ; + break; + + case 2: + rx_buffer[rx_data_ptr++] = rx_data; + if (rx_data_ptr == payload_length) { + rx_state++; + } + rx_timeout = RX_TIMEOUT + ; + break; + + case 3: + crc_high = rx_data; + rx_state++; + rx_timeout = RX_TIMEOUT + ; + break; + + case 4: + crc_low = rx_data; + rx_state++; + rx_timeout = RX_TIMEOUT + ; + break; + + case 5: + if (rx_data == 3) { + if (crc16(rx_buffer, payload_length) + == ((unsigned short) crc_high << 8 + | (unsigned short) crc_low)) { + // Packet received! + if (process_func) { + process_func(rx_buffer, payload_length); + } + } + } + + rx_state = 0; + break; + + default: + rx_state = 0; + break; + } +} diff --git a/packet.h b/packet.h new file mode 100644 index 000000000..1718abea0 --- /dev/null +++ b/packet.h @@ -0,0 +1,20 @@ +/* + * packet.h + * + * Created on: 21 mar 2013 + * Author: benjamin + */ + +#ifndef PACKET_H_ +#define PACKET_H_ + +#include + +// Functions +void packet_init(void (*s_func)(unsigned char *data, unsigned char len), + void (*p_func)(unsigned char *data, unsigned char len)); +void packet_process_byte(uint8_t rx_data); +void packet_timerfunc(void); +void packet_send_packet(unsigned char *data, unsigned char len); + +#endif /* PACKET_H_ */ diff --git a/servo.c b/servo.c new file mode 100644 index 000000000..13ef27655 --- /dev/null +++ b/servo.c @@ -0,0 +1,541 @@ +/* + Copyright 2012-2014 Benjamin Vedder benjamin@vedder.se + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +/* + * servo.c + * + * Created on: 2009-apr-25 + * Author: Benjamin Vedder + * Driver for controlling servos + */ +#include "servo.h" +#include +#include + +#include "ch.h" +#include "hal.h" + +volatile SERVO servos[SERVOS_NUM]; + +#if USE_COMMANDS +volatile SERVO_CMD commands[SERVOS_NUM]; +volatile unsigned int cmd_counter = 0, cmd_time_to_run = 0; +volatile unsigned char cmd_repeat = 0; +#endif + +#if TEST_CYCLE_TIME + volatile unsigned int restart_cnt = 0; + volatile unsigned int interrupt_cnt = 0; +#endif + +static SERVO sorted_servos[SERVOS_NUM]; +static volatile signed char int_index; +static volatile signed char servo_int_index; +static volatile signed char masks_ports_index; +static volatile unsigned char driver_active; +static volatile unsigned int delays[SERVOS_NUM + 1]; +static volatile unsigned char same_pos[SERVOS_NUM + 1]; +static volatile unsigned char same_bits[SERVOS_NUM + 1]; +static volatile unsigned int masks[SERVOS_NUM]; +static GPIO_TypeDef *ports[SERVOS_NUM]; +static volatile unsigned char length; +static size_t servo_struct_size = sizeof(SERVO); +static size_t sorted_servos_len = sizeof(sorted_servos) / sizeof(SERVO); + +static Thread *servo_tp; +static WORKING_AREA(servo_thread_wa, 512); +static msg_t servo_thread(void *arg); + +/* + * Private functions + */ +static void servo_get_copy(SERVO *a); +static void servo_init_timer(void); +static void servo_start_pulse(void); + +/* + * HW-specific START + */ +#define SERVO_CNT TIM7->CNT +#define SERVO_CNT_TOP TIM7->ARR + +static void servo_init_timer(void) { + TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; + uint16_t PrescalerValue = 0; + NVIC_InitTypeDef NVIC_InitStructure; + + // ------------- Timer7 ------------- // + // Compute the prescaler value + // TIM7 clock enable + RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM7, ENABLE); + + PrescalerValue = (uint16_t) ((168e6L / 2) / SERVO_CNT_SPEED) - 1; + + // Time base configuration + TIM_TimeBaseStructure.TIM_Period = 0xFFFF; + TIM_TimeBaseStructure.TIM_Prescaler = 0; + TIM_TimeBaseStructure.TIM_ClockDivision = 0; + TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; + TIM_TimeBaseInit(TIM7, &TIM_TimeBaseStructure); + + // Prescaler configuration + TIM_PrescalerConfig(TIM7, PrescalerValue, TIM_PSCReloadMode_Immediate); + + // Disable ARR buffering + TIM_ARRPreloadConfig(TIM7, DISABLE); + + // Interrupt generation + TIM_ITConfig(TIM7, TIM_IT_Update, ENABLE); + + // TIM6 enable counter + TIM_Cmd(TIM7, ENABLE); + + // NVIC + NVIC_InitStructure.NVIC_IRQChannel = TIM7_IRQn; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2; + NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2; + NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; + NVIC_Init(&NVIC_InitStructure); +} + +void servo_init(void) { + // Set up GPIO ports + RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE); + RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE); + + // Set up servo structures + servos[0].gpio = GPIOB; + servos[0].pin = 5; + servos[0].offset = 0; + servos[0].pos = 0; + + servos[1].gpio = GPIOB; + servos[1].pin = 4; + servos[1].offset = 0; + servos[1].pos = 0; + + for (int i = 0; i < SERVOS_NUM; i++) { + palSetPadMode(servos[i].gpio, servos[i].pin, PAL_MODE_OUTPUT_PUSHPULL | + PAL_STM32_OSPEED_HIGHEST); + palClearPad(servos[i].gpio, servos[i].pin); + } + + int_index = 0; + length = 0; + +#if USE_COMMANDS + unsigned char i; + for (i = 0; i < SERVOS_NUM; i++) { + commands[i].active = 0; + commands[i].last = 0; + } +#endif + + chThdCreateStatic(servo_thread_wa, sizeof(servo_thread_wa), NORMALPRIO, servo_thread, NULL); + + servo_init_timer(); + driver_active = 1; +} + +static void servo_start_pulse(void) { + unsigned char i; + for (i = 0; i < SERVOS_NUM; i++) { + palSetPad(servos[i].gpio, servos[i].pin); + } +} + +/* + * Stop servo driver + */ +void servo_stop_driver(void) { + // Disable clock + RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, DISABLE); + + TIM_Cmd(TIM7, DISABLE); + RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM7, DISABLE); + + driver_active = 0; +} +/* + * HW-specific END + */ + +void servo_irq(void) { + if (int_index < 0) { + // Start pulse + servo_start_pulse(); + SERVO_CNT_TOP = SERVO_START_OFFSET + delays[0]; + int_index = 0; + return; + } + + if (int_index == length) { + // Start Cooldown + SERVO_CNT_TOP = SERVO_COOLDOWN_FACTOR; + chSysLockFromIsr(); + chEvtSignalI(servo_tp, (eventmask_t) 1); + chSysUnlockFromIsr(); + return; + } + + // End pulse + while (same_pos[int_index]--) { + palClearPort(ports[masks_ports_index], masks[masks_ports_index]); + servo_int_index += same_bits[masks_ports_index]; + masks_ports_index++; + } + + int_index++; + + SERVO_CNT_TOP = delays[int_index]; + + +#if TEST_CYCLE_TIME + if (int_index < length && int_index > 0) { + interrupt_cnt = SERVO_CNT; + } +#endif +} + +static int servo_cmp_by_pos(const void *a, const void *b) { + SERVO *ia = (SERVO *) a; + SERVO *ib = (SERVO *) b; + + if (ACTUAL_POS_PTR(ia) == ACTUAL_POS_PTR(ib)) { + return 0; + } else if (ACTUAL_POS_PTR(ia) < ACTUAL_POS_PTR(ib)) { + return -1; + } else { + return 1; + } +} + +static msg_t servo_thread(void *arg) { + (void)arg; + + chRegSetThreadName("Servo"); + + servo_tp = chThdSelf(); + + for(;;) { + chEvtWaitAny((eventmask_t) 1); + +#if TEST_CYCLE_TIME + unsigned int pwm_start = SERVO_CNT; +#endif + + servo_int_index = 0; + masks_ports_index = 0; + + servo_get_copy(sorted_servos); + qsort(sorted_servos, sorted_servos_len, servo_struct_size, servo_cmp_by_pos); + + delays[0] = ACTUAL_POS(sorted_servos[0]) * SERVO_CPU_FACTOR; + masks[0] = _BV(sorted_servos[0].pin); + ports[0] = sorted_servos[0].gpio; + + unsigned short i = 0, j = 1, k = 0; + + for (i = 0; i <= SERVOS_NUM; i++) { + same_pos[i] = 1; + same_bits[i] = 1; + } + + i = 0; + + for(;;) { + while (ACTUAL_POS(sorted_servos[j]) == ACTUAL_POS(sorted_servos[j - 1]) && j < SERVOS_NUM) { + if (sorted_servos[j].gpio == sorted_servos[j - 1].gpio) { + masks[k] |= _BV(sorted_servos[j].pin); + same_bits[k]++; + } else { + same_pos[i]++; + ports[++k] = sorted_servos[j].gpio; + masks[k] = _BV(sorted_servos[j].pin); + } + + j++; + } + + i++; + + if (j < SERVOS_NUM) { + delays[i] = (ACTUAL_POS(sorted_servos[j]) - ACTUAL_POS(sorted_servos[j - 1])) * SERVO_CPU_FACTOR; + ports[++k] = sorted_servos[j].gpio; + masks[k] = _BV(sorted_servos[j].pin); + j++; + } else { + break; + } + } + + /* | + * Add a few extra cycles here to make sure the interrupt is able to finish. \|/ + */ + delays[i] = (ACTUAL_POS(sorted_servos[SERVOS_NUM - 1])) * SERVO_CPU_FACTOR + 120; + length = i; + +#if USE_COMMANDS + /* + * Run the commands for the servos here. + */ + for (i = 0;i < SERVOS_NUM;i++) { + if (commands[i].active) { + signed short p = commands[i].pos, ps = servos[i].pos; + + if (p == ps) { + commands[i].active = 0; + commands[i].last = 0; + continue; + } + + commands[i].last += commands[i].speed; + signed short delta = commands[i].last >> 5; + + if (p < ps) { + servos[i].pos -= delta; + if (delta > (ps - p)) { + servos[i].pos = p; + } + } else { + servos[i].pos += delta; + if (delta > (p - ps)) { + servos[i].pos = p; + } + } + + //if (delta > 0) { + // commands[i].last = 0; + //} + commands[i].last -= delta << 5; + } + } + + cmd_counter++; + + if (cmd_counter == cmd_time_to_run && cmd_seq_running) { + signed short tmp1 = 0, tmp2 = 0, tmp3 = 0, tmp4 = 0; + + while (cmd_seq_running) { + switch (cmd_seq[cmd_ptr++]) { + case CMD_MOVE_SERVO: + tmp1 = cmd_seq[cmd_ptr++]; + tmp2 = cmd_seq[cmd_ptr++]; + tmp3 = cmd_seq[cmd_ptr++]; + + if (tmp3 == 0) { + commands[tmp1].active = 0; + servos[tmp1].pos = tmp2; + } else { + commands[tmp1].speed = tmp3; + commands[tmp1].pos = tmp2; + commands[tmp1].active = 1; + } + break; + + case CMD_MOVE_SERVO_REL: + tmp1 = cmd_seq[cmd_ptr++]; + tmp2 = cmd_seq[cmd_ptr++]; + tmp3 = cmd_seq[cmd_ptr++]; + + if (tmp3 == 0) { + commands[tmp1].active = 0; + servos[tmp1].pos += tmp2; + } else { + commands[tmp1].speed = tmp3; + commands[tmp1].pos += tmp2; + commands[tmp1].active = 1; + } + break; + + case CMD_MOVE_MULTIPLE_SERVOS: + tmp4 = cmd_seq[cmd_ptr++]; + tmp3 = cmd_seq[cmd_ptr++]; + + for (i = 0; i < tmp4; i++) { + tmp1 = cmd_seq[cmd_ptr++]; + tmp2 = cmd_seq[cmd_ptr++]; + + servo_move_within_time(tmp1, tmp2, tmp3); + } + break; + + case CMD_CENTER_ALL: + tmp4 = cmd_seq[cmd_ptr++]; + for (i = 0;i < SERVOS_NUM;i++) { + servo_move_within_time(i, 0, tmp4); + } + break; + + case CMD_WAIT: + cmd_counter = 0; + cmd_time_to_run = cmd_seq[cmd_ptr++] * CMD_WAIT_FACTOR; + goto end_cmds; + break; + + case CMD_WAIT_SERVO: + tmp1 = cmd_seq[cmd_ptr++]; + if (commands[tmp1].active) { + cmd_ptr -= 2; + cmd_counter = 0; + cmd_time_to_run = 1; + goto end_cmds; + } + break; + + case CMD_WAIT_ALL_SERVOS: + for (tmp1 = 0; tmp1 < SERVOS_NUM; tmp1++) { + if (commands[tmp1].active) { + cmd_ptr--; + cmd_counter = 0; + cmd_time_to_run = 1; + goto end_cmds; + } + } + break; + + case CMD_STOP_DRIVER: + servo_stop_cmds(); + servo_stop_driver(); + goto end_cmds; + break; + + case CMD_STOP_CMDS: + servo_stop_cmds(); + goto end_cmds; + break; + + case CMD_RESTART: + cmd_ptr = 0; + break; + + case CMD_REPEAT: + tmp1 = cmd_seq[cmd_ptr++]; + cmd_repeat++; + if (cmd_repeat < tmp1) { + cmd_ptr = 0; + } + break; + + default: + servo_stop_cmds(); + goto end_cmds; + break; + } + + } + + end_cmds: ; + } + +#endif + +#if TEST_CYCLE_TIME + restart_cnt = SERVO_CNT - pwm_start; +#endif + + int_index = -1; + } + + return 0; +} + +#if USE_COMMANDS +volatile signed char cmd_seq_running = 0; +volatile unsigned int cmd_ptr = 0; +volatile const signed short *cmd_seq; + +void servo_move(unsigned char servo, signed short position, unsigned char speed) { + if (speed == 0) { + servos[servo].pos = position; + commands[servo].active = 0; + return; + } + + commands[servo].speed = speed; + commands[servo].pos = position; + commands[servo].active = 1; +} + +void servo_run_cmds(const signed short *cmds) { + cmd_ptr = 0; + cmd_seq = cmds; + cmd_seq_running = 1; + cmd_counter = 0; + cmd_time_to_run = 1; + cmd_repeat = 0; +} + +void servo_stop_cmds(void) { + cmd_seq_running = 0; +} + +void servo_reset_pos(unsigned char speed) { + int i; + for(i = 0;i < SERVOS_NUM;i++) { + servo_move(i, 0, speed); + } +} + +void servo_wait_for_cmds() { + while (cmd_seq_running) {} +} + +void servo_move_within_time(unsigned char servo, signed short pos, unsigned short time_ms) { + if (time_ms == 0) { + servos[servo].pos = pos; + commands[servo].active = 0; + return; + } + + unsigned short diff = abs(pos - servos[servo].pos); + + commands[servo].speed = ((diff * SERVO_PERIOD_TIME_MS) << 1) / (time_ms >> 4); + commands[servo].pos = pos; + commands[servo].active = 1; +} + +void servo_move_within_time_multiple(unsigned short time_ms, unsigned short num, ...) { + va_list arguments; + unsigned char x = 0; + + va_start(arguments, 2 * num); + + for (x = 0; x < num; x++) { + unsigned short servo = va_arg(arguments, unsigned int); + unsigned short pos = va_arg(arguments, unsigned int); + + servo_move_within_time(servo, pos, time_ms); + } + va_end(arguments); +} +#endif + +unsigned char servo_driver_is_active() { + return driver_active; +} + +static void servo_get_copy(SERVO *a) { + int i; + + for(i = 0;i < SERVOS_NUM;i++) { + a[i].gpio = servos[i].gpio; + a[i].pin = servos[i].pin; + a[i].pos = servos[i].pos; + a[i].offset = servos[i].offset; + } +} diff --git a/servo.h b/servo.h new file mode 100644 index 000000000..81a37dd53 --- /dev/null +++ b/servo.h @@ -0,0 +1,260 @@ +/* + Copyright 2009-2012 Benjamin Vedder benjamin@vedder.se + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +/* + * servo.h + * + * Created on: 2009-apr-25 + * Author: Benjamin + * + * + * Changelog: + * 2011-12-08 + * - Renamed functions + * - Fixed pulse issue when pulses are close together + * + * 2011-07-17 + * - Changed some variables to volatile. + * - Added another command. + * + * 2010-11-13 + * - Calculate the servo timings independent of F_CPU. + * - Cleanup + * + */ + +#ifndef SERVO_H_ +#define SERVO_H_ + +#include "ch.h" +#include "hal.h" + +#include "stm32f4xx_conf.h" + +#ifndef _BV +#define _BV(bit) (1 << (bit)) +#endif + +#ifndef MIN +#define MIN(a,b) (((a)<(b))?(a):(b)) +#endif + +#ifndef MAX +#define MAX(a,b) (((a)>(b))?(a):(b)) +#endif + +// Change these parameters +#define SERVOS_NUM 2 // Number of servos to use + +// Servo timer speed in HZ +#define SERVO_CNT_SPEED 1000000L + +/* + * Servo timing (in uS) + * + * The default signal to drive servos looks like this: + * + * ----1000us----|----1000us----|---20000us---- + * _____________________________ + * |______________|_____...______ + * + * -S_STARTPULSE-|--S_PULSELEN--|--S_COOLDOWN-- + * + * And the default parameters are the following + * #define S_STARTPULSE 1000L + * #define S_PULSELEN 1000L + * #define S_COOLDOWN 20000L + * + * You can experiment with these to make your servo move further. + * For mg995 these can be WAY out of spec. + * + * Note that S_PULSELEN is not accurate at all for low F_CPU. However, + * it will be rounded up to the nearest possible value (hence the strange + * calculation below) + * + */ +#define S_STARTPULSE 1000L +#define S_PULSELEN 1000L +#define S_COOLDOWN 20000L + +/* + * Dynamic servo parameters + * Calculated from F_CPU + */ +#define SERVO_START_OFFSET (SERVO_CNT_SPEED / (1000000L / S_STARTPULSE)) +#define SERVO_CPU_FACTOR ((SERVO_CNT_SPEED + ((1000000L / S_PULSELEN) * 256L) - 1L) / ((1000000L / S_PULSELEN) * 256L)) // Round up +#define SERVO_COOLDOWN_FACTOR (SERVO_CNT_SPEED / (1000000L / S_COOLDOWN)) + +/* + * Compile with commands to mode servos with a specified speed + * to s specified position interrupt driven. Enabling this will + * use some extra ram and a few bytes of flash. + */ +#define USE_COMMANDS 0 + +/* + * Calculate how many clock cycles it takes to generate PWM. + */ +#define TEST_CYCLE_TIME 0 + +#if TEST_CYCLE_TIME +extern volatile unsigned int restart_cnt; +extern volatile unsigned int interrupt_cnt; + +#define get_restart_cycles() (restart_cnt) +#define get_interrupt_cycles() (interrupt_cnt) +#endif + +// Servo macros +#define ACTUAL_POS(servo) (MAX(MIN(servo.pos + (signed short)servo.offset, 255), 0)) +#define ACTUAL_POS_PTR(servo) (MAX(MIN(servo->pos + (signed short)servo->offset, 255), 0)) +#define CMD_MS_TO_VAL(ms) ((ms) / (((S_STARTPULSE + S_PULSELEN + S_COOLDOWN) * CMD_WAIT_FACTOR) / 1000)) +#define SERVO_PERIOD_TIME_MS ((S_STARTPULSE + S_PULSELEN + S_COOLDOWN) / 1000) + +// Some servo speed defines +// TODO + +typedef struct { + volatile signed short pos; + volatile unsigned char offset; + GPIO_TypeDef* gpio; + volatile unsigned int pin; +} SERVO; + +#if USE_COMMANDS +typedef struct { + volatile signed char active; + volatile signed short pos; + volatile signed short speed; + volatile signed short last; +} SERVO_CMD; + +extern volatile signed char cmd_seq_running; +extern volatile unsigned int cmd_ptr; +extern volatile const signed short *cmd_seq; + +/* + * The number of servo cycles to wait for each time unit in the wait command. + * + * The wait time can be calculated with: + * (S_STARTPULSE + S_PULSELEN + S_COOLDOWN) * CMD_WAIT_FACTOR + * + */ +#define CMD_WAIT_FACTOR 1 + +/* + * Servo commands. + */ + +/* + * Move servo to given position with given speed. + * + * Param 1: Servo. + * Param 2: Position. + * Param 3: Speed. 0 for max speed. + */ +#define CMD_MOVE_SERVO 0 + +/* + * Move servo to given relative position with given speed. + * + * Param 1: Servo. + * Param 2: Relative position. + * Param 3: Speed. 0 for max speed. + */ +#define CMD_MOVE_SERVO_REL 1 + +/* + * Move multiple servos such that they arrive at the same time + * + * Param 1: Number of servos. + * Param 2: Time for movement in milliseconds + * Param 3: Servo 1 index + * Param 4: Servo 1 pos + * Param 5: Servo 2 index + * Param 6: Servo 2 pos + * ... and so on + */ +#define CMD_MOVE_MULTIPLE_SERVOS 2 + +/* + * Move all servos to center position + * + * Param 1: Time for movement in milliseconds + */ +#define CMD_CENTER_ALL 3 + +/* + * Wait for a while. See configuration for more info. + * + * Param 1: Time to wait + */ +#define CMD_WAIT 4 + +/* + * Wait for servo to be ready. + * + * Param 1: Servo to wait for. + */ +#define CMD_WAIT_SERVO 5 + +/* + * Wait for all servo commands to get ready. + */ +#define CMD_WAIT_ALL_SERVOS 6 + +/* + * Stop servo driver. + */ +#define CMD_STOP_DRIVER 7 + +/* + * End of command. + */ +#define CMD_STOP_CMDS 8 + +/* + * Restart this command sequence + */ +#define CMD_RESTART 9 + +/* + * Repeat command sequence a number of times + * + * Param 1: Number of times to repeat commands. + */ +#define CMD_REPEAT 10 +#endif + +extern volatile SERVO servos[SERVOS_NUM]; + +void servo_init(void); +void servo_stop_driver(void); +unsigned char servo_driver_is_active(void); +void servo_irq(void); + +#if USE_COMMANDS +void servo_move(unsigned char servo, signed short position, unsigned char speed); +void servo_run_cmds(const signed short *cmds); +void servo_stop_cmds(void); +void servo_reset_pos(unsigned char speed); +void servo_wait_for_cmds(void); +void servo_move_within_time(unsigned char servo, signed short pos, unsigned short time_ms); +void servo_move_within_time_multiple(unsigned short time_ms, unsigned short num, ...); +#endif + +#endif /* SERVO_H_ */ diff --git a/servo_dec.c b/servo_dec.c new file mode 100644 index 000000000..6613e0531 --- /dev/null +++ b/servo_dec.c @@ -0,0 +1,176 @@ +/* + Copyright 2012-2014 Benjamin Vedder benjamin@vedder.se + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +/* + * servo_dec.c + * + * Created on: 20 jan 2013 + * Author: benjamin + */ + +#include "servo_dec.h" +#include "stm32f4xx_conf.h" +#include "ch.h" +#include "hal.h" + +/* + * Settings + */ +#define SERVO_NUM 3 +#define TIMER_FREQ 1000000 +#define INTERRUPT_TRESHOLD 4 + +static volatile uint32_t interrupt_time = 0; +static volatile int8_t servo_pos[SERVO_NUM]; +static volatile uint32_t time_since_update; + +void servodec_init(void) { + // Initialize variables + time_since_update = 0; + interrupt_time = 0; + + NVIC_InitTypeDef NVIC_InitStructure; + EXTI_InitTypeDef EXTI_InitStructure; + TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; + uint16_t PrescalerValue = 0; + + // ------------- EXTI -------------- // + // Clocks + RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE); + RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE); + RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE); + + // Configure GPIO input floating + palSetPadMode(GPIOA, 13, PAL_MODE_INPUT_PULLDOWN); + palSetPadMode(GPIOA, 14, PAL_MODE_INPUT_PULLDOWN); + palSetPadMode(GPIOB, 3, PAL_MODE_INPUT_PULLDOWN); + + // Connect EXTI Lines + SYSCFG_EXTILineConfig(EXTI_PortSourceGPIOA, EXTI_PinSource13); + SYSCFG_EXTILineConfig(EXTI_PortSourceGPIOA, EXTI_PinSource14); + SYSCFG_EXTILineConfig(EXTI_PortSourceGPIOB, EXTI_PinSource3); + + // Configure EXTI Lines + EXTI_InitStructure.EXTI_Line = EXTI_Line3 | EXTI_Line13 | EXTI_Line14; + EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt; + EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Rising_Falling; + EXTI_InitStructure.EXTI_LineCmd = ENABLE; + EXTI_Init(&EXTI_InitStructure); + + // Enable and set EXTI Line Interrupts + NVIC_InitStructure.NVIC_IRQChannel = EXTI3_IRQn; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2; + NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2; + NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; + NVIC_Init(&NVIC_InitStructure); + + NVIC_InitStructure.NVIC_IRQChannel = EXTI15_10_IRQn; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2; + NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2; + NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; + NVIC_Init(&NVIC_InitStructure); + + // ------------- Timer3 ------------- // + /* Compute the prescaler value */ + /* TIM3 clock enable */ + RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE); + + PrescalerValue = (uint16_t) ((168000000 / 2) / TIMER_FREQ) - 1; + + /* Time base configuration */ + TIM_TimeBaseStructure.TIM_Period = 65535; + TIM_TimeBaseStructure.TIM_Prescaler = 0; + TIM_TimeBaseStructure.TIM_ClockDivision = 0; + TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; + + TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure); + + /* Prescaler configuration */ + TIM_PrescalerConfig(TIM3, PrescalerValue, TIM_PSCReloadMode_Immediate); + + /* TIM3 enable counter */ + TIM_Cmd(TIM3, ENABLE); +} + +void servodec_timerfunc(void) { + interrupt_time++; + time_since_update++; +} + +void servodec_int_handler(void) { + static int curr_index = 0; + + // Long time since last interrupt means that a new cycle has started + if (interrupt_time >= INTERRUPT_TRESHOLD) { + curr_index = 0; + interrupt_time = 0; + TIM3->CNT = 0; + return; + } + + if (curr_index < SERVO_NUM) { + // Use floating point because we can :) + float time_ms = (float)(TIM3->CNT); + time_ms = (time_ms * 1000.0) / (float)TIMER_FREQ; + + if (time_ms < 0.4) { + return; + } + + TIM3->CNT = 0; + + // Check if pulse is within valid range + if (time_ms > 0.8 && time_ms < 2.2) { + + // Truncate (just in case) + if (time_ms > 2.0) { + time_ms = 2.0; + } + + if (time_ms < 1.0) { + time_ms = 1.0; + } + + // Update position + servo_pos[curr_index] = (int8_t)((time_ms - 1.5) * 255.0); + } + } + + curr_index++; + + if (curr_index == SERVO_NUM) { + time_since_update = 0; + } + + interrupt_time = 0; +} + +int8_t servodec_get_servo(int servo_num) { + if (servo_num < SERVO_NUM) { + return servo_pos[servo_num]; + } else { + return 0; + } +} + +/* + * Get the amount of milliseconds that has passed since + * the last time servo positions were received + */ +uint32_t servodec_get_time_since_update(void) { + return time_since_update; +} diff --git a/servo_dec.h b/servo_dec.h new file mode 100644 index 000000000..9d99b596f --- /dev/null +++ b/servo_dec.h @@ -0,0 +1,25 @@ +/* + * servo_dec.h + * + * Created on: 20 jan 2013 + * Author: benjamin + */ + +#ifndef SERVO_DEC_H_ +#define SERVO_DEC_H_ + +#include + +// Servo function indexes +#define SERVODEC_IND_STEERING 0 +#define SERVODEC_IND_THROTTLE 1 +#define SERVODEC_IND_AUX 2 + +// Functions +void servodec_init(void); +void servodec_timerfunc(void); +void servodec_int_handler(void); +int8_t servodec_get_servo(int servo_num); +uint32_t servodec_get_time_since_update(void); + +#endif /* SERVO_DEC_H_ */ diff --git a/stm32-bv_openocd.cfg b/stm32-bv_openocd.cfg new file mode 100644 index 000000000..eac4730a7 --- /dev/null +++ b/stm32-bv_openocd.cfg @@ -0,0 +1,6 @@ +source [find interface/stlink-v2.cfg] +source [find target/stm32f4x_stlink.cfg] + +# use hardware reset, connect under reset +reset_config srst_only srst_nogate + diff --git a/stm32f4xx_conf.h b/stm32f4xx_conf.h new file mode 100644 index 000000000..52b98afc0 --- /dev/null +++ b/stm32f4xx_conf.h @@ -0,0 +1,95 @@ +/** + ****************************************************************************** + * @file stm32f4xx_conf.h + * @author MCD Application Team + * @version V1.0.0 + * @date 19-September-2011 + * @brief Library configuration file. + ****************************************************************************** + * @attention + * + * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS + * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE + * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY + * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING + * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE + * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. + * + *

© COPYRIGHT 2011 STMicroelectronics

+ ****************************************************************************** + */ + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __STM32F4xx_CONF_H +#define __STM32F4xx_CONF_H + +#if defined (HSE_VALUE) +/* Redefine the HSE value; it's equal to 8 MHz on the STM32F4-DISCOVERY Kit */ + #undef HSE_VALUE + #define HSE_VALUE ((uint32_t)8000000) +#endif /* HSE_VALUE */ + +/* Includes ------------------------------------------------------------------*/ +/* Uncomment the line below to enable peripheral header file inclusion */ +#include "stm32f4xx_adc.h" +#include "stm32f4xx_can.h" +#include "stm32f4xx_crc.h" +#include "stm32f4xx_cryp.h" +#include "stm32f4xx_dac.h" +#include "stm32f4xx_dbgmcu.h" +#include "stm32f4xx_dcmi.h" +#include "stm32f4xx_dma.h" +#include "stm32f4xx_exti.h" +#include "stm32f4xx_flash.h" +#include "stm32f4xx_fsmc.h" +#include "stm32f4xx_hash.h" +//#include "stm32f4xx_gpio.h" +#include "stm32f4_gpio_af.h" +#include "stm32f4xx_i2c.h" +#include "stm32f4xx_iwdg.h" +#include "stm32f4xx_pwr.h" +#include "stm32f4xx_rcc.h" +#include "stm32f4xx_rng.h" +#include "stm32f4xx_rtc.h" +#include "stm32f4xx_sdio.h" +#include "stm32f4xx_spi.h" +#include "stm32f4xx_syscfg.h" +#include "stm32f4xx_tim.h" +#include "stm32f4xx_usart.h" +#include "stm32f4xx_wwdg.h" +#include "misc.h" /* High level functions for NVIC and SysTick (add-on to CMSIS functions) */ + +/* Exported types ------------------------------------------------------------*/ +/* Exported constants --------------------------------------------------------*/ + +/* If an external clock source is used, then the value of the following define + should be set to the value of the external clock source, else, if no external + clock is used, keep this define commented */ +/*#define I2S_EXTERNAL_CLOCK_VAL 12288000 */ /* Value of the external clock in Hz */ + + +/* Uncomment the line below to expanse the "assert_param" macro in the + Standard Peripheral Library drivers code */ +/* #define USE_FULL_ASSERT 1 */ + +/* Exported macro ------------------------------------------------------------*/ +#ifdef USE_FULL_ASSERT + +/** + * @brief The assert_param macro is used for function's parameters check. + * @param expr: If expr is false, it calls assert_failed function + * which reports the name of the source file and the source + * line number of the call that failed. + * If expr is true, it returns no value. + * @retval None + */ + #define assert_param(expr) ((expr) ? (void)0 : assert_failed((uint8_t *)__FILE__, __LINE__)) +/* Exported functions ------------------------------------------------------- */ + void assert_failed(uint8_t* file, uint32_t line); +#else + #define assert_param(expr) ((void)0) +#endif /* USE_FULL_ASSERT */ + +#endif /* __STM32F4xx_CONF_H */ + +/******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/ diff --git a/terminal.c b/terminal.c new file mode 100644 index 000000000..fa2d41f0a --- /dev/null +++ b/terminal.c @@ -0,0 +1,119 @@ +/* + Copyright 2012-2014 Benjamin Vedder benjamin@vedder.se + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +/* + * terminal.c + * + * Created on: 26 dec 2013 + * Author: benjamin + */ + +#include "ch.h" +#include "hal.h" +#include "terminal.h" +#include "comm.h" +#include "mcpwm.h" +#include "main.h" + +#include +#include + +void terminal_process_string(char *str) { + enum { kMaxArgs = 64 }; + int argc = 0; + char *argv[kMaxArgs]; + static char buffer[256]; + + char *p2 = strtok(str, " "); + while (p2 && argc < kMaxArgs) { + argv[argc++] = p2; + p2 = strtok(0, " "); + } + + if (argc == 0) { + comm_print("No command received\n"); + return; + } + + if (strcmp(argv[0], "ping") == 0) { + comm_print("pong\n"); + } else if (strcmp(argv[0], "stop") == 0) { + mcpwm_use_pid(0); + mcpwm_set_duty(0); + comm_print("Motor stopped\n"); + } else if (strcmp(argv[0], "last_adc_duration") == 0) { + sprintf(buffer, "Latest ADC duration: %.4f ms", (double)(mcpwm_get_last_adc_isr_duration() * 1000.0)); + comm_print(buffer); + sprintf(buffer, "Latest injected ADC duration: %.4f ms", (double)(mcpwm_get_last_inj_adc_isr_duration() * 1000.0)); + comm_print(buffer); + sprintf(buffer, "Latest main ADC duration: %.4f ms\n", (double)(main_get_last_adc_isr_duration() * 1000.0)); + comm_print(buffer); + } else if (strcmp(argv[0], "kv") == 0) { + sprintf(buffer, "Calculated KV: %.2f rpm/volt\n", (double)mcpwm_get_kv_filtered()); + comm_print(buffer); + } else if (strcmp(argv[0], "mem") == 0) { + size_t n, size; + n = chHeapStatus(NULL, &size); + sprintf(buffer, "core free memory : %u bytes", chCoreStatus()); + comm_print(buffer); + sprintf(buffer, "heap fragments : %u", n); + comm_print(buffer); + sprintf(buffer, "heap free total : %u bytes\n", size); + comm_print(buffer); + } else if (strcmp(argv[0], "threads") == 0) { + Thread *tp; + static const char *states[] = {THD_STATE_NAMES}; + comm_print(" addr stack prio refs state name time "); + comm_print("-------------------------------------------------------------"); + tp = chRegFirstThread(); + do { + sprintf(buffer, "%.8lx %.8lx %4lu %4lu %9s %14s %lu", + (uint32_t)tp, (uint32_t)tp->p_ctx.r13, + (uint32_t)tp->p_prio, (uint32_t)(tp->p_refs - 1), + states[tp->p_state], tp->p_name, (uint32_t)tp->p_time); + comm_print(buffer); + tp = chRegNextThread(tp); + } while (tp != NULL); + comm_print(""); + } else if (strcmp(argv[0], "help") == 0) { + comm_print("Valid commands are:"); + comm_print("help"); + comm_print(" Show this help"); + + comm_print("ping"); + comm_print(" Print pong here to see if the reply works"); + + comm_print("stop"); + comm_print(" Stop the motor"); + + comm_print("last_adc_duration"); + comm_print(" The time the latest ADC interrupt consumed"); + + comm_print("kv"); + comm_print(" The calculated kv of the motor"); + + comm_print("mem"); + comm_print(" Show memory usage"); + + comm_print("threads"); + comm_print(" List all threads\n"); + } else { + sprintf(buffer, "Invalid command: %s\n" + "type help to list all available commands\n", argv[0]); + comm_print(buffer); + } +} diff --git a/terminal.h b/terminal.h new file mode 100644 index 000000000..e5edcf79f --- /dev/null +++ b/terminal.h @@ -0,0 +1,14 @@ +/* + * terminal.h + * + * Created on: 26 dec 2013 + * Author: benjamin + */ + +#ifndef TERMINAL_H_ +#define TERMINAL_H_ + +// Functions +void terminal_process_string(char *str); + +#endif /* TERMINAL_H_ */ diff --git a/usbdescriptor.h b/usbdescriptor.h new file mode 100644 index 000000000..61f788851 --- /dev/null +++ b/usbdescriptor.h @@ -0,0 +1,177 @@ +#ifndef USBDESCRIPTOR_H_INCLUDED +#define USBDESCRIPTOR_H_INCLUDED + +/* +* Endpoints to be used for USBD1. +*/ +#define USBD1_DATA_REQUEST_EP 1 +#define USBD1_DATA_AVAILABLE_EP 1 +#define USBD1_INTERRUPT_REQUEST_EP 2 + +/* +* USB Device Descriptor. +*/ +static const uint8_t vcom_device_descriptor_data[18] = { + USB_DESC_DEVICE (0x0110, /* bcdUSB (1.1). */ + 0x02, /* bDeviceClass (CDC). */ + 0x00, /* bDeviceSubClass. */ + 0x00, /* bDeviceProtocol. */ + 0x40, /* bMaxPacketSize. */ + 0x0483, /* idVendor (ST). */ + 0x5740, /* idProduct. */ + 0x0200, /* bcdDevice. */ + 1, /* iManufacturer. */ + 2, /* iProduct. */ + 3, /* iSerialNumber. */ + 1) /* bNumConfigurations. */ +}; + +/* +* Device Descriptor wrapper. +*/ +static const USBDescriptor vcom_device_descriptor = { + sizeof vcom_device_descriptor_data, + vcom_device_descriptor_data +}; + +/* Configuration Descriptor tree for a CDC.*/ +static const uint8_t vcom_configuration_descriptor_data[67] = { + /* Configuration Descriptor.*/ + USB_DESC_CONFIGURATION(67, /* wTotalLength. */ + 0x02, /* bNumInterfaces. */ + 0x01, /* bConfigurationValue. */ + 0, /* iConfiguration. */ + 0xC0, /* bmAttributes (self powered). */ + 50), /* bMaxPower (100mA). */ + /* Interface Descriptor.*/ + USB_DESC_INTERFACE (0x00, /* bInterfaceNumber. */ + 0x00, /* bAlternateSetting. */ + 0x01, /* bNumEndpoints. */ + 0x02, /* bInterfaceClass (Communications +Interface Class, CDC section +4.2). */ + 0x02, /* bInterfaceSubClass (Abstract +Control Model, CDC section 4.3). */ + 0x01, /* bInterfaceProtocol (AT commands, +CDC section 4.4). */ + 0), /* iInterface. */ + /* Header Functional Descriptor (CDC section 5.2.3).*/ + USB_DESC_BYTE (5), /* bLength. */ + USB_DESC_BYTE (0x24), /* bDescriptorType (CS_INTERFACE). */ + USB_DESC_BYTE (0x00), /* bDescriptorSubtype (Header +Functional Descriptor. */ + USB_DESC_BCD (0x0110), /* bcdCDC. */ + /* Call Management Functional Descriptor. */ + USB_DESC_BYTE (5), /* bFunctionLength. */ + USB_DESC_BYTE (0x24), /* bDescriptorType (CS_INTERFACE). */ + USB_DESC_BYTE (0x01), /* bDescriptorSubtype (Call Management +Functional Descriptor). */ + USB_DESC_BYTE (0x00), /* bmCapabilities (D0+D1). */ + USB_DESC_BYTE (0x01), /* bDataInterface. */ + /* ACM Functional Descriptor.*/ + USB_DESC_BYTE (4), /* bFunctionLength. */ + USB_DESC_BYTE (0x24), /* bDescriptorType (CS_INTERFACE). */ + USB_DESC_BYTE (0x02), /* bDescriptorSubtype (Abstract +Control Management Descriptor). */ + USB_DESC_BYTE (0x02), /* bmCapabilities. */ + /* Union Functional Descriptor.*/ + USB_DESC_BYTE (5), /* bFunctionLength. */ + USB_DESC_BYTE (0x24), /* bDescriptorType (CS_INTERFACE). */ + USB_DESC_BYTE (0x06), /* bDescriptorSubtype (Union +Functional Descriptor). */ + USB_DESC_BYTE (0x00), /* bMasterInterface (Communication +Class Interface). */ + USB_DESC_BYTE (0x01), /* bSlaveInterface0 (Data Class +Interface). */ + /* Endpoint 2 Descriptor.*/ + USB_DESC_ENDPOINT (USBD1_INTERRUPT_REQUEST_EP|0x80, + 0x03, /* bmAttributes (Interrupt). */ + 0x0008, /* wMaxPacketSize. */ + 0xFF), /* bInterval. */ + /* Interface Descriptor.*/ + USB_DESC_INTERFACE (0x01, /* bInterfaceNumber. */ + 0x00, /* bAlternateSetting. */ + 0x02, /* bNumEndpoints. */ + 0x0A, /* bInterfaceClass (Data Class +Interface, CDC section 4.5). */ + 0x00, /* bInterfaceSubClass (CDC section +4.6). */ + 0x00, /* bInterfaceProtocol (CDC section +4.7). */ + 0x00), /* iInterface. */ + /* Endpoint 3 Descriptor.*/ + USB_DESC_ENDPOINT (USBD1_DATA_AVAILABLE_EP, /* bEndpointAddress.*/ + 0x02, /* bmAttributes (Bulk). */ + 0x0040, /* wMaxPacketSize. */ + 0x00), /* bInterval. */ + /* Endpoint 1 Descriptor.*/ + USB_DESC_ENDPOINT (USBD1_DATA_REQUEST_EP|0x80, /* bEndpointAddress.*/ + 0x02, /* bmAttributes (Bulk). */ + 0x0040, /* wMaxPacketSize. */ + 0x00) /* bInterval. */ +}; + +/* +* Configuration Descriptor wrapper. +*/ +static const USBDescriptor vcom_configuration_descriptor = { + sizeof vcom_configuration_descriptor_data, + vcom_configuration_descriptor_data +}; + +/* +* U.S. English language identifier. +*/ +static const uint8_t vcom_string0[] = { + USB_DESC_BYTE(4), /* bLength. */ + USB_DESC_BYTE(USB_DESCRIPTOR_STRING), /* bDescriptorType. */ + USB_DESC_WORD(0x0409) /* wLANGID (U.S. English). */ +}; + +/* +* Vendor string. +*/ +static const uint8_t vcom_string1[] = { + USB_DESC_BYTE(38), /* bLength. */ + USB_DESC_BYTE(USB_DESCRIPTOR_STRING), /* bDescriptorType. */ + 'S', 0, 'T', 0, 'M', 0, 'i', 0, 'c', 0, 'r', 0, 'o', 0, 'e', 0, + 'l', 0, 'e', 0, 'c', 0, 't', 0, 'r', 0, 'o', 0, 'n', 0, 'i', 0, + 'c', 0, 's', 0 +}; + +/* +* Device Description string. +*/ +static const uint8_t vcom_string2[] = { + USB_DESC_BYTE(56), /* bLength. */ + USB_DESC_BYTE(USB_DESCRIPTOR_STRING), /* bDescriptorType. */ + 'C', 0, 'h', 0, 'i', 0, 'b', 0, 'i', 0, 'O', 0, 'S', 0, '/', 0, + 'R', 0, 'T', 0, ' ', 0, 'V', 0, 'i', 0, 'r', 0, 't', 0, 'u', 0, + 'a', 0, 'l', 0, ' ', 0, 'C', 0, 'O', 0, 'M', 0, ' ', 0, 'P', 0, + 'o', 0, 'r', 0, 't', 0 +}; + +/* +* Serial Number string. +*/ +static const uint8_t vcom_string3[] = { + USB_DESC_BYTE(8), /* bLength. */ + USB_DESC_BYTE(USB_DESCRIPTOR_STRING), /* bDescriptorType. */ + '0' + CH_KERNEL_MAJOR, 0, + '0' + CH_KERNEL_MINOR, 0, + '0' + CH_KERNEL_PATCH, 0 +}; + +/* +* Strings wrappers array. +*/ +static const USBDescriptor vcom_strings[] = { + {sizeof vcom_string0, vcom_string0}, + {sizeof vcom_string1, vcom_string1}, + {sizeof vcom_string2, vcom_string2}, + {sizeof vcom_string3, vcom_string3} +}; + + + +#endif // USBDESCRIPTOR_H_INCLUDED diff --git a/utils.c b/utils.c new file mode 100644 index 000000000..3aae818c5 --- /dev/null +++ b/utils.c @@ -0,0 +1,43 @@ +/* + Copyright 2012-2014 Benjamin Vedder benjamin@vedder.se + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +/* + * utils.c + * + * Created on: 16 maj 2013 + * Author: benjamin + */ + +void step_towards(float *value, float goal, float step) { + if (*value < goal) { + if ((*value + step) < goal) { + *value += step; + } else { + *value = goal; + } + } else if (*value > goal) { + if ((*value - step) > goal) { + *value -= step; + } else { + *value = goal; + } + } +} + +float calc_ratio(float low, float high, float val) { + return (val - low) / (high - low); +} diff --git a/utils.h b/utils.h new file mode 100644 index 000000000..5d7f2e2af --- /dev/null +++ b/utils.h @@ -0,0 +1,14 @@ +/* + * utils.h + * + * Created on: 16 maj 2013 + * Author: benjamin + */ + +#ifndef UTILS_H_ +#define UTILS_H_ + +void step_towards(float *value, float goal, float step); +float calc_ratio(float low, float high, float val); + +#endif /* UTILS_H_ */