Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 517 → Rev 518

/branches/Flight-Ctrl_V0_67g_3_GPS_work_Jochen/fc.c
79,10 → 79,16
unsigned char HoehenReglerAktiv = 0;
long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L;
 
unsigned char blinkcount_LED1 = 0;//Hilfszähler für die blinkende LED (310807Kr)
unsigned char LED1_TOTALTIME = 0;//Parameter für Blinkverhalten von LED1 (091207Kr)
unsigned char LED1_ONTIME = 0;//Parameter für Blinkverhalten von LED1 (091207Kr)
unsigned int modell_fliegt_gps = 0;//(030907Kr)
 
 
float GyroFaktor;
float IntegralFaktor;
volatile int DiffNick,DiffRoll;
int Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0;
int Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0, Poti5 = 0, Poti6 = 0, Poti7 = 0, Poti8 = 0; //PPM24-Erweiterung (121007Kr)
volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count;
unsigned char MotorWert[5];
volatile unsigned char SenderOkay = 0;
181,7 → 187,6
HoeheD = 0;
Mess_Integral_Hoch = 0;
KompassStartwert = KompassValue;
GPS_Neutral();
beeptime = 50;
Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L;
286,21 → 291,36
IntegralNick2 = Mess_IntegralNick2;
IntegralRoll2 = Mess_IntegralRoll2;
 
if(EE_Parameter.GlobalConfig & CFG_DREHRATEN_BEGRENZER && !Looping_Nick && !Looping_Roll)
//------------------------------------------------------------------------------
if(EE_Parameter.GlobalConfig & CFG_DREHRATEN_BEGRENZER && !Looping_Nick && !Looping_Roll && !(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD)) //um Heading_Hold erweitert //(071107Kr)
{
if(MesswertNick > 200) MesswertNick += 4 * (MesswertNick - 200);
else if(MesswertNick < -200) MesswertNick += 4 * (MesswertNick + 200);
if(MesswertRoll > 200) MesswertRoll += 4 * (MesswertRoll - 200);
else if(MesswertRoll < -200) MesswertRoll += 4 * (MesswertRoll + 200);
}
}
//------------------------------------------------------------------------------
if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--;
if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--;
if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--;
if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--;
if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;
//PPM24-Erweiterung (121007Kr)
if(Poti5 < PPM_in[9] + 110) Poti5++; else if(Poti5 > PPM_in[9] + 110 && Poti5) Poti5--;
if(Poti6 < PPM_in[10] + 110) Poti6++; else if(Poti6 > PPM_in[10] + 110 && Poti6) Poti6--;
if(Poti7 < PPM_in[11] + 110) Poti7++; else if(Poti7 > PPM_in[11] + 110 && Poti7) Poti7--;
if(Poti8 < PPM_in[12] + 110) Poti8++; else if(Poti8 > PPM_in[12] + 110 && Poti8) Poti8--;
if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;
if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;
if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;
if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;
//PPM24-Erweiterung (121007Kr)
if(Poti5 < 0) Poti5 = 0; else if(Poti5 > 255) Poti5 = 255;
if(Poti6 < 0) Poti6 = 0; else if(Poti6 > 255) Poti6 = 255;
if(Poti7 < 0) Poti7 = 0; else if(Poti7 > 255) Poti7 = 255;
if(Poti8 < 0) Poti8 = 0; else if(Poti8 > 255) Poti8 = 255;
}
 
