//############################################################################
// ÖÐÁ¢µã¸´Î»
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);
}
