149,6 → 149,7 |
long GIER_GRAD_FAKTOR = 1291; |
signed int KopplungsteilNickRoll,KopplungsteilRollNick; |
|
|
void Piep(unsigned char Anzahl) |
{ |
while(Anzahl--) |
198,8 → 199,8 |
StartNeutralNick = AdNeutralNick; |
if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) > 4) |
{ |
NeutralAccY = abs(Mittelwert_AccRoll) / ACC_AMPLIFY; |
NeutralAccX = abs(Mittelwert_AccNick) / ACC_AMPLIFY; |
NeutralAccY = abs(Mittelwert_AccRoll) / (2*ACC_AMPLIFY); |
NeutralAccX = abs(Mittelwert_AccNick) / (2*ACC_AMPLIFY); |
NeutralAccZ = Aktuell_az; |
} |
else |
267,6 → 268,14 |
NaviCntAcc++; |
IntegralAccZ += Aktuell_az - NeutralAccZ; |
|
if(abs(Mittelwert_AccRoll > 50)) { DebugOut.Analog[16]++; DebugOut.Analog[17] = Mittelwert_AccRoll;}; |
|
//++++++++++++++++++++++++++++++++++++++++++++++++ |
// ADC einschalten |
AdReady = 0; |
ANALOG_ON; |
//++++++++++++++++++++++++++++++++++++++++++++++++ |
|
if(Mess_IntegralRoll > 93000L) winkel_roll = 93000L; |
else if(Mess_IntegralRoll <-93000L) winkel_roll = -93000L; |
else winkel_roll = Mess_IntegralRoll; |
368,11 → 377,6 |
if(AdWertNick > 2015) MesswertNick = +2000; |
} |
|
//++++++++++++++++++++++++++++++++++++++++++++++++ |
// ADC einschalten |
ANALOG_ON; |
//++++++++++++++++++++++++++++++++++++++++++++++++ |
|
Integral_Gier = Mess_Integral_Gier; |
IntegralNick = Mess_IntegralNick; |
IntegralRoll = Mess_IntegralRoll; |
546,7 → 550,7 |
void MotorRegler(void) |
//############################################################################ |
{ |
int motorwert,pd_ergebnis, pd_ergebnis_nick,pd_ergebnis_roll,h,tmp_int; |
int motorwert,pd_ergebnis_nick,pd_ergebnis_roll,h,tmp_int; |
int GierMischanteil,GasMischanteil; |
static long SummeNick=0,SummeRoll=0; |
static long sollGier = 0,tmp_long,tmp_long2; |
926,7 → 930,8 |
tmp_long /= 3; |
tmp_long2 /= 3; |
} |
#define AUSGLEICH 32 |
|
#define AUSGLEICH 32 |
if(tmp_long > AUSGLEICH) tmp_long = AUSGLEICH; |
if(tmp_long < -AUSGLEICH) tmp_long =-AUSGLEICH; |
if(tmp_long2 > AUSGLEICH) tmp_long2 = AUSGLEICH; |
1338,7 → 1343,6 |
if(GasMischanteil > (MAX_GAS - 20) * STICK_GAIN) GasMischanteil = (MAX_GAS - 20) * STICK_GAIN; |
|
|
#ifdef OCTO |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Mischer und PI-Regler |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1391,65 → 1395,64 |
if(pd_ergebnis_roll > tmp_int) pd_ergebnis_roll = tmp_int; |
if(pd_ergebnis_roll < -tmp_int) pd_ergebnis_roll = -tmp_int; |
|
#ifndef OCTO |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Quadro-Mischer |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
motorwert = GasMischanteil + pd_ergebnis_nick + GierMischanteil; // Mischer |
motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS); |
Motor_Vorne = motorwert; |
|
motorwert = GasMischanteil - pd_ergebnis_nick + GierMischanteil; |
motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS); |
Motor_Hinten = motorwert; |
|
motorwert = GasMischanteil + pd_ergebnis_roll - GierMischanteil; |
motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS); |
Motor_Links = motorwert; |
|
motorwert = GasMischanteil - pd_ergebnis_roll - GierMischanteil; |
motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS); |
Motor_Rechts = motorwert; |
// +++++++++++++++++++++++++++++++++++++++++++++++ |
|
#else |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Octo-Mischer |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
motorwert = GasMischanteil + pd_ergebnis_nick + pd_ergebnis_roll + GierMischanteil; |
motorwert /= STICK_GAIN; |
if ((motorwert < 0)) motorwert = 0; |
else if(motorwert > MAX_GAS) motorwert = MAX_GAS; |
if (motorwert < MIN_GAS) motorwert = MIN_GAS; |
motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS); |
Motor1 = motorwert; |
|
motorwert = GasMischanteil + pd_ergebnis_nick - pd_ergebnis_roll - GierMischanteil; |
motorwert /= STICK_GAIN; |
if ((motorwert < 0)) motorwert = 0; |
else if(motorwert > MAX_GAS) motorwert = MAX_GAS; |
if (motorwert < MIN_GAS) motorwert = MIN_GAS; |
motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS); |
Motor2 = motorwert; |
|
motorwert = GasMischanteil + pd_ergebnis_nick - pd_ergebnis_roll + GierMischanteil; |
motorwert /= STICK_GAIN; |
if ((motorwert < 0)) motorwert = 0; |
else if(motorwert > MAX_GAS) motorwert = MAX_GAS; |
if (motorwert < MIN_GAS) motorwert = MIN_GAS; |
motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS); |
Motor3 = motorwert; |
|
motorwert = GasMischanteil - pd_ergebnis_nick - pd_ergebnis_roll - GierMischanteil; |
motorwert /= STICK_GAIN; |
if ((motorwert < 0)) motorwert = 0; |
else if(motorwert > MAX_GAS) motorwert = MAX_GAS; |
if (motorwert < MIN_GAS) motorwert = MIN_GAS; |
motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS); |
Motor4 = motorwert; |
|
motorwert = GasMischanteil - pd_ergebnis_nick - pd_ergebnis_roll + GierMischanteil; |
motorwert /= STICK_GAIN; |
if ((motorwert < 0)) motorwert = 0; |
else if(motorwert > MAX_GAS) motorwert = MAX_GAS; |
if (motorwert < MIN_GAS) motorwert = MIN_GAS; |
motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS); |
Motor5 = motorwert; |
|
motorwert = GasMischanteil - pd_ergebnis_nick + pd_ergebnis_roll - GierMischanteil; |
motorwert /= STICK_GAIN; |
if ((motorwert < 0)) motorwert = 0; |
else if(motorwert > MAX_GAS) motorwert = MAX_GAS; |
if (motorwert < MIN_GAS) motorwert = MIN_GAS; |
motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS); |
Motor6 = motorwert; |
|
motorwert = GasMischanteil - pd_ergebnis_nick + pd_ergebnis_roll + GierMischanteil; |
motorwert /= STICK_GAIN; |
if ((motorwert < 0)) motorwert = 0; |
else if(motorwert > MAX_GAS) motorwert = MAX_GAS; |
if (motorwert < MIN_GAS) motorwert = MIN_GAS; |
motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS); |
Motor7 = motorwert; |
|
motorwert = GasMischanteil + pd_ergebnis_nick + pd_ergebnis_roll - GierMischanteil; |
motorwert /= STICK_GAIN; |
if ((motorwert < 0)) motorwert = 0; |
else if(motorwert > MAX_GAS) motorwert = MAX_GAS; |
if (motorwert < MIN_GAS) motorwert = MIN_GAS; |
motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS); |
Motor8 = motorwert; |
// +++++++++++++++++++++++++++++++++++++++++++++++ |
#endif |
/* |
if(Poti1 > 20) Motor1 = 0; |
if(Poti1 > 90) Motor6 = 0; |
1457,91 → 1460,3 |
if(Poti1 > 200) Motor7 = 0; |
*/ |
} |
#else |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Mischer und PI-Regler |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
DebugOut.Analog[7] = GasMischanteil; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Gier-Anteil |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
#define MUL_G 1.0 |
GierMischanteil = MesswertGier - sollGier * STICK_GAIN; // Regler für Gier |
//GierMischanteil = 0; |
#define MIN_GIERGAS (40*STICK_GAIN) // unter diesem Gaswert trotzdem Gieren |
if(GasMischanteil > MIN_GIERGAS) |
{ |
if(GierMischanteil > (GasMischanteil / 2)) GierMischanteil = GasMischanteil / 2; |
if(GierMischanteil < -(GasMischanteil / 2)) GierMischanteil = -(GasMischanteil / 2); |
} |
else |
{ |
if(GierMischanteil > (MIN_GIERGAS / 2)) GierMischanteil = MIN_GIERGAS / 2; |
if(GierMischanteil < -(MIN_GIERGAS / 2)) GierMischanteil = -(MIN_GIERGAS / 2); |
} |
tmp_int = MAX_GAS*STICK_GAIN; |
if(GierMischanteil > ((tmp_int - GasMischanteil))) GierMischanteil = ((tmp_int - GasMischanteil)); |
if(GierMischanteil < -((tmp_int - GasMischanteil))) GierMischanteil = -((tmp_int - GasMischanteil)); |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Nick-Achse |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
DiffNick = MesswertNick - StickNick; // Differenz bestimmen |
if(IntegralFaktor) SummeNick += IntegralNickMalFaktor - StickNick; // I-Anteil bei Winkelregelung |
else SummeNick += DiffNick; // I-Anteil bei HH |
if(SummeNick > (STICK_GAIN * 16000L)) SummeNick = (STICK_GAIN * 16000L); |
if(SummeNick < -(16000L * STICK_GAIN)) SummeNick = -(16000L * STICK_GAIN); |
pd_ergebnis = DiffNick + SummeNick / Ki; // 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 |
motorwert /= STICK_GAIN; |
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; |
motorwert /= STICK_GAIN; |
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; // Differenz bestimmen |
if(IntegralFaktor) SummeRoll += IntegralRollMalFaktor - StickRoll;// I-Anteil bei Winkelregelung |
else SummeRoll += DiffRoll; // I-Anteil bei HH |
if(SummeRoll > (STICK_GAIN * 16000L)) SummeRoll = (STICK_GAIN * 16000L); |
if(SummeRoll < -(16000L * STICK_GAIN)) SummeRoll = -(16000L * STICK_GAIN); |
pd_ergebnis = DiffRoll + SummeRoll / Ki; // 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; |
motorwert /= STICK_GAIN; |
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; |
motorwert /= STICK_GAIN; |
if ((motorwert < 0)) motorwert = 0; |
else if(motorwert > MAX_GAS) motorwert = MAX_GAS; |
if (motorwert < MIN_GAS) motorwert = MIN_GAS; |
Motor_Rechts = motorwert; |
// +++++++++++++++++++++++++++++++++++++++++++++++ |
//Motor_Hinten = 0; |
//Motor_Vorne = 0; |
//Motor_Links = 0; |
//Motor_Rechts = 0; |
} |
|
#endif |