//############################################################################
322,10 → 342,24
if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--;
if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--;
if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--;
//PPM24-Erweiterung (121007Kr)
if(Poti5 < PPM_in[9] + 110) Poti5++; else if(Poti5 > PPM_in[9] + 110 && Poti5) Poti5--;
if(Poti6 < PPM_in[10] + 110) Poti6++; else if(Poti6 > PPM_in[10] + 110 && Poti6) Poti6--;
if(Poti7 < PPM_in[11] + 110) Poti7++; else if(Poti7 > PPM_in[11] + 110 && Poti7) Poti7--;
if(Poti8 < PPM_in[12] + 110) Poti8++; else if(Poti8 > PPM_in[12] + 110 && Poti8) Poti8--;
if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;
if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;
if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;
if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;
//PPM24-Erweiterung (121007Kr)
if(Poti5 < 0) Poti5 = 0; else if(Poti5 > 255) Poti5 = 255;
if(Poti6 < 0) Poti6 = 0; else if(Poti6 > 255) Poti6 = 255;
if(Poti7 < 0) Poti7 = 0; else if(Poti7 > 255) Poti7 = 255;
if(Poti8 < 0) Poti8 = 0; else if(Poti8 > 255) Poti8 = 255;
 
Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
348,10 → 382,10
if(MotorTest[3]) Motor_Rechts = MotorTest[3];
}
 
DebugOut.Analog[12] = Motor_Vorne;
DebugOut.Analog[13] = Motor_Hinten;
DebugOut.Analog[14] = Motor_Links;
DebugOut.Analog[15] = Motor_Rechts;
// DebugOut.Analog[12] = Motor_Vorne;
// DebugOut.Analog[13] = Motor_Hinten;
// DebugOut.Analog[14] = Motor_Links;
// DebugOut.Analog[15] = Motor_Rechts;
 
//Start I2C Interrupt Mode
twi_state = 0;
466,6 → 500,7
if(GasMischanteil > 40)
{
if(modell_fliegt < 0xffff) modell_fliegt++;
modell_fliegt_gps = modell_fliegt;// (030907Kr)
}
if((modell_fliegt < 200) || (GasMischanteil < 40))
{
610,7 → 645,8
if(GyroFaktor < 0) GyroFaktor = 0;
if(IntegralFaktor < 0) IntegralFaktor = 0;
// greift in den Stick ein, um ungewolltes überschlagen zu verhindern
if(!(EE_Parameter.LoopConfig & CFG_LOOP_LINKS) && !(EE_Parameter.LoopConfig & CFG_LOOP_RECHTS))
//bei aktiviertem GPS dürfen die Stickwerte nicht beeinflusst werden, da ansonsten ein Neulernen der Sollposition stattfinden könnte //(051107Kr)
if(!(EE_Parameter.LoopConfig & CFG_LOOP_LINKS) && !(EE_Parameter.LoopConfig & CFG_LOOP_RECHTS) && !(EE_Parameter.GlobalConfig & CFG_GPS_AKTIV) && !(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD)) //(051107Kr)
{
if(IntegralNick > 60000)
{
635,7 → 671,39
if(IntegralRoll > 80000) StickRoll -= 16 * EE_Parameter.Stick_P;
}
}
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// LED Stuff
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
LED1_TOTALTIME = Parameter_UserParam7;
LED1_ONTIME = Parameter_UserParam8;
LED1_OFF;
if(LED1_TOTALTIME >= 200)
{
LED1_ON;
blinkcount_LED1 = 0;
}
else if(blinkcount_LED1 >= LED1_TOTALTIME && LED1_TOTALTIME > 0 && MotorenEin == 1)
{
LED1_ON;
if(blinkcount_LED1 >= LED1_ONTIME + LED1_TOTALTIME)
{
LED1_OFF;
blinkcount_LED1 = 0;
}
}
blinkcount_LED1++;
 
//if(PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] > 75) LED1_ON; else LED1_OFF; //Erweiterung von Smartie aus dem MK-Forum vom 14.06.07. Dadurch können die Transistorausgänge J16 und J17 geschaltet werden. // (160607Kr)
//if(PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] > 75) LED2_ON; else LED2_OFF; //Erweiterung von Smartie aus dem MK-Forum vom 14.06.07. Dadurch können die Transistorausgänge J16 und J17 geschaltet werden. // (160607Kr)
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Looping?
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_LINKS) Looping_Links = 1;
785,15 → 853,15
// IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2;
 
 
DebugOut.Analog[17] = IntegralAccNick / 26;
DebugOut.Analog[18] = IntegralAccRoll / 26;
DebugOut.Analog[19] = IntegralFehlerNick;// / 26;
DebugOut.Analog[20] = IntegralFehlerRoll;// / 26;
DebugOut.Analog[21] = MittelIntegralNick / 26;
DebugOut.Analog[22] = MittelIntegralRoll / 26;
//DebugOut.Analog[17] = IntegralAccNick / 26;
//DebugOut.Analog[18] = IntegralAccRoll / 26;
//DebugOut.Analog[19] = IntegralFehlerNick;// / 26;
//DebugOut.Analog[20] = IntegralFehlerRoll;// / 26;
//DebugOut.Analog[21] = MittelIntegralNick / 26;
//DebugOut.Analog[22] = MittelIntegralRoll / 26;
//DebugOut.Analog[28] = ausgleichNick;
DebugOut.Analog[29] = ausgleichRoll;
DebugOut.Analog[30] = LageKorrekturRoll * 10;
//DebugOut.Analog[29] = ausgleichRoll;
//DebugOut.Analog[30] = LageKorrekturRoll * 10;
 
