Rev 51 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
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 |