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 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |