| 183,7 → 183,10 |
|---|
| signed int CosAttitude; // for projection of hoover gas |
| unsigned char ACC_AltitudeControl = 0; |
| long NickSum, RollSum; |
| long Gierwert, D_NSum, D_RSum; |
| signed int AccNick_err, AccRoll_err; |
| signed int P_NickFusion, D_NickFusion; |
| signed int P_RollFusion, D_RollFusion; |
| |
| #if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__)) |
| #define OPA_OFFSET_STEP 5 |
| 489,6 → 492,7 |
|---|
| KopplungsteilRollNick = tmpl4; |
| tmpl4 -= tmpl3; |
| ErsatzKompass += tmpl4; |
| Gierwert = MesswertGier - (MesswertGier>>4) + tmpl4 + (tmpl4>>2); // 0.43°/s |
| if(!Parameter_CouplingYawCorrection) Mess_Integral_Gier -= tmpl4/2; // Gier nachhelfen |
| |
| tmpl = ((MesswertGier + tmpl4) * winkel_nick) / 2048L; |
| 1306,19 → 1310,53 |
|---|
| if(!Looping_Nick && !Looping_Roll && (Aktuell_az > 512 || MotorenEin)) |
| { |
| long tmp_long, tmp_long2; |
| if(MotorenEin && HoehenReglerAktiv) |
| long dRollSum, dNickSum; |
| // if(MotorenEin && HoehenReglerAktiv) |
| if(Parameter_UserParam5 > 50) |
| { |
| NickSum = NickSum + (long)((AdWertNickFilter + (TrimNick * 9))*9 - AccNick_err); |
| AccNick_err = NickSum / 2048; // T=4s, gain: 8*9/4=18, res: 0.025deg |
| tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)(Mittelwert_AccNick + AccNick_err)); |
| tmp_long /= 16; // T ca. 1s |
| RollSum = RollSum + (long)((AdWertRollFilter + (TrimRoll * 9))*9 - AccRoll_err); |
| AccRoll_err = RollSum / 2048; |
| tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)(Mittelwert_AccRoll + AccRoll_err)); |
| tmp_long2 /= 16; |
| // Schätzung Beschleunigung |
| tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor); |
| NickSum = NickSum - AccNick_err; |
| // AccNick_err = NickSum / 1024 + tmp_long; // T=2s, res: 0.025deg (Quadro) |
| AccNick_err = NickSum / 2048 + tmp_long; // T=4s, res: 0.025deg (HexaXL) |
| tmp_long = tmp_long - (long)(Mittelwert_AccNick + AccNick_err); |
| // tmp_long /= 16; // T ca. 1s |
| P_NickFusion = tmp_long/32; // P=0.5 |
| D_NSum = D_NSum - D_NickFusion; |
| D_NickFusion = D_NSum/128 + tmp_long; // gain=16, HP=0.25s, D=4 |
| tmp_long = P_NickFusion + D_NickFusion/4; // D=1 |
| |
| tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor); |
| RollSum = RollSum - AccRoll_err; |
| // AccRoll_err = RollSum / 1024 + tmp_long2; // T=2s, res: 0.025deg |
| AccRoll_err = RollSum / 2048 + tmp_long2; // T=4s, res: 0.025deg |
| tmp_long2 = tmp_long2 - (long)(Mittelwert_AccRoll + AccRoll_err); |
| // tmp_long2 /= 16; |
| P_RollFusion = tmp_long2/32; |
| D_RSum = D_RSum - D_RollFusion; |
| D_RollFusion = D_RSum/128 + tmp_long2; // gain=16, HP=0.25s |
| tmp_long2 = P_RollFusion + D_RollFusion/4; |
| |
| // dreht Vektor aus RollSum u. NickSum |
| // Gierwert Auflösung = 0.43°/s, damit ist rad/LSB*dt = 2^16 |
| dRollSum = -Gierwert * (NickSum>>16); |
| dNickSum = Gierwert * (RollSum>>16); |
| RollSum = RollSum - dRollSum; |
| NickSum = NickSum - dNickSum; |
| |
| if((MaxStickNick > 64) || (MaxStickRoll > 64)) |
| { |
| tmp_long /= 2; |
| tmp_long2 /= 2; |
| } |
| if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25) |
| { |
| tmp_long /= 4; |
| tmp_long2 /= 4; |
| } |
| |
| KompassFusion = 25; |
| #define AUSGLEICH 32 |
| #define AUSGLEICH 32 |
| if(tmp_long > AUSGLEICH) tmp_long = AUSGLEICH; |
| if(tmp_long < -AUSGLEICH) tmp_long =-AUSGLEICH; |
| if(tmp_long2 > AUSGLEICH) tmp_long2 = AUSGLEICH; |