Rev 1166 | Rev 1171 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1166 | Rev 1167 | ||
---|---|---|---|
Line 266... | Line 266... | ||
266 | RohMesswertNick = MesswertNick; |
266 | RohMesswertNick = MesswertNick; |
267 | RohMesswertRoll = MesswertRoll; |
267 | RohMesswertRoll = MesswertRoll; |
268 | //DebugOut.Analog[21] = MesswertNick; |
268 | //DebugOut.Analog[21] = MesswertNick; |
269 | //DebugOut.Analog[22] = MesswertRoll; |
269 | //DebugOut.Analog[22] = MesswertRoll; |
270 | //DebugOut.Analog[22] = Mess_Integral_Gier; |
270 | //DebugOut.Analog[22] = Mess_Integral_Gier; |
271 | - | ||
272 | //DebugOut.Analog[21] = MesswertNick; |
271 | //DebugOut.Analog[21] = MesswertNick; |
273 | //DebugOut.Analog[22] = MesswertRoll; |
272 | //DebugOut.Analog[22] = MesswertRoll; |
Line 274... | Line 273... | ||
274 | 273 | ||
275 | // Beschleunigungssensor ++++++++++++++++++++++++++++++++++++++++++++++++ |
274 | // Beschleunigungssensor ++++++++++++++++++++++++++++++++++++++++++++++++ |
Line 332... | Line 331... | ||
332 | 331 | ||
333 | // Kompasswert begrenzen ++++++++++++++++++++++++++++++++++++++++++++++++ |
332 | // Kompasswert begrenzen ++++++++++++++++++++++++++++++++++++++++++++++++ |
334 | if(ErsatzKompass >= (360L * GIER_GRAD_FAKTOR)) ErsatzKompass -= 360L * GIER_GRAD_FAKTOR; // 360° Umschlag |
333 | if(ErsatzKompass >= (360L * GIER_GRAD_FAKTOR)) ErsatzKompass -= 360L * GIER_GRAD_FAKTOR; // 360° Umschlag |
335 | if(ErsatzKompass < 0) ErsatzKompass += 360L * GIER_GRAD_FAKTOR; |
334 | if(ErsatzKompass < 0) ErsatzKompass += 360L * GIER_GRAD_FAKTOR; |
336 | // Roll ++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
337 | // MesswertRoll += tmpl; |
- | |
338 | // MesswertRoll += tmpl2 / 100L; // War: *5/512 |
- | |
339 | // MesswertRoll += (tmpl2*Parameter_AchsGegenKopplung1)/512L; //109 |
335 | // Roll ++++++++++++++++++++++++++++++++++++++++++++++++ |
340 | Mess_IntegralRoll2 += MesswertRoll + TrimRoll; |
336 | Mess_IntegralRoll2 += MesswertRoll + TrimRoll; |
341 | Mess_IntegralRoll += MesswertRoll + TrimRoll - LageKorrekturRoll; |
- | |
342 | 337 | Mess_IntegralRoll += MesswertRoll + TrimRoll - LageKorrekturRoll; |
|
343 | if(Mess_IntegralRoll > Umschlag180Roll) |
338 | if(Mess_IntegralRoll > Umschlag180Roll) |
344 | { |
339 | { |
345 | Mess_IntegralRoll = -(Umschlag180Roll - 25000L); |
340 | Mess_IntegralRoll = -(Umschlag180Roll - 25000L); |
346 | Mess_IntegralRoll2 = Mess_IntegralRoll; |
341 | Mess_IntegralRoll2 = Mess_IntegralRoll; |
347 | } |
342 | } |
348 | if(Mess_IntegralRoll <-Umschlag180Roll) |
343 | if(Mess_IntegralRoll <-Umschlag180Roll) |
349 | { |
344 | { |
350 | Mess_IntegralRoll = (Umschlag180Roll - 25000L); |
345 | Mess_IntegralRoll = (Umschlag180Roll - 25000L); |
351 | Mess_IntegralRoll2 = Mess_IntegralRoll; |
346 | Mess_IntegralRoll2 = Mess_IntegralRoll; |
352 | } |
- | |
353 | if(AdWertRoll < 15) MesswertRoll = -1000; |
- | |
354 | if(AdWertRoll < 7) MesswertRoll = -2000; |
- | |
355 | if(PlatinenVersion == 10) |
- | |
356 | { |
- | |
357 | if(AdWertRoll > 1010) MesswertRoll = +1000; |
- | |
358 | if(AdWertRoll > 1017) MesswertRoll = +2000; |
- | |
359 | } |
- | |
360 | else |
- | |
361 | { |
- | |
362 | if(AdWertRoll > 2000) MesswertRoll = +1000; |
- | |
363 | if(AdWertRoll > 2015) MesswertRoll = +2000; |
- | |
364 | } |
347 | } |
365 | // Nick ++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
366 | // MesswertNick -= tmpl2; |
- | |
367 | // MesswertNick -= (tmpl*Parameter_AchsGegenKopplung1)/512L; |
- | |
368 | // MesswertNick -= tmpl / 100L; //109 |
348 | // Nick ++++++++++++++++++++++++++++++++++++++++++++++++ |
369 | Mess_IntegralNick2 += MesswertNick + TrimNick; |
349 | Mess_IntegralNick2 += MesswertNick + TrimNick; |
370 | Mess_IntegralNick += MesswertNick + TrimNick - LageKorrekturNick; |
- | |
371 | 350 | Mess_IntegralNick += MesswertNick + TrimNick - LageKorrekturNick; |
|
372 | if(Mess_IntegralNick > Umschlag180Nick) |
351 | if(Mess_IntegralNick > Umschlag180Nick) |
373 | { |
352 | { |
374 | Mess_IntegralNick = -(Umschlag180Nick - 25000L); |
353 | Mess_IntegralNick = -(Umschlag180Nick - 25000L); |
375 | Mess_IntegralNick2 = Mess_IntegralNick; |
354 | Mess_IntegralNick2 = Mess_IntegralNick; |
376 | } |
355 | } |
377 | if(Mess_IntegralNick <-Umschlag180Nick) |
356 | if(Mess_IntegralNick <-Umschlag180Nick) |
378 | { |
357 | { |
379 | Mess_IntegralNick = (Umschlag180Nick - 25000L); |
358 | Mess_IntegralNick = (Umschlag180Nick - 25000L); |
380 | Mess_IntegralNick2 = Mess_IntegralNick; |
359 | Mess_IntegralNick2 = Mess_IntegralNick; |
381 | } |
- | |
382 | if(AdWertNick < 15) MesswertNick = -1000; |
- | |
383 | if(AdWertNick < 7) MesswertNick = -2000; |
- | |
384 | if(PlatinenVersion == 10) |
- | |
385 | { |
- | |
386 | if(AdWertNick > 1010) MesswertNick = +1000; |
- | |
387 | if(AdWertNick > 1017) MesswertNick = +2000; |
- | |
388 | } |
- | |
389 | else |
- | |
390 | { |
- | |
391 | if(AdWertNick > 2000) MesswertNick = +1000; |
- | |
392 | if(AdWertNick > 2015) MesswertNick = +2000; |
- | |
Line 393... | Line 360... | ||
393 | } |
360 | } |
394 | 361 | ||
395 | Integral_Gier = Mess_Integral_Gier; |
362 | Integral_Gier = Mess_Integral_Gier; |
396 | IntegralNick = Mess_IntegralNick; |
363 | IntegralNick = Mess_IntegralNick; |
397 | IntegralRoll = Mess_IntegralRoll; |
364 | IntegralRoll = Mess_IntegralRoll; |
398 | IntegralNick2 = Mess_IntegralNick2; |
- | |
399 | IntegralRoll2 = Mess_IntegralRoll2; |
- | |
Line 400... | Line 365... | ||
400 | // MesswertNick = (RohMesswertNick + 2 * MesswertNick) / 3; |
365 | IntegralNick2 = Mess_IntegralNick2; |
Line 401... | Line 366... | ||
401 | // MesswertRoll = (RohMesswertRoll + 2 * MesswertRoll) / 3; |
366 | IntegralRoll2 = Mess_IntegralRoll2; |
402 | 367 | ||
- | 368 | #define D_LIMIT 128 |
|
- | 369 | ||
- | 370 | MesswertNick = HiResNick / 20; |
|
- | 371 | MesswertRoll = HiResRoll / 20; |
|
- | 372 | ||
- | 373 | if(AdWertNick < 15) MesswertNick = -1000; if(AdWertNick < 7) MesswertNick = -2000; |
|
- | 374 | if(PlatinenVersion == 10) { if(AdWertNick > 1010) MesswertNick = +1000; if(AdWertNick > 1017) MesswertNick = +2000; } |
|
Line 403... | Line 375... | ||
403 | #define D_LIMIT 128 |
375 | else { if(AdWertNick > 2000) MesswertNick = +1000; if(AdWertNick > 2015) MesswertNick = +2000; } |
404 | 376 | if(AdWertRoll < 15) MesswertRoll = -1000; if(AdWertRoll < 7) MesswertRoll = -2000; |
|
405 | MesswertNick = HiResNick / 20; |
377 | if(PlatinenVersion == 10) { if(AdWertRoll > 1010) MesswertRoll = +1000; if(AdWertRoll > 1017) MesswertRoll = +2000; } |
406 | MesswertRoll = HiResRoll / 20; |
378 | else { if(AdWertRoll > 2000) MesswertRoll = +1000; if(AdWertRoll > 2015) MesswertRoll = +2000; } |
Line 537... | Line 509... | ||
537 | CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255); |
509 | CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255); |
538 | CHK_POTI(Parameter_LoopGasLimit,EE_Parameter.LoopGasLimit,0,255); |
510 | CHK_POTI(Parameter_LoopGasLimit,EE_Parameter.LoopGasLimit,0,255); |
539 | CHK_POTI(Parameter_AchsKopplung1, EE_Parameter.AchsKopplung1,0,255); |
511 | CHK_POTI(Parameter_AchsKopplung1, EE_Parameter.AchsKopplung1,0,255); |
540 | CHK_POTI(Parameter_AchsKopplung2, EE_Parameter.AchsKopplung2,0,255); |
512 | CHK_POTI(Parameter_AchsKopplung2, EE_Parameter.AchsKopplung2,0,255); |
541 | CHK_POTI(Parameter_CouplingYawCorrection,EE_Parameter.CouplingYawCorrection,0,255); |
513 | CHK_POTI(Parameter_CouplingYawCorrection,EE_Parameter.CouplingYawCorrection,0,255); |
542 | - | ||
543 | // CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255); |
514 | // CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255); |
544 | CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability,0,255); |
515 | CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability,0,255); |
545 | CHK_POTI_MM(Parameter_J16Timing,EE_Parameter.J16Timing,1,255); |
516 | CHK_POTI_MM(Parameter_J16Timing,EE_Parameter.J16Timing,1,255); |
546 | CHK_POTI_MM(Parameter_J17Timing,EE_Parameter.J17Timing,1,255); |
517 | CHK_POTI_MM(Parameter_J17Timing,EE_Parameter.J17Timing,1,255); |
547 | - | ||
548 | // CHK_POTI(Parameter_NaviGpsModeControl,EE_Parameter.NaviGpsModeControl,0,255); |
- | |
549 | //CHK_POTI(Parameter_NaviGpsGain,EE_Parameter.NaviGpsGain,0,255); |
- | |
550 | // CHK_POTI(Parameter_NaviGpsP,EE_Parameter.NaviGpsP,0,255); |
- | |
551 | // CHK_POTI(Parameter_NaviGpsI,EE_Parameter.NaviGpsI,0,255); |
- | |
552 | // CHK_POTI(Parameter_NaviGpsD,EE_Parameter.NaviGpsD,0,255); |
- | |
553 | // CHK_POTI(Parameter_NaviGpsACC,EE_Parameter.NaviGpsACC,0,255); |
- | |
554 | // CHK_POTI_MM(Parameter_NaviOperatingRadius,EE_Parameter.NaviOperatingRadius,10,255); |
- | |
555 | // CHK_POTI(Parameter_NaviWindCorrection,EE_Parameter.NaviWindCorrection,0,255); |
- | |
556 | // CHK_POTI(Parameter_NaviSpeedCompensation,EE_Parameter.NaviSpeedCompensation,0,255); |
- | |
557 | - | ||
558 | CHK_POTI(Parameter_ExternalControl,EE_Parameter.ExternalControl,0,255); |
518 | CHK_POTI(Parameter_ExternalControl,EE_Parameter.ExternalControl,0,255); |
559 | - | ||
560 | //Ki = (float) Parameter_I_Faktor * 0.0001; |
- | |
561 | Ki = 10300 / (Parameter_I_Faktor + 1); |
519 | Ki = 10300 / (Parameter_I_Faktor + 1); |
562 | MAX_GAS = EE_Parameter.Gas_Max; |
520 | MAX_GAS = EE_Parameter.Gas_Max; |
563 | MIN_GAS = EE_Parameter.Gas_Min; |
521 | MIN_GAS = EE_Parameter.Gas_Min; |
564 | } |
522 | } |
Line 950... | Line 908... | ||
950 | { |
908 | { |
951 | tmp_long /= 3; |
909 | tmp_long /= 3; |
952 | tmp_long2 /= 3; |
910 | tmp_long2 /= 3; |
953 | } |
911 | } |
Line 954... | Line -... | ||
954 | - | ||
955 | 912 | ||
956 | #define AUSGLEICH 32 |
913 | #define AUSGLEICH 32 |
957 | if(tmp_long > AUSGLEICH) tmp_long = AUSGLEICH; |
914 | if(tmp_long > AUSGLEICH) tmp_long = AUSGLEICH; |
958 | if(tmp_long < -AUSGLEICH) tmp_long =-AUSGLEICH; |
915 | if(tmp_long < -AUSGLEICH) tmp_long =-AUSGLEICH; |
959 | if(tmp_long2 > AUSGLEICH) tmp_long2 = AUSGLEICH; |
916 | if(tmp_long2 > AUSGLEICH) tmp_long2 = AUSGLEICH; |
Line 1269... | Line 1226... | ||
1269 | if(TrichterFlug) { SummeRoll = 0; SummeNick = 0;}; |
1226 | if(TrichterFlug) { SummeRoll = 0; SummeNick = 0;}; |
Line 1270... | Line 1227... | ||
1270 | 1227 | ||
1271 | if(!Looping_Nick) IntegralNickMalFaktor = (IntegralNick * IntegralFaktor) / (44000 / STICK_GAIN); else IntegralNickMalFaktor = 0; |
1228 | if(!Looping_Nick) IntegralNickMalFaktor = (IntegralNick * IntegralFaktor) / (44000 / STICK_GAIN); else IntegralNickMalFaktor = 0; |
Line 1272... | Line 1229... | ||
1272 | if(!Looping_Roll) IntegralRollMalFaktor = (IntegralRoll * IntegralFaktor) / (44000 / STICK_GAIN); else IntegralRollMalFaktor = 0; |
1229 | if(!Looping_Roll) IntegralRollMalFaktor = (IntegralRoll * IntegralFaktor) / (44000 / STICK_GAIN); else IntegralRollMalFaktor = 0; |
1273 | 1230 | ||
1274 | #define TRIM_MAX (Poti1 * 2) |
1231 | #define TRIM_MAX 200 |
Line 1275... | Line 1232... | ||
1275 | if(TrimNick > TRIM_MAX) TrimNick = TRIM_MAX; else if(TrimNick <-TRIM_MAX) TrimNick =-TRIM_MAX; |
1232 | if(TrimNick > TRIM_MAX) TrimNick = TRIM_MAX; else if(TrimNick <-TRIM_MAX) TrimNick =-TRIM_MAX; |
1276 | if(TrimRoll > TRIM_MAX) TrimRoll = TRIM_MAX; else if(TrimRoll <-TRIM_MAX) TrimRoll =-TRIM_MAX; |
1233 | if(TrimRoll > TRIM_MAX) TrimRoll = TRIM_MAX; else if(TrimRoll <-TRIM_MAX) TrimRoll =-TRIM_MAX; |
Line 1522... | Line 1479... | ||
1522 | motorwert = GasMischanteil + pd_ergebnis_nick + pd_ergebnis_roll - GierMischanteil; |
1479 | motorwert = GasMischanteil + pd_ergebnis_nick + pd_ergebnis_roll - GierMischanteil; |
1523 | motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS); |
1480 | motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS); |
1524 | Motor8 = motorwert; |
1481 | Motor8 = motorwert; |
1525 | // +++++++++++++++++++++++++++++++++++++++++++++++ |
1482 | // +++++++++++++++++++++++++++++++++++++++++++++++ |
1526 | #endif |
1483 | #endif |
1527 | 1484 | /* |
|
1528 | if(Poti1 > 20) Motor1 = 0; |
1485 | if(Poti1 > 20) Motor1 = 0; |
1529 | if(Poti1 > 90) Motor6 = 0; |
1486 | if(Poti1 > 90) Motor6 = 0; |
1530 | if(Poti1 > 140) Motor2 = 0; |
1487 | if(Poti1 > 140) Motor2 = 0; |
1531 | if(Poti1 > 200) Motor7 = 0; |
1488 | if(Poti1 > 200) Motor7 = 0; |
1532 | 1489 | */ |
|
1533 | } |
1490 | } |