WebSVN - Projects - Blame - /Transportables_Koptertool/PKT/GPL_PKT_V3

Rev	Author	Line No.	Line
2136	-	1	/****************************************************************/
		2	/* */
		3	/* NG-Video 5,8GHz */
		4	/* */
		5	/* Copyright (C) 2011 - gebad */
		6	/* */
		7	/* This code is distributed under the GNU Public License */
		8	/* which can be found at http://www.gnu.org/licenses/gpl.txt */
		9	/* */
		10	/****************************************************************/
		11
		12	//############################################################################
		13	//# HISTORY mymath.c
		14	//#
		15	//# 24.04.2013 OG
		16	//# - chg: 'uint16_t atantab[T] PROGMEM' nach 'const uint16_t atantab[T] PROGMEM'
		17	//# wegen Compile-Error in AtmelStudio 6
		18	//############################################################################
		19
		20
		21	#include <stdio.h>
		22	#include <stdlib.h>
		23	#include <math.h>
		24	#include <avr/pgmspace.h>
		25
		26	#include "mymath.h"
		27
		28	// http://www.mikrocontroller.net/articles/AVR_Arithmetik#avr-gcc_Implementierung_.2832_Bit.29
		29	unsigned int sqrt32(uint32_t q)
		30	{ uint16_t r, mask;
		31	uint16_t i = 8*sizeof (r) -1;
		32
		33	r = mask = 1 << i;
		34	for (; i > 0; i--) {
		35	mask >>= 1;
		36
		37	if (q < (uint32_t) r*r)
		38	r -= mask;
		39	else
		40	r += mask;
		41	}
		42
		43	if (q < (uint32_t) r*r)
		44	r -= 1;
		45
		46	return r;
		47	}
		48
		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.
		51	const uint16_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, \
		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, \
		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, \
		57	7834, 7875, 7913, 7949, 7982, 8013, 8041, 8068, 8091, 8112, 8131, 8147, \
		58	8161, 8172, 8181, 8187, 8191, 8192};
		59
		60	//double c_cos_8192(int16_t angle)
		61	int16_t c_cos_8192(int16_t angle)
		62	{
		63	int8_t m,n;
		64	int16_t sinus;
		65
		66	angle = 90 - angle;
		67	// avoid negative angles
		68	if (angle < 0)
		69	{
		70	m = -1;
		71	angle = -angle;
		72	}
		73	else m = +1;
		74
		75	// fold angle to interval 0 to 359
		76	angle %= 360;
		77
		78	// check quadrant
		79	if (angle <= 90) n = 1; // first quadrant
		80	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
		82	else {angle = 360 - angle; n = -1;} //fourth quadrant
		83	// get lookup value
		84	sinus = sinlookup[angle];
		85	// calculate sinus value
		86	return (sinus * m * n);
		87	}
		88
		89	// ----------------------------------------------------------------------------------------------
		90
		91
		92	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,
		94	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,
		96	18, 14, 10, 0};
		97
		98
		99
		100	int16_t c_arccos2(int32_t a, int32_t b)
		101	{
		102	if(a>b) return(0);
		103	return(arccos64[64 * a / b]);
		104	}
		105
		106
		107	/* ----------------------------------------------------------------------------------------------
		108
		109	atan2.S
		110
		111	Author: Reiner Patommel
		112
		113	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
		115	functions and is until today implemented in most pocket calculators.
		116	The algorithm makes only use of simple integer arithmetic.
		117
		118	The CORDIC equations in vectoring mode for trigonometric functions are:
		119	Xi+1 = Xi - Si * (Yi * 1 / 2^i)
		120	Yi+1 = Yi + Si * (Xi * 1 / 2^i)
		121	Zi+1 = Zi - Si * atan(1 / 2^i)
		122	where Si = +1 if Yi < 0 else Si = -1
		123	The rotation angles are limited to -PI/2 and PI/2 i.e.
		124	-90 degrees ... +90 degrees
		125
		126	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).
		128	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
		130	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
		132	step is 45 degrees, the last step is 0.002036 degrees for n = 15).
		133
		134	After n rotations the variables will have the following values:
		135	Yn = ideally 0
		136	Xn = c*sqrt(Xo^2+Yo^2) (magnitude of the vector)
		137	Zn = Zo+atan(Yo/Xo) (accumulated rotation angles or radians)
		138
		139	c, the cordic gain, is the product Pn of sqrt(1+2^(-2i)) and amounts to
		140	approx. 1.64676 for n = 15.
		141
		142	In order to represent X, Y and Z as integers we introduce a scaling factor Q
		143	which is chosen as follows:
		144	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.
		146	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
		148	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)
		150	4. If X is an unsigned int, Xnmax = 65536, and Q = 65536/2.329 = 28140.
		151	i.e.
		152	Xnmax = 65536 --> unsigned int
		153	Ynmax = +/- 28140 --> signed int
		154	Znmax = PI/2 * 28140 = 44202 --> unsigned int
		155	The systematic error is 90/44202 = 0.002036 degrees or 0.0000355 rad.
		156
		157	Below is atan2 and atan in C: */
		158
		159	#define getAtanVal(x) (uint16_t)pgm_read_word(&atantab[x])
		160
		161	const uint16_t atantab[T] PROGMEM = {22101,13047,6894,3499,1756,879,440,220,110,55,27,14,7,3,2,1};
		162
		163	int16_t my_atan2(int32_t y, int32_t x)
		164	{ unsigned char i;
		165	uint32_t xQ;
		166	int32_t yQ;
		167	uint32_t angle = 0;
		168	uint32_t tmp;
		169	double x1, y1;
		170
		171	x1 = abs(x);
		172	y1 = abs(y);
		173
		174	if (y1 == 0) {
		175	if (x < 0)
		176	return (180);
		177	else
		178	return 0;
		179	}
		180
		181	if (x1 < y1) {
		182	x1 /= y1;
		183	y1 = 1;
		184	}
		185	else {
		186	y1 /= x1;
		187	x1 = 1;
		188	}
		189
		190	xQ = SCALED(x1);
		191	yQ = SCALED(y1);
		192
		193	for (i = 0; i < T-1; i++) {
		194	tmp = xQ >> i;
		195	if (yQ < 0) {
		196	xQ -= yQ >> i;
		197	yQ += tmp;
		198	angle -= getAtanVal(i);
		199	}
		200	else {
		201	xQ += yQ >> i;
		202	yQ -= tmp;
		203	angle += getAtanVal(i);
		204	}
		205	}
		206
		207	angle = RAD_TO_DEG * angle/Q;
		208	if (x <= 0)
		209	angle = 180 - angle;
		210	if (y <= 0)
		211	return(-angle);
		212	return(angle);
		213	}
		214

Subversion Repositories Projects

(root)/Transportables_Koptertool/PKT/GPL_PKT_V3_85f/gps/mymath.c @ 2139 - Rev