clyne
/
stmdsp
1
0
Fork 0
Code Issues Pull Requests Projects Releases 1 Wiki Activity

error recv. draft; add trig/math funcs

Browse Source
pull/3/head
Clyne 4 years ago
parent a4d9689259
commit e132d52adf
4 changed files with 162 additions and 50 deletions
Show Stats Download Patch File Download Diff File

2
Makefile
Unescape Escape View File

@ -148,7 +148,7 @@ CPPWARN = -Wall -Wextra -Wundef
# #
# List all user C define here, like -D_DEBUG=1 # 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 # Define ASM defines here
UADEFS = UADEFS =

94
source/cordic.cpp
Unescape Escape View File

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

18
source/cordic.hpp
Unescape Escape View File

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

98
source/main.cpp
Unescape Escape View File

@ -19,6 +19,7 @@ static_assert(sizeof(dacsample_t) == sizeof(uint16_t));
#include "error.hpp" #include "error.hpp"
#include "adc.hpp" #include "adc.hpp"
#include "cordic.hpp"
#include "dac.hpp" #include "dac.hpp"
#include "elf_load.hpp" #include "elf_load.hpp"
#include "sclock.hpp" #include "sclock.hpp"
@ -31,7 +32,8 @@ constexpr unsigned int MAX_ELF_FILE_SIZE = 8 * 1024;
enum class RunStatus : char enum class RunStatus : char
{ {
Idle = '1', Idle = '1',
Running Running,
Recovering
}; };
static RunStatus run_status = RunStatus::Idle; static RunStatus run_status = RunStatus::Idle;
@ -80,6 +82,7 @@ int main()
DAC::begin(); DAC::begin();
SClock::begin(); SClock::begin();
USBSerial::begin(); USBSerial::begin();
math::init();
SClock::setRate(SClock::Rate::R32K); SClock::setRate(SClock::Rate::R32K);
ADC::setRate(SClock::Rate::R32K); ADC::setRate(SClock::Rate::R32K);
@ -89,25 +92,6 @@ int main()
chThdCreateStatic(conversionThreadWA, sizeof(conversionThreadWA), chThdCreateStatic(conversionThreadWA, sizeof(conversionThreadWA),
NORMALPRIO, conversionThread, nullptr); 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(); main_loop();
} }
@ -287,6 +271,9 @@ void conversion_abort()
DAC::stop(0); DAC::stop(0);
ADC::stop(); ADC::stop();
EM.add(Error::ConversionAborted); EM.add(Error::ConversionAborted);
chMBReset(&conversionMB);
run_status = RunStatus::Idle;
} }
THD_FUNCTION(conversionThread, arg) THD_FUNCTION(conversionThread, arg)
@ -294,14 +281,26 @@ THD_FUNCTION(conversionThread, arg)
(void)arg; (void)arg;
while (1) { while (1) {
// Recover from algorithm fault if necessary
if (run_status == RunStatus::Recovering)
conversion_abort();
msg_t message; msg_t message;
if (chMBFetchTimeout(&conversionMB, &message, TIME_INFINITE) == MSG_OK) { if (chMBFetchTimeout(&conversionMB, &message, TIME_INFINITE) == MSG_OK) {
auto samples = MSG_FOR_FIRST(message) ? samplesIn.data() : samplesIn.middata(); static Sample *samples = nullptr;
auto size = samplesIn.size() / 2; static unsigned int size = 0;
samples = MSG_FOR_FIRST(message) ? samplesIn.data() : samplesIn.middata();
size = samplesIn.size() / 2;
if (elf_entry) { if (elf_entry) {
if (!MSG_FOR_MEASURE(message)) { 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 { } else {
chTMStartMeasurementX(&conversion_time_measurement); chTMStartMeasurementX(&conversion_time_measurement);
samples = elf_entry(samples, size); samples = elf_entry(samples, size);
@ -369,33 +368,34 @@ extern "C" {
__attribute__((naked)) __attribute__((naked))
void HardFault_Handler() void HardFault_Handler()
{ {
// Below not working (yet)
while (1); while (1);
// //asm("push {lr}");
// // 1. Get the stack pointer
// uint32_t *stack; uint32_t *stack;
// uint32_t lr; uint32_t lr;
// asm("\ asm("\
// tst lr, #4; \ tst lr, #4; \
// ite eq; \ ite eq; \
// mrseq %0, msp; \ mrseq %0, msp; \
// mrsne %0, psp; \ mrsne %0, psp; \
// mov %1, lr; \ mov %1, lr; \
// " : "=r" (stack), "=r" (lr)); " : "=r" (stack), "=r" (lr));
// //stack++;
// stack[7] |= (1 << 24); // Keep Thumb mode enabled // 2. Only attempt to recover from failed algorithm code
// if ((lr & 4) == 0 || run_status != RunStatus::Running)
// conversion_abort(); while (1);
//
// // TODO test lr and decide how to recover // 3. Post the failure and unload algorithm
// elf_entry = nullptr;
// //if (run_status == RunStatus::Converting) { EM.add(Error::ConversionAborted);
// stack[6] = stack[5]; // Escape from elf_entry code run_status = RunStatus::Recovering;
// //} else /*if (run_status == RunStatus::Recovered)*/ {
// // stack[6] = (uint32_t)main_loop & ~1; // Return to safety // 4. Make this exception return to point after algorithm exec.
// //} stack[6] = stack[5];
// stack[7] |= (1 << 24); // Ensure Thumb mode stays enabled
// //asm("pop {lr}; bx lr");
// asm("bx lr"); asm("mov lr, %0; bx lr" :: "r" (lr));
} }
} // extern "C" } // extern "C"

Write Preview
Loading…
Cancel
Save