fix trig funcs; idle sleep w/ button to wake for debug

pull/3/head
Clyne 4 years ago
parent e132d52adf
commit 1a14ef827e

@ -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
##############################################################################

@ -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.;
}
}

@ -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)

Loading…
Cancel
Save