Rev 1413 | Rev 1423 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1413 | Rev 1420 | ||
---|---|---|---|
Line 152... | Line 152... | ||
152 | unsigned char Parameter_Servo3,Parameter_Servo4,Parameter_Servo5; |
152 | unsigned char Parameter_Servo3,Parameter_Servo4,Parameter_Servo5; |
153 | struct mk_param_struct EE_Parameter; |
153 | struct mk_param_struct EE_Parameter; |
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 MikroKopterFlags = 0; |
157 | volatile unsigned char FCFlags = 0; |
158 | long GIER_GRAD_FAKTOR = 1291; |
158 | long GIER_GRAD_FAKTOR = 1291; |
159 | signed int KopplungsteilNickRoll,KopplungsteilRollNick; |
159 | signed int KopplungsteilNickRoll,KopplungsteilRollNick; |
160 | unsigned char RequiredMotors = 4; |
160 | unsigned char RequiredMotors = 4; |
161 | unsigned char Motor[MAX_MOTORS]; |
161 | unsigned char Motor[MAX_MOTORS]; |
162 | signed int tmp_motorwert[MAX_MOTORS]; |
162 | signed int tmp_motorwert[MAX_MOTORS]; |
Line 259... | Line 259... | ||
259 | ExternHoehenValue = 0; |
259 | ExternHoehenValue = 0; |
260 | ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR; |
260 | ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR; |
261 | GierGyroFehler = 0; |
261 | GierGyroFehler = 0; |
262 | SendVersionToNavi = 1; |
262 | SendVersionToNavi = 1; |
263 | LED_Init(); |
263 | LED_Init(); |
264 | MikroKopterFlags |= FLAG_CALIBRATE; |
264 | FCFlags |= FCFLAG_CALIBRATE; |
265 | FromNaviCtrl_Value.Kalman_K = -1; |
265 | FromNaviCtrl_Value.Kalman_K = -1; |
266 | FromNaviCtrl_Value.Kalman_MaxDrift = 0; |
266 | FromNaviCtrl_Value.Kalman_MaxDrift = 0; |
267 | FromNaviCtrl_Value.Kalman_MaxFusion = 32; |
267 | FromNaviCtrl_Value.Kalman_MaxFusion = 32; |
Line 268... | Line 268... | ||
268 | 268 | ||
Line 475... | Line 475... | ||
475 | //############################################################################ |
475 | //############################################################################ |
476 | { |
476 | { |
477 | unsigned char i; |
477 | unsigned char i; |
478 | if(!MotorenEin) |
478 | if(!MotorenEin) |
479 | { |
479 | { |
480 | MikroKopterFlags &= ~(FLAG_MOTOR_RUN | FLAG_FLY); |
480 | FCFlags &= ~(FCFLAG_MOTOR_RUN | FCFLAG_FLY); |
481 | for(i=0;i<MAX_MOTORS;i++) |
481 | for(i=0;i<MAX_MOTORS;i++) |
482 | { |
482 | { |
483 | if(!PC_MotortestActive) MotorTest[i] = 0; |
483 | if(!PC_MotortestActive) MotorTest[i] = 0; |
484 | Motor[i] = MotorTest[i]; |
484 | Motor[i] = MotorTest[i]; |
485 | } |
485 | } |
486 | if(PC_MotortestActive) PC_MotortestActive--; |
486 | if(PC_MotortestActive) PC_MotortestActive--; |
487 | } |
487 | } |
488 | else MikroKopterFlags |= FLAG_MOTOR_RUN; |
488 | else FCFlags |= FCFLAG_MOTOR_RUN; |
Line 489... | Line 489... | ||
489 | 489 | ||
490 | DebugOut.Analog[12] = Motor[0]; |
490 | DebugOut.Analog[12] = Motor[0]; |
491 | DebugOut.Analog[13] = Motor[1]; |
491 | DebugOut.Analog[13] = Motor[1]; |
492 | DebugOut.Analog[14] = Motor[3]; |
492 | DebugOut.Analog[14] = Motor[3]; |
Line 583... | Line 583... | ||
583 | { |
583 | { |
584 | if(RcLostTimer) RcLostTimer--; |
584 | if(RcLostTimer) RcLostTimer--; |
585 | else |
585 | else |
586 | { |
586 | { |
587 | MotorenEin = 0; |
587 | MotorenEin = 0; |
588 | MikroKopterFlags &= ~FLAG_NOTLANDUNG; |
588 | FCFlags &= ~FCFLAG_NOTLANDUNG; |
589 | } |
589 | } |
590 | ROT_ON; |
590 | ROT_ON; |
591 | if(modell_fliegt > 1000) // wahrscheinlich in der Luft --> langsam absenken |
591 | if(modell_fliegt > 1000) // wahrscheinlich in der Luft --> langsam absenken |
592 | { |
592 | { |
593 | GasMischanteil = EE_Parameter.NotGas; |
593 | GasMischanteil = EE_Parameter.NotGas; |
594 | MikroKopterFlags |= FLAG_NOTLANDUNG; |
594 | FCFlags |= FCFLAG_NOTLANDUNG; |
595 | PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] = 0; |
595 | PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] = 0; |
596 | PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] = 0; |
596 | PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] = 0; |
597 | PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] = 0; |
597 | PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] = 0; |
598 | PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] = 0; |
598 | PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] = 0; |
599 | PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] = 0; |
599 | PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] = 0; |
Line 604... | Line 604... | ||
604 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
604 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
605 | // Emfang gut |
605 | // Emfang gut |
606 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
606 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
607 | if(SenderOkay > 140) |
607 | if(SenderOkay > 140) |
608 | { |
608 | { |
609 | MikroKopterFlags &= ~FLAG_NOTLANDUNG; |
609 | FCFlags &= ~FCFLAG_NOTLANDUNG; |
610 | RcLostTimer = EE_Parameter.NotGasZeit * 50; |
610 | RcLostTimer = EE_Parameter.NotGasZeit * 50; |
611 | if(GasMischanteil > 40 && MotorenEin) |
611 | if(GasMischanteil > 40 && MotorenEin) |
612 | { |
612 | { |
613 | if(modell_fliegt < 0xffff) modell_fliegt++; |
613 | if(modell_fliegt < 0xffff) modell_fliegt++; |
614 | } |
614 | } |
Line 621... | Line 621... | ||
621 | NeueKompassRichtungMerken = 1; |
621 | NeueKompassRichtungMerken = 1; |
622 | sollGier = 0; |
622 | sollGier = 0; |
623 | Mess_Integral_Gier = 0; |
623 | Mess_Integral_Gier = 0; |
624 | // Mess_Integral_Gier2 = 0; |
624 | // Mess_Integral_Gier2 = 0; |
625 | } |
625 | } |
626 | } else MikroKopterFlags |= FLAG_FLY; |
626 | } else FCFlags |= FCFLAG_FLY; |
Line 627... | Line 627... | ||
627 | 627 | ||
628 | if((PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] > 80) && MotorenEin == 0) |
628 | if((PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] > 80) && MotorenEin == 0) |
629 | { |
629 | { |
630 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
630 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
Line 715... | Line 715... | ||
715 | Mess_IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll; |
715 | Mess_IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll; |
716 | Mess_IntegralNick2 = IntegralNick; |
716 | Mess_IntegralNick2 = IntegralNick; |
717 | Mess_IntegralRoll2 = IntegralRoll; |
717 | Mess_IntegralRoll2 = IntegralRoll; |
718 | SummeNick = 0; |
718 | SummeNick = 0; |
719 | SummeRoll = 0; |
719 | SummeRoll = 0; |
720 | MikroKopterFlags |= FLAG_START; |
720 | FCFlags |= FCFLAG_START; |
721 | } |
721 | } |
722 | } |
722 | } |
723 | else delay_einschalten = 0; |
723 | else delay_einschalten = 0; |
724 | //Auf Neutralwerte setzen |
724 | //Auf Neutralwerte setzen |
725 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
725 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
Line 739... | Line 739... | ||
739 | } |
739 | } |
740 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
740 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
741 | // neue Werte von der Funke |
741 | // neue Werte von der Funke |
742 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
742 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
Line 743... | Line 743... | ||
743 | 743 | ||
744 | if(!NewPpmData-- || (MikroKopterFlags & FLAG_NOTLANDUNG)) |
744 | if(!NewPpmData-- || (FCFlags & FCFLAG_NOTLANDUNG)) |
745 | { |
745 | { |
746 | static int stick_nick,stick_roll; |
746 | static int stick_nick,stick_roll; |
747 | ParameterZuordnung(); |
747 | ParameterZuordnung(); |
748 | stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4; |
748 | stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4; |
Line 791... | Line 791... | ||
791 | { |
791 | { |
792 | MaxStickRoll = abs(StickRoll)/STICK_GAIN; |
792 | MaxStickRoll = abs(StickRoll)/STICK_GAIN; |
793 | if(MaxStickRoll > 100) MaxStickRoll = 100; |
793 | if(MaxStickRoll > 100) MaxStickRoll = 100; |
794 | } |
794 | } |
795 | else MaxStickRoll--; |
795 | else MaxStickRoll--; |
796 | if(MikroKopterFlags & FLAG_NOTLANDUNG) {MaxStickNick = 0; MaxStickRoll = 0;} |
796 | if(FCFlags & FCFLAG_NOTLANDUNG) {MaxStickNick = 0; MaxStickRoll = 0;} |
Line 797... | Line 797... | ||
797 | 797 | ||
798 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
798 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
799 | // Looping? |
799 | // Looping? |
800 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
800 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
Line 843... | Line 843... | ||
843 | 843 | ||
844 | 844 | ||
845 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
845 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
846 | // Bei Empfangsausfall im Flug |
846 | // Bei Empfangsausfall im Flug |
847 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
847 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
848 | if(MikroKopterFlags & FLAG_NOTLANDUNG) |
848 | if(FCFlags & FCFLAG_NOTLANDUNG) |
849 | { |
849 | { |
850 | StickGier = 0; |
850 | StickGier = 0; |
851 | StickNick = 0; |
851 | StickNick = 0; |
Line 1327... | Line 1327... | ||
1327 | tmp_int = (int)(IntegralNick/GIER_GRAD_FAKTOR); // nick angle in deg |
1327 | tmp_int = (int)(IntegralNick/GIER_GRAD_FAKTOR); // nick angle in deg |
1328 | tmp_int2 = (int)(IntegralRoll/GIER_GRAD_FAKTOR); // roll angle in deg |
1328 | tmp_int2 = (int)(IntegralRoll/GIER_GRAD_FAKTOR); // roll angle in deg |
1329 | CosAttitude = (int16_t)ihypot(tmp_int, tmp_int2); // phytagoras gives effective attitude angle in deg |
1329 | CosAttitude = (int16_t)ihypot(tmp_int, tmp_int2); // phytagoras gives effective attitude angle in deg |
1330 | LIMIT_MAX(CosAttitude, 60); // limit effective attitude angle |
1330 | LIMIT_MAX(CosAttitude, 60); // limit effective attitude angle |
1331 | CosAttitude = c_cos_8192(CosAttitude); // cos of actual attitude |
1331 | CosAttitude = c_cos_8192(CosAttitude); // cos of actual attitude |
1332 | if(HoehenReglerAktiv && !(MikroKopterFlags & FLAG_NOTLANDUNG)) |
1332 | if(HoehenReglerAktiv && !(FCFlags & FCFLAG_NOTLANDUNG)) |
1333 | { |
1333 | { |
1334 | #define HEIGHT_TRIM_UP 0x01 |
1334 | #define HEIGHT_TRIM_UP 0x01 |
1335 | #define HEIGHT_TRIM_DOWN 0x02 |
1335 | #define HEIGHT_TRIM_DOWN 0x02 |
1336 | static unsigned char HeightTrimmingFlag = 0x00; |
1336 | static unsigned char HeightTrimmingFlag = 0x00; |
Line 1350... | Line 1350... | ||
1350 | { |
1350 | { |
1351 | // alternative height control |
1351 | // alternative height control |
1352 | // PD-Control with respect to hoover point |
1352 | // PD-Control with respect to hoover point |
1353 | // the thrust loss out of horizontal attitude is compensated |
1353 | // the thrust loss out of horizontal attitude is compensated |
1354 | // the setpoint will be fine adjusted with the gas stick position |
1354 | // the setpoint will be fine adjusted with the gas stick position |
1355 | if(MikroKopterFlags & FLAG_FLY) // trim setpoint only when flying |
1355 | if(FCFlags & FCFLAG_FLY) // trim setpoint only when flying |
1356 | { // gas stick is above hoover point |
1356 | { // gas stick is above hoover point |
1357 | if(StickGas > (StickGasHoover + HEIGHT_CONTROL_STICKTHRESHOLD) && !BaroAtUpperLimit) |
1357 | if(StickGas > (StickGasHoover + HEIGHT_CONTROL_STICKTHRESHOLD) && !BaroAtUpperLimit) |
1358 | { |
1358 | { |
1359 | if(HeightTrimmingFlag & HEIGHT_TRIM_DOWN) |
1359 | if(HeightTrimmingFlag & HEIGHT_TRIM_DOWN) |
1360 | { |
1360 | { |
Line 1398... | Line 1398... | ||
1398 | if(StickGasHoover < 70) StickGasHoover = 70; |
1398 | if(StickGasHoover < 70) StickGasHoover = 70; |
1399 | else if(StickGasHoover > 150) StickGasHoover = 150; |
1399 | else if(StickGasHoover > 150) StickGasHoover = 150; |
1400 | } |
1400 | } |
1401 | } |
1401 | } |
1402 | if(BaroExpandActive) SollHoehe = HoehenWert; // update setpoint to current altitude if Expanding is active |
1402 | if(BaroExpandActive) SollHoehe = HoehenWert; // update setpoint to current altitude if Expanding is active |
1403 | } //if MikroKopterFlags & MKFLAG_FLY |
1403 | } //if FCFlags & MKFCFLAG_FLY |
1404 | else |
1404 | else |
1405 | { |
1405 | { |
1406 | SollHoehe = HoehenWert - 400; |
1406 | SollHoehe = HoehenWert - 400; |
1407 | if(EE_Parameter.Hoehe_StickNeutralPoint) StickGasHoover = EE_Parameter.Hoehe_StickNeutralPoint; |
1407 | if(EE_Parameter.Hoehe_StickNeutralPoint) StickGasHoover = EE_Parameter.Hoehe_StickNeutralPoint; |
1408 | else StickGasHoover = 120; |
1408 | else StickGasHoover = 120; |
Line 1473... | Line 1473... | ||
1473 | FilterHCGas = GasMischanteil; |
1473 | FilterHCGas = GasMischanteil; |
1474 | } |
1474 | } |
Line 1475... | Line 1475... | ||
1475 | 1475 | ||
1476 | // Hoover gas estimation by averaging gas control output on small z-velocities |
1476 | // Hoover gas estimation by averaging gas control output on small z-velocities |
1477 | // this is done only if height contol option is selected in global config and aircraft is flying |
1477 | // this is done only if height contol option is selected in global config and aircraft is flying |
1478 | if((MikroKopterFlags & FLAG_FLY) && !(MikroKopterFlags & FLAG_NOTLANDUNG)) |
1478 | if((FCFlags & FCFLAG_FLY) && !(FCFlags & FCFLAG_NOTLANDUNG)) |
1479 | { |
1479 | { |
1480 | if(HooverGasFilter == 0) HooverGasFilter = HOOVER_GAS_AVERAGE * (unsigned long)(GasMischanteil); // init estimation |
1480 | if(HooverGasFilter == 0) HooverGasFilter = HOOVER_GAS_AVERAGE * (unsigned long)(GasMischanteil); // init estimation |
1481 | if(abs(VarioMeter) < 100) // only on small vertical speed |
1481 | if(abs(VarioMeter) < 100) // only on small vertical speed |
1482 | { |
1482 | { |