Rev 2203 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
2136 | - | 1 | /* |
2 | * FollowMe.c |
||
3 | * |
||
4 | * Created on: 18.05.2012 |
||
5 | * Author: cebra |
||
6 | */ |
||
7 | /***************************************************************************** |
||
8 | * Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net * |
||
9 | * * |
||
10 | * This program is free software; you can redistribute it and/or modify * |
||
11 | * it under the terms of the GNU General Public License as published by * |
||
12 | * the Free Software Foundation; either version 2 of the License. * |
||
13 | * * |
||
14 | * This program is distributed in the hope that it will be useful, * |
||
15 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * |
||
16 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
||
17 | * GNU General Public License for more details. * |
||
18 | * * |
||
19 | * You should have received a copy of the GNU General Public License * |
||
20 | * along with this program; if not, write to the * |
||
21 | * Free Software Foundation, Inc., * |
||
22 | * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
||
23 | * * |
||
24 | * * |
||
25 | * Credits to: * |
||
26 | * Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN * |
||
27 | * http://www.mikrokopter.de * |
||
28 | * Gregor "killagreg" Stobrawa for his version of the MK code * |
||
29 | * Thomas Kaiser "thkais" for the original project. See * |
||
30 | * http://www.ft-fanpage.de/mikrokopter/ * |
||
31 | * http://forum.mikrokopter.de/topic-4061-1.html * |
||
32 | * Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code * |
||
33 | * http://www.mylifesucks.de/oss/c-osd/ * |
||
34 | * Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility* |
||
35 | *****************************************************************************/ |
||
36 | //############################################################################ |
||
37 | //# HISTORY followme.c |
||
38 | //# |
||
2200 | - | 39 | //# 20.09.2015 Starter |
40 | //# FollowMeStep2 erweitert auf aktuelle GPS-Daten und followme_calculate_offset(...) |
||
41 | //# |
||
2136 | - | 42 | //# 03.08.2015 Cebra |
43 | //# - add: void Debug_GPS (void) hinzugefügt zur Test der GPS Berechnung FollowMe |
||
44 | //# |
||
45 | //# 20.07.2015 CB |
||
46 | //# - chg: FollowMe Datensatz an NC.211 angepasst |
||
47 | //# |
||
48 | //# 27.06.2014 OG |
||
49 | //# - chg: Anzeige von Satfix und Satanzahl auf MINVERSX, MNORMALX |
||
50 | //# |
||
51 | //# 26.06.2014 OG |
||
52 | //# - chg: angepasst auf neue NMEA-Datenstruktur (gps_nmea.h) |
||
53 | //# |
||
54 | //# 24.06.2014 OG |
||
55 | //# - chg: FollowMe() angepasst auf geaendertes GPSMouse_ShowData() |
||
56 | //# |
||
57 | //# 22.06.2014 OG |
||
58 | //# - chg: FollowMe() umgestellt auf GPSMouse_ShowData() (in gps/gpsmouse.c) |
||
59 | //# - del: Variable CheckGPS |
||
60 | //# |
||
61 | //# 21.06.2014 OG |
||
62 | //# - chg: verschiedene Layoutaenderungen am Anzeigescreen und Anzeige der |
||
63 | //# Entfernung zwischen Kopter und Target |
||
64 | //# - chg: MK-Timeoutout Erkennung verbessert/angepasst |
||
65 | //# |
||
66 | //# 19.06.2014 OG |
||
67 | //# - erster Prototyp der Follow Me zum Kopter sendet |
||
68 | //# - etliche Aenderungen im Codeaufbau |
||
69 | //# |
||
70 | //# 01.06.2014 OG |
||
71 | //# - chg: FollowMe() - verschiedene Anzeigefunktionen auskommentiert und |
||
72 | //# als DEPRICATED Klassifiziert wegen Cleanup der alten OSD Screens |
||
73 | //# - chg: FollowMe() - Check bzgl. NC-Hardware entfernt da das bereits durch das |
||
74 | //# Hauptmenue erledigt wird |
||
75 | //# |
||
76 | //# 13.05.2014 OG |
||
77 | //# - chg: FollowMe() - Variable 'flag' auskommentiert |
||
78 | //# wegen Warning: variable set but not used |
||
79 | //# - chg: FollowMe() - Variable 'old_FCFlags' auskommentiert |
||
80 | //# wegen Warning: variable set but not used |
||
81 | //# - chg: FollowMe() - den Bereich in dem FC-Settings geladen werdeb |
||
82 | //# auskommentiert weil man das a) vorallem nicht benoetigt |
||
83 | //# und b) die load_setting() so nicht mehr existiert |
||
84 | //# (der Codebereich kann meines erachtens geloescht werden) |
||
85 | //# - del: verschiedene Verweise auf 'mk_param_struct' entfernt, weil es |
||
86 | //# das a) nicht mehr gibt und b) hier gar nicht benoetigt wird |
||
87 | //# - chg: FollowMe() - OSD_Timeout() entfernt (weil es das nicht mehr gibt) |
||
88 | //# und erstmal durch ein PKT_Message_P() ersetzt |
||
89 | //# * ACHTUNG: UNGETESTET! * (bei Bedarf anpassen, followme hat niedrige Prio) |
||
90 | //# - add: #include "../pkt/pkt.h" |
||
91 | //# |
||
92 | //# 05.05.2013 Cebra |
||
93 | //# - chg: #ifdef USE_FOLLOWME |
||
94 | //# |
||
95 | //############################################################################ |
||
96 | |||
97 | #include "../cpu.h" |
||
98 | #include <avr/io.h> |
||
99 | #include <inttypes.h> |
||
100 | #include <stdlib.h> |
||
101 | #include <avr/pgmspace.h> |
||
102 | #include <util/delay.h> |
||
103 | |||
104 | #include <string.h> |
||
105 | #include <stdarg.h> |
||
106 | #include <stdio.h> |
||
107 | |||
108 | #include "../main.h" |
||
109 | |||
110 | #ifdef USE_FOLLOWME |
||
111 | |||
112 | #include "../followme/followme.h" |
||
113 | #include "../osd/osd.h" |
||
114 | #include "../lcd/lcd.h" |
||
115 | #include "../timer/timer.h" |
||
116 | #include "../uart/usart.h" |
||
117 | #include "../eeprom/eeprom.h" |
||
118 | #include "../messages.h" |
||
119 | #include "../bluetooth/bluetooth.h" |
||
120 | #include "../setup/setup.h" |
||
121 | #include "../uart/uart1.h" |
||
122 | #include "../mk-data-structs.h" |
||
123 | #include "../pkt/pkt.h" |
||
124 | #include "../gps/gps.h" |
||
125 | //#include "../gps/gps_nmea.h" |
||
126 | #include "../avr-nmea-gps-library/nmea.h" |
||
127 | #include "../gps/gpsmouse.h" |
||
128 | |||
129 | |||
130 | //####################################################################################################################### |
||
131 | |||
132 | |||
133 | //-------------------- |
||
134 | // Timings |
||
135 | //-------------------- |
||
136 | //#define MK_TIMEOUT 300 // MK-Verbindungsfehler wenn fuer n Zeit keine gueltigen Daten hereinkommen (3 sec) |
||
137 | #define MK_TIMEOUT 400 // MK-Verbindungsfehler wenn fuer n Zeit keine gueltigen Daten hereinkommen (4 sec) |
||
138 | |||
139 | |||
140 | |||
141 | //-------------------- |
||
142 | #define COSD_WASFLYING 4 |
||
143 | |||
144 | // global definitions and global vars |
||
145 | NaviData_t *naviData; |
||
146 | unsigned char Element; |
||
147 | uint16_t heading_home; |
||
148 | |||
149 | // Hier Höhenanzeigefehler Korrigieren |
||
150 | #define AltimeterAdjust 1.5 |
||
151 | |||
2204 | - | 152 | positionOffset followMeOffset; |
2136 | - | 153 | |
154 | |||
2204 | - | 155 | |
2136 | - | 156 | // Flags |
157 | //uint8_t COSD_FLAGS2 = 0; |
||
158 | // |
||
159 | //GPS_Pos_t last5pos[7]; |
||
160 | uint8_t FM_error = 0; |
||
161 | |||
162 | |||
163 | //--------------------- |
||
164 | // Waypoint Types |
||
165 | // TODO OG: verschieben nach: mk-data-structs.h |
||
166 | //--------------------- |
||
167 | #define POINT_TYPE_INVALID 255 |
||
168 | #define POINT_TYPE_WP 0 |
||
169 | #define POINT_TYPE_POI 1 |
||
170 | |||
171 | |||
172 | //--------------------- |
||
173 | // Status |
||
174 | // GPS_Pos_t |
||
175 | // aus MK source |
||
176 | // |
||
177 | // TODO OG: verschieben nach: mk-data-structs.h |
||
178 | //--------------------- |
||
179 | #define INVALID 0x00 |
||
180 | #define NEWDATA 0x01 |
||
181 | #define PROCESSED 0x02 |
||
182 | |||
183 | |||
184 | |||
185 | //-------------------------------------------------------------- |
||
186 | //-------------------------------------------------------------- |
||
187 | void FollowMe( void ) |
||
188 | { |
||
189 | //uint8_t status; |
||
190 | GPS_Pos_t currpos; |
||
191 | uint8_t tmp_dat; |
||
192 | uint8_t redraw; |
||
193 | uint8_t drawmode; |
||
194 | uint32_t NMEA_GPGGA_counter_old; // Merker: zaehlt empfangene GPGGA-Pakete |
||
195 | uint32_t send_followme_counter; |
||
196 | |||
197 | int8_t ok; |
||
198 | int8_t xoffs; |
||
199 | int8_t yoffs; |
||
200 | Point_t FollowMe; |
||
201 | uint8_t mktimeout = false; |
||
202 | |||
203 | |||
204 | |||
205 | GPS_PosDev_t targetdev; |
||
206 | |||
207 | |||
208 | //--------------------- |
||
209 | // 1. Daten GPS-Maus |
||
210 | //--------------------- |
||
211 | ok = GPSMouse_ShowData( GPSMOUSE_SHOW_WAITSATFIX, 500 ); // 500 = 5 Sekunden Verzoegerung nach Satfix |
||
212 | if( ok <= 0 ) |
||
213 | { |
||
214 | return; // Fehler bzgl. BT GPS-Maus -> exit |
||
215 | } |
||
216 | |||
217 | |||
218 | //--------------------- |
||
219 | // 2. Follow Me |
||
220 | //--------------------- |
||
221 | set_beep( 25, 0xffff, BeepNormal ); // kurzer Bestaetigungs-Beep |
||
222 | |||
223 | lcd_cls (); |
||
224 | |||
225 | SwitchToNC(); |
||
226 | |||
227 | mode = 'O'; |
||
228 | |||
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 | SendOutData ('o', ADDRESS_NC, 1, &tmp_dat, 1); |
||
238 | |||
239 | //OSD_active = true; // benötigt für Navidata Ausgabe an SV2 |
||
240 | |||
241 | //------------------------- |
||
242 | // Init: Timer & Flags |
||
243 | //------------------------- |
||
244 | timer_mk_timeout = MK_TIMEOUT; |
||
245 | |||
246 | abo_timer = ABO_TIMEOUT; |
||
247 | |||
248 | |||
249 | MKLiPoCells_Init(); |
||
250 | |||
251 | redraw = true; |
||
252 | |||
253 | NMEA.Counter = 0; |
||
254 | NMEA_GPGGA_counter_old = 0; |
||
255 | send_followme_counter = 0; |
||
256 | |||
257 | |||
258 | while( (receiveNMEA) ) |
||
259 | { |
||
260 | //----------------------------------------- |
||
261 | // Screen redraw |
||
262 | //----------------------------------------- |
||
263 | if( redraw ) |
||
264 | { |
||
265 | lcd_cls(); |
||
266 | |||
267 | lcdx_printf_center_P( 0, MNORMAL, 1,0, PSTR("FollowMe") ); // Titel: oben, mitte |
||
268 | |||
269 | lcd_line( (6*6-3), 0, (6*6-3), 11, 1); // Linie Vertikal links |
||
270 | lcd_line( (15*6+5), 0, (15*6+5), 11, 1); // Linie Vertikal rechts |
||
271 | lcd_line( 0, 12, 127, 12, 1); // Linie Horizontal |
||
272 | |||
273 | //lcd_line( 0, 39, 127, 39, 1); // Linie Horizontal Mitte |
||
274 | lcd_line( 66, 39, 127, 39, 1); // Linie Horizontal Mitte |
||
275 | |||
276 | lcd_rect_round( 0, 33, 66, 12, 1, R1); // Rahmen fuer "Di" (Distance) |
||
277 | |||
278 | lcdx_printf_at_P( 3, 2, MNORMAL, 3,-1, PSTR("Al:") ); // Label: "Al:" |
||
279 | |||
280 | draw_icon_mk( 1, 18); |
||
281 | draw_icon_target_round( 1, 50); |
||
282 | |||
283 | redraw = false; |
||
284 | } |
||
285 | |||
286 | |||
287 | |||
288 | //----------------------------------------- |
||
289 | // neue MK Daten vorhanden |
||
290 | //----------------------------------------- |
||
291 | if( rxd_buffer_locked ) // neue MK Daten |
||
292 | { |
||
293 | Decode64 (); |
||
294 | naviData = (NaviData_t *) pRxData; |
||
295 | |||
296 | if( mktimeout ) redraw = true; |
||
297 | mktimeout = false; |
||
298 | |||
299 | FM_error = 0; // noch benoetigt? |
||
300 | |||
301 | currpos.Latitude = naviData->CurrentPosition.Latitude; |
||
302 | currpos.Longitude = naviData->CurrentPosition.Longitude; |
||
303 | |||
304 | //---------------------------------- |
||
305 | // speichere letzte GPS-Positionen |
||
306 | //---------------------------------- |
||
307 | Config.LastLatitude = naviData->CurrentPosition.Latitude; // speichere letzte Position in Config |
||
308 | Config.LastLongitude = naviData->CurrentPosition.Longitude; // speichere letzte Position in Config |
||
309 | |||
310 | |||
311 | //---------------------------------- |
||
312 | // LiPo Cell Check |
||
313 | //---------------------------------- |
||
314 | MKLiPoCells_Check(); // ggf. Zellenzahl ermitteln |
||
315 | |||
316 | |||
317 | //################# |
||
318 | //# MK |
||
319 | //################# |
||
320 | |||
321 | //----------------- |
||
322 | // Oben: MK-Batt Level (Volt) |
||
323 | //----------------- |
||
324 | OSD_Element_BattLevel2( 0, 0, 0,0 ); |
||
325 | |||
326 | //----------------- |
||
327 | // Oben: Batt Level Bar |
||
328 | //----------------- |
||
329 | OSD_Element_Battery_Bar( 0, 9, 30, 1, ORIENTATION_H); |
||
330 | |||
331 | //----------------- |
||
332 | // Oben: MK-Flugzeit |
||
333 | //----------------- |
||
334 | writex_time(16, 0, naviData->FlyingTime, MNORMAL, 2,0); |
||
335 | |||
336 | //----------------- |
||
337 | // Hoehe |
||
338 | //----------------- |
||
339 | xoffs = 3; |
||
340 | yoffs = -1; |
||
341 | writex_altimeter( 7, 2, naviData->Altimeter, MNORMAL, xoffs,yoffs ); |
||
342 | |||
343 | //----------------- |
||
344 | // MK: Sat Anzahl |
||
345 | //----------------- |
||
346 | yoffs = -27; |
||
347 | if( naviData->SatsInUse > 5 ) drawmode = MNORMALX; |
||
348 | else drawmode = MINVERSX; |
||
349 | writex_ndigit_number_u( 17, 5, naviData->SatsInUse, 2, 0,drawmode, 3,2+yoffs); |
||
350 | draw_icon_satmini( 116+3, 42+yoffs); |
||
351 | |||
352 | |||
353 | //----------------- |
||
354 | // MK: GPS |
||
355 | //----------------- |
||
356 | writex_gpspos( 3, 4, currpos.Latitude , MNORMAL,-3,-8 ); // Line 4 L: Latitude |
||
357 | writex_gpspos(12, 4, currpos.Longitude, MNORMAL, 1,-8 ); // Line 4 R: Longitude |
||
358 | // lcdx_printf_at_P( 1, 4, MNORMAL, -3,-8 , PSTR("%d"), NMEA.Latitude); |
||
359 | // lcdx_printf_at_P( 10, 4, MNORMAL, -1,-8 , PSTR("%d"), NMEA.Longitude); |
||
360 | |||
361 | |||
362 | |||
363 | //################# |
||
364 | //# DISTANCE TO TARGET |
||
365 | //################# |
||
366 | |||
367 | //----------------- |
||
368 | // Target: Distance to Target |
||
369 | //----------------- |
||
370 | xoffs = 7; |
||
371 | yoffs = 4; |
||
372 | //GPS_PosDev_t gps_Deviation( GPS_Pos_t pos1, GPS_Pos_t pos2 ) |
||
373 | targetdev = gps_Deviation( FollowMe.Position, naviData->CurrentPosition); |
||
374 | lcdx_printf_at_P( 0, 4, MNORMAL, xoffs,yoffs , PSTR("Di: %3d m"), (targetdev.Distance / 10) ); |
||
375 | |||
376 | |||
377 | //################# |
||
378 | //# TARGET (GPS Maus) |
||
379 | //################# |
||
380 | |||
381 | yoffs = 8; |
||
382 | |||
383 | //----------------- |
||
384 | // Send Counter |
||
385 | //----------------- |
||
386 | //lcdx_printf_at_P(4, 5, MNORMAL, 0,5, PSTR("S: %ld"), send_followme_counter ); // Anzahl gesendeter Target-Positionen |
||
387 | lcdx_printf_at_P( 4, 5, MNORMAL, -3,yoffs, PSTR("Tx:%5ld"), send_followme_counter ); // Anzahl gesendeter Target-Positionen |
||
388 | |||
389 | |||
390 | //----------------- |
||
391 | // Target: Fix |
||
392 | //----------------- |
||
393 | if( NMEA.SatFix == 1 || NMEA.SatFix == 2 ) |
||
394 | drawmode = MNORMALX; |
||
395 | else |
||
396 | drawmode = MINVERSX; |
||
397 | |||
398 | lcdx_printf_at_P( 14, 5, drawmode, 1,yoffs, PSTR("F:%1d"), NMEA.SatFix ); // GPS-Maus: Satfix |
||
399 | |||
400 | |||
401 | //----------------- |
||
402 | // Target: Sat Anzahl |
||
403 | //----------------- |
||
404 | if( NMEA.SatsInUse > 5 ) |
||
405 | drawmode = MNORMALX; |
||
406 | else |
||
407 | drawmode = MINVERSX; |
||
408 | |||
409 | writex_ndigit_number_u( 17, 5, NMEA.SatsInUse, 2, 0,drawmode, 4,yoffs); |
||
410 | draw_icon_satmini( 116+4, 40+yoffs); |
||
411 | |||
412 | |||
413 | //----------------- |
||
414 | // Target: Lat / Long |
||
415 | //----------------- |
||
416 | writex_gpspos( 3, 7, NMEA.Latitude , MNORMAL,-3,1 ); // GPS-Maus: Latitude |
||
417 | writex_gpspos(12, 7, NMEA.Longitude, MNORMAL, 1,1 ); // GPS-Maus: Longitude |
||
418 | |||
419 | rxd_buffer_locked = FALSE; |
||
420 | |||
421 | timer_mk_timeout = MK_TIMEOUT; |
||
422 | } |
||
423 | |||
424 | //----------------------------------------- |
||
425 | // if( !NMEA_receiveBT() ) |
||
426 | // { |
||
427 | // receiveNMEA = false; |
||
428 | // } |
||
429 | // else |
||
430 | // { |
||
431 | // NMEA_GetNewData(); |
||
432 | // } |
||
433 | |||
434 | |||
435 | // Sourcecode aus NaviCtrl/V2.10e/uart1.c |
||
436 | // FOLLOW_ME |
||
437 | // else if(CheckDelay(UART1_FollowMe_Timer) && (UART1_tx_buffer.Locked == FALSE)) |
||
438 | // { |
||
439 | // if((GPSData.Status != INVALID) && (GPSData.SatFix == SATFIX_3D) && (GPSData.Flags & FLAG_GPSFIXOK) && (GPSData.NumOfSats >= 4)) |
||
440 | // { |
||
441 | // TransmitAlsoToFC = 1; |
||
442 | // // update FollowMe content |
||
443 | // FollowMe.Position.Longitude = GPSData.Position.Longitude; |
||
444 | // FollowMe.Position.Latitude = GPSData.Position.Latitude; |
||
445 | // FollowMe.Position.Status = NEWDATA; |
||
446 | // FollowMe.Position.Altitude = 1; |
||
447 | // // 0 -> no Orientation |
||
448 | // // 1-360 -> CompassCourse Setpoint |
||
449 | // // -1 -> points to WP1 -> itself |
||
450 | // FollowMe.Heading = -1; |
||
451 | // FollowMe.ToleranceRadius = 1; |
||
452 | // FollowMe.HoldTime = 60; |
||
453 | // FollowMe.Event_Flag = 1; |
||
454 | // FollowMe.Index = 1; // 0 = Delete List, 1 place at first entry in the list |
||
455 | // FollowMe.Type = POINT_TYPE_WP; |
||
456 | // FollowMe.WP_EventChannelValue = 100; // set servo value |
||
457 | // FollowMe.AltitudeRate = 0; // do not change height |
||
458 | // FollowMe.Speed = 0; // rate to change the Position (0 = max) |
||
459 | // FollowMe.CamAngle = 255; // Camera servo angle in degree (255 -> POI-Automatic) |
||
460 | // FollowMe.Name[0] = 'F'; // Name of that point (ASCII) |
||
461 | // FollowMe.Name[1] = 'O'; // Name of that point (ASCII) |
||
462 | // FollowMe.Name[2] = 'L'; // Name of that point (ASCII) |
||
463 | // FollowMe.Name[3] = 'L'; // Name of that point (ASCII) |
||
464 | // FollowMe.reserve[0] = 0; // reserve |
||
465 | // FollowMe.reserve[1] = 0; // reserve |
||
466 | // MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 's', NC_ADDRESS, 1, (u8 *)&FollowMe, sizeof(FollowMe)); |
||
467 | // } |
||
468 | // UART1_FollowMe_Timer = SetDelay(FOLLOW_ME_INTERVAL); // set new update time |
||
469 | // } |
||
470 | // |
||
471 | |||
2204 | - | 472 | #ifdef USE_FOLLOWME_STEP2 |
2136 | - | 473 | |
2204 | - | 474 | sendFollowMeData(&FollowMe, &send_followme_counter, &NMEA_GPGGA_counter_old); |
2136 | - | 475 | |
2204 | - | 476 | #else |
2136 | - | 477 | |
478 | //----------------------------------------- |
||
479 | // neue NMEA Daten? |
||
480 | //----------------------------------------- |
||
481 | if(NMEA_isdataready() && receiveNMEA) |
||
482 | { |
||
483 | if( NMEA.Counter > NMEA_GPGGA_counter_old ) |
||
484 | { |
||
485 | if( (NMEA.SatsInUse > 5) && (NMEA.SatFix == 1 || NMEA.SatFix == 2) ) |
||
486 | { |
||
487 | //Config.FM_Refresh |
||
488 | |||
489 | FollowMe.Position.Status = NEWDATA; |
||
490 | FollowMe.Position.Longitude = NMEA.Longitude; |
||
491 | FollowMe.Position.Latitude = NMEA.Latitude; |
||
492 | FollowMe.Position.Altitude = 1; // 20.7.2015 CB |
||
493 | // FollowMe.Position.Altitude = NMEA.Altitude; // ist das wirklich ok? NEIN C.B. |
||
494 | |||
495 | FollowMe.Heading = -1; // invalid heading |
||
496 | FollowMe.ToleranceRadius = Config.FM_Radius; // 5 meter default |
||
497 | FollowMe.HoldTime = 60; // ????? go home after 60s without any update ?????? |
||
498 | // FollowMe.Event_Flag = 0; // no event |
||
499 | FollowMe.Event_Flag = 1; // 20.7.2015 CB |
||
500 | FollowMe.Index = 1; // 2st wp, 0 = Delete List, 1 place at first entry in the list |
||
501 | FollowMe.Type = POINT_TYPE_WP; // Typ des Wegpunktes |
||
502 | FollowMe.Name[0] = 'F'; // Name des Wegpunktes (ASCII) |
||
503 | FollowMe.Name[1] = 'O'; |
||
504 | FollowMe.Name[2] = 'L'; |
||
505 | FollowMe.Name[3] = 'L'; |
||
506 | // FollowMe.WP_EventChannelValue = 0; // Will be transferred to the FC and can be used as Poti value there |
||
507 | FollowMe.WP_EventChannelValue = 100; // set servo value 20.7.2015 |
||
508 | FollowMe.AltitudeRate = 0; // rate to change the Aetpoint |
||
509 | FollowMe.Speed = Config.FM_Speed; // rate to change the Position |
||
510 | FollowMe.CamAngle = 255; // Camera servo angle in degree (255 -> POI-Automatic) |
||
511 | FollowMe.reserve[0] = 0; // reserve |
||
512 | FollowMe.reserve[1] = 0; // reserve |
||
513 | |||
514 | SendOutData( 's', ADDRESS_NC, 1, &FollowMe, sizeof(FollowMe) ); //'s' = target Position 'w' = Waypoint |
||
515 | send_followme_counter++; |
||
516 | |||
517 | //void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) // uint8_t *pdata, uint8_t len, ... |
||
518 | // SendOutData ('d', ADDRESS_ANY, 1, &tmp_dat, 1); |
||
519 | //SendOutData( 's', NC_ADDRESS, 1, (uint8_t *)&FollowMe, sizeof(FollowMe)); //'s' = target Position 'w' = Waypoint |
||
520 | |||
521 | } |
||
522 | |||
523 | NMEA_GPGGA_counter_old = NMEA.Counter; |
||
524 | } |
||
525 | } |
||
526 | |||
2204 | - | 527 | #endif |
528 | |||
2136 | - | 529 | //----------------------------------------- |
530 | // TASTEN |
||
531 | //----------------------------------------- |
||
532 | if( get_key_press(1 << KEY_ESC) ) |
||
533 | { |
||
534 | break; |
||
535 | } |
||
536 | |||
537 | if( get_key_press(1 << KEY_ENTER) ) |
||
538 | { |
||
539 | break; |
||
540 | } |
||
541 | |||
542 | //----------------------------------------- |
||
543 | //----------------------------------------- |
||
544 | if( !abo_timer ) |
||
545 | { |
||
546 | // renew abo every 3 sec |
||
547 | // request OSD Data from NC every 100ms |
||
548 | // RS232_request_mk_data (1, 'o', 100); |
||
549 | tmp_dat = 10; |
||
550 | SendOutData ( 'o', ADDRESS_NC, 1, &tmp_dat, 1); |
||
551 | |||
552 | abo_timer = ABO_TIMEOUT; |
||
553 | } |
||
554 | |||
555 | //-------------------------- |
||
556 | // Daten Timeout vom MK? |
||
557 | //-------------------------- |
||
558 | if( timer_mk_timeout == 0 ) |
||
559 | { |
||
560 | if( !mktimeout ) OSD_MK_ShowTimeout(); // nur anzeigen wenn noch nicht im mktimeout-Modus |
||
561 | set_beep ( 200, 0x0080, BeepNormal); // Beep |
||
562 | mktimeout = true; |
||
563 | timer_mk_timeout = MK_TIMEOUT; |
||
564 | //OSDScreenRefresh = OSD_SCREEN_REDRAW; |
||
565 | // OSD_MK_Connect( MK_CONNECT ); |
||
566 | } |
||
567 | |||
568 | } // end: while( (receiveNMEA) ); |
||
569 | |||
2204 | - | 570 | OSD_active = false; |
2136 | - | 571 | |
2204 | - | 572 | GPSMouse_Disconnect(); |
573 | } |
||
2136 | - | 574 | |
575 | |||
576 | |||
2204 | - | 577 | |
578 | // Funktion sendFollowMeData angepasst auf Zeiger |
||
579 | // ausgelagert als Funktion, da auch von FollowMeStep2 verwendet wird |
||
580 | // TODO: testen mit Simulator und Kopter :-) |
||
581 | |||
582 | void sendFollowMeData(Point_t *tFollowMe, uint32_t *tsend_followme_counter, uint32_t *tNMEA_GPGGA_counter_old) |
||
583 | { |
||
584 | Point_t FollowMe = *tFollowMe; |
||
585 | |||
586 | if(NMEA_isdataready() && receiveNMEA) |
||
587 | { |
||
588 | if( NMEA.Counter > *tNMEA_GPGGA_counter_old ) |
||
589 | { |
||
590 | if( (NMEA.SatsInUse > 5) && (NMEA.SatFix == 1 || NMEA.SatFix == 2) ) |
||
591 | { |
||
592 | FollowMe.Position.Status = NEWDATA; |
||
593 | FollowMe.Position.Longitude = NMEA.Longitude; |
||
594 | FollowMe.Position.Latitude = NMEA.Latitude; |
||
595 | FollowMe.Position.Altitude = 1; // 20.7.2015 CB |
||
596 | |||
597 | FollowMe.Heading = -1; // invalid heading |
||
598 | FollowMe.ToleranceRadius = Config.FM_Radius; // 5 meter default |
||
599 | FollowMe.HoldTime = 60; // ????? go home after 60s without any update ?????? |
||
600 | FollowMe.Event_Flag = 1; // 20.7.2015 CB |
||
601 | FollowMe.Index = 1; // 2st wp, 0 = Delete List, 1 place at first entry in the list |
||
602 | FollowMe.Type = POINT_TYPE_WP; // Typ des Wegpunktes |
||
603 | FollowMe.Name[0] = 'F'; // Name des Wegpunktes (ASCII) |
||
604 | FollowMe.Name[1] = 'O'; |
||
605 | FollowMe.Name[2] = 'L'; |
||
606 | FollowMe.Name[3] = 'L'; |
||
607 | FollowMe.WP_EventChannelValue = 100; // set servo value 20.7.2015 |
||
608 | FollowMe.AltitudeRate = 0; // rate to change the Aetpoint |
||
609 | FollowMe.Speed = Config.FM_Speed; // rate to change the Position |
||
610 | FollowMe.CamAngle = 255; // Camera servo angle in degree (255 -> POI-Automatic) |
||
611 | FollowMe.reserve[0] = 0; // reserve |
||
612 | FollowMe.reserve[1] = 0; // reserve |
||
613 | |||
614 | SendOutData( 's', ADDRESS_NC, 1, &FollowMe, sizeof(FollowMe) ); //'s' = target Position 'w' = Waypoint |
||
615 | *tsend_followme_counter += 1; |
||
616 | } |
||
617 | |||
618 | *tNMEA_GPGGA_counter_old = NMEA.Counter; |
||
619 | } |
||
620 | } |
||
2136 | - | 621 | } |
622 | |||
623 | |||
624 | |||
625 | |||
2204 | - | 626 | // FollowMeStep2: |
2136 | - | 627 | |
2200 | - | 628 | #ifdef USE_FOLLOWME_STEP2 |
2136 | - | 629 | void Debug_GPS (void) |
630 | { |
||
2199 | - | 631 | uint8_t redraw; |
2195 | - | 632 | |
2199 | - | 633 | set_beep( 25, 0xffff, BeepNormal ); // kurzer Bestaetigungs-Beep |
634 | redraw = true; |
||
2136 | - | 635 | |
2200 | - | 636 | int retcode = GPSMouse_Connect(); // Abfrage der GPS-Daten zum testen Quick an Dirty ;-) |
637 | if( retcode <= 0 ) |
||
638 | { |
||
639 | return; |
||
640 | } |
||
2195 | - | 641 | |
2199 | - | 642 | while( true ) |
643 | { |
||
2204 | - | 644 | NMEApos.lat = NMEA.Latitude; |
645 | NMEApos.lon = NMEA.Longitude; |
||
2136 | - | 646 | |
2199 | - | 647 | if( redraw ) |
648 | { |
||
649 | lcd_cls(); |
||
2136 | - | 650 | |
2200 | - | 651 | lcdx_printf_center_P( 0, MNORMAL, 1,0, PSTR("FollowMeStep2") ); // Titel: oben, mitte |
652 | lcdx_printf_center_P( 1, MNORMAL, 1,0, PSTR(" Source Lat/Lon") ); |
||
653 | lcdx_printf_center_P( 3, MNORMAL, 1,0, PSTR(" Target Lat/Lon") ); |
||
2136 | - | 654 | |
2199 | - | 655 | redraw = false; |
656 | } |
||
2136 | - | 657 | |
2199 | - | 658 | //################# |
659 | //# DISTANCE TO TARGET |
||
660 | //################# |
||
2136 | - | 661 | |
2200 | - | 662 | //lcdx_printf_at_P( 0, 1, MNORMAL,0,0, PSTR("Distance: %3d Meter"), Config.FM_Distance ); |
2136 | - | 663 | |
2199 | - | 664 | //################# |
665 | //# TARGET Azimuth |
||
666 | //################# |
||
2200 | - | 667 | //lcdx_printf_at_P( 0, 2, MNORMAL, 0,0, PSTR(" Azimuth: %3d Grad"), Config.FM_Azimuth); |
2136 | - | 668 | |
669 | |||
2200 | - | 670 | writex_gpspos( 1, 2, NMEApos.lat , MNORMAL,0,0 ); // GPS-Maus: Latitude |
671 | writex_gpspos(10, 2, NMEApos.lon, MNORMAL, 0,0 ); // GPS-Maus: Longitude |
||
2136 | - | 672 | |
2204 | - | 673 | //followMeOffset.offset_lat = 10000; |
674 | //followMeOffset.offset_long = 10000; |
||
2136 | - | 675 | |
2204 | - | 676 | followme_calculate_offset(Config.FM_Distance, Config.FM_Azimuth, &followMeOffset); |
677 | followme_add_offset(&NMEApos, &NMEATarget, &followMeOffset); |
||
678 | |||
2200 | - | 679 | writex_gpspos( 1, 4, (int32_t)NMEATarget.lat , MNORMAL, 0,0 ); // Ziel Latitude |
680 | writex_gpspos(10, 4, (int32_t)NMEATarget.lon , MNORMAL, 0,0 ); // Ziel Longitude |
||
2136 | - | 681 | |
682 | |||
2199 | - | 683 | if( get_key_press(1 << KEY_ESC) ) |
684 | { |
||
2200 | - | 685 | GPSMouse_Disconnect(); |
2199 | - | 686 | break; |
687 | } |
||
2136 | - | 688 | |
2199 | - | 689 | if( get_key_press(1 << KEY_ENTER) ) |
690 | { |
||
691 | redraw = true; |
||
692 | // break; |
||
693 | } |
||
2195 | - | 694 | } |
2136 | - | 695 | } |
696 | |||
2195 | - | 697 | |
698 | |||
699 | |||
2199 | - | 700 | |
2204 | - | 701 | #endif // FOLLOW_ME_STEP2 |
2136 | - | 702 | |
703 | #endif // #ifdef USE_FOLLOWME |
||
704 | |||
705 |