//############################################################################ // ÖÐÁ¢µã¸´Î» 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; }
void SucheLuftruckOffset(void) { unsigned int off; ExpandBaro = 0; CalcExpandBaroStep(); off = GetParamByte(PID_PRESSURE_OFFSET); if(off < 240) off += 10; OCR0A = off; OCR0B = 255-off; Delay_ms_Mess(150); if(MessLuftdruck > DESIRED_H_ADC) off = 240; for(; off > 5; off--) { OCR0A = off; OCR0B = 255-off; Delay_ms_Mess(100); printf("."); if(MessLuftdruck > DESIRED_H_ADC) break; } DruckOffsetSetting = off; SetParamByte(PID_PRESSURE_OFFSET, off); if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG) && (DruckOffsetSetting < 10 || DruckOffsetSetting >= 230)) VersionInfo.HardwareError[0] |= FC_ERROR0_PRESSURE; #if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__)) // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // + correction of the altitude error in higher altitudes // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ CalAthmospheare = 15; // re-claibrated from 16 to 15 at 2.09 -> the baro-Altimeter was about 7% too high if(ACC_AltitudeControl) { if(PlatinenVersion < 23) { if(off < 140) CalAthmospheare += (160 - off) / 26; } // else { if(off < 170) CalAthmospheare += (188 - off) / 19; } else { if(off < 170) CalAthmospheare += (188 - off) / 15; } // rescaled at 2.09 } Luftdruck = MessLuftdruck * CalAthmospheare; #endif Delay_ms_Mess(300); }
void SucheGyroOffset(void) { unsigned char i, ready = 0; int timeout; timeout = SetDelay(2000); for(i=140; i != 0; i--) { if(ready == 3 && i > 10) i = 9; ready = 0; if(AdWertNick < 1020) AnalogOffsetNick--; else if(AdWertNick > 1030) AnalogOffsetNick++; else ready++; if(AdWertRoll < 1020) AnalogOffsetRoll--; else if(AdWertRoll > 1030) AnalogOffsetRoll++; else ready++; if(AdWertGier < 1020) AnalogOffsetGier--; else if(AdWertGier > 1030) AnalogOffsetGier++; else ready++; I2C_Start(TWI_STATE_GYRO_OFFSET_TX); if(AnalogOffsetNick < 10) { VersionInfo.HardwareError[0] |= FC_ERROR0_GYRO_NICK; AnalogOffsetNick = 10;}; if(AnalogOffsetNick > 245) { VersionInfo.HardwareError[0] |= FC_ERROR0_GYRO_NICK; AnalogOffsetNick = 245;}; if(AnalogOffsetRoll < 10) { VersionInfo.HardwareError[0] |= FC_ERROR0_GYRO_ROLL; AnalogOffsetRoll = 10;}; if(AnalogOffsetRoll > 245) { VersionInfo.HardwareError[0] |= FC_ERROR0_GYRO_ROLL; AnalogOffsetRoll = 245;}; if(AnalogOffsetGier < 10) { VersionInfo.HardwareError[0] |= FC_ERROR0_GYRO_YAW; AnalogOffsetGier = 10;}; if(AnalogOffsetGier > 245) { VersionInfo.HardwareError[0] |= FC_ERROR0_GYRO_YAW; AnalogOffsetGier = 245;}; while(twi_state) if(CheckDelay(timeout)) {printf("\n\r DAC or I2C ERROR! Check I2C, 3Vref, DAC and BL-Ctrl"); break;} AdReady = 0; ANALOG_ON; while(!AdReady); if(i<10) Delay_ms_Mess(10); } Delay_ms_Mess(70); }
void SucheLuftruckOffset(void) { unsigned int off; ExpandBaro = 0; #if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__)) { unsigned char off2; OCR0A = 150; off2 = GetParamByte(PID_PRESSURE_OFFSET); if(off2 < 230) off2 += 10; OCR0B = off2; Delay_ms_Mess(100); if(MessLuftdruck > DESIRED_H_ADC) off2 = 240; for(; off2 >= 5; off2 -= 5) { OCR0B = off2; Delay_ms_Mess(50); printf("*"); if(MessLuftdruck > DESIRED_H_ADC) break; } SetParamByte(PID_PRESSURE_OFFSET, off2); if(off2 >= 15) off = 140; else off = 0; for(; off < 250;off++) { OCR0A = off; Delay_ms_Mess(50); printf("."); if(MessLuftdruck < DESIRED_H_ADC) break; } DruckOffsetSetting = off; } #else off = GetParamByte(PID_PRESSURE_OFFSET); if(off > 20) off -= 10; OCR0A = off; Delay_ms_Mess(100); if(MessLuftdruck < DESIRED_H_ADC) off = 0; for(; off < 250;off++) { OCR0A = off; Delay_ms_Mess(50); printf("."); if(MessLuftdruck < DESIRED_H_ADC) break; } DruckOffsetSetting = off; SetParamByte(PID_PRESSURE_OFFSET, off); #endif if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG) && (DruckOffsetSetting < 10 || DruckOffsetSetting >= 245)) VersionInfo.HardwareError[0] |= FC_ERROR0_PRESSURE; OCR0A = off; Delay_ms_Mess(300); }