89,7 → 89,11 |
float IntegralFaktor; |
volatile int DiffNick,DiffRoll; |
int Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0; |
#ifdef HEXAKOPTER |
volatile unsigned char Motor_VorneLinks,Motor_VorneRechts,Motor_HintenLinks,Motor_HintenRechts,Motor_Rechts,Motor_Links, Count; |
#else |
volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count; |
#endif |
volatile unsigned char SenderOkay = 0; |
int StickNick = 0,StickRoll = 0,StickGier = 0,StickGas = 0; |
char MotorenEin = 0; |
227,7 → 231,7 |
MesswertNick = (signed int) AdWertNick - AdNeutralNick; |
|
//DebugOut.Analog[26] = MesswertNick; |
DebugOut.Analog[28] = MesswertRoll; |
//DebugOut.Analog[28] = MesswertRoll; |
|
// Beschleunigungssensor ++++++++++++++++++++++++++++++++++++++++++++++++ |
Mittelwert_AccNick = ((long)Mittelwert_AccNick * 1 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 2L; |
376,6 → 380,18 |
{ |
if(!MotorenEin) |
{ |
#ifdef HEXAKOPTER |
Motor_HintenLinks = 0; |
Motor_HintenRechts = 0; |
Motor_VorneLinks = 0; |
Motor_VorneRechts = 0; |
Motor_Rechts = 0; |
Motor_Links = 0; |
if(MotorTest[0]) Motor_VorneLinks = Motor_VorneRechts = MotorTest[0]; |
if(MotorTest[1]) Motor_HintenLinks = Motor_HintenRechts = MotorTest[1]; |
if(MotorTest[2]) Motor_Links = MotorTest[2]; |
if(MotorTest[3]) Motor_Rechts = MotorTest[3]; |
#else |
Motor_Hinten = 0; |
Motor_Vorne = 0; |
Motor_Rechts = 0; |
384,14 → 400,23 |
if(MotorTest[1]) Motor_Hinten = MotorTest[1]; |
if(MotorTest[2]) Motor_Links = MotorTest[2]; |
if(MotorTest[3]) Motor_Rechts = MotorTest[3]; |
#endif |
MikroKopterFlags &= ~(FLAG_MOTOR_RUN | FLAG_FLY); |
} else MikroKopterFlags |= FLAG_MOTOR_RUN; |
|
#ifdef HEXAKOPTER |
DebugOut.Analog[10] = Motor_VorneLinks; |
DebugOut.Analog[11] = Motor_VorneRechts; |
DebugOut.Analog[12] = Motor_HintenLinks; |
DebugOut.Analog[13] = Motor_HintenRechts; |
DebugOut.Analog[14] = Motor_Links; |
DebugOut.Analog[15] = Motor_Rechts; |
#else |
DebugOut.Analog[12] = Motor_Vorne; |
DebugOut.Analog[13] = Motor_Hinten; |
DebugOut.Analog[14] = Motor_Links; |
DebugOut.Analog[15] = Motor_Rechts; |
|
#endif |
//Start I2C Interrupt Mode |
twi_state = 0; |
motor = 0; |
454,6 → 479,7 |
{ |
int motorwert,pd_ergebnis,h,tmp_int; |
int GierMischanteil,GasMischanteil; |
int NickMischanteil, RollMischanteil; |
static long SummeNick=0,SummeRoll=0; |
static long sollGier = 0,tmp_long,tmp_long2; |
static long IntegralFehlerNick = 0; |
1086,8 → 1112,9 |
DebugOut.Analog[6] = (Mess_Integral_Hoch / 512); |
DebugOut.Analog[8] = KompassValue; |
DebugOut.Analog[9] = UBat; |
DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR; |
DebugOut.Analog[10] = SenderOkay; |
DebugOut.Analog[28] = ErsatzKompass / GIER_GRAD_FAKTOR; |
DebugOut.Analog[29] = SenderOkay; |
|
//DebugOut.Analog[16] = Mittelwert_AccHoch; |
// DebugOut.Analog[17] = FromNaviCtrl_Value.Distance; |
// DebugOut.Analog[18] = (int)FromNaviCtrl_Value.OsdBar; |
1231,24 → 1258,13 |
if(SummeNick > (STICK_GAIN * 16000L)) SummeNick = (STICK_GAIN * 16000L); |
if(SummeNick < -(16000L * STICK_GAIN)) SummeNick = -(16000L * STICK_GAIN); |
pd_ergebnis = DiffNick + Ki * SummeNick; // 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; |
NickMischanteil = (pd_ergebnis*4) / 7; |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Roll-Achse |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1260,21 → 1276,87 |
pd_ergebnis = DiffRoll + Ki * SummeRoll; // 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; |
if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int; |
|
RollMischanteil = pd_ergebnis/3; |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Mischer für die Motorenanteile |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
|
#ifdef HEXAKOPTER |
// Motor Vorn Links |
motorwert = GasMischanteil + NickMischanteil + RollMischanteil/2 - 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_VorneLinks = motorwert; |
// Motor Vorn Rechts |
motorwert = GasMischanteil + NickMischanteil - RollMischanteil/2 + 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_VorneRechts = motorwert; |
// Motor Hinten Links |
motorwert = GasMischanteil - NickMischanteil + RollMischanteil/2 - 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_HintenLinks = motorwert; |
// Motor HintenRechts |
motorwert = GasMischanteil - NickMischanteil - RollMischanteil/2 + 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_HintenRechts = motorwert; |
// Motor Rechts |
motorwert = GasMischanteil - RollMischanteil - 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 Links |
motorwert = GasMischanteil + pd_ergebnis - GierMischanteil; |
motorwert = GasMischanteil + RollMischanteil + 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; |
#else |
// Motor Vorn |
motorwert = GasMischanteil + NickMischanteil + 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 - NickMischanteil + 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; |
// Motor Links |
motorwert = GasMischanteil + RollMischanteil - 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 = GasMischanteil - RollMischanteil - 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_Rechts = motorwert; |
#endif |
} |
|