Subversion Repositories Projects

Rev

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