Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 130 → Rev 243

/branches/GPS_BETA_chris2798_hallo2/fc.c
56,7 → 56,7
#include "main.h"
 
unsigned char h,m,s;
volatile unsigned char Timeout = 0;
volatile unsigned int I2CTimeout = 100;
volatile int MesswertNick,MesswertRoll,MesswertGier;
volatile int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0;
volatile int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch, NeutralAccX=0, NeutralAccY=0;
88,6 → 88,7
char MotorenEin = 0;
int HoehenWert = 0;
int SollHoehe = 0;
unsigned char Looping_Nick = 0,Looping_Roll = 0;
 
float Kp = FAKTOR_P;
float Ki = FAKTOR_I;
97,7 → 98,7
unsigned char Parameter_Hoehe_P = 16; // Wert : 0-32
unsigned char Parameter_Hoehe_ACC_Wirkung = 58; // Wert : 0-250
unsigned char Parameter_KompassWirkung = 64; // Wert : 0-250
unsigned char Parameter_Gyro_P = 50; // Wert : 10-250
unsigned char Parameter_Gyro_P = 150; // Wert : 10-250
unsigned char Parameter_Gyro_I = 150; // Wert : 0-250
unsigned char Parameter_Gier_P = 2; // Wert : 1-20
unsigned char Parameter_I_Faktor = 10; // Wert : 1-20
106,6 → 107,7
unsigned char Parameter_UserParam3 = 0;
unsigned char Parameter_UserParam4 = 0;
unsigned char Parameter_ServoNickControl = 100;
unsigned char Parameter_LoopGasLimit = 70;
struct mk_param_struct EE_Parameter;
 
void Piep(unsigned char Anzahl)
138,9 → 140,19
{
if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
}
AdNeutralNick= abs(MesswertNick);
AdNeutralRoll= abs(MesswertRoll);
AdNeutralGier= abs(MesswertGier);
 
if(PlatinenVersion == 10)
{
AdNeutralNick= abs(MesswertNick);
AdNeutralRoll= abs(MesswertRoll);
AdNeutralGier= abs(MesswertGier);
}
else
{
AdNeutralNick= abs(MesswertNick) / 2;
AdNeutralRoll= abs(MesswertRoll) / 2;
AdNeutralGier= abs(MesswertGier) / 2;
}
NeutralAccY = abs(Mittelwert_AccRoll) / ACC_AMPLIFY;
NeutralAccX = abs(Mittelwert_AccNick) / ACC_AMPLIFY;
NeutralAccZ = Aktuell_az;
157,6 → 169,7
HoeheD = 0;
Mess_Integral_Hoch = 0;
KompassStartwert = KompassValue;
//GPS_Neutral();
beeptime = 50;
}
 
188,15 → 201,16
// ADC einschalten
ANALOG_ON;
 
