Blame | Last modification | View Log | RSS feed
/*#######################################################################################Flight Control#######################################################################################*/// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++// + Copyright (c) 04.2007 Holger Buss// + Nur für den privaten Gebrauch// + www.MikroKopter.com// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt// + bzgl. der Nutzungsbedingungen aufzunehmen.// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,// + Verkauf von Luftbildaufnahmen, usw.// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"// + eindeutig als Ursprung verlinkt werden// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion// + Benutzung auf eigene Gefahr// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur// + mit unserer Zustimmung zulässig// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++// + Redistributions of source code (with or without modifications) must retain the above copyright notice,// + this list of conditions and the following disclaimer.// + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived// + from this software without specific prior written permission.// + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet// + for non-commercial use (directly or indirectly)// + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted// + with our written permission// + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be// + clearly linked as origin// + * porting to systems other than hardware from www.mikrokopter.de is not allowed// + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"// + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE// + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE// + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE// + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR// + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF// + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS// + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE// + POSSIBILITY OF SUCH DAMAGE.// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++#include "main.h"#include "eeprom.c"unsigned char h,m,s;volatile unsigned int I2CTimeout = 100;int MesswertNick,MesswertRoll,MesswertGier,MesswertGierBias, RohMesswertNick,RohMesswertRoll;int TrimNick, TrimRoll;int AdNeutralGierBias;int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0;int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch, NeutralAccX=0, NeutralAccY=0;int NaviAccNick, NaviAccRoll,NaviCntAcc = 0;volatile float NeutralAccZ = 0;unsigned char CosinusNickWinkel = 0, CosinusRollWinkel = 0;long IntegralNick = 0,IntegralNick2 = 0;long IntegralRoll = 0,IntegralRoll2 = 0;long IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 0;long Integral_Gier = 0;long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0;long Mess_IntegralRoll = 0,Mess_IntegralRoll2 = 0;long Mess_Integral_Gier = 0,Mess_Integral_Gier2 = 0;long MittelIntegralNick,MittelIntegralRoll,MittelIntegralNick2,MittelIntegralRoll2;volatile long Mess_Integral_Hoch = 0;int KompassValue = 0;int KompassStartwert = 0;int KompassRichtung = 0;unsigned int KompassSignalSchlecht = 500;unsigned char MAX_GAS,MIN_GAS;unsigned char Notlandung = 0;unsigned char HoehenReglerAktiv = 0;unsigned char TrichterFlug = 0;long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L;long ErsatzKompass;int ErsatzKompassInGrad; // Kompasswert in Gradint GierGyroFehler = 0;char GyroFaktor,GyroFaktorGier;char IntegralFaktor,IntegralFaktorGier;int DiffNick,DiffRoll;int Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0;volatile unsigned char SenderOkay = 0;int StickNick = 0,StickRoll = 0,StickGier = 0,StickGas = 0;char MotorenEin = 0;int HoehenWert = 0;int SollHoehe = 0;int LageKorrekturRoll = 0,LageKorrekturNick = 0;//float Ki = FAKTOR_I;int Ki = 10300 / 33;unsigned char Looping_Nick = 0,Looping_Roll = 0;unsigned char Looping_Links = 0, Looping_Rechts = 0, Looping_Unten = 0, Looping_Oben = 0;unsigned char Parameter_Luftdruck_D = 48; // Wert : 0-250unsigned char Parameter_MaxHoehe = 251; // Wert : 0-250unsigned char Parameter_Hoehe_P = 16; // Wert : 0-32unsigned char Parameter_Hoehe_ACC_Wirkung = 58; // Wert : 0-250unsigned char Parameter_KompassWirkung = 64; // Wert : 0-250unsigned char Parameter_Gyro_D = 8; // Wert : 0-250unsigned char Parameter_Gyro_P = 150; // Wert : 10-250unsigned char Parameter_Gyro_I = 150; // Wert : 0-250unsigned char Parameter_Gier_P = 2; // Wert : 1-20unsigned char Parameter_I_Faktor = 10; // Wert : 1-20unsigned char Parameter_UserParam1 = 0;unsigned char Parameter_UserParam2 = 0;unsigned char Parameter_UserParam3 = 0;unsigned char Parameter_UserParam4 = 0;unsigned char Parameter_UserParam5 = 0;unsigned char Parameter_UserParam6 = 0;unsigned char Parameter_UserParam7 = 0;unsigned char Parameter_UserParam8 = 0;unsigned char Parameter_ServoNickControl = 100;unsigned char Parameter_LoopGasLimit = 70;unsigned char Parameter_AchsKopplung1 = 90;unsigned char Parameter_AchsKopplung2 = 65;unsigned char Parameter_CouplingYawCorrection = 64;//unsigned char Parameter_AchsGegenKopplung1 = 0;unsigned char Parameter_DynamicStability = 100;unsigned char Parameter_J16Bitmask; // for the J16 Outputunsigned char Parameter_J16Timing; // for the J16 Outputunsigned char Parameter_J17Bitmask; // for the J17 Outputunsigned char Parameter_J17Timing; // for the J17 Outputunsigned char Parameter_NaviGpsModeControl; // Parameters for the Naviboardunsigned char Parameter_NaviGpsGain;unsigned char Parameter_NaviGpsP;unsigned char Parameter_NaviGpsI;unsigned char Parameter_NaviGpsD;unsigned char Parameter_NaviGpsACC;unsigned char Parameter_NaviOperatingRadius;unsigned char Parameter_NaviWindCorrection;unsigned char Parameter_NaviSpeedCompensation;unsigned char Parameter_ExternalControl;struct mk_param_struct EE_Parameter;signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20;int MaxStickNick = 0,MaxStickRoll = 0;unsigned int modell_fliegt = 0;volatile unsigned char MikroKopterFlags = 0;long GIER_GRAD_FAKTOR = 1291;signed int KopplungsteilNickRoll,KopplungsteilRollNick;unsigned char RequiredMotors = 4;unsigned char Motor[MAX_MOTORS];signed int tmp_motorwert[MAX_MOTORS];int MotorSmoothing(int neu, int alt){int motor;if(neu > alt) motor = (1*(int)alt + neu) / 2;else motor = neu - (alt - neu)*1;//if(Poti2 < 20) return(neu);return(motor);}void Piep(unsigned char Anzahl){while(Anzahl--){if(MotorenEin) return; //auf keinen Fall im Flug!beeptime = 100;Delay_ms(250);}}//############################################################################// Nullwerte ermittelnvoid SetNeutral(void)//############################################################################{unsigned char i;unsigned int gier_neutral=0, nick_neutral=0, roll_neutral=0;ServoActive = 0; HEF4017R_ON;NeutralAccX = 0;NeutralAccY = 0;NeutralAccZ = 0;AdNeutralNick = 0;AdNeutralRoll = 0;AdNeutralGier = 0;AdNeutralGierBias = 0;Parameter_AchsKopplung1 = 0;Parameter_AchsKopplung2 = 0;ExpandBaro = 0;CalibrierMittelwert();Delay_ms_Mess(100);CalibrierMittelwert();if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert?{if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();}#define NEUTRAL_FILTER 32for(i=0; i<NEUTRAL_FILTER; i++){Delay_ms_Mess(10);gier_neutral += AdWertGier;nick_neutral += AdWertNick;roll_neutral += AdWertRoll;}AdNeutralNick= (nick_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER / 8);AdNeutralRoll= (roll_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER / 8);AdNeutralGier= (gier_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER);AdNeutralGierBias = AdNeutralGier;StartNeutralRoll = AdNeutralRoll;StartNeutralNick = AdNeutralNick;if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) > 4){NeutralAccY = abs(Mittelwert_AccRoll) / (2*ACC_AMPLIFY);NeutralAccX = abs(Mittelwert_AccNick) / (2*ACC_AMPLIFY);NeutralAccZ = Aktuell_az;}else{NeutralAccX = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK+1]);NeutralAccY = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL+1]);NeutralAccZ = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_Z]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_Z+1]);}MesswertNick = 0;MesswertRoll = 0;MesswertGier = 0;Delay_ms_Mess(100);Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick;Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll;IntegralNick = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccNick;IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll;Mess_IntegralNick2 = IntegralNick;Mess_IntegralRoll2 = IntegralRoll;Mess_Integral_Gier = 0;StartLuftdruck = Luftdruck;HoeheD = 0;Mess_Integral_Hoch = 0;KompassStartwert = KompassValue;GPS_Neutral();beeptime = 50;Umschlag180Nick = ((long) EE_Parameter.WinkelUmschlagNick * 2500L) + 15000L;Umschlag180Roll = ((long) EE_Parameter.WinkelUmschlagRoll * 2500L) + 15000L;ExternHoehenValue = 0;ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR;GierGyroFehler = 0;SendVersionToNavi = 1;LED_Init();MikroKopterFlags |= FLAG_CALIBRATE;FromNaviCtrl_Value.Kalman_K = -1;FromNaviCtrl_Value.Kalman_MaxDrift = 0;FromNaviCtrl_Value.Kalman_MaxFusion = 32;Poti1 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110;Poti2 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110;Poti3 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110;Poti4 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110;ServoActive = 1;SenderOkay = 100;}//############################################################################// Bearbeitet die Messwertevoid Mittelwert(void)//############################################################################{static signed long tmpl,tmpl2,tmpl3,tmpl4;static signed int oldNick, oldRoll, d2Roll, d2Nick;signed long winkel_nick, winkel_roll;MesswertGier = (signed int) AdNeutralGier - AdWertGier;// MesswertGierBias = (signed int) AdNeutralGierBias - AdWertGier;MesswertNick = (signed int) AdWertNickFilter / 8;MesswertRoll = (signed int) AdWertRollFilter / 8;RohMesswertNick = MesswertNick;RohMesswertRoll = MesswertRoll;//DebugOut.Analog[21] = MesswertNick;//DebugOut.Analog[22] = MesswertRoll;//DebugOut.Analog[22] = Mess_Integral_Gier;//DebugOut.Analog[21] = MesswertNick;//DebugOut.Analog[22] = MesswertRoll;// Beschleunigungssensor ++++++++++++++++++++++++++++++++++++++++++++++++Mittelwert_AccNick = ((long)Mittelwert_AccNick * 3 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 4L;Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 3 + ((ACC_AMPLIFY * (long)AdWertAccRoll))) / 4L;Mittelwert_AccHoch = ((long)Mittelwert_AccHoch * 3 + ((long)AdWertAccHoch)) / 4L;IntegralAccNick += ACC_AMPLIFY * AdWertAccNick;IntegralAccRoll += ACC_AMPLIFY * AdWertAccRoll;NaviAccNick += AdWertAccNick;NaviAccRoll += AdWertAccRoll;NaviCntAcc++;IntegralAccZ += Aktuell_az - NeutralAccZ;//++++++++++++++++++++++++++++++++++++++++++++++++// ADC einschaltenANALOG_ON;AdReady = 0;//++++++++++++++++++++++++++++++++++++++++++++++++if(Mess_IntegralRoll > 93000L) winkel_roll = 93000L;else if(Mess_IntegralRoll <-93000L) winkel_roll = -93000L;else winkel_roll = Mess_IntegralRoll;if(Mess_IntegralNick > 93000L) winkel_nick = 93000L;else if(Mess_IntegralNick <-93000L) winkel_nick = -93000L;else winkel_nick = Mess_IntegralNick;// Gier ++++++++++++++++++++++++++++++++++++++++++++++++Mess_Integral_Gier += MesswertGier;ErsatzKompass += MesswertGier;// Kopplungsanteil +++++++++++++++++++++++++++++++++++++if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV)){tmpl3 = (MesswertRoll * winkel_nick) / 2048L;tmpl3 *= Parameter_AchsKopplung2; //65tmpl3 /= 4096L;tmpl4 = (MesswertNick * winkel_roll) / 2048L;tmpl4 *= Parameter_AchsKopplung2; //65tmpl4 /= 4096L;KopplungsteilNickRoll = tmpl3;KopplungsteilRollNick = tmpl4;tmpl4 -= tmpl3;ErsatzKompass += tmpl4;if(!Parameter_CouplingYawCorrection) Mess_Integral_Gier -= tmpl4/2; // Gier nachhelfentmpl = ((MesswertGier + tmpl4) * winkel_nick) / 2048L;tmpl *= Parameter_AchsKopplung1; // 90tmpl /= 4096L;tmpl2 = ((MesswertGier + tmpl4) * winkel_roll) / 2048L;tmpl2 *= Parameter_AchsKopplung1;tmpl2 /= 4096L;if(abs(MesswertGier) > 64) if(labs(tmpl) > 128 || labs(tmpl2) > 128) TrichterFlug = 1;//MesswertGier += (Parameter_CouplingYawCorrection * tmpl4) / 256;}else tmpl = tmpl2 = KopplungsteilNickRoll = KopplungsteilRollNick = 0;TrimRoll = tmpl - tmpl2 / 100L;TrimNick = -tmpl2 + tmpl / 100L;// Kompasswert begrenzen ++++++++++++++++++++++++++++++++++++++++++++++++if(ErsatzKompass >= (360L * GIER_GRAD_FAKTOR)) ErsatzKompass -= 360L * GIER_GRAD_FAKTOR; // 360° Umschlagif(ErsatzKompass < 0) ErsatzKompass += 360L * GIER_GRAD_FAKTOR;// Roll ++++++++++++++++++++++++++++++++++++++++++++++++Mess_IntegralRoll2 += MesswertRoll + TrimRoll;Mess_IntegralRoll += MesswertRoll + TrimRoll - LageKorrekturRoll;if(Mess_IntegralRoll > Umschlag180Roll){Mess_IntegralRoll = -(Umschlag180Roll - 25000L);Mess_IntegralRoll2 = Mess_IntegralRoll;}if(Mess_IntegralRoll <-Umschlag180Roll){Mess_IntegralRoll = (Umschlag180Roll - 25000L);Mess_IntegralRoll2 = Mess_IntegralRoll;}// Nick ++++++++++++++++++++++++++++++++++++++++++++++++Mess_IntegralNick2 += MesswertNick + TrimNick;Mess_IntegralNick += MesswertNick + TrimNick - LageKorrekturNick;if(Mess_IntegralNick > Umschlag180Nick){Mess_IntegralNick = -(Umschlag180Nick - 25000L);Mess_IntegralNick2 = Mess_IntegralNick;}if(Mess_IntegralNick <-Umschlag180Nick){Mess_IntegralNick = (Umschlag180Nick - 25000L);Mess_IntegralNick2 = Mess_IntegralNick;}Integral_Gier = Mess_Integral_Gier;IntegralNick = Mess_IntegralNick;IntegralRoll = Mess_IntegralRoll;IntegralNick2 = Mess_IntegralNick2;IntegralRoll2 = Mess_IntegralRoll2;#define D_LIMIT 128MesswertNick = HiResNick / 8;MesswertRoll = HiResRoll / 8;if(AdWertNick < 15) MesswertNick = -1000; if(AdWertNick < 7) MesswertNick = -2000;if(PlatinenVersion == 10) { if(AdWertNick > 1010) MesswertNick = +1000; if(AdWertNick > 1017) MesswertNick = +2000; }else { if(AdWertNick > 2000) MesswertNick = +1000; if(AdWertNick > 2015) MesswertNick = +2000; }if(AdWertRoll < 15) MesswertRoll = -1000; if(AdWertRoll < 7) MesswertRoll = -2000;if(PlatinenVersion == 10) { if(AdWertRoll > 1010) MesswertRoll = +1000; if(AdWertRoll > 1017) MesswertRoll = +2000; }else { if(AdWertRoll > 2000) MesswertRoll = +1000; if(AdWertRoll > 2015) MesswertRoll = +2000; }if(Parameter_Gyro_D){d2Nick = HiResNick - oldNick;oldNick = (oldNick + HiResNick)/2;if(d2Nick > D_LIMIT) d2Nick = D_LIMIT;else if(d2Nick < -D_LIMIT) d2Nick = -D_LIMIT;MesswertNick += (d2Nick * (signed int) Parameter_Gyro_D) / 16;d2Roll = HiResRoll - oldRoll;oldRoll = (oldRoll + HiResRoll)/2;if(d2Roll > D_LIMIT) d2Roll = D_LIMIT;else if(d2Roll < -D_LIMIT) d2Roll = -D_LIMIT;MesswertRoll += (d2Roll * (signed int) Parameter_Gyro_D) / 16;HiResNick += (d2Nick * (signed int) Parameter_Gyro_D);HiResRoll += (d2Roll * (signed int) Parameter_Gyro_D);}if(RohMesswertRoll > 0) TrimRoll += ((long) abs(KopplungsteilNickRoll) * Parameter_CouplingYawCorrection) / 64L;else TrimRoll -= ((long) abs(KopplungsteilNickRoll) * Parameter_CouplingYawCorrection) / 64L;if(RohMesswertNick > 0) TrimNick += ((long) abs(KopplungsteilRollNick) * Parameter_CouplingYawCorrection) / 64L;else TrimNick -= ((long) abs(KopplungsteilRollNick) * Parameter_CouplingYawCorrection) / 64L;if(EE_Parameter.GlobalConfig & CFG_DREHRATEN_BEGRENZER && !Looping_Nick && !Looping_Roll){if(RohMesswertNick > 256) MesswertNick += 1 * (RohMesswertNick - 256);else if(RohMesswertNick < -256) MesswertNick += 1 * (RohMesswertNick + 256);if(RohMesswertRoll > 256) MesswertRoll += 1 * (RohMesswertRoll - 256);else if(RohMesswertRoll < -256) MesswertRoll += 1 * (RohMesswertRoll + 256);}if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--;if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--;if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--;if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--;if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;}//############################################################################// Messwerte beim Ermitteln der Nullagevoid CalibrierMittelwert(void)//############################################################################{if(PlatinenVersion == 13) SucheGyroOffset();// ADC auschalten, damit die Werte sich nicht während der Berechnung ändernANALOG_OFF;MesswertNick = AdWertNick;MesswertRoll = AdWertRoll;MesswertGier = AdWertGier;Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick;Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll;Mittelwert_AccHoch = (long)AdWertAccHoch;// ADC einschaltenANALOG_ON;if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--;if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--;if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--;if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--;if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L;}//############################################################################// Senden der Motorwerte per I2C-Busvoid SendMotorData(void)//############################################################################{unsigned char i;if(!MotorenEin){MikroKopterFlags &= ~(FLAG_MOTOR_RUN | FLAG_FLY);for(i=0;i<MAX_MOTORS;i++){if(!PC_MotortestActive) MotorTest[i] = 0;Motor[i] = MotorTest[i];}if(PC_MotortestActive) PC_MotortestActive--;}else MikroKopterFlags |= FLAG_MOTOR_RUN;DebugOut.Analog[12] = Motor[0];DebugOut.Analog[13] = Motor[1];DebugOut.Analog[14] = Motor[3];DebugOut.Analog[15] = Motor[2];//Start I2C Interrupt Modetwi_state = 0;motor = 0;i2c_start();}//############################################################################// Trägt ggf. das Poti als Parameter einvoid ParameterZuordnung(void)//############################################################################{#define CHK_POTI_MM(b,a,min,max) { if(a > 250) { if(a == 251) b = Poti1; else if(a == 252) b = Poti2; else if(a == 253) b = Poti3; else if(a == 254) b = Poti4;} else b = a; if(b <= min) b = min; else if(b >= max) b = max;}#define CHK_POTI(b,a,min,max) { if(a > 250) { if(a == 251) b = Poti1; else if(a == 252) b = Poti2; else if(a == 253) b = Poti3; else if(a == 254) b = Poti4;} else b = a; }CHK_POTI(Parameter_MaxHoehe,EE_Parameter.MaxHoehe,0,255);CHK_POTI_MM(Parameter_Luftdruck_D,EE_Parameter.Luftdruck_D,0,100);CHK_POTI_MM(Parameter_Hoehe_P,EE_Parameter.Hoehe_P,0,100);CHK_POTI(Parameter_Hoehe_ACC_Wirkung,EE_Parameter.Hoehe_ACC_Wirkung,0,255);CHK_POTI(Parameter_KompassWirkung,EE_Parameter.KompassWirkung,0,255);CHK_POTI_MM(Parameter_Gyro_P,EE_Parameter.Gyro_P,10,255);CHK_POTI(Parameter_Gyro_I,EE_Parameter.Gyro_I,0,255);CHK_POTI(Parameter_Gyro_D,EE_Parameter.Gyro_D,0,255);CHK_POTI(Parameter_I_Faktor,EE_Parameter.I_Faktor,0,255);CHK_POTI(Parameter_UserParam1,EE_Parameter.UserParam1,0,255);CHK_POTI(Parameter_UserParam2,EE_Parameter.UserParam2,0,255);CHK_POTI(Parameter_UserParam3,EE_Parameter.UserParam3,0,255);CHK_POTI(Parameter_UserParam4,EE_Parameter.UserParam4,0,255);CHK_POTI(Parameter_UserParam5,EE_Parameter.UserParam5,0,255);CHK_POTI(Parameter_UserParam6,EE_Parameter.UserParam6,0,255);CHK_POTI(Parameter_UserParam7,EE_Parameter.UserParam7,0,255);CHK_POTI(Parameter_UserParam8,EE_Parameter.UserParam8,0,255);CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255);CHK_POTI(Parameter_LoopGasLimit,EE_Parameter.LoopGasLimit,0,255);CHK_POTI(Parameter_AchsKopplung1, EE_Parameter.AchsKopplung1,0,255);CHK_POTI(Parameter_AchsKopplung2, EE_Parameter.AchsKopplung2,0,255);CHK_POTI(Parameter_CouplingYawCorrection,EE_Parameter.CouplingYawCorrection,0,255);// CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255);CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability,0,255);CHK_POTI_MM(Parameter_J16Timing,EE_Parameter.J16Timing,1,255);CHK_POTI_MM(Parameter_J17Timing,EE_Parameter.J17Timing,1,255);CHK_POTI(Parameter_ExternalControl,EE_Parameter.ExternalControl,0,255);Ki = 10300 / (Parameter_I_Faktor + 1);MAX_GAS = EE_Parameter.Gas_Max;MIN_GAS = EE_Parameter.Gas_Min;}//############################################################################//void MotorRegler(void)//############################################################################{int pd_ergebnis_nick,pd_ergebnis_roll,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 int hoehenregler = 0;static char TimerWerteausgabe = 0;static char NeueKompassRichtungMerken = 0;static long ausgleichNick, ausgleichRoll;int IntegralNickMalFaktor,IntegralRollMalFaktor;unsigned char i;Mittelwert();GRN_ON;// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++// Gaswert ermitteln// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++GasMischanteil = StickGas;if(GasMischanteil < MIN_GAS + 10) GasMischanteil = MIN_GAS + 10;// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++// Empfang 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 > 1000) // wahrscheinlich in der Luft --> langsam absenken{GasMischanteil = EE_Parameter.NotGas;Notlandung = 1;PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] = 0;PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;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 && MotorenEin){if(modell_fliegt < 0xffff) modell_fliegt++;}if((modell_fliegt < 256)){SummeNick = 0;SummeRoll = 0;if(modell_fliegt == 250){NeueKompassRichtungMerken = 1;sollGier = 0;Mess_Integral_Gier = 0;// Mess_Integral_Gier2 = 0;}} else MikroKopterFlags |= FLAG_FLY;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;SetActiveParamSetNumber(setting); // aktiven Datensatz merken}// elseif(abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) < 30 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -70){WinkelOut.CalcState = 1;beeptime = 1000;}else{ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert?{if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();}SetNeutral();Piep(GetActiveParamSetNumber());}}}elseif(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öschenMotorenEin = 0;delay_neutral = 0;modell_fliegt = 0;SetNeutral();eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK],NeutralAccX / 256); // ACC-NeutralWerte speicherneeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK+1],NeutralAccX % 256); // ACC-NeutralWerte speicherneeprom_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){// Startenif(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 = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccNick;Mess_IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll;Mess_IntegralNick2 = IntegralNick;Mess_IntegralRoll2 = IntegralRoll;SummeNick = 0;SummeRoll = 0;MikroKopterFlags |= FLAG_START;}}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){static int stick_nick,stick_roll;ParameterZuordnung();stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4;stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;StickNick = stick_nick - (GPS_Nick + GPS_Nick2);stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4;stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;StickRoll = stick_roll - (GPS_Roll + GPS_Roll2);StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];if(StickGier > 2) StickGier -= 2; elseif(StickGier < -2) StickGier += 2; else StickGier = 0;StickGas = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;GyroFaktor = (Parameter_Gyro_P + 10.0);IntegralFaktor = Parameter_Gyro_I;GyroFaktorGier = (Parameter_Gyro_P + 10.0);IntegralFaktorGier = Parameter_Gyro_I;//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//+ Analoge Steuerung per Seriell//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++if(ExternControl.Config & 0x01 && Parameter_ExternalControl > 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(StickGas < 0) StickGas = 0;if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor = 0;if(GyroFaktor < 0) GyroFaktor = 0;if(IntegralFaktor < 0) IntegralFaktor = 0;if(abs(StickNick/STICK_GAIN) > MaxStickNick){MaxStickNick = abs(StickNick)/STICK_GAIN;if(MaxStickNick > 100) MaxStickNick = 100;}else MaxStickNick--;if(abs(StickRoll/STICK_GAIN) > MaxStickRoll){MaxStickRoll = abs(StickRoll)/STICK_GAIN;if(MaxStickRoll > 100) MaxStickRoll = 100;}else MaxStickRoll--;if(Notlandung) {MaxStickNick = 0; MaxStickRoll = 0;}// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++// Looping?// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > EE_Parameter.LoopThreshold) && EE_Parameter.BitConfig & 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.BitConfig & 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.BitConfig & 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.BitConfig & 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-Werteif(Looping_Roll || Looping_Nick){if(GasMischanteil > EE_Parameter.LoopGasLimit) GasMischanteil = EE_Parameter.LoopGasLimit;TrichterFlug = 1;}// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++// Bei Empfangsausfall im Flug// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++if(Notlandung){StickGier = 0;StickNick = 0;StickRoll = 0;GyroFaktor = 90;IntegralFaktor = 120;GyroFaktorGier = 90;IntegralFaktorGier = 120;Looping_Roll = 0;Looping_Nick = 0;}// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++// Integrale auf ACC-Signal abgleichen// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++#define ABGLEICH_ANZAHL 256LMittelIntegralNick += IntegralNick; // Für die Mittelwertbildung aufsummierenMittelIntegralRoll += 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 && (Aktuell_az > 512 || MotorenEin)){long tmp_long, tmp_long2;if(FromNaviCtrl_Value.Kalman_K != -1 /*&& !TrichterFlug*/){tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick);tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll);tmp_long = (tmp_long * FromNaviCtrl_Value.Kalman_K) / (32 * 16);tmp_long2 = (tmp_long2 * FromNaviCtrl_Value.Kalman_K) / (32 * 16);if((MaxStickNick > 64) || (MaxStickRoll > 64)){tmp_long /= 2;tmp_long2 /= 2;}if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25){tmp_long /= 3;tmp_long2 /= 3;}if(tmp_long > (long) FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long = (long) FromNaviCtrl_Value.Kalman_MaxFusion;if(tmp_long < (long)-FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long = (long)-FromNaviCtrl_Value.Kalman_MaxFusion;if(tmp_long2 > (long) FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long2 = (long) FromNaviCtrl_Value.Kalman_MaxFusion;if(tmp_long2 < (long)-FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long2 = (long)-FromNaviCtrl_Value.Kalman_MaxFusion;}else{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 > 64) || (MaxStickRoll > 64)){tmp_long /= 3;tmp_long2 /= 3;}if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25){tmp_long /= 3;tmp_long2 /= 3;}#define AUSGLEICH 32if(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;}//if(Poti2 > 20) { tmp_long = 0; tmp_long2 = 0;}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 && !TrichterFlug && EE_Parameter.Driftkomp){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 > 64) || (MaxStickRoll > 64) || (abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)) && (FromNaviCtrl_Value.Kalman_K == -1)){LageKorrekturNick /= 2;LageKorrekturRoll /= 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;if(EE_Parameter.Driftkomp){if(GierGyroFehler > ABGLEICH_ANZAHL/2) { AdNeutralGier++; AdNeutralGierBias++; }if(GierGyroFehler <-ABGLEICH_ANZAHL/2) { AdNeutralGier--; AdNeutralGierBias--; }}//DebugOut.Analog[22] = MittelIntegralRoll / 26;//DebugOut.Analog[24] = GierGyroFehler;GierGyroFehler = 0;/*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;//MittelIntegralRoll = MittelIntegralRoll;//DebugOut.Analog[28] = ausgleichNick;/*DebugOut.Analog[29] = ausgleichRoll;DebugOut.Analog[30] = LageKorrekturRoll * 10;*/#define FEHLER_LIMIT (ABGLEICH_ANZAHL / 2)#define FEHLER_LIMIT1 (ABGLEICH_ANZAHL * 2) //4#define FEHLER_LIMIT2 (ABGLEICH_ANZAHL * 16) //16#define BEWEGUNGS_LIMIT 20000// Nick +++++++++++++++++++++++++++++++++++++++++++++++++cnt = 1;// + labs(IntegralFehlerNick) / 4096;if(labs(IntegralFehlerNick) > FEHLER_LIMIT1) cnt = 4;if(labs(MittelIntegralNick_Alt - MittelIntegralNick) < BEWEGUNGS_LIMIT || (FromNaviCtrl_Value.Kalman_MaxDrift > 3*8)){if(IntegralFehlerNick > FEHLER_LIMIT2){if(last_n_p){cnt += labs(IntegralFehlerNick) / (FEHLER_LIMIT2 / 8);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 / 8);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;KompassSignalSchlecht = 1000;}if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;if(FromNaviCtrl_Value.Kalman_MaxDrift) if(cnt > FromNaviCtrl_Value.Kalman_MaxDrift) cnt = FromNaviCtrl_Value.Kalman_MaxDrift;if(IntegralFehlerNick > FEHLER_LIMIT) AdNeutralNick += cnt;if(IntegralFehlerNick < -FEHLER_LIMIT) AdNeutralNick -= cnt;// Roll +++++++++++++++++++++++++++++++++++++++++++++++++cnt = 1;// + labs(IntegralFehlerNick) / 4096;if(labs(IntegralFehlerRoll) > FEHLER_LIMIT1) cnt = 4;ausgleichRoll = 0;if(labs(MittelIntegralRoll_Alt - MittelIntegralRoll) < BEWEGUNGS_LIMIT || (FromNaviCtrl_Value.Kalman_MaxDrift > 3*8)){if(IntegralFehlerRoll > FEHLER_LIMIT2){if(last_r_p){cnt += labs(IntegralFehlerRoll) / (FEHLER_LIMIT2 / 8);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 / 8);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;KompassSignalSchlecht = 1000;}if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;if(FromNaviCtrl_Value.Kalman_MaxDrift) if(cnt > FromNaviCtrl_Value.Kalman_MaxDrift) cnt = FromNaviCtrl_Value.Kalman_MaxDrift;if(IntegralFehlerRoll > FEHLER_LIMIT) AdNeutralRoll += cnt;if(IntegralFehlerRoll < -FEHLER_LIMIT) AdNeutralRoll -= cnt;}else{LageKorrekturRoll = 0;LageKorrekturNick = 0;TrichterFlug = 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;} // ZaehlMessungen >= ABGLEICH_ANZAHL// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++// Gieren// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++// if(GasMischanteil < 35) { if(StickGier > 10) StickGier = 10; else if(StickGier < -10) StickGier = -10;};if(abs(StickGier) > 15) // war 35{KompassSignalSchlecht = 1000;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; // begrenzenif(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000;// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++// Kompass// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//DebugOut.Analog[16] = KompassSignalSchlecht;if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)){int w,v,r,fehler,korrektur;w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosselnv = abs(IntegralRoll /512);if(v > w) w = v; // grösste Neigung ermittelnkorrektur = w / 8 + 1;fehler = ((540 + KompassValue - (ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180;if(abs(MesswertGier) > 128){fehler = 0;}if(!KompassSignalSchlecht && w < 25){GierGyroFehler += fehler;if(NeueKompassRichtungMerken){beeptime = 200;// KompassStartwert = KompassValue;ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR;KompassStartwert = (ErsatzKompass/GIER_GRAD_FAKTOR);NeueKompassRichtungMerken = 0;}}ErsatzKompass += (fehler * 8) / korrektur;w = (w * Parameter_KompassWirkung) / 32; // auf die Wirkung normierenw = Parameter_KompassWirkung - w; // Wirkung ggf drosselnif(w >= 0){if(!KompassSignalSchlecht){v = 64 + ((MaxStickNick + MaxStickRoll)) / 8;r = ((540 + (ErsatzKompass/GIER_GRAD_FAKTOR) - KompassStartwert) % 360) - 180;// r = KompassRichtung;v = (r * w) / v; // nach Kompass ausrichtenw = 3 * Parameter_KompassWirkung;if(v > w) v = w; // Begrenzenelseif(v < -w) v = -w;Mess_Integral_Gier += v;}if(KompassSignalSchlecht) KompassSignalSchlecht--;}else KompassSignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek}// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++// Debugwerte zuordnen// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++if(!TimerWerteausgabe--){TimerWerteausgabe = 24;DebugOut.Analog[0] = IntegralNick / (EE_Parameter.GyroAccFaktor * 4);DebugOut.Analog[1] = IntegralRoll / (EE_Parameter.GyroAccFaktor * 4);DebugOut.Analog[2] = Mittelwert_AccNick / 4;DebugOut.Analog[3] = Mittelwert_AccRoll / 4;DebugOut.Analog[4] = MesswertGier;DebugOut.Analog[5] = HoehenWert;DebugOut.Analog[6] = Aktuell_az;//(Mess_Integral_Hoch / 512);//Aktuell_az;DebugOut.Analog[8] = KompassValue;DebugOut.Analog[9] = UBat;DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR;DebugOut.Analog[10] = SenderOkay;//DebugOut.Analog[16] = Mittelwert_AccHoch;//DebugOut.Analog[17] = FromNaviCtrl_Value.Distance;//DebugOut.Analog[18] = (int)FromNaviCtrl_Value.OsdBar;DebugOut.Analog[19] = WinkelOut.CalcState;DebugOut.Analog[20] = ServoValue;// DebugOut.Analog[24] = MesswertNick/2;// DebugOut.Analog[25] = MesswertRoll/2;DebugOut.Analog[27] = (int)FromNaviCtrl_Value.Kalman_MaxDrift;// DebugOut.Analog[28] = (int)FromNaviCtrl_Value.Kalman_MaxFusion;// DebugOut.Analog[29] = (int)FromNaviCtrl_Value.Kalman_K;DebugOut.Analog[29] = FromNaviCtrl_Value.SerialDataOkay;DebugOut.Analog[30] = GPS_Nick;DebugOut.Analog[31] = GPS_Roll;// DebugOut.Analog[19] -= DebugOut.Analog[19]/128;// if(DebugOut.Analog[19] > 0) DebugOut.Analog[19]--; else DebugOut.Analog[19]++;/* DebugOut.Analog[16] = motor_rx[0];DebugOut.Analog[17] = motor_rx[1];DebugOut.Analog[18] = motor_rx[2];DebugOut.Analog[19] = motor_rx[3];DebugOut.Analog[20] = motor_rx[0] + motor_rx[1] + motor_rx[2] + motor_rx[3];DebugOut.Analog[20] /= 14;DebugOut.Analog[21] = motor_rx[4];DebugOut.Analog[22] = motor_rx[5];DebugOut.Analog[23] = motor_rx[6];DebugOut.Analog[24] = motor_rx[7];DebugOut.Analog[25] = motor_rx[4] + motor_rx[5] + motor_rx[6] + motor_rx[7];*/// DebugOut.Analog[9] = MesswertNick;// DebugOut.Analog[9] = SollHoehe;// DebugOut.Analog[10] = Mess_Integral_Gier / 128;// DebugOut.Analog[11] = KompassStartwert;// DebugOut.Analog[10] = Parameter_Gyro_I;// DebugOut.Analog[10] = EE_Parameter.Gyro_I;// DebugOut.Analog[9] = KompassRichtung;// DebugOut.Analog[10] = GasMischanteil;// DebugOut.Analog[3] = HoeheD * 32;// DebugOut.Analog[4] = hoehenregler;}// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++// Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++if(TrichterFlug) { SummeRoll = 0; SummeNick = 0;};if(!Looping_Nick) IntegralNickMalFaktor = (IntegralNick * IntegralFaktor) / (44000 / STICK_GAIN); else IntegralNickMalFaktor = 0;if(!Looping_Roll) IntegralRollMalFaktor = (IntegralRoll * IntegralFaktor) / (44000 / STICK_GAIN); else IntegralRollMalFaktor = 0;#define TRIM_MAX 200if(TrimNick > TRIM_MAX) TrimNick = TRIM_MAX; else if(TrimNick <-TRIM_MAX) TrimNick =-TRIM_MAX;if(TrimRoll > TRIM_MAX) TrimRoll = TRIM_MAX; else if(TrimRoll <-TRIM_MAX) TrimRoll =-TRIM_MAX;MesswertNick = IntegralNickMalFaktor + (long)((long)MesswertNick * GyroFaktor + (long)TrimNick * 128L) / (256L / STICK_GAIN);MesswertRoll = IntegralRollMalFaktor + (long)((long)MesswertRoll * GyroFaktor + (long)TrimRoll * 128L) / (256L / STICK_GAIN);MesswertGier = (long)(MesswertGier * 2 * (long)GyroFaktorGier) / (256L / STICK_GAIN) + (long)(Integral_Gier * IntegralFaktorGier) / (2 * (44000 / STICK_GAIN));// Maximalwerte abfangen// #define MAX_SENSOR (4096*STICK_GAIN)#define MAX_SENSOR (4096*4)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;// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++// all BL-Ctrl connected?// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++if(MissingMotor) if(modell_fliegt > 1 && modell_fliegt < 50 && GasMischanteil > 0){modell_fliegt = 1;GasMischanteil = MIN_GAS;}// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++// Höhenregelung// Die Höhenregelung schwächt lediglich das Gas ab, erhöht es allerdings nicht// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++GasMischanteil *= STICK_GAIN;if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung{int tmp_int;static char delay = 100;if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER) // Regler wird über Schalter gesteuert{if(Parameter_MaxHoehe < 50){if(!delay--){if((MessLuftdruck > 1000) && OCR0A < 254){if(OCR0A < 244){ExpandBaro -= 10;OCR0A = DruckOffsetSetting - ExpandBaro;}else OCR0A = 254;beeptime = 300;delay = 250;}elseif((MessLuftdruck < 100) && OCR0A > 1){if(OCR0A > 10){ExpandBaro += 10;OCR0A = DruckOffsetSetting - ExpandBaro;}else OCR0A = 1;beeptime = 300;delay = 250;}else{SollHoehe = HoehenWert - 20; // Parameter_MaxHoehe ist der PPM-Wert des SchaltersHoehenReglerAktiv = 0;delay = 1;}}}else{HoehenReglerAktiv = 1;delay = 200;}}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 / STICK_GAIN); // Differenz bestimmen --> P-Anteilh = GasMischanteil - h; // vom Gas abziehenh -= (HoeheD)/(8/STICK_GAIN); // D-Anteiltmp_int = ((Mess_Integral_Hoch / 128) * (signed long) Parameter_Hoehe_ACC_Wirkung) / (128 / STICK_GAIN);if(tmp_int > 70*STICK_GAIN) tmp_int = 70*STICK_GAIN;else if(tmp_int < -(70*STICK_GAIN)) tmp_int = -(70*STICK_GAIN);h -= tmp_int;hoehenregler = (hoehenregler*15 + h) / 16;if(hoehenregler < EE_Parameter.Hoehe_MinGas * STICK_GAIN) // nicht unter MIN{if(GasMischanteil >= EE_Parameter.Hoehe_MinGas * STICK_GAIN) hoehenregler = EE_Parameter.Hoehe_MinGas * STICK_GAIN;if(GasMischanteil < EE_Parameter.Hoehe_MinGas * STICK_GAIN) hoehenregler = GasMischanteil;}if(hoehenregler > GasMischanteil) hoehenregler = GasMischanteil; // nicht mehr als GasGasMischanteil = hoehenregler;}}if(GasMischanteil > (MAX_GAS - 20) * STICK_GAIN) GasMischanteil = (MAX_GAS - 20) * STICK_GAIN;// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++// + Mischer und PI-Regler// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++DebugOut.Analog[7] = GasMischanteil;// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++// Gier-Anteil// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++#define MUL_G 1.0GierMischanteil = MesswertGier - sollGier * STICK_GAIN; // Regler für Gier// GierMischanteil = 0;#define MIN_GIERGAS (40*STICK_GAIN) // unter diesem Gaswert trotzdem Gierenif(GasMischanteil > MIN_GIERGAS){if(GierMischanteil > (GasMischanteil / 2)) GierMischanteil = GasMischanteil / 2;if(GierMischanteil < -(GasMischanteil / 2)) GierMischanteil = -(GasMischanteil / 2);}else{if(GierMischanteil > (MIN_GIERGAS / 2)) GierMischanteil = MIN_GIERGAS / 2;if(GierMischanteil < -(MIN_GIERGAS / 2)) GierMischanteil = -(MIN_GIERGAS / 2);}tmp_int = MAX_GAS*STICK_GAIN;if(GierMischanteil > ((tmp_int - GasMischanteil))) GierMischanteil = ((tmp_int - GasMischanteil));if(GierMischanteil < -((tmp_int - GasMischanteil))) GierMischanteil = -((tmp_int - GasMischanteil));// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++// Nick-Achse// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++DiffNick = MesswertNick - StickNick; // Differenz bestimmenif(IntegralFaktor) SummeNick += IntegralNickMalFaktor - StickNick; // I-Anteil bei Winkelregelungelse SummeNick += DiffNick; // I-Anteil bei HHif(SummeNick > (STICK_GAIN * 16000L)) SummeNick = (STICK_GAIN * 16000L);if(SummeNick < -(16000L * STICK_GAIN)) SummeNick = -(16000L * STICK_GAIN);pd_ergebnis_nick = DiffNick + SummeNick / Ki; // PI-Regler für Nick// Motor Vorntmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;if(pd_ergebnis_nick > tmp_int) pd_ergebnis_nick = tmp_int;if(pd_ergebnis_nick < -tmp_int) pd_ergebnis_nick = -tmp_int;// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++// Roll-Achse// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++DiffRoll = MesswertRoll - StickRoll; // Differenz bestimmenif(IntegralFaktor) SummeRoll += IntegralRollMalFaktor - StickRoll;// I-Anteil bei Winkelregelungelse SummeRoll += DiffRoll; // I-Anteil bei HHif(SummeRoll > (STICK_GAIN * 16000L)) SummeRoll = (STICK_GAIN * 16000L);if(SummeRoll < -(16000L * STICK_GAIN)) SummeRoll = -(16000L * STICK_GAIN);pd_ergebnis_roll = DiffRoll + SummeRoll / Ki; // PI-Regler für Rolltmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;if(pd_ergebnis_roll > tmp_int) pd_ergebnis_roll = tmp_int;if(pd_ergebnis_roll < -tmp_int) pd_ergebnis_roll = -tmp_int;// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++// Universal Mixer// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++for(i=0; i<MAX_MOTORS; i++){signed int tmp_int;if(Mixer.Motor[i][0] > 0){tmp_int = ((long)GasMischanteil * Mixer.Motor[i][0]) / 64L;tmp_int += ((long)pd_ergebnis_nick * Mixer.Motor[i][1]) / 64L;tmp_int += ((long)pd_ergebnis_roll * Mixer.Motor[i][2]) / 64L;tmp_int += ((long)GierMischanteil * Mixer.Motor[i][3]) / 64L;tmp_motorwert[i] = MotorSmoothing(tmp_int,tmp_motorwert[i]); // Filtertmp_int = tmp_motorwert[i] / STICK_GAIN;CHECK_MIN_MAX(tmp_int,MIN_GAS,MAX_GAS);Motor[i] = tmp_int;}else Motor[i] = 0;}/*if(Poti1 > 20) Motor1 = 0;if(Poti1 > 90) Motor6 = 0;if(Poti1 > 140) Motor2 = 0;//if(Poti1 > 200) Motor7 = 0;*/}