WebSVN - FlightCtrl - Diff - Rev 51 and 125 - /branches/V0.60_GPS_BETA

Rev 51		Rev 125
1	//------------------------------------------------------------------------------	1	//------------------------------------------------------------------------------
2	// _ _	2	// _ _
3	// \| \| \| \|	3	// \| \| \| \|
4	// ___ _ __ ___ ___ _ _ ___\| \|_ ___ ___\| \|__	4	// ___ _ __ ___ ___ _ _ ___\| \|_ ___ ___\| \|__
5	// / _ \ '_ ` _ \/ __\| \| \| / __\| __/ _ \/ __\| '_ \.	5	// / _ \ '_ ` _ \/ __\| \| \| / __\| __/ _ \/ __\| '_ \.
6	// \| __/ \| \| \| \| \__ \ \|_\| \__ \ \|\| __/ (__\| \| \| \|	6	// \| __/ \| \| \| \| \__ \ \|_\| \__ \ \|\| __/ (__\| \| \| \|
7	// \___\|_\| \|_\| \|_\|___/\__, \|___/\__\___\|\___\|_\| \|_\|	7	// \___\|_\| \|_\| \|_\|___/\__, \|___/\__\___\|\___\|_\| \|_\|
8	// __/ \|	8	// __/ \|
9	// \|___/ Engineering	9	// \|___/ Engineering
10	//	10	//
11	// Filename: GPS.c	11	// Filename: GPS.c
12	// Description:	12	// Description:
13	//	13	//
14	// Author: Martin Steppuhn	14	// Author: Martin Steppuhn
15	// History: 15.06.2007 Initial version	15	// History: 15.06.2007 Initial version
16	//	16	//
17	//------------------------------------------------------------------------------	17	//------------------------------------------------------------------------------
18		18
19		19
20	/** Includes **************************************************************/	20	/** Includes **************************************************************/
21		21
22	#include "std_c.h"	22	#include "std_c.h"
23	#include "main.h"	23	#include "main.h"
24	#include "fc.h"	24	#include "fc.h"
25	#include "gps_ubx.h"	25	#include "gps_ubx.h"
26		26
27	/** Preprocessing directives (#define) ************************************/	27	/** Preprocessing directives (#define) ************************************/
28		28
29	/** Type definitions (typedef) ********************************************/	29	/** Type definitions (typedef) ********************************************/
30		30
31	/** Global constants ******************************************************/	31	/** Global constants ******************************************************/
32		32
33	/** Global variables ******************************************************/	33	/** Global variables ******************************************************/
34		34
35	signed int GPS_Nick = 0;	35	signed int GPS_Nick = 0;
36	signed int GPS_Roll = 0;	36	signed int GPS_Roll = 0;
37		37
38	/** Local constants ******************************************************/	38	/** Local constants ******************************************************/
39		39
40	/** Local variables *******************************************************/	40	/** Local variables *******************************************************/
41		41
42	signed int test_nick = 0;	42	signed int test_nick = 0;
43	signed int test_roll = 0;	43	signed int test_roll = 0;
44	long GpsAktuell_X = 0;	44	long GpsAktuell_X = 0;
45	long GpsAktuell_Y = 0;	45	long GpsAktuell_Y = 0;
-		46	long GpsAktuell_Z = 0;
46	long GpsZiel_X = 0;	47	long GpsZiel_X = 0;
47	long GpsZiel_Y = 0;	48	long GpsZiel_Y = 0;
48	long gps_dx;	49	long gps_dx;
49	long gps_dy;	50	long gps_dy;
-		51	long gps_dz;
50	long gps_vx;	52	long gps_vx;
51	long gps_vy;	53	long gps_vy;
-		54	long gps_vz;
52	bool gps_hold;	55	bool gps_hold;
53	uint8 print_pos;	56	uint8 print_pos;
54		57
55	/** Local function prototypes *********************************************/	58	/** Local function prototypes *********************************************/
56		59
57	void print_uint16(uint16 value,uint8 width);	60	void print_uint16(uint16 value,uint8 width);
58	void print_int16(int16 value,uint8 width);	61	void print_int16(int16 value,uint8 width);
59	void print_string(char *s);	62	void print_string(char *s);
60		63
61	//------------------------------------------------------------------------------	64	//------------------------------------------------------------------------------
62	// Name: GPS_Neutral	65	// Name: GPS_Neutral
63	// Function:	66	// Function:
64	//	67	//
65	// Parameter:	68	// Parameter:
66	// Return:	69	// Return:
67	//------------------------------------------------------------------------------	70	//------------------------------------------------------------------------------
68	void GPS_Neutral(void)	71	void GPS_Neutral(void)
69	{	72	{
70	GpsZiel_X = GpsAktuell_X;	73	GpsZiel_X = GpsAktuell_X;
71	GpsZiel_Y = GpsAktuell_Y;	74	GpsZiel_Y = GpsAktuell_Y;
72		75
73	GPS_Nick = 0;	76	GPS_Nick = 0;
74	GPS_Roll = 0;	77	GPS_Roll = 0;
75		78
76	// LED_GPS_DATA_OFF;	79	// LED_GPS_DATA_OFF;
77	//LED_GPS_FIX_OFF;	80	//LED_GPS_FIX_OFF;
78	//LED_GPS_DATA_ON;	81	//LED_GPS_DATA_ON;
79		82
80	}	83	}
81		84
82	//------------------------------------------------------------------------------	85	//------------------------------------------------------------------------------
83	// Name: gps_trace	86	// Name: gps_trace
84	// Function:	87	// Function:
85	//	88	//
86	// Parameter:	89	// Parameter:
87	// Return:	90	// Return:
88	//------------------------------------------------------------------------------	91	//------------------------------------------------------------------------------
89	void gps_trace(void)	92	void gps_trace(void)
90	{	93	{
91	print_pos = 0;	94	print_pos = 0;
92		95
93	print_uint16(nav_sol.gpsfix,1);	96	print_uint16(nav_sol.gpsfix,1);
94	print_string(" ");	97	print_string(" ");
95	print_int16(gps_dy,7);	98	print_int16(gps_dy,7);
96	print_int16(gps_dx,7);	99	print_int16(gps_dx,7);
97		100
98	print_string(" \| ");	101	print_string(" \| ");
99		102
100	print_int16(gps_vy,7);	103	print_int16(gps_vy,7);
101	print_int16(gps_vx,7);	104	print_int16(gps_vx,7);
102		105
103	print_string(" \| ");	106	print_string(" \| ");
104		107
105	print_int16(GPS_Roll,7);	108	print_int16(GPS_Roll,7);
106	print_int16(GPS_Nick,7);	109	print_int16(GPS_Nick,7);
107		110
108		111
109	print_string(" # ");	112	print_string(" # ");
110		113
111	print_int16(StickRoll,7);	114	print_int16(StickRoll,7);
112	print_int16(StickNick,7);	115	print_int16(StickNick,7);
113		116
114		117
115	print_string("\r\n");	118	print_string("\r\n");
116	UebertragungAbgeschlossen = 0;	119	UebertragungAbgeschlossen = 0;
117	UDR = SendeBuffer[0]; // Start	120	UDR = SendeBuffer[0]; // Start
118	}	121	}
119		122
120	//------------------------------------------------------------------------------	123	//------------------------------------------------------------------------------
121	// Name: gps_main	124	// Name: gps_main
122	// Function:	125	// Function:
123	//	126	//
124	// Parameter:	127	// Parameter:
125	// Return:	128	// Return:
126	//------------------------------------------------------------------------------	129	//------------------------------------------------------------------------------
127	void GPS_Main(void)	130	void GPS_Main(void)
128	{	131	{
129	if((Poti3 > 125) && (nav_sol.gpsfix) && (gps_hold == false)) // Enable Hold	132	if((Poti3 > 125) && (nav_sol.gpsfix) && (gps_hold == false)) // Enable Hold
130	{	133	{
131	GpsZiel_X = GpsAktuell_X;	134	GpsZiel_X = GpsAktuell_X;
132	GpsZiel_Y = GpsAktuell_Y;	135	GpsZiel_Y = GpsAktuell_Y;
133	gps_hold = true;	136	gps_hold = true;
134	}	137	}
135		138
136	if(Poti3 < 125) // Disable Hold	139	if(Poti3 < 125) // Disable Hold
137	{	140	{
138	GpsZiel_X = 0;	141	GpsZiel_X = 0;
139	GpsZiel_Y = 0;	142	GpsZiel_Y = 0;
140	GPS_Nick = 0;	143	GPS_Nick = 0;
141	GPS_Roll = 0;	144	GPS_Roll = 0;
142		145
143	gps_hold = false;	146	gps_hold = false;
144	}	147	}
145		148
146		149
147		150
148		151
149	if(ubx.update)	152	if(ubx.update)
150	{	153	{
151		154
152	ubx.update = false;	155	ubx.update = false;
153	ubx_decode(&ubx);	156	ubx_decode(&ubx);
154	if(nav_sol.update)	157	if(nav_sol.update)
155	{	158	{
156	//trace_nav_sol();	159	//trace_nav_sol();
157	nav_sol.update = false;	160	nav_sol.update = false;
158		161
159	if(nav_sol.gpsfix)	162	if(nav_sol.gpsfix)
160	{	163	{
161	LED_GPS_FIX_TOGGLE;	164	LED_GPS_FIX_TOGGLE;
162	LED_GPS_DATA_OFF;	165	LED_GPS_DATA_OFF;
163	}	166	}
164	else	167	else
165	{	168	{
166		169
167	LED_GPS_DATA_TOGGLE;	170	LED_GPS_DATA_TOGGLE;
168	LED_GPS_FIX_OFF;	171	LED_GPS_FIX_OFF;
169	}	172	}
170		173
171	//=== new Position ====================	174	//=== new Position ====================
172		175
173	GpsAktuell_X = nav_sol.ecef_x;	176	GpsAktuell_X = nav_sol.ecef_x;
174	GpsAktuell_Y = nav_sol.ecef_y;	177	GpsAktuell_Y = nav_sol.ecef_y;
175		178
176	gps_dx = GpsZiel_X - GpsAktuell_X; // Süden +	179	gps_dx = GpsZiel_X - GpsAktuell_X; // Süden +
177	gps_dy = GpsZiel_Y - GpsAktuell_Y; // Osten +	180	gps_dy = GpsZiel_Y - GpsAktuell_Y; // Osten +
178		181
179		182
180	gps_vx = -nav_sol.ecefvx; // cm/s ECEF X velocity	183	gps_vx = -nav_sol.ecefvx; // cm/s ECEF X velocity
181	gps_vy = -nav_sol.ecefvy; // cm/s ECEF Y velocity	184	gps_vy = -nav_sol.ecefvy; // cm/s ECEF Y velocity
182		185
183		186
184	if(gps_hold)	187	if(gps_hold)
185	{	188	{
186	GPS_Nick = ((gps_dx * Poti1) / 512) + ((gps_vx * Poti2) / 128);	189	GPS_Nick = ((gps_dx * Poti1) / 512) + ((gps_vx * Poti2) / 128);
187	GPS_Roll = ((gps_dy * Poti1) / 512) + ((gps_vy * Poti2) / 128);	190	GPS_Roll = ((gps_dy * Poti1) / 512) + ((gps_vy * Poti2) / 128);
188	}	191	}
189	else	192	else
190	{	193	{
191	GPS_Nick = 0;	194	GPS_Nick = 0;
192	GPS_Roll = 0;	195	GPS_Roll = 0;
193	}	196	}
194		197
195	//=====================================	198	//=====================================
196	}	199	}
197	gps_trace();	200	gps_trace();
198	}	201	}
199	}	202	}
200		203
201		204
202		205
203	//==============================================================================	206	//==============================================================================
204	//==============================================================================	207	//==============================================================================
205	//==============================================================================	208	//==============================================================================
206	//	209	//
207	//==============================================================================	210	//==============================================================================
208	//==============================================================================	211	//==============================================================================
209	//==============================================================================	212	//==============================================================================
210		213
211		214
212	//------------------------------------------------------------------------------	215	//------------------------------------------------------------------------------
213	// Name:	216	// Name:
214	// Function:	217	// Function:
215	//	218	//
216	// Parameter:	219	// Parameter:
217	// Return:	220	// Return:
218	//------------------------------------------------------------------------------	221	//------------------------------------------------------------------------------
219	void print_uint16(uint16 value,uint8 width)	222	void print_uint16(uint16 value,uint8 width)
220	{	223	{
221	uint8 i,s[10];	224	uint8 i,s[10];
222		225
223	for (i = 0; i < width; i++)	226	for (i = 0; i < width; i++)
224	{	227	{
225	s[width - i - 1] = '0' + (value % 10);	228	s[width - i - 1] = '0' + (value % 10);
226	value /= 10;	229	value /= 10;
227	}	230	}
228	for (i=0; i<(width - 1); i++) // Overwrite trailing Zeros	231	for (i=0; i<(width - 1); i++) // Overwrite trailing Zeros
229	{	232	{
230	if (s[i] == '0') s[i] = ' ';	233	if (s[i] == '0') s[i] = ' ';
231	else break;	234	else break;
232	}	235	}
233		236
234	for (i=0; i<width; i++) SendeBuffer[print_pos++] = s[i]; // Output String	237	for (i=0; i<width; i++) SendeBuffer[print_pos++] = s[i]; // Output String
235	// for (i=0; i<width; i++) putchar(s[i]); // Output String	238	// for (i=0; i<width; i++) putchar(s[i]); // Output String
236		239
237	}	240	}
238		241
239	//------------------------------------------------------------------------------	242	//------------------------------------------------------------------------------
240	// Name:	243	// Name:
241	// Function:	244	// Function:
242	//	245	//
243	// Parameter:	246	// Parameter:
244	// Return:	247	// Return:
245	//------------------------------------------------------------------------------	248	//------------------------------------------------------------------------------
246	void print_int16(int16 value,uint8 width)	249	void print_int16(int16 value,uint8 width)
247	{	250	{
248	uint8 i,s[10];	251	uint8 i,s[10];
249	bool neg;	252	bool neg;
250		253
251	neg = false;	254	neg = false;
252	if(value < 0) { value = -value; neg = true; }	255	if(value < 0) { value = -value; neg = true; }
253		256
254	for (i = 0; i < width; i++)	257	for (i = 0; i < width; i++)
255	{	258	{
256	s[width - i - 1] = '0' + (value % 10);	259	s[width - i - 1] = '0' + (value % 10);
257	value /= 10;	260	value /= 10;
258	}	261	}
259		262
260	for (i=0; i<(width - 1); i++) // Overwrite trailing Zeros	263	for (i=0; i<(width - 1); i++) // Overwrite trailing Zeros
261	{	264	{
262	if (s[i] == '0') s[i] = ' ';	265	if (s[i] == '0') s[i] = ' ';
263	else	266	else
264	{	267	{
265	if(neg) s[i-1] = '-';	268	if(neg) s[i-1] = '-';
266	break;	269	break;
267	}	270	}
268	}	271	}
269	for (i=0; i<width; i++) SendeBuffer[print_pos++] = s[i]; // Output String	272	for (i=0; i<width; i++) SendeBuffer[print_pos++] = s[i]; // Output String
270	// for (i=0; i<width; i++) putchar(s[i]); // Output String	273	// for (i=0; i<width; i++) putchar(s[i]); // Output String
271	}	274	}
272		275
273	//------------------------------------------------------------------------------	276	//------------------------------------------------------------------------------
274	// Name:	277	// Name:
275	// Function:	278	// Function:
276	//	279	//
277	// Parameter:	280	// Parameter:
278	// Return:	281	// Return:
279	//------------------------------------------------------------------------------	282	//------------------------------------------------------------------------------
280	void print_string(char *s)	283	void print_string(char *s)
281	{	284	{
282	while(*s)	285	while(*s)
283	{	286	{
284	SendeBuffer[print_pos++] = *s;	287	SendeBuffer[print_pos++] = *s;
285	s++;	288	s++;
286	}	289	}
287	}	290	}
288		291

Subversion Repositories FlightCtrl

(root)/branches/V0.60_GPS_BETA_martosi/GPS.c @ 2304 - Rev 51 → 125