Go to most recent revision | Details | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
1465 | - | 1 | |
2 | /****************************************************************/ |
||
3 | /* */ |
||
4 | /* NG-Video 5,8GHz */ |
||
5 | /* */ |
||
6 | /* Copyright (C) 2011 - gebad */ |
||
7 | /* */ |
||
8 | /* This code is distributed under the GNU Public License */ |
||
9 | /* which can be found at http://www.gnu.org/licenses/gpl.txt */ |
||
10 | /* */ |
||
11 | /****************************************************************/ |
||
12 | |||
13 | #include <stdio.h> |
||
14 | #include <stdlib.h> |
||
15 | #include <string.h> |
||
16 | #include <math.h> |
||
17 | #include <util/delay.h> |
||
18 | |||
19 | #include "tracking.h" |
||
20 | #include "ngvideo.h" |
||
21 | #include "servo.h" |
||
22 | #include "tools.h" |
||
23 | #include "usart.h" |
||
24 | #include "config.h" |
||
25 | #include "mk.h" |
||
26 | #include "keys.h" |
||
27 | |||
28 | #define DLEFT 0 |
||
29 | #define DRIGHT 1 |
||
30 | #define DEG_TO_RAD 0.0174533 // degrees to radians = PI / 180 |
||
31 | #define RAD_TO_DEG 57.2957795 // radians to degrees = 180 / PI |
||
32 | #define AltFaktor 22.5 |
||
33 | |||
34 | HomePos_t MK_pos; // Home position of station |
||
35 | GPS_Pos_t currentPos; // Current position of flying object |
||
36 | int8_t satsInUse; // Number of satelites currently in use |
||
37 | |||
38 | uint8_t tracking = TRACKING_MIN; |
||
39 | uint8_t track_hyst = TRACKING_HYSTERESE; |
||
40 | uint8_t track_tx = 0; |
||
41 | |||
42 | geo_t geo; |
||
43 | int anglePan, angleTilt; |
||
44 | |||
45 | /**************************************************************/ |
||
46 | /* */ |
||
47 | /* RSSI Tracking */ |
||
48 | /* */ |
||
49 | /**************************************************************/ |
||
50 | |||
51 | void Tracking_RSSI(void) |
||
52 | { uint16_t u0, u1; |
||
53 | static uint8_t direction; |
||
54 | static uint16_t servo1pos; |
||
55 | |||
56 | u0 = (ADC_Read(RSSI0) * (uint32_t)udbm.korr_1)/UDBM_KORR_FA; |
||
57 | u1 = (ADC_Read(RSSI1) * (uint32_t)udbm.korr_2)/UDBM_KORR_FA; |
||
58 | |||
59 | if (direction == DLEFT) { |
||
60 | if (u0 < u1) { |
||
61 | if ( servo1pos > 0) --servo1pos; // Servo Endposition? |
||
62 | servoSetPosition(SERVO_PAN, servo1pos); // beibehaltene Richtung ==> sofort ausführen |
||
63 | } |
||
64 | else |
||
65 | if (u0 > (u1 + track_hyst)) direction = DRIGHT; // Richtungwechsel, wenn Hysterese überschritten |
||
66 | } |
||
67 | else { |
||
68 | if (u0 > u1) { |
||
69 | if ( servo1pos < ServoSteps()) ++servo1pos; // Servo Endposition? |
||
70 | servoSetPosition(SERVO_PAN, servo1pos); // beibehaltene Richtung ==> sofort ausführen |
||
71 | } |
||
72 | else |
||
73 | if ((track_hyst + u0) < u1) direction = DLEFT; // Richtungwechsel, wenn Hysterese überschritten |
||
74 | } |
||
75 | } |
||
76 | |||
77 | /**************************************************************/ |
||
78 | /* */ |
||
79 | /* GPS Tracking */ |
||
80 | /* */ |
||
81 | /**************************************************************/ |
||
82 | |||
83 | // Datenempfang vom MK ==> blinkt Antennensymbol |
||
84 | void Displ_wiRX(void) |
||
85 | { static uint8_t timer = 0; |
||
86 | static uint8_t picturedSats; |
||
87 | |||
88 | if (!bat_low && (pmenu[0] == '\0')) { // nicht im Menü und nicht bei Akku leer blinken |
||
89 | lcdGotoXY(1, 0); |
||
90 | if (wi232RX) { // Datensatz vom MK empfangen? |
||
91 | if (timer < BLINK_PERIOD/2) { |
||
92 | picturedSats = 0; |
||
93 | lcdPutc(5); // Antennensymbol darstellen |
||
94 | } |
||
95 | else { |
||
96 | if (!picturedSats) { // Änderung Satelittenanzahl innerhalb dieser Periode nicht anzeigen |
||
97 | picturedSats = 1; |
||
98 | // aktuell empfangene Satellitenanzahl, Anzeige einstellig, da kein Platz auf Display |
||
99 | lcdPutc(satsInUse > 9? 'X': satsInUse + '0'); |
||
100 | } |
||
101 | } |
||
102 | } |
||
103 | else |
||
104 | lcdPutc(':'); |
||
105 | wi232RX = 0; |
||
106 | if (++timer == BLINK_PERIOD) timer = 0; |
||
107 | } |
||
108 | } |
||
109 | |||
110 | // Berechnung von Distanz und Winkel aus GPS-Daten home(MK eingeschaltet) |
||
111 | // zur aktuellen Position(nach Motorstart) |
||
112 | geo_t calc_geo(HomePos_t *home, GPS_Pos_t *pos) |
||
113 | { double lat1, lon1, lat2, lon2, d1, dlat; |
||
114 | geo_t geo; |
||
115 | |||
116 | lon1 = home->Home_Lon; |
||
117 | lat1 = home->Home_Lat; |
||
118 | lon2 = (double)pos->Longitude / 10000000.0; |
||
119 | lat2 = (double)pos->Latitude / 10000000.0; |
||
120 | |||
121 | // Formel verwendet von http://www.kompf.de/gps/distcalc.html |
||
122 | // 111.3 km = Abstand zweier Breitenkreise und/oder zweier Längenkreise am Äquator |
||
123 | // es wird jedoch in Meter weiter gerechnet |
||
124 | d1 = 111300 * (double)cos((double)(lat1 + lat2) / 2 * DEG_TO_RAD) * (lon1 - lon2); |
||
125 | dlat = 111300 * (double)(lat1 - lat2); |
||
126 | // returns a value in metres http://www.kompf.de/gps/distcalc.html |
||
127 | geo.bearing = fmod((RAD_TO_DEG * (double)atan2(d1, dlat)) + 180, 360); // +180 besserer Vergleich mit MkCockpit |
||
128 | if (geo.bearing > 360) geo.bearing -= 360; // bekam schon Werte über 400 |
||
129 | geo.distance = sqrt(d1 * d1 + dlat * dlat); |
||
130 | return(geo); |
||
131 | } |
||
132 | |||
133 | // Anzeige eines GPS-Wertes auf LCD |
||
134 | void Displ_Format_Data(uint8_t x, uint8_t y, char *description, char *data, char measure) |
||
135 | { |
||
136 | lcdGotoXY(x, y); |
||
137 | lcdPuts(description); |
||
138 | lcdPuts(data); |
||
139 | if (measure != '\0') |
||
140 | lcdPutc(measure); |
||
141 | } |
||
142 | |||
143 | |||
144 | //void Displ_GPS(int anglePan, int angleTilt, geo_t *geo) |
||
145 | void Displ_GPS(void) |
||
146 | { uint16_t dDCurrent = 0; |
||
147 | // int32_t time_remaining = M59S59; |
||
148 | int32_t time_remaining = 0; |
||
149 | |||
150 | if (gps_display > GPS_DISP_NONE && (!coldstart || gps_display == GPS_MISC) && !bat_low) { |
||
151 | if (gps_disp_clear && !coldstart) { |
||
152 | gps_disp_clear = 0; |
||
153 | lcdClear(); |
||
154 | } |
||
155 | // makefile ohne! Minimalistic printf version |
||
156 | switch (gps_display) { |
||
157 | case GPS_DISP_CALC: |
||
158 | Displ_Format_Data(0, 0, "Dir:", my_itoa(MK_pos.direction, 3, 0, 0), '\xdf'); // '\xdf' entspricht '°' |
||
159 | Displ_Format_Data(8, 0, "Dis:", my_itoa((uint32_t)geo.distance, 3, 0, 0), 'm'); |
||
160 | Displ_Format_Data(0, 1, "Bea:", my_itoa((uint32_t)geo.bearing, 3, 0, 0), '\xdf'); |
||
161 | Displ_Format_Data(8, 1, "Alt:", my_itoa(currentPos.Altitude - MK_pos.Home_Alt, 3, 3, 1), 'm'); |
||
162 | Displ_Format_Data(0, 2, "Pan:", my_itoa(anglePan, 3, 0, 0), '\xdf'); |
||
163 | Displ_Format_Data(8, 2, "Til:", my_itoa(angleTilt, 3, 0, 0), '\xdf'); |
||
164 | break; |
||
165 | case GPS_DISP_CURRENT: |
||
166 | Displ_Format_Data(0, 0, "aLon:", my_itoa(currentPos.Longitude, 11, 7, 7), '\0'); |
||
167 | Displ_Format_Data(0, 1, "aLat:", my_itoa(currentPos.Latitude, 11, 7, 7), '\0'); |
||
168 | Displ_Format_Data(0, 2, "aAlt:", my_itoa(currentPos.Altitude, 11, 3, 1), '\0'); |
||
169 | break; |
||
170 | case GPS_DISP_HOME: |
||
171 | Displ_Format_Data(0, 0, "hLon:", my_itoa(MK_pos.Home_Lon7, 11, 7, 7), '\0'); |
||
172 | Displ_Format_Data(0, 1, "hLat:", my_itoa(MK_pos.Home_Lat7, 11, 7, 7), '\0'); |
||
173 | Displ_Format_Data(0, 2, "hAlt:", my_itoa(MK_pos.Home_Alt, 11, 3, 1), '\0'); |
||
174 | // >> Menueauswahl nach oben |
||
175 | if (Get_Key_Press( 1<<SW_PLUS ) || Get_Key_Press( 1<<SW_MINUS )) coldstart = 1; |
||
176 | break; |
||
177 | case GPS_MISC: |
||
178 | Displ_Format_Data(0, 0, "U:", my_itoa(mk_UBat, 4, 1, 1), 'V'); |
||
179 | Displ_Format_Data(8, 0, "W:", my_itoa(mk_dUsedCapacity, 4, 0, 0), '\0'); |
||
180 | Displ_Format_Data(0, 1, "I:", my_itoa(mk_Ikorr, 4, 1, 1), 'A'); |
||
181 | if (mk_current.Count > 0) { |
||
182 | dDCurrent = mk_current.Sum / mk_current.Count; |
||
183 | if (dDCurrent > 0) { |
||
184 | time_remaining = ((int32_t)mk_lipo.Capacity - mk_dUsedCapacity) * 36 / dDCurrent; |
||
185 | if (abs(time_remaining) > M59S59) |
||
186 | time_remaining = time_remaining < 0? -M59S59: M59S59; // M59S59 sind 59 Minuten und 59 Sekunden |
||
187 | } |
||
188 | } |
||
189 | Displ_Format_Data(8, 1, "\xee:", my_itoa(dDCurrent, 4, 1, 1), 'A');// Durchschnittszeichen |
||
190 | lcdGotoXY(0,2); |
||
191 | lcdPutc('\x1a'); // Summenzeichen |
||
192 | Displ_TimeMS(mk_lipo.time_on / T2SECDIV); |
||
193 | lcdGotoXY(8,2); |
||
194 | lcdPuts("R"); |
||
195 | Displ_TimeMS(time_remaining); |
||
196 | break; |
||
197 | } |
||
198 | } |
||
199 | Displ_wiRX(); |
||
200 | } |
||
201 | |||
202 | void do_tracking(void) |
||
203 | { static uint8_t hysteresis = 0; |
||
204 | |||
205 | geo = calc_geo(&MK_pos, ¤tPos); |
||
206 | angleTilt = RAD_TO_DEG * (double)atan2((double)(currentPos.Altitude - MK_pos.Home_Alt) / 1000, geo.distance); |
||
207 | if (geo.distance < 4 || (geo.distance < 6 && hysteresis)) { // < 4m ==> Pan-Servo in Mittelstellung. Hysterese bis 6m, damit Servo im Grenzbereich nicht wild rumschlägt |
||
208 | geo.bearing = MK_pos.direction; |
||
209 | angleTilt = 0; |
||
210 | hysteresis = 1; |
||
211 | } |
||
212 | else { |
||
213 | hysteresis = 0; |
||
214 | } |
||
215 | |||
216 | // egal wo der Übergangspunkt 359, 360, 1grd ist => Winkelübergangspunkt auf 0 bzw. 180grd des Servos bringen |
||
217 | // 360 grd negative Winkelwerte als positive |
||
218 | anglePan = fmod(geo.bearing - MK_pos.direction + 360 + 90, 360); |
||
219 | if (anglePan > 360) anglePan -= 360; |
||
220 | |||
221 | if (angleTilt < 0) angleTilt = 0; |
||
222 | if (angleTilt > 180) angleTilt = 180; |
||
223 | |||
224 | if (anglePan >= 180) { // zwecks 360grd-Abdeckung flipt Pan-/Tilt-Servo |
||
225 | anglePan = anglePan - 180; |
||
226 | angleTilt = 180 - angleTilt; |
||
227 | } |
||
228 | |||
229 | servoSetAngle(0, anglePan); |
||
230 | servoSetAngle(1, angleTilt); |
||
231 | } |
||
232 | |||
233 | |||
234 | /**************************************************************/ |
||
235 | /* */ |
||
236 | /* MK GPS Tracking */ |
||
237 | /* */ |
||
238 | /**************************************************************/ |
||
239 | |||
240 | // MK OSD-Daten lesen und verifizieren |
||
241 | uint8_t OSD_Data_valid(NaviData_t **navi_data) |
||
242 | { uint8_t ret = 0; |
||
243 | char *tx_osd = {"#co?]==EH\r"}; |
||
244 | // char interval[2] = {10, '\0'}; |
||
245 | |||
246 | if (rx_line_decode('O')) { // OSD-Datensatz prüfen/dekodieren |
||
247 | *navi_data = (NaviData_t*)data_decode; // dekodierte Daten mit Struktur OSD-Daten versehen |
||
248 | if (rx_timeout < RX_TIME_OLD) { // GPS-Daten nicht zu alt und ok. |
||
249 | currentPos = (*navi_data)->CurrentPosition; |
||
250 | if ((*navi_data)->NCFlags & NC_FLAG_GPS_OK) |
||
251 | ret = 1; |
||
252 | // aus MkCockpit http://forum.mikrokopter.de/topic-post216136.html#post216136 |
||
253 | // (4 * (........))/5 ==> Wichtung Luftdruck-Höhe zu GPS |
||
254 | currentPos.Altitude = MK_pos.Home_Alt + (4000 * (int32_t)((*navi_data)->Altimeter) / AltFaktor + currentPos.Altitude - MK_pos.Home_Alt) / 5; |
||
255 | MK_Motor_run = (*navi_data)->FCStatusFlags & FC_FLAG_MOTOR_RUN; |
||
256 | MK_ProcessLipo(*navi_data); |
||
257 | satsInUse = (*navi_data)->SatsInUse; |
||
258 | } |
||
259 | } |
||
260 | else |
||
261 | if (rx_timeout == RX_TIME_END) store_LipoData();// ca. 4 Sekunden nach Signalverlust |
||
262 | // ca. 210ms keine OSD-Daten empfangen ==> sende neue Anforderung an MK |
||
263 | // if ((track_tx) && (rx_timeout > RX_TIMEOUT)) tx_Mk(NC_ADDRESS, 'o', interval, 1); // 420 * 0.5ms interval |
||
264 | if ((track_tx) && (rx_timeout > RX_TIMEOUT)) USART_send_Str(tx_osd); // 420 * 0.5ms interval |
||
265 | return ret; |
||
266 | } |
||
267 | |||
268 | |||
269 | // MK eingeschaltet und GPS-ok, danach Motoren gestartet ==> Berechnung horizontaler/vertikaler Servowinkel |
||
270 | // Hauptprogramm von GPS Antennen-Nachführung |
||
271 | void Tracking_GPS(void) |
||
272 | { NaviData_t *navi_data; |
||
273 | static uint8_t track_running = 0; |
||
274 | |||
275 | if (!track_running) { |
||
276 | track_running = 1; // verhindert doppelten Aufruf, wenn in Eingabeschleife Menu_MK_BatteryChangeNr() !!! |
||
277 | if (OSD_Data_valid(&navi_data)) { |
||
278 | if (coldstart) { |
||
279 | // erst nach Neustart NGVideo und beim Motorstart werden Daten vom MK übernommen |
||
280 | if (navi_data->FCStatusFlags & FC_FLAG_MOTOR_START) { |
||
281 | MK_pos.Home_Lon = (double)navi_data->HomePosition.Longitude / 10000000.0; |
||
282 | MK_pos.Home_Lat = (double)navi_data->HomePosition.Latitude / 10000000.0; |
||
283 | MK_pos.Home_Lon7 = navi_data->HomePosition.Longitude; |
||
284 | MK_pos.Home_Lat7 = navi_data->HomePosition.Latitude; |
||
285 | MK_pos.Home_Alt = navi_data->HomePosition.Altitude; |
||
286 | MK_pos.direction = navi_data->CompassHeading; |
||
287 | coldstart = 0; |
||
288 | } |
||
289 | } |
||
290 | else { |
||
291 | do_tracking(); |
||
292 | } |
||
293 | } |
||
294 | Displ_GPS(); // letzte empfangene Daten auch bei ausgeschalteten MK sichtbar |
||
295 | track_running = 0; |
||
296 | } |
||
297 | } |
||
298 | |||
299 | /**************************************************************/ |
||
300 | /* */ |
||
301 | /* NMEA GPS Tracking */ |
||
302 | /* */ |
||
303 | /**************************************************************/ |
||
304 | |||
305 | void setNMEAdir(void) |
||
306 | { |
||
307 | if (!coldstart) |
||
308 | MK_pos.direction = geo.bearing; |
||
309 | } |
||
310 | |||
311 | // NMEA latitudes are in the form ddmm.mmmmm, we want an integer in 1E-7 degree steps |
||
312 | int32_t getLatitude(const char *s, const char *NS) |
||
313 | { int32_t deg = (s[0] - '0') * 10 + s[1] - '0'; // First 2 chars are full degrees |
||
314 | int32_t min = floatStrToInt(&s[2], 6) / 6; // Minutes * 1E5 * 100 / 60 = Minutes * 1E6 / 6 = 1E-7 degree steps |
||
315 | |||
316 | deg = deg * 10000000 + min; |
||
317 | if (*NS == 'S') deg = -deg; |
||
318 | return deg; |
||
319 | } |
||
320 | |||
321 | |||
322 | // NMEA longitudes are in the form dddmm.mmmmm, we want an integer in 1E-7 degree steps |
||
323 | int32_t getLongitude(const char *s, const char *WE) |
||
324 | { int32_t deg = ((s[0] - '0') * 10 + s[1] - '0') * 10 + s[2] - '0'; // First 3 chars are full degrees |
||
325 | int32_t min = floatStrToInt(&s[3], 6) / 6; // Minutes * 1E5 * 100 / 60 = Minutes * 1E6 / 6 = 1E-7 degree steps |
||
326 | |||
327 | deg = deg * 10000000 + min; |
||
328 | if (*WE == 'W') deg = -deg; |
||
329 | return deg; |
||
330 | } |
||
331 | |||
332 | |||
333 | void Tracking_NMEA(void) |
||
334 | { char *token; |
||
335 | int32_t latitude, longitude; |
||
336 | |||
337 | if (decodeNMEA()) { |
||
338 | token = strtok((char*)data_decode, ","); |
||
339 | // http://de.wikipedia.org/wiki/NMEA_0183 |
||
340 | if (!strcmp(token, "GPGGA")) { |
||
341 | // $GPGGA,220613.400,4843.5080,N,00922.9583,E,1,7,2.23,287.1,M,48.0,M,, |
||
342 | // Skip time |
||
343 | strtok(0, ","); |
||
344 | // Latitude |
||
345 | latitude = getLatitude(strtok(0, ","), strtok(0, ",")); |
||
346 | // Longitude |
||
347 | longitude = getLongitude(strtok(0, ","), strtok(0, ",")); |
||
348 | // Signal valid? (Position Fix Indicator) |
||
349 | if (*strtok(0, ",") != '0') { |
||
350 | // Satellites in use |
||
351 | satsInUse = atoi(strtok(0, ",")); |
||
352 | // Skip dilution |
||
353 | strtok(0, ","); |
||
354 | // Altitude |
||
355 | currentPos.Altitude = floatStrToInt(strtok(0, ","), 3); |
||
356 | currentPos.Latitude = latitude; |
||
357 | currentPos.Longitude = longitude; |
||
358 | |||
359 | if ((coldstart) && (satsInUse > 5)) { |
||
360 | // First position after reboot (or change of mode) will be the home position (facing north) |
||
361 | MK_pos.Home_Lon = (double)currentPos.Longitude / 10000000.0; |
||
362 | MK_pos.Home_Lat = (double)currentPos.Latitude / 10000000.0; |
||
363 | MK_pos.Home_Lon7 = currentPos.Longitude; |
||
364 | MK_pos.Home_Lat7 = currentPos.Latitude; |
||
365 | MK_pos.Home_Alt = currentPos.Altitude; |
||
366 | MK_pos.direction = 0; |
||
367 | coldstart = 0; |
||
368 | Double_Beep(DBEEPNMEAFIX, DBEEPMEAFIXP); |
||
369 | } |
||
370 | do_tracking(); |
||
371 | } |
||
372 | } |
||
373 | } |
||
374 | Displ_GPS(); // letzte empfangene Daten auch bei ausgeschalteter NMEA sichtbar |
||
375 | } |
||
376 | |||
377 | |||
378 | /************************************************************************/ |
||
379 | /* */ |
||
380 | /* MKCockPit Tracking */ |
||
381 | /* */ |
||
382 | /* http://www.pololu.com/file/download/ssc03a_guide.pdf?file_id=0J37 */ |
||
383 | /* */ |
||
384 | /************************************************************************/ |
||
385 | |||
386 | void Tracking_MKCockpit(void) |
||
387 | { uint16_t ServoPos; |
||
388 | |||
389 | if (Get_Pololu_cmd(data_decode, POLOLU_CMD)) { |
||
390 | switch (data_decode[0]) { |
||
391 | // Command 0: Set Parameters(1 data byte) |
||
392 | case 0x00: ; |
||
393 | break; |
||
394 | // Command 1: Set Speed (1 data byte) |
||
395 | case 0x01: ; |
||
396 | break; |
||
397 | // Command 4: Set Position,Absollute(2 data bytes) - angepasst für default MKCockPit |
||
398 | case 0x04: ServoPos = data_decode[2]; |
||
399 | ServoPos = ((ServoPos << 7) | data_decode[3]) / 20; // ServoPos * ServoSteps() sonst zu groß |
||
400 | |||
401 | if (ServoPos >= 60) ServoPos -= 60; else ServoPos = 0; |
||
402 | if (ServoPos > 180) ServoPos = 180; |
||
403 | servoSetAngle(data_decode[1], ServoPos); |
||
404 | } |
||
405 | } |
||
406 | } |