Rev 1111 | Rev 1121 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1111 | Rev 1120 | ||
---|---|---|---|
Line 119... | Line 119... | ||
119 | unsigned char Parameter_UserParam6 = 0; |
119 | unsigned char Parameter_UserParam6 = 0; |
120 | unsigned char Parameter_UserParam7 = 0; |
120 | unsigned char Parameter_UserParam7 = 0; |
121 | unsigned char Parameter_UserParam8 = 0; |
121 | unsigned char Parameter_UserParam8 = 0; |
122 | unsigned char Parameter_ServoNickControl = 100; |
122 | unsigned char Parameter_ServoNickControl = 100; |
123 | unsigned char Parameter_LoopGasLimit = 70; |
123 | unsigned char Parameter_LoopGasLimit = 70; |
124 | unsigned char Parameter_AchsKopplung1 = 0; |
124 | unsigned char Parameter_AchsKopplung1 = 90; |
125 | unsigned char Parameter_AchsKopplung2 = 6; |
125 | unsigned char Parameter_AchsKopplung2 = 65; |
- | 126 | unsigned char Parameter_CouplingYawCorrection = 64; |
|
126 | unsigned char Parameter_AchsGegenKopplung1 = 0; |
127 | //unsigned char Parameter_AchsGegenKopplung1 = 0; |
127 | unsigned char Parameter_DynamicStability = 100; |
128 | unsigned char Parameter_DynamicStability = 100; |
128 | unsigned char Parameter_J16Bitmask; // for the J16 Output |
129 | unsigned char Parameter_J16Bitmask; // for the J16 Output |
129 | unsigned char Parameter_J16Timing; // for the J16 Output |
130 | unsigned char Parameter_J16Timing; // for the J16 Output |
130 | unsigned char Parameter_J17Bitmask; // for the J17 Output |
131 | unsigned char Parameter_J17Bitmask; // for the J17 Output |
131 | unsigned char Parameter_J17Timing; // for the J17 Output |
132 | unsigned char Parameter_J17Timing; // for the J17 Output |
Line 142... | Line 143... | ||
142 | struct mk_param_struct EE_Parameter; |
143 | struct mk_param_struct EE_Parameter; |
143 | signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20; |
144 | signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20; |
144 | int MaxStickNick = 0,MaxStickRoll = 0; |
145 | int MaxStickNick = 0,MaxStickRoll = 0; |
145 | unsigned int modell_fliegt = 0; |
146 | unsigned int modell_fliegt = 0; |
146 | unsigned char MikroKopterFlags = 0; |
147 | unsigned char MikroKopterFlags = 0; |
147 | unsigned int GIER_GRAD_FAKTOR = 1291; |
148 | unsigned long GIER_GRAD_FAKTOR = 1291; |
Line 148... | Line 149... | ||
148 | 149 | ||
149 | void Piep(unsigned char Anzahl) |
150 | void Piep(unsigned char Anzahl) |
150 | { |
151 | { |
151 | while(Anzahl--) |
152 | while(Anzahl--) |
Line 169... | Line 170... | ||
169 | AdNeutralNick = 0; |
170 | AdNeutralNick = 0; |
170 | AdNeutralRoll = 0; |
171 | AdNeutralRoll = 0; |
171 | AdNeutralGier = 0; |
172 | AdNeutralGier = 0; |
172 | AdNeutralGierBias = 0; |
173 | AdNeutralGierBias = 0; |
173 | Parameter_AchsKopplung1 = 0; |
174 | Parameter_AchsKopplung1 = 0; |
174 | Parameter_AchsGegenKopplung1 = 0; |
175 | Parameter_AchsKopplung2 = 0; |
175 | ExpandBaro = 0; |
176 | ExpandBaro = 0; |
176 | CalibrierMittelwert(); |
177 | CalibrierMittelwert(); |
177 | Delay_ms_Mess(100); |
178 | Delay_ms_Mess(100); |
178 | CalibrierMittelwert(); |
179 | CalibrierMittelwert(); |
179 | if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert? |
180 | if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert? |
Line 245... | Line 246... | ||
245 | MesswertGier = (signed int) AdNeutralGier - AdWertGier; |
246 | MesswertGier = (signed int) AdNeutralGier - AdWertGier; |
246 | ErsatzKompass += MesswertGier; |
247 | ErsatzKompass += MesswertGier; |
247 | // MesswertGierBias = (signed int) AdNeutralGierBias - AdWertGier; |
248 | // MesswertGierBias = (signed int) AdNeutralGierBias - AdWertGier; |
248 | MesswertRoll = (signed int) AdWertRoll - AdNeutralRoll; |
249 | MesswertRoll = (signed int) AdWertRoll - AdNeutralRoll; |
249 | MesswertNick = (signed int) AdWertNick - AdNeutralNick; |
250 | MesswertNick = (signed int) AdWertNick - AdNeutralNick; |
250 | DebugOut.Analog[21] = MesswertNick; |
251 | //DebugOut.Analog[21] = MesswertNick; |
251 | DebugOut.Analog[22] = MesswertRoll; |
252 | //DebugOut.Analog[22] = MesswertRoll; |
- | 253 | DebugOut.Analog[22] = Mess_Integral_Gier; |
|
Line 252... | Line 254... | ||
252 | 254 | ||
253 | // Beschleunigungssensor ++++++++++++++++++++++++++++++++++++++++++++++++ |
255 | // Beschleunigungssensor ++++++++++++++++++++++++++++++++++++++++++++++++ |
254 | Mittelwert_AccNick = ((long)Mittelwert_AccNick * 1 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 2L; |
256 | Mittelwert_AccNick = ((long)Mittelwert_AccNick * 1 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 2L; |
255 | Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 1 + ((ACC_AMPLIFY * (long)AdWertAccRoll))) / 2L; |
257 | Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 1 + ((ACC_AMPLIFY * (long)AdWertAccRoll))) / 2L; |
Line 258... | Line 260... | ||
258 | IntegralAccRoll += ACC_AMPLIFY * AdWertAccRoll; |
260 | IntegralAccRoll += ACC_AMPLIFY * AdWertAccRoll; |
259 | NaviAccNick += AdWertAccNick; |
261 | NaviAccNick += AdWertAccNick; |
260 | NaviAccRoll += AdWertAccRoll; |
262 | NaviAccRoll += AdWertAccRoll; |
261 | NaviCntAcc++; |
263 | NaviCntAcc++; |
262 | IntegralAccZ += Aktuell_az - NeutralAccZ; |
264 | IntegralAccZ += Aktuell_az - NeutralAccZ; |
263 | - | ||
264 | // Gier ++++++++++++++++++++++++++++++++++++++++++++++++ |
265 | // Gier ++++++++++++++++++++++++++++++++++++++++++++++++ |
265 | // Mess_Integral_Gier2 += MesswertGier; |
266 | Mess_Integral_Gier += MesswertGier; |
- | 267 | ||
266 | // Kopplungsanteil +++++++++++++++++++++++++++++++++++++ |
268 | // Kopplungsanteil +++++++++++++++++++++++++++++++++++++ |
267 | if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV)) |
269 | if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV)) |
268 | { |
270 | { |
269 | tmpl3 = (MesswertRoll * Mess_IntegralNick) / 2048L; |
271 | tmpl3 = (MesswertRoll * Mess_IntegralNick) / 2048L; |
270 | tmpl3 *= Parameter_AchsKopplung1; //65 |
272 | tmpl3 *= Parameter_AchsKopplung2; //65 |
271 | tmpl3 /= 4096L; |
273 | tmpl3 /= 4096L; |
272 | tmpl4 = (MesswertNick * Mess_IntegralRoll) / 2048L; |
274 | tmpl4 = (MesswertNick * Mess_IntegralRoll) / 2048L; |
273 | tmpl4 *= Parameter_AchsKopplung1; //65 |
275 | tmpl4 *= Parameter_AchsKopplung2; //65 |
274 | tmpl4 /= 4096L; |
276 | tmpl4 /= 4096L; |
275 | tmpl4 -= tmpl3; |
277 | tmpl4 -= tmpl3; |
276 | ErsatzKompass += tmpl4; |
278 | ErsatzKompass += tmpl4; |
Line 277... | Line 279... | ||
277 | 279 | ||
Line 280... | Line 282... | ||
280 | tmpl /= 4096L; |
282 | tmpl /= 4096L; |
281 | tmpl2 = ((MesswertGier + tmpl4) * Mess_IntegralRoll) / 2048L; |
283 | tmpl2 = ((MesswertGier + tmpl4) * Mess_IntegralRoll) / 2048L; |
282 | tmpl2 *= Parameter_AchsKopplung1; |
284 | tmpl2 *= Parameter_AchsKopplung1; |
283 | tmpl2 /= 4096L; |
285 | tmpl2 /= 4096L; |
284 | if(labs(tmpl) > 128 || labs(tmpl2) > 128) TrichterFlug = 1; |
286 | if(labs(tmpl) > 128 || labs(tmpl2) > 128) TrichterFlug = 1; |
- | 287 | ||
285 | MesswertGier += tmpl4 / 2; |
288 | MesswertGier += (Parameter_CouplingYawCorrection * tmpl4) / 128; |
286 | } |
289 | } |
287 | else tmpl = tmpl2 = 0; |
290 | else tmpl = tmpl2 = 0; |
Line -... | Line 291... | ||
- | 291 | ||
288 | 292 | ||
289 | // Kompasswert begrenzen ++++++++++++++++++++++++++++++++++++++++++++++++ |
293 | // Kompasswert begrenzen ++++++++++++++++++++++++++++++++++++++++++++++++ |
290 | if(ErsatzKompass >= (360L * GIER_GRAD_FAKTOR)) ErsatzKompass -= 360L * GIER_GRAD_FAKTOR; // 360° Umschlag |
294 | if(ErsatzKompass >= (360L * GIER_GRAD_FAKTOR)) ErsatzKompass -= 360L * GIER_GRAD_FAKTOR; // 360° Umschlag |
291 | if(ErsatzKompass < 0) ErsatzKompass += 360L * GIER_GRAD_FAKTOR; |
295 | if(ErsatzKompass < 0) ErsatzKompass += 360L * GIER_GRAD_FAKTOR; |
292 | // Roll ++++++++++++++++++++++++++++++++++++++++++++++++ |
296 | // Roll ++++++++++++++++++++++++++++++++++++++++++++++++ |
- | 297 | MesswertRoll += tmpl; |
|
293 | MesswertRoll += tmpl; |
298 | MesswertRoll += tmpl2 / 100L; //109 |
294 | MesswertRoll += (tmpl2*Parameter_AchsGegenKopplung1)/512L; //109 |
299 | // MesswertRoll += (tmpl2*Parameter_AchsGegenKopplung1)/512L; //109 |
295 | Mess_IntegralRoll2 += MesswertRoll; |
300 | Mess_IntegralRoll2 += MesswertRoll; |
- | 301 | Mess_IntegralRoll += MesswertRoll - LageKorrekturRoll; |
|
296 | Mess_IntegralRoll += MesswertRoll - LageKorrekturRoll; |
302 | |
297 | if(Mess_IntegralRoll > Umschlag180Roll) |
303 | if(Mess_IntegralRoll > Umschlag180Roll) |
298 | { |
304 | { |
299 | Mess_IntegralRoll = -(Umschlag180Roll - 25000L); |
305 | Mess_IntegralRoll = -(Umschlag180Roll - 25000L); |
300 | Mess_IntegralRoll2 = Mess_IntegralRoll; |
306 | Mess_IntegralRoll2 = Mess_IntegralRoll; |
Line 316... | Line 322... | ||
316 | if(AdWertRoll > 2000) MesswertRoll = +1000; |
322 | if(AdWertRoll > 2000) MesswertRoll = +1000; |
317 | if(AdWertRoll > 2015) MesswertRoll = +2000; |
323 | if(AdWertRoll > 2015) MesswertRoll = +2000; |
318 | } |
324 | } |
319 | // Nick ++++++++++++++++++++++++++++++++++++++++++++++++ |
325 | // Nick ++++++++++++++++++++++++++++++++++++++++++++++++ |
320 | MesswertNick -= tmpl2; |
326 | MesswertNick -= tmpl2; |
321 | MesswertNick -= (tmpl*Parameter_AchsGegenKopplung1)/512L; |
327 | // MesswertNick -= (tmpl*Parameter_AchsGegenKopplung1)/512L; |
- | 328 | MesswertNick -= tmpl / 100L; //109 |
|
322 | Mess_IntegralNick2 += MesswertNick; |
329 | Mess_IntegralNick2 += MesswertNick; |
323 | Mess_IntegralNick += MesswertNick - LageKorrekturNick; |
330 | Mess_IntegralNick += MesswertNick - LageKorrekturNick; |
Line 324... | Line 331... | ||
324 | 331 | ||
325 | if(Mess_IntegralNick > Umschlag180Nick) |
332 | if(Mess_IntegralNick > Umschlag180Nick) |
Line 362... | Line 369... | ||
362 | else if(MesswertNick < -200) MesswertNick += 4 * (MesswertNick + 200); |
369 | else if(MesswertNick < -200) MesswertNick += 4 * (MesswertNick + 200); |
363 | if(MesswertRoll > 200) MesswertRoll += 4 * (MesswertRoll - 200); |
370 | if(MesswertRoll > 200) MesswertRoll += 4 * (MesswertRoll - 200); |
364 | else if(MesswertRoll < -200) MesswertRoll += 4 * (MesswertRoll + 200); |
371 | else if(MesswertRoll < -200) MesswertRoll += 4 * (MesswertRoll + 200); |
365 | } |
372 | } |
Line 366... | Line -... | ||
366 | - | ||
367 | if(PlatinenVersion >= 20) Parameter_Gyro_D = 8; else Parameter_Gyro_D = 0; |
- | |
368 | 373 | ||
369 | #define D_LIMIT 64 |
374 | #define D_LIMIT 8 |
370 | if(Parameter_Gyro_D) |
375 | if(Parameter_Gyro_D) |
371 | { |
376 | { |
372 | d2Nick = (((MesswertNick - oldNick) * (signed int) Parameter_Gyro_D)); |
377 | d2Nick = (((MesswertNick - oldNick))); |
373 | oldNick = MesswertNick; |
378 | oldNick = MesswertNick; |
374 | if(d2Nick > D_LIMIT) d2Nick = D_LIMIT; |
379 | if(d2Nick > D_LIMIT) d2Nick = D_LIMIT; |
375 | else if(d2Nick < -D_LIMIT) d2Nick = -D_LIMIT; |
380 | else if(d2Nick < -D_LIMIT) d2Nick = -D_LIMIT; |
Line 376... | Line 381... | ||
376 | MesswertNick += d2Nick; |
381 | MesswertNick += (d2Nick * (signed int) Parameter_Gyro_D); |
377 | 382 | ||
378 | d2Roll = (((MesswertRoll - oldRoll) * (signed int) Parameter_Gyro_D)); |
383 | d2Roll = (((MesswertRoll - oldRoll))); |
379 | oldRoll = MesswertRoll; |
384 | oldRoll = MesswertRoll; |
380 | if(d2Roll > D_LIMIT) d2Roll = D_LIMIT; |
385 | if(d2Roll > D_LIMIT) d2Roll = D_LIMIT; |
381 | else if(d2Roll < -D_LIMIT) d2Roll = -D_LIMIT; |
386 | else if(d2Roll < -D_LIMIT) d2Roll = -D_LIMIT; |
Line 382... | Line 387... | ||
382 | MesswertRoll += d2Roll; |
387 | MesswertRoll += (d2Roll * (signed int) Parameter_Gyro_D); |
383 | } |
388 | } |
384 | 389 | ||
Line 470... | Line 475... | ||
470 | CHK_POTI_MM(Parameter_Hoehe_P,EE_Parameter.Hoehe_P,0,100); |
475 | CHK_POTI_MM(Parameter_Hoehe_P,EE_Parameter.Hoehe_P,0,100); |
471 | CHK_POTI(Parameter_Hoehe_ACC_Wirkung,EE_Parameter.Hoehe_ACC_Wirkung,0,255); |
476 | CHK_POTI(Parameter_Hoehe_ACC_Wirkung,EE_Parameter.Hoehe_ACC_Wirkung,0,255); |
472 | CHK_POTI(Parameter_KompassWirkung,EE_Parameter.KompassWirkung,0,255); |
477 | CHK_POTI(Parameter_KompassWirkung,EE_Parameter.KompassWirkung,0,255); |
473 | CHK_POTI_MM(Parameter_Gyro_P,EE_Parameter.Gyro_P,10,255); |
478 | CHK_POTI_MM(Parameter_Gyro_P,EE_Parameter.Gyro_P,10,255); |
474 | CHK_POTI(Parameter_Gyro_I,EE_Parameter.Gyro_I,0,255); |
479 | CHK_POTI(Parameter_Gyro_I,EE_Parameter.Gyro_I,0,255); |
- | 480 | CHK_POTI(Parameter_Gyro_D,EE_Parameter.Gyro_D,0,255); |
|
475 | CHK_POTI(Parameter_I_Faktor,EE_Parameter.I_Faktor,0,255); |
481 | CHK_POTI(Parameter_I_Faktor,EE_Parameter.I_Faktor,0,255); |
476 | CHK_POTI(Parameter_UserParam1,EE_Parameter.UserParam1,0,255); |
482 | CHK_POTI(Parameter_UserParam1,EE_Parameter.UserParam1,0,255); |
477 | CHK_POTI(Parameter_UserParam2,EE_Parameter.UserParam2,0,255); |
483 | CHK_POTI(Parameter_UserParam2,EE_Parameter.UserParam2,0,255); |
478 | CHK_POTI(Parameter_UserParam3,EE_Parameter.UserParam3,0,255); |
484 | CHK_POTI(Parameter_UserParam3,EE_Parameter.UserParam3,0,255); |
479 | CHK_POTI(Parameter_UserParam4,EE_Parameter.UserParam4,0,255); |
485 | CHK_POTI(Parameter_UserParam4,EE_Parameter.UserParam4,0,255); |
Line 482... | Line 488... | ||
482 | CHK_POTI(Parameter_UserParam7,EE_Parameter.UserParam7,0,255); |
488 | CHK_POTI(Parameter_UserParam7,EE_Parameter.UserParam7,0,255); |
483 | CHK_POTI(Parameter_UserParam8,EE_Parameter.UserParam8,0,255); |
489 | CHK_POTI(Parameter_UserParam8,EE_Parameter.UserParam8,0,255); |
484 | CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255); |
490 | CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255); |
485 | CHK_POTI(Parameter_LoopGasLimit,EE_Parameter.LoopGasLimit,0,255); |
491 | CHK_POTI(Parameter_LoopGasLimit,EE_Parameter.LoopGasLimit,0,255); |
486 | CHK_POTI(Parameter_AchsKopplung1, EE_Parameter.AchsKopplung1,0,255); |
492 | CHK_POTI(Parameter_AchsKopplung1, EE_Parameter.AchsKopplung1,0,255); |
- | 493 | CHK_POTI(Parameter_AchsKopplung2, EE_Parameter.AchsKopplung2,0,255); |
|
- | 494 | CHK_POTI(Parameter_CouplingYawCorrection,EE_Parameter.CouplingYawCorrection,0,255); |
|
- | 495 | ||
487 | CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255); |
496 | // CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255); |
488 | CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability,0,255); |
497 | CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability,0,255); |
489 | CHK_POTI_MM(Parameter_J16Timing,EE_Parameter.J16Timing,1,255); |
498 | CHK_POTI_MM(Parameter_J16Timing,EE_Parameter.J16Timing,1,255); |
490 | CHK_POTI_MM(Parameter_J17Timing,EE_Parameter.J17Timing,1,255); |
499 | CHK_POTI_MM(Parameter_J17Timing,EE_Parameter.J17Timing,1,255); |
Line 491... | Line 500... | ||
491 | 500 | ||
Line 710... | Line 719... | ||
710 | static int stick_nick,stick_roll; |
719 | static int stick_nick,stick_roll; |
711 | ParameterZuordnung(); |
720 | ParameterZuordnung(); |
712 | stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4; |
721 | stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4; |
713 | stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D; |
722 | stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D; |
714 | StickNick = stick_nick - (GPS_Nick + GPS_Nick2); |
723 | StickNick = stick_nick - (GPS_Nick + GPS_Nick2); |
715 | // StickNick = (StickNick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4; |
- | |
Line 716... | Line 724... | ||
716 | 724 | ||
717 | stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4; |
725 | stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4; |
718 | stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D; |
726 | stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D; |
Line 719... | Line -... | ||
719 | StickRoll = stick_roll - (GPS_Roll + GPS_Roll2); |
- | |
Line 720... | Line 727... | ||
720 | 727 | StickRoll = stick_roll - (GPS_Roll + GPS_Roll2); |
|
- | 728 | ||
- | 729 | ||
- | 730 | StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]; |
|
721 | // StickRoll = (StickRoll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4; |
731 | if(StickGier > 2) StickGier -= 2; else |
Line 722... | Line 732... | ||
722 | 732 | if(StickGier < -2) StickGier += 2; else StickGier = 0; |
|
723 | StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]; |
733 | |
724 | StickGas = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120; |
734 | StickGas = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120; |
Line 850... | Line 860... | ||
850 | 860 | ||
851 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
861 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
852 | if(!Looping_Nick && !Looping_Roll) |
862 | if(!Looping_Nick && !Looping_Roll) |
853 | { |
863 | { |
854 | long tmp_long, tmp_long2; |
864 | long tmp_long, tmp_long2; |
855 | if(FromNaviCtrl_Value.Kalman_K != -1) |
865 | if(FromNaviCtrl_Value.Kalman_K != -1 && !TrichterFlug) |
856 | { |
866 | { |
857 | tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick); |
867 | tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick); |
858 | tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll); |
868 | tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll); |
859 | tmp_long = (tmp_long * FromNaviCtrl_Value.Kalman_K) / (32 * 16); |
869 | tmp_long = (tmp_long * FromNaviCtrl_Value.Kalman_K) / (32 * 16); |
Line 882... | Line 892... | ||
882 | if((MaxStickNick > 64) || (MaxStickRoll > 64)) |
892 | if((MaxStickNick > 64) || (MaxStickRoll > 64)) |
883 | { |
893 | { |
884 | tmp_long /= 3; |
894 | tmp_long /= 3; |
885 | tmp_long2 /= 3; |
895 | tmp_long2 /= 3; |
886 | } |
896 | } |
887 | if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25) |
897 | if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25) |
888 | { |
898 | { |
889 | tmp_long /= 3; |
899 | tmp_long /= 3; |
890 | tmp_long2 /= 3; |
900 | tmp_long2 /= 3; |
891 | } |
901 | } |
892 | #define AUSGLEICH 32 |
902 | #define AUSGLEICH 32 |
Line 1081... | Line 1091... | ||
1081 | }; |
1091 | }; |
1082 | } |
1092 | } |
1083 | tmp_int = (long) EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo y = ax + bx² |
1093 | tmp_int = (long) EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo y = ax + bx² |
1084 | tmp_int += (EE_Parameter.Gier_P * StickGier) / 4; |
1094 | tmp_int += (EE_Parameter.Gier_P * StickGier) / 4; |
1085 | sollGier = tmp_int; |
1095 | sollGier = tmp_int; |
- | 1096 | DebugOut.Analog[21] = tmp_int; |
|
1086 | Mess_Integral_Gier -= tmp_int; |
1097 | Mess_Integral_Gier -= tmp_int; |
1087 | if(Mess_Integral_Gier > 50000) Mess_Integral_Gier = 50000; // begrenzen |
1098 | if(Mess_Integral_Gier > 50000) Mess_Integral_Gier = 50000; // begrenzen |
1088 | if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000; |
1099 | if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000; |
Line 1089... | Line 1100... | ||
1089 | 1100 |