87,6 → 87,7 |
int GierGyroFehler = 0; |
float GyroFaktor; |
float IntegralFaktor; |
// float IntegralFaktor_Gier; // MartinR |
volatile int DiffNick,DiffRoll; |
int Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0; |
volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count; |
135,7 → 136,7 |
unsigned char Parameter_ExternalControl; |
struct mk_param_struct EE_Parameter; |
signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20; |
int MaxStickNick = 0,MaxStickRoll = 0; |
int MaxStickNick = 0,MaxStickRoll = 0,stick_nick_neutral = 0,stick_roll_neutral = 0; // MartinR: stick_.._neutral hinzugefügt |
unsigned int modell_fliegt = 0; |
unsigned char MikroKopterFlags = 0; |
|
226,8 → 227,8 |
MesswertRoll = (signed int) AdWertRoll - AdNeutralRoll; |
MesswertNick = (signed int) AdWertNick - AdNeutralNick; |
|
//DebugOut.Analog[26] = MesswertNick; |
DebugOut.Analog[28] = MesswertRoll; |
// DebugOut.Analog[26] = MesswertNick; |
// DebugOut.Analog[28] = MesswertRoll; |
|
// Beschleunigungssensor ++++++++++++++++++++++++++++++++++++++++++++++++ |
Mittelwert_AccNick = ((long)Mittelwert_AccNick * 1 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 2L; |
246,7 → 247,9 |
if(ErsatzKompass >= (360L * GIER_GRAD_FAKTOR)) ErsatzKompass -= 360L * GIER_GRAD_FAKTOR; // 360° Umschlag |
if(ErsatzKompass < 0) ErsatzKompass += 360L * GIER_GRAD_FAKTOR; |
// Kopplungsanteil +++++++++++++++++++++++++++++++++++++ |
if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV)) |
if((EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) || (Parameter_UserParam1 > 50)) IntegralFaktor = 0; // Doppelt ?? MartinR |
|
if(!Looping_Nick && !Looping_Roll && IntegralFaktor && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV)) |
{ |
tmpl = (MesswertGierBias * Mess_IntegralNick) / 2048L; |
tmpl *= Parameter_AchsKopplung1; //125 |
348,9 → 351,13 |
if(PlatinenVersion >= 13) SucheGyroOffset(); |
// ADC auschalten, damit die Werte sich nicht während der Berechnung ändern |
ANALOG_OFF; |
MesswertNick = AdWertNick; |
MesswertRoll = AdWertRoll; |
MesswertGier = AdWertGier; |
// MesswertNick = AdWertNick; // original |
//MesswertNick = (signed int) AdWertNick - AdNeutralNick; // MartinR: weshhalb nicht so?? |
//MesswertRoll = AdWertRoll; // original |
//MesswertRoll = (signed int) AdWertRoll - AdNeutralRoll; // MartinR: weshhalb nicht so?? |
//MesswertGier = AdWertGier; // original |
//MesswertGier = (signed int) AdNeutralGier - AdWertGier; // MartinR: weshhalb nicht so?? |
|
Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick; |
Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll; |
Mittelwert_AccHoch = (long)AdWertAccHoch; |
439,7 → 446,10 |
CHK_POTI(Parameter_NaviGpsACC,EE_Parameter.NaviGpsACC,0,255); |
CHK_POTI(Parameter_ExternalControl,EE_Parameter.ExternalControl,0,255); |
|
Ki = (float) Parameter_I_Faktor * 0.0001; |
Ki = (float) Parameter_I_Faktor * 0.0001; |
|
if(!IntegralFaktor) Parameter_NaviGpsModeControl= 100; // MartinR: wenn HH dann GPS auf free- Mode |
|
MAX_GAS = EE_Parameter.Gas_Max; |
MIN_GAS = EE_Parameter.Gas_Min; |
} |
646,16 → 656,45 |
if(!NewPpmData-- || Notlandung) |
{ |
int tmp_int; |
static int stick_nick,stick_roll; |
static int stick_nick,stick_roll; // .._neutral hinzugefügt aber Deklaration siehe oben MartinR |
ParameterZuordnung(); |
stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4; |
|
|
if(Parameter_UserParam1 > 50) // MartinR: zweiter Stick_P Wert nur, wenn HH über Schalter aktiv ist |
{ |
stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * Parameter_UserParam2 - stick_nick_neutral) / 4; |
stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * Parameter_UserParam2 - stick_roll_neutral) / 4 ; |
} |
|
else |
{ |
stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4; |
stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4; |
} |
|
if(IntegralFaktor) |
{ |
stick_nick_neutral = stick_nick; // beim Umschalten auf HH wird derletzte Stickwert als Neutralposition verwendet, MartinR |
stick_roll_neutral = stick_roll; // beim Umschalten auf HH wird derletzte Stickwert als Neutralposition verwendet, MartinR |
// stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D; // MartinR: deaktiviert |
// stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D; // MartinR: deaktiviert |
|
StickNick = stick_nick - (GPS_Nick + GPS_Nick2); // MartinR: GPS nur im ACC-Mode wirksam |
StickRoll = stick_roll - (GPS_Roll + GPS_Roll2); // MartinR: GPS nur im ACC-Mode wirksam |
} |
else // wenn HH , MartinR |
{ |
// stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D; // MartinR: deaktiviert |
// stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D; // MartinR: deaktiviert |
StickNick = stick_nick; // MartinR: GPS nur im ACC-Mode wirksam |
StickRoll = stick_roll; // MartinR: GPS nur im ACC-Mode wirksam |
} |
|
stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D; |
StickNick = stick_nick - (GPS_Nick + GPS_Nick2); |
|
// StickNick = (StickNick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4; |
|
stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4; |
|
stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D; |
StickRoll = stick_roll - (GPS_Roll + GPS_Roll2); |
|
// StickRoll = (StickRoll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4; |
|
667,12 → 706,14 |
if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) > MaxStickRoll) |
MaxStickRoll = abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]); else MaxStickRoll--; |
*/ |
GyroFaktor = ((float)Parameter_Gyro_P + 10.0) / (256.0/STICK_GAIN); |
IntegralFaktor = ((float) Parameter_Gyro_I) / (44000 / STICK_GAIN); |
|
|
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//+ Digitale Steuerung per DubWise |
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
// To Do: Umschaltung auf HH deaktivieren //MartinR |
|
#define KEY_VALUE (Parameter_ExternalControl * 4) //(Poti3 * 8) |
if(DubWiseKeys[1]) beeptime = 10; |
if(DubWiseKeys[1] & DUB_KEY_UP) tmp_int = KEY_VALUE; else |
693,6 → 734,9 |
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//+ Analoge Steuerung per Seriell |
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
// To Do: Umschaltung auf HH deaktivieren //MartinR |
|
if(ExternControl.Config & 0x01 && Parameter_ExternalControl > 128) |
{ |
StickNick += (int) ExternControl.Nick * (int) EE_Parameter.Stick_P; |
701,9 → 745,19 |
ExternHoehenValue = (int) ExternControl.Hight * (int)EE_Parameter.Hoehe_Verstaerkung; |
if(ExternControl.Gas < StickGas) StickGas = ExternControl.Gas; |
} |
if(StickGas < 0) StickGas = 0; |
if(StickGas < 0) StickGas = 0; |
|
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//+ Reglermodus // MartinR |
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
GyroFaktor = ((float)Parameter_Gyro_P + 10.0) / (256.0/STICK_GAIN); |
IntegralFaktor = ((float) Parameter_Gyro_I) / (44000 / STICK_GAIN); |
//IntegralFaktor_Gier = ((float) Parameter_UserParam3) / (44000 / STICK_GAIN); // MartinR |
|
// if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor = 0; |
if((EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) || (Parameter_UserParam1 > 50)) IntegralFaktor = 0; // Doppelt!? Doppel wieder aktiviert, da Impuls beim Umschalten. MartinR |
|
if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor = 0; |
if(GyroFaktor < 0) GyroFaktor = 0; |
if(IntegralFaktor < 0) IntegralFaktor = 0; |
|
777,6 → 831,7 |
StickRoll = 0; |
GyroFaktor = (float) 100 / (256.0 / STICK_GAIN); |
IntegralFaktor = (float) 120 / (44000 / STICK_GAIN); |
// IntegralFaktor_Gier= (float) 120 / (44000 / STICK_GAIN);// MartinR |
Looping_Roll = 0; |
Looping_Nick = 0; |
} |
792,7 → 847,8 |
MittelIntegralNick2 += IntegralNick2; |
MittelIntegralRoll2 += IntegralRoll2; |
|
if(Looping_Nick || Looping_Roll) |
|
if(Looping_Nick || Looping_Roll || !IntegralFaktor) |
{ |
IntegralAccNick = 0; |
IntegralAccRoll = 0; |
800,6 → 856,16 |
MittelIntegralRoll = 0; |
MittelIntegralNick2 = 0; |
MittelIntegralRoll2 = 0; |
|
|
IntegralNick = 0; // MartinR: im HH-Modus alle unbenutzten Integratoren = 0 |
IntegralRoll = 0; // MartinR: im HH-Modus alle unbenutzten Integratoren = 0 |
Mess_IntegralNick = 0; // MartinR: im HH-Modus alle unbenutzten Integratoren = 0 |
Mess_IntegralRoll = 0; // MartinR: im HH-Modus alle unbenutzten Integratoren = 0 |
Mess_Integral_Gier = 0; // MartinR: im HH-Modus alle unbenutzten Integratoren = 0 |
Mess_Integral_Gier2 = 0; // MartinR: im HH-Modus alle unbenutzten Integratoren = 0 |
|
|
Mess_IntegralNick2 = Mess_IntegralNick; |
Mess_IntegralRoll2 = Mess_IntegralRoll; |
ZaehlMessungen = 0; |
808,7 → 874,7 |
} |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(!Looping_Nick && !Looping_Roll) |
if(!Looping_Nick && !Looping_Roll && IntegralFaktor) |
{ |
long tmp_long, tmp_long2; |
tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick); |
837,12 → 903,12 |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
if(ZaehlMessungen >= ABGLEICH_ANZAHL) |
if(ZaehlMessungen >= ABGLEICH_ANZAHL) // AbgleichAnzahl: fest definierter Wert |
{ |
static int cnt = 0; |
static char last_n_p,last_n_n,last_r_p,last_r_n; |
static long MittelIntegralNick_Alt,MittelIntegralRoll_Alt; |
if(!Looping_Nick && !Looping_Roll && !TrichterFlug) |
if(!Looping_Nick && !Looping_Roll && !TrichterFlug && IntegralFaktor) |
{ |
MittelIntegralNick /= ABGLEICH_ANZAHL; |
MittelIntegralRoll /= ABGLEICH_ANZAHL; |
905,7 → 971,7 |
|
#define FEHLER_LIMIT (ABGLEICH_ANZAHL * 4) |
#define FEHLER_LIMIT2 (ABGLEICH_ANZAHL * 16) |
#define BEWEGUNGS_LIMIT 20000 |
#define BEWEGUNGS_LIMIT 20000 |
// Nick +++++++++++++++++++++++++++++++++++++++++++++++++ |
cnt = 1;// + labs(IntegralFehlerNick) / 4096; |
if(labs(MittelIntegralNick_Alt - MittelIntegralNick) < BEWEGUNGS_LIMIT) |
1094,6 → 1160,10 |
DebugOut.Analog[19] = WinkelOut.CalcState; |
DebugOut.Analog[20] = ServoValue; |
|
|
// DebugOut.Analog[26] = MesswertNick; |
// DebugOut.Analog[28] = MesswertRoll; |
|
DebugOut.Analog[30] = GPS_Nick; |
DebugOut.Analog[31] = GPS_Roll; |
|
1129,11 → 1199,13 |
// Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
if(Looping_Nick) MesswertNick = MesswertNick * GyroFaktor; |
if(Looping_Nick || !IntegralFaktor) MesswertNick = MesswertNick * GyroFaktor; // erweitert um !Integralfaktor, MartinR |
else MesswertNick = IntegralNick * IntegralFaktor + MesswertNick * GyroFaktor; |
if(Looping_Roll) MesswertRoll = MesswertRoll * GyroFaktor; |
if(Looping_Roll || !IntegralFaktor) MesswertRoll = MesswertRoll * GyroFaktor; |
else MesswertRoll = IntegralRoll * IntegralFaktor + MesswertRoll * GyroFaktor; |
MesswertGier = MesswertGier * (2 * GyroFaktor) + Integral_Gier * IntegralFaktor / 2; |
|
MesswertGier = MesswertGier * (2 * GyroFaktor) + Integral_Gier * IntegralFaktor / 2; |
// MesswertGier = MesswertGier * (2 * GyroFaktor) + Integral_Gier * IntegralFaktor_Gier / 2; // Eigener IntegralFaktor für Gier, MartinR |
|
DebugOut.Analog[21] = MesswertNick; |
DebugOut.Analog[22] = MesswertRoll; |
1155,7 → 1227,8 |
//DruckOffsetSetting = OCR0B; |
GasMischanteil *= STICK_GAIN; |
|
if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung |
|
if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG) && !(Parameter_UserParam1 > 50)) // Höhenregelung // MartinR: Höhenregelung bei HH abschalten |
{ |
int tmp_int; |
if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER) // Regler wird über Schalter gesteuert |
1226,11 → 1299,18 |
// Nick-Achse |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
DiffNick = MesswertNick - StickNick; // Differenz bestimmen |
if(IntegralFaktor) SummeNick += IntegralNick * IntegralFaktor - StickNick; // I-Anteil bei Winkelregelung |
// if(IntegralFaktor) SummeNick += IntegralNick * IntegralFaktor - StickNick; // I-Anteil bei Winkelregelung // so war es, MartinR |
// das IntegralNick ist bereits im MesswertNick. Die SummeNick wird bei mir im ACC-Mode mit sowiso über Ki auf Null gesetzt. MartinR |
|
if(IntegralFaktor) SummeNick = (IntegralNick * IntegralFaktor - StickNick + (stick_nick_neutral/4)) / Ki ; // Startwert von SummeNick bei Umschaltung auf HH, MartinR |
// im ACC-Mode wird SummeNick nicht mehr verwendet. SummeNick im ACC-Mode ist nur der Startwert für SummeNick im HH-Mode, MartinR |
else SummeNick += DiffNick; // I-Anteil bei HH |
|
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 |
|
if(IntegralFaktor) pd_ergebnis = DiffNick; // PI-Regler im ACC-Mode , MartinR |
else pd_ergebnis = DiffNick + Ki * SummeNick; // PI-Regler für Nick // bei HH , MartinR |
// Motor Vorn |
tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; |
if(pd_ergebnis > tmp_int) pd_ergebnis = tmp_int; |
1253,11 → 1333,16 |
// Roll-Achse |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
DiffRoll = MesswertRoll - StickRoll; // Differenz bestimmen |
if(IntegralFaktor) SummeRoll += IntegralRoll * IntegralFaktor - StickRoll;// I-Anteil bei Winkelregelung |
// if(IntegralFaktor) SummeRoll += IntegralRoll * IntegralFaktor - StickRoll;// I-Anteil bei Winkelregelung |
if(IntegralFaktor) SummeRoll = (IntegralRoll * IntegralFaktor - StickRoll + (stick_roll_neutral/4)) / Ki; // Startwert von SummeRoll bei Umschaltung auf HH, MartinR |
// im ACC-Mode wird SummeRoll nicht mehr verwendet. SummeRoll im ACC-Mode ist nur der Startwert für SummeRoll im HH-Mode, MartinR |
else SummeRoll += DiffRoll; // I-Anteil bei HH |
|
if(SummeRoll > (STICK_GAIN * 16000L)) SummeRoll = (STICK_GAIN * 16000L); |
if(SummeRoll < -(16000L * STICK_GAIN)) SummeRoll = -(16000L * STICK_GAIN); |
pd_ergebnis = DiffRoll + Ki * SummeRoll; // PI-Regler für Roll |
|
if(IntegralFaktor) pd_ergebnis = DiffRoll; // PI-Regler im ACC-Mode , MartinR |
else 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; |