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