Blame |
Last modification |
View Log
| RSS feed
/****************************************************************/
/* */
/* NG-Video 5,8GHz */
/* */
/* Copyright (C) 2011 - gebad */
/* */
/* This code is distributed under the GNU Public License */
/* which can be found at http://www.gnu.org/licenses/gpl.txt */
/* */
/****************************************************************/
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <avr/pgmspace.h>
#include "mymath.h"
// http://www.mikrocontroller.net/articles/AVR_Arithmetik#avr-gcc_Implementierung_.2832_Bit.29
unsigned int sqrt32
(uint32_t q
)
{ uint16_t r
, mask
;
uint16_t i
= 8*sizeof (r
) -1;
r
= mask
= 1 << i
;
for (; i
> 0; i
--) {
mask
>>= 1;
if (q
< (uint32_t) r
*r
)
r
-= mask
;
else
r
+= mask
;
}
if (q
< (uint32_t) r
*r
)
r
-= 1;
return r
;
}
// aus Orginal MK source code
// sinus with argument in degree at an angular resolution of 1 degree and a discretisation of 13 bit.
const uint16_t sinlookup
[91] = {0, 143, 286, 429, 571, 714, 856, 998, 1140, 1282, 1423, 1563, 1703, \
1843, 1982, 2120, 2258, 2395, 2531, 2667, 2802, 2936, 3069, 3201, 3332, \
3462, 3591, 3719, 3846, 3972, 4096, 4219, 4341, 4462, 4581, 4699, 4815, \
4930, 5043, 5155, 5266, 5374, 5482, 5587, 5691, 5793, 5893, 5991, 6088, \
6183, 6275, 6366, 6455, 6542, 6627, 6710, 6791, 6870, 6947, 7022, 7094, \
7165, 7233, 7299, 7363, 7424, 7484, 7541, 7595, 7648, 7698, 7746, 7791, \
7834, 7875, 7913, 7949, 7982, 8013, 8041, 8068, 8091, 8112, 8131, 8147, \
8161, 8172, 8181, 8187, 8191, 8192};
//double c_cos_8192(int16_t angle)
int16_t c_cos_8192
(int16_t angle
)
{
int8_t m
,n
;
int16_t sinus
;
angle
= 90 - angle
;
// avoid negative angles
if (angle
< 0)
{
m
= -1;
angle
= -angle
;
}
else m
= +1;
// fold angle to interval 0 to 359
angle
%= 360;
// check quadrant
if (angle
<= 90) n
= 1; // first quadrant
else if ((angle
> 90) && (angle
<= 180)) {angle
= 180 - angle
; n
= 1;} // second quadrant
else if ((angle
> 180) && (angle
<= 270)) {angle
= angle
- 180; n
= -1;} // third quadrant
else {angle
= 360 - angle
; n
= -1;} //fourth quadrant
// get lookup value
sinus
= sinlookup
[angle
];
// calculate sinus value
return (sinus
* m
* n
);
}
/* ----------------------------------------------------------------------------------------------
atan2.S
Author: Reiner Patommel
atan2.S uses CORDIC, an algorithm which was developed in 1956 by Jack Volder.
CORDIC can be used to calculate trigonometric, hyperbolic and linear
functions and is until today implemented in most pocket calculators.
The algorithm makes only use of simple integer arithmetic.
The CORDIC equations in vectoring mode for trigonometric functions are:
Xi+1 = Xi - Si * (Yi * 1 / 2^i)
Yi+1 = Yi + Si * (Xi * 1 / 2^i)
Zi+1 = Zi - Si * atan(1 / 2^i)
where Si = +1 if Yi < 0 else Si = -1
The rotation angles are limited to -PI/2 and PI/2 i.e.
-90 degrees ... +90 degrees
For the calculation of atan2(x,y) we need a small pre-calculated table of
angles or radians with the values Tn = atan(1/2^i).
We rotate the vector(Xo,Yo) in steps to the x-axis i.e. we drive Y to 0 and
accumulate the rotated angles or radians in Z. The direction of the rotation
will be positive or negative depending on the sign of Y after the previous
rotation and the rotation angle will decrease from step to step. (The first
step is 45 degrees, the last step is 0.002036 degrees for n = 15).
After n rotations the variables will have the following values:
Yn = ideally 0
Xn = c*sqrt(Xo^2+Yo^2) (magnitude of the vector)
Zn = Zo+atan(Yo/Xo) (accumulated rotation angles or radians)
c, the cordic gain, is the product Pn of sqrt(1+2^(-2i)) and amounts to
approx. 1.64676 for n = 15.
In order to represent X, Y and Z as integers we introduce a scaling factor Q
which is chosen as follows:
1. We normalize Xo and Yo (starting values) to be less than or equal to 1*Q and
set Zo = 0 i.e. Xomax = 1*Q, Yomax = 1*Q, Zo = 0.
2. With Xo = 1*Q and Yo = 1*Q, Xn will be Xnmax = Q*c*sqrt(2) = 2.329*Q
3. In order to boost accuracy we only cover the rotation angles between 0 and PI/2
i.e. X and Z are always > 0 and therefore can be unsigned.
(We do the quadrant correction afterwards based on the initial signs of x and y)
4. If X is an unsigned int, Xnmax = 65536, and Q = 65536/2.329 = 28140.
i.e.
Xnmax = 65536 --> unsigned int
Ynmax = +/- 28140 --> signed int
Znmax = PI/2 * 28140 = 44202 --> unsigned int
The systematic error is 90/44202 = 0.002036 degrees or 0.0000355 rad.
Below is atan2 and atan in C: */
#define getAtanVal(x) (uint16_t)pgm_read_word(&atantab[x])
//uint16_t atantab[T] PROGMEM = {22101,13047,6894,3499,1756,879,440,220,110,55,27,14,7,3,2,1}; // T 16
// Genauigkeit etwas eingeschränkt, da Servos nur in 1° Abständen positioniert werden
uint16_t atantab
[T
] PROGMEM
= {22101,13047,6894,3499,1756,879,440,220,110,55,27,14,7,3};
// max. Wert für y oder x ist 76314
int16_t my_atan2
(int32_t y
, int32_t x
)
{ unsigned char i
;
uint32_t x1
;
int32_t y1
;
uint32_t angle
= 0;
uint32_t tmp
;
x1
= abs(x
);
y1
= abs(y
);
if (y1
== 0) {
if (x
< 0)
return (180);
else
return 0;
}
if (x1
< y1
) {
x1
= SCALED
(x1
) / y1
;
y1
= Q
;
}
else {
y1
= SCALED
(y1
) / x1
;
x1
= Q
;
}
for (i
= 0; i
< T
-1; i
++) {
tmp
= x1
>> i
;
if (y1
< 0) {
x1
-= y1
>> i
;
y1
+= tmp
;
angle
-= getAtanVal
(i
);
}
else {
x1
+= y1
>> i
;
y1
-= tmp
;
angle
+= getAtanVal
(i
);
}
}
angle
= angle
/RQ
; // RAD_TO_DEG * angle / Q
if (x
<= 0)
angle
= 180 - angle
;
if (y
<= 0)
return(-angle
);
return(angle
);
}