Rev 670 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 670 | Rev 676 | ||
|---|---|---|---|
| Line 424... | Line 424... | ||
| 424 | d2 = abs (gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north ); |
424 | d2 = abs (gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north ); |
| 425 | d3 = (dist_2home - dist_flown); // Restdistanz zum Ziel |
425 | d3 = (dist_2home - dist_flown); // Restdistanz zum Ziel |
| Line 426... | Line 426... | ||
| 426 | 426 | ||
| 427 | if (d3 > GPS_G2T_DIST_MAX_STOP) // Schneller Rueckflug, noch weit weg vom Ziel |
427 | if (d3 > GPS_G2T_DIST_MAX_STOP) // Schneller Rueckflug, noch weit weg vom Ziel |
| 428 | { |
428 | { |
| 429 | if ((d1 < GPS_G2T_FAST_TOL) && (d2 < GPS_G2T_FAST_TOL)) //nur weiter wenn Lage innerhalb der Toleranz |
429 | if ((d1 < GPS_G2T_FAST_TOL/2) && (d2 < GPS_G2T_FAST_TOL/2)) //voll Stoff weiter wenn Lage gut innerhalb der Toleranz |
| 430 | { |
430 | { |
| 431 | if (gps_g2t_act_v < GPS_G2T_V_MAX-3) gps_g2t_act_v += 4; //Geschwindigkeit erhoehen |
431 | if (gps_g2t_act_v < GPS_G2T_V_MAX-3) gps_g2t_act_v += 4; //Geschwindigkeit erhoehen |
| 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 | } |
|
| - | 435 | else if ((d1 < GPS_G2T_FAST_TOL) && (d2 < GPS_G2T_FAST_TOL)) //nur weiter wenn Lage innerhalb der Toleranz |
|
| - | 436 | { |
|
| - | 437 | if (gps_g2t_act_v > GPS_G2T_V_MAX/2) gps_g2t_act_v -= 1; //Geschwindigkeit auf Haaelte runter oder rauffahren |
|
| - | 438 | else if (gps_g2t_act_v < GPS_G2T_V_MAX/2) gps_g2t_act_v += 1; //Geschwindigkeit erhoehen |
|
| - | 439 | dist_flown +=gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit |
|
| - | 440 | gps_sub_state = GPS_HOME_FAST_IN_TOL; |
|
| 434 | } |
441 | } |
| 435 | else //Den Lageregler in Ruhe arbeiten lassen weil ausserhalb der Toleranz |
442 | else //Den Lageregler in Ruhe arbeiten lassen weil ausserhalb der Toleranz |
| 436 | { |
443 | { |
| 437 | 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 |
| 438 | // dist_flown++; //Auch ausserhalb der Toleranz langsam erhoehen |
445 | // dist_flown++; //Auch ausserhalb der Toleranz langsam erhoehen |
| Line 543... | Line 550... | ||
| 543 | { |
550 | { |
| 544 | amplfy_speed_east = DIFF_Y_F_MAX; |
551 | amplfy_speed_east = DIFF_Y_F_MAX; |
| 545 | amplfy_speed_north = DIFF_Y_F_MAX; |
552 | amplfy_speed_north = DIFF_Y_F_MAX; |
| 546 | amplfy_speed_east *= (Parameter_UserParam3/GPS_USR_PAR_FKT); |
553 | amplfy_speed_east *= (Parameter_UserParam3/GPS_USR_PAR_FKT); |
| 547 | amplfy_speed_north *= (Parameter_UserParam3/GPS_USR_PAR_FKT); |
554 | amplfy_speed_north *= (Parameter_UserParam3/GPS_USR_PAR_FKT); |
| 548 | speed_east = (speed_east * (long)amplfy_speed_east) /50; |
555 | speed_east = (speed_east * (long)amplfy_speed_east) /100; |
| 549 | speed_north = (speed_north * (long)amplfy_speed_north)/50; |
556 | speed_north = (speed_north * (long)amplfy_speed_north)/100; |
| 550 | // D Werte begrenzen |
557 | // D Werte begrenzen |
| 551 | #define D_F_MAX (GPS_NICKROLL_MAX * GPS_V*7)/10 // auf 70 Prozent des Maximalen Nick/Rollwert begrenzen |
558 | #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; |
559 | if (speed_east > D_F_MAX) speed_east = D_F_MAX; |
| 553 | else if (speed_east < -D_F_MAX) speed_east = -D_F_MAX; |
560 | else if (speed_east < -D_F_MAX) speed_east = -D_F_MAX; |
| 554 | if (speed_north > D_F_MAX) speed_north = D_F_MAX; |
561 | if (speed_north > D_F_MAX) speed_north = D_F_MAX; |