WebSVN - Projects - Diff - Rev 2147 and 2212 - /Transportables_Koptertool/PKT/branches/branch


/****************************************************************/
/*                                                                                                                                                                                                                                                      */
/*                                                                       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               */
/*                                                                                                                                                                                                                                                      */
/****************************************************************/
 
//############################################################################
//# HISTORY  mymath.c
//#
//# 24.04.2013 OG
//# - chg: 'uint16_t atantab[T] PROGMEM' nach 'const uint16_t atantab[T] PROGMEM'
//#        wegen Compile-Error in AtmelStudio 6
//############################################################################
 
 
#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 int16_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};
 
 
 
int16_t c_cos_8192(int16_t angle)
{
        return (c_sin_8192(90 - angle));
}
 
 
 
int16_t c_sin_8192(int16_t angle)
{
        int8_t m, n;
 
        int16_t sinus;
 
        // 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);
}
 
// ----------------------------------------------------------------------------------------------
 
 
const int16_t arccos64[65] = {90,89,88,87,86, 85, 84, 83, 83, 82, 81, 80, 79, 78, 77, 76,
                          75, 74, 73, 72, 71, 71, 70, 69, 68, 67, 66, 65, 64, 63, 62,
                          61, 60, 59, 58, 57, 56, 55, 54, 53, 51, 50, 49, 48, 47, 45,
                          44, 43, 41, 40, 39, 37, 36, 34, 32, 31, 29, 27, 25, 23, 20,
                          18, 14, 10, 0};
 
 
 
