error recv. draft; add trig/math funcs

pull/3/head
Clyne 4 years ago
parent a4d9689259
commit e132d52adf

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

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

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

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

Loading…
Cancel
Save