Rev 703 | Rev 707 | 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; |