Rev 676 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 676 | Rev 677 | ||
|---|---|---|---|
| Line 432... | Line 432... | ||
| 432 | dist_flown +=gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit |
432 | dist_flown +=gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit |
| 433 | gps_sub_state = GPS_HOME_FAST_IN_TOL; |
433 | gps_sub_state = GPS_HOME_FAST_IN_TOL; |
| 434 | } |
434 | } |
| 435 | else if ((d1 < GPS_G2T_FAST_TOL) && (d2 < GPS_G2T_FAST_TOL)) //nur weiter wenn Lage innerhalb der Toleranz |
435 | else if ((d1 < GPS_G2T_FAST_TOL) && (d2 < GPS_G2T_FAST_TOL)) //nur weiter wenn Lage innerhalb der Toleranz |
| 436 | { |
436 | { |
| 437 | if (gps_g2t_act_v > GPS_G2T_V_MAX/2) gps_g2t_act_v -= 1; //Geschwindigkeit auf Haaelte runter oder rauffahren |
437 | if (gps_g2t_act_v > GPS_G2T_V_MAX/2) gps_g2t_act_v -= 1; //Geschwindigkeit auf Haeflte runter oder rauffahren |
| 438 | else if (gps_g2t_act_v < GPS_G2T_V_MAX/2) gps_g2t_act_v += 1; //Geschwindigkeit erhoehen |
438 | else if (gps_g2t_act_v < GPS_G2T_V_MAX/2) gps_g2t_act_v += 1; |
| 439 | dist_flown +=gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit |
439 | dist_flown +=gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit |
| 440 | gps_sub_state = GPS_HOME_FAST_IN_TOL; |
440 | gps_sub_state = GPS_HOME_FAST_IN_TOL; |
| 441 | } |
441 | } |
| 442 | else //Den Lageregler in Ruhe arbeiten lassen weil ausserhalb der Toleranz |
442 | else //Den Lageregler in Ruhe arbeiten lassen weil ausserhalb der Toleranz |
| 443 | { |
443 | { |
| 444 | if (gps_g2t_act_v > 1) gps_g2t_act_v--; // Geschwindigkeit reduzieren |
444 | if (gps_g2t_act_v > 1) gps_g2t_act_v--; // Geschwindigkeit reduzieren |
| 445 | // dist_flown++; //Auch ausserhalb der Toleranz langsam erhoehen |
445 | // dist_flown++; //Auch ausserhalb der Toleranz langsam erhoehen |
| 446 | gps_sub_state = GPS_HOME_FAST_OUTOF_TOL; |
446 | gps_sub_state = GPS_HOME_FAST_OUTOF_TOL; |
| 447 | } |
447 | } |
| 448 | hold_reset_int = 0; // Integrator einsschalten |
448 | hold_reset_int = 1; // Integrator aussschalten |
| 449 | hold_fast = 1; // Regler fuer schnellen Flug |
449 | hold_fast = 1; // Regler fuer schnellen Flug |
| 450 | dist_frm_start_east = (int)(((long)dist_flown * (long)sin_i(hdng_2home))/1000); |
450 | dist_frm_start_east = (int)(((long)dist_flown * (long)sin_i(hdng_2home))/1000); |
| 451 | dist_frm_start_north = (int)(((long)dist_flown * (long)cos_i(hdng_2home))/1000); |
451 | dist_frm_start_north = (int)(((long)dist_flown * (long)cos_i(hdng_2home))/1000); |
| 452 | gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + dist_frm_start_east; //naechster Zielpunkt |
452 | gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + dist_frm_start_east; //naechster Zielpunkt |
| 453 | gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + dist_frm_start_north; //naechster Zielpunkt |
453 | gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + dist_frm_start_north; //naechster Zielpunkt |
| Line 517... | Line 517... | ||
| 517 | gps_updte_flag = 0; // Neue Werte koennen vom GPS geholt werden |
517 | gps_updte_flag = 0; // Neue Werte koennen vom GPS geholt werden |
| 518 | dist_east = (int)delta_east; //merken |
518 | dist_east = (int)delta_east; //merken |
| 519 | dist_north = (int)delta_north; |
519 | dist_north = (int)delta_north; |
| Line 520... | Line 520... | ||
| 520 | 520 | ||
| - | 521 | ||
| 521 | 522 | // #define GPSINT_MAX 3000 // Neues Verfahren ab 30.12.2007 bei Integratoroverflow |
|
| 522 | #define GPSINT_MAX 3000 // Neues Verfahren ab 30.12.2007 bei Integratoroverflow |
523 | long int gpsintmax; |
| - | 524 | if (Parameter_UserParam2 > 0) |
|
| - | 525 | { |
|
| - | 526 | gpsintmax = (GPS_NICKROLL_MAX * GPS_V * GPS_USR_PAR_FKT * ((32*4)/10))/(long)Parameter_UserParam2; //auf ungefeahren Maximalwert begrenzen |
|
| 523 | if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) |
527 | if ((abs(int_east) > (int)gpsintmax) || (abs(int_north)> (int)gpsintmax)) |
| - | 528 | { |
|
| - | 529 | int_ovfl_cnt += 1; // Zahl der Overflows zaehlen |
|
| - | 530 | } |
|
| - | 531 | if (int_ovfl_cnt > 0) //bei Overflow Wert Integratorwert reduzieren |
|
| - | 532 | { |
|
| - | 533 | int_ovfl_cnt -= 1; |
|
| - | 534 | int_east = (int_east*7)/8; // Wert reduzieren |
|
| - | 535 | int_north = (int_north*7)/8; |
|
| - | 536 | } |
|
| - | 537 | ||
| - | 538 | if (hold_reset_int > 0) //Im Schnellen Mode Integrator abschalten |
|
| - | 539 | { |
|
| - | 540 | int_east = 0; |
|
| 524 | { |
541 | int_north = 0; |
| 525 | if (int_ovfl_cnt < 40) int_ovfl_cnt += 1; // Zahl der Overflows zaehlen |
542 | } |
| 526 | } |
543 | } |
| 527 | if (int_ovfl_cnt > 0) //bei Overflow Wert Integratorwert reduzieren |
544 | else // Integrator deaktiviert |
| 528 | { |
- | |
| 529 | int_ovfl_cnt -= 1; |
545 | { |
| 530 | int_east = (int_east*7)/8; // Wert reduzieren |
546 | int_east = 0; |
| Line 531... | Line -... | ||
| 531 | int_north = (int_north*7)/8; |
- | |
| 532 | } |
- | |
| 533 | 547 | int_north = 0; |
|
| 534 | if (hold_reset_int > 0) //Im Schnellen Mode Integrator abschalten |
548 | } |
| 535 | { |
- | |
| Line 536... | Line 549... | ||
| 536 | int_east = 0; |
549 | |
| 537 | int_north = 0; |
550 | debug_gp_4 = (int)int_east; // zum Debuggen |
| 538 | } |
551 | debug_gp_5 = (int)int_north; // zum Debuggen |
| 539 | 552 | ||
| 540 | //I Werte begrenzen |
553 | //I Werte begrenzen |
| 541 | #define INT1_MAX (GPS_NICKROLL_MAX * GPS_V*5)/10// auf 40 Prozent des maximalen Nick/Rollwert begrenzen |
554 | #define INT1_MAX (GPS_NICKROLL_MAX * GPS_V*2)/10// auf 20 Prozent des maximalen Nick/Rollwert begrenzen |
| 542 | int_east1 = ((((long)int_east) * Parameter_UserParam2)/32)/GPS_USR_PAR_FKT; |
555 | int_east1 = ((((long)int_east) * Parameter_UserParam2)/32)/GPS_USR_PAR_FKT; |
| Line 550... | Line 563... | ||
| 550 | { |
563 | { |
| 551 | amplfy_speed_east = DIFF_Y_F_MAX; |
564 | amplfy_speed_east = DIFF_Y_F_MAX; |
| 552 | amplfy_speed_north = DIFF_Y_F_MAX; |
565 | amplfy_speed_north = DIFF_Y_F_MAX; |
| 553 | amplfy_speed_east *= (Parameter_UserParam3/GPS_USR_PAR_FKT); |
566 | amplfy_speed_east *= (Parameter_UserParam3/GPS_USR_PAR_FKT); |
| 554 | amplfy_speed_north *= (Parameter_UserParam3/GPS_USR_PAR_FKT); |
567 | amplfy_speed_north *= (Parameter_UserParam3/GPS_USR_PAR_FKT); |
| 555 | speed_east = (speed_east * (long)amplfy_speed_east) /100; |
568 | speed_east = (speed_east * (long)amplfy_speed_east) /50; |
| 556 | speed_north = (speed_north * (long)amplfy_speed_north)/100; |
569 | speed_north = (speed_north * (long)amplfy_speed_north)/50; |
| 557 | // D Werte begrenzen |
570 | // D Werte begrenzen |
| 558 | #define D_F_MAX (GPS_NICKROLL_MAX * GPS_V*7)/10 // auf 70 Prozent des Maximalen Nick/Rollwert begrenzen |
571 | #define D_F_MAX (GPS_NICKROLL_MAX * GPS_V*7)/10 // auf 70 Prozent des Maximalen Nick/Rollwert begrenzen |
| 559 | if (speed_east > D_F_MAX) speed_east = D_F_MAX; |
572 | if (speed_east > D_F_MAX) speed_east = D_F_MAX; |
| 560 | else if (speed_east < -D_F_MAX) speed_east = -D_F_MAX; |
573 | else if (speed_east < -D_F_MAX) speed_east = -D_F_MAX; |
| 561 | if (speed_north > D_F_MAX) speed_north = D_F_MAX; |
574 | if (speed_north > D_F_MAX) speed_north = D_F_MAX; |