67,7 → 67,7 |
unsigned int NeutralAccX=0, NeutralAccY=0; |
int NaviAccNick, NaviAccRoll,NaviCntAcc = 0; |
int NeutralAccZ = 0; |
unsigned char CosinusNickWinkel = 0, CosinusRollWinkel = 0; |
unsigned char CosinusNickWinkel = 0, CosinusRollWinkel = 0 , ControlHeading = 0; |
long IntegralNick = 0,IntegralNick2 = 0; |
long IntegralRoll = 0,IntegralRoll2 = 0; |
long IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 0; |
148,6 → 148,7 |
unsigned char Parameter_NaviSpeedCompensation; |
unsigned char Parameter_ExternalControl; |
unsigned char Parameter_Servo3,Parameter_Servo4,Parameter_Servo5; |
unsigned char HeadFree = 0; |
|
signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20; |
int MaxStickNick = 0,MaxStickRoll = 0; |
168,7 → 169,6 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
void CopyDebugValues(void) |
{ |
|
DebugOut.Analog[0] = IntegralNick / (EE_Parameter.GyroAccFaktor * 4); |
DebugOut.Analog[1] = IntegralRoll / (EE_Parameter.GyroAccFaktor * 4); |
DebugOut.Analog[2] = Mittelwert_AccNick / 4; |
180,7 → 180,7 |
DebugOut.Analog[9] = UBat; |
DebugOut.Analog[10] = SenderOkay; |
DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR; |
DebugOut.Analog[16] = NeutralAccZ; |
//DebugOut.Analog[16] = NeutralAccZ; |
//DebugOut.Analog[16] = Motor[0].Temperature; |
//DebugOut.Analog[17] = FromNaviCtrl_Value.Distance; |
// DebugOut.Analog[18] = VarioMeter; |
329,6 → 329,7 |
VarioMeter = 0; |
Mess_Integral_Hoch = 0; |
KompassStartwert = KompassValue; |
ControlHeading = KompassValue / 15; |
GPS_Neutral(); |
beeptime = 50; |
Umschlag180Nick = ((long) EE_Parameter.WinkelUmschlagNick * 2500L) + 15000L; |
418,15 → 419,13 |
tmpl /= 4096L; |
tmpl2 = ((MesswertGier + tmpl4) * winkel_roll) / 2048L; |
tmpl2 *= Parameter_AchsKopplung1; |
tmpl2 /= 4096L; |
tmpl2 /= 4096L; |
if(abs(MesswertGier) > 64) if(labs(tmpl) > 128 || labs(tmpl2) > 128) TrichterFlug = 1; |
//MesswertGier += (Parameter_CouplingYawCorrection * tmpl4) / 256; |
} |
else tmpl = tmpl2 = KopplungsteilNickRoll = KopplungsteilRollNick = 0; |
|
TrimRoll = tmpl - tmpl2 / 100L; |
TrimNick = -tmpl2 + tmpl / 100L; |
|
TrimRoll = tmpl - tmpl2 / 100L; |
TrimNick = -tmpl2 + tmpl / 100L; |
// Kompasswert begrenzen ++++++++++++++++++++++++++++++++++++++++++++++++ |
if(ErsatzKompass >= (360L * GIER_GRAD_FAKTOR)) ErsatzKompass -= 360L * GIER_GRAD_FAKTOR; // 360° Umschlag |
if(ErsatzKompass < 0) ErsatzKompass += 360L * GIER_GRAD_FAKTOR; |
599,7 → 598,9 |
CHK_POTI(Parameter_ExternalControl,EE_Parameter.ExternalControl); |
Ki = (10300 / 2) / (Parameter_I_Faktor + 1); |
MAX_GAS = EE_Parameter.Gas_Max; |
MIN_GAS = EE_Parameter.Gas_Min; |
MIN_GAS = EE_Parameter.Gas_Min; |
if(Poti4 > 50) HeadFree = 1; else HeadFree = 0; |
if(HeadFree) {if(Parameter_AchsKopplung1 < 210) Parameter_AchsKopplung1 += 30;} |
} |
|
//############################################################################ |
805,19 → 806,39 |
if(!NewPpmData-- || (FCFlags & FCFLAG_NOTLANDUNG)) |
{ |
static int stick_nick,stick_roll; |
|
unsigned char angle = 180/15; |
signed char sintab[31] = { 0, 4, 8, 11, 14, 16, 16, 16, 14, 11, 8, 4, 0, -4, -8, -11, -14, -16, -16, -16, -14, -11, -8, -4, 0, 4, 8, 11, 14, 16, 16}; |
//signed char costab[24] = {16, 16, 14, 11, 8, 4, 0, -4, -8, -11, -14, -16, -16, -16, -14, -11, -8, -4, 0, 4, 8, 11, 14, 16}; |
angle = (ControlHeading + (360/15) - ErsatzKompass / (GIER_GRAD_FAKTOR * 15)) % 24; |
|
signed int cos_h, sin_h; |
cos_h = sintab[angle + 6]/2; |
sin_h = sintab[angle]/2; |
|
ParameterZuordnung(); |
stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4; |
stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D; |
StickNick = stick_nick - (GPS_Nick + GPS_Nick2); |
|
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); |
|
if(HeadFree) |
{ |
StickNick = ((cos_h * stick_nick) + (sin_h * stick_roll))/8; |
StickRoll = ((cos_h * stick_roll) - (sin_h * stick_nick))/8; |
} |
else |
{ |
StickNick = stick_nick; |
StickRoll = stick_roll; |
} |
|
StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]; |
if(StickGier > 2) StickGier -= 2; else |
if(StickGier < -2) StickGier += 2; else StickGier = 0; |
|
StickNick -= (GPS_Nick + GPS_Nick2); |
StickRoll -= (GPS_Roll + GPS_Roll2); |
StickGas = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120; |
|
GyroFaktor = (Parameter_Gyro_P + 10.0); |
1174,7 → 1195,7 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Kompass |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//DebugOut.Analog[16] = KompassSignalSchlecht; |
DebugOut.Analog[18] = KompassSignalSchlecht; |
|
if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)) |
{ |
1182,11 → 1203,11 |
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; |
korrektur = w / 8 + 2; |
fehler = ((540 + KompassValue - (ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180; |
if(abs(MesswertGier) > 128) |
{ |
fehler = 0; |
// korrektur = korrektur = w / 16 + 2; // schnell konvergieren |
} |
if(!KompassSignalSchlecht && w < 25) |
{ |
1193,14 → 1214,14 |
GierGyroFehler += fehler; |
if(NeueKompassRichtungMerken) |
{ |
// beeptime = 200; |
// KompassStartwert = KompassValue; |
ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR; |
KompassStartwert = (ErsatzKompass/GIER_GRAD_FAKTOR); |
NeueKompassRichtungMerken = 0; |
} |
} |
ErsatzKompass += (fehler * 8) / korrektur; |
DebugOut.Analog[16] = fehler; |
DebugOut.Analog[17] = korrektur; |
ErsatzKompass += (fehler * 16) / korrektur; |
w = (w * Parameter_KompassWirkung) / 32; // auf die Wirkung normieren |
w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln |
if(w >= 0) |