int16_t c_arccos2(int32_t a, int32_t b)
{
        if(a>b) return(0);
        return(arccos64[64 * a / b]);
}
 
 
/* ----------------------------------------------------------------------------------------------
 
  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])
 
const uint16_t atantab[T] PROGMEM = {22101,13047,6894,3499,1756,879,440,220,110,55,27,14,7,3,2,1};
 
int16_t my_atan2(int32_t y, int32_t x)
{       unsigned char i;
        uint32_t xQ;
        int32_t yQ;
        uint32_t angle = 0;
        uint32_t tmp;
        double x1, y1;
 
        x1 = abs(x);
        y1 = abs(y);
 
        if (y1 == 0) {
                if (x < 0)
                        return (180);
                else
                        return 0;
        }
       
        if (x1 < y1) {
                x1 /= y1;
                y1 = 1;
        }
        else {
                y1 /= x1;
                x1 = 1;
        }
       
        xQ = SCALED(x1);
        yQ = SCALED(y1);
 
        for (i = 0; i < T-1; i++) {
                tmp = xQ >> i;
                if (yQ < 0) {
                        xQ -= yQ >> i;
                        yQ += tmp;
                        angle -= getAtanVal(i);
                }
                else {
                        xQ += yQ >> i;
                        yQ -= tmp;
                        angle += getAtanVal(i);
                }
        }
 
        angle = RAD_TO_DEG * angle/Q;
        if (x <= 0)
                angle = 180 - angle;
        if (y <= 0)
                return(-angle);
        return(angle);
}
 
 

Rev 2147	Rev 2212
1	/****************************************************************/	1	/****************************************************************/
2	/* */	2	/* */
3	/* NG-Video 5,8GHz */	3	/* NG-Video 5,8GHz */
4	/* */	4	/* */
5	/* Copyright (C) 2011 - gebad */	5	/* Copyright (C) 2011 - gebad */
6	/* */	6	/* */
7	/* This code is distributed under the GNU Public License */	7	/* This code is distributed under the GNU Public License */
8	/* which can be found at http://www.gnu.org/licenses/gpl.txt */	8	/* which can be found at http://www.gnu.org/licenses/gpl.txt */
9	/* */	9	/* */
10	/****************************************************************/	10	/****************************************************************/
11		11
12	//############################################################################	12	//############################################################################
13	//# HISTORY mymath.c	13	//# HISTORY mymath.c
14	//#	14	//#
15	//# 24.04.2013 OG	15	//# 24.04.2013 OG
16	//# - chg: 'uint16_t atantab[T] PROGMEM' nach 'const uint16_t atantab[T] PROGMEM'	16	//# - chg: 'uint16_t atantab[T] PROGMEM' nach 'const uint16_t atantab[T] PROGMEM'
17	//# wegen Compile-Error in AtmelStudio 6	17	//# wegen Compile-Error in AtmelStudio 6
18	//############################################################################	18	//############################################################################
19		19
20		20
21	#include <stdio.h>	21	#include <stdio.h>
22	#include <stdlib.h>	22	#include <stdlib.h>
23	#include <math.h>	23	#include <math.h>
24	#include <avr/pgmspace.h>	24	#include <avr/pgmspace.h>
25		25
26	#include "mymath.h"	26	#include "mymath.h"
27		27
28	// http://www.mikrocontroller.net/articles/AVR_Arithmetik#avr-gcc_Implementierung_.2832_Bit.29	28	// http://www.mikrocontroller.net/articles/AVR_Arithmetik#avr-gcc_Implementierung_.2832_Bit.29
29	unsigned int sqrt32(uint32_t q)	29	unsigned int sqrt32(uint32_t q)
30	{ uint16_t r, mask;	30	{ uint16_t r, mask;
31	uint16_t i = 8*sizeof (r) -1;	31	uint16_t i = 8*sizeof (r) -1;
32		32
33	r = mask = 1 << i;	33	r = mask = 1 << i;
34	for (; i > 0; i--) {	34	for (; i > 0; i--) {
35	mask >>= 1;	35	mask >>= 1;
36		36
37	if (q < (uint32_t) r*r)	37	if (q < (uint32_t) r*r)
38	r -= mask;	38	r -= mask;
39	else	39	else
40	r += mask;	40	r += mask;
41	}	41	}
42		42
43	if (q < (uint32_t) r*r)	43	if (q < (uint32_t) r*r)
44	r -= 1;	44	r -= 1;
45		45
46	return r;	46	return r;
47	}	47	}
48		48
49	// aus Orginal MK source code	49	// aus Orginal MK source code
50	// sinus with argument in degree at an angular resolution of 1 degree and a discretisation of 13 bit.	50	// sinus with argument in degree at an angular resolution of 1 degree and a discretisation of 13 bit.
51	const uint16_t sinlookup[91] = {0, 143, 286, 429, 571, 714, 856, 998, 1140, 1282, 1423, 1563, 1703, \	51	const int16_t sinlookup[91] = { 0, 143, 286, 429, 571, 714, 856, 998, 1140, 1282, 1423, 1563, 1703, \
52	1843, 1982, 2120, 2258, 2395, 2531, 2667, 2802, 2936, 3069, 3201, 3332, \	52	1843, 1982, 2120, 2258, 2395, 2531, 2667, 2802, 2936, 3069, 3201, 3332, \
53	3462, 3591, 3719, 3846, 3972, 4096, 4219, 4341, 4462, 4581, 4699, 4815, \	53	3462, 3591, 3719, 3846, 3972, 4096, 4219, 4341, 4462, 4581, 4699, 4815, \
54	4930, 5043, 5155, 5266, 5374, 5482, 5587, 5691, 5793, 5893, 5991, 6088, \	54	4930, 5043, 5155, 5266, 5374, 5482, 5587, 5691, 5793, 5893, 5991, 6088, \
55	6183, 6275, 6366, 6455, 6542, 6627, 6710, 6791, 6870, 6947, 7022, 7094, \	55	6183, 6275, 6366, 6455, 6542, 6627, 6710, 6791, 6870, 6947, 7022, 7094, \
56	7165, 7233, 7299, 7363, 7424, 7484, 7541, 7595, 7648, 7698, 7746, 7791, \	56	7165, 7233, 7299, 7363, 7424, 7484, 7541, 7595, 7648, 7698, 7746, 7791, \
57	7834, 7875, 7913, 7949, 7982, 8013, 8041, 8068, 8091, 8112, 8131, 8147, \	57	7834, 7875, 7913, 7949, 7982, 8013, 8041, 8068, 8091, 8112, 8131, 8147, \
58	8161, 8172, 8181, 8187, 8191, 8192};	58	8161, 8172, 8181, 8187, 8191, 8192};
59		-
-		59
60	//double c_cos_8192(int16_t angle)	60
61	int16_t c_cos_8192(int16_t angle)	61	int16_t c_cos_8192(int16_t angle)
-		62	{
-		63	return (c_sin_8192(90 - angle));
-		64	}
-		65
-		66
-		67
-		68	int16_t c_sin_8192(int16_t angle)
62	{	69	{
63	int8_t m,n;	70	int8_t m, n;
64	int16_t sinus;	-
65		71	int16_t sinus;
66	angle = 90 - angle;	72
67	// avoid negative angles	73	// avoid negative angles
68	if (angle < 0)	74	if (angle < 0)
69	{	75	{
70	m = -1;	76	m = -1;
71	angle = -angle;	77	angle = -angle;
72	}	78	}
73	else m = +1;	79	else m = +1;
74		80
75	// fold angle to interval 0 to 359	81	// fold angle to interval 0 to 359
76	angle %= 360;	82	angle %= 360;
77		83
78	// check quadrant	84	// check quadrant
79	if (angle <= 90) n = 1; // first quadrant	85	if (angle <= 90) n = 1; // first quadrant
80	else if ((angle > 90) && (angle <= 180)) {angle = 180 - angle; n = 1;} // second quadrant	86	else if ((angle > 90) && (angle <= 180)) {angle = 180 - angle; n = 1;} // second quadrant
81	else if ((angle > 180) && (angle <= 270)) {angle = angle - 180; n = -1;} // third quadrant	87	else if ((angle > 180) && (angle <= 270)) {angle = angle - 180; n = -1;} // third quadrant
82	else {angle = 360 - angle; n = -1;} //fourth quadrant	88	else {angle = 360 - angle; n = -1;} //fourth quadrant
83	// get lookup value	89	// get lookup value
84	sinus = sinlookup[angle];	90	sinus = sinlookup[angle];
85	// calculate sinus value	91	// calculate sinus value
86	return (sinus * m * n);	92	return (sinus * m * n);
87	}	93	}
88		94
89	// ----------------------------------------------------------------------------------------------	95	// ----------------------------------------------------------------------------------------------
90		96
91		97
92	const int16_t arccos64[65] = {90,89,88,87,86, 85, 84, 83, 83, 82, 81, 80, 79, 78, 77, 76,	98	const int16_t arccos64[65] = {90,89,88,87,86, 85, 84, 83, 83, 82, 81, 80, 79, 78, 77, 76,
93	75, 74, 73, 72, 71, 71, 70, 69, 68, 67, 66, 65, 64, 63, 62,	99	75, 74, 73, 72, 71, 71, 70, 69, 68, 67, 66, 65, 64, 63, 62,
94	61, 60, 59, 58, 57, 56, 55, 54, 53, 51, 50, 49, 48, 47, 45,	100	61, 60, 59, 58, 57, 56, 55, 54, 53, 51, 50, 49, 48, 47, 45,
95	44, 43, 41, 40, 39, 37, 36, 34, 32, 31, 29, 27, 25, 23, 20,	101	44, 43, 41, 40, 39, 37, 36, 34, 32, 31, 29, 27, 25, 23, 20,
96	18, 14, 10, 0};	102	18, 14, 10, 0};
97		103
98		104
99		105
100	int16_t c_arccos2(int32_t a, int32_t b)	106	int16_t c_arccos2(int32_t a, int32_t b)
101	{	107	{
102	if(a>b) return(0);	108	if(a>b) return(0);
103	return(arccos64[64 * a / b]);	109	return(arccos64[64 * a / b]);
104	}	110	}
105		111
106		112
107	/* ----------------------------------------------------------------------------------------------	113	/* ----------------------------------------------------------------------------------------------
108		114
109	atan2.S	115	atan2.S
110		116
111	Author: Reiner Patommel	117	Author: Reiner Patommel
112		118
113	atan2.S uses CORDIC, an algorithm which was developed in 1956 by Jack Volder.	119	atan2.S uses CORDIC, an algorithm which was developed in 1956 by Jack Volder.
114	CORDIC can be used to calculate trigonometric, hyperbolic and linear	120	CORDIC can be used to calculate trigonometric, hyperbolic and linear
115	functions and is until today implemented in most pocket calculators.	121	functions and is until today implemented in most pocket calculators.
116	The algorithm makes only use of simple integer arithmetic.	122	The algorithm makes only use of simple integer arithmetic.
117		123
118	The CORDIC equations in vectoring mode for trigonometric functions are:	124	The CORDIC equations in vectoring mode for trigonometric functions are:
119	Xi+1 = Xi - Si * (Yi * 1 / 2^i)	125	Xi+1 = Xi - Si * (Yi * 1 / 2^i)
120	Yi+1 = Yi + Si * (Xi * 1 / 2^i)	126	Yi+1 = Yi + Si * (Xi * 1 / 2^i)
121	Zi+1 = Zi - Si * atan(1 / 2^i)	127	Zi+1 = Zi - Si * atan(1 / 2^i)
122	where Si = +1 if Yi < 0 else Si = -1	128	where Si = +1 if Yi < 0 else Si = -1
123	The rotation angles are limited to -PI/2 and PI/2 i.e.	129	The rotation angles are limited to -PI/2 and PI/2 i.e.
124	-90 degrees ... +90 degrees	130	-90 degrees ... +90 degrees
125		131
126	For the calculation of atan2(x,y) we need a small pre-calculated table of	132	For the calculation of atan2(x,y) we need a small pre-calculated table of
127	angles or radians with the values Tn = atan(1/2^i).	133	angles or radians with the values Tn = atan(1/2^i).
128	We rotate the vector(Xo,Yo) in steps to the x-axis i.e. we drive Y to 0 and	134	We rotate the vector(Xo,Yo) in steps to the x-axis i.e. we drive Y to 0 and
129	accumulate the rotated angles or radians in Z. The direction of the rotation	135	accumulate the rotated angles or radians in Z. The direction of the rotation
130	will be positive or negative depending on the sign of Y after the previous	136	will be positive or negative depending on the sign of Y after the previous
131	rotation and the rotation angle will decrease from step to step. (The first	137	rotation and the rotation angle will decrease from step to step. (The first
132	step is 45 degrees, the last step is 0.002036 degrees for n = 15).	138	step is 45 degrees, the last step is 0.002036 degrees for n = 15).
133		139
134	After n rotations the variables will have the following values:	140	After n rotations the variables will have the following values:
135	Yn = ideally 0	141	Yn = ideally 0
136	Xn = c*sqrt(Xo^2+Yo^2) (magnitude of the vector)	142	Xn = c*sqrt(Xo^2+Yo^2) (magnitude of the vector)
137	Zn = Zo+atan(Yo/Xo) (accumulated rotation angles or radians)	143	Zn = Zo+atan(Yo/Xo) (accumulated rotation angles or radians)
138		144
139	c, the cordic gain, is the product Pn of sqrt(1+2^(-2i)) and amounts to	145	c, the cordic gain, is the product Pn of sqrt(1+2^(-2i)) and amounts to
140	approx. 1.64676 for n = 15.	146	approx. 1.64676 for n = 15.
141		147
142	In order to represent X, Y and Z as integers we introduce a scaling factor Q	148	In order to represent X, Y and Z as integers we introduce a scaling factor Q
143	which is chosen as follows:	149	which is chosen as follows:
144	1. We normalize Xo and Yo (starting values) to be less than or equal to 1*Q and	150	1. We normalize Xo and Yo (starting values) to be less than or equal to 1*Q and
145	set Zo = 0 i.e. Xomax = 1Q, Yomax = 1Q, Zo = 0.	151	set Zo = 0 i.e. Xomax = 1Q, Yomax = 1Q, Zo = 0.
146	2. With Xo = 1Q and Yo = 1Q, Xn will be Xnmax = Qcsqrt(2) = 2.329*Q	152	2. With Xo = 1Q and Yo = 1Q, Xn will be Xnmax = Qcsqrt(2) = 2.329*Q
147	3. In order to boost accuracy we only cover the rotation angles between 0 and PI/2	153	3. In order to boost accuracy we only cover the rotation angles between 0 and PI/2
148	i.e. X and Z are always > 0 and therefore can be unsigned.	154	i.e. X and Z are always > 0 and therefore can be unsigned.
149	(We do the quadrant correction afterwards based on the initial signs of x and y)	155	(We do the quadrant correction afterwards based on the initial signs of x and y)
150	4. If X is an unsigned int, Xnmax = 65536, and Q = 65536/2.329 = 28140.	156	4. If X is an unsigned int, Xnmax = 65536, and Q = 65536/2.329 = 28140.
151	i.e.	157	i.e.
152	Xnmax = 65536 --> unsigned int	158	Xnmax = 65536 --> unsigned int
153	Ynmax = +/- 28140 --> signed int	159	Ynmax = +/- 28140 --> signed int
154	Znmax = PI/2 * 28140 = 44202 --> unsigned int	160	Znmax = PI/2 * 28140 = 44202 --> unsigned int
155	The systematic error is 90/44202 = 0.002036 degrees or 0.0000355 rad.	161	The systematic error is 90/44202 = 0.002036 degrees or 0.0000355 rad.
156		162
157	Below is atan2 and atan in C: */	163	Below is atan2 and atan in C: */
158		164
159	#define getAtanVal(x) (uint16_t)pgm_read_word(&atantab[x])	165	#define getAtanVal(x) (uint16_t)pgm_read_word(&atantab[x])
160		166
161	const uint16_t atantab[T] PROGMEM = {22101,13047,6894,3499,1756,879,440,220,110,55,27,14,7,3,2,1};	167	const uint16_t atantab[T] PROGMEM = {22101,13047,6894,3499,1756,879,440,220,110,55,27,14,7,3,2,1};
162		168
163	int16_t my_atan2(int32_t y, int32_t x)	169	int16_t my_atan2(int32_t y, int32_t x)
164	{ unsigned char i;	170	{ unsigned char i;
165	uint32_t xQ;	171	uint32_t xQ;
166	int32_t yQ;	172	int32_t yQ;
167	uint32_t angle = 0;	173	uint32_t angle = 0;
168	uint32_t tmp;	174	uint32_t tmp;
169	double x1, y1;	175	double x1, y1;
170		176
171	x1 = abs(x);	177	x1 = abs(x);
172	y1 = abs(y);	178	y1 = abs(y);
173		179
174	if (y1 == 0) {	180	if (y1 == 0) {
175	if (x < 0)	181	if (x < 0)
176	return (180);	182	return (180);
177	else	183	else
178	return 0;	184	return 0;
179	}	185	}
180		186
181	if (x1 < y1) {	187	if (x1 < y1) {
182	x1 /= y1;	188	x1 /= y1;
183	y1 = 1;	189	y1 = 1;
184	}	190	}
185	else {	191	else {
186	y1 /= x1;	192	y1 /= x1;
187	x1 = 1;	193	x1 = 1;
188	}	194	}
189		195
190	xQ = SCALED(x1);	196	xQ = SCALED(x1);
191	yQ = SCALED(y1);	197	yQ = SCALED(y1);
192		198
193	for (i = 0; i < T-1; i++) {	199	for (i = 0; i < T-1; i++) {
194	tmp = xQ >> i;	200	tmp = xQ >> i;
195	if (yQ < 0) {	201	if (yQ < 0) {
196	xQ -= yQ >> i;	202	xQ -= yQ >> i;
197	yQ += tmp;	203	yQ += tmp;
198	angle -= getAtanVal(i);	204	angle -= getAtanVal(i);
199	}	205	}
200	else {	206	else {
201	xQ += yQ >> i;	207	xQ += yQ >> i;
202	yQ -= tmp;	208	yQ -= tmp;
203	angle += getAtanVal(i);	209	angle += getAtanVal(i);
204	}	210	}
205	}	211	}
206		212
207	angle = RAD_TO_DEG * angle/Q;	213	angle = RAD_TO_DEG * angle/Q;
208	if (x <= 0)	214	if (x <= 0)
209	angle = 180 - angle;	215	angle = 180 - angle;
210	if (y <= 0)	216	if (y <= 0)
211	return(-angle);	217	return(-angle);
212	return(angle);	218	return(angle);
213	}	219	}
214		220
215		221

Subversion Repositories Projects

(root)/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/gps/mymath.c - Rev 2147 → 2212