Details | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
1932 | - | 1 | /* |
2 | * tracking.c |
||
3 | * |
||
4 | * Created on: 13.02.2012 |
||
5 | * Author: cebra |
||
6 | */ |
||
7 | |||
8 | #include "../cpu.h" |
||
9 | #include <string.h> |
||
10 | #include <util/delay.h> |
||
11 | #include <avr/interrupt.h> |
||
12 | #include <stdlib.h> |
||
13 | #include <math.h> |
||
14 | #include "../main.h" |
||
15 | #include "../tracking/tracking.h" |
||
16 | #include "../tracking/ng_servo.h" |
||
17 | #include <avr/pgmspace.h> |
||
18 | #include "../bluetooth/fifo.h" |
||
19 | #include "../bluetooth/bluetooth.h" |
||
20 | #include "../lcd/lcd.h" |
||
21 | |||
22 | #include "../mk-data-structs.h" |
||
23 | #include "tools.h" |
||
24 | #include "../messages.h" |
||
25 | #include "../lcd/lcd.h" |
||
26 | #include "../eeprom/eeprom.h" |
||
27 | #include "../timer/timer.h" |
||
28 | #include "../uart/uart1.h" |
||
29 | #include "../uart/usart.h" |
||
30 | #include "../osd/osd.h" |
||
31 | #include "../tracking/mymath.h" |
||
32 | #include "../setup/setup.h" |
||
33 | |||
34 | |||
35 | |||
36 | |||
37 | #ifdef HWVERSION3_9 |
||
38 | |||
39 | //#define MAX_POWER 10 |
||
40 | //#define getPower(x) (int32_t)pgm_read_dword(&powers[x]) |
||
41 | //const int32_t PROGMEM powers[MAX_POWER] = {1, 10, 100, 1000, 10000, 100000, 1000000, 10000000, 100000000, 1000000000}; |
||
42 | #define DLEFT 0 |
||
43 | #define DRIGHT 1 |
||
44 | #define DEG_TO_RAD 0.0174533 // degrees to radians = PI / 180 |
||
45 | #define RAD_TO_DEG 57.2957795 // radians to degrees = 180 / PI |
||
46 | #define AltFaktor 22.5 |
||
47 | #define TIMEOUT 200 // 2 sec |
||
48 | |||
49 | NaviData_t *naviData; |
||
50 | |||
51 | mk_param_struct_t *mk_param_struct; |
||
52 | |||
53 | HomePos_t MK_pos; // Home position of station |
||
54 | GPS_Pos_t currentPos; // Current position of flying object |
||
55 | uint8_t NMEAsatsInUse; // Number of satelites currently in use BT-Mouse |
||
56 | //uint8_t MKsatsInUse; // Number of satelites currently in use Mikrokopter |
||
57 | int32_t NMEAlatitude, NMEAlongitude; // Longitude, Latitude |
||
58 | int16_t NMEAaltitude; // Höhe in Meter |
||
59 | uint8_t posfix; // GPS Fix, 0 = Fix not available or invalid,1 = GPS SPS Mode, fix valid, |
||
60 | // 2 = Differential GPS, SPS Mode, fix valid, 6 = Dead Reckoning Mode, fix valid |
||
61 | int16_t HDOP; // Horizontal Dilution of Precision, 1.1 -xx.x, niederiger = besser |
||
62 | |||
63 | |||
64 | uint8_t tracking = TRACKING_MIN; |
||
65 | uint8_t track_hyst = TRACKING_HYSTERESE; |
||
66 | uint8_t track_tx =0; |
||
67 | uint8_t coldstart; // Flag erstmaliger MK-Start(Motore) nur nach GPS-Fix |
||
68 | geo_t geo; |
||
69 | int16_t anglePan, angleTilt; |
||
70 | |||
71 | |||
72 | char NMEAtime[9] = "GP:Ti:me"; |
||
73 | //char NMEADate [6]; |
||
74 | |||
75 | //// Berechnung von Distanz und Winkel aus GPS-Daten home(MK eingeschaltet) |
||
76 | //// zur aktuellen Position(nach Motorstart) |
||
77 | //geo_t calc_geo(HomePos_t *home, GPS_Pos_t *pos) |
||
78 | //{ double lat1, lon1, lat2, lon2, d1, dlat; |
||
79 | // geo_t geo; |
||
80 | // |
||
81 | // lon1 = MK_pos.Home_Lon; |
||
82 | // lat1 = MK_pos.Home_Lat; |
||
83 | // lon2 = (double)pos->Longitude / 10000000.0; |
||
84 | // lat2 = (double)pos->Latitude / 10000000.0; |
||
85 | // |
||
86 | // // Formel verwendet von http://www.kompf.de/gps/distcalc.html |
||
87 | // // 111.3 km = Abstand zweier Breitenkreise und/oder zweier Längenkreise am Äquator |
||
88 | // // es wird jedoch in Meter weiter gerechnet |
||
89 | // d1 = 111300 * (double)cos((double)(lat1 + lat2) / 2 * DEG_TO_RAD) * (lon1 - lon2); |
||
90 | // dlat = 111300 * (double)(lat1 - lat2); |
||
91 | // // returns a value in metres http://www.kompf.de/gps/distcalc.html |
||
92 | // geo.bearing = fmod((RAD_TO_DEG * (double)atan2(d1, dlat)) + 180, 360); // +180 besserer Vergleich mit MkCockpit |
||
93 | // if (geo.bearing > 360) geo.bearing -= 360; // bekam schon Werte über 400 |
||
94 | // geo.distance = sqrt(d1 * d1 + dlat * dlat); |
||
95 | // return(geo); |
||
96 | //} |
||
97 | |||
98 | // Berechnung von Distanz und Winkel aus GPS-Daten home(MK eingeschaltet) |
||
99 | // zur aktuellen Position(nach Motorstart) |
||
100 | geo_t calc_geo(HomePos_t *home, GPS_Pos_t *pos) |
||
101 | { int32_t lat1, lon1, lat2, lon2; |
||
102 | int32_t d1, dlat; |
||
103 | geo_t geo; |
||
104 | |||
105 | lon1 = home->Home_Lon; |
||
106 | lat1 = home->Home_Lat; |
||
107 | lon2 = pos->Longitude; |
||
108 | lat2 = pos->Latitude; |
||
109 | if (!CheckGPS) |
||
110 | { |
||
111 | lcd_puts_at (0, 3, my_itoa(home->Home_Lat, 10, 7, 7), 0); |
||
112 | lcd_puts_at (11, 3, my_itoa(home->Home_Lon, 10, 7, 7), 0); |
||
113 | lcd_puts_at (0, 4, my_itoa(pos->Latitude, 10, 7, 7), 0); |
||
114 | lcd_puts_at (11, 4, my_itoa(pos->Longitude, 10, 7, 7), 0); |
||
115 | } |
||
116 | // lcd_printp_at (0, 3, PSTR("H"), 0); |
||
117 | // lcd_printp_at (0, 4, PSTR("M"), 0); |
||
118 | |||
119 | // Formel verwendet von http://www.kompf.de/gps/distcalc.html |
||
120 | // 111.3 km = Abstand zweier Breitenkreise und/oder zweier Langenkreise am Äquator |
||
121 | // es wird jedoch in dm Meter weiter gerechnet |
||
122 | // (tlon1 - tlon2)/10) sonst uint32_t-Überlauf bei cos(0) gleich 1 |
||
123 | d1 = (1359 * (int32_t)(c_cos_8192((lat1 + lat2) / 20000000)) * ((lon1 - lon2)/10))/ 10000000; |
||
124 | dlat = 1113 * (lat1 - lat2) / 10000; |
||
125 | geo.bearing = (my_atan2(d1, dlat) + 540) % 360; // 360 +180 besserer Vergleich mit MkCockpit |
||
126 | geo.distance = sqrt32(d1 * d1 + dlat * dlat); |
||
127 | if (!CheckGPS) |
||
128 | { |
||
129 | lcd_printp_at (0, 5, PSTR("Bear:"), 0); |
||
130 | lcd_puts_at (5, 5, my_itoa((uint32_t)geo.bearing, 3, 0, 0), 0); |
||
131 | lcd_printp_at (8, 5, PSTR("\x1e"), 0); |
||
132 | lcd_printp_at (9, 5, PSTR("Dist:"), 0); |
||
133 | lcd_puts_at (15, 5, my_itoa((uint32_t)geo.distance, 3, 1, 1), 0); |
||
134 | lcd_printp_at (20, 5, PSTR("m"), 0); |
||
135 | } |
||
136 | |||
137 | |||
138 | return(geo); |
||
139 | } |
||
140 | |||
141 | |||
142 | void do_tracking(void) |
||
143 | { static uint8_t hysteresis = 0; |
||
144 | |||
145 | geo = calc_geo(&MK_pos, ¤tPos); |
||
146 | angleTilt = my_atan2((currentPos.Altitude - MK_pos.Home_Alt) / 100, geo.distance); |
||
147 | if (geo.distance < 40 || (geo.distance < 60 && hysteresis)) { // < 4m ==> Pan-Servo in Mittelstellung. Hysterese bis 6m, damit Servo im Grenzbereich nicht wild rumschl�gt |
||
148 | geo.bearing = MK_pos.direction; |
||
149 | if (currentPos.Altitude - MK_pos.Home_Alt < 4000) angleTilt = 0; // man fliegt nicht direkt �ber Kopf |
||
150 | hysteresis = 1; |
||
151 | } |
||
152 | else { |
||
153 | hysteresis = 0; |
||
154 | } |
||
155 | |||
156 | // egal wo der Übergangspunkt 359, 360, 1grd ist => Winkelübergangspunkt auf 0 bzw. 180grd des Servos bringen |
||
157 | // 360 grd negative Winkelwerte als positive |
||
158 | anglePan = (geo.bearing + 450 - MK_pos.direction) % 360; // 450 = 360 + 90 |
||
159 | |||
160 | if (angleTilt < 0) angleTilt = 0; |
||
161 | if (angleTilt > 180) angleTilt = 180; |
||
162 | |||
163 | if (anglePan >= 180) { // zwecks 360grd-Abdeckung flipt Pan-/Tilt-Servo |
||
164 | anglePan = anglePan - 180; |
||
165 | angleTilt = 180 - angleTilt; |
||
166 | } |
||
167 | |||
168 | servoSetAngle(0, anglePan); |
||
169 | servoSetAngle(1, angleTilt); |
||
170 | if (!CheckGPS) |
||
171 | { |
||
172 | lcd_printp_at (0, 6, PSTR("Pan :"), 0); |
||
173 | write_ndigit_number_u (6, 6, anglePan, 3, 1,0); |
||
174 | lcd_printp_at (11, 6, PSTR("Tilt:"), 0); |
||
175 | write_ndigit_number_u (17, 6, angleTilt, 3, 1,0); |
||
176 | } |
||
177 | |||
178 | // write_ndigit_number_u (0, 5, (uint16_t)(currentPos.Altitude/10000000), 2, 0,0); |
||
179 | //// lcd_printp_at (4, 4, PSTR("."), 0); |
||
180 | // write_ndigit_number_u (2, 5, (uint16_t)((currentPos.Altitude/1000) % 10000), 4, 1,0); |
||
181 | // write_ndigit_number_u (6, 5, (uint16_t)((currentPos.Altitude/10) % 100), 2, 1,0); |
||
182 | // |
||
183 | // write_ndigit_number_u (10, 5, (uint16_t)(MK_pos.Home_Alt/10000000), 2, 0,0); |
||
184 | //// lcd_printp_at (4, 4, PSTR("."), 0); |
||
185 | // write_ndigit_number_u (12, 5, (uint16_t)((MK_pos.Home_Alt/1000) % 10000), 4, 1,0); |
||
186 | // write_ndigit_number_u (16, 5, (uint16_t)((MK_pos.Home_Alt/10) % 100), 2, 1,0); |
||
187 | |||
188 | |||
189 | } |
||
190 | //******************************************************************************************************* |
||
191 | uint8_t PKT_trackingBT(void) // Tracking mit NMEA-Daten von BT-Maus |
||
192 | |||
193 | { |
||
194 | |||
195 | uint8_t BT_WhasOn = 0; |
||
196 | uint8_t BT_status; |
||
197 | uint8_t flag; |
||
198 | uint8_t tmp_dat; |
||
199 | coldstart =1; |
||
200 | |||
201 | { |
||
202 | // lcd_printp_at(0,1, PSTR("try NMEA data from:"), 0); |
||
203 | lcd_puts_at (0, 1,Config.gps_UsedDevName, 0); |
||
204 | set_BTOn(); |
||
205 | BT_WhasOn = true; |
||
206 | if (Config.BTIsSlave==true) |
||
207 | { |
||
208 | bt_downlink_init(); |
||
209 | } |
||
210 | lcd_printp_at (18, 1, PSTR(" ?? "), 0); |
||
211 | BT_status = bt_connect(Config.gps_UsedMac); |
||
212 | if (BT_status==true) |
||
213 | { |
||
214 | lcd_printp_at (18, 1, PSTR(" OK "), 0); |
||
215 | receiveNMEA = true; |
||
216 | } |
||
217 | else lcd_printp_at (17, 1, PSTR("FAIL"), 2); |
||
218 | |||
219 | if (receiveNMEA==true) |
||
220 | { |
||
221 | lcd_printp_at (0, 2, PSTR("S Latitude Longitude"), 2); |
||
222 | lcd_cls_line (0,1,20); |
||
223 | lcd_printp_at (0, 3, PSTR("H"), 0); |
||
224 | lcd_printp_at (0, 4, PSTR("M"), 0); |
||
225 | bt_rx_ready = 0; |
||
226 | |||
227 | SwitchToNC(); |
||
228 | mode = 'O'; |
||
229 | // disable debug... |
||
230 | // RS232_request_mk_data (0, 'd', 0); |
||
231 | tmp_dat = 0; |
||
232 | SendOutData ('d', ADDRESS_ANY, 1, &tmp_dat, 1); |
||
233 | |||
234 | // request OSD Data from NC every 100ms |
||
235 | // RS232_request_mk_data (1, 'o', 100); |
||
236 | tmp_dat = 10; |
||
237 | // OSD_active = true; // benötigt für Navidata Ausgabe an SV2 |
||
238 | SendOutData ('o', ADDRESS_NC, 1, &tmp_dat, 1); |
||
239 | |||
240 | flag = 0; |
||
241 | timer = TIMEOUT; |
||
242 | abo_timer = ABO_TIMEOUT; |
||
243 | |||
244 | |||
245 | do |
||
246 | { |
||
247 | // bt_rx_ready = 0; |
||
248 | if (!bt_receiveNMEA()) |
||
249 | break; |
||
250 | if (rxd_buffer_locked) |
||
251 | { |
||
252 | timer = TIMEOUT; |
||
253 | Decode64 (); |
||
254 | naviData = (NaviData_t *) pRxData; |
||
255 | |||
256 | //#ifdef DEBUG |
||
257 | // debug_pgm(PSTR("setup Tracking_NMEA")); |
||
258 | //#endif |
||
259 | currentPos = naviData->CurrentPosition; |
||
260 | // currentPos.Altitude = MK_pos.Home_Alt + (4000 * (int32_t)(naviData->Altimeter) / AltFaktor + currentPos.Altitude - MK_pos.Home_Alt) / 5; |
||
261 | |||
262 | // uint32_t lat = currentPos.Latitude; |
||
263 | // uint32_t lon = currentPos.Longitude; |
||
264 | |||
265 | // write_ndigit_number_u (2, 4, (uint16_t)(lat/10000000), 2, 0,0); |
||
266 | // lcd_printp_at (4, 4, PSTR("."), 0); |
||
267 | // write_ndigit_number_u (5, 4, (uint16_t)((lat/1000) % 10000), 4, 1,0); |
||
268 | // write_ndigit_number_u (9, 4, (uint16_t)((lat/10) % 100), 2, 1,0); |
||
269 | // |
||
270 | |||
271 | // write_ndigit_number_u (12, 4, (uint16_t)(lon/10000000), 2, 0,0); |
||
272 | // lcd_printp_at (14, 4, PSTR("."), 0); |
||
273 | // write_ndigit_number_u (15, 4, (uint16_t)((lon/1000) % 10000), 4, 1,0); |
||
274 | // write_ndigit_number_u (19, 4, (uint16_t)((lon/10) % 100),2, 1,0); |
||
275 | |||
276 | Tracking_NMEA(); |
||
277 | |||
278 | |||
279 | // write_ndigit_number_u (2, 3, (uint16_t)(NMEAlatitude/10000000), 2, 0,0); |
||
280 | // lcd_printp_at (4, 3, PSTR("."), 0); |
||
281 | // write_ndigit_number_u (5, 3, (uint16_t)((NMEAlatitude/1000) % 10000), 4, 1,0); |
||
282 | // write_ndigit_number_u (9, 3, (uint16_t)((NMEAlatitude/10) % 100), 2, 1,0); |
||
283 | // |
||
284 | // |
||
285 | // write_ndigit_number_u (12, 3, (uint16_t)(NMEAlongitude/10000000), 2, 0,0); |
||
286 | // lcd_printp_at (14, 3, PSTR("."), 0); |
||
287 | // write_ndigit_number_u (15, 3, (uint16_t)((NMEAlongitude/1000) % 10000), 4, 1,0); |
||
288 | // write_ndigit_number_u (19, 3, (uint16_t)((NMEAlongitude/10) % 100), 2, 1,0); |
||
289 | |||
290 | // lcd_printp_at (0, 2, PSTR("GPS Time: "), 0); |
||
291 | if (!CheckGPS) |
||
292 | { |
||
293 | lcd_puts_at (13, 0, NMEAtime, 2); |
||
294 | lcd_printp_at (16, 1, PSTR("Sat:"), 0); |
||
295 | write_ndigit_number_u (19, 1, NMEAsatsInUse, 2, 1,0); |
||
296 | lcd_printp_at (0, 1, PSTR("Fix:"), 0); |
||
297 | write_ndigit_number_u (4, 1, posfix, 1, 1,0); |
||
298 | lcd_printp_at (6, 1, PSTR("HDOP:"), 0); |
||
299 | write_ndigit_number_u_10th (11, 1, HDOP, 3, 0,0); |
||
300 | } |
||
301 | |||
302 | |||
303 | rxd_buffer_locked = FALSE; |
||
304 | |||
305 | if (!abo_timer) |
||
306 | { // renew abo every 3 sec |
||
307 | // request OSD Data from NC every 100ms |
||
308 | // RS232_request_mk_data (1, 'o', 100); |
||
309 | tmp_dat = 10; |
||
310 | SendOutData ('o', ADDRESS_NC, 1, &tmp_dat, 1); |
||
311 | |||
312 | abo_timer = ABO_TIMEOUT; |
||
313 | } |
||
314 | }//if (rxd_buffer_locked) |
||
315 | |||
316 | if (!timer) |
||
317 | { |
||
318 | OSD_Timeout(flag); |
||
319 | flag = 0; |
||
320 | error = 1; |
||
321 | |||
322 | } |
||
323 | |||
324 | |||
325 | } //end do |
||
326 | |||
327 | |||
328 | while (!get_key_press (1 << KEY_ENTER) || !receiveNMEA==true || error ==1); |
||
329 | // while (!get_key_press (1 << KEY_ENTER)); |
||
330 | |||
331 | lcd_cls_line(0,1,21); |
||
332 | lcd_cls_line(0,2,21); |
||
333 | lcd_cls_line(0,3,21); |
||
334 | lcd_cls_line(0,4,21); |
||
335 | lcd_cls_line(0,5,21); |
||
336 | lcd_cls_line(0,6,21); |
||
337 | if (!receiveNMEA) lcd_printp_at (0, 2, PSTR("lost BT data"), 0); |
||
338 | // if (error ==1) lcd_printp_at (0, 2, PSTR("lost Wi.232 data"), 0); |
||
339 | lcd_printp_at (0, 3, PSTR("GPS trennen"), 0); |
||
340 | } |
||
341 | else |
||
342 | { |
||
343 | lcd_printp_at (0, 4, PSTR("Error at connecting"), 0); |
||
344 | lcd_printp_at (0, 5, PSTR("switch on BT Mouse!!"), 0); |
||
345 | while (!get_key_press (1 << KEY_ENTER)); |
||
346 | } |
||
347 | receiveNMEA = false; |
||
348 | if (!bt_disconnect()) lcd_printp_at (0, 3, PSTR("Fehler beim Trennen"), 0); |
||
349 | |||
350 | set_BTOff(); |
||
351 | return true; |
||
352 | } |
||
353 | } |
||
354 | |||
355 | //******************************************************************************************************* |
||
356 | |||
357 | uint8_t PKT_trackingMK(void) // Tracking mit GPS-Daten vom Mikrokopter |
||
358 | |||
359 | { |
||
360 | |||
361 | // uint8_t BT_WhasOn = 0; |
||
362 | // uint8_t BT_status; |
||
363 | uint8_t GPSfix=0; |
||
364 | uint8_t tmp_dat; |
||
365 | uint8_t toggletimer=0; |
||
366 | coldstart = true; |
||
367 | |||
368 | |||
369 | lcd_printp_at (0, 2, PSTR("S Latitude Longitude"), 2); |
||
370 | |||
371 | lcd_cls_line (0,1,20); |
||
372 | // lcd_printp_at (0, 3, PSTR("H"), 0); |
||
373 | // lcd_printp_at (0, 4, PSTR("M"), 0); |
||
374 | |||
375 | SwitchToNC(); |
||
376 | mode = 'O'; |
||
377 | // disable debug... |
||
378 | // RS232_request_mk_data (0, 'd', 0); |
||
379 | tmp_dat = 0; |
||
380 | SendOutData ('d', ADDRESS_ANY, 1, &tmp_dat, 1); |
||
381 | |||
382 | // request OSD Data from NC every 100ms |
||
383 | // RS232_request_mk_data (1, 'o', 100); |
||
384 | tmp_dat = 10; |
||
385 | SendOutData ('o', ADDRESS_NC, 1, &tmp_dat, 1); |
||
386 | timer = TIMEOUT; |
||
387 | abo_timer = ABO_TIMEOUT; |
||
388 | error = 0; |
||
389 | |||
390 | do |
||
391 | { |
||
392 | |||
393 | if (rxd_buffer_locked) |
||
394 | { |
||
395 | timer = TIMEOUT; |
||
396 | Decode64 (); |
||
397 | naviData = (NaviData_t *) pRxData; |
||
398 | //OSD_Screen_Element (18, 1, OSD_SATS_IN_USE,1); |
||
399 | //if (GPSfix == true) OSD_Screen_Element (0, 1, OSD_STATUS_FLAGS,1); |
||
400 | OSD_Element_SatsInUse( 18, 1, 1); |
||
401 | if (GPSfix == true) OSD_Element_StatusFlags( 0, 1); |
||
402 | |||
403 | if (!(naviData->NCFlags & NC_FLAG_GPS_OK)) |
||
404 | { |
||
405 | toggletimer++; |
||
406 | if (toggletimer == 50) toggletimer = 0; |
||
407 | if (toggletimer == 25) lcd_printp_at(0,1, PSTR("Whait for GPS Fix "), 2); |
||
408 | if (toggletimer == 1) lcd_printp_at(0,1, PSTR("Whait for GPS Fix "), 0); |
||
409 | |||
410 | rxd_buffer_locked = false; |
||
411 | GPSfix = false; |
||
412 | |||
413 | } |
||
414 | else GPSfix = true; |
||
415 | |||
416 | if (GPSfix) |
||
417 | { |
||
418 | if (coldstart) |
||
419 | { |
||
420 | |||
421 | // erst nach Neustart NGVideo und beim Motorstart werden Daten vom MK übernommen |
||
422 | if (naviData->FCStatusFlags & FC_FLAG_MOTOR_START) { |
||
423 | MK_pos.Home_Lon = naviData->HomePosition.Longitude; |
||
424 | MK_pos.Home_Lat = naviData->HomePosition.Latitude; |
||
425 | MK_pos.Home_Alt = naviData->HomePosition.Altitude; |
||
426 | MK_pos.direction = naviData->CompassHeading; |
||
427 | coldstart = false; |
||
428 | rxd_buffer_locked = false; |
||
429 | lcd_printp_at(0,1, PSTR(" "), 0); |
||
430 | |||
431 | } |
||
432 | else |
||
433 | { |
||
434 | lcd_printp_at(0,1, PSTR("GPS ok, start ok "), 0); |
||
435 | rxd_buffer_locked = false; |
||
436 | } |
||
437 | |||
438 | } |
||
439 | else |
||
440 | { //run |
||
441 | |||
442 | |||
443 | currentPos = naviData->CurrentPosition; |
||
444 | currentPos.Altitude = MK_pos.Home_Alt + (4000 * (int32_t)(naviData->Altimeter) / AltFaktor + currentPos.Altitude - MK_pos.Home_Alt) / 5; |
||
445 | do_tracking(); |
||
446 | // lcd_puts_at (13, 0, NMEAtime, 2); |
||
447 | // lcd_printp_at (16, 1, PSTR("Sat:"), 0); |
||
448 | // write_ndigit_number_u (19, 1, NMEAsatsInUse, 2, 1,0); |
||
449 | // lcd_printp_at (0, 1, PSTR("Fix:"), 0); |
||
450 | // write_ndigit_number_u (4, 1, posfix, 1, 1,0); |
||
451 | // lcd_printp_at (6, 1, PSTR("HDOP:"), 0); |
||
452 | // write_ndigit_number_u_10th (11, 1, HDOP, 3, 0,0); |
||
453 | rxd_buffer_locked = FALSE; |
||
454 | |||
455 | |||
456 | } // run |
||
457 | } |
||
458 | if (!abo_timer) |
||
459 | { // renew abo every 3 sec |
||
460 | // request OSD Data from NC every 100ms |
||
461 | // RS232_request_mk_data (1, 'o', 100); |
||
462 | tmp_dat = 10; |
||
463 | SendOutData ('o', ADDRESS_NC, 1, &tmp_dat, 1); |
||
464 | |||
465 | abo_timer = ABO_TIMEOUT; |
||
466 | } |
||
467 | |||
468 | } //rx_buffer_locked |
||
469 | |||
470 | if (!timer) |
||
471 | { |
||
472 | OSD_Timeout(1); |
||
473 | error = 1; |
||
474 | } |
||
475 | |||
476 | |||
477 | |||
478 | } //end do |
||
479 | |||
480 | |||
481 | while ((!get_key_press (1 << KEY_ENTER)) && (error ==0)); |
||
482 | |||
483 | |||
484 | lcd_cls_line(0,1,21); |
||
485 | lcd_cls_line(0,2,21); |
||
486 | lcd_cls_line(0,3,21); |
||
487 | lcd_cls_line(0,4,21); |
||
488 | lcd_cls_line(0,5,21); |
||
489 | lcd_cls_line(0,6,21); |
||
490 | |||
491 | if (error ==1) |
||
492 | { |
||
493 | lcd_printp_at (0, 2, PSTR("lost Wi.232 data"), 0); |
||
494 | _delay_ms(2000); |
||
495 | } |
||
496 | return true; |
||
497 | } |
||
498 | |||
499 | //******************************************************************************************************* |
||
500 | |||
501 | void PKT_tracking(void) |
||
502 | { |
||
503 | |||
504 | |||
505 | |||
506 | get_key_press(KEY_ALL); |
||
507 | lcd_cls (); |
||
508 | if (Config.gps_UsedGPSMouse==GPS_Bluetoothmouse1) lcd_printp_at(0,0, PSTR("Tracking Bluetooth "), 2); |
||
509 | if (Config.gps_UsedGPSMouse==GPS_Mikrokopter) lcd_printp_at(0,0, PSTR(" Tracking Mikrokopter"), 2); |
||
510 | lcd_printp_at (16, 7, PSTR("Ende"), 0); |
||
511 | |||
512 | |||
513 | if (Config.gps_UsedGPSMouse==GPS_Bluetoothmouse1) PKT_trackingBT(); |
||
514 | if (Config.gps_UsedGPSMouse==GPS_Mikrokopter) PKT_trackingMK(); |
||
515 | get_key_press(KEY_ALL); |
||
516 | |||
517 | } |
||
518 | |||
519 | |||
520 | |||
521 | |||
522 | |||
523 | //// Trying to avoid floating point maths here. Converts a floating point string to an integer with a smaller unit |
||
524 | //// i.e. floatStrToInt("4.5", 2) = 4.5 * 1E2 = 450 |
||
525 | //int32_t floatStrToInt(const char *s, int32_t power1) |
||
526 | //{ char *endPtr; |
||
527 | // int32_t v = strtol(s, &endPtr, 10); |
||
528 | // |
||
529 | // if (*endPtr == '.') { |
||
530 | // for (s = endPtr + 1; *s && power1; s++) { |
||
531 | // v = v * 10 + (*s - '0'); |
||
532 | // --power1; |
||
533 | // } |
||
534 | // } |
||
535 | // if (power1) { |
||
536 | // // Table to avoid multiple multiplications |
||
537 | // v = v * getPower(power1); |
||
538 | // } |
||
539 | // return v; |
||
540 | //} |
||
541 | |||
542 | // NMEA latitudes are in the form ddmm.mmmmm, we want an integer in 1E-7 degree steps |
||
543 | int32_t getLatitude(const char *s, const char *NS) |
||
544 | { int32_t deg = (s[0] - '0') * 10 + s[1] - '0'; // First 2 chars are full degrees |
||
545 | int32_t min = floatStrToInt(&s[2], 6) / 6; // Minutes * 1E5 * 100 / 60 = Minutes * 1E6 / 6 = 1E-7 degree steps |
||
546 | |||
547 | deg = deg * 10000000 + min; |
||
548 | if (*NS == 'S') deg = -deg; |
||
549 | return deg; |
||
550 | } |
||
551 | |||
552 | |||
553 | // NMEA longitudes are in the form dddmm.mmmmm, we want an integer in 1E-7 degree steps |
||
554 | int32_t getLongitude(const char *s, const char *WE) |
||
555 | { int32_t deg = ((s[0] - '0') * 10 + s[1] - '0') * 10 + s[2] - '0'; // First 3 chars are full degrees |
||
556 | int32_t min = floatStrToInt(&s[3], 6) / 6; // Minutes * 1E5 * 100 / 60 = Minutes * 1E6 / 6 = 1E-7 degree steps |
||
557 | |||
558 | deg = deg * 10000000 + min; |
||
559 | if (*WE == 'W') deg = -deg; |
||
560 | return deg; |
||
561 | } |
||
562 | |||
563 | void getNMEATime( const char *s) |
||
564 | { |
||
565 | uint8_t sem = 0; |
||
566 | uint8_t i; |
||
567 | |||
568 | for ( i=0;i < 6; i++ ) |
||
569 | { |
||
570 | NMEAtime[sem++] = s[i]; |
||
571 | if (i==1 || i==3) NMEAtime[sem++] = ':'; |
||
572 | |||
573 | } |
||
574 | NMEAtime[sem] = '\0'; |
||
575 | } |
||
576 | |||
577 | |||
578 | |||
579 | |||
580 | //$GPGGA,191410.000,4735.5634,N,00739.3538,E,1,04,4.4,351.5,M,48.0,M,,*45 |
||
581 | // ^ ^ ^ ^ ^ ^ ^ ^ |
||
582 | // | | | | | | | | |
||
583 | // | | | | | | | Höhe Geoid minus |
||
584 | // | | | | | | | Höhe Ellipsoid (WGS84) |
||
585 | // | | | | | | | in Metern (48.0,M) |
||
586 | // | | | | | | | |
||
587 | // | | | | | | Höhe über Meer (über Geoid)in Metern (351.5,M) |
||
588 | // | | | | | | |
||
589 | // | | | | | HDOP (horizontal dilution |
||
590 | // | | | | | of precision) Genauigkeit |
||
591 | // | | | | | |
||
592 | // | | | | Anzahl der erfassten Satelliten |
||
593 | // | | | | |
||
594 | // | | | Qualität der Messung |
||
595 | // | | | (0 = ungültig) |
||
596 | // | | | (1 = GPS) |
||
597 | // | | | (2 = DGPS) |
||
598 | // | | | (6 = geschätzt nur NMEA-0183 2.3) |
||
599 | // | | | |
||
600 | // | | Längengrad |
||
601 | // | | |
||
602 | // | Breitengrad |
||
603 | // | |
||
604 | // Uhrzeit |
||
605 | |||
606 | void Tracking_NMEA(void) |
||
607 | { |
||
608 | char *token; |
||
609 | |||
610 | |||
611 | |||
612 | if (decodeNMEA()) { |
||
613 | token = strtok((char*)data_decode, ","); |
||
614 | if (!strcmp(token, "GPGGA")) |
||
615 | { |
||
616 | // $GPGGA,220613.400,4843.5080,N,00922.9583,E,1,7,2.23,287.1,M,48.0,M,, |
||
617 | // Time |
||
618 | getNMEATime(strtok(0, ".")); //Zeit |
||
619 | |||
620 | strtok(0, ","); // Skip Rest von der Zeit |
||
621 | // Latitude |
||
622 | NMEAlatitude = getLatitude(strtok(0, ","), strtok(0, ",")); //N |
||
623 | // Longitude |
||
624 | NMEAlongitude = getLongitude(strtok(0, ","), strtok(0, ","));//E |
||
625 | // Signal valid? (Position Fix Indicator) |
||
626 | posfix = atoi(strtok(0, ",")); // Qualität |
||
627 | // Satellites in use |
||
628 | NMEAsatsInUse = atoi(strtok(0, ",")); //Anzahl Sats |
||
629 | // Dilition, best = 0.0 |
||
630 | HDOP = floatStrToInt(strtok(0, ","),1); //Dilution |
||
631 | |||
632 | // // Altitude |
||
633 | NMEAaltitude = floatStrToInt(strtok(0, ","), 1); |
||
634 | // currentPos.Altitude = altitude; |
||
635 | // currentPos.Latitude = latitude; |
||
636 | // currentPos.Longitude = longitude; |
||
637 | |||
638 | //TODO: erstmal test if ((coldstart) && (satsInUse > 5)) { |
||
639 | // First position after reboot (or change of mode) will be the home position (facing north) |
||
640 | MK_pos.Home_Lon = NMEAlongitude; |
||
641 | MK_pos.Home_Lat = NMEAlatitude; |
||
642 | |||
643 | MK_pos.Home_Alt = NMEAaltitude; |
||
644 | MK_pos.direction = 0; |
||
645 | // coldstart = 0; |
||
646 | // Double_Beep(DBEEPNMEAFIX, DBEEPMEAFIXP); |
||
647 | |||
648 | do_tracking(); |
||
649 | // } |
||
650 | } |
||
651 | } |
||
652 | // Displ_GPS(); // letzte empfangene Daten auch bei ausgeschalteter NMEA sichtbar |
||
653 | } |
||
654 | |||
655 | |||
656 | uint8_t hexDigitToInt(uint8_t digit) |
||
657 | { |
||
658 | if (digit >= '0' && digit <= '9') return digit - '0'; |
||
659 | if (digit >= 'a' && digit <= 'f') return digit - 'a' + 10; |
||
660 | if (digit >= 'A' && digit <= 'F') return digit - 'A' + 10; |
||
661 | return 0; |
||
662 | } |
||
663 | |||
664 | |||
665 | uint8_t decodeNMEA(void) |
||
666 | { |
||
667 | uint8_t ret = 0; |
||
668 | uint8_t crc; |
||
669 | uint8_t tmpCRC = 0; |
||
670 | uint8_t i; |
||
671 | |||
672 | if (bt_rx_ready == 1 && bt_rx_len > 0) { |
||
673 | // Calculate checksum |
||
674 | for (i = 1; i < bt_rx_len && bt_rx_buffer[i] != '*'; i++) { |
||
675 | tmpCRC ^= bt_rx_buffer[i]; |
||
676 | } |
||
677 | if (bt_rx_len >= i + 3) { |
||
678 | crc = hexDigitToInt(bt_rx_buffer[i + 1]) << 4 | hexDigitToInt(bt_rx_buffer[i + 2]); |
||
679 | if (crc == tmpCRC) { |
||
680 | bt_rx_buffer[i] = 0; |
||
681 | strcpy(data_decode, &bt_rx_buffer[1]); // Data without $, crc |
||
682 | ret = 1; |
||
683 | // wi232RX = 1; // So antenna-symbol will blink |
||
684 | // cli(); |
||
685 | // rx_timeout = 0; // Got valid data, reset counter |
||
686 | // sei(); |
||
687 | } |
||
688 | } |
||
689 | } |
||
690 | // if (rx_timeout < RX_TIME_OLD) wi232RX = 1; |
||
691 | bt_rx_ready = 0; // Unlock buffer, next NMEA string can be received |
||
692 | return ret; |
||
693 | } |
||
694 | #endif |