1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
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;
}
}
|
