📄 fc.c
字号:
if(Mess_Integral_Gier > 50000) Mess_Integral_Gier = 50000; // begrenzen
if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Kompass
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//DebugOut.Analog[16] = KompassSignalSchlecht;
if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV))
{
int w,v,r,fehler,korrektur;
w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln
v = abs(IntegralRoll /512);
if(v > w) w = v; // gr?sste Neigung ermitteln
korrektur = w / 8 + 1;
fehler = ((540 + KompassValue - (ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180;
if(abs(MesswertGier) > 128)
{
fehler = 0;
}
if(!KompassSignalSchlecht && w < 25)
{
GierGyroFehler += fehler;
if(NeueKompassRichtungMerken)
{
beeptime = 200;
// KompassStartwert = KompassValue;
ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR;
KompassStartwert = (ErsatzKompass/GIER_GRAD_FAKTOR);
NeueKompassRichtungMerken = 0;
}
}
ErsatzKompass += (fehler * 8) / korrektur;
w = (w * Parameter_KompassWirkung) / 32; // auf die Wirkung normieren
w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln
if(w >= 0)
{
if(!KompassSignalSchlecht)
{
v = 64 + ((MaxStickNick + MaxStickRoll)) / 8;
r = ((540 + (ErsatzKompass/GIER_GRAD_FAKTOR) - KompassStartwert) % 360) - 180;
// r = KompassRichtung;
v = (r * w) / v; // nach Kompass ausrichten
w = 3 * Parameter_KompassWirkung;
if(v > w) v = w; // Begrenzen
else
if(v < -w) v = -w;
Mess_Integral_Gier += v;
}
if(KompassSignalSchlecht) KompassSignalSchlecht--;
}
else KompassSignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Debugwerte zuordnen
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(!TimerWerteausgabe--)
{
TimerWerteausgabe = 24;
DebugOut.Analog[0] = IntegralNick / (EE_Parameter.GyroAccFaktor * 4);
DebugOut.Analog[1] = IntegralRoll / (EE_Parameter.GyroAccFaktor * 4);
DebugOut.Analog[2] = Mittelwert_AccNick / 4;
DebugOut.Analog[3] = Mittelwert_AccRoll / 4;
DebugOut.Analog[4] = MesswertGier;
DebugOut.Analog[5] = HoehenWert;
DebugOut.Analog[6] = Aktuell_az;//(Mess_Integral_Hoch / 512);//Aktuell_az;
DebugOut.Analog[8] = KompassValue;
DebugOut.Analog[9] = UBat;
DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR;
DebugOut.Analog[10] = SenderOkay;
//DebugOut.Analog[16] = Mittelwert_AccHoch;
//DebugOut.Analog[17] = FromNaviCtrl_Value.Distance;
//DebugOut.Analog[18] = (int)FromNaviCtrl_Value.OsdBar;
DebugOut.Analog[19] = WinkelOut.CalcState;
DebugOut.Analog[20] = ServoValue;
// DebugOut.Analog[24] = MesswertNick/2;
// DebugOut.Analog[25] = MesswertRoll/2;
DebugOut.Analog[27] = (int)FromNaviCtrl_Value.Kalman_MaxDrift;
// DebugOut.Analog[28] = (int)FromNaviCtrl_Value.Kalman_MaxFusion;
// DebugOut.Analog[29] = (int)FromNaviCtrl_Value.Kalman_K;
DebugOut.Analog[29] = FromNaviCtrl_Value.SerialDataOkay;
DebugOut.Analog[30] = GPS_Nick;
DebugOut.Analog[31] = GPS_Roll;
// DebugOut.Analog[19] -= DebugOut.Analog[19]/128;
// if(DebugOut.Analog[19] > 0) DebugOut.Analog[19]--; else DebugOut.Analog[19]++;
/* 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;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(TrichterFlug) { SummeRoll = 0; SummeNick = 0;};
if(!Looping_Nick) IntegralNickMalFaktor = (IntegralNick * IntegralFaktor) / (44000 / STICK_GAIN); else IntegralNickMalFaktor = 0;
if(!Looping_Roll) IntegralRollMalFaktor = (IntegralRoll * IntegralFaktor) / (44000 / STICK_GAIN); else IntegralRollMalFaktor = 0;
#define TRIM_MAX 200
if(TrimNick > TRIM_MAX) TrimNick = TRIM_MAX; else if(TrimNick <-TRIM_MAX) TrimNick =-TRIM_MAX;
if(TrimRoll > TRIM_MAX) TrimRoll = TRIM_MAX; else if(TrimRoll <-TRIM_MAX) TrimRoll =-TRIM_MAX;
MesswertNick = IntegralNickMalFaktor + (long)((long)MesswertNick * GyroFaktor + (long)TrimNick * 128L) / (256L / STICK_GAIN);
MesswertRoll = IntegralRollMalFaktor + (long)((long)MesswertRoll * GyroFaktor + (long)TrimRoll * 128L) / (256L / STICK_GAIN);
MesswertGier = (long)(MesswertGier * 2 * (long)GyroFaktorGier) / (256L / STICK_GAIN) + (long)(Integral_Gier * IntegralFaktorGier) / (2 * (44000 / STICK_GAIN));
// Maximalwerte abfangen
// #define MAX_SENSOR (4096*STICK_GAIN)
#define MAX_SENSOR (4096*4)
if(MesswertNick > MAX_SENSOR) MesswertNick = MAX_SENSOR;
if(MesswertNick < -MAX_SENSOR) MesswertNick = -MAX_SENSOR;
if(MesswertRoll > MAX_SENSOR) MesswertRoll = MAX_SENSOR;
if(MesswertRoll < -MAX_SENSOR) MesswertRoll = -MAX_SENSOR;
if(MesswertGier > MAX_SENSOR) MesswertGier = MAX_SENSOR;
if(MesswertGier < -MAX_SENSOR) MesswertGier = -MAX_SENSOR;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// all BL-Ctrl connected?
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(MissingMotor) if(modell_fliegt > 1 && modell_fliegt < 50 && GasMischanteil > 0)
{
modell_fliegt = 1;
GasMischanteil = MIN_GAS;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// H?henregelung
// Die H?henregelung schw?cht lediglich das Gas ab, erh?ht es allerdings nicht
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
GasMischanteil *= STICK_GAIN;
if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // H?henregelung
{
int tmp_int;
static char delay = 100;
if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER) // Regler wird ?ber Schalter gesteuert
{
if(Parameter_MaxHoehe < 50)
{
if(!delay--)
{
if((MessLuftdruck > 1000) && OCR0A < 254)
{
if(OCR0A < 244)
{
ExpandBaro -= 10;
OCR0A = DruckOffsetSetting - ExpandBaro;
}
else OCR0A = 254;
beeptime = 300;
delay = 250;
}
else
if((MessLuftdruck < 100) && OCR0A > 1)
{
if(OCR0A > 10)
{
ExpandBaro += 10;
OCR0A = DruckOffsetSetting - ExpandBaro;
}
else OCR0A = 1;
beeptime = 300;
delay = 250;
}
else
{
SollHoehe = HoehenWert - 20; // Parameter_MaxHoehe ist der PPM-Wert des Schalters
HoehenReglerAktiv = 0;
delay = 1;
}
}
}
else
{
HoehenReglerAktiv = 1;
delay = 200;
}
}
else
{
SollHoehe = ((int) ExternHoehenValue + (int) Parameter_MaxHoehe) * (int)EE_Parameter.Hoehe_Verstaerkung - 20;
HoehenReglerAktiv = 1;
}
if(Notlandung) SollHoehe = 0;
h = HoehenWert;
if((h > SollHoehe) && HoehenReglerAktiv) // zu hoch --> drosseln
{
h = ((h - SollHoehe) * (int) Parameter_Hoehe_P) / (16 / STICK_GAIN); // Differenz bestimmen --> P-Anteil
h = GasMischanteil - h; // vom Gas abziehen
h -= (HoeheD)/(8/STICK_GAIN); // D-Anteil
tmp_int = ((Mess_Integral_Hoch / 128) * (signed long) Parameter_Hoehe_ACC_Wirkung) / (128 / STICK_GAIN);
if(tmp_int > 70*STICK_GAIN) tmp_int = 70*STICK_GAIN;
else if(tmp_int < -(70*STICK_GAIN)) tmp_int = -(70*STICK_GAIN);
h -= tmp_int;
hoehenregler = (hoehenregler*15 + h) / 16;
if(hoehenregler < EE_Parameter.Hoehe_MinGas * STICK_GAIN) // nicht unter MIN
{
if(GasMischanteil >= EE_Parameter.Hoehe_MinGas * STICK_GAIN) hoehenregler = EE_Parameter.Hoehe_MinGas * STICK_GAIN;
if(GasMischanteil < EE_Parameter.Hoehe_MinGas * STICK_GAIN) hoehenregler = GasMischanteil;
}
if(hoehenregler > GasMischanteil) hoehenregler = GasMischanteil; // nicht mehr als Gas
GasMischanteil = hoehenregler;
}
}
if(GasMischanteil > (MAX_GAS - 20) * STICK_GAIN) GasMischanteil = (MAX_GAS - 20) * STICK_GAIN;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Mischer und PI-Regler
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
DebugOut.Analog[7] = GasMischanteil;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Gier-Anteil
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#define MUL_G 1.0
GierMischanteil = MesswertGier - sollGier * STICK_GAIN; // Regler f?r Gier
// GierMischanteil = 0;
#define MIN_GIERGAS (40*STICK_GAIN) // unter diesem Gaswert trotzdem Gieren
if(GasMischanteil > MIN_GIERGAS)
{
if(GierMischanteil > (GasMischanteil / 2)) GierMischanteil = GasMischanteil / 2;
if(GierMischanteil < -(GasMischanteil / 2)) GierMischanteil = -(GasMischanteil / 2);
}
else
{
if(GierMischanteil > (MIN_GIERGAS / 2)) GierMischanteil = MIN_GIERGAS / 2;
if(GierMischanteil < -(MIN_GIERGAS / 2)) GierMischanteil = -(MIN_GIERGAS / 2);
}
tmp_int = MAX_GAS*STICK_GAIN;
if(GierMischanteil > ((tmp_int - GasMischanteil))) GierMischanteil = ((tmp_int - GasMischanteil));
if(GierMischanteil < -((tmp_int - GasMischanteil))) GierMischanteil = -((tmp_int - GasMischanteil));
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Nick-Achse
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
DiffNick = MesswertNick - StickNick; // Differenz bestimmen
if(IntegralFaktor) SummeNick += IntegralNickMalFaktor - StickNick; // I-Anteil bei Winkelregelung
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_nick = DiffNick + SummeNick / Ki; // PI-Regler f?r Nick
// Motor Vorn
tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
if(pd_ergebnis_nick > tmp_int) pd_ergebnis_nick = tmp_int;
if(pd_ergebnis_nick < -tmp_int) pd_ergebnis_nick = -tmp_int;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Roll-Achse
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -