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 |