//############################################################################ // ÖÐÁ¢µã¸´Î» void SetNeutral(void) //############################################################################ { NeutralAccX = 0; NeutralAccY = 0; NeutralAccZ = 0; AdNeutralNick = 0; AdNeutralRoll = 0; AdNeutralGier = 0; Parameter_AchsKopplung1 = 0; Parameter_AchsGegenKopplung1 = 0; CalibrierMittelwert(); Delay_ms_Mess(100); CalibrierMittelwert(); if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert? { if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset(); } AdNeutralNick= AdWertNick; AdNeutralRoll= AdWertRoll; AdNeutralGier= AdWertGier; StartNeutralRoll = AdNeutralRoll; StartNeutralNick = AdNeutralNick; if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) > 4) { NeutralAccY = abs(Mittelwert_AccRoll) / ACC_AMPLIFY; NeutralAccX = abs(Mittelwert_AccNick) / 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]); } Mess_IntegralNick = 0; Mess_IntegralNick2 = 0; Mess_IntegralRoll = 0; Mess_IntegralRoll2 = 0; Mess_Integral_Gier = 0; MesswertNick = 0; MesswertRoll = 0; MesswertGier = 0; StartLuftdruck = Luftdruck; HoeheD = 0; Mess_Integral_Hoch = 0; KompassStartwert = KompassValue; GPS_Neutral(); beeptime = 50; Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L; Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L; ExternHoehenValue = 0; }
//############################################################################ //主程序 int main (void) //############################################################################ { unsigned int timer; //unsigned int timer2 = 0; DDRB = 0x00; PORTB = 0x00; for(timer = 0; timer < 1000; timer++); if(PINB & 0x01) PlatinenVersion = 11; else PlatinenVersion = 10; DDRC = 0x81; // SCL PORTC = 0xff; // Pullup SDA DDRB = 0x1B; // LEDs und offset PORTB = 0x01; // LED_Rot DDRD = 0x3E; // Speaker & TXD & J3 J4 J5 DDRD |=0x80; // J7 PORTD = 0xF7; // LED MCUSR &=~(1<<WDRF); WDTCSR |= (1<<WDCE)|(1<<WDE); WDTCSR = 0; beeptime = 2000; StickGier = 0; PPM_in[K_GAS] = 0; StickRoll = 0; StickNick = 0; ROT_OFF; Timer_Init(); UART_Init(); rc_sum_init(); ADC_Init(); i2c_init(); SPI_MasterInit(); sei(); VersionInfo.Hauptversion = VERSION_HAUPTVERSION; VersionInfo.Nebenversion = VERSION_NEBENVERSION; VersionInfo.PCKompatibel = VERSION_KOMPATIBEL; printf("\n\rFlightControl\n\rHardware:%d.%d\n\rSoftware:V%d.%d%c ",PlatinenVersion/10,PlatinenVersion%10, VERSION_HAUPTVERSION, VERSION_NEBENVERSION,VERSION_INDEX + 'a'); printf("\n\r=============================="); GRN_ON; #define EE_DATENREVISION 69 // wird angepasst, wenn sich die EEPROM-Daten ge鋘dert haben if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_VALID]) != EE_DATENREVISION) { printf("\n\rInit. EEPROM: Generiere Default-Parameter..."); DefaultKonstanten1(); for (unsigned char i=0; i<6; i++) { if(i==2) DefaultKonstanten2(); // 相机 if(i==3) DefaultKonstanten3(); // 初学者 if(i>3) DefaultKonstanten2(); // 相机 WriteParameterSet(i, (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE); } eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], 3); // default-Setting eeprom_write_byte(&EEPromArray[EEPROM_ADR_VALID], EE_DATENREVISION); } if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) > 4) { printf("\n\rACC nicht abgeglichen!"); } ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE); printf("\n\rBenutze Parametersatz %d", GetActiveParamSetNumber()); if(EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG) { printf("\n\rAbgleich Luftdrucksensor.."); timer = SetDelay(1000); SucheLuftruckOffset(); while (!CheckDelay(timer)); printf("OK\n\r"); } SetNeutral(); ROT_OFF; beeptime = 2000; ExternControl.Digital[0] = 0x55; printf("\n\rSteuerung: "); if (EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) printf("HeadingHold"); else printf("Neutral"); printf("\n\n\r"); LcdClear(); I2CTimeout = 5000; while (1) { if(UpdateMotor) // 间隙调整 { SPI_TransmitByte(); //# UpdateMotor=0; MotorRegler(); SendMotorData(); ROT_OFF; if(PcZugriff) PcZugriff--; else { DubWiseKeys[0] = 0; DubWiseKeys[1] = 0; ExternControl.Config = 0; ExternStickNick = 0; ExternStickRoll = 0; ExternStickGier = 0; } if(SenderOkay) SenderOkay--; if(!I2CTimeout) { I2CTimeout = 5; i2c_reset(); if((BeepMuster == 0xffff) && MotorenEin) { beeptime = 10000; BeepMuster = 0x0080; } } else { I2CTimeout--; ROT_OFF; } if(SIO_DEBUG && !UpdateMotor) { DatenUebertragung(); BearbeiteRxDaten(); } else BearbeiteRxDaten(); if(CheckDelay(timer)) { if(UBat < EE_Parameter.UnterspannungsWarnung) { if(BeepMuster == 0xffff) { beeptime = 6000; BeepMuster = 0x0300; } } SPI_StartTransmitPacket();//# timer = SetDelay(100); } } } return (1); }
//############################################################################ // 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;
//############################################################################ //Hauptprogramm int main (void) //############################################################################ { unsigned int timer,i,timer2 = 0, timerPolling; DDRB = 0x00; PORTB = 0x00; for(timer = 0; timer < 1000; timer++); // verzögern #if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__)) PlatinenVersion = 21; #else if(PINB & 0x01) { if(PINB & 0x02) PlatinenVersion = 13; else PlatinenVersion = 11; } else { if(PINB & 0x02) PlatinenVersion = 20; else PlatinenVersion = 10; } #endif DDRC = 0x81; // SCL DDRC |=0x40; // HEF4017 Reset PORTC = 0xff; // Pullup SDA DDRB = 0x1B; // LEDs und Druckoffset PORTB = 0x01; // LED_Rot DDRD = 0x3E; // Speaker & TXD & J3 J4 J5 PORTD = 0x47; // LED HEF4017R_ON; MCUSR &=~(1<<WDRF); WDTCSR |= (1<<WDCE)|(1<<WDE); WDTCSR = 0; beeptime = 2500; StickGier = 0; PPM_in[K_GAS] = 0; StickRoll = 0; StickNick = 0; if(PlatinenVersion >= 20) GIER_GRAD_FAKTOR = 1220; else GIER_GRAD_FAKTOR = 1291; // unterschiedlich für ME und ENC ROT_OFF; Timer_Init(); TIMER2_Init(); UART_Init(); rc_sum_init(); ADC_Init(); I2C_Init(1); SPI_MasterInit(); Capacity_Init(); LIBFC_Init(); GRN_ON; sei(); ParamSet_Init(); // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // + Check connected BL-Ctrls // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // Check connected BL-Ctrls BLFlags |= BLFLAG_READ_VERSION; motor_read = 0; // read the first I2C-Data SendMotorData(); timer = SetDelay(500); while(!(BLFlags & BLFLAG_TX_COMPLETE) && !CheckDelay(timer)); //wait for complete transfer printf("\n\rFound BL-Ctrl: "); timer = SetDelay(4000); for(i=0; i < MAX_MOTORS; i++) { SendMotorData(); while(!(BLFlags & BLFLAG_TX_COMPLETE) && !CheckDelay(timer)); //wait for complete transfer if(Mixer.Motor[i][0] > 0) // wait max 4 sec for the BL-Ctrls to wake up { while(!CheckDelay(timer) && !(Motor[i].State & MOTOR_STATE_PRESENT_MASK) ) { SendMotorData(); while(!(BLFlags & BLFLAG_TX_COMPLETE) && !CheckDelay(timer)); //wait for complete transfer } } if(Motor[i].State & MOTOR_STATE_PRESENT_MASK) { printf("%d",i+1); FoundMotors++; // if(Motor[i].Version & MOTOR_STATE_NEW_PROTOCOL_MASK) printf("(new) "); } } for(i=0; i < MAX_MOTORS; i++) { if(!(Motor[i].State & MOTOR_STATE_PRESENT_MASK) && Mixer.Motor[i][0] > 0) { printf("\n\r\n\r!! MISSING BL-CTRL: %d !!",i+1); ServoActive = 2; // just in case the FC would be used as camera-stabilizer } Motor[i].State &= ~MOTOR_STATE_ERROR_MASK; // clear error counter } printf("\n\r==================================="); if(RequiredMotors < FoundMotors) VersionInfo.HardwareError[1] |= FC_ERROR1_MIXER; //if(EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG) { printf("\n\rCalibrating pressure sensor.."); timer = SetDelay(1000); SucheLuftruckOffset(); while (!CheckDelay(timer)); printf("OK\n\r"); } SetNeutral(0); ROT_OFF; beeptime = 2000; ExternControl.Digital[0] = 0x55; FlugMinuten = (unsigned int)GetParamByte(PID_FLIGHT_MINUTES) * 256 + (unsigned int)GetParamByte(PID_FLIGHT_MINUTES + 1); FlugMinutenGesamt = (unsigned int)GetParamByte(PID_FLIGHT_MINUTES_TOTAL) * 256 + (unsigned int)GetParamByte(PID_FLIGHT_MINUTES_TOTAL + 1); if((FlugMinutenGesamt == 0xFFFF) || (FlugMinuten == 0xFFFF)) { FlugMinuten = 0; FlugMinutenGesamt = 0; } printf("\n\rFlight-time %u min Total:%u min", FlugMinuten, FlugMinutenGesamt); printf("\n\rControl: "); if (EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) printf("HeadingHold"); else printf("Normal (ACC-Mode)"); LcdClear(); I2CTimeout = 5000; WinkelOut.Orientation = 1; LipoDetection(1); LIBFC_ReceiverInit(EE_Parameter.Receiver); printf("\n\r===================================\n\r"); //SpektrumBinding(); timer = SetDelay(2000); timerPolling = SetDelay(250); Debug(ANSI_CLEAR "FC-Start!\n\rFlugzeit: %d min", FlugMinutenGesamt); // Note: this won't waste flash memory, if #DEBUG is not active DebugOut.Status[0] = 0x01 | 0x02; JetiBeep = 0; while (1) { if (JetiUpdateModeActive) while (1); if(CheckDelay(timerPolling)) { timerPolling = SetDelay(100); LIBFC_Polling(); } if(UpdateMotor && AdReady) // ReglerIntervall { UpdateMotor=0; if(WinkelOut.CalcState) CalMk3Mag(); else MotorRegler(); SendMotorData(); ROT_OFF; if(SenderOkay) { SenderOkay--; VersionInfo.HardwareError[1] &= ~FC_ERROR1_PPM; } else { TIMSK1 |= _BV(ICIE1); // enable PPM-Input PPM_in[0] = 0; // set RSSI to zero on data timeout VersionInfo.HardwareError[1] |= FC_ERROR1_PPM; } //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ //if(HoehenReglerAktiv && NaviDataOkay && SenderOkay < 160 && SenderOkay > 10 && FromNaviCtrl_Value.SerialDataOkay > 220) SenderOkay = 160; //if(HoehenReglerAktiv && NaviDataOkay && SenderOkay < 101 && SenderOkay > 10 && FromNaviCtrl_Value.SerialDataOkay > 1) SenderOkay = 101; //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ if(!--I2CTimeout || MissingMotor) { if(!I2CTimeout) { I2C_Reset(); I2CTimeout = 5; DebugOut.Analog[28]++; // I2C-Error VersionInfo.HardwareError[1] |= FC_ERROR1_I2C; DebugOut.Status[1] |= 0x02; // BL-Error-Status } if((BeepMuster == 0xffff) && MotorenEin) { beeptime = 10000; BeepMuster = 0x0080; } } else { ROT_OFF; if(!beeptime) { VersionInfo.HardwareError[1] &= ~FC_ERROR1_I2C; } } if(!UpdateMotor) { if(CalculateServoSignals) CalculateServo(); DatenUebertragung(); BearbeiteRxDaten(); if(CheckDelay(timer)) { static unsigned char second; timer += 20; // 20 ms interval if(MissingMotor) { VersionInfo.HardwareError[1] |= FC_ERROR1_BL_MISSING; DebugOut.Status[1] |= 0x02; // BL-Error-Status } else { VersionInfo.HardwareError[1] &= ~FC_ERROR1_BL_MISSING; if(I2CTimeout > 6) DebugOut.Status[1] &= ~0x02; // BL-Error-Status } if(I2CTimeout > 6) VersionInfo.HardwareError[1] &= ~FC_ERROR1_I2C; if(PcZugriff) PcZugriff--; else { ExternControl.Config = 0; ExternStickNick = 0; ExternStickRoll = 0; ExternStickGier = 0; if(BeepMuster == 0xffff && SenderOkay == 0) { beeptime = 15000; BeepMuster = 0x0c00; } } if(NaviDataOkay > 200) { NaviDataOkay--; VersionInfo.HardwareError[1] &= ~FC_ERROR1_SPI_RX; } else { if(NC_Version.Compatible) { VersionInfo.HardwareError[1] |= FC_ERROR1_SPI_RX; if(BeepMuster == 0xffff && MotorenEin) { beeptime = 15000; BeepMuster = 0xA800; } } GPS_Nick = 0; GPS_Roll = 0; //if(!beeptime) FromNaviCtrl.CompassValue = -1; NaviDataOkay = 0; } if(UBat < BattLowVoltageWarning) { FC_StatusFlags |= FC_STATUS_LOWBAT; if(BeepMuster == 0xffff) { beeptime = 6000; BeepMuster = 0x0300; } } else if(!beeptime) FC_StatusFlags &= ~FC_STATUS_LOWBAT; SPI_StartTransmitPacket(); SendSPI = 4; if(!MotorenEin) timer2 = 1450; // 0,5 Minuten aufrunden else if(++second == 49) { second = 0; FlugSekunden++; } if(++timer2 == 2930) // eine Minute { timer2 = 0; FlugMinuten++; FlugMinutenGesamt++; SetParamByte(PID_FLIGHT_MINUTES,FlugMinuten / 256); SetParamByte(PID_FLIGHT_MINUTES+1,FlugMinuten % 256); SetParamByte(PID_FLIGHT_MINUTES_TOTAL,FlugMinutenGesamt / 256); SetParamByte(PID_FLIGHT_MINUTES_TOTAL+1,FlugMinutenGesamt % 256); timer = SetDelay(20); // falls "timer += 20;" mal nicht geht } } LED_Update(); Capacity_Update(); } //else DebugOut.Analog[26]++; } if(!SendSPI) { SPI_TransmitByte(); } } return (1); }