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

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)