Rev 703 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 703 | Rev 706 | ||
|---|---|---|---|
| Line 98... | Line 98... | ||
| 98 | volatile int16_t CompassHeading = 0; |
98 | volatile int16_t CompassHeading = 0; |
| 99 | volatile int16_t CompassCourse = 0; |
99 | volatile int16_t CompassCourse = 0; |
| 100 | volatile int16_t CompassOffCourse = 0; |
100 | volatile int16_t CompassOffCourse = 0; |
| Line 101... | Line 101... | ||
| 101 | 101 | ||
| 102 | // flags |
- | |
| 103 | uint8_t EmergencyLanding = 0; |
- | |
| 104 | uint8_t HightControlActive = 0; |
102 | // flags |
| Line 105... | Line 103... | ||
| 105 | uint8_t MotorsOn = 0; |
103 | uint8_t MotorsOn = 0; |
| Line 106... | Line 104... | ||
| 106 | 104 | ||
| 107 | int32_t TurnOver180Pitch = 250000L, TurnOver180Roll = 250000L; |
105 | int32_t TurnOver180Pitch = 250000L, TurnOver180Roll = 250000L; |
| Line 108... | Line 106... | ||
| 108 | 106 | ||
| Line 109... | Line 107... | ||
| 109 | float GyroFactor; |
107 | float Gyro_P_Factor; |
| Line 162... | Line 160... | ||
| 162 | NeutralAccY = 0; |
160 | NeutralAccY = 0; |
| 163 | NeutralAccZ = 0; |
161 | NeutralAccZ = 0; |
| 164 | AdNeutralPitch = 0; |
162 | AdNeutralPitch = 0; |
| 165 | AdNeutralRoll = 0; |
163 | AdNeutralRoll = 0; |
| 166 | AdNeutralYaw = 0; |
164 | AdNeutralYaw = 0; |
| 167 | FCParam.AchsKopplung1 = 0; |
165 | FCParam.Yaw_PosFeedback = 0; |
| 168 | FCParam.AchsGegenKopplung1 = 0; |
166 | FCParam.Yaw_NegFeedback = 0; |
| 169 | CalibMean(); |
167 | CalibMean(); |
| 170 | Delay_ms_Mess(100); |
168 | Delay_ms_Mess(100); |
| 171 | CalibMean(); |
169 | CalibMean(); |
| 172 | if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL)) // Hight Control activated? |
170 | if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL)) // Hight Control activated? |
| 173 | { |
171 | { |
| Line 240... | Line 238... | ||
| 240 | // Coupling fraction |
238 | // Coupling fraction |
| 241 | if(!Looping_Pitch && !Looping_Roll && (ParamSet.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE)) |
239 | if(!Looping_Pitch && !Looping_Roll && (ParamSet.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE)) |
| 242 | { |
240 | { |
| 243 | tmpl = Reading_IntegralPitch / 4096L; |
241 | tmpl = Reading_IntegralPitch / 4096L; |
| 244 | tmpl *= ReadingYaw; |
242 | tmpl *= ReadingYaw; |
| 245 | tmpl *= FCParam.AchsKopplung1; //125 |
243 | tmpl *= FCParam.Yaw_PosFeedback; //125 |
| 246 | tmpl /= 2048L; |
244 | tmpl /= 2048L; |
| 247 | tmpl2 = Reading_IntegralRoll / 4096L; |
245 | tmpl2 = Reading_IntegralRoll / 4096L; |
| 248 | tmpl2 *= ReadingYaw; |
246 | tmpl2 *= ReadingYaw; |
| 249 | tmpl2 *= FCParam.AchsKopplung1; |
247 | tmpl2 *= FCParam.Yaw_PosFeedback; |
| 250 | tmpl2 /= 2048L; |
248 | tmpl2 /= 2048L; |
| 251 | } |
249 | } |
| 252 | else tmpl = tmpl2 = 0; |
250 | else tmpl = tmpl2 = 0; |
| 253 | // Roll |
251 | // Roll |
| 254 | ReadingRoll += tmpl; |
252 | ReadingRoll += tmpl; |
| 255 | ReadingRoll += (tmpl2 * FCParam.AchsGegenKopplung1) / 512L; //109 |
253 | ReadingRoll += (tmpl2 * FCParam.Yaw_NegFeedback) / 512L; //109 |
| 256 | Reading_IntegralRoll2 += ReadingRoll; |
254 | Reading_IntegralRoll2 += ReadingRoll; |
| 257 | Reading_IntegralRoll += ReadingRoll - AttitudeCorrectionRoll; |
255 | Reading_IntegralRoll += ReadingRoll - AttitudeCorrectionRoll; |
| 258 | if(Reading_IntegralRoll > TurnOver180Roll) |
256 | if(Reading_IntegralRoll > TurnOver180Roll) |
| 259 | { |
257 | { |
| 260 | Reading_IntegralRoll = -(TurnOver180Roll - 10000L); |
258 | Reading_IntegralRoll = -(TurnOver180Roll - 10000L); |
| Line 277... | Line 275... | ||
| 277 | if(AdValueGyrRoll > 2020) ReadingRoll = +1000; |
275 | if(AdValueGyrRoll > 2020) ReadingRoll = +1000; |
| 278 | if(AdValueGyrRoll > 2034) ReadingRoll = +2000; |
276 | if(AdValueGyrRoll > 2034) ReadingRoll = +2000; |
| 279 | } |
277 | } |
| 280 | // Pitch |
278 | // Pitch |
| 281 | ReadingPitch -= tmpl2; |
279 | ReadingPitch -= tmpl2; |
| 282 | ReadingPitch -= (tmpl*FCParam.AchsGegenKopplung1) / 512L; |
280 | ReadingPitch -= (tmpl*FCParam.Yaw_NegFeedback) / 512L; |
| 283 | Reading_IntegralPitch2 += ReadingPitch; |
281 | Reading_IntegralPitch2 += ReadingPitch; |
| 284 | Reading_IntegralPitch += ReadingPitch - AttitudeCorrectionPitch; |
282 | Reading_IntegralPitch += ReadingPitch - AttitudeCorrectionPitch; |
| 285 | if(Reading_IntegralPitch > TurnOver180Pitch) |
283 | if(Reading_IntegralPitch > TurnOver180Pitch) |
| 286 | { |
284 | { |
| 287 | Reading_IntegralPitch = -(TurnOver180Pitch - 10000L); |
285 | Reading_IntegralPitch = -(TurnOver180Pitch - 10000L); |
| Line 418... | Line 416... | ||
| 418 | CHK_POTI(FCParam.UserParam6,ParamSet.UserParam6,0,255); |
416 | CHK_POTI(FCParam.UserParam6,ParamSet.UserParam6,0,255); |
| 419 | CHK_POTI(FCParam.UserParam7,ParamSet.UserParam7,0,255); |
417 | CHK_POTI(FCParam.UserParam7,ParamSet.UserParam7,0,255); |
| 420 | CHK_POTI(FCParam.UserParam8,ParamSet.UserParam8,0,255); |
418 | CHK_POTI(FCParam.UserParam8,ParamSet.UserParam8,0,255); |
| 421 | CHK_POTI(FCParam.ServoPitchControl,ParamSet.ServoPitchControl,0,255); |
419 | CHK_POTI(FCParam.ServoPitchControl,ParamSet.ServoPitchControl,0,255); |
| 422 | CHK_POTI(FCParam.LoopGasLimit,ParamSet.LoopGasLimit,0,255); |
420 | CHK_POTI(FCParam.LoopGasLimit,ParamSet.LoopGasLimit,0,255); |
| 423 | CHK_POTI(FCParam.AchsKopplung1, ParamSet.AchsKopplung1,0,255); |
421 | CHK_POTI(FCParam.Yaw_PosFeedback,ParamSet.Yaw_PosFeedback,0,255); |
| 424 | CHK_POTI(FCParam.AchsGegenKopplung1,ParamSet.AchsGegenKopplung1,0,255); |
422 | CHK_POTI(FCParam.Yaw_NegFeedback,ParamSet.Yaw_NegFeedback,0,255); |
| 425 | CHK_POTI(FCParam.DynamicStability,ParamSet.DynamicStability,0,255); |
423 | CHK_POTI(FCParam.DynamicStability,ParamSet.DynamicStability,0,255); |
| Line 426... | Line 424... | ||
| 426 | 424 | ||
| 427 | Ki = (float) FCParam.I_Factor * FACTOR_I; |
425 | Ki = (float) FCParam.I_Factor * FACTOR_I; |
| Line 428... | Line 426... | ||
| 428 | } |
426 | } |
| 429 | - | ||
| - | 427 | ||
| - | 428 | ||
| 430 | 429 | /************************************************************************/ |
|
| 431 | //############################################################################ |
- | |
| 432 | // |
430 | /* MotorControl */ |
| 433 | void MotorRegler(void) |
431 | /************************************************************************/ |
| 434 | //############################################################################ |
432 | void MotorRegler(void) |
| 435 | { |
433 | { |
| 436 | int MotorValue, pd_ergebnis, h, tmp_int; |
434 | int16_t MotorValue, pd_ergebnis, h, tmp_int; |
| 437 | int YawMixingFraction, GasMixingFraction; |
435 | int16_t YawMixingFraction, GasMixingFraction; |
| 438 | static long SumPitch = 0, SumRoll = 0; |
436 | static int32_t SumPitch = 0, SumRoll = 0; |
| 439 | static long SetPointYaw = 0, tmp_long, tmp_long2; |
437 | static int32_t SetPointYaw = 0, tmp_long, tmp_long2; |
| - | 438 | static int32_t IntegralErrorPitch = 0; |
|
| 440 | static long IntegralErrorPitch = 0; |
439 | static int32_t IntegralErrorRoll = 0; |
| 441 | static long IntegralErrorRoll = 0; |
440 | static uint16_t RcLostTimer; |
| 442 | static unsigned int RcLostTimer; |
441 | static uint8_t delay_neutral = 0, delay_startmotors = 0, delay_stopmotors = 0; |
| 443 | static unsigned char delay_neutral = 0; |
442 | static uint16_t Modell_Is_Flying = 0; |
| 444 | static unsigned char delay_einschalten = 0,delay_ausschalten = 0; |
443 | static uint8_t EmergencyLanding = 0; |
| 445 | static unsigned int modell_fliegt = 0; |
444 | static uint8_t HightControlActive = 0; |
| 446 | static int hoehenregler = 0; |
445 | static int16_t hoehenregler = 0; |
| Line 447... | Line 446... | ||
| 447 | static char TimerDebugOut = 0; |
446 | static int8_t TimerDebugOut = 0; |
| Line 448... | Line 447... | ||
| 448 | static char StoreNewCompassCourse = 0; |
447 | static int8_t StoreNewCompassCourse = 0; |
| 449 | static long CorrectionPitch, CorrectionRoll; |
448 | static int32_t CorrectionPitch, CorrectionRoll; |
| 450 | 449 | ||
| 451 | Mean(); |
450 | Mean(); |
| 452 | 451 | ||
| 453 | GRN_ON; |
452 | GRN_ON; |
| 454 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
453 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 455 | // Gaswert ermitteln |
454 | // determine GAS value |
| 456 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
455 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| - | 456 | GasMixingFraction = StickGas; |
|
| - | 457 | if(GasMixingFraction < 0) GasMixingFraction = 0; |
|
| - | 458 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
| - | 459 | // RC-signal is bad |
|
| - | 460 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
| 457 | GasMixingFraction = StickGas; |
461 | // SenderOkay is incremented at good rc-level, i.e. if the ppm-signal deviation |
| 458 | if(GasMixingFraction < 0) GasMixingFraction = 0; |
462 | // of a channel to previous frame is less than 1% the SenderOkay is incremented by 10. |
| 459 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
463 | // Typicaly within a frame of 8 channels (22.5ms) the SenderOkay is incremented by 8 * 10 = 80 |
| 460 | // Emfang schlecht |
464 | // The decremtation of 1 in the mainloop is done every 2 ms, i.e. within a time of one rc frame |
| 461 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
465 | // the main loop is running 11 times that decrements the SenderOkay by 11. |
| 462 | if(SenderOkay < 100) |
466 | if(SenderOkay < 100) // the rc-frame signal is not reveived or noisy |
| 463 | { |
467 | { |
| 464 | if(!PcZugriff) |
468 | if(!PcAccess) // if also no PC-Access via UART |
| 465 | { |
469 | { |
| 466 | if(BeepModulation == 0xFFFF) |
470 | if(BeepModulation == 0xFFFF) |
| 467 | { |
471 | { |
| 468 | BeepTime = 15000; // 1.5 seconds |
472 | BeepTime = 15000; // 1.5 seconds |
| 469 | BeepModulation = 0x0C00; |
473 | BeepModulation = 0x0C00; |
| 470 | } |
474 | } |
| 471 | } |
475 | } |
| 472 | if(RcLostTimer) RcLostTimer--; |
476 | if(RcLostTimer) RcLostTimer--; // decremtent timer after rc sigal lost |
| 473 | else |
477 | else // rc lost countdown finished |
| 474 | { |
478 | { |
| 475 | MotorsOn = 0; |
479 | MotorsOn = 0; // stop all motors |
| 476 | EmergencyLanding = 0; |
480 | EmergencyLanding = 0; // emergency landing is over |
| 477 | } |
481 | } |
| - | 482 | ROT_ON; // set red led |
|
| - | 483 | if(Modell_Is_Flying > 2000) // wahrscheinlich in der Luft --> langsam absenken |
|
| - | 484 | { |
|
| 478 | ROT_ON; |
485 | GasMixingFraction = ParamSet.EmergencyGas; // set emergency gas |
| 479 | if(modell_fliegt > 2000) // wahrscheinlich in der Luft --> langsam absenken |
486 | EmergencyLanding = 1; // enable emergency landing |
| 480 | { |
487 | // set neutral rc inputs |
| 481 | GasMixingFraction = ParamSet.EmergencyGas; |
488 | PPM_diff[ParamSet.ChannelAssignment[CH_PITCH]] = 0; |
| 482 | EmergencyLanding = 1; |
489 | PPM_diff[ParamSet.ChannelAssignment[CH_ROLL]] = 0; |
| 483 | PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] = 0; |
490 | PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] = 0; |
| 484 | PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] = 0; |
491 | PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] = 0; |
| 485 | PPM_in[ParamSet.ChannelAssignment[CH_YAW]] = 0; |
492 | PPM_in[ParamSet.ChannelAssignment[CH_YAW]] = 0; |
| 486 | } |
493 | } |
| 487 | else MotorsOn = 0; |
494 | else MotorsOn = 0; // switch of all motors |
| Line 599... | Line -... | ||
| 599 | } |
- | |
| 600 | } |
644 | MaxStickPitch = abs(PPM_in[ParamSet.ChannelAssignment[CH_PITCH]]); |
| 601 | else delay_ausschalten = 0; |
- | |
| 602 | } |
- | |
| 603 | } |
- | |
| 604 | - | ||
| 605 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
| 606 | // neue Werte von der Funke |
- | |
| 607 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
| 608 | if(!NewPpmData-- || EmergencyLanding) |
- | |
| 609 | { |
- | |
| 610 | int tmp_int; |
- | |
| 611 | ParameterMapping(); |
- | |
| 612 | StickPitch = (StickPitch * 3 + PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] * ParamSet.Stick_P) / 4; |
- | |
| 613 | StickPitch += PPM_diff[ParamSet.ChannelAssignment[CH_PITCH]] * ParamSet.Stick_D; |
- | |
| 614 | StickRoll = (StickRoll * 3 + PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.Stick_P) / 4; |
- | |
| 615 | StickRoll += PPM_diff[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.Stick_D; |
- | |
| 616 | - | ||
| 617 | StickYaw = -PPM_in[ParamSet.ChannelAssignment[CH_YAW]]; |
- | |
| 618 | StickGas = PPM_in[ParamSet.ChannelAssignment[CH_GAS]] + 120; |
- | |
| Line 619... | Line 645... | ||
| 619 | 645 | else MaxStickPitch--; |
|
| 620 | if(abs(PPM_in[ParamSet.ChannelAssignment[CH_PITCH]]) > MaxStickPitch) |
646 | if(abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) > MaxStickRoll) |
| Line 621... | Line 647... | ||
| 621 | MaxStickPitch = abs(PPM_in[ParamSet.ChannelAssignment[CH_PITCH]]); else MaxStickPitch--; |
647 | MaxStickRoll = abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]); |
| 622 | if(abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) > MaxStickRoll) |
648 | else MaxStickRoll--; |
| 623 | MaxStickRoll = abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]); else MaxStickRoll--; |
- | |
| 624 | if(EmergencyLanding) {MaxStickPitch = 0; MaxStickRoll = 0;} |
- | |
| 625 | - | ||
| 626 | GyroFactor = ((float)FCParam.Gyro_P + 10.0) / 256.0; |
- | |
| 627 | IntegralFactor = ((float) FCParam.Gyro_I) / 44000; |
- | |
| 628 | - | ||
| 629 | //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
| 630 | //+ Digitale Steuerung per DubWise |
- | |
| 631 | //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
| 632 | #define KEY_VALUE (FCParam.UserParam1 * 4) //(Poti3 * 8) |
- | |
| 633 | if(DubWiseKeys[1]) BeepTime = 10; |
- | |
| 634 | if(DubWiseKeys[1] & DUB_KEY_UP) tmp_int = KEY_VALUE; else |
- | |
| 635 | if(DubWiseKeys[1] & DUB_KEY_DOWN) tmp_int = -KEY_VALUE; else tmp_int = 0; |
- | |
| 636 | ExternStickPitch= (ExternStickPitch* 7 + tmp_int) / 8; |
- | |
| 637 | if(DubWiseKeys[1] & DUB_KEY_LEFT) tmp_int = KEY_VALUE; else |
- | |
| 638 | if(DubWiseKeys[1] & DUB_KEY_RIGHT) tmp_int = -KEY_VALUE; else tmp_int = 0; |
- | |
| 639 | ExternStickRoll = (ExternStickRoll * 7 + tmp_int) / 8; |
- | |
| 640 | - | ||
| 641 | if(DubWiseKeys[0] & 8) ExternStickYaw = 50;else |
649 | |
| 642 | if(DubWiseKeys[0] & 4) ExternStickYaw =-50;else ExternStickYaw = 0; |
- | |
| 643 | if(DubWiseKeys[0] & 2) ExternHightValue++; |
- | |
| 644 | if(DubWiseKeys[0] & 16) ExternHightValue--; |
- | |
| 645 | - | ||
| 646 | StickPitch += ExternStickPitch/ 8; |
- | |
| 647 | StickRoll += ExternStickRoll / 8; |
- | |
| 648 | StickYaw += ExternStickYaw; |
- | |
| 649 | //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
| 650 | //+ Analoge Steuerung per Seriell |
- | |
| 651 | //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
| Line 652... | Line 650... | ||
| 652 | if(ExternControl.Config & 0x01 && FCParam.UserParam1 > 128) |
650 | // update gyro control loop factors |
| 653 | { |
651 | |
| 654 | StickPitch += (int) ExternControl.Pitch * (int) ParamSet.Stick_P; |
652 | Gyro_P_Factor = ((float) FCParam.Gyro_P + 10.0) / 256.0; |
| 655 | StickRoll += (int) ExternControl.Roll * (int) ParamSet.Stick_P; |
653 | Gyro_I_Factor = ((float) FCParam.Gyro_I) / 44000; |
| 656 | StickYaw += ExternControl.Yaw; |
654 | |
| 657 | ExternHightValue = (int) ExternControl.Hight * (int)ParamSet.Hight_Gain; |
655 | //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 658 | if(ExternControl.Gas < StickGas) StickGas = ExternControl.Gas; |
656 | // Digital Control via DubWise |
| - | 657 | //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
| 659 | } |
658 | |
| 660 | - | ||
| 661 | if(ParamSet.GlobalConfig & CFG_HEADING_HOLD) IntegralFactor = 0; |
- | |
| 662 | if(GyroFactor < 0) GyroFactor = 0; |
659 | #define KEY_VALUE (FCParam.UserParam1 * 4) // step width |
| 663 | if(IntegralFactor < 0) IntegralFactor = 0; |
- | |
| 664 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
660 | if(DubWiseKeys[1]) BeepTime = 10; |
| - | 661 | if(DubWiseKeys[1] & DUB_KEY_UP) tmp_int = KEY_VALUE; |
|
| 665 | // Looping? |
662 | else if(DubWiseKeys[1] & DUB_KEY_DOWN) tmp_int = -KEY_VALUE; |
| 666 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
663 | else tmp_int = 0; |
| 667 | if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_LEFT) Looping_Left = 1; |
- | |
| 668 | else |
664 | ExternStickPitch = (ExternStickPitch * 7 + tmp_int) / 8; |
| 669 | { |
665 | if(DubWiseKeys[1] & DUB_KEY_LEFT) tmp_int = KEY_VALUE; |
| 670 | { |
666 | else if(DubWiseKeys[1] & DUB_KEY_RIGHT) tmp_int = -KEY_VALUE; |
| 671 | if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < (ParamSet.LoopThreshold - ParamSet.LoopHysteresis))) Looping_Left = 0; |
667 | else tmp_int = 0; |
| 672 | } |
668 | ExternStickRoll = (ExternStickRoll * 7 + tmp_int) / 8; |
| Line 673... | Line -... | ||
| 673 | } |
- | |
| 674 | if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < -ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_RIGHT) Looping_Right = 1; |
- | |
| 675 | else |
- | |
| 676 | { |
- | |
| 677 | if(Looping_Right) // Hysterese |
- | |
| 678 | { |
- | |
| 679 | if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > -(ParamSet.LoopThreshold - ParamSet.LoopHysteresis)) Looping_Right = 0; |
- | |
| 680 | } |
- | |
| 681 | } |
- | |
| 682 | - | ||
| 683 | if((PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_UP) Looping_Top = 1; |
- | |
| 684 | else |
- | |
| 685 | { |
- | |
| 686 | if(Looping_Top) // Hysterese |
- | |
| 687 | { |
- | |
| 688 | if((PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] < (ParamSet.LoopThreshold - ParamSet.LoopHysteresis))) Looping_Top = 0; |
- | |
| 689 | } |
- | |
| 690 | } |
- | |
| 691 | if((PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] < -ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_DOWN) Looping_Down = 1; |
- | |
| 692 | else |
- | |
| 693 | { |
- | |
| 694 | if(Looping_Down) // Hysterese |
- | |
| 695 | { |
- | |
| 696 | if(PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > -(ParamSet.LoopThreshold - ParamSet.LoopHysteresis)) Looping_Down = 0; |
- | |
| 697 | } |
- | |
| 698 | } |
- | |
| 699 | - | ||
| 700 | if(Looping_Left || Looping_Right) Looping_Roll = 1; else Looping_Roll = 0; |
- | |
| 701 | if(Looping_Top || Looping_Down) {Looping_Pitch = 1; Looping_Roll = 0; Looping_Left = 0; Looping_Right = 0;} else Looping_Pitch = 0; |
669 | |
| 702 | } // Ende neue Funken-Werte |
670 | if(DubWiseKeys[0] & 8) ExternStickYaw = 50;else |
| 703 | 671 | if(DubWiseKeys[0] & 4) ExternStickYaw =-50;else ExternStickYaw = 0; |
|
| 704 | if(Looping_Roll) BeepTime = 100; |
- | |
| 705 | if(Looping_Roll || Looping_Pitch) |
- | |
| 706 | { |
- | |
| 707 | if(GasMixingFraction > ParamSet.LoopGasLimit) GasMixingFraction = ParamSet.LoopGasLimit; |
- | |
| 708 | } |
- | |
| 709 | - | ||
| 710 | - | ||
| 711 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
| 712 | // Bei Empfangsausfall im Flug |
- | |
| 713 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
| Line -... | Line 672... | ||
| - | 672 | if(DubWiseKeys[0] & 2) ExternHightValue++; |
|
| - | 673 | if(DubWiseKeys[0] & 16) ExternHightValue--; |
|
| - | 674 | ||
| - | 675 | StickPitch += ExternStickPitch / 8; |
|
| - | 676 | StickRoll += ExternStickRoll / 8; |
|
| - | 677 | StickYaw += ExternStickYaw; |
|
| - | 678 | ||
| - | 679 | //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
| - | 680 | //+ Analoge Control via serial communication |
|
| - | 681 | //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
| - | 682 | ||
| - | 683 | if(ExternControl.Config & 0x01 && FCParam.UserParam1 > 128) |
|
| - | 684 | { |
|
| Line 714... | Line 685... | ||
| 714 | if(EmergencyLanding) |
685 | StickPitch += (int16_t) ExternControl.Pitch * (int16_t) ParamSet.Stick_P; |
| 715 | { |
- | |
| 716 | StickYaw = 0; |
- | |
| 717 | StickPitch = 0; |
- | |
| 718 | StickRoll = 0; |
- | |
| 719 | GyroFactor = 0.5;//Originalwert von Holger 0.1; |
- | |
| 720 | IntegralFactor = 0.003; //Originalwert von Holger 0.005; |
- | |
| 721 | Looping_Roll = 0; |
- | |
| 722 | Looping_Pitch = 0; |
- | |
| 723 | } |
- | |
| 724 | - | ||
| 725 | - | ||
| 726 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
| 727 | // Integrale auf ACC-Signal abgleichen |
- | |
| 728 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
| 729 | #define ABGLEICH_ANZAHL 256L |
- | |
| 730 | - | ||
| 731 | MeanIntegralPitch += IntegralPitch; // Für die Mittelwertbildung aufsummieren |
- | |
| 732 | MeanIntegralRoll += IntegralRoll; |
- | |
| 733 | MeanIntegralPitch2 += IntegralPitch2; |
- | |
| 734 | MeanIntegralRoll2 += IntegralRoll2; |
686 | StickRoll += (int16_t) ExternControl.Roll * (int16_t) ParamSet.Stick_P; |
| 735 | - | ||
| 736 | if(Looping_Pitch || Looping_Roll) |
- | |
| 737 | { |
- | |
| 738 | IntegralAccPitch = 0; |
- | |
| 739 | IntegralAccRoll = 0; |
687 | StickYaw += ExternControl.Yaw; |
| 740 | MeanIntegralPitch = 0; |
- | |
| 741 | MeanIntegralRoll = 0; |
- | |
| 742 | MeanIntegralPitch2 = 0; |
- | |
| 743 | MeanIntegralRoll2 = 0; |
- | |
| 744 | Reading_IntegralPitch2 = Reading_IntegralPitch; |
- | |
| 745 | Reading_IntegralRoll2 = Reading_IntegralRoll; |
- | |
| 746 | ZaehlMessungen = 0; |
- | |
| 747 | AttitudeCorrectionPitch = 0; |
- | |
| 748 | AttitudeCorrectionRoll = 0; |
- | |
| 749 | } |
- | |
| 750 | - | ||
| 751 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
| 752 | if(!Looping_Pitch && !Looping_Roll) |
- | |
| 753 | { |
- | |
| 754 | long tmp_long, tmp_long2; |
- | |
| 755 | tmp_long = (long)(IntegralPitch / ParamSet.GyroAccFaktor - (long)Mean_AccPitch); |
- | |
| 756 | tmp_long2 = (long)(IntegralRoll / ParamSet.GyroAccFaktor - (long)Mean_AccRoll); |
- | |
| Line -... | Line 688... | ||
| - | 688 | ExternHightValue = (int16_t) ExternControl.Hight * (int16_t)ParamSet.Hight_Gain; |
|
| - | 689 | if(ExternControl.Gas < StickGas) StickGas = ExternControl.Gas; |
|
| - | 690 | } |
|
| - | 691 | // disable I part of gyro control feedback |
|
| - | 692 | if(ParamSet.GlobalConfig & CFG_HEADING_HOLD) Gyro_I_Factor = 0; |
|
| - | 693 | // avoid negative scaling factors |
|
| - | 694 | if(Gyro_P_Factor < 0) Gyro_P_Factor = 0; |
|
| - | 695 | if(Gyro_I_Factor < 0) Gyro_I_Factor = 0; |
|
| - | 696 | ||
| - | 697 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
| - | 698 | // Looping? |
|
| - | 699 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
| - | 700 | ||
| - | 701 | if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_LEFT) Looping_Left = 1; |
|
| - | 702 | else |
|
| - | 703 | { |
|
| - | 704 | { |
|
| - | 705 | if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < (ParamSet.LoopThreshold - ParamSet.LoopHysteresis))) Looping_Left = 0; |
|
| - | 706 | } |
|
| - | 707 | } |
|
| - | 708 | if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < -ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_RIGHT) Looping_Right = 1; |
|
| - | 709 | else |
|
| - | 710 | { |
|
| - | 711 | if(Looping_Right) // Hysterese |
|
| - | 712 | { |
|
| - | 713 | if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > -(ParamSet.LoopThreshold - ParamSet.LoopHysteresis)) Looping_Right = 0; |
|
| - | 714 | } |
|
| - | 715 | } |
|
| - | 716 | ||
| - | 717 | if((PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_UP) Looping_Top = 1; |
|
| - | 718 | else |
|
| - | 719 | { |
|
| - | 720 | if(Looping_Top) // Hysterese |
|
| - | 721 | { |
|
| - | 722 | if((PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] < (ParamSet.LoopThreshold - ParamSet.LoopHysteresis))) Looping_Top = 0; |
|
| - | 723 | } |
|
| - | 724 | } |
|
| - | 725 | if((PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] < -ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_DOWN) Looping_Down = 1; |
|
| - | 726 | else |
|
| - | 727 | { |
|
| - | 728 | if(Looping_Down) // Hysterese |
|
| - | 729 | { |
|
| - | 730 | if(PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > -(ParamSet.LoopThreshold - ParamSet.LoopHysteresis)) Looping_Down = 0; |
|
| - | 731 | } |
|
| - | 732 | } |
|
| - | 733 | ||
| - | 734 | if(Looping_Left || Looping_Right) Looping_Roll = 1; else Looping_Roll = 0; |
|
| - | 735 | if(Looping_Top || Looping_Down) {Looping_Pitch = 1; Looping_Roll = 0; Looping_Left = 0; Looping_Right = 0;} else Looping_Pitch = 0; |
|
| - | 736 | } // End of new RC values |
|
| - | 737 | ||
| - | 738 | ||
| - | 739 | if(Looping_Roll) BeepTime = 100; |
|
| - | 740 | if(Looping_Roll || Looping_Pitch) |
|
| - | 741 | { |
|
| - | 742 | if(GasMixingFraction > ParamSet.LoopGasLimit) GasMixingFraction = ParamSet.LoopGasLimit; |
|
| - | 743 | } |
|
| - | 744 | ||
| - | 745 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
| - | 746 | // in case of emergency landing |
|
| - | 747 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
| - | 748 | ||
| - | 749 | if(EmergencyLanding) |
|
| - | 750 | { |
|
| - | 751 | StickYaw = 0; |
|
| - | 752 | StickPitch = 0; |
|
| - | 753 | StickRoll = 0; |
|
| - | 754 | Gyro_P_Factor = 0.5; |
|
| - | 755 | Gyro_I_Factor = 0.003; |
|
| - | 756 | Looping_Roll = 0; |
|
| - | 757 | Looping_Pitch = 0; |
|
| - | 758 | MaxStickPitch = 0; |
|
| - | 759 | MaxStickRoll = 0; |
|
| - | 760 | } |
|
| - | 761 | ||
| - | 762 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
| - | 763 | // Trim Gyro-Integrals to ACC-Signals |
|
| - | 764 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
| - | 765 | ||
| - | 766 | #define ABGLEICH_ANZAHL 256L |
|
| - | 767 | ||
| - | 768 | MeanIntegralPitch += IntegralPitch; // sum for averaging |
|
| - | 769 | MeanIntegralRoll += IntegralRoll; |
|
| - | 770 | MeanIntegralPitch2 += IntegralPitch2; |
|
| - | 771 | MeanIntegralRoll2 += IntegralRoll2; |
|
| - | 772 | ||
| - | 773 | if(Looping_Pitch || Looping_Roll) // if looping in any direction |
|
| - | 774 | { |
|
| - | 775 | IntegralAccPitch = 0; |
|
| - | 776 | IntegralAccRoll = 0; |
|
| - | 777 | MeanIntegralPitch = 0; |
|
| - | 778 | MeanIntegralRoll = 0; |
|
| - | 779 | MeanIntegralPitch2 = 0; |
|
| - | 780 | MeanIntegralRoll2 = 0; |
|
| - | 781 | Reading_IntegralPitch2 = Reading_IntegralPitch; |
|
| - | 782 | Reading_IntegralRoll2 = Reading_IntegralRoll; |
|
| - | 783 | ZaehlMessungen = 0; |
|
| - | 784 | AttitudeCorrectionPitch = 0; |
|
| - | 785 | AttitudeCorrectionRoll = 0; |
|
| - | 786 | } |
|
| - | 787 | ||
| - | 788 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
| - | 789 | if(!Looping_Pitch && !Looping_Roll) // if not lopping in any direction |
|
| - | 790 | { |
|
| - | 791 | int32_t tmp_long, tmp_long2; |
|
| - | 792 | tmp_long = (int32_t)(IntegralPitch / ParamSet.GyroAccFaktor - (int32_t)Mean_AccPitch); |
|
| - | 793 | tmp_long2 = (int32_t)(IntegralRoll / ParamSet.GyroAccFaktor - (int32_t)Mean_AccRoll); |
|
| 757 | tmp_long /= 16; |
794 | tmp_long /= 16; |
| 758 | tmp_long2 /= 16; |
795 | tmp_long2 /= 16; |
| 759 | if((MaxStickPitch > 15) || (MaxStickRoll > 15)) |
796 | if((MaxStickPitch > 15) || (MaxStickRoll > 15)) |
| 760 | { |
797 | { |
| 761 | tmp_long /= 3; |
798 | tmp_long /= 3; |
| 762 | tmp_long2 /= 3; |
799 | tmp_long2 /= 3; |
| 763 | } |
800 | } |
| 764 | if(abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 25) |
801 | if(abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 25) |
| 765 | { |
802 | { |
| 766 | tmp_long /= 3; |
803 | tmp_long /= 3; |
| Line 767... | Line 804... | ||
| 767 | tmp_long2 /= 3; |
804 | tmp_long2 /= 3; |
| 768 | } |
805 | } |
| 769 | 806 | ||
| Line 913... | Line 950... | ||
| 913 | { |
950 | { |
| 914 | AttitudeCorrectionRoll = 0; |
951 | AttitudeCorrectionRoll = 0; |
| 915 | AttitudeCorrectionPitch = 0; |
952 | AttitudeCorrectionPitch = 0; |
| 916 | } |
953 | } |
| Line 917... | Line 954... | ||
| 917 | 954 | ||
| 918 | if(!IntegralFactor) { AttitudeCorrectionRoll = 0; AttitudeCorrectionPitch = 0;} // z.B. bei HH |
955 | if(!Gyro_I_Factor) { AttitudeCorrectionRoll = 0; AttitudeCorrectionPitch = 0;} // z.B. bei HH |
| 919 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++ |
956 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 920 | MeanIntegralPitch_old = MeanIntegralPitch; |
957 | MeanIntegralPitch_old = MeanIntegralPitch; |
| 921 | MeanIntegralRoll_old = MeanIntegralRoll; |
958 | MeanIntegralRoll_old = MeanIntegralRoll; |
| 922 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++ |
959 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| Line 927... | Line 964... | ||
| 927 | MeanIntegralRoll = 0; |
964 | MeanIntegralRoll = 0; |
| 928 | MeanIntegralPitch2 = 0; |
965 | MeanIntegralPitch2 = 0; |
| 929 | MeanIntegralRoll2 = 0; |
966 | MeanIntegralRoll2 = 0; |
| 930 | ZaehlMessungen = 0; |
967 | ZaehlMessungen = 0; |
| 931 | } |
968 | } |
| 932 | //DebugOut.Analog[31] = StickRoll / (26*IntegralFactor); |
969 | //DebugOut.Analog[31] = StickRoll / (26*Gyro_I_Factor); |
| Line 933... | Line 970... | ||
| 933 | 970 | ||
| 934 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
971 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 935 | // Gieren |
972 | // Gieren |
| 936 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
973 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| Line 1023... | Line 1060... | ||
| 1023 | // Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen |
1060 | // Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen |
| 1024 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1061 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 1025 | //DebugOut.Analog[26] = ReadingPitch; |
1062 | //DebugOut.Analog[26] = ReadingPitch; |
| 1026 | //DebugOut.Analog[28] = ReadingRoll; |
1063 | //DebugOut.Analog[28] = ReadingRoll; |
| Line 1027... | Line 1064... | ||
| 1027 | 1064 | ||
| 1028 | if(Looping_Pitch) ReadingPitch = ReadingPitch * GyroFactor; |
1065 | if(Looping_Pitch) ReadingPitch = ReadingPitch * Gyro_P_Factor; |
| 1029 | else ReadingPitch = IntegralPitch * IntegralFactor + ReadingPitch * GyroFactor; |
1066 | else ReadingPitch = IntegralPitch * Gyro_I_Factor + ReadingPitch * Gyro_P_Factor; |
| 1030 | if(Looping_Roll) ReadingRoll = ReadingRoll * GyroFactor; |
1067 | if(Looping_Roll) ReadingRoll = ReadingRoll * Gyro_P_Factor; |
| 1031 | else ReadingRoll = IntegralRoll * IntegralFactor + ReadingRoll * GyroFactor; |
1068 | else ReadingRoll = IntegralRoll * Gyro_I_Factor + ReadingRoll * Gyro_P_Factor; |
| Line 1032... | Line 1069... | ||
| 1032 | ReadingYaw = ReadingYaw * (2 * GyroFactor) + IntegralYaw * IntegralFactor / 2; |
1069 | ReadingYaw = ReadingYaw * (2 * Gyro_P_Factor) + IntegralYaw * Gyro_I_Factor / 2; |
| 1033 | 1070 | ||
| 1034 | DebugOut.Analog[25] = IntegralRoll * IntegralFactor; |
1071 | DebugOut.Analog[25] = IntegralRoll * Gyro_I_Factor; |
| Line 1035... | Line 1072... | ||
| 1035 | DebugOut.Analog[31] = StickRoll;// / (26*IntegralFactor); |
1072 | DebugOut.Analog[31] = StickRoll;// / (26*Gyro_I_Factor); |
| 1036 | DebugOut.Analog[28] = ReadingRoll; |
1073 | DebugOut.Analog[28] = ReadingRoll; |
| 1037 | 1074 | ||
| Line 1109... | Line 1146... | ||
| 1109 | if(GasMixingFraction < 20) YawMixingFraction = 0; |
1146 | if(GasMixingFraction < 20) YawMixingFraction = 0; |
| 1110 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1147 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 1111 | // Pitch-Achse |
1148 | // Pitch-Achse |
| 1112 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1149 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 1113 | DiffPitch = ReadingPitch - (StickPitch - GPS_Pitch); // Differenz bestimmen |
1150 | DiffPitch = ReadingPitch - (StickPitch - GPS_Pitch); // Differenz bestimmen |
| 1114 | if(IntegralFactor) SumPitch += IntegralPitch * IntegralFactor - (StickPitch - GPS_Pitch); // I-Anteil bei Winkelregelung |
1151 | if(Gyro_I_Factor) SumPitch += IntegralPitch * Gyro_I_Factor - (StickPitch - GPS_Pitch); // I-Anteil bei Winkelregelung |
| 1115 | else SumPitch += DiffPitch; // I-Anteil bei HH |
1152 | else SumPitch += DiffPitch; // I-Anteil bei HH |
| 1116 | if(SumPitch > 16000) SumPitch = 16000; |
1153 | if(SumPitch > 16000) SumPitch = 16000; |
| 1117 | if(SumPitch < -16000) SumPitch = -16000; |
1154 | if(SumPitch < -16000) SumPitch = -16000; |
| 1118 | pd_ergebnis = DiffPitch + Ki * SumPitch; // PI-Regler für Pitch |
1155 | pd_ergebnis = DiffPitch + Ki * SumPitch; // PI-Regler für Pitch |
| 1119 | // Motor Vorn |
1156 | // Motor Vorn |
| Line 1134... | Line 1171... | ||
| 1134 | Motor_Rear = MotorValue; |
1171 | Motor_Rear = MotorValue; |
| 1135 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1172 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 1136 | // Roll-Achse |
1173 | // Roll-Achse |
| 1137 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1174 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 1138 | DiffRoll = ReadingRoll - (StickRoll - GPS_Roll); // Differenz bestimmen |
1175 | DiffRoll = ReadingRoll - (StickRoll - GPS_Roll); // Differenz bestimmen |
| 1139 | if(IntegralFactor) SumRoll += IntegralRoll * IntegralFactor - (StickRoll - GPS_Roll);// I-Anteil bei Winkelregelung |
1176 | if(Gyro_I_Factor) SumRoll += IntegralRoll * Gyro_I_Factor - (StickRoll - GPS_Roll);// I-Anteil bei Winkelregelung |
| 1140 | else SumRoll += DiffRoll; // I-Anteil bei HH |
1177 | else SumRoll += DiffRoll; // I-Anteil bei HH |
| 1141 | if(SumRoll > 16000) SumRoll = 16000; |
1178 | if(SumRoll > 16000) SumRoll = 16000; |
| 1142 | if(SumRoll < -16000) SumRoll = -16000; |
1179 | if(SumRoll < -16000) SumRoll = -16000; |
| 1143 | pd_ergebnis = DiffRoll + Ki * SumRoll; // PI-Regler für Roll |
1180 | pd_ergebnis = DiffRoll + Ki * SumRoll; // PI-Regler für Roll |
| 1144 | tmp_int = (long)((long)FCParam.DynamicStability * (long)(GasMixingFraction + abs(YawMixingFraction)/2)) / 64; |
1181 | tmp_int = (long)((long)FCParam.DynamicStability * (long)(GasMixingFraction + abs(YawMixingFraction)/2)) / 64; |