Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 933 → Rev 964

/branches/FC_V070d_ACC-HH_Umschaltung/fc.c
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;
/branches/FC_V070d_ACC-HH_Umschaltung/version.txt
155,4 → 155,14
V0.70d H.Buss 02.08.2008
- Transistorausgänge: das oberste Bit der Blinkmaske (im KopterTool linkes Bit) gibt nun den Zustand des Ausgangs im Schalterbetrieb an
V0.70d_HH MartinR 07.09.2008
- Diverse Änderungen in der fc.c um im Flug zwischen ACC-Mode und HH- Mode umschalten zu können.
- über UserParameter1 (>50, per Schalter) kann im ACC- Modus auf HH umgeschaltet werden.
UserParameter2 ist dann Stick-P. Hier unbedingt den für HH gewünschten Stick-P Wert eitragen!
Wird der HH-Mode über UserParameter1 aktiviert, so ist die Stick-Position im Moment des Umschaltens die Neutralposition für den HH-Mode. Hierdurch bleibt der ACC-Mode trimmbar. Der Nick- / Roll-Knüppel muß daher aber beim Umschalten von ACC auf HH in Mittelstellung stehen. Umschaltung von HH auf ACC kann bei beliebiger Stick-Position erfolgen.
Der Höhenregler ist beim HH-Mode über UserParameter1 deaktiviert.
- GyroFaktor I ist nun ausschließlich der I-Faktor im ACC-Mode.
- Main I ist nun ausschließlich der I-Faktor im HH-Mode.
- HH-Mode wurde reduziert auf die Regler-Grundfunktionen!
keine Driftkompensation, Gierfunktion nur mit P-Regler, keine GPS- oder Kompass Funktion.