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