#define FEHLER_LIMIT (ABGLEICH_ANZAHL * 4)
#define FEHLER_LIMIT2 (ABGLEICH_ANZAHL * 16)
861,13 → 929,13
{
cnt = 0;
}
DebugOut.Analog[27] = ausgleichRoll;
//DebugOut.Analog[27] = ausgleichRoll;
if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
//if(cnt > 1) beeptime = 50;
if(IntegralFehlerRoll > FEHLER_LIMIT) AdNeutralRoll += cnt;
if(IntegralFehlerRoll < -FEHLER_LIMIT) AdNeutralRoll -= cnt;
DebugOut.Analog[23] = AdNeutralNick;//10*(AdNeutralNick - StartNeutralNick);
DebugOut.Analog[24] = 10*(AdNeutralRoll - StartNeutralRoll);
//DebugOut.Analog[23] = AdNeutralNick;//10*(AdNeutralNick - StartNeutralNick);
//DebugOut.Analog[24] = 10*(AdNeutralRoll - StartNeutralRoll);
}
else
{
929,7 → 997,22
else SignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// GPS
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(EE_Parameter.GlobalConfig & CFG_GPS_AKTIV)
{
P_GPS_Verstaerkung = Parameter_UserParam5;
D_GPS_Verstaerkung = Parameter_UserParam6;
gps_main();
}
else
{
GPS_Nick = 0;
GPS_Roll = 0;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Debugwerte zuordnen
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
936,40 → 1019,52
if(!TimerWerteausgabe--)
{
TimerWerteausgabe = 24;
 
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;
DebugOut.Analog[5] = HoehenWert;
DebugOut.Analog[6] =(Mess_Integral_Hoch / 512);
DebugOut.Analog[8] = KompassValue;
DebugOut.Analog[9] = UBat;
DebugOut.Analog[10] = SenderOkay;
DebugOut.Analog[16] = Mittelwert_AccHoch;
DebugOut.Analog[4] = Parameter_UserParam1;
DebugOut.Analog[5] = Parameter_UserParam2;
DebugOut.Analog[6] = Soll_Position_North;
DebugOut.Analog[7] = Soll_Position_East;
DebugOut.Analog[8] = KompassValue;
DebugOut.Analog[9] = UBat;
DebugOut.Analog[10] = SenderOkay;
DebugOut.Analog[11] = actualPos.GPSFix;
DebugOut.Analog[12] = (motor_rx[0]+motor_rx[1]+motor_rx[2]+motor_rx[3]); //Gesamtstrom
DebugOut.Analog[13] = actualPos.GSpeed; //Geschwindigkeit über Grund
DebugOut.Analog[14] = actualPos.northing;
DebugOut.Analog[15] = actualPos.easting;
DebugOut.Analog[16] = Poti3;
DebugOut.Analog[17] = actualPos.altitude;
DebugOut.Analog[18] = GPS_Home_North;
DebugOut.Analog[19] = GPS_Home_East;
DebugOut.Analog[20] = GPS_Positionsabweichung_North;
DebugOut.Analog[21] = GPS_Positionsabweichung_East;
DebugOut.Analog[22] = P_Einfluss_North;
DebugOut.Analog[23] = P_Einfluss_East;
DebugOut.Analog[24] = D_Einfluss_North;
DebugOut.Analog[25] = D_Einfluss_East;
DebugOut.Analog[26] = NORTH_MITTEL;
DebugOut.Analog[27] = EAST_MITTEL;
DebugOut.Analog[28] = GPS_Nick;
DebugOut.Analog[29] = GPS_Roll;
DebugOut.Analog[30] = StickNick;
DebugOut.Analog[31] = StickRoll;
//DebugOut.Analog[xx] = motor_rx[0]; //Motorstrom vorne
//DebugOut.Analog[xx] = motor_rx[1]; //Motorstrom hinten
//DebugOut.Analog[xx] = motor_rx[2]; //Motorstrom links
//DebugOut.Analog[xx] = motor_rx[3]; //Motorstrom rechts
//DebugOut.Analog[xx] = actualPos.velNorth;
//DebugOut.Analog[xx] = actualPos.velEast;
//DebugOut.Analog[xx] = Parameter_UserParam3;
//DebugOut.Analog[xx] = Parameter_UserParam4;
//DebugOut.Analog[xx] = StickGier;
//DebugOut.Analog[xx] = GPS_North;
//DebugOut.Analog[xx] = GPS_East;
 
/* DebugOut.Analog[16] = motor_rx[0];
DebugOut.Analog[17] = motor_rx[1];
DebugOut.Analog[18] = motor_rx[2];
DebugOut.Analog[19] = motor_rx[3];
DebugOut.Analog[20] = motor_rx[0] + motor_rx[1] + motor_rx[2] + motor_rx[3];
DebugOut.Analog[20] /= 14;
DebugOut.Analog[21] = motor_rx[4];
DebugOut.Analog[22] = motor_rx[5];
DebugOut.Analog[23] = motor_rx[6];
DebugOut.Analog[24] = motor_rx[7];
DebugOut.Analog[25] = motor_rx[4] + motor_rx[5] + motor_rx[6] + motor_rx[7];
*/
// DebugOut.Analog[9] = MesswertNick;
// DebugOut.Analog[9] = SollHoehe;
// DebugOut.Analog[10] = Mess_Integral_Gier / 128;
// DebugOut.Analog[11] = KompassStartwert;
// DebugOut.Analog[10] = Parameter_Gyro_I;
// DebugOut.Analog[10] = EE_Parameter.Gyro_I;
// DebugOut.Analog[9] = KompassRichtung;
// DebugOut.Analog[10] = GasMischanteil;
// DebugOut.Analog[3] = HoeheD * 32;
// DebugOut.Analog[4] = hoehenregler;
}
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
982,9 → 1077,9
// MesswertGier = MesswertGier * (GyroFaktor/2) + Integral_Gier * IntegralFaktor;
MesswertGier = MesswertGier * (GyroFaktor) + Integral_Gier * IntegralFaktor/2;
 
DebugOut.Analog[28] = MesswertRoll;
DebugOut.Analog[25] = IntegralRoll * IntegralFaktor;
DebugOut.Analog[31] = StickRoll;// / (26*IntegralFaktor);
//DebugOut.Analog[28] = MesswertRoll;
//DebugOut.Analog[25] = IntegralRoll * IntegralFaktor;
//DebugOut.Analog[31] = StickRoll;// / (26*IntegralFaktor);
 
// Maximalwerte abfangen
#define MAX_SENSOR 2048
1043,7 → 1138,7
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Mischer und PI-Regler
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
DebugOut.Analog[7] = GasMischanteil;
// DebugOut.Analog[7] = GasMischanteil;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Gier-Anteil
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++