Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 976 → Rev 1256

/branches/V0.70d HexaLotte/fc.c
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
}