| 1091,13 → 1091,19 |
|---|
| if(!NewPpmData-- || (FC_StatusFlags & FC_STATUS_EMERGENCY_LANDING)) |
| { |
| static int stick_nick,stick_roll; |
| unsigned char stick_p; |
| unsigned char stick_p, stick_d; |
| ParameterZuordnung(); |
| stick_p = EE_Parameter.Stick_P; |
| stick_d = EE_Parameter.Stick_D; |
| if(HoehenReglerAktiv) |
| { |
| LIMIT_MAX(stick_p, 4); |
| LIMIT_MAX(stick_d, 6); |
| } |
| stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * stick_p) / 4; |
| stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D; |
| stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * stick_d; |
| stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * stick_p) / 4; |
| stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D; |
| stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * stick_d; |
| |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // CareFree und freie Wahl der vorderen Richtung |
| 1269,8 → 1275,25 |
|---|
| if(!Looping_Nick && !Looping_Roll && (Aktuell_az > 512 || MotorenEin)) |
| { |
| long tmp_long, tmp_long2; |
| if(FromNaviCtrl_Value.Kalman_K > 0 /*&& Parameter_UserParam1 < 100*/) |
| { |
| if(MotorenEin && HoehenReglerAktiv) |
| { |
| 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; |
| |
| KompassFusion = 25; |
| #define AUSGLEICH 32 |
| if(tmp_long > AUSGLEICH) tmp_long = AUSGLEICH; |
| if(tmp_long < -AUSGLEICH) tmp_long =-AUSGLEICH; |
| if(tmp_long2 > AUSGLEICH) tmp_long2 = AUSGLEICH; |
| if(tmp_long2 <-AUSGLEICH) tmp_long2 =-AUSGLEICH; |
| } else if(FromNaviCtrl_Value.Kalman_K > 0) |
| { |
| tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)(Mittelwert_AccNick - FromNaviCtrl.AccErrorN)); |
| tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)(Mittelwert_AccRoll - FromNaviCtrl.AccErrorR)); |
| tmp_long = (tmp_long * FromNaviCtrl_Value.Kalman_K) / (32 * 16); |
| 1277,39 → 1300,36 |
|---|
| tmp_long2 = (tmp_long2 * FromNaviCtrl_Value.Kalman_K) / (32 * 16); |
| if((MaxStickNick > 64) || (MaxStickRoll > 64)) |
| { |
| tmp_long /= 2; |
| tmp_long2 /= 2; |
| tmp_long /= 2; |
| tmp_long2 /= 2; |
| } |
| if(tmp_long > (long) FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long = (long) FromNaviCtrl_Value.Kalman_MaxFusion; |
| if(tmp_long < (long)-FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long = (long)-FromNaviCtrl_Value.Kalman_MaxFusion; |
| if(tmp_long2 > (long) FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long2 = (long) FromNaviCtrl_Value.Kalman_MaxFusion; |
| if(tmp_long2 < (long)-FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long2 = (long)-FromNaviCtrl_Value.Kalman_MaxFusion; |
| } |
| else |
| { |
| if(MotorenEin) |
| } |
| else |
| { |
| tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick); |
| tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll); |
| tmp_long /= 16; |
| tmp_long2 /= 16; |
| if((MaxStickNick > 64) || (MaxStickRoll > 64)) |
| { |
| 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; |
| } else { // zum Abgleich Acc zu Gyro |
| tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick); |
| tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll); |
| tmp_long /= 16; |
| tmp_long2 /= 16; |
| tmp_long /= 3; |
| tmp_long2 /= 3; |
| } |
| if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25) |
| { |
| tmp_long /= 3; |
| tmp_long2 /= 3; |
| } |
| KompassFusion = 25; |
| #define AUSGLEICH 32 |
| if(tmp_long > AUSGLEICH) tmp_long = AUSGLEICH; |
| if(tmp_long < -AUSGLEICH) tmp_long =-AUSGLEICH; |
| if(tmp_long2 > AUSGLEICH) tmp_long2 = AUSGLEICH; |
| if(tmp_long2 <-AUSGLEICH) tmp_long2 =-AUSGLEICH; |
| } |
| } |
| |
| Mess_IntegralNick -= tmp_long; |
| Mess_IntegralRoll -= tmp_long2; |