Rev 668 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 668 | Rev 670 | ||
|---|---|---|---|
| Line 14... | Line 14... | ||
| 14 | /*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
14 | /*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 15 | von Peter Muehlenbrock alias Salvo |
15 | von Peter Muehlenbrock alias Salvo |
| 16 | Auswertung der Daten vom GPS im ublox Format |
16 | Auswertung der Daten vom GPS im ublox Format |
| 17 | Hold Modus mit PID Regler |
17 | Hold Modus mit PID Regler |
| 18 | Rückstuerz zur Basis Funktion |
18 | Rückstuerz zur Basis Funktion |
| 19 | Stand 14.1.2008 |
19 | Stand 20.1.2008 |
| Line 20... | Line 20... | ||
| 20 | 20 | ||
| 21 | ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
21 | ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 22 | */ |
22 | */ |
| 23 | #include "main.h" |
23 | #include "main.h" |
| Line 529... | Line 529... | ||
| 529 | int_east = 0; |
529 | int_east = 0; |
| 530 | int_north = 0; |
530 | int_north = 0; |
| 531 | } |
531 | } |
| Line 532... | Line 532... | ||
| 532 | 532 | ||
| 533 | //I Werte begrenzen |
533 | //I Werte begrenzen |
| 534 | #define INT1_MAX (GPS_NICKROLL_MAX * GPS_V*3)/10// auf 30 Prozent des maximalen Nick/Rollwert begrenzen |
534 | #define INT1_MAX (GPS_NICKROLL_MAX * GPS_V*5)/10// auf 40 Prozent des maximalen Nick/Rollwert begrenzen |
| 535 | int_east1 = ((((long)int_east) * Parameter_UserParam2)/32)/GPS_USR_PAR_FKT; |
535 | int_east1 = ((((long)int_east) * Parameter_UserParam2)/32)/GPS_USR_PAR_FKT; |
| 536 | int_north1 = ((((long)int_north) * Parameter_UserParam2)/32)/GPS_USR_PAR_FKT; |
536 | int_north1 = ((((long)int_north) * Parameter_UserParam2)/32)/GPS_USR_PAR_FKT; |
| 537 | if (int_east1 > INT1_MAX) int_east1 = INT1_MAX; //begrenzen |
537 | if (int_east1 > INT1_MAX) int_east1 = INT1_MAX; //begrenzen |
| 538 | else if (int_east1 < -INT1_MAX) int_east1 = -INT1_MAX; |
538 | else if (int_east1 < -INT1_MAX) int_east1 = -INT1_MAX; |
| Line 543... | Line 543... | ||
| 543 | { |
543 | { |
| 544 | amplfy_speed_east = DIFF_Y_F_MAX; |
544 | amplfy_speed_east = DIFF_Y_F_MAX; |
| 545 | amplfy_speed_north = DIFF_Y_F_MAX; |
545 | amplfy_speed_north = DIFF_Y_F_MAX; |
| 546 | amplfy_speed_east *= (Parameter_UserParam3/GPS_USR_PAR_FKT); |
546 | amplfy_speed_east *= (Parameter_UserParam3/GPS_USR_PAR_FKT); |
| 547 | amplfy_speed_north *= (Parameter_UserParam3/GPS_USR_PAR_FKT); |
547 | amplfy_speed_north *= (Parameter_UserParam3/GPS_USR_PAR_FKT); |
| 548 | speed_east = (speed_east * (long)amplfy_speed_east) /100; |
548 | speed_east = (speed_east * (long)amplfy_speed_east) /50; |
| 549 | speed_north = (speed_north * (long)amplfy_speed_north)/100; |
549 | speed_north = (speed_north * (long)amplfy_speed_north)/50; |
| 550 | // D Werte begrenzen |
550 | // D Werte begrenzen |
| 551 | #define D_F_MAX (GPS_NICKROLL_MAX * GPS_V*7)/10 // auf xx Prozent des Maximalen Nick/Rollwert begrenzen |
551 | #define D_F_MAX (GPS_NICKROLL_MAX * GPS_V*7)/10 // auf 70 Prozent des Maximalen Nick/Rollwert begrenzen |
| 552 | if (speed_east > D_F_MAX) speed_east = D_F_MAX; |
552 | if (speed_east > D_F_MAX) speed_east = D_F_MAX; |
| 553 | else if (speed_east < -D_F_MAX) speed_east = -D_F_MAX; |
553 | else if (speed_east < -D_F_MAX) speed_east = -D_F_MAX; |
| 554 | if (speed_north > D_F_MAX) speed_north = D_F_MAX; |
554 | if (speed_north > D_F_MAX) speed_north = D_F_MAX; |
| 555 | else if (speed_north < -D_F_MAX) speed_north = -D_F_MAX; |
555 | else if (speed_north < -D_F_MAX) speed_north = -D_F_MAX; |
| Line 560... | Line 560... | ||
| 560 | { |
560 | { |
| 561 | amplfy_speed_east = DIFF_Y_N_MAX; |
561 | amplfy_speed_east = DIFF_Y_N_MAX; |
| 562 | amplfy_speed_north = DIFF_Y_N_MAX; |
562 | amplfy_speed_north = DIFF_Y_N_MAX; |
| 563 | amplfy_speed_east *= (Parameter_UserParam3/GPS_USR_PAR_FKT); |
563 | amplfy_speed_east *= (Parameter_UserParam3/GPS_USR_PAR_FKT); |
| 564 | amplfy_speed_north *= (Parameter_UserParam3/GPS_USR_PAR_FKT); |
564 | amplfy_speed_north *= (Parameter_UserParam3/GPS_USR_PAR_FKT); |
| 565 | speed_east = (speed_east * (long)amplfy_speed_east) /50; |
565 | speed_east = (speed_east * (long)amplfy_speed_east) /25; |
| 566 | speed_north = (speed_north * (long)amplfy_speed_north)/50; |
566 | speed_north = (speed_north * (long)amplfy_speed_north)/25; |
| 567 | // D Werte begrenzen |
567 | // D Werte begrenzen |
| 568 | #define D_N_MAX (GPS_NICKROLL_MAX * GPS_V*8)/10 // auf xx Prozent des Maximalen Nick/Rollwert begrenzen |
568 | #define D_N_MAX (GPS_NICKROLL_MAX * GPS_V*8)/10 // auf 80 Prozent des Maximalen Nick/Rollwert begrenzen |
| 569 | if (speed_east > D_N_MAX) speed_east = D_N_MAX; |
569 | if (speed_east > D_N_MAX) speed_east = D_N_MAX; |
| 570 | else if (speed_east < -D_N_MAX) speed_east = -D_N_MAX; |
570 | else if (speed_east < -D_N_MAX) speed_east = -D_N_MAX; |
| 571 | if (speed_north > D_N_MAX) speed_north = D_N_MAX; |
571 | if (speed_north > D_N_MAX) speed_north = D_N_MAX; |
| 572 | else if (speed_north < -D_N_MAX) speed_north = -D_N_MAX; |
572 | else if (speed_north < -D_N_MAX) speed_north = -D_N_MAX; |
| Line 582... | Line 582... | ||
| 582 | delta_north = (delta_north * (long)diff_p)/(20); |
582 | delta_north = (delta_north * (long)diff_p)/(20); |
| Line 583... | Line 583... | ||
| 583 | 583 | ||
| 584 | if (hold_fast > 0) //schneller Coming Home Modus |
584 | if (hold_fast > 0) //schneller Coming Home Modus |
| 585 | { |
585 | { |
| 586 | // P Werte begrenzen |
586 | // P Werte begrenzen |
| 587 | #define P1_F_MAX (GPS_NICKROLL_MAX * GPS_V*8)/10 // auf xx Prozent des Maximalen Nick/Rollwert begrenzen |
587 | #define P1_F_MAX (GPS_NICKROLL_MAX * GPS_V*8)/10 // auf 80 Prozent des Maximalen Nick/Rollwert begrenzen |
| 588 | if (delta_east > P1_F_MAX) delta_east = P1_F_MAX; |
588 | if (delta_east > P1_F_MAX) delta_east = P1_F_MAX; |
| 589 | else if (delta_east < -P1_F_MAX) delta_east = -P1_F_MAX; |
589 | else if (delta_east < -P1_F_MAX) delta_east = -P1_F_MAX; |
| 590 | if (delta_north > P1_F_MAX) delta_north = P1_F_MAX; |
590 | if (delta_north > P1_F_MAX) delta_north = P1_F_MAX; |
| 591 | else if (delta_north < -P1_F_MAX) delta_north = -P1_F_MAX; |
591 | else if (delta_north < -P1_F_MAX) delta_north = -P1_F_MAX; |
| 592 | } |
592 | } |
| 593 | else // Hold modus |
593 | else // Hold modus |
| 594 | { |
594 | { |
| 595 | // P Werte begrenzen |
595 | // P Werte begrenzen |
| 596 | #define P1_N_MAX (GPS_NICKROLL_MAX * GPS_V*7)/10 // auf xx Prozent des Maximalen Nick/Rollwert begrenzen |
596 | #define P1_N_MAX (GPS_NICKROLL_MAX * GPS_V*7)/10 // auf 70 Prozent des Maximalen Nick/Rollwert begrenzen |
| 597 | if (delta_east > P1_N_MAX) delta_east = P1_N_MAX; |
597 | if (delta_east > P1_N_MAX) delta_east = P1_N_MAX; |
| 598 | else if (delta_east < -P1_N_MAX) delta_east = -P1_N_MAX; |
598 | else if (delta_east < -P1_N_MAX) delta_east = -P1_N_MAX; |
| 599 | if (delta_north > P1_N_MAX) delta_north = P1_N_MAX; |
599 | if (delta_north > P1_N_MAX) delta_north = P1_N_MAX; |
| 600 | else if (delta_north < -P1_N_MAX) delta_north = -P1_N_MAX; |
600 | else if (delta_north < -P1_N_MAX) delta_north = -P1_N_MAX; |