From e132d52adf52b91cc44741c9d13c178219d7a5f5 Mon Sep 17 00:00:00 2001 From: Clyne Sullivan Date: Sun, 24 Jan 2021 13:16:20 -0500 Subject: [PATCH] error recv. draft; add trig/math funcs --- Makefile | 2 +- source/cordic.cpp | 94 +++++++++++++++++++++++++++++++++++++++++++++ source/cordic.hpp | 18 +++++++++ source/main.cpp | 98 +++++++++++++++++++++++------------------------ 4 files changed, 162 insertions(+), 50 deletions(-) create mode 100644 source/cordic.cpp create mode 100644 source/cordic.hpp diff --git a/Makefile b/Makefile index a609c42..e560cfc 100644 --- a/Makefile +++ b/Makefile @@ -148,7 +148,7 @@ CPPWARN = -Wall -Wextra -Wundef # # List all user C define here, like -D_DEBUG=1 -UDEFS = -DCORTEX_ENABLE_WFI_IDLE=FALSE +UDEFS = -DCORTEX_ENABLE_WFI_IDLE=FALSE -DPORT_USE_SYSCALL=TRUE # Define ASM defines here UADEFS = diff --git a/source/cordic.cpp b/source/cordic.cpp new file mode 100644 index 0000000..2a44015 --- /dev/null +++ b/source/cordic.cpp @@ -0,0 +1,94 @@ +#include "cordic.hpp" +#include "hal.h" + +namespace math { + +void init() +{ + RCC->AHB2ENR |= RCC_AHB2ENR_CORDICEN; +} + +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 double qtod(uint32_t in) { + int32_t ini = in; + double res = ini; + return res / 0x7FFFFFFF; +} + +__attribute__((naked)) +double mod(double n, double) { + asm("vdiv.f64 d2, d0, d1;" + "vrintz.f64 d2;" + "vmul.f64 d1, d1, d2;" + "vsub.f64 d0, d0, d1;" + "bx lr"); + return n; +} + +double cos(double x) { + x = (mod(x, 2 * math::PI) - math::PI) / math::PI; + 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; + while (!(CORDIC->CSR & CORDIC_CSR_RRDY)); + + double cosx = qtod(CORDIC->RDATA) / x; + in = CORDIC->RDATA; + return cosx; +} + +double sin(double x) { + x = (mod(x, 2 * math::PI) - math::PI) / math::PI; + 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; + while (!(CORDIC->CSR & CORDIC_CSR_RRDY)); + + double sinx = qtod(CORDIC->RDATA) / x; + in = CORDIC->RDATA; + return sinx; +} + +double tan(double x) { + x = (mod(x, 2 * math::PI) - math::PI) / math::PI; + 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; + while (!(CORDIC->CSR & CORDIC_CSR_RRDY)); + + double sinx = qtod(CORDIC->RDATA) / x; + double tanx = sinx * x / qtod(CORDIC->RDATA); + return tanx; +} + +__attribute__((naked)) +double sqrt(double x) { + asm("vsqrt.f64 d0, d0; bx lr"); + return x; +} + +} + diff --git a/source/cordic.hpp b/source/cordic.hpp new file mode 100644 index 0000000..755fddb --- /dev/null +++ b/source/cordic.hpp @@ -0,0 +1,18 @@ +#ifndef CORDIC_HPP_ +#define CORDIC_HPP_ + +namespace math { + constexpr double PI = 3.1415926535L; + + void init(); + + double mod(double n, double d); + + double cos(double x); + double sin(double x); + double tan(double x); + double sqrt(double x); +} + +#endif // CORDIC_HPP_ + diff --git a/source/main.cpp b/source/main.cpp index 5afed63..a682cfe 100644 --- a/source/main.cpp +++ b/source/main.cpp @@ -19,6 +19,7 @@ static_assert(sizeof(dacsample_t) == sizeof(uint16_t)); #include "error.hpp" #include "adc.hpp" +#include "cordic.hpp" #include "dac.hpp" #include "elf_load.hpp" #include "sclock.hpp" @@ -31,7 +32,8 @@ constexpr unsigned int MAX_ELF_FILE_SIZE = 8 * 1024; enum class RunStatus : char { Idle = '1', - Running + Running, + Recovering }; static RunStatus run_status = RunStatus::Idle; @@ -80,6 +82,7 @@ int main() DAC::begin(); SClock::begin(); USBSerial::begin(); + math::init(); SClock::setRate(SClock::Rate::R32K); ADC::setRate(SClock::Rate::R32K); @@ -89,25 +92,6 @@ int main() chThdCreateStatic(conversionThreadWA, sizeof(conversionThreadWA), NORMALPRIO, conversionThread, nullptr); - // TEST (success!) - /*double input = 0.25L; - - RCC->AHB2ENR |= RCC_AHB2ENR_CORDICEN; - uint32_t dummy = 0; - while (CORDIC->CSR & CORDIC_CSR_RRDY) - dummy = CORDIC->RDATA; - - CORDIC->CSR = (3 << CORDIC_CSR_PRECISION_Pos) | - (9 << CORDIC_CSR_FUNC_Pos); // 1 arg, 3 iterations, sqrt - - input *= 0x7FFFFFFF; - dummy = input; - CORDIC->WDATA = dummy; - while (!(CORDIC->CSR & CORDIC_CSR_RRDY)); - // result: - dummy = CORDIC->RDATA; // m cos() - double output = (double)dummy / 0x7FFFFFFF;*/ - main_loop(); } @@ -287,6 +271,9 @@ void conversion_abort() DAC::stop(0); ADC::stop(); EM.add(Error::ConversionAborted); + + chMBReset(&conversionMB); + run_status = RunStatus::Idle; } THD_FUNCTION(conversionThread, arg) @@ -294,14 +281,26 @@ THD_FUNCTION(conversionThread, arg) (void)arg; while (1) { + // Recover from algorithm fault if necessary + if (run_status == RunStatus::Recovering) + conversion_abort(); + msg_t message; if (chMBFetchTimeout(&conversionMB, &message, TIME_INFINITE) == MSG_OK) { - auto samples = MSG_FOR_FIRST(message) ? samplesIn.data() : samplesIn.middata(); - auto size = samplesIn.size() / 2; + static Sample *samples = nullptr; + static unsigned int size = 0; + samples = MSG_FOR_FIRST(message) ? samplesIn.data() : samplesIn.middata(); + size = samplesIn.size() / 2; if (elf_entry) { if (!MSG_FOR_MEASURE(message)) { - samples = elf_entry(samples, size); + //asm("cpsid i"); + //mpuDisable(); + //port_unprivileged_jump((uint32_t)+[] { + samples = elf_entry(samples, size); + //}, 0xF800); + //mpuEnable(MPU_CTRL_PRIVDEFENA); + //asm("cpsie i"); } else { chTMStartMeasurementX(&conversion_time_measurement); samples = elf_entry(samples, size); @@ -369,33 +368,34 @@ extern "C" { __attribute__((naked)) void HardFault_Handler() { + // Below not working (yet) while (1); -// //asm("push {lr}"); -// -// uint32_t *stack; -// uint32_t lr; -// asm("\ -// tst lr, #4; \ -// ite eq; \ -// mrseq %0, msp; \ -// mrsne %0, psp; \ -// mov %1, lr; \ -// " : "=r" (stack), "=r" (lr)); -// //stack++; -// stack[7] |= (1 << 24); // Keep Thumb mode enabled -// -// conversion_abort(); -// -// // TODO test lr and decide how to recover -// -// //if (run_status == RunStatus::Converting) { -// stack[6] = stack[5]; // Escape from elf_entry code -// //} else /*if (run_status == RunStatus::Recovered)*/ { -// // stack[6] = (uint32_t)main_loop & ~1; // Return to safety -// //} -// -// //asm("pop {lr}; bx lr"); -// asm("bx lr"); + + // 1. Get the stack pointer + uint32_t *stack; + uint32_t lr; + asm("\ + tst lr, #4; \ + ite eq; \ + mrseq %0, msp; \ + mrsne %0, psp; \ + mov %1, lr; \ + " : "=r" (stack), "=r" (lr)); + + // 2. Only attempt to recover from failed algorithm code + if ((lr & 4) == 0 || run_status != RunStatus::Running) + while (1); + + // 3. Post the failure and unload algorithm + elf_entry = nullptr; + EM.add(Error::ConversionAborted); + run_status = RunStatus::Recovering; + + // 4. Make this exception return to point after algorithm exec. + stack[6] = stack[5]; + stack[7] |= (1 << 24); // Ensure Thumb mode stays enabled + + asm("mov lr, %0; bx lr" :: "r" (lr)); } } // extern "C"