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