Go to most recent revision | Details | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
2136 | - | 1 | |
2 | /****************************************************************/ |
||
3 | /* */ |
||
4 | /* NG-Video 5,8GHz */ |
||
5 | /* */ |
||
6 | /* Copyright (C) 2011 - gebad */ |
||
7 | /* */ |
||
8 | /* This code is distributed under the GNU Public License */ |
||
9 | /* which can be found at http://www.gnu.org/licenses/gpl.txt */ |
||
10 | /* */ |
||
11 | /****************************************************************/ |
||
12 | |||
13 | //############################################################################ |
||
14 | //# HISTORY servo_setup.c |
||
15 | //# |
||
16 | //# 27.06.2014 OG |
||
17 | //# - del: #include "tools.h" |
||
18 | //# - del: #include "mymath.h" |
||
19 | //# |
||
20 | //# 30.05.2014 OG |
||
21 | //# - chg: etliche Edit-Anzeigen/Funktionen auf neues Layout umgeschrieben; |
||
22 | //# einige Optimierungen an Aufruffunktionen (unnoetige Menue- |
||
23 | //# Interfacefunktionen entfernt) |
||
24 | //# |
||
25 | //# 13.05.2014 OG |
||
26 | //# - chg: Menu_Test_Start() - Variable 'range' auskommentiert |
||
27 | //# wegen Warning: variable set but not used |
||
28 | //# - chg: Displ_PulseWidth() - Variable 'me' auskommentiert |
||
29 | //# wegen Warning: variable set but not used |
||
30 | //# - chg: Change_Value_plmi() - Variable 'tmp_val' entfernt |
||
31 | //# wegen Warning: variable set but not used |
||
32 | //# |
||
33 | //# 11.05.2014 OG |
||
34 | //# - chg: einge Setup-Menues umgestellt auf MenuCtrl_SetTitleFromParentItem() |
||
35 | //# -> die Menues 'erben' damit ihren Titel vom aufrufenden Menuepunkt |
||
36 | //# |
||
37 | //# 30.03.2014 OG |
||
38 | //# - chg: ein paar englische Texte auf DE gemappt weil der Unterschied unerheblich war |
||
39 | //# - chg: Sprache Hollaendisch vollstaendig entfernt |
||
40 | //# - chg: MenuCtrl_PushML_P() umgestellt auf MenuCtrl_PushML2_P() |
||
41 | //# |
||
42 | //# 25.08.2013 Cebra |
||
43 | //# - add: #ifdef USE_TRACKING |
||
44 | //# |
||
45 | //# 30.05.2013 cebra |
||
46 | //# - chg: doppelte Texte auf #define umgestellt |
||
47 | //# |
||
48 | //# 24.05.2013 OG |
||
49 | //# - chg: Aufrufe von MenuCtrl_Push() umgestellt auf MenuCtrl_PushML_P() |
||
50 | //# |
||
51 | //# 22.05.2013 OG |
||
52 | //# - umgestellt auf menuctrl: Setup_ServoTracking(), Setup_ServoTest(), |
||
53 | //# Setup_ServoTestCont(), Setup_ServoAdjust() |
||
54 | //# - del: include utils/menuold.h |
||
55 | //# |
||
56 | //# 17.05.2013 OG |
||
57 | //# - chg: umgestellt auf utils/menuold.h |
||
58 | //# |
||
59 | //# 16.04.2013 Cebra |
||
60 | //# - chg: PROGMEM angepasst auf neue avr-libc und GCC, prog_char depreciated |
||
61 | //# |
||
62 | //# 04.04.2013 Cebra |
||
63 | //# - chg: Texttuning |
||
64 | //# |
||
65 | //# 03.04.2013 Cebra |
||
66 | //# - chg: Holländische Menütexte |
||
67 | //# |
||
68 | //# 27.03.2013 Cebra |
||
69 | //# - chg: Fehler bei der Men�steuerung behoben |
||
70 | //############################################################################ |
||
71 | |||
72 | #include <stdio.h> |
||
73 | #include <stdlib.h> |
||
74 | #include <string.h> |
||
75 | #include <math.h> |
||
76 | #include "../cpu.h" |
||
77 | #include <util/delay.h> |
||
78 | #include <avr/pgmspace.h> |
||
79 | #include <avr/interrupt.h> |
||
80 | #include "../main.h" |
||
81 | #include "../timer/timer.h" |
||
82 | #include "servo_setup.h" |
||
83 | #include "tracking.h" |
||
84 | #include "../lcd/lcd.h" |
||
85 | #include "../timer/timer.h" |
||
86 | #include "../menu.h" |
||
87 | #include "../messages.h" |
||
88 | #include "../mk-data-structs.h" |
||
89 | #include "../uart/usart.h" |
||
90 | #include "../osd/osd.h" |
||
91 | #include "../eeprom/eeprom.h" |
||
92 | #include "../setup/setup.h" |
||
93 | #include "../utils/menuctrl.h" |
||
94 | #include "../pkt/pkt.h" |
||
95 | |||
96 | |||
97 | #ifdef USE_TRACKING |
||
98 | |||
99 | |||
100 | //#include "ng_usart.h" |
||
101 | //#include "ng_config.h" |
||
102 | //#include "servo.h" |
||
103 | //#include "tools.h" |
||
104 | //#include "mk.h" |
||
105 | //#include "keys.h" |
||
106 | //#include "mymath.h" |
||
107 | // |
||
108 | |||
109 | //GPS_Pos_t last5pos[7]; |
||
110 | //uint8_t error1 = 0; |
||
111 | //NaviData_t *naviData; |
||
112 | //HomePos_t MK_pos; // Home position of station |
||
113 | //GPS_Pos_t currentPos; // Current position of flying object |
||
114 | //int8_t satsInUse; // Number of satelites currently in use |
||
115 | |||
116 | //uint8_t tracking = TRACKING_MIN; |
||
117 | //uint8_t track_hyst = TRACKING_HYSTERESE; |
||
118 | //uint8_t track_tx = 0; |
||
119 | |||
120 | //geo_t geo; |
||
121 | //int16_t anglePan, angleTilt; // Servo Winkel |
||
122 | //uint8_t coldstart = 1; |
||
123 | |||
124 | uint8_t servo_nr; // zwischen Servo 1 und 2 wird nur mit global servo_nr unterschieden |
||
125 | //uint8_t FCStatusFlags; |
||
126 | |||
127 | |||
128 | //############################################################################ |
||
129 | |||
130 | //----------------------------- |
||
131 | // Setup_ServoTracking() |
||
132 | //----------------------------- |
||
133 | #define MSERVO_STEPS 1 |
||
134 | #define MSERVO_SERVO1 2 |
||
135 | #define MSERVO_SERVO2 3 |
||
136 | #define MSERVO_SERVOTEST 4 |
||
137 | |||
138 | static const char MSERVO_STEPS_de[] PROGMEM = "Servoschritte"; |
||
139 | static const char MSERVO_STEPS_en[] PROGMEM = "servo steps"; |
||
140 | |||
141 | static const char MSERVO_SERVO1_de[] PROGMEM = "Servo 1"; |
||
142 | #define MSERVO_SERVO1_en MSERVO_SERVO1_de |
||
143 | |||
144 | static const char MSERVO_SERVO2_de[] PROGMEM = "Servo 2"; |
||
145 | #define MSERVO_SERVO2_en MSERVO_SERVO2_de |
||
146 | |||
147 | static const char MSERVO_SERVOTEST_de[] PROGMEM = "Servotest"; |
||
148 | #define MSERVO_SERVOTEST_en MSERVO_SERVOTEST_de |
||
149 | |||
150 | |||
151 | //############################################################################ |
||
152 | |||
153 | //----------------------------- |
||
154 | // Setup_ServoTest() |
||
155 | //----------------------------- |
||
156 | #define MSERVOT_PULS 1 |
||
157 | #define MSERVOT_CONT 2 |
||
158 | #define MSERVOT_SERVO 3 |
||
159 | #define MSERVOT_FRAME 4 |
||
160 | |||
161 | static const char MSERVOT_PULS_de[] PROGMEM = "Test Pulslänge"; |
||
162 | static const char MSERVOT_PULS_en[] PROGMEM = "test puls width"; |
||
163 | |||
164 | static const char MSERVOT_CONT_de[] PROGMEM = "Test Fortlfd."; |
||
165 | static const char MSERVOT_CONT_en[] PROGMEM = "test cont."; |
||
166 | |||
167 | static const char MSERVOT_SERVO_de[] PROGMEM = "Servo"; |
||
168 | #define MSERVOT_SERVO_en MSERVOT_SERVO_de |
||
169 | |||
170 | static const char MSERVOT_FRAME_de[] PROGMEM = "Periode"; |
||
171 | static const char MSERVOT_FRAME_en[] PROGMEM = "frame"; |
||
172 | |||
173 | //############################################################################ |
||
174 | |||
175 | //----------------------------- |
||
176 | // Setup_ServoTestCont() |
||
177 | //----------------------------- |
||
178 | #define MSERVOTC_START 1 |
||
179 | #define MSERVOTC_SINGLE 2 |
||
180 | #define MSERVOTC_COUNT 3 |
||
181 | #define MSERVOTC_PAUSEEND 4 |
||
182 | #define MSERVOTC_PAUSEINC 5 |
||
183 | |||
184 | static const char MSERVOTC_START_de[] PROGMEM = "Start Test"; |
||
185 | static const char MSERVOTC_START_en[] PROGMEM = "start test"; |
||
186 | |||
187 | static const char MSERVOTC_SINGLE_de[] PROGMEM = "Einzelschritt"; |
||
188 | static const char MSERVOTC_SINGLE_en[] PROGMEM = "single step"; |
||
189 | |||
190 | static const char MSERVOTC_COUNT_de[] PROGMEM = "Anzahl Test"; |
||
191 | static const char MSERVOTC_COUNT_en[] PROGMEM = "number of test"; |
||
192 | |||
193 | static const char MSERVOTC_PAUSEEND_de[] PROGMEM = "Pause Endpos."; |
||
194 | static const char MSERVOTC_PAUSEEND_en[] PROGMEM = "pause end pos"; |
||
195 | |||
196 | static const char MSERVOTC_PAUSEINC_de[] PROGMEM = "Pause pro Inc."; |
||
197 | static const char MSERVOTC_PAUSEINC_en[] PROGMEM = "pause proc inc."; |
||
198 | |||
199 | //############################################################################ |
||
200 | |||
201 | //----------------------------- |
||
202 | // Setup_ServoAdjust() |
||
203 | //----------------------------- |
||
204 | #define MSERVOADJ_REV 1 |
||
205 | #define MSERVOADJ_LEFT 2 |
||
206 | #define MSERVOADJ_RIGHT 3 |
||
207 | #define MSERVOADJ_MID 4 |
||
208 | |||
209 | static const char MSERVOADJ_REV_de[] PROGMEM = "Reverse"; |
||
210 | static const char MSERVOADJ_REV_en[] PROGMEM = "reverse"; |
||
211 | |||
212 | static const char MSERVOADJ_LEFT_de[] PROGMEM = "Links"; |
||
213 | static const char MSERVOADJ_LEFT_en[] PROGMEM = "left"; |
||
214 | |||
215 | static const char MSERVOADJ_RIGHT_de[] PROGMEM = "Rechts"; |
||
216 | static const char MSERVOADJ_RIGHT_en[] PROGMEM = "right"; |
||
217 | |||
218 | static const char MSERVOADJ_MID_de[] PROGMEM = "Mitte"; |
||
219 | static const char MSERVOADJ_MID_en[] PROGMEM = "middle"; |
||
220 | |||
221 | //############################################################################ |
||
222 | |||
223 | |||
224 | |||
225 | //-------------------------------------------------------------- |
||
226 | //-------------------------------------------------------------- |
||
227 | void draw_input_screen( void ) |
||
228 | { |
||
229 | lcd_cls (); |
||
230 | PKT_TitleFromMenuItem( true ); |
||
231 | lcdx_printf_at_P( 0, 2, MNORMAL, 0,0, PSTR("%S:"), MenuCtrl_GetItemText() ); // Menuetext muss im PGM sein! (aktuell keine Unterscheidung RAM/PGM) |
||
232 | lcd_printp_at(0, 7, strGet(KEYLINE2), MNORMAL); |
||
233 | } |
||
234 | |||
235 | |||
236 | |||
237 | ///************************************************************************************/ |
||
238 | ///* */ |
||
239 | ///* �ndern der Werte mit Tasten +,- und Anzeige */ |
||
240 | ///* z.B. f�r U-Offset, Batterie leer Eingabe ... */ |
||
241 | ///* */ |
||
242 | ///* Parameter: */ |
||
243 | ///* uint16_t val :zu �ndernter Wert */ |
||
244 | ///* uint16_t min_val, max_val :min, max Grenze Wert �ndern darf */ |
||
245 | ///* uint8_t posX, posY :Darstellung Wert xPos, YPos auf LCD */ |
||
246 | ///* Displ_Fnct_t Displ_Fnct :Index um variable Display Funktion aufzurufen */ |
||
247 | ///* uint8_t cycle :0 begrenzt Anzeige bei man_val, bzw. max_val */ |
||
248 | ///* :1 springt nach max_val auf min_val und umgedreht */ |
||
249 | ///* uint8_t vrepeat :beschleunigte Repeat-Funktion aus/ein */ |
||
250 | ///* uint16_t Change_Value_plmi(...) :R�ckgabe ge�nderter Wert */ |
||
251 | ///* */ |
||
252 | ///************************************************************************************/ |
||
253 | |||
254 | void Servo_tmp_Original(uint8_t track) |
||
255 | { |
||
256 | servoSetDefaultPos(); |
||
257 | // tracking = track; // urspr�nglicher Wert Tracking aus, RSSI oder GPS |
||
258 | // NoTracking_ServosOff(); // Servos sind nur zum Tracking oder bei Kalibrierung eingeschaltet |
||
259 | // Jump_Menu(pmenu); |
||
260 | } |
||
261 | |||
262 | uint8_t Servo_tmp_on(uint8_t servo_period) |
||
263 | { |
||
264 | // uint8_t tmp_tracking = tracking; |
||
265 | |||
266 | // tracking = 0; // Servopositionierung durch tracking abschalten |
||
267 | // if (tracking == TRACKING_MIN) servoInit(servo_period); // falls aus, Servos einschalten |
||
268 | servoInit(servo_period); |
||
269 | // lcdGotoXY(0, 0); // lcd Cursor vorpositionieren |
||
270 | // return(tmp_tracking); |
||
271 | return (0); |
||
272 | } |
||
273 | |||
274 | |||
275 | void Displ_Off_On(uint16_t val) |
||
276 | { |
||
277 | if (val == 0) lcd_printp_at(17, 2, strGet(OFF), 0); else lcd_printp_at(17, 2, strGet(ON), 0); |
||
278 | } |
||
279 | |||
280 | |||
281 | |||
282 | uint16_t Change_Value_plmi(uint16_t val, uint16_t min_val, uint16_t max_val, uint8_t posX, uint8_t posY,Displ_Fnct_t Displ_Fnct) |
||
283 | { |
||
284 | // >> Menueauswahl nach oben |
||
285 | |||
286 | if (get_key_press (1 << KEY_PLUS) || get_key_long_rpt_sp ((1 << KEY_PLUS), 3)) |
||
287 | { |
||
288 | if (val < max_val) { |
||
289 | edit = 1; |
||
290 | val++; |
||
291 | |||
292 | } |
||
293 | else |
||
294 | { |
||
295 | val = min_val; |
||
296 | } |
||
297 | Displ_Fnct(val); // ge�nderten Wert darstellen, je nach Men�punkt |
||
298 | |||
299 | |||
300 | } |
||
301 | // >> Menueauswahl nach unten |
||
302 | |||
303 | if (get_key_press (1 << KEY_MINUS) || get_key_long_rpt_sp ((1 << KEY_MINUS), 3)) |
||
304 | { |
||
305 | if (val > min_val) { |
||
306 | val--; |
||
307 | edit = 1; |
||
308 | |||
309 | } |
||
310 | else |
||
311 | { |
||
312 | val = max_val; |
||
313 | } |
||
314 | Displ_Fnct(val); // ge�nderten Wert darstellen, je nach Men�punkt |
||
315 | |||
316 | |||
317 | } |
||
318 | |||
319 | return(val); |
||
320 | } |
||
321 | // |
||
322 | ///************************************************************************************/ |
||
323 | ///* */ |
||
324 | ///* �ndern der Werte mit Tasten +,- repetierend; (long)Entertaste und Anzeige */ |
||
325 | ///* z.B. f�r U-Offset, Batterie leer Eingabe ... */ |
||
326 | ///* */ |
||
327 | ///* Parameter: */ |
||
328 | ///* uint16_t *val :zu �ndernter Wert */ |
||
329 | ///* uint16_t min_val, max_val :min, max Grenze Wert �ndern darf */ |
||
330 | ///* uint8_t fl_pos :Bit 7 beschleunigte Repeat-Funktion aus/ein */ |
||
331 | ///* Bit 6 zyklische Werte�nderung aus/ein */ |
||
332 | ///* Bit 4-5 z.Z. ohne Funktion */ |
||
333 | ///* Bit 0-3 Wert xPos auf LCD */ |
||
334 | ///* Displ_Fnct_t Displ_Fnct :Index um variable Display Funktion aufzurufen */ |
||
335 | ///* uint8_t Change_Value(...) :R�ckgabe ge�ndert ergibt TRUE */ |
||
336 | ///* */ |
||
337 | ///************************************************************************************/ |
||
338 | //// Bei Bedarf k�nnte einfach innerhalp fl_pos auch noch pos_y (Bit 4-5) �bergeben werden |
||
339 | uint8_t Change_Value(uint16_t *val, uint16_t min_val, uint16_t max_val,Displ_Fnct_t Displ_Fnct) |
||
340 | { |
||
341 | uint16_t tmp_val; |
||
342 | |||
343 | tmp_val = *val; |
||
344 | Displ_Fnct(tmp_val); // initiale Wertdarstellung, je nach Men�punkt |
||
345 | while(!get_key_press(1<<KEY_ENTER) && !get_key_press(1<<KEY_ESC)) |
||
346 | *val = Change_Value_plmi(*val, min_val, max_val, 16,2, Displ_Fnct); |
||
347 | |||
348 | |||
349 | if (*val == tmp_val) { |
||
350 | edit = 0; |
||
351 | // lcd_printp_at (0, 5, PSTR("Edit=0"), 0); |
||
352 | // _delay_ms(500); |
||
353 | //// return (*val); |
||
354 | } |
||
355 | // |
||
356 | else |
||
357 | { |
||
358 | edit = 1; |
||
359 | // lcd_printp_at (0, 5, PSTR("Edit=1"), 0); |
||
360 | // _delay_ms(500); |
||
361 | } |
||
362 | |||
363 | return (tmp_val != *val); |
||
364 | |||
365 | |||
366 | } |
||
367 | |||
368 | uint16_t calc_range(int16_t PosProzent, int16_t min, int16_t max, int16_t mid) |
||
369 | { uint16_t range; |
||
370 | |||
371 | if (PosProzent < 0) { |
||
372 | range = mid - min; |
||
373 | // if (chrxs == CHRRS) { // falls Richtung ge�ndert, anderen Zeichensatz laden |
||
374 | // chrxs = CHRLS; |
||
375 | //// lcdWriteCGRAM_Array(lcdSpecialChrLs, 5);// LCD-Char mit Rahmensymbole vom Graph |
||
376 | // } |
||
377 | } |
||
378 | else { |
||
379 | range = max - mid; |
||
380 | // if (chrxs == CHRLS) { // falls Richtung ge�ndert, anderen Zeichensatz laden |
||
381 | //// lcdWriteCGRAM_Array(lcdSpecialChrRs, 5);// LCD-Char mit Rahmensymbole vom Graph |
||
382 | // chrxs = CHRRS; |
||
383 | // } |
||
384 | } |
||
385 | return(range); |
||
386 | } |
||
387 | |||
388 | |||
389 | |||
390 | /************************************************************************************/ |
||
391 | /* zeigt einen max. 3-stelligen Integerwert auf Display an */ |
||
392 | /* Parameter: */ |
||
393 | /* uint16_t val :anzuzeigender Wert, */ |
||
394 | /* uint16_t wegen Vereinheitlichung f. Funktionsaufrauf */ |
||
395 | /* */ |
||
396 | /************************************************************************************/ |
||
397 | void Displ_Format_Int( uint16_t val ) |
||
398 | { |
||
399 | lcdx_printf_at_P( 17, 2, MNORMAL, 0,0, PSTR("%3d"), val ); |
||
400 | } |
||
401 | |||
402 | |||
403 | |||
404 | //-------------------------------------------------------------- |
||
405 | //-------------------------------------------------------------- |
||
406 | void Displ_PulseWidth( uint16_t val ) |
||
407 | { |
||
408 | int16_t PosProzent, range; |
||
409 | |||
410 | servoSetPositionRaw( servo_nr, val); |
||
411 | |||
412 | PosProzent = val - steps_pw[Config.sIdxSteps].mid; |
||
413 | range = calc_range(PosProzent, steps_pw[Config.sIdxSteps].min, steps_pw[Config.sIdxSteps].max, steps_pw[Config.sIdxSteps].mid); |
||
414 | PosProzent = (int32_t)1000 * PosProzent / range; |
||
415 | |||
416 | lcdx_printf_at_P( 15, 2, MNORMAL, 0,0, PSTR("%4.1d"), PosProzent ); |
||
417 | } |
||
418 | |||
419 | |||
420 | |||
421 | /************************************************************************************/ |
||
422 | /* zeigt Pausenl�nge der Links-, Mittel- und Rechtsposition auf Display an */ |
||
423 | /* Parameter: */ |
||
424 | /* uint16_t val : Zeit in 1ms * 100 */ |
||
425 | /* */ |
||
426 | /************************************************************************************/ |
||
427 | void Displ_Pause(uint16_t val) |
||
428 | { |
||
429 | lcdx_printf_at_P( 17, 2, MNORMAL, 0,0, PSTR("%1.1d"), val ); |
||
430 | //lcdPuts("s "); |
||
431 | } |
||
432 | |||
433 | |||
434 | |||
435 | /************************************************************************************/ |
||
436 | /* zeigt aus oder Integerwert auf Display an */ |
||
437 | /* Parameter: */ |
||
438 | /* uint16_t val : val = 0 ==> aus, sont Integerwert */ |
||
439 | /* */ |
||
440 | /************************************************************************************/ |
||
441 | void Displ_Off_Format_Int( uint16_t val ) |
||
442 | { |
||
443 | if( val == 0 ) |
||
444 | { |
||
445 | lcd_printp_at( 17, 2, strGet(OFF), MNORMAL); |
||
446 | } |
||
447 | else |
||
448 | { |
||
449 | Displ_Format_Int( val ); |
||
450 | } |
||
451 | } |
||
452 | |||
453 | |||
454 | |||
455 | /************************************************************************************/ |
||
456 | /* zeigt aus oder Pausenzeit zwischen 2 Servoschritte auf Display an */ |
||
457 | /* Parameter: */ |
||
458 | /* uint16_t val : val = 0 ==> aus, sont Integerwert */ |
||
459 | /* */ |
||
460 | /************************************************************************************/ |
||
461 | void Displ_Pause_Step( uint16_t val ) |
||
462 | { |
||
463 | Displ_Off_Format_Int( val ); |
||
464 | } |
||
465 | |||
466 | |||
467 | |||
468 | /************************************************************************************/ |
||
469 | /* zeigt zu testende Servonummer zur Auswahl auf Display an */ |
||
470 | /* Parameter: */ |
||
471 | /* uint16_t val :0 = Servo 1 oder 1 = Servo 2, */ |
||
472 | /* uint16_t wegen Vereinheitlichung f. Funktionsaufrauf */ |
||
473 | /* */ |
||
474 | /************************************************************************************/ |
||
475 | void Displ_ServoNr( uint16_t val ) |
||
476 | { |
||
477 | Displ_Format_Int( val+1 ); |
||
478 | } |
||
479 | |||
480 | |||
481 | |||
482 | /**************************/ |
||
483 | /* */ |
||
484 | /* Servos-Tests */ |
||
485 | /* */ |
||
486 | /**************************/ |
||
487 | //void Menu_Servo_Test(void) |
||
488 | //{ uint8_t scr_sub_menu[SCROLL_MAX_6] = {SCROLL_MAX_6 - 2, MSG_RETURN, MSG_PULSE_WIDTH, MSG_CONTINOUS, MSG_SERVO, MSG_FRAME}; |
||
489 | // |
||
490 | // Scroll_Menu(scr_sub_menu, m_pkt); // pmenu global |
||
491 | // servo_nr = eeprom_read_byte(&ep_servo_nr); |
||
492 | // Jump_Menu(pmenu); |
||
493 | //} |
||
494 | |||
495 | |||
496 | |||
497 | //-------------------------------------------------------------- |
||
498 | //-------------------------------------------------------------- |
||
499 | void Menu_Test_Frame( void ) |
||
500 | { |
||
501 | uint16_t tmp_val; |
||
502 | |||
503 | draw_input_screen(); |
||
504 | |||
505 | tmp_val = Config.servo_frame; |
||
506 | if( Change_Value( &tmp_val, SERVO_PERIODE_MIN, SERVO_PERIODE_MAX, Displ_Format_Int) ) |
||
507 | { |
||
508 | Config.servo_frame = tmp_val; |
||
509 | } |
||
510 | } |
||
511 | |||
512 | |||
513 | |||
514 | //-------------------------------------------------------------- |
||
515 | //-------------------------------------------------------------- |
||
516 | void Menu_Test_ServoNr( void ) |
||
517 | { |
||
518 | uint16_t tmp_val; |
||
519 | |||
520 | draw_input_screen(); |
||
521 | |||
522 | tmp_val = servo_nr; |
||
523 | |||
524 | if( Change_Value( &tmp_val, 0, 1, Displ_ServoNr) ) |
||
525 | { // pmenu global; es gibt nur 0=Servo1, 1=Servo2 |
||
526 | servo_nr = tmp_val; |
||
527 | } |
||
528 | } |
||
529 | |||
530 | |||
531 | |||
532 | //-------------------------------------------------------------- |
||
533 | // Dieser Test im raw-Modus ohne Anschlagkalibrierung (normiert) z.B.: f�r Modelleinstellungen ohne Empf�nger |
||
534 | //-------------------------------------------------------------- |
||
535 | void Menu_Test_PulseWidth( void ) |
||
536 | { |
||
537 | uint8_t tmp_tracking; |
||
538 | uint16_t tmp_val; |
||
539 | |||
540 | draw_input_screen(); |
||
541 | |||
542 | // 30.05.2014 OG: macht dieser Code Sinn? |
||
543 | // Change_Value() -> 'tmp_val' und 'tmp_val' ist local - was soll mir das sagen? |
||
544 | |||
545 | tmp_tracking = Servo_tmp_on(Config.servo_frame); |
||
546 | tmp_val = steps_pw[Config.sIdxSteps].mid; |
||
547 | Change_Value( &tmp_val, steps_pw[Config.sIdxSteps].min, steps_pw[Config.sIdxSteps].max, Displ_PulseWidth); // pmenu global |
||
548 | cli(); |
||
549 | servoInit( SERVO_PERIODE ); |
||
550 | sei(); |
||
551 | Servo_tmp_Original(tmp_tracking); |
||
552 | } |
||
553 | |||
554 | |||
555 | |||
556 | //-------------------------------------------------------------- |
||
557 | //-------------------------------------------------------------- |
||
558 | void Menu_Test_SingleStep(void) |
||
559 | { |
||
560 | uint16_t tmp_val; |
||
561 | |||
562 | draw_input_screen(); |
||
563 | |||
564 | tmp_val = Config.single_step; |
||
565 | |||
566 | if( Change_Value( &tmp_val, SINGLE_STEP_MIN, SINGLE_STEP_MAX, Displ_Off_Format_Int) ) |
||
567 | { // pmenu global |
||
568 | Config.single_step = tmp_val; |
||
569 | } |
||
570 | } |
||
571 | |||
572 | |||
573 | |||
574 | //-------------------------------------------------------------- |
||
575 | //-------------------------------------------------------------- |
||
576 | void Menu_Test_Repeat( void ) |
||
577 | { |
||
578 | uint16_t tmp_val; |
||
579 | |||
580 | draw_input_screen(); |
||
581 | |||
582 | tmp_val = Config.repeat; |
||
583 | |||
584 | if( Change_Value( &tmp_val, REPEAT_MIN, REPEAT_MAX, Displ_Format_Int) ) |
||
585 | { |
||
586 | Config.repeat = tmp_val; |
||
587 | } |
||
588 | } |
||
589 | |||
590 | |||
591 | |||
592 | //-------------------------------------------------------------- |
||
593 | //-------------------------------------------------------------- |
||
594 | void Menu_Test_Pause( void ) |
||
595 | { |
||
596 | uint16_t tmp_val; |
||
597 | |||
598 | draw_input_screen(); |
||
599 | |||
600 | tmp_val = Config.pause; |
||
601 | |||
602 | if( Change_Value( &tmp_val, PAUSE_MIN, PAUSE_MAX, Displ_Pause) ) |
||
603 | { |
||
604 | Config.pause = tmp_val; |
||
605 | } |
||
606 | } |
||
607 | |||
608 | |||
609 | |||
610 | //-------------------------------------------------------------- |
||
611 | //-------------------------------------------------------------- |
||
612 | void Menu_Test_Pause_Step( void ) |
||
613 | { |
||
614 | uint16_t tmp_val; |
||
615 | |||
616 | draw_input_screen(); |
||
617 | |||
618 | tmp_val = Config.pause_step; |
||
619 | |||
620 | if( Change_Value( &tmp_val, PAUSE_STEP_MIN, PAUSE_STEP_MAX, Displ_Pause_Step) ) |
||
621 | { |
||
622 | Config.pause_step = tmp_val; |
||
623 | } |
||
624 | } |
||
625 | |||
626 | |||
627 | |||
628 | int8_t calc_dir(uint8_t idx, int16_t *Position) |
||
629 | { uint8_t nextIdx; |
||
630 | int8_t nextDir = 1; |
||
631 | |||
632 | nextIdx = idx; |
||
633 | if ((idx + 1) < POSIDX_MAX) |
||
634 | nextIdx++; |
||
635 | else |
||
636 | nextIdx = 0; |
||
637 | if (Position[PosIdx[idx]] > Position[PosIdx[nextIdx]]) nextDir = -1; |
||
638 | return(nextDir); |
||
639 | } |
||
640 | |||
641 | |||
642 | |||
643 | void Displ_LoopCounter( uint8_t val ) |
||
644 | { |
||
645 | lcdx_printf_at_P( 16, 2, MNORMAL, 0,0, PSTR("%4d"), val ); |
||
646 | //lcd_puts_at(16,2,my_itoa(val, 4, 0, 0),0); |
||
647 | } |
||
648 | |||
649 | |||
650 | |||
651 | |||
652 | // Test �ber Scalierung der Servos mit Anschlagkalibrierung |
||
653 | void Menu_Test_Start(void) |
||
654 | { |
||
655 | uint8_t tmp_tracking, idx, rep; |
||
656 | int8_t dir; |
||
657 | int16_t sPos; |
||
658 | int16_t Position[3]; |
||
659 | //int16_t range; |
||
660 | |||
661 | tmp_tracking = Servo_tmp_on(Config.servo_frame); |
||
662 | // lcdWriteCGRAM_Array(lcdSpecialChrLs, 8); // LCD-Char mit Rahmensymbole vom Graph |
||
663 | // chrxs = CHRLS; // Flag, welche K�stchensymbole geladen |
||
664 | // Displ_Title(MSG_CONTINOUS); |
||
665 | |||
666 | lcd_cls (); |
||
667 | PKT_TitleFromMenuItem( true ); |
||
668 | lcd_printp_at(0, 7, strGet(KEYLINE2), 0); |
||
669 | |||
670 | Displ_LoopCounter(Config.repeat); |
||
671 | Position[0] = 0; // skalierte Servoposition aber unterschiedliche Schrittanzahl m�glich |
||
672 | Position[1] = ServoSteps()/2; |
||
673 | Position[2] = ServoSteps(); |
||
674 | // init Einzelschritt |
||
675 | idx = 0; |
||
676 | dir = calc_dir(idx, Position); |
||
677 | sPos = Position[PosIdx[idx]]; |
||
678 | idx++; |
||
679 | rep = Config.repeat; |
||
680 | |||
681 | // Test bis Ende der Wiederholungen oder irgendein Enter |
||
682 | while(!get_key_press(1<<KEY_ENTER) && !get_key_press(1<<KEY_ESC)) { |
||
683 | calc_range(sPos - Position[1], Position[0], Position[2], Position[1]); |
||
684 | //range = calc_range(sPos - Position[1], Position[0], Position[2], Position[1]); |
||
685 | // draw_bar(sPos - Position[1], range, 1); // eingerahmter Balkengraph auf 2. Display-Zeile |
||
686 | servoSetPosition(servo_nr, sPos); |
||
687 | |||
688 | if ( sPos != Position[PosIdx[idx]]) { // Links-, Mittel- oder Rechtsposotion erreicht? |
||
689 | sPos += (Config.single_step * dir); // variable Schrittweite subtrahieren oder addieren |
||
690 | if (((dir < 0) && (sPos < Position[PosIdx[idx]])) || ((dir > 0) && (sPos > Position[PosIdx[idx]])) || !(Config.single_step)) |
||
691 | sPos = Position[PosIdx[idx]]; // �berlauf bei variabler Schrittweite ber�cksichtigen oder Einzelschritt |
||
692 | Delay_ms(Config.servo_frame + 1 + Config.pause_step);// Bei Schrittweite um 1 w�rden welche �bersprungen, zus�tzlich pro Servoschritt verz�gert |
||
693 | } |
||
694 | else { |
||
695 | dir = calc_dir(idx, Position); // Richtungs�nderung |
||
696 | if (idx < (POSIDX_MAX - 1)) { |
||
697 | if (idx == 0) { |
||
698 | rep--; // bei jeden vollen Durchlauf Wiederholz�hler verringern |
||
699 | Displ_LoopCounter(rep); |
||
700 | } |
||
701 | idx++; // Index f�r n�chsten Positionswert ==> Array PosIdx[] bestimmt Anschlagreihenfolge |
||
702 | } |
||
703 | else |
||
704 | idx = 0; |
||
705 | |||
706 | _delay_ms( Config.pause*100 ); // variable Pause bei Links-, Mittel- und Rechtsposotion Mindestzeit 400ms (Servolauf) |
||
707 | } |
||
708 | } |
||
709 | |||
710 | // lcdClear(); |
||
711 | // if (pmenu[0] == '\0') |
||
712 | // Displ_Main_Disp(); |
||
713 | // else |
||
714 | // return_m_pkt(strlen(pmenu)); // um bei R�cksprung auf urspr�nglichen Men�punkt zeigen oder Displ_Main_Disp() |
||
715 | // lcdWriteCGRAM_Array(lcdSpecialChr, 7); // LCD-Char f�r Bargraph zur�ckschreiben |
||
716 | cli(); |
||
717 | servoInit(SERVO_PERIODE); |
||
718 | sei(); |
||
719 | Servo_tmp_Original(tmp_tracking); |
||
720 | } |
||
721 | |||
722 | |||
723 | |||
724 | |||
725 | |||
726 | /********************************************************************************/ |
||
727 | /* zeigt Servo-Anschlagposition links auf Display an */ |
||
728 | /* mit sofortiger Wirkung auf Servo */ |
||
729 | /* Parameter: */ |
||
730 | /* uint16_t val :anzuzeigender Wert, */ |
||
731 | /* uint16_t wegen Vereinheitlichung f. Funktionsaufrauf */ |
||
732 | /* */ |
||
733 | /********************************************************************************/ |
||
734 | void Displ_Servo_Min(uint16_t val) |
||
735 | { |
||
736 | uint16_t steps = 0; |
||
737 | |||
738 | write_ndigit_number_s( 15, 2, val, 5, 0,0); |
||
739 | servoSet_min(servo_nr, val); // Wert setzen damit nachfolgend die |
||
740 | if (Config.servo[servo_nr].rev) steps = ServoSteps(); |
||
741 | servoSetPosition(servo_nr, steps); // �nderung direkt am Servo sichtbar ist |
||
742 | } |
||
743 | |||
744 | |||
745 | /************************************************************************************/ |
||
746 | /* zeigt Servo-Anschlagposition rechts auf Display an */ |
||
747 | /* mit sofortiger Wirkung auf Servo */ |
||
748 | /* Parameter: */ |
||
749 | /* uint16_t val :anzuzeigender Wert, */ |
||
750 | /* uint16_t wegen Vereinheitlichung f. Funktionsaufrauf */ |
||
751 | /* */ |
||
752 | /************************************************************************************/ |
||
753 | void Displ_Servo_Max(uint16_t val) |
||
754 | { |
||
755 | uint16_t steps = ServoSteps(); |
||
756 | |||
757 | write_ndigit_number_u( 15, 2, val, 5, 0,0); |
||
758 | servoSet_max(servo_nr, val); // Wert setzen damit nachfolgend die |
||
759 | if (Config.servo[servo_nr].rev) steps = 0; |
||
760 | servoSetPosition(servo_nr, steps); // �nderung direkt am Servo sichtbar ist |
||
761 | } |
||
762 | |||
763 | |||
764 | |||
765 | /************************************************************************************/ |
||
766 | /* zeigt Servo-Anschlagposition Mitte auf Display an */ |
||
767 | /* mit sofortiger Wirkung auf Servo */ |
||
768 | /* Parameter: */ |
||
769 | /* uint16_t val :anzuzeigender Wert, */ |
||
770 | /* uint16_t wegen Vereinheitlichung f. Funktionsaufrauf */ |
||
771 | /* */ |
||
772 | /************************************************************************************/ |
||
773 | void Displ_Servo_Mid( uint16_t val ) |
||
774 | { |
||
775 | int16_t mid_val; |
||
776 | |||
777 | mid_val = val - ServoSteps()/2; |
||
778 | |||
779 | lcdx_printf_at_P( 17, 2, MNORMAL, 0,0, PSTR("%3d"), mid_val ); |
||
780 | |||
781 | servoSet_mid(servo_nr, val); // Wert setzen damit nachfolgend die |
||
782 | servoSetPosition(servo_nr, ServoSteps()/2); // �nderung direkt am Servo sichtbar ist |
||
783 | } |
||
784 | |||
785 | |||
786 | |||
787 | //-------------------------------------------------------------- |
||
788 | //-------------------------------------------------------------- |
||
789 | void Servo_rev( void ) |
||
790 | { |
||
791 | uint16_t tmp_val; |
||
792 | uint8_t tmp_tracking; |
||
793 | |||
794 | draw_input_screen(); |
||
795 | |||
796 | tmp_tracking = Servo_tmp_on(SERVO_PERIODE); |
||
797 | tmp_val = Config.servo[servo_nr].rev; |
||
798 | |||
799 | if( Change_Value(&tmp_val , 0, 1, Displ_Off_On) ) |
||
800 | { //reverse gibt es nur 0=off, 1=on |
||
801 | Config.servo[servo_nr].rev = tmp_val ; |
||
802 | servoSet_rev(servo_nr, tmp_val ); |
||
803 | } |
||
804 | Servo_tmp_Original(tmp_tracking); |
||
805 | } |
||
806 | |||
807 | |||
808 | |||
809 | //-------------------------------------------------------------- |
||
810 | //-------------------------------------------------------------- |
||
811 | void Servo_left( void ) |
||
812 | { |
||
813 | uint16_t tmp_val; |
||
814 | uint8_t tmp_tracking; |
||
815 | |||
816 | draw_input_screen(); |
||
817 | |||
818 | tmp_tracking = Servo_tmp_on(SERVO_PERIODE); |
||
819 | servoSetPosition(servo_nr, ServoSteps()); // Linkssanschlag um Kalibrierung am Servo zu sehen |
||
820 | tmp_val = Config.servo[servo_nr].max; |
||
821 | |||
822 | if( Change_Value(&tmp_val , servo_limit[Config.sIdxSteps][LEFT].min, servo_limit[Config.sIdxSteps][LEFT].max, Displ_Servo_Max) ) |
||
823 | { |
||
824 | Config.servo[servo_nr].max = tmp_val; |
||
825 | servoSet_mid(servo_nr, Config.servo[servo_nr].mid); // Mittelposition muss sich bei Ausschlags�nderung verschieben |
||
826 | } |
||
827 | Servo_tmp_Original(tmp_tracking); |
||
828 | } |
||
829 | |||
830 | |||
831 | |||
832 | //-------------------------------------------------------------- |
||
833 | //-------------------------------------------------------------- |
||
834 | void Servo_right( void ) |
||
835 | { |
||
836 | uint16_t tmp_val; |
||
837 | uint8_t tmp_tracking; |
||
838 | |||
839 | draw_input_screen(); |
||
840 | |||
841 | tmp_tracking = Servo_tmp_on(SERVO_PERIODE); |
||
842 | servoSetPosition(servo_nr, 0); // Rechtsanschlag um Kalibrierung am Servo zu sehen |
||
843 | tmp_val = Config.servo[servo_nr].min; |
||
844 | |||
845 | if( Change_Value(&tmp_val , servo_limit[Config.sIdxSteps][RIGHT].min, servo_limit[Config.sIdxSteps][RIGHT].max, Displ_Servo_Min) ) |
||
846 | { |
||
847 | Config.servo[servo_nr].min = tmp_val; |
||
848 | servoSet_mid(servo_nr, Config.servo[servo_nr].mid); // Mittelposition muss sich bei Ausschlags�nderung verschieben |
||
849 | } |
||
850 | Servo_tmp_Original(tmp_tracking); |
||
851 | } |
||
852 | |||
853 | |||
854 | |||
855 | //-------------------------------------------------------------- |
||
856 | //-------------------------------------------------------------- |
||
857 | void Servo_middle( void ) |
||
858 | { |
||
859 | uint16_t tmp_val; |
||
860 | uint8_t tmp_tracking; |
||
861 | |||
862 | draw_input_screen(); |
||
863 | |||
864 | tmp_tracking = Servo_tmp_on(SERVO_PERIODE); |
||
865 | servoSetPosition(servo_nr, ServoSteps()/2); // Mittelposition um Kalibrierung am Servo zu sehen |
||
866 | tmp_val = Config.servo[servo_nr].mid; |
||
867 | |||
868 | if( Change_Value(&tmp_val , servo_limit[Config.sIdxSteps][MIDDLE].min, servo_limit[Config.sIdxSteps][MIDDLE].max, Displ_Servo_Mid) ) |
||
869 | { |
||
870 | Config.servo[servo_nr].mid = tmp_val; |
||
871 | } |
||
872 | Servo_tmp_Original(tmp_tracking); |
||
873 | } |
||
874 | |||
875 | |||
876 | |||
877 | void Servo_NewValues(uint8_t idx_presc) |
||
878 | { |
||
879 | for (uint8_t i = 0; i < SERVO_NUM_CHANNELS; i++) { |
||
880 | if (idx_presc == STEPS_255) { // Werte umrechnen f�r Prescaler = 256 |
||
881 | Config.servo[i].min /= 4; |
||
882 | Config.servo[i].max /= 4; |
||
883 | Config.servo[i].mid /= 4; |
||
884 | } |
||
885 | else { // Werte umrechnen f�r Prescaler = 64 |
||
886 | Config.servo[i].min *= 4; |
||
887 | Config.servo[i].max *= 4; |
||
888 | Config.servo[i].mid = (Config.servo[i].mid + 1) * 4 - 1; |
||
889 | } |
||
890 | servoSet_min(i, Config.servo[i].min); |
||
891 | servoSet_max(i, Config.servo[i].max); |
||
892 | servoSet_mid(i, Config.servo[i].mid); |
||
893 | // eeprom_write_block(&servo[i],&ep_servo[i],sizeof(servo_t)); |
||
894 | } |
||
895 | // Vorberechnung von ServoChannels[channel].duty |
||
896 | servoSetDefaultPos(); // Ausgangsstellung beider Servos |
||
897 | |||
898 | } |
||
899 | |||
900 | /************************************************************************************/ |
||
901 | /* zeigt Servoschritte zur Auswahl auf Display an */ |
||
902 | /* Parameter: */ |
||
903 | /* uint16_t val :0 = 255 oder 1 = 1023, */ |
||
904 | /* uint16_t wegen Vereinheitlichung f. Funktionsaufrauf */ |
||
905 | /* */ |
||
906 | /************************************************************************************/ |
||
907 | void Displ_Servo_Steps(uint16_t val) |
||
908 | { |
||
909 | if (val==0) |
||
910 | lcd_puts_at(16,2,INTERNAT_STEPS_255,0 ); |
||
911 | else |
||
912 | lcd_puts_at(16,2,INTERNAT_STEPS_1023,0 ); |
||
913 | |||
914 | } |
||
915 | |||
916 | |||
917 | |||
918 | //-------------------------------------------------------------- |
||
919 | //-------------------------------------------------------------- |
||
920 | void Menu_Servo_Steps(void) |
||
921 | { |
||
922 | uint16_t tmp_val; |
||
923 | |||
924 | draw_input_screen(); |
||
925 | |||
926 | tmp_val = Config.sIdxSteps; |
||
927 | |||
928 | if( Change_Value( &tmp_val, STEPS_255, STEPS_1023,Displ_Servo_Steps) ) |
||
929 | { |
||
930 | cli(); |
||
931 | Config.sIdxSteps = tmp_val; |
||
932 | Servo_NewValues( Config.sIdxSteps ); // hier ist der neue Index anzugeben! |
||
933 | servoInit(SERVO_PERIODE); |
||
934 | sei(); |
||
935 | |||
936 | |||
937 | } |
||
938 | } |
||
939 | |||
940 | |||
941 | |||
942 | //-------------------------------------------------------------- |
||
943 | // Setup_ServoAdjust() |
||
944 | //-------------------------------------------------------------- |
||
945 | void Setup_ServoAdjust( uint8_t servo ) |
||
946 | { |
||
947 | servo_nr = servo; |
||
948 | |||
949 | //--------------- |
||
950 | // Create |
||
951 | //--------------- |
||
952 | MenuCtrl_Create(); |
||
953 | MenuCtrl_SetTitleFromParentItem(); // "Servo 1" oder "Servo 1" |
||
954 | //MenuCtrl_SetTitle_P( PSTR("Servo Adjust") ); |
||
955 | |||
956 | |||
957 | //--------------- |
||
958 | // Menuitems |
||
959 | //--------------- |
||
960 | MenuCtrl_PushML2_P( MSERVOADJ_REV , MENU_ITEM, &Servo_rev , MSERVOADJ_REV_de , MSERVOADJ_REV_en ); |
||
961 | MenuCtrl_PushML2_P( MSERVOADJ_LEFT , MENU_ITEM, &Servo_left , MSERVOADJ_LEFT_de , MSERVOADJ_LEFT_en ); |
||
962 | MenuCtrl_PushML2_P( MSERVOADJ_RIGHT , MENU_ITEM, &Servo_right , MSERVOADJ_RIGHT_de , MSERVOADJ_RIGHT_en ); |
||
963 | MenuCtrl_PushML2_P( MSERVOADJ_MID , MENU_ITEM, &Servo_middle , MSERVOADJ_MID_de , MSERVOADJ_MID_en ); |
||
964 | |||
965 | //--------------- |
||
966 | // Control |
||
967 | //--------------- |
||
968 | MenuCtrl_Control( MENUCTRL_EVENT ); |
||
969 | |||
970 | |||
971 | //--------------- |
||
972 | // Destroy |
||
973 | //--------------- |
||
974 | MenuCtrl_Destroy(); |
||
975 | } |
||
976 | |||
977 | |||
978 | |||
979 | //-------------------------------------------------------------- |
||
980 | // Setup_ServoTestCont() |
||
981 | //-------------------------------------------------------------- |
||
982 | void Setup_ServoTestCont( void ) |
||
983 | { |
||
984 | //--------------- |
||
985 | // Create |
||
986 | //--------------- |
||
987 | MenuCtrl_Create(); |
||
988 | MenuCtrl_SetTitleFromParentItem(); // "Test fortlfd." |
||
989 | //MenuCtrl_SetTitle_P( PSTR("Servotest Cont.") ); |
||
990 | |||
991 | //--------------- |
||
992 | // Menuitems |
||
993 | //--------------- |
||
994 | MenuCtrl_PushML2_P( MSERVOTC_START , MENU_ITEM, &Menu_Test_Start , MSERVOTC_START_de , MSERVOTC_START_en ); |
||
995 | MenuCtrl_PushML2_P( MSERVOTC_SINGLE , MENU_ITEM, &Menu_Test_SingleStep , MSERVOTC_SINGLE_de , MSERVOTC_SINGLE_en ); |
||
996 | MenuCtrl_PushML2_P( MSERVOTC_COUNT , MENU_ITEM, &Menu_Test_Repeat , MSERVOTC_COUNT_de , MSERVOTC_COUNT_en ); |
||
997 | MenuCtrl_PushML2_P( MSERVOTC_PAUSEEND , MENU_ITEM, &Menu_Test_Pause , MSERVOTC_PAUSEEND_de , MSERVOTC_PAUSEEND_en ); |
||
998 | MenuCtrl_PushML2_P( MSERVOTC_PAUSEINC , MENU_ITEM, &Menu_Test_Pause_Step , MSERVOTC_PAUSEINC_de , MSERVOTC_PAUSEINC_en ); |
||
999 | |||
1000 | //--------------- |
||
1001 | // Control |
||
1002 | //--------------- |
||
1003 | MenuCtrl_Control( MENUCTRL_EVENT ); |
||
1004 | |||
1005 | //--------------- |
||
1006 | // Destroy |
||
1007 | //--------------- |
||
1008 | MenuCtrl_Destroy(); |
||
1009 | } |
||
1010 | |||
1011 | |||
1012 | |||
1013 | //-------------------------------------------------------------- |
||
1014 | // Setup_ServoTest() |
||
1015 | //-------------------------------------------------------------- |
||
1016 | void Setup_ServoTest( void ) |
||
1017 | { |
||
1018 | //--------------- |
||
1019 | // Create |
||
1020 | //--------------- |
||
1021 | MenuCtrl_Create(); |
||
1022 | MenuCtrl_SetTitleFromParentItem(); // "Servotest" |
||
1023 | |||
1024 | //--------------- |
||
1025 | // Menuitems |
||
1026 | //--------------- |
||
1027 | MenuCtrl_PushML2_P( MSERVOT_PULS , MENU_ITEM, &Menu_Test_PulseWidth , MSERVOT_PULS_de , MSERVOT_PULS_en ); |
||
1028 | MenuCtrl_PushML2_P( MSERVOT_CONT , MENU_SUB , &Setup_ServoTestCont , MSERVOT_CONT_de , MSERVOT_CONT_en ); |
||
1029 | MenuCtrl_PushML2_P( MSERVOT_SERVO , MENU_ITEM, &Menu_Test_ServoNr , MSERVOT_SERVO_de , MSERVOT_SERVO_en ); |
||
1030 | MenuCtrl_PushML2_P( MSERVOT_FRAME , MENU_ITEM, &Menu_Test_Frame , MSERVOT_FRAME_de , MSERVOT_FRAME_en ); |
||
1031 | |||
1032 | //--------------- |
||
1033 | // Control |
||
1034 | //--------------- |
||
1035 | MenuCtrl_Control( MENUCTRL_EVENT ); |
||
1036 | |||
1037 | //--------------- |
||
1038 | // Destroy |
||
1039 | //--------------- |
||
1040 | MenuCtrl_Destroy(); |
||
1041 | } |
||
1042 | |||
1043 | |||
1044 | |||
1045 | //-------------------------------------------------------------- |
||
1046 | // Setup_ServoTracking() |
||
1047 | //-------------------------------------------------------------- |
||
1048 | void Setup_ServoTracking( void ) |
||
1049 | { |
||
1050 | uint8_t itemid; |
||
1051 | |||
1052 | //--------------- |
||
1053 | // Create |
||
1054 | //--------------- |
||
1055 | MenuCtrl_Create(); |
||
1056 | MenuCtrl_SetTitleFromParentItem(); // "Tracking" |
||
1057 | |||
1058 | //--------------- |
||
1059 | // Menuitems |
||
1060 | //--------------- |
||
1061 | MenuCtrl_PushML2_P( MSERVO_STEPS , MENU_ITEM, &Menu_Servo_Steps , MSERVO_STEPS_de , MSERVO_STEPS_en ); |
||
1062 | MenuCtrl_PushML2_P( MSERVO_SERVO1 , MENU_SUB , NOFUNC , MSERVO_SERVO1_de , MSERVO_SERVO1_en ); |
||
1063 | MenuCtrl_PushML2_P( MSERVO_SERVO2 , MENU_SUB , NOFUNC , MSERVO_SERVO2_de , MSERVO_SERVO2_en ); |
||
1064 | MenuCtrl_PushML2_P( MSERVO_SERVOTEST , MENU_SUB , &Setup_ServoTest , MSERVO_SERVOTEST_de , MSERVO_SERVOTEST_en ); |
||
1065 | |||
1066 | |||
1067 | //--------------- |
||
1068 | // Control |
||
1069 | //--------------- |
||
1070 | while( true ) |
||
1071 | { |
||
1072 | MenuCtrl_Control( MENUCTRL_EVENT ); |
||
1073 | |||
1074 | if( MenuCtrl_GetKey() == KEY_ESC ) break; // ENDE |
||
1075 | |||
1076 | itemid = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID) |
||
1077 | |||
1078 | if( itemid == MSERVO_SERVO1 ) { Setup_ServoAdjust(0); } |
||
1079 | if( itemid == MSERVO_SERVO2 ) { Setup_ServoAdjust(1); } |
||
1080 | } |
||
1081 | |||
1082 | //--------------- |
||
1083 | // Destroy |
||
1084 | //--------------- |
||
1085 | MenuCtrl_Destroy(); |
||
1086 | } |
||
1087 | |||
1088 | |||
1089 | |||
1090 | |||
1091 | //-------------------------------------------------------------- |
||
1092 | |||
1093 | //void start_tracking(void) |
||
1094 | //{ |
||
1095 | // #define TIMEOUT 200 // 2 sec |
||
1096 | // |
||
1097 | // uint16_t old_anglePan = 0; |
||
1098 | // uint16_t old_angleTilt = 0; |
||
1099 | // |
||
1100 | // //uint16_t old_hh = 0; |
||
1101 | // uint8_t flag; |
||
1102 | // uint8_t tmp_dat; |
||
1103 | // |
||
1104 | // lcd_cls (); |
||
1105 | // //lcd_printpns_at(0, 0, PSTR("start_tracking "), 2); |
||
1106 | // |
||
1107 | // //lcd_printpns_at(0, 1, PSTR("ab jetzt Tracking"), 0); |
||
1108 | // |
||
1109 | // lcd_ecircle(22, 35, 16, 1); |
||
1110 | // lcd_ecircle(88, 35, 16, 1); |
||
1111 | // lcd_putc (10, 1, 0x1e, 0); // degree symbol |
||
1112 | // lcd_putc (20, 1, 0x1e, 0); // degree symbol |
||
1113 | //// lcd_printp_at (0, 7, PSTR(KEY_LINE_1), 0); |
||
1114 | // lcd_puts_at(0, 7, strGet(KEYLINE1), 0); |
||
1115 | // SwitchToNC(); |
||
1116 | // |
||
1117 | // mode = 'O'; |
||
1118 | // |
||
1119 | // // disable debug... |
||
1120 | // // RS232_request_mk_data (0, 'd', 0); |
||
1121 | // tmp_dat = 0; |
||
1122 | // SendOutData ('d', ADDRESS_ANY, 1, &tmp_dat, 1); |
||
1123 | // |
||
1124 | // // request OSD Data from NC every 100ms |
||
1125 | // // RS232_request_mk_data (1, 'o', 100); |
||
1126 | // tmp_dat = 10; |
||
1127 | // SendOutData ('o', ADDRESS_NC, 1, &tmp_dat, 1); |
||
1128 | // |
||
1129 | // if (rxd_buffer_locked) |
||
1130 | // { |
||
1131 | // timer = TIMEOUT; |
||
1132 | // Decode64 (); |
||
1133 | // naviData = (NaviData_t *) pRxData; |
||
1134 | // |
||
1135 | // if(error1 == 1) |
||
1136 | // lcd_cls(); |
||
1137 | // } |
||
1138 | // |
||
1139 | // GPS_Pos_t currpos; |
||
1140 | // currpos.Latitude = naviData->CurrentPosition.Latitude; |
||
1141 | // currpos.Longitude = naviData->CurrentPosition.Longitude; |
||
1142 | // |
||
1143 | // flag = 0; |
||
1144 | // timer = TIMEOUT; |
||
1145 | // abo_timer = ABO_TIMEOUT; |
||
1146 | // |
||
1147 | // coldstart = 1; |
||
1148 | // |
||
1149 | // do |
||
1150 | // { |
||
1151 | // if (rxd_buffer_locked) |
||
1152 | // { |
||
1153 | // timer = TIMEOUT; |
||
1154 | // Decode64 (); |
||
1155 | // naviData = (NaviData_t *) pRxData; |
||
1156 | // |
||
1157 | // |
||
1158 | ////CB uint8_t FCStatusFlag = naviData->FCFlags; |
||
1159 | // uint8_t FCStatusFlag = naviData->FCStatusFlags; |
||
1160 | // //write_ndigit_number_u (0, 0, FCStatusFlag); |
||
1161 | // |
||
1162 | // Tracking_GPS(); |
||
1163 | // |
||
1164 | // //uint16_t heading_home = (naviData->HomePositionDeviation.Bearing + 360 - naviData->CompassHeading) % 360; |
||
1165 | // |
||
1166 | // // alte Linien l�schen |
||
1167 | // //lcd_ecirc_line (22, 35, 15, old_hh, 0); |
||
1168 | // //old_hh = heading_home; |
||
1169 | // lcd_ecirc_line (22, 35, 15, old_anglePan, 0); |
||
1170 | // old_anglePan = anglePan; |
||
1171 | // lcd_ecirc_line (88, 35, 15, old_angleTilt, 0); |
||
1172 | // old_angleTilt = angleTilt; |
||
1173 | // |
||
1174 | // lcd_ecirc_line (22, 35, 15, anglePan, 1); |
||
1175 | // write_ndigit_number_u (7, 1, anglePan, 3, 0,0); |
||
1176 | // lcd_ecirc_line (88, 35, 15, angleTilt, 1); |
||
1177 | // write_ndigit_number_u (17, 1, angleTilt, 3, 0,0); |
||
1178 | // |
||
1179 | // rxd_buffer_locked = FALSE; |
||
1180 | // |
||
1181 | // if (!abo_timer) |
||
1182 | // { // renew abo every 3 sec |
||
1183 | // // request OSD Data from NC every 100ms |
||
1184 | // // RS232_request_mk_data (1, 'o', 100); |
||
1185 | // tmp_dat = 10; |
||
1186 | // SendOutData ('o', ADDRESS_NC, 1, &tmp_dat, 1); |
||
1187 | // |
||
1188 | // abo_timer = ABO_TIMEOUT; |
||
1189 | // } |
||
1190 | // } |
||
1191 | // |
||
1192 | // if (!timer) |
||
1193 | // { |
||
1194 | // OSD_Timeout(flag); |
||
1195 | // flag = 0; |
||
1196 | // } |
||
1197 | // } |
||
1198 | // while(!get_key_press (1 << KEY_ESC)); |
||
1199 | // |
||
1200 | // //lcd_cls(); |
||
1201 | // //return; |
||
1202 | //} |
||
1203 | |||
1204 | //-------------------------------------------------------------- |
||
1205 | // |
||
1206 | //void conect2at_unit(void) |
||
1207 | //{ |
||
1208 | // lcd_cls (); |
||
1209 | // lcd_printpns_at(0, 0, PSTR("conect2at_unit "), 2); |
||
1210 | // |
||
1211 | // lcd_printpns_at(0, 3, PSTR("work in progress ;)"), 2); |
||
1212 | // |
||
1213 | //// lcd_printp_at (0, 7, PSTR(KEY_LINE_1), 0); |
||
1214 | // lcd_puts_at(0, 7, strGet(KEYLINE1), 0); |
||
1215 | // |
||
1216 | // while(!get_key_press (1 << KEY_ESC)); |
||
1217 | // |
||
1218 | // lcd_cls(); |
||
1219 | // return; |
||
1220 | //} |
||
1221 | // |
||
1222 | ////-------------------------------------------------------------- |
||
1223 | // |
||
1224 | //void conect2gps_ser (void) |
||
1225 | //{ |
||
1226 | // lcd_cls (); |
||
1227 | // lcd_printpns_at(0, 0, PSTR("conect2gps_ser "), 2); |
||
1228 | // |
||
1229 | // lcd_printpns_at(0, 3, PSTR("work in progress ;)"), 2); |
||
1230 | // |
||
1231 | //// lcd_printp_at (0, 7, PSTR(KEY_LINE_1), 0); |
||
1232 | // lcd_puts_at(0, 7, strGet(KEYLINE1), 0); |
||
1233 | // while(!get_key_press (1 << KEY_ESC)); |
||
1234 | // |
||
1235 | // lcd_cls(); |
||
1236 | // return; |
||
1237 | //} |
||
1238 | // |
||
1239 | ////-------------------------------------------------------------- |
||
1240 | // |
||
1241 | //void conect2gps_bt (void) |
||
1242 | //{ |
||
1243 | // lcd_cls (); |
||
1244 | // lcd_printpns_at(0, 0, PSTR("conect2gps_bt "), 2); |
||
1245 | // |
||
1246 | // lcd_printpns_at(0, 3, PSTR("work in progress ;)"), 2); |
||
1247 | // |
||
1248 | //// lcd_printp_at (0, 7, PSTR(KEY_LINE_1), 0); |
||
1249 | // lcd_puts_at(0, 7, strGet(KEYLINE1), 0); |
||
1250 | // while(!get_key_press (1 << KEY_ESC)); |
||
1251 | // |
||
1252 | // lcd_cls(); |
||
1253 | // return; |
||
1254 | //} |
||
1255 | |||
1256 | //-------------------------------------------------------------- |
||
1257 | |||
1258 | //void conect2gps_menu(void) |
||
1259 | //{ |
||
1260 | // uint8_t ii = 0; |
||
1261 | // uint8_t Offset = 0; |
||
1262 | // uint8_t size = ITEMS_CONECT_GPS; |
||
1263 | // uint8_t dmode = 0; |
||
1264 | // uint8_t target_pos = 1; |
||
1265 | // uint8_t val = 0; |
||
1266 | // |
||
1267 | // while(1) |
||
1268 | // { |
||
1269 | // lcd_cls (); |
||
1270 | // lcd_printpns_at(0, 0, PSTR("conect2gps_menu "), 2); |
||
1271 | //// lcd_printp_at (0, 7, PSTR(KEY_LINE_1), 0); |
||
1272 | // lcd_puts_at(0, 7, strGet(KEYLINE1), 0); |
||
1273 | // while(2) |
||
1274 | // { |
||
1275 | // ii = 0; |
||
1276 | // if(Offset > 0) |
||
1277 | // { |
||
1278 | // lcd_printp_at(1,1, PSTR("\x12"), 0); |
||
1279 | // } |
||
1280 | // for(ii = 0;ii < 6 ; ii++) |
||
1281 | // { |
||
1282 | // if((ii+Offset) < size) |
||
1283 | // { |
||
1284 | // lcd_printp_at(3,ii+1,conect_gps_menuitems[ii+Offset], 0); |
||
1285 | // } |
||
1286 | // if((ii == 5)&&(ii+Offset < (size-1))) |
||
1287 | // { |
||
1288 | // lcd_printp_at(1,6, PSTR("\x13"), 0); |
||
1289 | // } |
||
1290 | // } |
||
1291 | // if(dmode == 0) |
||
1292 | // { |
||
1293 | // if(Offset == 0) |
||
1294 | // { |
||
1295 | // if(size > 6) |
||
1296 | // { |
||
1297 | // val = menu_choose2 (1, 5, target_pos,0,1); |
||
1298 | // } |
||
1299 | // else |
||
1300 | // { |
||
1301 | // val = menu_choose2 (1, size, target_pos,0,0); |
||
1302 | // } |
||
1303 | // } |
||
1304 | // else |
||
1305 | // { |
||
1306 | // val = menu_choose2 (2, 5, target_pos,1,1); |
||
1307 | // } |
||
1308 | // } |
||
1309 | // if(dmode == 1) |
||
1310 | // { |
||
1311 | // if(Offset+7 > size) |
||
1312 | // { |
||
1313 | // val = menu_choose2 (2, 6, target_pos,1,0); |
||
1314 | // } |
||
1315 | // else |
||
1316 | // { |
||
1317 | // val = menu_choose2 (2, 5, target_pos,1,1); |
||
1318 | // } |
||
1319 | // } |
||
1320 | // if(val == 254) |
||
1321 | // { |
||
1322 | // Offset++; |
||
1323 | // dmode = 1; |
||
1324 | // target_pos = 5; |
||
1325 | // } |
||
1326 | // else if(val == 253) |
||
1327 | // { |
||
1328 | // Offset--; |
||
1329 | // dmode = 0; |
||
1330 | // target_pos = 2; |
||
1331 | // } |
||
1332 | // else if(val == 255) |
||
1333 | // { |
||
1334 | // return; |
||
1335 | // } |
||
1336 | // else |
||
1337 | // { |
||
1338 | // break; |
||
1339 | // } |
||
1340 | // } |
||
1341 | // target_pos = val; |
||
1342 | // |
||
1343 | // if((val+Offset) == 1 ) |
||
1344 | // conect2gps_ser(); |
||
1345 | // if((val+Offset) == 2 ) |
||
1346 | // conect2gps_bt(); |
||
1347 | // } |
||
1348 | //} |
||
1349 | //-------------------------------------------------------------- |
||
1350 | //void tracking_menu(void) |
||
1351 | //{ |
||
1352 | // uint8_t ii = 0; |
||
1353 | // uint8_t Offset = 0; |
||
1354 | // uint8_t size = ITEMS_AT; |
||
1355 | // uint8_t dmode = 0; |
||
1356 | // uint8_t target_pos = 1; |
||
1357 | // uint8_t val = 0; |
||
1358 | // |
||
1359 | // while(1) |
||
1360 | // { |
||
1361 | // lcd_cls (); |
||
1362 | // lcd_printpns_at(1, 0, PSTR("Tracking Men\x06 V.01 "), 2); |
||
1363 | //// lcd_printp_at (0, 7, PSTR(KEY_LINE_1), 0); |
||
1364 | // lcd_puts_at(0, 7, strGet(KEYLINE1), 0); |
||
1365 | // while(2) |
||
1366 | // { |
||
1367 | // ii = 0; |
||
1368 | // if(Offset > 0) |
||
1369 | // { |
||
1370 | // lcd_printp_at(1,1, PSTR("\x12"), 0); |
||
1371 | // } |
||
1372 | // for(ii = 0;ii < 6 ; ii++) |
||
1373 | // { |
||
1374 | // if((ii+Offset) < size) |
||
1375 | // { |
||
1376 | // lcd_printp_at(3,ii+1,at_menuitems[ii+Offset], 0); |
||
1377 | // } |
||
1378 | // if((ii == 5)&&(ii+Offset < (size-1))) |
||
1379 | // { |
||
1380 | // lcd_printp_at(1,6, PSTR("\x13"), 0); |
||
1381 | // } |
||
1382 | // } |
||
1383 | // if(dmode == 0) |
||
1384 | // { |
||
1385 | // if(Offset == 0) |
||
1386 | // { |
||
1387 | // if(size > 6) |
||
1388 | // { |
||
1389 | // val = menu_choose2 (1, 5, target_pos,0,1); |
||
1390 | // } |
||
1391 | // else |
||
1392 | // { |
||
1393 | // val = menu_choose2 (1, size, target_pos,0,0); |
||
1394 | // } |
||
1395 | // } |
||
1396 | // else |
||
1397 | // { |
||
1398 | // val = menu_choose2 (2, 5, target_pos,1,1); |
||
1399 | // } |
||
1400 | // } |
||
1401 | // if(dmode == 1) |
||
1402 | // { |
||
1403 | // if(Offset+7 > size) |
||
1404 | // { |
||
1405 | // val = menu_choose2 (2, 6, target_pos,1,0); |
||
1406 | // } |
||
1407 | // else |
||
1408 | // { |
||
1409 | // val = menu_choose2 (2, 5, target_pos,1,1); |
||
1410 | // } |
||
1411 | // } |
||
1412 | // if(val == 254) |
||
1413 | // { |
||
1414 | // Offset++; |
||
1415 | // dmode = 1; |
||
1416 | // target_pos = 5; |
||
1417 | // } |
||
1418 | // else if(val == 253) |
||
1419 | // { |
||
1420 | // Offset--; |
||
1421 | // dmode = 0; |
||
1422 | // target_pos = 2; |
||
1423 | // } |
||
1424 | // else if(val == 255) |
||
1425 | // { |
||
1426 | // return; |
||
1427 | // } |
||
1428 | // else |
||
1429 | // { |
||
1430 | // break; |
||
1431 | // } |
||
1432 | // } |
||
1433 | // target_pos = val; |
||
1434 | // |
||
1435 | // if((val+Offset) == 1 ) |
||
1436 | // test_servo_menu(); |
||
1437 | // if((val+Offset) == 2 ) |
||
1438 | // adjust_servo_menu(); |
||
1439 | // if((val+Offset) == 3 ) |
||
1440 | // show_angle(); |
||
1441 | // if((val+Offset) == 4 ) |
||
1442 | ////TODO: start_tracking(); |
||
1443 | // if((val+Offset) == 5 ) |
||
1444 | // conect2at_unit(); |
||
1445 | // if((val+Offset) == 6 ) |
||
1446 | // conect2gps_menu(); |
||
1447 | // } |
||
1448 | //} |
||
1449 | |||
1450 | //-------------------------------------------------------------- |
||
1451 | // kapeschi Ant.Treking Funktionen |
||
1452 | //-------------------------------------------------------------- |
||
1453 | |||
1454 | // Berechnung von Distanz und Winkel aus GPS-Daten home(MK eingeschaltet) |
||
1455 | // zur aktuellen Position(nach Motorstart) |
||
1456 | //geo_t calc_geo(HomePos_t *home, GPS_Pos_t *pos) |
||
1457 | //{ int32_t lat1, lon1, lat2, lon2; |
||
1458 | // int32_t d1, dlat; |
||
1459 | // geo_t geo; |
||
1460 | // |
||
1461 | // lon1 = MK_pos.Home_Lon; |
||
1462 | // lat1 = MK_pos.Home_Lat; |
||
1463 | // lon2 = pos->Longitude; |
||
1464 | // lat2 = pos->Latitude; |
||
1465 | // |
||
1466 | // // Formel verwendet von http://www.kompf.de/gps/distcalc.html |
||
1467 | // // 111.3 km = Abstand zweier Breitenkreise und/oder zweier L�ngenkreise am �quator |
||
1468 | // // es wird jedoch in dm Meter weiter gerechnet |
||
1469 | // // (tlon1 - tlon2)/10) sonst uint32_t-�berlauf bei cos(0) gleich 1 |
||
1470 | // d1 = (1359 * (int32_t)(c_cos_8192((lat1 + lat2) / 20000000)) * ((lon1 - lon2)/10))/ 10000000; |
||
1471 | // dlat = 1113 * (lat1 - lat2) / 10000; |
||
1472 | // geo.bearing = (my_atan2(d1, dlat) + 540) % 360; // 360 +180 besserer Vergleich mit MkCockpit |
||
1473 | // geo.distance = sqrt32(d1 * d1 + dlat * dlat); |
||
1474 | // return(geo); |
||
1475 | //} |
||
1476 | |||
1477 | //void do_tracking(void) |
||
1478 | //{ //static uint8_t hysteresis = 0; |
||
1479 | // // aus MkCockpit http://forum.mikrokopter.de/topic-post216136.html#post216136 |
||
1480 | // // (4 * (........))/5 ==> Wichtung Luftdruck-H�he zu GPS |
||
1481 | // currentPos.Altitude = MK_pos.Home_Alt + (4000 * (int32_t)(naviData->Altimeter) / AltFaktor + currentPos.Altitude - MK_pos.Home_Alt) / 5; |
||
1482 | // |
||
1483 | // geo = calc_geo(&MK_pos, ¤tPos); |
||
1484 | // angleTilt = RAD_TO_DEG * (double)atan2((double)(currentPos.Altitude - MK_pos.Home_Alt) / 1000, geo.distance); |
||
1485 | // //if (geo.distance < 4 || (geo.distance < 6 && hysteresis)) { // < 4m ==> Pan-Servo in Mittelstellung. Hysterese bis 6m, damit Servo im Grenzbereich nicht wild rumschl�gt |
||
1486 | // //geo.bearing = MK_pos.direction; |
||
1487 | // //angleTilt = 0; |
||
1488 | // //hysteresis = 1; |
||
1489 | // //} |
||
1490 | // //else { |
||
1491 | // //hysteresis = 0; |
||
1492 | // //} |
||
1493 | //// |
||
1494 | // //// egal wo der �bergangspunkt 359, 360, 1grd ist => Winkel�bergangspunkt auf 0 bzw. 180grd des Servos bringen |
||
1495 | // //// 360 grd negative Winkelwerte als positive |
||
1496 | // anglePan = (geo.bearing + 450 - MK_pos.direction) % 360; // 450 = 360 + 90 |
||
1497 | // |
||
1498 | // //if (angleTilt < 0) angleTilt = 0; |
||
1499 | // //if (angleTilt > 180) angleTilt = 180; |
||
1500 | //// |
||
1501 | // //if (anglePan >= 180) { // zwecks 360grd-Abdeckung flipt Pan-/Tilt-Servo |
||
1502 | // //anglePan = anglePan - 180; |
||
1503 | // //angleTilt = 180 - angleTilt; |
||
1504 | // // |
||
1505 | // //} |
||
1506 | ////angleTilt = 180; |
||
1507 | ////angleTilt = 180; |
||
1508 | // |
||
1509 | //// servoSetAngle(0, anglePan); |
||
1510 | //// servoSetAngle(1, angleTilt); |
||
1511 | //} |
||
1512 | |||
1513 | |||
1514 | /****************************************************************/ |
||
1515 | /* */ |
||
1516 | /* MK GPS Tracking */ |
||
1517 | /* */ |
||
1518 | /****************************************************************/ |
||
1519 | |||
1520 | // MK OSD-Daten lesen und verifizieren |
||
1521 | //uint8_t OSD_Data_valid(NaviData_t **navi_data) |
||
1522 | //{ uint8_t ret = 0; |
||
1523 | //char *tx_osd = {"#co?]==EH\r"}; |
||
1524 | //// char interval[2] = {10, '\0'}; |
||
1525 | // |
||
1526 | //if (rx_line_decode('O')) { // OSD-Datensatz pr�fen/dekodieren |
||
1527 | ////*navi_data = (NaviData_t*)data_decode; // dekodierte Daten mit Struktur OSD-Daten versehen |
||
1528 | //if (rx_timeout < RX_TIME_OLD) { // GPS-Daten nicht zu alt und ok. |
||
1529 | //currentPos = (*navi_data)->CurrentPosition; |
||
1530 | //if ((*navi_data)->NCFlags & NC_FLAG_GPS_OK) |
||
1531 | //ret = 1; |
||
1532 | //// aus MkCockpit http://forum.mikrokopter.de/topic-post216136.html#post216136 |
||
1533 | //// (4 * (........))/5 ==> Wichtung Luftdruck-H�he zu GPS |
||
1534 | //currentPos.Altitude = MK_pos.Home_Alt + (4000 * (int32_t)((*navi_data)->Altimeter) / AltFaktor + currentPos.Altitude - MK_pos.Home_Alt) / 5; |
||
1535 | //satsInUse = (*navi_data)->SatsInUse; |
||
1536 | //} |
||
1537 | //} |
||
1538 | //// ca. 210ms keine OSD-Daten empfangen ==> sende neue Anforderung an MK |
||
1539 | //// if ((track_tx) && (rx_timeout > RX_TIMEOUT)) tx_Mk(NC_ADDRESS, 'o', interval, 1); // 420 * 0.5ms interval |
||
1540 | //if ((track_tx) && (rx_timeout > RX_TIMEOUT)) SendOutData(tx_osd); // 420 * 0.5ms interval |
||
1541 | //return ret; |
||
1542 | //} |
||
1543 | // |
||
1544 | |||
1545 | // MK eingeschaltet und GPS-ok, danach Motoren gestartet ==> Berechnung horizontaler/vertikaler Servowinkel |
||
1546 | // Hauptprogramm von GPS Antennen-Nachf�hrung |
||
1547 | //void Tracking_GPS(void) |
||
1548 | //{ //NaviData_t *navidata; |
||
1549 | // static uint8_t track_running = 0; |
||
1550 | // |
||
1551 | // if (!track_running) |
||
1552 | // { |
||
1553 | // //track_running = 1; // verhindert doppelten Aufruf, wenn in Eingabeschleife Menu_MK_BatteryChangeNr() !!! |
||
1554 | // //if (OSD_Data_valid(&naviData)) { |
||
1555 | // if (coldstart) |
||
1556 | // { |
||
1557 | // //// erst nach Neustart NGVideo und beim Motorstart werden Daten vom MK �bernommen |
||
1558 | // //if (naviData->FCFlags & FC_FLAG_MOTOR_START) |
||
1559 | // //{ |
||
1560 | // MK_pos.Home_Lon = (double)naviData->HomePosition.Longitude / 10000000.0; |
||
1561 | // MK_pos.Home_Lat = (double)naviData->HomePosition.Latitude / 10000000.0; |
||
1562 | // MK_pos.Home_Lon7 = naviData->HomePosition.Longitude; |
||
1563 | // MK_pos.Home_Lat7 = naviData->HomePosition.Latitude; |
||
1564 | // MK_pos.Home_Alt = naviData->HomePosition.Altitude; |
||
1565 | // MK_pos.direction = naviData->CompassHeading; |
||
1566 | // coldstart = 0; |
||
1567 | // //} |
||
1568 | // //} |
||
1569 | // //else { |
||
1570 | // //do_tracking(); |
||
1571 | // } |
||
1572 | // //} |
||
1573 | // track_running = 0; |
||
1574 | // } |
||
1575 | // do_tracking(); |
||
1576 | //} |
||
1577 | #endif |