From 1a14ef827ed99a814b00d8ea4b98b8633582b945 Mon Sep 17 00:00:00 2001 From: Clyne Sullivan Date: Mon, 25 Jan 2021 18:13:51 -0500 Subject: [PATCH] fix trig funcs; idle sleep w/ button to wake for debug --- Makefile | 374 +++++++++++++++++++++++----------------------- source/cordic.cpp | 62 ++++---- source/main.cpp | 14 ++ 3 files changed, 236 insertions(+), 214 deletions(-) diff --git a/Makefile b/Makefile index e560cfc..efb47ad 100644 --- a/Makefile +++ b/Makefile @@ -1,187 +1,187 @@ -############################################################################## -# Build global options -# NOTE: Can be overridden externally. -# - -# Compiler options here. -ifeq ($(USE_OPT),) - USE_OPT = -O0 -ggdb -fomit-frame-pointer -falign-functions=16 --specs=nosys.specs -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 = -std=c++17 -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 - -# Linker extra options here. -ifeq ($(USE_LDOPT),) - USE_LDOPT = -endif - -# Enable this if you want link time optimizations (LTO). -ifeq ($(USE_LTO),) - USE_LTO = yes -endif - -# Enable this if you want to see the full log while compiling. -ifeq ($(USE_VERBOSE_COMPILE),) - USE_VERBOSE_COMPILE = no -endif - -# If enabled, this option makes the build process faster by not compiling -# modules not used in the current configuration. -ifeq ($(USE_SMART_BUILD),) - USE_SMART_BUILD = yes -endif - -# -# Build global options -############################################################################## - -############################################################################## -# Architecture or project specific options -# - -# Stack size to be allocated to the Cortex-M process stack. This stack is -# the stack used by the main() thread. -ifeq ($(USE_PROCESS_STACKSIZE),) - USE_PROCESS_STACKSIZE = 0x400 -endif - -# Stack size to the allocated to the Cortex-M main/exceptions stack. This -# stack is used for processing interrupts and exceptions. -ifeq ($(USE_EXCEPTIONS_STACKSIZE),) - USE_EXCEPTIONS_STACKSIZE = 0x400 -endif - -# Enables the use of FPU (no, softfp, hard). -ifeq ($(USE_FPU),) - USE_FPU = hard -endif - -# FPU-related options. -ifeq ($(USE_FPU_OPT),) - USE_FPU_OPT = -mfloat-abi=$(USE_FPU) -mfpu=fpv5-d16 -endif - -# -# Architecture or project specific options -############################################################################## - -############################################################################## -# Project, target, sources and paths -# - -# Define project name here -PROJECT = ch - -# Target settings. -MCU = cortex-m7 - -# Imported source files and paths. -CHIBIOS := ./ChibiOS_20.3.2 -CONFDIR := ./cfg -BUILDDIR := ./build -DEPDIR := ./.dep - -# Licensing files. -include $(CHIBIOS)/os/license/license.mk -# Startup files. -include $(CHIBIOS)/os/common/startup/ARMCMx/compilers/GCC/mk/startup_stm32h7xx.mk -# HAL-OSAL files (optional). -include $(CHIBIOS)/os/hal/hal.mk -include $(CHIBIOS)/os/hal/ports/STM32/STM32H7xx/platform.mk -include ./board/board.mk -include $(CHIBIOS)/os/hal/osal/rt-nil/osal.mk -# RTOS files (optional). -include $(CHIBIOS)/os/rt/rt.mk -include $(CHIBIOS)/os/common/ports/ARMCMx/compilers/GCC/mk/port_v7m.mk -# Auto-build files in ./source recursively. -include $(CHIBIOS)/tools/mk/autobuild.mk -# Other files (optional). -#include $(CHIBIOS)/test/lib/test.mk -#include $(CHIBIOS)/test/rt/rt_test.mk -#include $(CHIBIOS)/test/oslib/oslib_test.mk - -# Define linker script file here -LDSCRIPT= STM32H723xG.ld - -# C sources that can be compiled in ARM or THUMB mode depending on the global -# setting. -CSRC = $(ALLCSRC) - -# C++ sources that can be compiled in ARM or THUMB mode depending on the global -# setting. -CPPSRC = $(ALLCPPSRC) - -# List ASM source files here. -ASMSRC = $(ALLASMSRC) - -# List ASM with preprocessor source files here. -ASMXSRC = $(ALLXASMSRC) - -# Inclusion directories. -INCDIR = $(CONFDIR) $(ALLINC) $(TESTINC) - -# Define C warning options here. -CWARN = -Wall -Wextra -Wundef -Wstrict-prototypes - -# Define C++ warning options here. -CPPWARN = -Wall -Wextra -Wundef - -# -# Project, target, sources and paths -############################################################################## - -############################################################################## -# Start of user section -# - -# List all user C define here, like -D_DEBUG=1 -UDEFS = -DCORTEX_ENABLE_WFI_IDLE=FALSE -DPORT_USE_SYSCALL=TRUE - -# 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 = - -# -# End of user section -############################################################################## - -############################################################################## -# Common rules -# - -RULESPATH = $(CHIBIOS)/os/common/startup/ARMCMx/compilers/GCC/mk -include $(RULESPATH)/arm-none-eabi.mk -include $(RULESPATH)/rules.mk - -# -# Common rules -############################################################################## - -############################################################################## -# Custom rules -# - -# -# Custom rules -############################################################################## +############################################################################## +# Build global options +# NOTE: Can be overridden externally. +# + +# Compiler options here. +ifeq ($(USE_OPT),) + USE_OPT = -O0 -ggdb -fomit-frame-pointer -falign-functions=16 --specs=nosys.specs +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 = -std=c++17 -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 + +# Linker extra options here. +ifeq ($(USE_LDOPT),) + USE_LDOPT = +endif + +# Enable this if you want link time optimizations (LTO). +ifeq ($(USE_LTO),) + USE_LTO = yes +endif + +# Enable this if you want to see the full log while compiling. +ifeq ($(USE_VERBOSE_COMPILE),) + USE_VERBOSE_COMPILE = no +endif + +# If enabled, this option makes the build process faster by not compiling +# modules not used in the current configuration. +ifeq ($(USE_SMART_BUILD),) + USE_SMART_BUILD = yes +endif + +# +# Build global options +############################################################################## + +############################################################################## +# Architecture or project specific options +# + +# Stack size to be allocated to the Cortex-M process stack. This stack is +# the stack used by the main() thread. +ifeq ($(USE_PROCESS_STACKSIZE),) + USE_PROCESS_STACKSIZE = 0x400 +endif + +# Stack size to the allocated to the Cortex-M main/exceptions stack. This +# stack is used for processing interrupts and exceptions. +ifeq ($(USE_EXCEPTIONS_STACKSIZE),) + USE_EXCEPTIONS_STACKSIZE = 0x400 +endif + +# Enables the use of FPU (no, softfp, hard). +ifeq ($(USE_FPU),) + USE_FPU = hard +endif + +# FPU-related options. +ifeq ($(USE_FPU_OPT),) + USE_FPU_OPT = -mfloat-abi=$(USE_FPU) -mfpu=fpv5-d16 +endif + +# +# Architecture or project specific options +############################################################################## + +############################################################################## +# Project, target, sources and paths +# + +# Define project name here +PROJECT = ch + +# Target settings. +MCU = cortex-m7 + +# Imported source files and paths. +CHIBIOS := ./ChibiOS_20.3.2 +CONFDIR := ./cfg +BUILDDIR := ./build +DEPDIR := ./.dep + +# Licensing files. +include $(CHIBIOS)/os/license/license.mk +# Startup files. +include $(CHIBIOS)/os/common/startup/ARMCMx/compilers/GCC/mk/startup_stm32h7xx.mk +# HAL-OSAL files (optional). +include $(CHIBIOS)/os/hal/hal.mk +include $(CHIBIOS)/os/hal/ports/STM32/STM32H7xx/platform.mk +include ./board/board.mk +include $(CHIBIOS)/os/hal/osal/rt-nil/osal.mk +# RTOS files (optional). +include $(CHIBIOS)/os/rt/rt.mk +include $(CHIBIOS)/os/common/ports/ARMCMx/compilers/GCC/mk/port_v7m.mk +# Auto-build files in ./source recursively. +include $(CHIBIOS)/tools/mk/autobuild.mk +# Other files (optional). +#include $(CHIBIOS)/test/lib/test.mk +#include $(CHIBIOS)/test/rt/rt_test.mk +#include $(CHIBIOS)/test/oslib/oslib_test.mk + +# Define linker script file here +LDSCRIPT= STM32H723xG.ld + +# C sources that can be compiled in ARM or THUMB mode depending on the global +# setting. +CSRC = $(ALLCSRC) + +# C++ sources that can be compiled in ARM or THUMB mode depending on the global +# setting. +CPPSRC = $(ALLCPPSRC) + +# List ASM source files here. +ASMSRC = $(ALLASMSRC) + +# List ASM with preprocessor source files here. +ASMXSRC = $(ALLXASMSRC) + +# Inclusion directories. +INCDIR = $(CONFDIR) $(ALLINC) $(TESTINC) + +# Define C warning options here. +CWARN = -Wall -Wextra -Wundef -Wstrict-prototypes + +# Define C++ warning options here. +CPPWARN = -Wall -Wextra -Wundef + +# +# Project, target, sources and paths +############################################################################## + +############################################################################## +# Start of user section +# + +# List all user C define here, like -D_DEBUG=1 +UDEFS = -DCORTEX_ENABLE_WFI_IDLE=TRUE -DPORT_USE_SYSCALL=TRUE + +# 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 = + +# +# End of user section +############################################################################## + +############################################################################## +# Common rules +# + +RULESPATH = $(CHIBIOS)/os/common/startup/ARMCMx/compilers/GCC/mk +include $(RULESPATH)/arm-none-eabi.mk +include $(RULESPATH)/rules.mk + +# +# Common rules +############################################################################## + +############################################################################## +# Custom rules +# + +# +# Custom rules +############################################################################## diff --git a/source/cordic.cpp b/source/cordic.cpp index 2a44015..4852563 100644 --- a/source/cordic.cpp +++ b/source/cordic.cpp @@ -12,71 +12,79 @@ static void prepare() { while (CORDIC->CSR & CORDIC_CSR_RRDY) asm("mov r0, %0" :: "r" (CORDIC->RDATA)); } -static uint32_t dtoq(double in) { - double res = in * 0x7FFFFFFF; - int32_t resi = res; - return resi; + +static uint32_t dtoq(double) { + uint32_t res; + asm("vcvt.s32.f64 d0, d0, #31;" + "vmov %0, r5, d0" + : "=r" (res)); + return res; } -static double qtod(uint32_t in) { - int32_t ini = in; - double res = ini; - return res / 0x7FFFFFFF; +__attribute__((naked)) +static double qtod(uint32_t) { + asm("eor r1, r1;" + "vmov d0, r0, r1;" + "vcvt.f64.s32 d0, d0, #31;" + "bx lr"); + return 0; } - __attribute__((naked)) -double mod(double n, double) { +double mod(double, double) { asm("vdiv.f64 d2, d0, d1;" "vrintz.f64 d2;" "vmul.f64 d1, d1, d2;" "vsub.f64 d0, d0, d1;" "bx lr"); - return n; + return 0; } double cos(double x) { - x = (mod(x, 2 * math::PI) - math::PI) / math::PI; + x = mod(x, 2 * math::PI) / math::PI; + auto input = dtoq(x > 1. ? x - 2 : x); + prepare(); CORDIC->CSR = CORDIC_CSR_NARGS | CORDIC_CSR_NRES | (6 << CORDIC_CSR_PRECISION_Pos) | (0 << CORDIC_CSR_FUNC_Pos); - auto in = dtoq(x); - CORDIC->WDATA = in; - CORDIC->WDATA = in & 0x7FFFFFFF; + CORDIC->WDATA = input; + CORDIC->WDATA = input; while (!(CORDIC->CSR & CORDIC_CSR_RRDY)); double cosx = qtod(CORDIC->RDATA) / x; - in = CORDIC->RDATA; + [[maybe_unused]] auto sinx = CORDIC->RDATA; return cosx; } double sin(double x) { - x = (mod(x, 2 * math::PI) - math::PI) / math::PI; + x = mod(x, 2 * math::PI) / math::PI; + auto input = dtoq(x > 1. ? x - 2 : x); + prepare(); CORDIC->CSR = CORDIC_CSR_NARGS | CORDIC_CSR_NRES | (6 << CORDIC_CSR_PRECISION_Pos) | (1 << CORDIC_CSR_FUNC_Pos); - auto in = dtoq(x); - CORDIC->WDATA = in; - CORDIC->WDATA = in & 0x7FFFFFFF; + CORDIC->WDATA = input; + CORDIC->WDATA = input; while (!(CORDIC->CSR & CORDIC_CSR_RRDY)); double sinx = qtod(CORDIC->RDATA) / x; - in = CORDIC->RDATA; + [[maybe_unused]] auto cosx = CORDIC->RDATA; return sinx; } double tan(double x) { - x = (mod(x, 2 * math::PI) - math::PI) / math::PI; + x = mod(x, 2 * math::PI) / math::PI; + auto input = dtoq(x > 1. ? x - 2 : x); + prepare(); CORDIC->CSR = CORDIC_CSR_NARGS | CORDIC_CSR_NRES | (6 << CORDIC_CSR_PRECISION_Pos) | (1 << CORDIC_CSR_FUNC_Pos); - auto in = dtoq(x); - CORDIC->WDATA = in; - CORDIC->WDATA = in & 0x7FFFFFFF; + CORDIC->WDATA = input; + CORDIC->WDATA = input; while (!(CORDIC->CSR & CORDIC_CSR_RRDY)); double sinx = qtod(CORDIC->RDATA) / x; @@ -85,9 +93,9 @@ double tan(double x) { } __attribute__((naked)) -double sqrt(double x) { +double sqrt(double) { asm("vsqrt.f64 d0, d0; bx lr"); - return x; + return 0.; } } diff --git a/source/main.cpp b/source/main.cpp index a682cfe..97612f4 100644 --- a/source/main.cpp +++ b/source/main.cpp @@ -75,6 +75,8 @@ int main() halInit(); chSysInit(); + palSetLineMode(LINE_BUTTON, PAL_MODE_INPUT); + // Enable FPU SCB->CPACR |= 0xF << 20; @@ -353,6 +355,18 @@ THD_FUNCTION(Thread1, arg) palClearLine(led); chThdSleepMilliseconds(delay); + if (run_status == RunStatus::Idle && palReadLine(LINE_BUTTON)) { + palSetLine(LINE_LED_RED); + palSetLine(LINE_LED_YELLOW); + asm("cpsid i"); + while (!palReadLine(LINE_BUTTON)) + asm("nop"); + asm("cpsie i"); + palClearLine(LINE_LED_RED); + palClearLine(LINE_LED_YELLOW); + chThdSleepMilliseconds(500); + } + if (auto err = EM.hasError(); err ^ erroron) { erroron = err; if (err)