/*
//------------------------------------------------------------------------------
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--;
285,25 → 299,25
EE_Parameter.Kanalbelegung[K_POTI4] = 8;
EE_Parameter.GlobalConfig = 0;//CFG_HOEHENREGELUNG | /*CFG_HOEHEN_SCHALTER |*/ CFG_KOMPASS_AKTIV | CFG_KOMPASS_FIX;//0x01;
EE_Parameter.Hoehe_MinGas = 30;
EE_Parameter.MaxHoehe = 251; // Wert : 0-32 251 -> Poti1
EE_Parameter.Hoehe_P = 10; // Wert : 0-32
EE_Parameter.Luftdruck_D = 70; // Wert : 0-250
EE_Parameter.Hoehe_ACC_Wirkung = 30; // Wert : 0-250
EE_Parameter.Hoehe_Verstaerkung = 2; // Wert : 0-50
EE_Parameter.Stick_P = 4; //2 // Wert : 1-6
EE_Parameter.Stick_D = 8; //8 // Wert : 0-64
EE_Parameter.Gier_P = 16; // Wert : 1-20
EE_Parameter.MaxHoehe = 251; // Wert : 0-32 251 -> Poti1
EE_Parameter.Hoehe_P = 10; // Wert : 0-32
EE_Parameter.Luftdruck_D = 50; // Wert : 0-250
EE_Parameter.Hoehe_ACC_Wirkung = 50; // Wert : 0-250
EE_Parameter.Hoehe_Verstaerkung = 4; // Wert : 0-50
EE_Parameter.Stick_P = 4; //2 // Wert : 1-6
EE_Parameter.Stick_D = 8; //8 // Wert : 0-64
EE_Parameter.Gier_P = 16; // Wert : 1-20
EE_Parameter.Gas_Min = 15; // Wert : 0-32
EE_Parameter.Gas_Max = 250; // Wert : 33-250
EE_Parameter.GyroAccFaktor = 26; // Wert : 1-64
EE_Parameter.Gas_Max = 250; // Wert : 33-250
EE_Parameter.GyroAccFaktor = 26; // Wert : 1-64
EE_Parameter.KompassWirkung = 128; // Wert : 0-250
EE_Parameter.Gyro_P = 120; //80 // Wert : 0-250
EE_Parameter.Gyro_I = 150; // Wert : 0-250
EE_Parameter.UnterspannungsWarnung = 90; // Wert : 0-250
EE_Parameter.UnterspannungsWarnung = 94; // Wert : 0-250
EE_Parameter.NotGas = 35; // Wert : 0-250 // Gaswert bei Empangsverlust
EE_Parameter.NotGasZeit = 20; // Wert : 0-250 // Zeit bis auf NotGas geschaltet wird, wg. Rx-Problemen
EE_Parameter.UfoAusrichtung = 0; // X oder + Formation
EE_Parameter.I_Faktor = 5;
EE_Parameter.I_Faktor = 0;
EE_Parameter.UserParam1 = 0; //zur freien Verwendung
EE_Parameter.UserParam2 = 0; //zur freien Verwendung
EE_Parameter.UserParam3 = 0; //zur freien Verwendung
311,10 → 325,13
EE_Parameter.ServoNickControl = 100; // Wert : 0-250 // Stellung des Servos
EE_Parameter.ServoNickComp = 40; // Wert : 0-250 // Einfluss Gyro/Servo
EE_Parameter.ServoNickCompInvert = 0; // Wert : 0-250 // Richtung Einfluss Gyro/Servo
EE_Parameter.ServoNickMin = 50; // Wert : 0-250 // Anschlag
EE_Parameter.ServoNickMin = 50; // Wert : 0-250 // Anschlag
EE_Parameter.ServoNickMax = 150; // Wert : 0-250 // Anschlag
EE_Parameter.ServoNickRefresh = 5;
memcpy(EE_Parameter.Name, "Normal\0", 12);
EE_Parameter.LoopGasLimit = 50;
EE_Parameter.LoopThreshold = 90; // Wert: 0-250 Schwelle für Stickausschlag
EE_Parameter.LoopConfig = 0; // Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts / wird getrennt behandelt
memcpy(EE_Parameter.Name, "Sport\0", 12);
}
 
