void LipoDetection(unsigned char print) { #define MAX_CELL_VOLTAGE 43 // max cell volatage for LiPO unsigned int timer, cells; if(print) printf("\n\rBatt:"); if(EE_Parameter.UnterspannungsWarnung < 50) // automatische Zellenerkennung { timer = SetDelay(500); if(print) while (!CheckDelay(timer)); // up to 6s LiPo, less than 2s is technical impossible for(cells = 2; cells < 7; cells++) { if(UBat < cells * MAX_CELL_VOLTAGE) break; } BattLowVoltageWarning = cells * EE_Parameter.UnterspannungsWarnung; if(print) { Piep(cells, 200); printf(" %d Cells ", cells); } } else BattLowVoltageWarning = EE_Parameter.UnterspannungsWarnung; if(print) printf(" Low warning level: %d.%d",BattLowVoltageWarning/10,BattLowVoltageWarning%10); }
void CalMk3Mag(void) { static unsigned char stick = 1; if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > -20) stick = 0; if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -70) && !stick) { stick = 1; WinkelOut.CalcState++; if(WinkelOut.CalcState > 4) { // WinkelOut.CalcState = 0; // in Uart.c beeptime = 1000; } else Piep(WinkelOut.CalcState,150); } DebugOut.Analog[19] = WinkelOut.CalcState; }
//############################################################################ // void MotorRegler(void) //############################################################################ { int motorwert,pd_ergebnis,h,tmp_int; int GierMischanteil,GasMischanteil; static long SummeNick=0,SummeRoll=0; static long sollGier = 0,tmp_long,tmp_long2; static long IntegralFehlerNick = 0; static long IntegralFehlerRoll = 0; static unsigned int RcLostTimer; static unsigned char delay_neutral = 0; static unsigned char delay_einschalten = 0,delay_ausschalten = 0; static unsigned int modell_fliegt = 0; static int hoehenregler = 0; static char TimerWerteausgabe = 0; static char NeueKompassRichtungMerken = 0; static long ausgleichNick, ausgleichRoll; Mittelwert(); GRN_ON; // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // Gaswert ermitteln // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ GasMischanteil = StickGas; if(GasMischanteil < 0) GasMischanteil = 0; // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // Emfang schlecht // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ if(SenderOkay < 100) { if(!PcZugriff) { if(BeepMuster == 0xffff) { beeptime = 15000; BeepMuster = 0x0c00; } } if(RcLostTimer) RcLostTimer--; else { MotorenEin = 0; Notlandung = 0; } ROT_ON; if(modell_fliegt > 2000) // wahrscheinlich in der Luft --> langsam absenken { GasMischanteil = EE_Parameter.NotGas; Notlandung = 1; PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] = 0; PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] = 0; PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] = 0; } else MotorenEin = 0; } else // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // Emfang gut // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ if(SenderOkay > 140) { Notlandung = 0; RcLostTimer = EE_Parameter.NotGasZeit * 50; if(GasMischanteil > 40) { if(modell_fliegt < 0xffff) modell_fliegt++; } if((modell_fliegt < 200) || (GasMischanteil < 40)) { SummeNick = 0; SummeRoll = 0; Mess_Integral_Gier = 0; Mess_Integral_Gier2 = 0; } if((PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] > 80) && MotorenEin == 0) { // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // auf Nullwerte kalibrieren // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75) // Neutralwerte { if(++delay_neutral > 200) // nicht sofort { GRN_OFF; MotorenEin = 0; delay_neutral = 0; modell_fliegt = 0; if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70 || abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) > 70) { unsigned char setting=1; if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 1; if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 2; if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 3; if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 4; if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 5; eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], setting); // aktiven Datensatz merken } if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert? { if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset(); } ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE); SetNeutral(); Piep(GetActiveParamSetNumber()); } } else if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75) // ACC Neutralwerte speichern { if(++delay_neutral > 200) // nicht sofort { GRN_OFF; eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK],0xff); // Werte löschen MotorenEin = 0; delay_neutral = 0; modell_fliegt = 0; SetNeutral(); eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK],NeutralAccX / 256); // ACC-NeutralWerte speichern eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK+1],NeutralAccX % 256); // ACC-NeutralWerte speichern eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL],NeutralAccY / 256); eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL+1],NeutralAccY % 256); eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_Z],(int)NeutralAccZ / 256); eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_Z+1],(int)NeutralAccZ % 256); Piep(GetActiveParamSetNumber()); } } else delay_neutral = 0; } // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // Gas ist unten // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ if(PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] < 35-120) { // Starten if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75) { // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // Einschalten // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ if(++delay_einschalten > 200) { delay_einschalten = 200; modell_fliegt = 1; MotorenEin = 1; sollGier = 0; Mess_Integral_Gier = 0; Mess_Integral_Gier2 = 0; Mess_IntegralNick = 0; Mess_IntegralRoll = 0; Mess_IntegralNick2 = IntegralNick; Mess_IntegralRoll2 = IntegralRoll; SummeNick = 0; SummeRoll = 0; } } else delay_einschalten = 0; //Auf Neutralwerte setzen // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // Auschalten // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75) { if(++delay_ausschalten > 200) // nicht sofort { MotorenEin = 0; delay_ausschalten = 200; modell_fliegt = 0; } } else delay_ausschalten = 0; } } // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // neue Werte von der Funke // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ if(!NewPpmData-- || Notlandung) { int tmp_int; static int stick_nick,stick_roll; ParameterZuordnung(); StickNick = (StickNick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4; StickNick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D; StickRoll = (StickRoll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4; StickRoll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D; StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]; StickGas = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120; if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]) > MaxStickNick) MaxStickNick = abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]); else MaxStickNick--; if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) > MaxStickRoll) MaxStickRoll = abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]); else MaxStickRoll--; if(Notlandung) {MaxStickNick = 0; MaxStickRoll = 0;} GyroFaktor = ((float)Parameter_Gyro_P + 10.0) / 256.0; IntegralFaktor = ((float) Parameter_Gyro_I) / 44000; //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ //+ Digitale Steuerung per DubWise //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ #define KEY_VALUE (Parameter_UserParam1 * 4) //(Poti3 * 8) if(DubWiseKeys[1]) beeptime = 10; if(DubWiseKeys[1] & DUB_KEY_UP) tmp_int = KEY_VALUE; else if(DubWiseKeys[1] & DUB_KEY_DOWN) tmp_int = -KEY_VALUE; else tmp_int = 0; ExternStickNick = (ExternStickNick * 7 + tmp_int) / 8; if(DubWiseKeys[1] & DUB_KEY_LEFT) tmp_int = KEY_VALUE; else if(DubWiseKeys[1] & DUB_KEY_RIGHT) tmp_int = -KEY_VALUE; else tmp_int = 0; ExternStickRoll = (ExternStickRoll * 7 + tmp_int) / 8; if(DubWiseKeys[0] & 8) ExternStickGier = 50;else if(DubWiseKeys[0] & 4) ExternStickGier =-50;else ExternStickGier = 0; if(DubWiseKeys[0] & 2) ExternHoehenValue++; if(DubWiseKeys[0] & 16) ExternHoehenValue--; StickNick += ExternStickNick / 8; StickRoll += ExternStickRoll / 8; StickGier += ExternStickGier; //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ //+ Analoge Steuerung per Seriell //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ if(ExternControl.Config & 0x01 && Parameter_UserParam1 > 128) { StickNick += (int) ExternControl.Nick * (int) EE_Parameter.Stick_P; StickRoll += (int) ExternControl.Roll * (int) EE_Parameter.Stick_P; StickGier += ExternControl.Gier; ExternHoehenValue = (int) ExternControl.Hight * (int)EE_Parameter.Hoehe_Verstaerkung; if(ExternControl.Gas < StickGas) StickGas = ExternControl.Gas; } if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor = 0; if(GyroFaktor < 0) GyroFaktor = 0; if(IntegralFaktor < 0) IntegralFaktor = 0; // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // Looping? // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_LINKS) Looping_Links = 1; else { { if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < (EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese))) Looping_Links = 0; } } if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_RECHTS) Looping_Rechts = 1; else { if(Looping_Rechts) // Hysterese { if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > -(EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese)) Looping_Rechts = 0; } } if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_OBEN) Looping_Oben = 1; else { if(Looping_Oben) // Hysterese { if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < (EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese))) Looping_Oben = 0; } } if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_UNTEN) Looping_Unten = 1; else { if(Looping_Unten) // Hysterese { if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > -(EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese)) Looping_Unten = 0; } } if(Looping_Links || Looping_Rechts) Looping_Roll = 1; else Looping_Roll = 0; if(Looping_Oben || Looping_Unten) {Looping_Nick = 1; Looping_Roll = 0; Looping_Links = 0; Looping_Rechts = 0;} else Looping_Nick = 0; } // Ende neue Funken-Werte if(Looping_Roll) beeptime = 100; if(Looping_Roll || Looping_Nick) { if(GasMischanteil > EE_Parameter.LoopGasLimit) GasMischanteil = EE_Parameter.LoopGasLimit; } // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // Bei Empfangsausfall im Flug // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ if(Notlandung) { StickGier = 0; StickNick = 0; StickRoll = 0; GyroFaktor = 0.1; IntegralFaktor = 0.005; Looping_Roll = 0; Looping_Nick = 0; } // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // Integrale auf ACC-Signal abgleichen // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ #define ABGLEICH_ANZAHL 256L MittelIntegralNick += IntegralNick; // Für die Mittelwertbildung aufsummieren MittelIntegralRoll += IntegralRoll; MittelIntegralNick2 += IntegralNick2; MittelIntegralRoll2 += IntegralRoll2; if(Looping_Nick || Looping_Roll) { IntegralAccNick = 0; IntegralAccRoll = 0; MittelIntegralNick = 0; MittelIntegralRoll = 0; MittelIntegralNick2 = 0; MittelIntegralRoll2 = 0; Mess_IntegralNick2 = Mess_IntegralNick; Mess_IntegralRoll2 = Mess_IntegralRoll; ZaehlMessungen = 0; LageKorrekturNick = 0; LageKorrekturRoll = 0; } // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ if(!Looping_Nick && !Looping_Roll) { long tmp_long, tmp_long2; tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick); tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll); tmp_long /= 16; tmp_long2 /= 16; if((MaxStickNick > 15) || (MaxStickRoll > 15)) { tmp_long /= 3; tmp_long2 /= 3; } if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25) { tmp_long /= 3; tmp_long2 /= 3; } #define AUSGLEICH 32 if(tmp_long > AUSGLEICH) tmp_long = AUSGLEICH; if(tmp_long < -AUSGLEICH) tmp_long =-AUSGLEICH; if(tmp_long2 > AUSGLEICH) tmp_long2 = AUSGLEICH; if(tmp_long2 <-AUSGLEICH) tmp_long2 =-AUSGLEICH; Mess_IntegralNick -= tmp_long; Mess_IntegralRoll -= tmp_long2; } // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ if(ZaehlMessungen >= ABGLEICH_ANZAHL) { static int cnt = 0; static char last_n_p,last_n_n,last_r_p,last_r_n; static long MittelIntegralNick_Alt,MittelIntegralRoll_Alt; if(!Looping_Nick && !Looping_Roll) { MittelIntegralNick /= ABGLEICH_ANZAHL; MittelIntegralRoll /= ABGLEICH_ANZAHL; IntegralAccNick = (EE_Parameter.GyroAccFaktor * IntegralAccNick) / ABGLEICH_ANZAHL; IntegralAccRoll = (EE_Parameter.GyroAccFaktor * IntegralAccRoll) / ABGLEICH_ANZAHL; IntegralAccZ = IntegralAccZ / ABGLEICH_ANZAHL; #define MAX_I 0//(Poti2/10) // Nick ++++++++++++++++++++++++++++++++++++++++++++++++ IntegralFehlerNick = (long)(MittelIntegralNick - (long)IntegralAccNick); ausgleichNick = IntegralFehlerNick / EE_Parameter.GyroAccAbgleich; // Roll ++++++++++++++++++++++++++++++++++++++++++++++++ IntegralFehlerRoll = (long)(MittelIntegralRoll - (long)IntegralAccRoll); ausgleichRoll = IntegralFehlerRoll / EE_Parameter.GyroAccAbgleich; LageKorrekturNick = ausgleichNick / ABGLEICH_ANZAHL; LageKorrekturRoll = ausgleichRoll / ABGLEICH_ANZAHL; if((MaxStickNick > 15) || (MaxStickRoll > 15) || (abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)) { LageKorrekturNick /= 2; LageKorrekturNick /= 2; } // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // Gyro-Drift ermitteln // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ MittelIntegralNick2 /= ABGLEICH_ANZAHL; MittelIntegralRoll2 /= ABGLEICH_ANZAHL; tmp_long = IntegralNick2 - IntegralNick; tmp_long2 = IntegralRoll2 - IntegralRoll; //DebugOut.Analog[25] = MittelIntegralRoll2 / 26; IntegralFehlerNick = tmp_long; IntegralFehlerRoll = tmp_long2; Mess_IntegralNick2 -= IntegralFehlerNick; Mess_IntegralRoll2 -= IntegralFehlerRoll; // IntegralFehlerNick = (IntegralFehlerNick * 1 + tmp_long) / 2; // IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2; DebugOut.Analog[17] = IntegralAccNick / 26; DebugOut.Analog[18] = IntegralAccRoll / 26; DebugOut.Analog[19] = IntegralFehlerNick;// / 26; DebugOut.Analog[20] = IntegralFehlerRoll;// / 26; DebugOut.Analog[21] = MittelIntegralNick / 26; DebugOut.Analog[22] = MittelIntegralRoll / 26; //DebugOut.Analog[28] = ausgleichNick; DebugOut.Analog[29] = ausgleichRoll; DebugOut.Analog[30] = LageKorrekturRoll * 10; #define FEHLER_LIMIT (ABGLEICH_ANZAHL * 4) #define FEHLER_LIMIT2 (ABGLEICH_ANZAHL * 16) #define BEWEGUNGS_LIMIT 20000 // Nick +++++++++++++++++++++++++++++++++++++++++++++++++ cnt = 1;// + labs(IntegralFehlerNick) / 4096; if(labs(MittelIntegralNick_Alt - MittelIntegralNick) < BEWEGUNGS_LIMIT) { if(IntegralFehlerNick > FEHLER_LIMIT2) { if(last_n_p) { cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2; ausgleichNick = IntegralFehlerNick / 8; if(ausgleichNick > 5000) ausgleichNick = 5000; LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL; } else last_n_p = 1; } else last_n_p = 0; if(IntegralFehlerNick < -FEHLER_LIMIT2) { if(last_n_n) { cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2; ausgleichNick = IntegralFehlerNick / 8; if(ausgleichNick < -5000) ausgleichNick = -5000; LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL; } else last_n_n = 1; } else last_n_n = 0; } else cnt = 0; if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp; if(IntegralFehlerNick > FEHLER_LIMIT) AdNeutralNick += cnt; if(IntegralFehlerNick < -FEHLER_LIMIT) AdNeutralNick -= cnt; // Roll +++++++++++++++++++++++++++++++++++++++++++++++++ cnt = 1;// + labs(IntegralFehlerNick) / 4096; ausgleichRoll = 0; if(labs(MittelIntegralRoll_Alt - MittelIntegralRoll) < BEWEGUNGS_LIMIT) { if(IntegralFehlerRoll > FEHLER_LIMIT2) { if(last_r_p) { cnt += labs(IntegralFehlerRoll) / FEHLER_LIMIT2; ausgleichRoll = IntegralFehlerRoll / 8; if(ausgleichRoll > 5000) ausgleichRoll = 5000; LageKorrekturRoll += ausgleichRoll / ABGLEICH_ANZAHL; } else last_r_p = 1; } else last_r_p = 0; if(IntegralFehlerRoll < -FEHLER_LIMIT2) { if(last_r_n) { cnt += labs(IntegralFehlerRoll) / FEHLER_LIMIT2; ausgleichRoll = IntegralFehlerRoll / 8; if(ausgleichRoll < -5000) ausgleichRoll = -5000; LageKorrekturRoll += ausgleichRoll / ABGLEICH_ANZAHL; } else last_r_n = 1; } else last_r_n = 0; } else { cnt = 0; } if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp; if(IntegralFehlerRoll > FEHLER_LIMIT) AdNeutralRoll += cnt; if(IntegralFehlerRoll < -FEHLER_LIMIT) AdNeutralRoll -= cnt; DebugOut.Analog[27] = ausgleichRoll; DebugOut.Analog[23] = AdNeutralNick;//10*(AdNeutralNick - StartNeutralNick); DebugOut.Analog[24] = 10*(AdNeutralRoll - StartNeutralRoll); } else { LageKorrekturRoll = 0; LageKorrekturNick = 0; } if(!IntegralFaktor) { LageKorrekturRoll = 0; LageKorrekturNick = 0;} // z.B. bei HH // +++++++++++++++++++++++++++++++++++++++++++++++++++++ MittelIntegralNick_Alt = MittelIntegralNick; MittelIntegralRoll_Alt = MittelIntegralRoll; // +++++++++++++++++++++++++++++++++++++++++++++++++++++ IntegralAccNick = 0; IntegralAccRoll = 0; IntegralAccZ = 0; MittelIntegralNick = 0; MittelIntegralRoll = 0; MittelIntegralNick2 = 0; MittelIntegralRoll2 = 0; ZaehlMessungen = 0; } //DebugOut.Analog[31] = StickRoll / (26*IntegralFaktor); // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // Gieren // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ if(abs(StickGier) > 20) // war 35 { if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1; } tmp_int = (long) EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo y = ax + bx² tmp_int += (EE_Parameter.Gier_P * StickGier) / 4; sollGier = tmp_int; Mess_Integral_Gier -= tmp_int; if(Mess_Integral_Gier > 50000) Mess_Integral_Gier = 50000; // begrenzen if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000; // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // Kompass // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)) { int w,v; static int SignalSchlecht = 0; w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln v = abs(IntegralRoll /512); if(v > w) w = v; // grösste Neigung ermitteln if(w < 25 && NeueKompassRichtungMerken && !SignalSchlecht) { KompassStartwert = KompassValue; NeueKompassRichtungMerken = 0; } w = (w * Parameter_KompassWirkung) / 64; // auf die Wirkung normieren w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln if(w > 0) { if(!SignalSchlecht) Mess_Integral_Gier += (KompassRichtung * w) / 32; // nach Kompass ausrichten if(SignalSchlecht) SignalSchlecht--; } else SignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek } // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // Debugwerte zuordnen // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ if(!TimerWerteausgabe--) { TimerWerteausgabe = 24; DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor; DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor; DebugOut.Analog[2] = Mittelwert_AccNick; DebugOut.Analog[3] = Mittelwert_AccRoll; DebugOut.Analog[4] = MesswertGier; DebugOut.Analog[5] = HoehenWert; DebugOut.Analog[6] =(Mess_Integral_Hoch / 512); DebugOut.Analog[8] = KompassValue; DebugOut.Analog[9] = UBat; DebugOut.Analog[10] = SenderOkay; DebugOut.Analog[16] = Mittelwert_AccHoch; if(Looping_Nick) MesswertNick = MesswertNick * GyroFaktor; else MesswertNick = IntegralNick * IntegralFaktor + MesswertNick * GyroFaktor; if(Looping_Roll) MesswertRoll = MesswertRoll * GyroFaktor; else MesswertRoll = IntegralRoll * IntegralFaktor + MesswertRoll * GyroFaktor; MesswertGier = MesswertGier * (2 * GyroFaktor) + Integral_Gier * IntegralFaktor / 2; DebugOut.Analog[25] = IntegralRoll * IntegralFaktor; DebugOut.Analog[31] = StickRoll;// / (26*IntegralFaktor); DebugOut.Analog[28] = MesswertRoll; // Maximalwerte abfangen #define MAX_SENSOR 2048 if(MesswertNick > MAX_SENSOR) MesswertNick = MAX_SENSOR; if(MesswertNick < -MAX_SENSOR) MesswertNick = -MAX_SENSOR; if(MesswertRoll > MAX_SENSOR) MesswertRoll = MAX_SENSOR; if(MesswertRoll < -MAX_SENSOR) MesswertRoll = -MAX_SENSOR; if(MesswertGier > MAX_SENSOR) MesswertGier = MAX_SENSOR; if(MesswertGier < -MAX_SENSOR) MesswertGier = -MAX_SENSOR; } // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // Höhenregelung // Die Höhenregelung schwächt lediglich das Gas ab, erhöht es allerdings nicht // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ //OCR0B = 180 - (Poti1 + 120) / 4; //DruckOffsetSetting = OCR0B; if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung { int tmp_int; if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER) // Regler wird über Schalter gesteuert { if(Parameter_MaxHoehe < 50) { SollHoehe = HoehenWert - 20; // Parameter_MaxHoehe ist der PPM-Wert des Schalters HoehenReglerAktiv = 0; } else HoehenReglerAktiv = 1; } else { SollHoehe = ((int) ExternHoehenValue + (int) Parameter_MaxHoehe) * (int)EE_Parameter.Hoehe_Verstaerkung - 20; HoehenReglerAktiv = 1; } if(Notlandung) SollHoehe = 0; h = HoehenWert; if((h > SollHoehe) && HoehenReglerAktiv) // zu hoch --> drosseln { h = ((h - SollHoehe) * (int) Parameter_Hoehe_P) / 16; // Differenz bestimmen --> P-Anteil h = GasMischanteil - h; // vom Gas abziehen h -= (HoeheD * Parameter_Luftdruck_D)/8; // D-Anteil tmp_int = ((Mess_Integral_Hoch / 512) * (signed long) Parameter_Hoehe_ACC_Wirkung) / 32; if(tmp_int > 50) tmp_int = 50; else if(tmp_int < -50) tmp_int = -50; h -= tmp_int; hoehenregler = (hoehenregler*15 + h) / 16; if(hoehenregler < EE_Parameter.Hoehe_MinGas) // nicht unter MIN { if(GasMischanteil >= EE_Parameter.Hoehe_MinGas) hoehenregler = EE_Parameter.Hoehe_MinGas; if(GasMischanteil < EE_Parameter.Hoehe_MinGas) hoehenregler = GasMischanteil; } if(hoehenregler > GasMischanteil) hoehenregler = GasMischanteil; // nicht mehr als Gas GasMischanteil = hoehenregler; } } if(GasMischanteil > MAX_GAS - 20) GasMischanteil = MAX_GAS - 20; // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // + Mischer und PI-Regler // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ DebugOut.Analog[7] = GasMischanteil; // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // Gier-Anteil // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ #define MUL_G 1.0 GierMischanteil = MesswertGier - sollGier; // Regler für Gier // GierMischanteil = 0; if(GierMischanteil > (GasMischanteil / 2)) GierMischanteil = GasMischanteil / 2; if(GierMischanteil < -(GasMischanteil / 2)) GierMischanteil = -(GasMischanteil / 2); if(GierMischanteil > ((MAX_GAS - GasMischanteil))) GierMischanteil = ((MAX_GAS - GasMischanteil)); if(GierMischanteil < -((MAX_GAS - GasMischanteil))) GierMischanteil = -((MAX_GAS - GasMischanteil)); if(GasMischanteil < 20) GierMischanteil = 0; // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // Nick-Achse // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ DiffNick = MesswertNick - (StickNick - GPS_Nick); // Differenz bestimmen if(IntegralFaktor) SummeNick += IntegralNick * IntegralFaktor - (StickNick - GPS_Nick); // I-Anteil bei Winkelregelung else SummeNick += DiffNick; // I-Anteil bei HH if(SummeNick > 16000) SummeNick = 16000; if(SummeNick < -16000) SummeNick = -16000; pd_ergebnis = DiffNick + Ki * SummeNick; // PI-Regler für Nick // Motor Vorn tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; if(pd_ergebnis > tmp_int) pd_ergebnis = tmp_int; if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int; motorwert = GasMischanteil + pd_ergebnis + GierMischanteil; // Mischer if ((motorwert < 0)) motorwert = 0; else if(motorwert > MAX_GAS) motorwert = MAX_GAS; if (motorwert < MIN_GAS) motorwert = MIN_GAS; Motor_Vorne = motorwert; // Motor Heck motorwert = GasMischanteil - pd_ergebnis + GierMischanteil; if ((motorwert < 0)) motorwert = 0; else if(motorwert > MAX_GAS) motorwert = MAX_GAS; if (motorwert < MIN_GAS) motorwert = MIN_GAS; Motor_Hinten = motorwert; // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // Roll-Achse // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ DiffRoll = MesswertRoll - (StickRoll - GPS_Roll); // Differenz bestimmen if(IntegralFaktor) SummeRoll += IntegralRoll * IntegralFaktor - (StickRoll - GPS_Roll);// I-Anteil bei Winkelregelung else SummeRoll += DiffRoll; // I-Anteil bei HH if(SummeRoll > 16000) SummeRoll = 16000; if(SummeRoll < -16000) SummeRoll = -16000; pd_ergebnis = DiffRoll + Ki * SummeRoll; // PI-Regler für Roll tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; if(pd_ergebnis > tmp_int) pd_ergebnis = tmp_int; if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int; // Motor Links motorwert = GasMischanteil + pd_ergebnis - GierMischanteil; #define GRENZE Poti1 if ((motorwert < 0)) motorwert = 0; else if(motorwert > MAX_GAS) motorwert = MAX_GAS; if (motorwert < MIN_GAS) motorwert = MIN_GAS; Motor_Links = motorwert; // Motor Rechts motorwert = GasMischanteil - pd_ergebnis - GierMischanteil; if ((motorwert < 0)) motorwert = 0; else if(motorwert > MAX_GAS) motorwert = MAX_GAS; if (motorwert < MIN_GAS) motorwert = MIN_GAS; Motor_Rechts = motorwert;