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