void DefaultKonstanten2(void)
326,6 → 343,7
EE_Parameter.Kanalbelegung[K_POTI1] = 5;
EE_Parameter.Kanalbelegung[K_POTI2] = 6;
EE_Parameter.Kanalbelegung[K_POTI3] = 7;
EE_Parameter.Kanalbelegung[K_POTI4] = 8;
EE_Parameter.GlobalConfig = 0;//CFG_HOEHENREGELUNG | /*CFG_HOEHEN_SCHALTER |*/ CFG_KOMPASS_AKTIV;//0x01;
EE_Parameter.Hoehe_MinGas = 30;
EE_Parameter.MaxHoehe = 251; // Wert : 0-32 251 -> Poti1
342,17 → 360,15
EE_Parameter.KompassWirkung = 128; // Wert : 0-250
EE_Parameter.Gyro_P = 175; //80 // Wert : 0-250
EE_Parameter.Gyro_I = 175; // Wert : 0-250
EE_Parameter.UnterspannungsWarnung = 90; // Wert : 0-250
EE_Parameter.UnterspannungsWarnung = 94; // Wert : 0-250
EE_Parameter.NotGas = 35; // Wert : 0-250 // Gaswert bei Empangsverlust
EE_Parameter.NotGasZeit = 20; // Wert : 0-250 // Zeit bis auf NotGas geschaltet wird, wg. Rx-Problemen
EE_Parameter.UfoAusrichtung = 0; // X oder + Formation
EE_Parameter.I_Faktor = 5;
EE_Parameter.I_Faktor = 0;
EE_Parameter.UserParam1 = 0; //zur freien Verwendung
EE_Parameter.UserParam2 = 0; //zur freien Verwendung
EE_Parameter.UserParam3 = 0; //zur freien Verwendung
EE_Parameter.UserParam4 = 0; //zur freien Verwendung
EE_Parameter.UserParam3 = 0; //zur freien Verwendung
EE_Parameter.UserParam4 = 0; //zur freien Verwendung
EE_Parameter.ServoNickControl = 100; // Wert : 0-250 // Stellung des Servos
EE_Parameter.ServoNickComp = 40; // Wert : 0-250 // Einfluss Gyro/Servo
EE_Parameter.ServoNickCompInvert = 0; // Wert : 0-250 // Richtung Einfluss Gyro/Servo
359,6 → 375,9
EE_Parameter.ServoNickMin = 50; // Wert : 0-250 // Anschlag
EE_Parameter.ServoNickMax = 150; // Wert : 0-250 // Anschlag
EE_Parameter.ServoNickRefresh = 5;
EE_Parameter.LoopGasLimit = 50;
EE_Parameter.LoopThreshold = 90; // Wert: 0-250 Schwelle für Stickausschlag
EE_Parameter.LoopConfig = 0; // Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts
memcpy(EE_Parameter.Name, "Kamera\0", 12);
}
 
382,16 → 401,10
CHK_POTI(Parameter_UserParam2,EE_Parameter.UserParam2,0,255);
CHK_POTI(Parameter_UserParam3,EE_Parameter.UserParam3,0,255);
CHK_POTI(Parameter_UserParam4,EE_Parameter.UserParam4,0,255);
 
unsigned char ServoNickComp; // Wert : 0-250 // Einfluss Gyro/Servo
unsigned char ServoNickCompInvert; // Wert : 0-250 // Richtung Einfluss Gyro/Servo
unsigned char ServoNickMin; // Wert : 0-250 // Anschlag
unsigned char ServoNickMax; // Wert : 0-250 // Anschlag
 
CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255);
CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255);
CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255);
CHK_POTI(Parameter_LoopGasLimit,EE_Parameter.LoopGasLimit,0,255);
 
Ki = (float) Parameter_I_Faktor * 0.0001;
MAX_GAS = EE_Parameter.Gas_Max;
408,8 → 421,8
int GierMischanteil,GasMischanteil;
static long SummeNick=0,SummeRoll=0;
static long sollGier = 0,tmp_long,tmp_long2;
static int IntegralFehlerNick = 0;
static int IntegralFehlerRoll = 0;
long IntegralFehlerNick = 0;
long IntegralFehlerRoll = 0;
static unsigned int RcLostTimer;
static unsigned char delay_neutral = 0;
static unsigned char delay_einschalten = 0,delay_ausschalten = 0;
426,13 → 439,19
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
GasMischanteil = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;
if(GasMischanteil < 0) GasMischanteil = 0;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Emfang schlecht
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(SenderOkay < 100)
{
if(!PcZugriff) beeptime = 500;
if(!PcZugriff)
{
if(BeepMuster == 0xffff)
{
beeptime = 15000;
BeepMuster = 0x0c00;
}
}
if(RcLostTimer) RcLostTimer--;
else
{
447,10 → 466,7
PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] = 0;
PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;
PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] = 0;
/* Poti1 = 65;
Poti2 = 48;
Poti3 = 0;
*/ }
}
else MotorenEin = 0;
}
else
483,7 → 499,6
if(++delay_neutral > 200) // nicht sofort
{
GRN_OFF;
SetNeutral();
MotorenEin = 0;
delay_neutral = 0;
modell_fliegt = 0;
496,12 → 511,13
if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 5;
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], setting); // aktiven Datensatz merken
}
ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
Piep(GetActiveParamSetNumber());
if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert?
{
if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
}
ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
SetNeutral();
Piep(GetActiveParamSetNumber());
}
}
else delay_neutral = 0;
562,7 → 578,6
StickRoll = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P;
StickRoll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
 
