Rev 517 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 517 | Rev 518 | ||
|---|---|---|---|
| Line 77... | Line 77... | ||
| 77 | unsigned char MAX_GAS,MIN_GAS; |
77 | unsigned char MAX_GAS,MIN_GAS; |
| 78 | unsigned char Notlandung = 0; |
78 | unsigned char Notlandung = 0; |
| 79 | unsigned char HoehenReglerAktiv = 0; |
79 | unsigned char HoehenReglerAktiv = 0; |
| 80 | long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L; |
80 | long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L; |
| Line -... | Line 81... | ||
| - | 81 | ||
| - | 82 | unsigned char blinkcount_LED1 = 0;//Hilfszähler für die blinkende LED (310807Kr) |
|
| - | 83 | unsigned char LED1_TOTALTIME = 0;//Parameter für Blinkverhalten von LED1 (091207Kr) |
|
| - | 84 | unsigned char LED1_ONTIME = 0;//Parameter für Blinkverhalten von LED1 (091207Kr) |
|
| - | 85 | unsigned int modell_fliegt_gps = 0;//(030907Kr) |
|
| - | 86 | ||
| 81 | 87 | ||
| 82 | float GyroFaktor; |
88 | float GyroFaktor; |
| 83 | float IntegralFaktor; |
89 | float IntegralFaktor; |
| 84 | volatile int DiffNick,DiffRoll; |
90 | volatile int DiffNick,DiffRoll; |
| 85 | int Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0; |
91 | int Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0, Poti5 = 0, Poti6 = 0, Poti7 = 0, Poti8 = 0; //PPM24-Erweiterung (121007Kr) |
| 86 | volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count; |
92 | volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count; |
| 87 | unsigned char MotorWert[5]; |
93 | unsigned char MotorWert[5]; |
| 88 | volatile unsigned char SenderOkay = 0; |
94 | volatile unsigned char SenderOkay = 0; |
| 89 | int StickNick = 0,StickRoll = 0,StickGier = 0; |
95 | int StickNick = 0,StickRoll = 0,StickGier = 0; |
| Line 179... | Line 185... | ||
| 179 | MesswertGier = 0; |
185 | MesswertGier = 0; |
| 180 | StartLuftdruck = Luftdruck; |
186 | StartLuftdruck = Luftdruck; |
| 181 | HoeheD = 0; |
187 | HoeheD = 0; |
| 182 | Mess_Integral_Hoch = 0; |
188 | Mess_Integral_Hoch = 0; |
| 183 | KompassStartwert = KompassValue; |
189 | KompassStartwert = KompassValue; |
| 184 | GPS_Neutral(); |
- | |
| 185 | beeptime = 50; |
190 | beeptime = 50; |
| 186 | Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L; |
191 | Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L; |
| 187 | Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L; |
192 | Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L; |
| 188 | ExternHoehenValue = 0; |
193 | ExternHoehenValue = 0; |
| 189 | } |
194 | } |
| Line 284... | Line 289... | ||
| 284 | IntegralNick = Mess_IntegralNick; |
289 | IntegralNick = Mess_IntegralNick; |
| 285 | IntegralRoll = Mess_IntegralRoll; |
290 | IntegralRoll = Mess_IntegralRoll; |
| 286 | IntegralNick2 = Mess_IntegralNick2; |
291 | IntegralNick2 = Mess_IntegralNick2; |
| 287 | IntegralRoll2 = Mess_IntegralRoll2; |
292 | IntegralRoll2 = Mess_IntegralRoll2; |
| Line -... | Line 293... | ||
| - | 293 | ||
| 288 | 294 | //------------------------------------------------------------------------------ |
|
| 289 | if(EE_Parameter.GlobalConfig & CFG_DREHRATEN_BEGRENZER && !Looping_Nick && !Looping_Roll) |
295 | if(EE_Parameter.GlobalConfig & CFG_DREHRATEN_BEGRENZER && !Looping_Nick && !Looping_Roll && !(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD)) //um Heading_Hold erweitert //(071107Kr) |
| 290 | { |
296 | { |
| 291 | if(MesswertNick > 200) MesswertNick += 4 * (MesswertNick - 200); |
297 | if(MesswertNick > 200) MesswertNick += 4 * (MesswertNick - 200); |
| 292 | else if(MesswertNick < -200) MesswertNick += 4 * (MesswertNick + 200); |
298 | else if(MesswertNick < -200) MesswertNick += 4 * (MesswertNick + 200); |
| 293 | if(MesswertRoll > 200) MesswertRoll += 4 * (MesswertRoll - 200); |
299 | if(MesswertRoll > 200) MesswertRoll += 4 * (MesswertRoll - 200); |
| 294 | else if(MesswertRoll < -200) MesswertRoll += 4 * (MesswertRoll + 200); |
300 | else if(MesswertRoll < -200) MesswertRoll += 4 * (MesswertRoll + 200); |
| - | 301 | } |
|
| - | 302 | //------------------------------------------------------------------------------ |
|
| 295 | } |
303 | |
| 296 | if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--; |
304 | if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--; |
| 297 | if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--; |
305 | if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--; |
| 298 | if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--; |
306 | if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--; |
| - | 307 | if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--; |
|
| - | 308 | //PPM24-Erweiterung (121007Kr) |
|
| - | 309 | if(Poti5 < PPM_in[9] + 110) Poti5++; else if(Poti5 > PPM_in[9] + 110 && Poti5) Poti5--; |
|
| - | 310 | if(Poti6 < PPM_in[10] + 110) Poti6++; else if(Poti6 > PPM_in[10] + 110 && Poti6) Poti6--; |
|
| - | 311 | if(Poti7 < PPM_in[11] + 110) Poti7++; else if(Poti7 > PPM_in[11] + 110 && Poti7) Poti7--; |
|
| - | 312 | if(Poti8 < PPM_in[12] + 110) Poti8++; else if(Poti8 > PPM_in[12] + 110 && Poti8) Poti8--; |
|
| - | 313 | ||
| 299 | if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--; |
314 | |
| 300 | if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255; |
315 | if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255; |
| 301 | if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255; |
316 | if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255; |
| 302 | if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255; |
317 | if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255; |
| - | 318 | if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255; |
|
| - | 319 | //PPM24-Erweiterung (121007Kr) |
|
| - | 320 | if(Poti5 < 0) Poti5 = 0; else if(Poti5 > 255) Poti5 = 255; |
|
| - | 321 | if(Poti6 < 0) Poti6 = 0; else if(Poti6 > 255) Poti6 = 255; |
|
| - | 322 | if(Poti7 < 0) Poti7 = 0; else if(Poti7 > 255) Poti7 = 255; |
|
| 303 | if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255; |
323 | if(Poti8 < 0) Poti8 = 0; else if(Poti8 > 255) Poti8 = 255; |
| Line 304... | Line 324... | ||
| 304 | } |
324 | } |
| 305 | 325 | ||
| 306 | //############################################################################ |
326 | //############################################################################ |
| Line 320... | Line 340... | ||
| 320 | ANALOG_ON; |
340 | ANALOG_ON; |
| 321 | if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--; |
341 | if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--; |
| 322 | if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--; |
342 | if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--; |
| 323 | if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--; |
343 | if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--; |
| 324 | if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--; |
344 | if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--; |
| - | 345 | //PPM24-Erweiterung (121007Kr) |
|
| - | 346 | if(Poti5 < PPM_in[9] + 110) Poti5++; else if(Poti5 > PPM_in[9] + 110 && Poti5) Poti5--; |
|
| - | 347 | if(Poti6 < PPM_in[10] + 110) Poti6++; else if(Poti6 > PPM_in[10] + 110 && Poti6) Poti6--; |
|
| - | 348 | if(Poti7 < PPM_in[11] + 110) Poti7++; else if(Poti7 > PPM_in[11] + 110 && Poti7) Poti7--; |
|
| - | 349 | if(Poti8 < PPM_in[12] + 110) Poti8++; else if(Poti8 > PPM_in[12] + 110 && Poti8) Poti8--; |
|
| - | 350 | ||
| - | 351 | ||
| - | 352 | ||
| - | 353 | ||
| 325 | if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255; |
354 | if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255; |
| 326 | if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255; |
355 | if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255; |
| 327 | if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255; |
356 | if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255; |
| 328 | if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255; |
357 | if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255; |
| - | 358 | //PPM24-Erweiterung (121007Kr) |
|
| - | 359 | if(Poti5 < 0) Poti5 = 0; else if(Poti5 > 255) Poti5 = 255; |
|
| - | 360 | if(Poti6 < 0) Poti6 = 0; else if(Poti6 > 255) Poti6 = 255; |
|
| - | 361 | if(Poti7 < 0) Poti7 = 0; else if(Poti7 > 255) Poti7 = 255; |
|
| - | 362 | if(Poti8 < 0) Poti8 = 0; else if(Poti8 > 255) Poti8 = 255; |
|
| Line 329... | Line 363... | ||
| 329 | 363 | ||
| 330 | Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L; |
364 | Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L; |
| 331 | Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagNick * 2500L; |
365 | Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagNick * 2500L; |
| Line 346... | Line 380... | ||
| 346 | if(MotorTest[1]) Motor_Hinten = MotorTest[1]; |
380 | if(MotorTest[1]) Motor_Hinten = MotorTest[1]; |
| 347 | if(MotorTest[2]) Motor_Links = MotorTest[2]; |
381 | if(MotorTest[2]) Motor_Links = MotorTest[2]; |
| 348 | if(MotorTest[3]) Motor_Rechts = MotorTest[3]; |
382 | if(MotorTest[3]) Motor_Rechts = MotorTest[3]; |
| 349 | } |
383 | } |
| Line 350... | Line 384... | ||
| 350 | 384 | ||
| 351 | DebugOut.Analog[12] = Motor_Vorne; |
385 | // DebugOut.Analog[12] = Motor_Vorne; |
| 352 | DebugOut.Analog[13] = Motor_Hinten; |
386 | // DebugOut.Analog[13] = Motor_Hinten; |
| 353 | DebugOut.Analog[14] = Motor_Links; |
387 | // DebugOut.Analog[14] = Motor_Links; |
| Line 354... | Line 388... | ||
| 354 | DebugOut.Analog[15] = Motor_Rechts; |
388 | // DebugOut.Analog[15] = Motor_Rechts; |
| 355 | 389 | ||
| 356 | //Start I2C Interrupt Mode |
390 | //Start I2C Interrupt Mode |
| 357 | twi_state = 0; |
391 | twi_state = 0; |
| Line 464... | Line 498... | ||
| 464 | Notlandung = 0; |
498 | Notlandung = 0; |
| 465 | RcLostTimer = EE_Parameter.NotGasZeit * 50; |
499 | RcLostTimer = EE_Parameter.NotGasZeit * 50; |
| 466 | if(GasMischanteil > 40) |
500 | if(GasMischanteil > 40) |
| 467 | { |
501 | { |
| 468 | if(modell_fliegt < 0xffff) modell_fliegt++; |
502 | if(modell_fliegt < 0xffff) modell_fliegt++; |
| - | 503 | modell_fliegt_gps = modell_fliegt;// (030907Kr) |
|
| 469 | } |
504 | } |
| 470 | if((modell_fliegt < 200) || (GasMischanteil < 40)) |
505 | if((modell_fliegt < 200) || (GasMischanteil < 40)) |
| 471 | { |
506 | { |
| 472 | SummeNick = 0; |
507 | SummeNick = 0; |
| 473 | SummeRoll = 0; |
508 | SummeRoll = 0; |
| Line 608... | Line 643... | ||
| 608 | 643 | ||
| 609 | if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor = 0; |
644 | if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor = 0; |
| 610 | if(GyroFaktor < 0) GyroFaktor = 0; |
645 | if(GyroFaktor < 0) GyroFaktor = 0; |
| 611 | if(IntegralFaktor < 0) IntegralFaktor = 0; |
646 | if(IntegralFaktor < 0) IntegralFaktor = 0; |
| - | 647 | // greift in den Stick ein, um ungewolltes überschlagen zu verhindern |
|
| 612 | // greift in den Stick ein, um ungewolltes überschlagen zu verhindern |
648 | //bei aktiviertem GPS dürfen die Stickwerte nicht beeinflusst werden, da ansonsten ein Neulernen der Sollposition stattfinden könnte //(051107Kr) |
| 613 | if(!(EE_Parameter.LoopConfig & CFG_LOOP_LINKS) && !(EE_Parameter.LoopConfig & CFG_LOOP_RECHTS)) |
649 | if(!(EE_Parameter.LoopConfig & CFG_LOOP_LINKS) && !(EE_Parameter.LoopConfig & CFG_LOOP_RECHTS) && !(EE_Parameter.GlobalConfig & CFG_GPS_AKTIV) && !(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD)) //(051107Kr) |
| 614 | { |
650 | { |
| 615 | if(IntegralNick > 60000) |
651 | if(IntegralNick > 60000) |
| 616 | { |
652 | { |
| 617 | StickNick -= 8 * EE_Parameter.Stick_P; |
653 | StickNick -= 8 * EE_Parameter.Stick_P; |
| Line 633... | Line 669... | ||
| 633 | { |
669 | { |
| 634 | StickRoll += 8 * EE_Parameter.Stick_P; |
670 | StickRoll += 8 * EE_Parameter.Stick_P; |
| 635 | if(IntegralRoll > 80000) StickRoll -= 16 * EE_Parameter.Stick_P; |
671 | if(IntegralRoll > 80000) StickRoll -= 16 * EE_Parameter.Stick_P; |
| 636 | } |
672 | } |
| 637 | } |
673 | } |
| - | 674 | ||
| - | 675 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
| - | 676 | // LED Stuff |
|
| - | 677 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
| - | 678 | ||
| - | 679 | LED1_TOTALTIME = Parameter_UserParam7; |
|
| - | 680 | LED1_ONTIME = Parameter_UserParam8; |
|
| - | 681 | ||
| - | 682 | LED1_OFF; |
|
| - | 683 | ||
| - | 684 | if(LED1_TOTALTIME >= 200) |
|
| - | 685 | { |
|
| - | 686 | LED1_ON; |
|
| - | 687 | blinkcount_LED1 = 0; |
|
| - | 688 | } |
|
| - | 689 | ||
| - | 690 | else if(blinkcount_LED1 >= LED1_TOTALTIME && LED1_TOTALTIME > 0 && MotorenEin == 1) |
|
| - | 691 | { |
|
| - | 692 | LED1_ON; |
|
| - | 693 | ||
| - | 694 | if(blinkcount_LED1 >= LED1_ONTIME + LED1_TOTALTIME) |
|
| - | 695 | { |
|
| - | 696 | LED1_OFF; |
|
| - | 697 | blinkcount_LED1 = 0; |
|
| - | 698 | } |
|
| - | 699 | } |
|
| - | 700 | ||
| - | 701 | blinkcount_LED1++; |
|
| - | 702 | ||
| - | 703 | //if(PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] > 75) LED1_ON; else LED1_OFF; //Erweiterung von Smartie aus dem MK-Forum vom 14.06.07. Dadurch können die Transistorausgänge J16 und J17 geschaltet werden. // (160607Kr) |
|
| - | 704 | //if(PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] > 75) LED2_ON; else LED2_OFF; //Erweiterung von Smartie aus dem MK-Forum vom 14.06.07. Dadurch können die Transistorausgänge J16 und J17 geschaltet werden. // (160607Kr) |
|
| - | 705 | ||
| 638 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
706 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 639 | // Looping? |
707 | // Looping? |
| 640 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
708 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 641 | if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_LINKS) Looping_Links = 1; |
709 | if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_LINKS) Looping_Links = 1; |
| 642 | else |
710 | else |
| Line 783... | Line 851... | ||
| 783 | 851 | ||
| 784 | // IntegralFehlerNick = (IntegralFehlerNick * 1 + tmp_long) / 2; |
852 | // IntegralFehlerNick = (IntegralFehlerNick * 1 + tmp_long) / 2; |
| Line 785... | Line 853... | ||
| 785 | // IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2; |
853 | // IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2; |
| 786 | 854 | ||
| 787 | 855 | ||
| 788 | DebugOut.Analog[17] = IntegralAccNick / 26; |
856 | //DebugOut.Analog[17] = IntegralAccNick / 26; |
| 789 | DebugOut.Analog[18] = IntegralAccRoll / 26; |
857 | //DebugOut.Analog[18] = IntegralAccRoll / 26; |
| 790 | DebugOut.Analog[19] = IntegralFehlerNick;// / 26; |
858 | //DebugOut.Analog[19] = IntegralFehlerNick;// / 26; |
| 791 | DebugOut.Analog[20] = IntegralFehlerRoll;// / 26; |
859 | //DebugOut.Analog[20] = IntegralFehlerRoll;// / 26; |
| 792 | DebugOut.Analog[21] = MittelIntegralNick / 26; |
860 | //DebugOut.Analog[21] = MittelIntegralNick / 26; |
| 793 | DebugOut.Analog[22] = MittelIntegralRoll / 26; |
861 | //DebugOut.Analog[22] = MittelIntegralRoll / 26; |
| Line 794... | Line 862... | ||
| 794 | //DebugOut.Analog[28] = ausgleichNick; |
862 | //DebugOut.Analog[28] = ausgleichNick; |
| 795 | DebugOut.Analog[29] = ausgleichRoll; |
863 | //DebugOut.Analog[29] = ausgleichRoll; |
| 796 | DebugOut.Analog[30] = LageKorrekturRoll * 10; |
864 | //DebugOut.Analog[30] = LageKorrekturRoll * 10; |
| 797 | 865 | ||
| Line 859... | Line 927... | ||
| 859 | } else last_r_n = 0; |
927 | } else last_r_n = 0; |
| 860 | } else |
928 | } else |
| 861 | { |
929 | { |
| 862 | cnt = 0; |
930 | cnt = 0; |
| 863 | } |
931 | } |
| 864 | DebugOut.Analog[27] = ausgleichRoll; |
932 | //DebugOut.Analog[27] = ausgleichRoll; |
| 865 | if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp; |
933 | if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp; |
| 866 | //if(cnt > 1) beeptime = 50; |
934 | //if(cnt > 1) beeptime = 50; |
| 867 | if(IntegralFehlerRoll > FEHLER_LIMIT) AdNeutralRoll += cnt; |
935 | if(IntegralFehlerRoll > FEHLER_LIMIT) AdNeutralRoll += cnt; |
| 868 | if(IntegralFehlerRoll < -FEHLER_LIMIT) AdNeutralRoll -= cnt; |
936 | if(IntegralFehlerRoll < -FEHLER_LIMIT) AdNeutralRoll -= cnt; |
| 869 | DebugOut.Analog[23] = AdNeutralNick;//10*(AdNeutralNick - StartNeutralNick); |
937 | //DebugOut.Analog[23] = AdNeutralNick;//10*(AdNeutralNick - StartNeutralNick); |
| 870 | DebugOut.Analog[24] = 10*(AdNeutralRoll - StartNeutralRoll); |
938 | //DebugOut.Analog[24] = 10*(AdNeutralRoll - StartNeutralRoll); |
| 871 | } |
939 | } |
| 872 | else |
940 | else |
| 873 | { |
941 | { |
| 874 | LageKorrekturRoll = 0; |
942 | LageKorrekturRoll = 0; |
| 875 | LageKorrekturNick = 0; |
943 | LageKorrekturNick = 0; |
| Line 927... | Line 995... | ||
| 927 | if(SignalSchlecht) SignalSchlecht--; |
995 | if(SignalSchlecht) SignalSchlecht--; |
| 928 | } |
996 | } |
| 929 | else SignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek |
997 | else SignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek |
| 930 | } |
998 | } |
| 931 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
999 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| - | 1000 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
| Line -... | Line 1001... | ||
| - | 1001 | ||
| - | 1002 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
| - | 1003 | // GPS |
|
| - | 1004 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
| - | 1005 | if(EE_Parameter.GlobalConfig & CFG_GPS_AKTIV) |
|
| - | 1006 | { |
|
| - | 1007 | P_GPS_Verstaerkung = Parameter_UserParam5; |
|
| - | 1008 | D_GPS_Verstaerkung = Parameter_UserParam6; |
|
| - | 1009 | gps_main(); |
|
| - | 1010 | } |
|
| - | 1011 | else |
|
| - | 1012 | { |
|
| - | 1013 | GPS_Nick = 0; |
|
| - | 1014 | GPS_Roll = 0; |
|
| 932 | 1015 | } |
|
| 933 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1016 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 934 | // Debugwerte zuordnen |
1017 | // Debugwerte zuordnen |
| 935 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1018 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 936 | if(!TimerWerteausgabe--) |
1019 | if(!TimerWerteausgabe--) |
| 937 | { |
1020 | { |
| - | 1021 | TimerWerteausgabe = 24; |
|
| 938 | TimerWerteausgabe = 24; |
1022 | |
| 939 | DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor; |
1023 | DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor; |
| 940 | DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor; |
1024 | DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor; |
| 941 | DebugOut.Analog[2] = Mittelwert_AccNick; |
1025 | DebugOut.Analog[2] = Mittelwert_AccNick; |
| 942 | DebugOut.Analog[3] = Mittelwert_AccRoll; |
1026 | DebugOut.Analog[3] = Mittelwert_AccRoll; |
| 943 | DebugOut.Analog[4] = MesswertGier; |
1027 | DebugOut.Analog[4] = Parameter_UserParam1; |
| - | 1028 | DebugOut.Analog[5] = Parameter_UserParam2; |
|
| 944 | DebugOut.Analog[5] = HoehenWert; |
1029 | DebugOut.Analog[6] = Soll_Position_North; |
| 945 | DebugOut.Analog[6] =(Mess_Integral_Hoch / 512); |
1030 | DebugOut.Analog[7] = Soll_Position_East; |
| 946 | DebugOut.Analog[8] = KompassValue; |
1031 | DebugOut.Analog[8] = KompassValue; |
| 947 | DebugOut.Analog[9] = UBat; |
1032 | DebugOut.Analog[9] = UBat; |
| - | 1033 | DebugOut.Analog[10] = SenderOkay; |
|
| - | 1034 | DebugOut.Analog[11] = actualPos.GPSFix; |
|
| - | 1035 | DebugOut.Analog[12] = (motor_rx[0]+motor_rx[1]+motor_rx[2]+motor_rx[3]); //Gesamtstrom |
|
| 948 | DebugOut.Analog[10] = SenderOkay; |
1036 | DebugOut.Analog[13] = actualPos.GSpeed; //Geschwindigkeit über Grund |
| 949 | DebugOut.Analog[16] = Mittelwert_AccHoch; |
- | |
| - | 1037 | DebugOut.Analog[14] = actualPos.northing; |
|
| 950 | 1038 | DebugOut.Analog[15] = actualPos.easting; |
|
| 951 | /* DebugOut.Analog[16] = motor_rx[0]; |
1039 | DebugOut.Analog[16] = Poti3; |
| 952 | DebugOut.Analog[17] = motor_rx[1]; |
1040 | DebugOut.Analog[17] = actualPos.altitude; |
| 953 | DebugOut.Analog[18] = motor_rx[2]; |
1041 | DebugOut.Analog[18] = GPS_Home_North; |
| 954 | DebugOut.Analog[19] = motor_rx[3]; |
1042 | DebugOut.Analog[19] = GPS_Home_East; |
| - | 1043 | DebugOut.Analog[20] = GPS_Positionsabweichung_North; |
|
| - | 1044 | DebugOut.Analog[21] = GPS_Positionsabweichung_East; |
|
| - | 1045 | DebugOut.Analog[22] = P_Einfluss_North; |
|
| - | 1046 | DebugOut.Analog[23] = P_Einfluss_East; |
|
| - | 1047 | DebugOut.Analog[24] = D_Einfluss_North; |
|
| 955 | DebugOut.Analog[20] = motor_rx[0] + motor_rx[1] + motor_rx[2] + motor_rx[3]; |
1048 | DebugOut.Analog[25] = D_Einfluss_East; |
| 956 | DebugOut.Analog[20] /= 14; |
1049 | DebugOut.Analog[26] = NORTH_MITTEL; |
| 957 | DebugOut.Analog[21] = motor_rx[4]; |
1050 | DebugOut.Analog[27] = EAST_MITTEL; |
| 958 | DebugOut.Analog[22] = motor_rx[5]; |
1051 | DebugOut.Analog[28] = GPS_Nick; |
| 959 | DebugOut.Analog[23] = motor_rx[6]; |
1052 | DebugOut.Analog[29] = GPS_Roll; |
| 960 | DebugOut.Analog[24] = motor_rx[7]; |
1053 | DebugOut.Analog[30] = StickNick; |
| 961 | DebugOut.Analog[25] = motor_rx[4] + motor_rx[5] + motor_rx[6] + motor_rx[7]; |
1054 | DebugOut.Analog[31] = StickRoll; |
| 962 | */ |
1055 | |
| 963 | // DebugOut.Analog[9] = MesswertNick; |
1056 | //DebugOut.Analog[xx] = motor_rx[0]; //Motorstrom vorne |
| 964 | // DebugOut.Analog[9] = SollHoehe; |
1057 | //DebugOut.Analog[xx] = motor_rx[1]; //Motorstrom hinten |
| - | 1058 | //DebugOut.Analog[xx] = motor_rx[2]; //Motorstrom links |
|
| 965 | // DebugOut.Analog[10] = Mess_Integral_Gier / 128; |
1059 | //DebugOut.Analog[xx] = motor_rx[3]; //Motorstrom rechts |
| 966 | // DebugOut.Analog[11] = KompassStartwert; |
1060 | //DebugOut.Analog[xx] = actualPos.velNorth; |
| 967 | // DebugOut.Analog[10] = Parameter_Gyro_I; |
1061 | //DebugOut.Analog[xx] = actualPos.velEast; |
| 968 | // DebugOut.Analog[10] = EE_Parameter.Gyro_I; |
1062 | //DebugOut.Analog[xx] = Parameter_UserParam3; |
| 969 | // DebugOut.Analog[9] = KompassRichtung; |
1063 | //DebugOut.Analog[xx] = Parameter_UserParam4; |
| 970 | // DebugOut.Analog[10] = GasMischanteil; |
1064 | //DebugOut.Analog[xx] = StickGier; |
| 971 | // DebugOut.Analog[3] = HoeheD * 32; |
1065 | //DebugOut.Analog[xx] = GPS_North; |
| - | 1066 | //DebugOut.Analog[xx] = GPS_East; |
|
| 972 | // DebugOut.Analog[4] = hoehenregler; |
1067 | |
| Line 973... | Line 1068... | ||
| 973 | } |
1068 | } |
| 974 | 1069 | ||
| 975 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1070 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| Line 980... | Line 1075... | ||
| 980 | if(Looping_Roll) MesswertRoll = MesswertRoll * GyroFaktor; |
1075 | if(Looping_Roll) MesswertRoll = MesswertRoll * GyroFaktor; |
| 981 | else MesswertRoll = IntegralRoll * IntegralFaktor + MesswertRoll * GyroFaktor; |
1076 | else MesswertRoll = IntegralRoll * IntegralFaktor + MesswertRoll * GyroFaktor; |
| 982 | // MesswertGier = MesswertGier * (GyroFaktor/2) + Integral_Gier * IntegralFaktor; |
1077 | // MesswertGier = MesswertGier * (GyroFaktor/2) + Integral_Gier * IntegralFaktor; |
| 983 | MesswertGier = MesswertGier * (GyroFaktor) + Integral_Gier * IntegralFaktor/2; |
1078 | MesswertGier = MesswertGier * (GyroFaktor) + Integral_Gier * IntegralFaktor/2; |
| Line 984... | Line 1079... | ||
| 984 | 1079 | ||
| 985 | DebugOut.Analog[28] = MesswertRoll; |
1080 | //DebugOut.Analog[28] = MesswertRoll; |
| 986 | DebugOut.Analog[25] = IntegralRoll * IntegralFaktor; |
1081 | //DebugOut.Analog[25] = IntegralRoll * IntegralFaktor; |
| Line 987... | Line 1082... | ||
| 987 | DebugOut.Analog[31] = StickRoll;// / (26*IntegralFaktor); |
1082 | //DebugOut.Analog[31] = StickRoll;// / (26*IntegralFaktor); |
| 988 | 1083 | ||
| 989 | // Maximalwerte abfangen |
1084 | // Maximalwerte abfangen |
| 990 | #define MAX_SENSOR 2048 |
1085 | #define MAX_SENSOR 2048 |
| Line 1041... | Line 1136... | ||
| 1041 | } |
1136 | } |
| 1042 | } |
1137 | } |
| 1043 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1138 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 1044 | // + Mischer und PI-Regler |
1139 | // + Mischer und PI-Regler |
| 1045 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1140 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 1046 | DebugOut.Analog[7] = GasMischanteil; |
1141 | // DebugOut.Analog[7] = GasMischanteil; |
| 1047 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1142 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 1048 | // Gier-Anteil |
1143 | // Gier-Anteil |
| 1049 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1144 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 1050 | #define MUL_G 1.0 |
1145 | #define MUL_G 1.0 |
| 1051 | GierMischanteil = MesswertGier - sollGier; // Regler für Gier |
1146 | GierMischanteil = MesswertGier - sollGier; // Regler für Gier |