148,6 → 148,11 |
acc_neutral.X = 0; |
acc_neutral.Y = 0; |
acc_neutral.Z = 0; |
|
Delay_ms(500); |
beeptime = 100; |
while (PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > -100) //Warten, bis Benutzer den Kopter neu ausgerichtet hat |
|
CalibrierMittelwert(); |
Delay_ms_Mess(100); |
CalibrierMittelwert(); |
155,21 → 160,15 |
acc_neutral.X = abs(Mittelwert_AccNick) / ACC_AMPLIFY; |
acc_neutral.Y = abs(Mittelwert_AccRoll) / ACC_AMPLIFY; |
acc_neutral.Z = Aktuell_az; |
/* |
while (PPM_in_gas > 100) //Warten, bis Benutzer den Kopter neu ausgerichtet hat |
{ |
uint8_t delay=9; |
|
Delay_ms(10); |
if (!delay--) |
{ |
delay = 9; |
beeptime = 100; |
} |
} |
|
beeptime = 100; |
Delay_ms(500); |
beeptime = 100; |
while (PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > -100) //Warten, bis Benutzer den Kopter neu ausgerichtet hat |
|
Delay_ms_Mess(100); |
acc_neutral.C = Aktuell_az; |
*/ |
acc_neutral.compass = Aktuell_az; |
|
eeprom_write_block(&acc_neutral,&ee_acc_neutral,sizeof(struct acc_neutral_struct)); |
} |
|
194,9 → 193,9 |
if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset(); |
} |
|
AdNeutralNick= AdWertNick; |
AdNeutralRoll= AdWertRoll; |
AdNeutralGier= AdWertGier; |
AdNeutralNick = AdWertNick; |
AdNeutralRoll = AdWertRoll; |
AdNeutralGier = AdWertGier; |
StartNeutralRoll = AdNeutralRoll; |
StartNeutralNick = AdNeutralNick; |
|
226,19 → 225,25 |
void Mittelwert(void) |
//############################################################################ |
{ |
static signed long tmpl,tmpl2; |
MesswertGier = (signed int) AdNeutralGier - AdWertGier; |
MesswertRoll = (signed int) AdWertRoll - AdNeutralRoll; |
MesswertNick = (signed int) AdWertNick - AdNeutralNick; |
|
static signed long tmpl,tmpl2; |
|
uint8_t tmp_sreg = SREG; |
cli(); |
MesswertGier = (int16_t)AdNeutralGier - AdWertGier; |
MesswertRoll = (int16_t)AdWertRoll - AdNeutralRoll; |
MesswertNick = (int16_t)AdWertNick - AdNeutralNick; |
int16_t AdWertAccNick_temp = AdWertAccNick; |
int16_t AdWertAccRoll_temp = AdWertAccRoll; |
SREG = tmp_sreg; |
|
//DebugOut.Analog[26] = MesswertNick; |
//DebugOut.Analog[28] = MesswertRoll; |
|
// Beschleunigungssensor ++++++++++++++++++++++++++++++++++++++++++++++++ |
Mittelwert_AccNick = ((long)Mittelwert_AccNick * 1 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 2L; |
Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 1 + ((ACC_AMPLIFY * (long)AdWertAccRoll))) / 2L; |
IntegralAccNick += ACC_AMPLIFY * AdWertAccNick; |
IntegralAccRoll += ACC_AMPLIFY * AdWertAccRoll; |
Mittelwert_AccNick = ((long)Mittelwert_AccNick * 1 + ((ACC_AMPLIFY * (long)AdWertAccNick_temp))) / 2L; |
Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 1 + ((ACC_AMPLIFY * (long)AdWertAccRoll_temp))) / 2L; |
IntegralAccNick += ACC_AMPLIFY * AdWertAccNick_temp; |
IntegralAccRoll += ACC_AMPLIFY * AdWertAccRoll_temp; |
// Gier ++++++++++++++++++++++++++++++++++++++++++++++++ |
Mess_Integral_Gier += MesswertGier; |
Mess_Integral_Gier2 += MesswertGier; |
332,16 → 337,16 |
// Messwerte beim Ermitteln der Nullage |
void CalibrierMittelwert(void) |
//############################################################################ |
{ |
// ADC auschalten, damit die Werte sich nicht während der Berechnung ändern |
ANALOG_OFF; |
MesswertNick = AdWertNick; |
MesswertRoll = AdWertRoll; |
MesswertGier = AdWertGier; |
Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick; |
Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll; |
// ADC einschalten |
ANALOG_ON; |
{ |
uint8_t tmp_sreg = SREG; |
cli(); |
MesswertNick = AdWertNick; |
MesswertRoll = AdWertRoll; |
MesswertGier = AdWertGier; |
Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick; |
Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll; |
SREG = tmp_sreg; |
|
if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 128 && Poti1 < 255) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 128 && Poti1 > 0) Poti1--; |
if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 128 && Poti2 < 255) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 128 && Poti2 > 0) Poti2--; |
if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 128 && Poti3 < 255) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 128 && Poti3 > 0) Poti3--; |
446,12 → 451,12 |
|
uint8_t tmp_sreg = SREG; |
cli(); |
PPM_in_nick = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]; |
PPM_diff_nick = PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]]; |
PPM_in_roll = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]; |
PPM_diff_roll = PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]]; |
PPM_in_gier = PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]; |
PPM_in_gas = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]]; |
PPM_in_nick = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]; |
PPM_diff_nick = PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]]; |
PPM_in_roll = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]; |
PPM_diff_roll = PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]]; |
PPM_in_gier = PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]; |
PPM_in_gas = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]]; |
SREG = tmp_sreg; |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
604,7 → 609,7 |
MaxStickNick = abs(PPM_in_nick); else MaxStickNick--; |
if(abs(PPM_in_roll) > MaxStickRoll) |
MaxStickRoll = abs(PPM_in_roll); else MaxStickRoll--; |
if(Notlandung) {MaxStickNick = 0; MaxStickRoll = 0;} |
if(Notlandung) {MaxStickNick = 0; MaxStickRoll = 0;} |
|
GyroFaktor = ((float)Parameter_Gyro_P + 10.0) / 256.0; |
IntegralFaktor = ((float)Parameter_Gyro_I) / 44000; |
912,7 → 917,7 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Gieren |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(abs(StickGier) > 20) |
if(abs(StickGier) > 15) |
{ |
if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1; |
} |
928,9 → 933,9 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) |
{ |
if (!updKompass--) // Aufruf mit ~15 Hz |
if (!updKompass--) // Aufruf mit ~20 Hz |
{ |
updKompass = 33; |
updKompass = 25; |
|
if ((MaxStickNick < 50) && (MaxStickRoll < 50)) // Bei extremen Flugmanövern keine Kompassauswertung |
{ |
952,8 → 957,8 |
if(!TimerWerteausgabe--) |
{ |
TimerWerteausgabe = 24; |
//DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor; |
//DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor; |
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; |