59,6 → 59,7 |
volatile int MesswertNick,MesswertRoll,MesswertGier; |
volatile int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0; |
volatile int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch, NeutralAccX=0, NeutralAccY=0; |
int NaviAccNick, NaviAccRoll,NaviCntAcc = 0; |
volatile float NeutralAccZ = 0; |
unsigned char CosinusNickWinkel = 0, CosinusRollWinkel = 0; |
long IntegralNick = 0,IntegralNick2 = 0; |
213,6 → 214,9 |
Mittelwert_AccHoch = ((long)Mittelwert_AccHoch * 1 + ((long)AdWertAccHoch)) / 2L; |
IntegralAccNick += ACC_AMPLIFY * AdWertAccNick; |
IntegralAccRoll += ACC_AMPLIFY * AdWertAccRoll; |
NaviAccNick += AdWertAccNick; |
NaviAccRoll += AdWertAccRoll; |
NaviCntAcc++; |
IntegralAccZ += Aktuell_az - NeutralAccZ; |
// Gier ++++++++++++++++++++++++++++++++++++++++++++++++ |
ErsatzKompass += MesswertGier; |
597,12 → 601,12 |
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; |
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; |
StickRoll = stick_roll - (GPS_Roll + GPS_Roll2); |
|
// StickRoll = (StickRoll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4; |
|
965,12 → 969,16 |
//v /= 4; |
//w /= 4; |
if(v > w) w = v; // grösste Neigung ermitteln |
korrektur = w + 8; |
korrektur = w + 4; |
if(w < 25 && NeueKompassRichtungMerken && !KompassSignalSchlecht) |
{ |
KompassStartwert = KompassValue; |
NeueKompassRichtungMerken = 0; |
} |
|
fehler = ((540 + KompassValue - (ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180; |
ErsatzKompass += (fehler * 8);// / korrektur; |
// DebugOut.Analog[10] = fehler; |
w = (w * Parameter_KompassWirkung) / 64; // auf die Wirkung normieren |
w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln |
if(w > 0) |
977,10 → 985,7 |
{ |
if(!KompassSignalSchlecht) |
{ |
fehler = ((540 + KompassValue - (ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180; |
GierGyroFehler += fehler; |
// DebugOut.Analog[10] = fehler; |
ErsatzKompass += (fehler * 16) / korrektur; |
v = (KompassRichtung * w) / 32; // nach Kompass ausrichten |
w = Parameter_KompassWirkung; |
if(v > w) v = w; // Begrenzen |
1000,6 → 1005,7 |
if(!TimerWerteausgabe--) |
{ |
TimerWerteausgabe = 24; |
|
DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor; |
DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor; |
DebugOut.Analog[2] = Mittelwert_AccNick; |
1006,7 → 1012,7 |
DebugOut.Analog[3] = Mittelwert_AccRoll; |
DebugOut.Analog[4] = MesswertGier; |
DebugOut.Analog[5] = HoehenWert; |
DebugOut.Analog[6] =(Mess_Integral_Hoch / 512); |
DebugOut.Analog[6] = (Mess_Integral_Hoch / 512); |
DebugOut.Analog[8] = KompassValue; |
DebugOut.Analog[9] = UBat; |
DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR; |
1019,6 → 1025,22 |
DebugOut.Analog[30] = GPS_Nick; |
DebugOut.Analog[31] = GPS_Roll; |
|
/* DebugOut.Analog[19] += (DebugOut.Analog[1] - Mittelwert_AccRoll)/32; |
DebugOut.Analog[19] -= DebugOut.Analog[19]/32; |
if(DebugOut.Analog[19] > 0) DebugOut.Analog[19]--; else DebugOut.Analog[19]++; |
GPS_Roll2 = (1 * GPS_Roll + ((DebugOut.Analog[19] / 16) * Poti3) / 32) / 2; |
|
DebugOut.Analog[20] += (DebugOut.Analog[0] - Mittelwert_AccNick)/32; |
DebugOut.Analog[20] -= DebugOut.Analog[20]/32; |
if(DebugOut.Analog[20] > 0) DebugOut.Analog[20]--; else DebugOut.Analog[20]++; |
GPS_Nick2 = (1 * GPS_Roll + ((DebugOut.Analog[20] / 16) * Poti3) / 32) / 2; |
*/ |
//GPS_Nick = (GPS_Nick + (DebugOut.Analog[20] * Poti3) / 32) / 2; |
|
|
// 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]; |
1146,8 → 1168,6 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
DiffNick = MesswertNick - StickNick; // Differenz bestimmen |
if(IntegralFaktor) SummeNick += IntegralNick * IntegralFaktor - StickNick; // I-Anteil bei Winkelregelung |
// DiffNick = MesswertNick - (StickNick - GPS_Nick); // Differenz bestimmen |
// if(IntegralFaktor) SummeNick += IntegralNick * IntegralFaktor - (StickNick - GPS_Nick); // I-Anteil bei Winkelregelung |
|
else SummeNick += DiffNick; // I-Anteil bei HH |
if(SummeNick > 16000) SummeNick = 16000; |
1174,8 → 1194,6 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
DiffRoll = MesswertRoll - StickRoll; // Differenz bestimmen |
if(IntegralFaktor) SummeRoll += IntegralRoll * IntegralFaktor - StickRoll;// I-Anteil bei Winkelregelung |
// DiffRoll = MesswertRoll - (StickRoll - GPS_Roll); // Differenz bestimmen |
// if(IntegralFaktor) SummeRoll += IntegralRoll * IntegralFaktor - (StickRoll - GPS_Roll);// I-Anteil bei Winkelregelung |
|
else SummeRoll += DiffRoll; // I-Anteil bei HH |
if(SummeRoll > 16000) SummeRoll = 16000; |