153,8 → 153,8 |
acc_neutral.X = abs(Mittelwert_AccNick) / ACC_AMPLIFY; |
acc_neutral.Y = abs(Mittelwert_AccRoll) / ACC_AMPLIFY; |
acc_neutral.Z = Aktuell_az; |
|
while (PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] > 100) //Warten, bis Benutzer den Kopter neu ausgerichtet hat |
/* |
while (PPM_in_gas > 100) //Warten, bis Benutzer den Kopter neu ausgerichtet hat |
{ |
uint8_t delay=9; |
|
167,7 → 167,7 |
} |
Delay_ms_Mess(100); |
acc_neutral.C = Aktuell_az; |
|
*/ |
eeprom_write_block(&acc_neutral,&ee_acc_neutral,sizeof(struct acc_neutral_struct)); |
} |
|
450,7 → 450,23 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
GasMischanteil = StickGas; |
if(GasMischanteil < 0) GasMischanteil = 0; |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Datenübernahme von RC |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
int16_t PPM_in_nick, PPM_in_roll, PPM_in_gier, PPM_diff_nick, PPM_diff_roll, PPM_in_gas; |
|
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]]; |
SREG = tmp_sreg; |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Emfang schlecht |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(SenderOkay < 100) |
474,11 → 490,11 |
{ |
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; |
PPM_diff_nick = 0; |
PPM_diff_roll = 0; |
PPM_in_nick = 0; |
PPM_in_roll = 0; |
PPM_in_gier = 0; |
} |
else MotorenEin = 0; |
} |
501,12 → 517,12 |
Mess_Integral_Gier = 0; |
Mess_Integral_Gier2 = 0; |
} |
if((PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] > 80) && MotorenEin == 0) |
if((PPM_in_gas > 80) && MotorenEin == 0) |
{ |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// auf Nullwerte kalibrieren |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75) // Neutralwerte |
if(PPM_in_gier > 75) // Neutralwerte |
{ |
if(++delay_neutral > 200) // nicht sofort |
{ |
514,14 → 530,14 |
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) |
if(PPM_in_nick > 70 || abs(PPM_in_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; |
if(PPM_in_roll > 70 && PPM_in_nick < 70) setting = 1; |
if(PPM_in_roll > 70 && PPM_in_nick > 70) setting = 2; |
if(PPM_in_roll < 70 && PPM_in_nick > 70) setting = 3; |
if(PPM_in_roll <-70 && PPM_in_nick > 70) setting = 4; |
if(PPM_in_roll <-70 && PPM_in_nick < 70) setting = 5; |
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], setting); // aktiven Datensatz merken |
} |
ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], sizeof(struct mk_param_struct)); |
534,10 → 550,10 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Gas ist unten |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] < 35-120) |
if(PPM_in_gas < 35-120) |
{ |
// Starten |
if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75) |
if(PPM_in_gier < -75) |
{ |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Einschalten |
563,7 → 579,7 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Auschalten |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75) |
if(PPM_in_gier > 75) |
{ |
if(++delay_ausschalten > 200) // nicht sofort |
{ |
583,18 → 599,18 |
{ |
int tmp_int; |
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; |
StickNick = (StickNick * 3 + PPM_in_nick * EE_Parameter.Stick_P) / 4; |
StickNick += PPM_diff_nick * EE_Parameter.Stick_D; |
StickRoll = (StickRoll * 3 + PPM_in_roll * EE_Parameter.Stick_P) / 4; |
StickRoll += PPM_diff_roll * EE_Parameter.Stick_D; |
|
StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]; |
StickGas = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120; |
StickGier = -PPM_in_gier; |
StickGas = PPM_in_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(abs(PPM_in_nick) > MaxStickNick) |
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;} |
|
GyroFaktor = ((float)Parameter_Gyro_P + 10.0) / 256.0; |
638,36 → 654,36 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Looping? |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_LINKS) Looping_Links = 1; |
if((PPM_in_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_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; |
if((PPM_in_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_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; |
if((PPM_in_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_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; |
if((PPM_in_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(PPM_in_nick > -(EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese)) Looping_Unten = 0; |
} |
} |
|
690,8 → 706,8 |
StickGier = 0; |
StickNick = 0; |
StickRoll = 0; |
GyroFaktor = 0.1; |
IntegralFaktor = 0.005; |
GyroFaktor = 0.5; |
IntegralFaktor = 0.003; |
Looping_Roll = 0; |
Looping_Nick = 0; |
} |
730,16 → 746,16 |
tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll); |
tmp_long /= 16; |
tmp_long2 /= 16; |
if((MaxStickNick > 15) || (MaxStickRoll > 15)) |
if((MaxStickNick > 15) || (MaxStickRoll > 15) || (abs(PPM_in_gier) > 25)) |
{ |
tmp_long /= 3; |
tmp_long2 /= 3; |
} |
if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25) |
{ |
tmp_long /= 3; |
tmp_long2 /= 3; |
} |
//if(abs(PPM_in_gier) > 25) |
// { |
// tmp_long /= 3; |
// tmp_long2 /= 3; |
// } |
|
#define AUSGLEICH 32 |
if(tmp_long > AUSGLEICH) tmp_long = AUSGLEICH; |
774,7 → 790,7 |
LageKorrekturNick = ausgleichNick / ABGLEICH_ANZAHL; |
LageKorrekturRoll = ausgleichRoll / ABGLEICH_ANZAHL; |
|
if((MaxStickNick > 15) || (MaxStickRoll > 15) || (abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)) |
if((MaxStickNick > 15) || (MaxStickRoll > 15) || (abs(PPM_in_gier) > 25)) |
{ |
LageKorrekturNick /= 2; |
LageKorrekturRoll /= 2; |
927,7 → 943,7 |
{ |
updKompass = 49; |
|
if ((MaxStickNick < 75) && (MaxStickRoll < 75)) // Bei extremen Flugmanövern keine Kompassauswertung |
if ((MaxStickNick < 50) && (MaxStickRoll < 50)) // Bei extremen Flugmanövern keine Kompassauswertung |
{ |
KompassValue = heading_MM3(); |
KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180; |