Rev 1760 | Rev 1767 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1760 | Rev 1765 | ||
---|---|---|---|
Line 152... | Line 152... | ||
152 | const signed char sintab[31] = { 0, 2, 4, 6, 7, 8, 8, 8, 7, 6, 4, 2, 0, -2, -4, -6, -7, -8, -8, -8, -7, -6, -4, -2, 0, 2, 4, 6, 7, 8, 8}; |
152 | const signed char sintab[31] = { 0, 2, 4, 6, 7, 8, 8, 8, 7, 6, 4, 2, 0, -2, -4, -6, -7, -8, -8, -8, -7, -6, -4, -2, 0, 2, 4, 6, 7, 8, 8}; |
Line 153... | Line 153... | ||
153 | 153 | ||
154 | signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20; |
154 | signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20; |
155 | int MaxStickNick = 0,MaxStickRoll = 0; |
155 | int MaxStickNick = 0,MaxStickRoll = 0; |
156 | unsigned int modell_fliegt = 0; |
156 | unsigned int modell_fliegt = 0; |
157 | volatile unsigned char FCFlags = 0; |
157 | volatile unsigned char FC_StatusFlags = 0; |
158 | long GIER_GRAD_FAKTOR = 1291; |
158 | long GIER_GRAD_FAKTOR = 1291; |
159 | signed int KopplungsteilNickRoll,KopplungsteilRollNick; |
159 | signed int KopplungsteilNickRoll,KopplungsteilRollNick; |
160 | signed int tmp_motorwert[MAX_MOTORS]; |
160 | signed int tmp_motorwert[MAX_MOTORS]; |
Line 334... | Line 334... | ||
334 | ExternHoehenValue = 0; |
334 | ExternHoehenValue = 0; |
335 | ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR; |
335 | ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR; |
336 | GierGyroFehler = 0; |
336 | GierGyroFehler = 0; |
337 | SendVersionToNavi = 1; |
337 | SendVersionToNavi = 1; |
338 | LED_Init(); |
338 | LED_Init(); |
339 | FCFlags |= FCFLAG_CALIBRATE; |
339 | FC_StatusFlags |= FC_STATUS_CALIBRATE; |
340 | FromNaviCtrl_Value.Kalman_K = -1; |
340 | FromNaviCtrl_Value.Kalman_K = -1; |
341 | FromNaviCtrl_Value.Kalman_MaxDrift = 0; |
341 | FromNaviCtrl_Value.Kalman_MaxDrift = 0; |
342 | FromNaviCtrl_Value.Kalman_MaxFusion = 32; |
342 | FromNaviCtrl_Value.Kalman_MaxFusion = 32; |
Line 343... | Line 343... | ||
343 | 343 | ||
Line 350... | Line 350... | ||
350 | { |
350 | { |
351 | HEF4017R_ON; |
351 | HEF4017R_ON; |
352 | DDRD |=0x80; // enable J7 -> Servo signal |
352 | DDRD |=0x80; // enable J7 -> Servo signal |
353 | } |
353 | } |
Line 354... | Line 354... | ||
354 | 354 | ||
355 | if((AdNeutralNick < 150 * 16) || (AdNeutralNick > 850 * 16)) { VersionInfo.HardwareError[0] |= DEFEKT_G_NICK; }; |
355 | if((AdNeutralNick < 150 * 16) || (AdNeutralNick > 850 * 16)) { VersionInfo.HardwareError[0] |= FC_ERROR0_GYRO_NICK; }; |
356 | if((AdNeutralRoll < 150 * 16) || (AdNeutralRoll > 850 * 16)) { VersionInfo.HardwareError[0] |= DEFEKT_G_ROLL; }; |
356 | if((AdNeutralRoll < 150 * 16) || (AdNeutralRoll > 850 * 16)) { VersionInfo.HardwareError[0] |= FC_ERROR0_GYRO_ROLL; }; |
357 | if((AdNeutralGier < 150 * 2) || (AdNeutralGier > 850 * 2)) { VersionInfo.HardwareError[0] |= DEFEKT_G_GIER; }; |
357 | if((AdNeutralGier < 150 * 2) || (AdNeutralGier > 850 * 2)) { VersionInfo.HardwareError[0] |= FC_ERROR0_GYRO_YAW; }; |
358 | if((NeutralAccX < 300) || (NeutralAccX > 750)) { VersionInfo.HardwareError[0] |= DEFEKT_A_NICK; }; |
358 | if((NeutralAccX < 300) || (NeutralAccX > 750)) { VersionInfo.HardwareError[0] |= FC_ERROR0_ACC_NICK; }; |
359 | if((NeutralAccY < 300) || (NeutralAccY > 750)) { VersionInfo.HardwareError[0] |= DEFEKT_A_ROLL; }; |
359 | if((NeutralAccY < 300) || (NeutralAccY > 750)) { VersionInfo.HardwareError[0] |= FC_ERROR0_ACC_ROLL; }; |
360 | if((NeutralAccZ < 512) || (NeutralAccZ > 850)) { VersionInfo.HardwareError[0] |= DEFEKT_A_Z; }; |
360 | if((NeutralAccZ < 512) || (NeutralAccZ > 850)) { VersionInfo.HardwareError[0] |= FC_ERROR0_ACC_TOP; }; |
Line 361... | Line 361... | ||
361 | } |
361 | } |
362 | 362 | ||
Line 526... | Line 526... | ||
526 | //############################################################################ |
526 | //############################################################################ |
527 | { |
527 | { |
528 | unsigned char i; |
528 | unsigned char i; |
529 | if(!MotorenEin) |
529 | if(!MotorenEin) |
530 | { |
530 | { |
531 | FCFlags &= ~(FCFLAG_MOTOR_RUN | FCFLAG_FLY); |
531 | FC_StatusFlags &= ~(FC_STATUS_MOTOR_RUN | FC_STATUS_FLY); |
532 | for(i=0;i<MAX_MOTORS;i++) |
532 | for(i=0;i<MAX_MOTORS;i++) |
533 | { |
533 | { |
534 | if(!PC_MotortestActive) MotorTest[i] = 0; |
534 | if(!PC_MotortestActive) MotorTest[i] = 0; |
535 | Motor[i].SetPoint = MotorTest[i]; |
535 | Motor[i].SetPoint = MotorTest[i]; |
536 | Motor[i].SetPointLowerBits = 0; |
536 | Motor[i].SetPointLowerBits = 0; |
Line 539... | Line 539... | ||
539 | Motor[i].SetPointLowerBits = MotorTest[i] % 4; |
539 | Motor[i].SetPointLowerBits = MotorTest[i] % 4; |
540 | */ |
540 | */ |
541 | } |
541 | } |
542 | if(PC_MotortestActive) PC_MotortestActive--; |
542 | if(PC_MotortestActive) PC_MotortestActive--; |
543 | } |
543 | } |
544 | else FCFlags |= FCFLAG_MOTOR_RUN; |
544 | else FC_StatusFlags |= FC_STATUS_MOTOR_RUN; |
Line 545... | Line 545... | ||
545 | 545 | ||
546 | if(I2C_TransferActive) |
546 | if(I2C_TransferActive) |
547 | { |
547 | { |
548 | I2C_TransferActive = 0; // enable for the next time |
548 | I2C_TransferActive = 0; // enable for the next time |
549 | } |
549 | } |
550 | else |
550 | else |
551 | { |
551 | { |
552 | motor_write = 0; |
552 | motor_write = 0; |
553 | I2C_Start(TWI_STATE_MOTOR_TX); //Start I2C Interrupt Mode |
553 | I2C_Start(TWI_STATE_MOTOR_TX); //Start I2C Interrupt Mode |
554 | } |
554 | } |
Line 555... | Line 555... | ||
555 | } |
555 | } |
Line 610... | Line 610... | ||
610 | #ifdef SWITCH_LEARNS_CAREFREE |
610 | #ifdef SWITCH_LEARNS_CAREFREE |
611 | if(!CareFree) ControlHeading = (((int) EE_Parameter.OrientationAngle * 15 + KompassValue) % 360) / 2; |
611 | if(!CareFree) ControlHeading = (((int) EE_Parameter.OrientationAngle * 15 + KompassValue) % 360) / 2; |
612 | #endif |
612 | #endif |
613 | CareFree = 1; |
613 | CareFree = 1; |
614 | if(tmp >= 248 && Poti[255 - tmp] < 50) CareFree = 0; |
614 | if(tmp >= 248 && Poti[255 - tmp] < 50) CareFree = 0; |
615 | if(FromNaviCtrl.CompassValue < 0 && CareFree) VersionInfo.HardwareError[0] |= DEFEKT_CAREFREE_ERR; else VersionInfo.HardwareError[0] &= ~DEFEKT_CAREFREE_ERR; |
615 | if(FromNaviCtrl.CompassValue < 0 && CareFree) VersionInfo.HardwareError[0] |= FC_ERROR0_CAREFREE; else VersionInfo.HardwareError[0] &= ~FC_ERROR0_CAREFREE; |
616 | } |
616 | } |
617 | else CareFree = 0; |
617 | else CareFree = 0; |
Line 618... | Line 618... | ||
618 | 618 | ||
619 | if(FromNaviCtrl.CompassValue < 0 && MotorenEin && CareFree && BeepMuster == 0xffff) // ungültiger Kompasswert |
619 | if(FromNaviCtrl.CompassValue < 0 && MotorenEin && CareFree && BeepMuster == 0xffff) // ungültiger Kompasswert |
620 | { |
620 | { |
621 | beeptime = 15000; |
621 | beeptime = 15000; |
622 | BeepMuster = 0xA400; |
622 | BeepMuster = 0xA400; |
623 | CareFree = 0; |
623 | CareFree = 0; |
Line 624... | Line 624... | ||
624 | } |
624 | } |
625 | 625 | ||
Line 626... | Line 626... | ||
626 | if(CareFree) {if(Parameter_AchsKopplung1 < 210) Parameter_AchsKopplung1 += 30;} |
626 | if(CareFree) {if(Parameter_AchsKopplung1 < 210) Parameter_AchsKopplung1 += 30;} |
Line 659... | Line 659... | ||
659 | { |
659 | { |
660 | if(RcLostTimer) RcLostTimer--; |
660 | if(RcLostTimer) RcLostTimer--; |
661 | else |
661 | else |
662 | { |
662 | { |
663 | MotorenEin = 0; |
663 | MotorenEin = 0; |
664 | FCFlags &= ~FCFLAG_NOTLANDUNG; |
664 | FC_StatusFlags &= ~FC_STATUS_EMERGENCY_LANDING; |
665 | } |
665 | } |
666 | ROT_ON; |
666 | ROT_ON; |
667 | if(modell_fliegt > 1000) // wahrscheinlich in der Luft --> langsam absenken |
667 | if(modell_fliegt > 1000) // wahrscheinlich in der Luft --> langsam absenken |
668 | { |
668 | { |
669 | GasMischanteil = EE_Parameter.NotGas; |
669 | GasMischanteil = EE_Parameter.NotGas; |
670 | FCFlags |= FCFLAG_NOTLANDUNG; |
670 | FC_StatusFlags |= FC_STATUS_EMERGENCY_LANDING; |
671 | PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] = 0; |
671 | PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] = 0; |
672 | PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] = 0; |
672 | PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] = 0; |
673 | PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] = 0; |
673 | PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] = 0; |
674 | PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] = 0; |
674 | PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] = 0; |
675 | PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] = 0; |
675 | PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] = 0; |
Line 680... | Line 680... | ||
680 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
680 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
681 | // Emfang gut |
681 | // Emfang gut |
682 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
682 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
683 | if(SenderOkay > 140) |
683 | if(SenderOkay > 140) |
684 | { |
684 | { |
685 | FCFlags &= ~FCFLAG_NOTLANDUNG; |
685 | FC_StatusFlags &= ~FC_STATUS_EMERGENCY_LANDING; |
686 | RcLostTimer = EE_Parameter.NotGasZeit * 50; |
686 | RcLostTimer = EE_Parameter.NotGasZeit * 50; |
687 | if(GasMischanteil > 40 && MotorenEin) |
687 | if(GasMischanteil > 40 && MotorenEin) |
688 | { |
688 | { |
689 | if(modell_fliegt < 0xffff) modell_fliegt++; |
689 | if(modell_fliegt < 0xffff) modell_fliegt++; |
690 | } |
690 | } |
Line 696... | Line 696... | ||
696 | Mess_Integral_Gier = 0; |
696 | Mess_Integral_Gier = 0; |
697 | if(modell_fliegt == 250) |
697 | if(modell_fliegt == 250) |
698 | { |
698 | { |
699 | NeueKompassRichtungMerken = 1; |
699 | NeueKompassRichtungMerken = 1; |
700 | } |
700 | } |
701 | } else FCFlags |= FCFLAG_FLY; |
701 | } else FC_StatusFlags |= FC_STATUS_FLY; |
Line 702... | Line 702... | ||
702 | 702 | ||
703 | if((PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] > 80) && MotorenEin == 0) |
703 | if((PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] > 80) && MotorenEin == 0) |
704 | { |
704 | { |
705 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
705 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
Line 737... | Line 737... | ||
737 | { |
737 | { |
738 | if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset(); |
738 | if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset(); |
739 | } |
739 | } |
740 | ServoActive = 0; |
740 | ServoActive = 0; |
741 | SetNeutral(0); |
741 | SetNeutral(0); |
742 | calibration_done = 1; |
742 | calibration_done = 1; |
743 | ServoActive = 1; |
743 | ServoActive = 1; |
744 | DDRD |=0x80; // enable J7 -> Servo signal |
744 | DDRD |=0x80; // enable J7 -> Servo signal |
745 | Piep(GetActiveParamSet(),120); |
745 | Piep(GetActiveParamSet(),120); |
746 | } |
746 | } |
747 | } |
747 | } |
Line 791... | Line 791... | ||
791 | Mess_IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll; |
791 | Mess_IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll; |
792 | Mess_IntegralNick2 = IntegralNick; |
792 | Mess_IntegralNick2 = IntegralNick; |
793 | Mess_IntegralRoll2 = IntegralRoll; |
793 | Mess_IntegralRoll2 = IntegralRoll; |
794 | SummeNick = 0; |
794 | SummeNick = 0; |
795 | SummeRoll = 0; |
795 | SummeRoll = 0; |
796 | FCFlags |= FCFLAG_START; |
796 | FC_StatusFlags |= FC_STATUS_START; |
797 | ControlHeading = (((int) EE_Parameter.OrientationAngle * 15 + KompassValue) % 360) / 2; |
797 | ControlHeading = (((int) EE_Parameter.OrientationAngle * 15 + KompassValue) % 360) / 2; |
798 | } |
798 | } |
799 | else |
799 | else |
800 | { |
800 | { |
801 | beeptime = 1500; // indicate missing calibration |
801 | beeptime = 1500; // indicate missing calibration |
Line 824... | Line 824... | ||
824 | } |
824 | } |
825 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
825 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
826 | // neue Werte von der Funke |
826 | // neue Werte von der Funke |
827 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
827 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
Line 828... | Line 828... | ||
828 | 828 | ||
829 | if(!NewPpmData-- || (FCFlags & FCFLAG_NOTLANDUNG)) |
829 | if(!NewPpmData-- || (FC_StatusFlags & FC_STATUS_EMERGENCY_LANDING)) |
830 | { |
830 | { |
831 | static int stick_nick,stick_roll; |
831 | static int stick_nick,stick_roll; |
832 | ParameterZuordnung(); |
832 | ParameterZuordnung(); |
833 | stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4; |
833 | stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4; |
Line 892... | Line 892... | ||
892 | { |
892 | { |
893 | MaxStickRoll = abs(StickRoll)/STICK_GAIN; |
893 | MaxStickRoll = abs(StickRoll)/STICK_GAIN; |
894 | if(MaxStickRoll > 100) MaxStickRoll = 100; |
894 | if(MaxStickRoll > 100) MaxStickRoll = 100; |
895 | } |
895 | } |
896 | else MaxStickRoll--; |
896 | else MaxStickRoll--; |
897 | if(FCFlags & FCFLAG_NOTLANDUNG) {MaxStickNick = 0; MaxStickRoll = 0;} |
897 | if(FC_StatusFlags & FC_STATUS_EMERGENCY_LANDING) {MaxStickNick = 0; MaxStickRoll = 0;} |
Line 898... | Line 898... | ||
898 | 898 | ||
899 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
899 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
900 | // Looping? |
900 | // Looping? |
901 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
901 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
Line 944... | Line 944... | ||
944 | 944 | ||
945 | 945 | ||
946 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
946 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
947 | // Bei Empfangsausfall im Flug |
947 | // Bei Empfangsausfall im Flug |
948 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
948 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
949 | if(FCFlags & FCFLAG_NOTLANDUNG) |
949 | if(FC_StatusFlags & FC_STATUS_EMERGENCY_LANDING) |
950 | { |
950 | { |
951 | StickGier = 0; |
951 | StickGier = 0; |
952 | StickNick = 0; |
952 | StickNick = 0; |
Line 1381... | Line 1381... | ||
1381 | tmp_int2 = (int)(IntegralRoll/GIER_GRAD_FAKTOR); // roll angle in deg |
1381 | tmp_int2 = (int)(IntegralRoll/GIER_GRAD_FAKTOR); // roll angle in deg |
1382 | CosAttitude = (int16_t)ihypot(tmp_int, tmp_int2); // phytagoras gives effective attitude angle in deg |
1382 | CosAttitude = (int16_t)ihypot(tmp_int, tmp_int2); // phytagoras gives effective attitude angle in deg |
1383 | LIMIT_MAX(CosAttitude, 60); // limit effective attitude angle |
1383 | LIMIT_MAX(CosAttitude, 60); // limit effective attitude angle |
1384 | CosAttitude = c_cos_8192(CosAttitude); // cos of actual attitude |
1384 | CosAttitude = c_cos_8192(CosAttitude); // cos of actual attitude |
1385 | VarioCharacter = ' '; |
1385 | VarioCharacter = ' '; |
1386 | if(HoehenReglerAktiv && !(FCFlags & FCFLAG_NOTLANDUNG)) |
1386 | if(HoehenReglerAktiv && !(FC_StatusFlags & FC_STATUS_EMERGENCY_LANDING)) |
1387 | { |
1387 | { |
1388 | #define HEIGHT_TRIM_UP 0x01 |
1388 | #define HEIGHT_TRIM_UP 0x01 |
1389 | #define HEIGHT_TRIM_DOWN 0x02 |
1389 | #define HEIGHT_TRIM_DOWN 0x02 |
1390 | static unsigned char HeightTrimmingFlag = 0x00; |
1390 | static unsigned char HeightTrimmingFlag = 0x00; |
Line 1404... | Line 1404... | ||
1404 | { |
1404 | { |
1405 | // alternative height control |
1405 | // alternative height control |
1406 | // PD-Control with respect to hoover point |
1406 | // PD-Control with respect to hoover point |
1407 | // the thrust loss out of horizontal attitude is compensated |
1407 | // the thrust loss out of horizontal attitude is compensated |
1408 | // the setpoint will be fine adjusted with the gas stick position |
1408 | // the setpoint will be fine adjusted with the gas stick position |
1409 | if(FCFlags & FCFLAG_FLY) // trim setpoint only when flying |
1409 | if(FC_StatusFlags & FC_STATUS_FLY) // trim setpoint only when flying |
1410 | { // gas stick is above hoover point |
1410 | { // gas stick is above hoover point |
1411 | if(StickGas > (StickGasHover + HEIGHT_CONTROL_STICKTHRESHOLD) && !BaroAtUpperLimit) |
1411 | if(StickGas > (StickGasHover + HEIGHT_CONTROL_STICKTHRESHOLD) && !BaroAtUpperLimit) |
1412 | { |
1412 | { |
1413 | if(HeightTrimmingFlag & HEIGHT_TRIM_DOWN) |
1413 | if(HeightTrimmingFlag & HEIGHT_TRIM_DOWN) |
1414 | { |
1414 | { |
Line 1439... | Line 1439... | ||
1439 | SollHoehe = HoehenWert; // update setpoint to current height |
1439 | SollHoehe = HoehenWert; // update setpoint to current height |
1440 | if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 500; |
1440 | if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 500; |
1441 | if(!StartTrigger && HoehenWert > 50) |
1441 | if(!StartTrigger && HoehenWert > 50) |
1442 | { |
1442 | { |
1443 | StartTrigger = 1; |
1443 | StartTrigger = 1; |
1444 | } |
1444 | } |
1445 | } |
1445 | } |
1446 | VarioCharacter = '='; |
1446 | VarioCharacter = '='; |
1447 | } |
1447 | } |
1448 | // Trim height set point |
1448 | // Trim height set point |
1449 | if(abs(HeightTrimming) > 512) |
1449 | if(abs(HeightTrimming) > 512) |
Line 1489... | Line 1489... | ||
1489 | tmp_long = (tmp_long * (long)Parameter_Hoehe_P) / 32L; // p-part |
1489 | tmp_long = (tmp_long * (long)Parameter_Hoehe_P) / 32L; // p-part |
1490 | LIMIT_MIN_MAX(tmp_long, -127 * STICK_GAIN, 256 * STICK_GAIN); // more than the full range makes no sense |
1490 | LIMIT_MIN_MAX(tmp_long, -127 * STICK_GAIN, 256 * STICK_GAIN); // more than the full range makes no sense |
1491 | GasReduction = tmp_long; |
1491 | GasReduction = tmp_long; |
1492 | // ------------------------- D-Part 1: Vario Meter ---------------------------- |
1492 | // ------------------------- D-Part 1: Vario Meter ---------------------------- |
1493 | tmp_int = VarioMeter / 8; |
1493 | tmp_int = VarioMeter / 8; |
1494 | LIMIT_MIN_MAX(tmp_int, -127, 128); |
1494 | LIMIT_MIN_MAX(tmp_int, -127, 128); |
1495 | tmp_int = (tmp_int * (long)Parameter_Luftdruck_D) / 4L; // scale to d-gain parameter |
1495 | tmp_int = (tmp_int * (long)Parameter_Luftdruck_D) / 4L; // scale to d-gain parameter |
1496 | LIMIT_MIN_MAX(tmp_int,-64 * STICK_GAIN, 64 * STICK_GAIN); |
1496 | LIMIT_MIN_MAX(tmp_int,-64 * STICK_GAIN, 64 * STICK_GAIN); |
1497 | if(HeightTrimmingFlag) tmp_int /= 4; // reduce d-part while trimming setpoint |
1497 | if(HeightTrimmingFlag) tmp_int /= 4; // reduce d-part while trimming setpoint |
1498 | else |
1498 | else |
1499 | if(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT) tmp_int /= 8; // reduce d-part in "Deckel" mode |
1499 | if(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT) tmp_int /= 8; // reduce d-part in "Deckel" mode |
1500 | GasReduction += tmp_int; |
1500 | GasReduction += tmp_int; |
1501 | } // EOF no baro range expanding |
1501 | } // EOF no baro range expanding |
1502 | // ------------------------ D-Part 2: ACC-Z Integral ------------------------ |
1502 | // ------------------------ D-Part 2: ACC-Z Integral ------------------------ |
1503 | if(Parameter_Hoehe_ACC_Wirkung) |
1503 | if(Parameter_Hoehe_ACC_Wirkung) |
1504 | { |
1504 | { |
1505 | tmp_long = ((Mess_Integral_Hoch / 128L) * (int32_t) Parameter_Hoehe_ACC_Wirkung) / (128L / STICK_GAIN); |
1505 | tmp_long = ((Mess_Integral_Hoch / 128L) * (int32_t) Parameter_Hoehe_ACC_Wirkung) / (128L / STICK_GAIN); |
1506 | LIMIT_MIN_MAX(tmp_long, -32 * STICK_GAIN, 64 * STICK_GAIN); |
1506 | LIMIT_MIN_MAX(tmp_long, -32 * STICK_GAIN, 64 * STICK_GAIN); |
1507 | GasReduction += tmp_long; |
1507 | GasReduction += tmp_long; |
1508 | } |
1508 | } |
1509 | // ------------------------ D-Part 3: GpsZ ---------------------------------- |
1509 | // ------------------------ D-Part 3: GpsZ ---------------------------------- |
1510 | tmp_int = (Parameter_Hoehe_GPS_Z * (int)FromNaviCtrl_Value.GpsZ)/128L; |
1510 | tmp_int = (Parameter_Hoehe_GPS_Z * (int)FromNaviCtrl_Value.GpsZ)/128L; |
1511 | LIMIT_MIN_MAX(tmp_int, -32 * STICK_GAIN, 64 * STICK_GAIN); |
1511 | LIMIT_MIN_MAX(tmp_int, -32 * STICK_GAIN, 64 * STICK_GAIN); |
1512 | GasReduction += tmp_int; |
1512 | GasReduction += tmp_int; |
1513 | GasReduction = (long)((long)GasReduction * HoverGas) / 512; // scale to the gas value |
1513 | GasReduction = (long)((long)GasReduction * HoverGas) / 512; // scale to the gas value |
1514 | // ------------------------ ---------------------------------- |
1514 | // ------------------------ ---------------------------------- |
1515 | HCGas -= GasReduction; |
1515 | HCGas -= GasReduction; |
1516 | // limit deviation from hoover point within the target region |
1516 | // limit deviation from hoover point within the target region |
1517 | if(!HeightTrimming && HoverGas > 0) // height setpoint is not changed and hoover gas not zero |
1517 | if(!HeightTrimming && HoverGas > 0) // height setpoint is not changed and hoover gas not zero |
1518 | { |
1518 | { |
1519 | unsigned int tmp; |
1519 | unsigned int tmp; |
1520 | tmp = abs(HeightDeviation); |
1520 | tmp = abs(HeightDeviation); |
1521 | if(tmp <= 60) |
1521 | if(tmp <= 60) |
1522 | { |
1522 | { |
1523 | LIMIT_MIN_MAX(HCGas, HoverGasMin, HoverGasMax); // limit gas around the hoover point |
1523 | LIMIT_MIN_MAX(HCGas, HoverGasMin, HoverGasMax); // limit gas around the hoover point |
1524 | } |
1524 | } |
1525 | else |
1525 | else |
1526 | { |
1526 | { |
1527 | tmp = (tmp - 60) / 32; |
1527 | tmp = (tmp - 60) / 32; |
1528 | if(tmp > 15) tmp = 15; |
1528 | if(tmp > 15) tmp = 15; |
1529 | if(HeightDeviation > 0) |
1529 | if(HeightDeviation > 0) |
1530 | { |
1530 | { |
1531 | tmp = (HoverGasMin * (16 - tmp)) / 16; |
1531 | tmp = (HoverGasMin * (16 - tmp)) / 16; |
Line 1551... | Line 1551... | ||
1551 | // set GasMischanteil to HeightControlGasFilter |
1551 | // set GasMischanteil to HeightControlGasFilter |
1552 | if(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT) |
1552 | if(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT) |
1553 | { // old version |
1553 | { // old version |
1554 | LIMIT_MAX(FilterHCGas, GasMischanteil); // nicht mehr als Gas |
1554 | LIMIT_MAX(FilterHCGas, GasMischanteil); // nicht mehr als Gas |
1555 | GasMischanteil = FilterHCGas; |
1555 | GasMischanteil = FilterHCGas; |
1556 | } |
1556 | } |
1557 | else GasMischanteil = FilterHCGas + (GasMischanteil - HoverGas) / 4; // only in Vario-Mode |
1557 | else GasMischanteil = FilterHCGas + (GasMischanteil - HoverGas) / 4; // only in Vario-Mode |
1558 | } |
1558 | } |
1559 | }// EOF height control active |
1559 | }// EOF height control active |
1560 | else // HC not active |
1560 | else // HC not active |
1561 | { |
1561 | { |
Line 1570... | Line 1570... | ||
1570 | FilterHCGas = GasMischanteil; |
1570 | FilterHCGas = GasMischanteil; |
1571 | } |
1571 | } |
Line 1572... | Line 1572... | ||
1572 | 1572 | ||
1573 | // Hover gas estimation by averaging gas control output on small z-velocities |
1573 | // Hover gas estimation by averaging gas control output on small z-velocities |
1574 | // this is done only if height contol option is selected in global config and aircraft is flying |
1574 | // this is done only if height contol option is selected in global config and aircraft is flying |
1575 | if((FCFlags & FCFLAG_FLY))// && !(FCFlags & FCFLAG_NOTLANDUNG)) |
1575 | if((FC_StatusFlags & FC_STATUS_FLY))// && !(FC_SatusFlags & FC_STATUS_EMERGENCY_LANDING)) |
1576 | { |
1576 | { |
1577 | if(HoverGasFilter == 0 || StartTrigger == 1) HoverGasFilter = HOVER_GAS_AVERAGE * (unsigned long)(GasMischanteil); // init estimation |
1577 | if(HoverGasFilter == 0 || StartTrigger == 1) HoverGasFilter = HOVER_GAS_AVERAGE * (unsigned long)(GasMischanteil); // init estimation |
1578 | if(StartTrigger == 1) StartTrigger = 2; |
1578 | if(StartTrigger == 1) StartTrigger = 2; |
1579 | tmp_long2 = (int32_t)GasMischanteil; // take current thrust |
1579 | tmp_long2 = (int32_t)GasMischanteil; // take current thrust |
Line 1607... | Line 1607... | ||
1607 | else |
1607 | else |
1608 | { // no limit |
1608 | { // no limit |
1609 | HoverGasMin = 0; |
1609 | HoverGasMin = 0; |
1610 | HoverGasMax = 1023; |
1610 | HoverGasMax = 1023; |
1611 | } |
1611 | } |
1612 | } |
1612 | } |
1613 | else |
1613 | else |
1614 | { |
1614 | { |
1615 | StartTrigger = 0; |
1615 | StartTrigger = 0; |
1616 | HoverGasFilter = 0; |
1616 | HoverGasFilter = 0; |
1617 | HoverGas = 0; |
1617 | HoverGas = 0; |
1618 | } |
1618 | } |
1619 | }// EOF ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL |
1619 | }// EOF ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL |
1620 | // limit gas to parameter setting |
1620 | // limit gas to parameter setting |
1621 | LIMIT_MIN(GasMischanteil, (MIN_GAS + 10) * STICK_GAIN); |
1621 | LIMIT_MIN(GasMischanteil, (MIN_GAS + 10) * STICK_GAIN); |
1622 | if(GasMischanteil > (MAX_GAS - 20) * STICK_GAIN) GasMischanteil = (MAX_GAS - 20) * STICK_GAIN; |
1622 | if(GasMischanteil > (MAX_GAS - 20) * STICK_GAIN) GasMischanteil = (MAX_GAS - 20) * STICK_GAIN; |