GyroFaktor = ((float)Parameter_Gyro_P + 10.0) / 256.0;
IntegralFaktor = ((float) Parameter_Gyro_I) / 44000;
 
569,8 → 584,57
if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor = 0;
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))
{
if(IntegralNick > 60000)
{
StickNick -= 8 * EE_Parameter.Stick_P;
if(IntegralNick > 80000) StickNick -= 16 * EE_Parameter.Stick_P;
}
else
if(IntegralNick < -60000)
{
StickNick += 8 * EE_Parameter.Stick_P;
if(IntegralNick > 80000) StickNick -= 16 * EE_Parameter.Stick_P;
}
if(IntegralRoll > 60000)
{
StickRoll -= 8 * EE_Parameter.Stick_P;
if(IntegralRoll > 80000) StickRoll -= 16 * EE_Parameter.Stick_P;
}
else
if(IntegralRoll < -60000)
{
StickRoll += 8 * EE_Parameter.Stick_P;
if(IntegralRoll > 80000) StickRoll -= 16 * EE_Parameter.Stick_P;
}
}
 
}
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Looping?
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_LINKS) ||
((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_RECHTS))
{
Looping_Roll = 1;
if(GasMischanteil > EE_Parameter.LoopGasLimit) GasMischanteil = EE_Parameter.LoopGasLimit;
}
else Looping_Roll = 0;
 
if(((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_OBEN) ||
((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_UNTEN))
{
Looping_Nick = 1;
Looping_Roll = 0;
if(GasMischanteil > EE_Parameter.LoopGasLimit) GasMischanteil = EE_Parameter.LoopGasLimit;
}
else Looping_Nick = 0;
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Bei Empfangsausfall im Flug
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(Notlandung)
580,6 → 644,8
StickRoll = 0;
GyroFaktor = 0.1;
IntegralFaktor = 0.005;
Looping_Roll = 0;
Looping_Nick = 0;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Gyro-Drift kompensieren
594,10 → 660,10
if(IntegralFehlerNick < -500/DRIFT_FAKTOR) AdNeutralNick--;
if(IntegralFehlerRoll > 500/DRIFT_FAKTOR) AdNeutralRoll++;
if(IntegralFehlerRoll < -500/DRIFT_FAKTOR) AdNeutralRoll--;
if(Mess_Integral_Gier2 > 500/DRIFT_FAKTOR) AdNeutralGier--;
if(Mess_Integral_Gier2 <-500/DRIFT_FAKTOR) AdNeutralGier++;
// if(Mess_Integral_Gier2 > 500/DRIFT_FAKTOR) AdNeutralGier--; // macht nur mit Referenz (Kompass Sinn)
// if(Mess_Integral_Gier2 <-500/DRIFT_FAKTOR) AdNeutralGier++; // macht nur mit Referenz (Kompass Sinn)
ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
Mess_IntegralNick2 = IntegralNick;
Mess_IntegralNick2 = IntegralNick;
Mess_IntegralRoll2 = IntegralRoll;
Mess_Integral_Gier2 = Integral_Gier;
ANALOG_ON; // ADC einschalten
605,7 → 671,11
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Integrale auf ACC-Signal abgleichen
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick) / 16;
if(EE_Parameter.GlobalConfig & CFG_GPS_AKTIV){
 
tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick) / 16;
tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll) / 16;
#define AUSGLEICH 500
if(tmp_long > AUSGLEICH) tmp_long = AUSGLEICH;
615,18 → 685,45
ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
Mess_IntegralNick -= tmp_long;
Mess_IntegralRoll -= tmp_long2;
}else{
if(IntegralFaktor && !Looping_Nick && !Looping_Roll)
{
tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick);
if(labs(Mittelwert_AccNick) < 200) tmp_long /= 8;
else tmp_long /= 16;
tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll);
if(labs(Mittelwert_AccRoll) < 200) tmp_long2 /= 8;
else tmp_long2 /= 16;
 
