Rev 629 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 629 | Rev 631 | ||
|---|---|---|---|
| Line 368... | Line 368... | ||
| 368 | // aktuelle positionsdaten abspeichern |
368 | // aktuelle positionsdaten abspeichern |
| 369 | if (gps_rel_act_position.status > 0) |
369 | if (gps_rel_act_position.status > 0) |
| 370 | { |
370 | { |
| 371 | hold_fast = 0; |
371 | hold_fast = 0; |
| 372 | hold_reset_int = 0; // Integrator enablen |
372 | hold_reset_int = 0; // Integrator enablen |
| 373 | int_east = 0, int_north = 0; |
373 | int_east = 0, int_north = 0; |
| 374 | gps_reg_x = 0, gps_reg_y = 0; |
374 | gps_reg_x = 0, gps_reg_y = 0; |
| 375 | dist_east = 0, dist_north = 0; |
375 | dist_east = 0, dist_north = 0; |
| 376 | speed_east = 0; speed_north= 0; |
376 | speed_east = 0; speed_north= 0; |
| 377 | int_ovfl_cnt = 0; |
377 | int_ovfl_cnt = 0; |
| 378 | gps_rel_hold_position.utm_east = gps_rel_act_position.utm_east; |
378 | gps_rel_hold_position.utm_east = gps_rel_act_position.utm_east; |
| 379 | gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north; |
379 | gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north; |
| 380 | gps_rel_hold_position.status = 1; // gueltige Positionsdaten |
380 | gps_rel_hold_position.status = 1; // gueltige Positionsdaten |
| 381 | gps_state = GPS_CRTL_HOLD_ACTIVE; |
381 | gps_state = GPS_CRTL_HOLD_ACTIVE; |
| 382 | return (GPS_STST_OK); |
382 | return (GPS_STST_OK); |
| 383 | } |
383 | } |
| 384 | else |
384 | else |
| 385 | { |
385 | { |
| 386 | gps_rel_hold_position.status = 0; //Keine Daten verfuegbar |
386 | gps_rel_hold_position.status = 0; //Keine Daten verfuegbar |
| 387 | gps_state = GPS_CRTL_IDLE; |
387 | gps_state = GPS_CRTL_IDLE; |
| 388 | return(GPS_STST_ERR); // Keine Daten da |
388 | return(GPS_STST_ERR); // Keine Daten da |
| 389 | } |
389 | } |
| 390 | } |
390 | } |
| 391 | else return(GPS_STST_PEND); // noch warten |
391 | else return(GPS_STST_PEND); // noch warten |
| Line 433... | Line 433... | ||
| 433 | 433 | ||
| 434 | if (d3 > GPS_G2T_DIST_MAX_STOP) // Schneller Rueckflug, noch weit weg vom Ziel |
434 | if (d3 > GPS_G2T_DIST_MAX_STOP) // Schneller Rueckflug, noch weit weg vom Ziel |
| 435 | { |
435 | { |
| 436 | if ((d1 < GPS_G2T_FAST_TOL) && (d2 < GPS_G2T_FAST_TOL)) //nur weiter wenn Lage innerhalb der Toleranz |
436 | if ((d1 < GPS_G2T_FAST_TOL) && (d2 < GPS_G2T_FAST_TOL)) //nur weiter wenn Lage innerhalb der Toleranz |
| 437 | { |
437 | { |
| 438 | if (gps_g2t_act_v < GPS_G2T_V_MAX-3) gps_g2t_act_v += 4; //Geschwindigkeit langsam erhoehen |
438 | if (gps_g2t_act_v < GPS_G2T_V_MAX-3) gps_g2t_act_v += 4; //Geschwindigkeit flott erhoehen |
| 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 |
| Line 484... | Line 484... | ||
| 484 | else if (gps_rel_hold_position.utm_north <= - GPS_G2T_V_MIN ) gps_rel_hold_position.utm_north += GPS_G2T_V_MIN; |
484 | else if (gps_rel_hold_position.utm_north <= - GPS_G2T_V_MIN ) gps_rel_hold_position.utm_north += GPS_G2T_V_MIN; |
| 485 | if ((abs(gps_rel_hold_position.utm_east) <= GPS_G2T_V_MIN) && (abs(gps_rel_hold_position.utm_north) <=GPS_G2T_V_MIN)) |
485 | if ((abs(gps_rel_hold_position.utm_east) <= GPS_G2T_V_MIN) && (abs(gps_rel_hold_position.utm_north) <=GPS_G2T_V_MIN)) |
| 486 | { |
486 | { |
| 487 | gps_rel_hold_position.utm_east = 0; |
487 | gps_rel_hold_position.utm_east = 0; |
| 488 | gps_rel_hold_position.utm_north = 0; |
488 | gps_rel_hold_position.utm_north = 0; |
| 489 | gps_sub_state = GPS_HOME_FINISHED; |
489 | gps_sub_state = GPS_HOME_FINISHED; |
| 490 | } |
490 | } |
| 491 | } |
491 | } |
| 492 | else gps_sub_state = GPS_HOME_OUTOF_TOL; |
492 | else gps_sub_state = GPS_HOME_OUTOF_TOL; |
| 493 | } |
493 | } |
| 494 | } |
494 | } |
| Line 530... | Line 530... | ||
| 530 | if (hold_reset_int > 0) //Im Schnellen Mode Integrator abschalten |
530 | if (hold_reset_int > 0) //Im Schnellen Mode Integrator abschalten |
| 531 | { |
531 | { |
| 532 | int_east = 0; |
532 | int_east = 0; |
| 533 | int_north = 0; |
533 | int_north = 0; |
| 534 | } |
534 | } |
| 535 | if (hold_fast > 0) //schneller Coming Home Modus Einfluss des D-Anteils verkleinern |
535 | if (hold_fast > 0) //schneller Coming Home Modus |
| 536 | { |
536 | { |
| - | 537 | // Verstaerkungsfaktor fuer D-Anteil ermitteln um exponentielles Verhalten zu erzielen |
|
| - | 538 | amplfy_speed_east = (((abs(speed_east)) *(DIFF_Y_F_MAX-1)*10)/DIFF_X_F_MAX) +10; |
|
| - | 539 | amplfy_speed_north = (((abs(speed_north))*(DIFF_Y_F_MAX-1)*10)/DIFF_X_F_MAX) +10; |
|
| - | 540 | if (amplfy_speed_east > (DIFF_Y_F_MAX *10)) amplfy_speed_east = DIFF_Y_F_MAX *10; // Begrenzung |
|
| - | 541 | if (amplfy_speed_north > (DIFF_Y_F_MAX *10)) amplfy_speed_north = DIFF_Y_F_MAX *10; // Begrenzung |
|
| 537 | amplfy_speed_east = (Parameter_UserParam3/GPS_USR_PAR_FKT); |
542 | amplfy_speed_east *= (Parameter_UserParam3/GPS_USR_PAR_FKT); |
| 538 | amplfy_speed_north = (Parameter_UserParam3/GPS_USR_PAR_FKT); |
543 | amplfy_speed_north *= (Parameter_UserParam3/GPS_USR_PAR_FKT); |
| - | 544 | amplfy_speed_east /= 10; //Faktor 10 wieder rausnehmen |
|
| - | 545 | amplfy_speed_north /= 10; //Faktor 10 wieder rausnehmen |
|
| 539 | } |
546 | } |
| 540 | else //langsamer Holdmodus |
547 | else //langsamer Holdmodus |
| 541 | { |
548 | { |
| 542 | // Verstaerkungsfaktor fuer Geschwindigkeit ermitteln um exponentielles Verhalten zu erzielen |
549 | // Verstaerkungsfaktor fuer D-Anteil ermitteln um exponentielles Verhalten zu erzielen |
| 543 | amplfy_speed_east = (((abs(speed_east)) *(DIFF_Y_MAX-1)*10)/DIFF_X_MAX) +10; |
550 | amplfy_speed_east = (((abs(speed_east)) *(DIFF_Y_N_MAX-1)*10)/DIFF_X_N_MAX) +10; |
| 544 | amplfy_speed_north = (((abs(speed_north))*(DIFF_Y_MAX-1)*10)/DIFF_X_MAX) +10; |
551 | amplfy_speed_north = (((abs(speed_north))*(DIFF_Y_N_MAX-1)*10)/DIFF_X_N_MAX) +10; |
| 545 | if (amplfy_speed_east > (DIFF_Y_MAX *10)) amplfy_speed_east = DIFF_Y_MAX *10; // Begrenzung |
552 | if (amplfy_speed_east > (DIFF_Y_N_MAX *10)) amplfy_speed_east = DIFF_Y_N_MAX *10; // Begrenzung |
| 546 | if (amplfy_speed_north > (DIFF_Y_MAX *10)) amplfy_speed_north = DIFF_Y_MAX *10; // Begrenzung |
553 | if (amplfy_speed_north > (DIFF_Y_N_MAX *10)) amplfy_speed_north = DIFF_Y_N_MAX *10; // Begrenzung |
| 547 | amplfy_speed_east *= (Parameter_UserParam3/GPS_USR_PAR_FKT); |
554 | amplfy_speed_east *= (Parameter_UserParam3/GPS_USR_PAR_FKT); |
| 548 | amplfy_speed_north *= (Parameter_UserParam3/GPS_USR_PAR_FKT); |
555 | amplfy_speed_north *= (Parameter_UserParam3/GPS_USR_PAR_FKT); |
| 549 | amplfy_speed_east /= 10; //Faktor 10 wieder rausnehmen |
556 | amplfy_speed_east /= 10; //Faktor 10 wieder rausnehmen |
| 550 | amplfy_speed_north /= 10; //Faktor 10 wieder rausnehmen |
557 | amplfy_speed_north /= 10; //Faktor 10 wieder rausnehmen |
| 551 | } |
558 | } |
| Line 560... | Line 567... | ||
| 560 | else diff_p = GPS_PROP_NRML_V; |
567 | else diff_p = GPS_PROP_NRML_V; |
| Line 561... | Line 568... | ||
| 561 | 568 | ||
| 562 | //I Werte begrenzen |
569 | //I Werte begrenzen |
| 563 | #define INT1_MAX (GPS_NICKROLL_MAX * GPS_V)/2 //ab 30.12.2007 auf Halben Maximalen Nick/Rollwert begrenzen |
570 | #define INT1_MAX (GPS_NICKROLL_MAX * GPS_V)/2 //ab 30.12.2007 auf Halben Maximalen Nick/Rollwert begrenzen |
| 564 | int_east1 = ((((long)int_east) * Parameter_UserParam2)/32)/GPS_USR_PAR_FKT; |
571 | int_east1 = ((((long)int_east) * Parameter_UserParam2)/32)/GPS_USR_PAR_FKT; |
| 565 | int_north1 = ((((long)int_north) * Parameter_UserParam2)/32)/GPS_USR_PAR_FKT; //Fehler behoben am 17.12.2007 vorher int_north= |
572 | int_north1 = ((((long)int_north) * Parameter_UserParam2)/32)/GPS_USR_PAR_FKT; |
| 566 | if (int_east1 > INT1_MAX) int_east1 = INT1_MAX; //begrenzen |
573 | if (int_east1 > INT1_MAX) int_east1 = INT1_MAX; //begrenzen |
| 567 | else if (int_east1 < -INT1_MAX) int_east1 = -INT1_MAX; |
574 | else if (int_east1 < -INT1_MAX) int_east1 = -INT1_MAX; |
| 568 | if (int_north1 > INT1_MAX) int_north1 = INT1_MAX; //begrenzen |
575 | if (int_north1 > INT1_MAX) int_north1 = INT1_MAX; //begrenzen |
| Line 609... | Line 616... | ||
| 609 | if (n1 > (GPS_NICKROLL_MAX * GPS_V)) n1 = (GPS_NICKROLL_MAX * GPS_V); |
616 | if (n1 > (GPS_NICKROLL_MAX * GPS_V)) n1 = (GPS_NICKROLL_MAX * GPS_V); |
| 610 | else if (n1 < -(GPS_NICKROLL_MAX * GPS_V)) n1 = -(GPS_NICKROLL_MAX * GPS_V); |
617 | else if (n1 < -(GPS_NICKROLL_MAX * GPS_V)) n1 = -(GPS_NICKROLL_MAX * GPS_V); |
| 611 | n = n/GPS_V; |
618 | n = n/GPS_V; |
| 612 | n1 = n1/GPS_V; |
619 | n1 = n1/GPS_V; |
| 613 | //Kleine Werte verstaerken, Grosse abschwaechen |
620 | //Kleine Werte verstaerken, Grosse abschwaechen |
| 614 | /* n = sin_i((GPS_Roll*90)/(GPS_NICKROLL_MAX * GPS_V)); |
621 | /* n = sin_i((GPS_Roll*90)/(GPS_NICKROLL_MAX * GPS_V)); |
| 615 | n_l = ((long) GPS_NICKROLL_MAX * (long) n)/1000; |
622 | n_l = ((long) GPS_NICKROLL_MAX * (long) n)/1000; |
| 616 | GPS_Roll = (int) n_l; |
623 | GPS_Roll = (int) n_l; |
| 617 | n = sin_i((GPS_Nick*90)/(GPS_NICKROLL_MAX * GPS_V)); |
624 | n = sin_i((GPS_Nick*90)/(GPS_NICKROLL_MAX * GPS_V)); |
| 618 | n_l = ((long) GPS_NICKROLL_MAX * (long) n)/1000; |
625 | n_l = ((long) GPS_NICKROLL_MAX * (long) n)/1000; |
| 619 | GPS_Nick = (int) n_l; |
626 | GPS_Nick = (int) n_l; |
| 620 | */ |
627 | */ |
| Line 621... | Line 628... | ||
| 621 | 628 | ||
| 622 | if ((abs(dist_east) > GPS_DIST_MAX) || (abs(dist_north) > GPS_DIST_MAX)) // bei zu grossem Abstand abbrechen |
629 | if ((abs(dist_east) > GPS_DIST_MAX) || (abs(dist_north) > GPS_DIST_MAX)) // bei zu grossem Abstand abbrechen |