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

diff --git a/Makefile b/Makefile
index e560cfc3b48299047d675d15c05dcce08fcf0d94..efb47ad4459ebe77cd732d3955a5a8747482f588 100644 (file)
--- a/Makefile
+++ b/Makefile
@@ -1,187 +1,187 @@
-##############################################################################\r
-# Build global options\r
-# NOTE: Can be overridden externally.\r
-#\r
-\r
-# Compiler options here.\r
-ifeq ($(USE_OPT),)\r
-  USE_OPT = -O0 -ggdb -fomit-frame-pointer -falign-functions=16 --specs=nosys.specs\r
-endif\r
-\r
-# C specific options here (added to USE_OPT).\r
-ifeq ($(USE_COPT),)\r
-  USE_COPT = \r
-endif\r
-\r
-# C++ specific options here (added to USE_OPT).\r
-ifeq ($(USE_CPPOPT),)\r
-  USE_CPPOPT = -std=c++17 -fno-rtti\r
-endif\r
-\r
-# Enable this if you want the linker to remove unused code and data.\r
-ifeq ($(USE_LINK_GC),)\r
-  USE_LINK_GC = yes\r
-endif\r
-\r
-# Linker extra options here.\r
-ifeq ($(USE_LDOPT),)\r
-  USE_LDOPT = \r
-endif\r
-\r
-# Enable this if you want link time optimizations (LTO).\r
-ifeq ($(USE_LTO),)\r
-  USE_LTO = yes\r
-endif\r
-\r
-# Enable this if you want to see the full log while compiling.\r
-ifeq ($(USE_VERBOSE_COMPILE),)\r
-  USE_VERBOSE_COMPILE = no\r
-endif\r
-\r
-# If enabled, this option makes the build process faster by not compiling\r
-# modules not used in the current configuration.\r
-ifeq ($(USE_SMART_BUILD),)\r
-  USE_SMART_BUILD = yes\r
-endif\r
-\r
-#\r
-# Build global options\r
-##############################################################################\r
-\r
-##############################################################################\r
-# Architecture or project specific options\r
-#\r
-\r
-# Stack size to be allocated to the Cortex-M process stack. This stack is\r
-# the stack used by the main() thread.\r
-ifeq ($(USE_PROCESS_STACKSIZE),)\r
-  USE_PROCESS_STACKSIZE = 0x400\r
-endif\r
-\r
-# Stack size to the allocated to the Cortex-M main/exceptions stack. This\r
-# stack is used for processing interrupts and exceptions.\r
-ifeq ($(USE_EXCEPTIONS_STACKSIZE),)\r
-  USE_EXCEPTIONS_STACKSIZE = 0x400\r
-endif\r
-\r
-# Enables the use of FPU (no, softfp, hard).\r
-ifeq ($(USE_FPU),)\r
-  USE_FPU = hard\r
-endif\r
-\r
-# FPU-related options.\r
-ifeq ($(USE_FPU_OPT),)\r
-  USE_FPU_OPT = -mfloat-abi=$(USE_FPU) -mfpu=fpv5-d16\r
-endif\r
-\r
-#\r
-# Architecture or project specific options\r
-##############################################################################\r
-\r
-##############################################################################\r
-# Project, target, sources and paths\r
-#\r
-\r
-# Define project name here\r
-PROJECT = ch\r
-\r
-# Target settings.\r
-MCU  = cortex-m7\r
-\r
-# Imported source files and paths.\r
-CHIBIOS  := ./ChibiOS_20.3.2\r
-CONFDIR  := ./cfg\r
-BUILDDIR := ./build\r
-DEPDIR   := ./.dep\r
-\r
-# Licensing files.\r
-include $(CHIBIOS)/os/license/license.mk\r
-# Startup files.\r
-include $(CHIBIOS)/os/common/startup/ARMCMx/compilers/GCC/mk/startup_stm32h7xx.mk\r
-# HAL-OSAL files (optional).\r
-include $(CHIBIOS)/os/hal/hal.mk\r
-include $(CHIBIOS)/os/hal/ports/STM32/STM32H7xx/platform.mk\r
-include ./board/board.mk\r
-include $(CHIBIOS)/os/hal/osal/rt-nil/osal.mk\r
-# RTOS files (optional).\r
-include $(CHIBIOS)/os/rt/rt.mk\r
-include $(CHIBIOS)/os/common/ports/ARMCMx/compilers/GCC/mk/port_v7m.mk\r
-# Auto-build files in ./source recursively.\r
-include $(CHIBIOS)/tools/mk/autobuild.mk\r
-# Other files (optional).\r
-#include $(CHIBIOS)/test/lib/test.mk\r
-#include $(CHIBIOS)/test/rt/rt_test.mk\r
-#include $(CHIBIOS)/test/oslib/oslib_test.mk\r
-\r
-# Define linker script file here\r
-LDSCRIPT= STM32H723xG.ld\r
-\r
-# C sources that can be compiled in ARM or THUMB mode depending on the global\r
-# setting.\r
-CSRC = $(ALLCSRC)\r
-\r
-# C++ sources that can be compiled in ARM or THUMB mode depending on the global\r
-# setting.\r
-CPPSRC = $(ALLCPPSRC)\r
-\r
-# List ASM source files here.\r
-ASMSRC = $(ALLASMSRC)\r
-\r
-# List ASM with preprocessor source files here.\r
-ASMXSRC = $(ALLXASMSRC)\r
-\r
-# Inclusion directories.\r
-INCDIR = $(CONFDIR) $(ALLINC) $(TESTINC)\r
-\r
-# Define C warning options here.\r
-CWARN = -Wall -Wextra -Wundef -Wstrict-prototypes\r
-\r
-# Define C++ warning options here.\r
-CPPWARN = -Wall -Wextra -Wundef\r
-\r
-#\r
-# Project, target, sources and paths\r
-##############################################################################\r
-\r
-##############################################################################\r
-# Start of user section\r
-#\r
-\r
-# List all user C define here, like -D_DEBUG=1\r
-UDEFS = -DCORTEX_ENABLE_WFI_IDLE=FALSE -DPORT_USE_SYSCALL=TRUE\r
-\r
-# Define ASM defines here\r
-UADEFS =\r
-\r
-# List all user directories here\r
-UINCDIR =\r
-\r
-# List the user directory to look for the libraries here\r
-ULIBDIR =\r
-\r
-# List all user libraries here\r
-ULIBS =\r
-\r
-#\r
-# End of user section\r
-##############################################################################\r
-\r
-##############################################################################\r
-# Common rules\r
-#\r
-\r
-RULESPATH = $(CHIBIOS)/os/common/startup/ARMCMx/compilers/GCC/mk\r
-include $(RULESPATH)/arm-none-eabi.mk\r
-include $(RULESPATH)/rules.mk\r
-\r
-#\r
-# Common rules\r
-##############################################################################\r
-\r
-##############################################################################\r
-# Custom rules\r
-#\r
-\r
-#\r
-# Custom rules\r
-##############################################################################\r
+##############################################################################
+# 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 2a44015d2777ab8e767780a4ae0a795fc8e2a69e..4852563137d1134cad90d6ed045db8f21b3efb12 100644 (file)
--- 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 a682cfee36417bafb7bc2f9c0903bdea12fbba71..97612f43c6a2ab6034233a45b64bfb606d6816c6 100644 (file)
--- 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)