#define AUSGLEICH 500
if(tmp_long > AUSGLEICH) tmp_long = AUSGLEICH;
if(tmp_long < -AUSGLEICH) tmp_long =-AUSGLEICH;
if(tmp_long2 > AUSGLEICH) tmp_long2 = AUSGLEICH;
if(tmp_long2 <-AUSGLEICH) tmp_long2 =-AUSGLEICH;
}
else
{
tmp_long = 0;
tmp_long2 = 0;
}
ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
Mess_IntegralNick -= tmp_long;
Mess_IntegralRoll -= tmp_long2;}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Gieren
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
sollGier = StickGier;
if(abs(StickGier) > 35)
if(abs(StickGier) > 20) // war 35
{
if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1;
}
tmp_int = EE_Parameter.Gier_P * (sollGier * abs(sollGier)) / 256; // expo
tmp_int = EE_Parameter.Gier_P * (StickGier * abs(StickGier)) / 512; // expo y = ax + bx²
tmp_int += (EE_Parameter.Gier_P * StickGier) / 4;
sollGier = tmp_int;
Mess_Integral_Gier -= tmp_int;
if(Mess_Integral_Gier > 30000) Mess_Integral_Gier = 30000; // begrenzen
if(Mess_Integral_Gier <-30000) Mess_Integral_Gier =-30000;
if(Mess_Integral_Gier > 25000) Mess_Integral_Gier = 25000; // begrenzen
if(Mess_Integral_Gier <-25000) Mess_Integral_Gier =-25000;
ANALOG_ON; // ADC einschalten
 
633,11 → 730,10
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Kompass
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//KompassValue = 12;
if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV))
{
int w,v;
static int SignalSchlecht = 0;
static int SignalSchlecht = 0;
w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln
v = abs(IntegralRoll /512);
if(v > w) w = v; // grösste Neigung ermitteln
658,35 → 754,55
else SignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// GPS
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(EE_Parameter.GlobalConfig & CFG_GPS_AKTIV)
{
 
gps_p = Parameter_UserParam1;
gps_d = Parameter_UserParam2;
skal = Parameter_UserParam3;
komp_dreh = Parameter_UserParam4*45;
if(EE_Parameter.GlobalConfig & CFG_GPS_AKTIV){
 
gps_main();
gpsmax=40; //maximale GPS Steuerausschläge
 
if (MotorenEin == 1 && gps_gethome == 0 && actualPos.state != 0){ //speichert GPS-Home-Position
gps_home_x = actualPos.northing;
gps_home_y = actualPos.easting;
home_height = HoehenWert+50; //30
 
beeptime = 80;
gps_gethome = 1;
}
else
{
GPS_Nick = 0;
GPS_Roll = 0;
 
if (MotorenEin ==0 && gps_gethome ==1)
{gps_gethome=0;}
 
 
if (Poti1>0) {
 
gps_main();
}else if (Poti1>0 && actualPos.state==0){beeptime = 80;}
if (Poti1==0){
gps_getpos=5;
GPS_Nick=0;
GPS_Roll=0;
}
if (Poti2==0){homing_state=0;}
}
 
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Debugwerte zuordnen
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
DebugOut.Sekunden++;
if(!TimerWerteausgabe--)
{
TimerWerteausgabe = 49;
// DebugOut.Analog[0] = MesswertNick;
// DebugOut.Analog[1] = MesswertRoll;
// DebugOut.Analog[2] = MesswertGier;
DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor;
DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor;
DebugOut.Analog[2] = Mittelwert_AccNick;
694,8 → 810,23
DebugOut.Analog[4] = MesswertGier;
DebugOut.Analog[5] = HoehenWert;
DebugOut.Analog[6] = (Mess_Integral_Hoch / 512);
DebugOut.Analog[7] = GasMischanteil;
DebugOut.Analog[8] = KompassValue;
DebugOut.Analog[10] = gps_h_d1;
DebugOut.Analog[11] = gps_h_d2;
 
/* 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;
707,13 → 838,15
// DebugOut.Analog[4] = hoehenregler;
}
 
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
MesswertNick = IntegralNick * IntegralFaktor + MesswertNick * GyroFaktor;
MesswertRoll = IntegralRoll * IntegralFaktor + MesswertRoll * GyroFaktor;
MesswertGier = MesswertGier * (GyroFaktor/2) + Integral_Gier * IntegralFaktor;
if(Looping_Nick) MesswertNick = MesswertNick * GyroFaktor;
else MesswertNick = IntegralNick * IntegralFaktor + MesswertNick * GyroFaktor;
if(Looping_Roll) MesswertRoll = MesswertRoll * GyroFaktor;
else MesswertRoll = IntegralRoll * IntegralFaktor + MesswertRoll * GyroFaktor;
// MesswertGier = MesswertGier * (GyroFaktor/2) + Integral_Gier * IntegralFaktor;
MesswertGier = MesswertGier * (GyroFaktor) + Integral_Gier * IntegralFaktor/2;
 
// Maximalwerte abfangen
#define MAX_SENSOR 2048
772,35 → 905,46
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Mischer und PI-Regler
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
DebugOut.Analog[7] = GasMischanteil;
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Gier-Anteil
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
GierMischanteil = MesswertGier - sollGier; // Regler für Gier
#define MUL_G 1.0
GierMischanteil = MesswertGier - sollGier; // Regler für Gier
//GierMischanteil = 0;
if(GierMischanteil > (MUL_G * GasMischanteil)) GierMischanteil = MUL_G * GasMischanteil;
if(GierMischanteil < -(MUL_G * GasMischanteil)) GierMischanteil = -(MUL_G * GasMischanteil);
if(GierMischanteil > 100) GierMischanteil = 100;
if(GierMischanteil < -100) GierMischanteil = -100;
 
if(GasMischanteil < 20) GierMischanteil = 0;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Nick-Achse
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
DiffNick = Kp * (MesswertNick - (StickNick - GPS_Nick)); // Differenz bestimmen
SummeNick += DiffNick; // I-Anteil
if(SummeNick > 0) SummeNick-= (abs(SummeNick)/256 + 1); else SummeNick += abs(SummeNick)/256 + 1;
if(SummeNick > 16000) SummeNick = 16000;
if(SummeNick < -16000) SummeNick = -16000;
pd_ergebnis = DiffNick + Ki * SummeNick; // PI-Regler für Nick
pd_ergebnis = DiffNick;// + Ki * SummeNick; // PI-Regler für Nick
// Motor Vorn
#define MUL 2
if(pd_ergebnis > MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis = MUL * (GasMischanteil + abs(GierMischanteil));
if(pd_ergebnis < -MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis = -MUL * (GasMischanteil + abs(GierMischanteil));
 
motorwert = GasMischanteil + pd_ergebnis + GierMischanteil; // Mischer
if ((motorwert < 0) | (GasMischanteil < 10)) motorwert = 0;
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;
if ((motorwert < 0) | (GasMischanteil < 10)) motorwert = 0;
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
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
809,20 → 953,21
if(SummeRoll > 0) SummeRoll-= (abs(SummeRoll)/256 + 1); else SummeRoll += abs(SummeRoll)/256 + 1;
if(SummeRoll > 16000) SummeRoll = 16000;
if(SummeRoll < -16000) SummeRoll = -16000;
pd_ergebnis = DiffRoll + Ki * SummeRoll; // PI-Regler für Roll
pd_ergebnis = DiffRoll;// + Ki * SummeRoll; // PI-Regler für Roll
if(pd_ergebnis > MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis = MUL * (GasMischanteil + abs(GierMischanteil));
if(pd_ergebnis < -MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis = -MUL * (GasMischanteil + abs(GierMischanteil));
// Motor Links
motorwert = GasMischanteil + pd_ergebnis - GierMischanteil;
if ((motorwert < 0) | (GasMischanteil < 10)) motorwert = 0;
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;
if ((motorwert < 0) | (GasMischanteil < 10)) motorwert = 0;
if ((motorwert < 0)) motorwert = 0;
else if(motorwert > MAX_GAS) motorwert = MAX_GAS;
if (motorwert < MIN_GAS) motorwert = MIN_GAS;
Motor_Rechts = motorwert;
// +++++++++++++++++++++++++++++++++++++++++++++++
 
}