Rev 236 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 236 | Rev 241 | ||
|---|---|---|---|
| Line 327... | Line 327... | ||
| 327 | gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north; |
327 | gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north; |
| 328 | gps_rel_hold_position.status = 1; // gueltige Positionsdaten |
328 | gps_rel_hold_position.status = 1; // gueltige Positionsdaten |
| 329 | //Richtung zur Home Positionbezogen auf Nordpol bestimmen |
329 | //Richtung zur Home Positionbezogen auf Nordpol bestimmen |
| 330 | hdng_2home = arctan_i(-gps_rel_start_position.utm_east,-gps_rel_start_position.utm_north); |
330 | hdng_2home = arctan_i(-gps_rel_start_position.utm_east,-gps_rel_start_position.utm_north); |
| 331 | // in Winkel 0...360 Grad umrechnen |
331 | // in Winkel 0...360 Grad umrechnen |
| 332 | if ((-gps_rel_start_position.utm_east >= 0)) hdng_2home = ( 90-hdng_2home); |
332 | if (( gps_rel_start_position.utm_east < 0)) hdng_2home = ( 90-hdng_2home); |
| 333 | else hdng_2home = (270 - hdng_2home); |
333 | else hdng_2home = (270 - hdng_2home); |
| 334 | dist_2home = (int) get_dist(gps_rel_start_position.utm_east,gps_rel_start_position.utm_north,hdng_2home); //Entfernung zur Home Position bestimmen |
334 | dist_2home = (int) get_dist(gps_rel_start_position.utm_east,gps_rel_start_position.utm_north,hdng_2home); //Entfernung zur Home Position bestimmen |
| 335 | gps_state = GPS_CRTL_HOME_ACTIVE; |
335 | gps_state = GPS_CRTL_HOME_ACTIVE; |
| 336 | return (GPS_STST_OK); |
336 | return (GPS_STST_OK); |
| 337 | } |
337 | } |
| Line 413... | Line 413... | ||
| 413 | int d1,d2,d3; |
413 | int d1,d2,d3; |
| 414 | d1 = abs (gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east ); |
414 | d1 = abs (gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east ); |
| 415 | d2 = abs (gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north ); |
415 | d2 = abs (gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north ); |
| 416 | d3 = (dist_2home - (int)dist_flown); // Restdistanz zum Ziel |
416 | d3 = (dist_2home - (int)dist_flown); // Restdistanz zum Ziel |
| Line 417... | Line 417... | ||
| 417 | 417 | ||
| 418 | if (d3 > GPS_G2T_DIST_MAX_STOP) |
418 | if (d3 > GPS_G2T_DIST_MAX_STOP) |
| - | 419 | { |
|
| - | 420 | hold_fast = 1; // Schnell Regeln |
|
| 419 | { |
421 | hold_reset_int = 1; // Integrator ausschalten |
| 420 | if ((d1 < GPS_G2T_TOL) && (d2 < GPS_G2T_TOL)) //nur weiter wenn Lage innerhalb der Toleranz |
422 | if ((d1 < GPS_G2T_TOL) && (d2 < GPS_G2T_TOL)) //nur weiter wenn Lage innerhalb der Toleranz |
| 421 | { |
- | |
| 422 | hold_fast = 1; // Schnell Regeln |
423 | { |
| 423 | if (gps_g2t_act_v < GPS_G2T_V_MAX) gps_g2t_act_v++; //Geschwindigkeit langsam erhoehen |
424 | if (gps_g2t_act_v < GPS_G2T_V_MAX) gps_g2t_act_v++; //Geschwindigkeit langsam erhoehen |
| 424 | dist_flown += (long)gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit |
425 | dist_flown += (long)gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit |
| 425 | dist_frm_start_east = (int)((dist_flown * (long)sin_i(hdng_2home))/1000); |
426 | dist_frm_start_east = (int)((dist_flown * (long)sin_i(hdng_2home))/1000); |
| 426 | dist_frm_start_north = (int)((dist_flown * (long)cos_i(hdng_2home))/1000); |
427 | dist_frm_start_north = (int)((dist_flown * (long)cos_i(hdng_2home))/1000); |
| 427 | gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + dist_frm_start_east; //naechster Zielpunkt |
428 | gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + dist_frm_start_east; //naechster Zielpunkt |
| - | 429 | gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + dist_frm_start_north; //naechster Zielpunkt |
|
| 428 | gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + dist_frm_start_north; //naechster Zielpunkt |
430 | gps_sub_state = GPS_HOME_FAST_IN_TOL; |
| 429 | } |
431 | } |
| 430 | else //Den Lageregler in Ruhe arbeiten lassen weil ausserhalb der Toleranz |
432 | else //Den Lageregler in Ruhe arbeiten lassen weil ausserhalb der Toleranz |
| - | 433 | { |
|
| - | 434 | /* if (gps_sub_state == GPS_HOME_FAST_IN_TOL) hold_reset_int = 1; //Integrator einmal am Beginn des normalen Regelns resetten |
|
| 431 | { |
435 | else hold_reset_int = 0; |
| 432 | if (gps_g2t_act_v > 0) gps_g2t_act_v--; |
436 | */ if (gps_g2t_act_v > 0) gps_g2t_act_v--; |
| 433 | if (gps_g2t_act_v <= (GPS_G2T_V_MAX/2)) hold_fast = 0; // Wieder normal regeln |
437 | gps_sub_state = GPS_HOME_FAST_OUTOF_TOL; |
| 434 | } |
438 | } |
| 435 | } |
439 | } |
| 436 | else //Schon ziemlich nahe am Ziel, deswegen abbremsen |
440 | else //Schon ziemlich nahe am Ziel, deswegen abbremsen |
| 437 | { |
- | |
| 438 | hold_fast = 0; // Wieder normal regeln |
441 | { |
| 439 | if ((d1 < GPS_G2T_TOL) && (d2 < GPS_G2T_TOL)) //nur weiter wenn Lage innerhalb der Toleranz |
442 | if ((d1 < GPS_G2T_TOL) && (d2 < GPS_G2T_TOL)) //nur weiter wenn Lage innerhalb der Toleranz |
| - | 443 | { |
|
| 440 | { |
444 | gps_sub_state = GPS_HOME_RMPDWN_IN_TOL; |
| 441 | if (d3 > GPS_G2T_DIST_HOLD) |
445 | if (d3 > GPS_G2T_DIST_HOLD) |
| 442 | { |
446 | { |
| 443 | if (gps_g2t_act_v < GPS_G2T_V_RAMP_DWN) gps_g2t_act_v++; //Geschwindigkeit langsam erhoehen |
447 | if (gps_g2t_act_v < GPS_G2T_V_RAMP_DWN) gps_g2t_act_v++; //Geschwindigkeit langsam erhoehen |
| 444 | dist_flown += (long)gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit |
448 | dist_flown += (long)gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit |
| Line 447... | Line 451... | ||
| 447 | gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + (int)dist_frm_start_east; //naechster Zielpunkt |
451 | gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + (int)dist_frm_start_east; //naechster Zielpunkt |
| 448 | gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + (int)dist_frm_start_north; //naechster Zielpunkt |
452 | gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + (int)dist_frm_start_north; //naechster Zielpunkt |
| 449 | } |
453 | } |
| 450 | else //Ziel fast erreicht, Jetzt noch Reste ausgleichen, weil Zielpunkt nicht exakt bestimmt werden konnte (Fehler in Winkelfkt) |
454 | else //Ziel fast erreicht, Jetzt noch Reste ausgleichen, weil Zielpunkt nicht exakt bestimmt werden konnte (Fehler in Winkelfkt) |
| 451 | { |
455 | { |
| - | 456 | hold_fast = 0; // Wieder normal regeln |
|
| - | 457 | hold_reset_int = 0; // Integrator wieder aktivieren |
|
| 452 | if (gps_rel_hold_position.utm_east > 1) gps_rel_hold_position.utm_east--; |
458 | if (gps_rel_hold_position.utm_east > GPS_G2T_V_MIN) gps_rel_hold_position.utm_east -= GPS_G2T_V_MIN; |
| 453 | else if (gps_rel_hold_position.utm_east <-1 ) gps_rel_hold_position.utm_east++; |
459 | else if (gps_rel_hold_position.utm_east < -GPS_G2T_V_MIN ) gps_rel_hold_position.utm_east += GPS_G2T_V_MIN; |
| 454 | if (gps_rel_hold_position.utm_north > 1) gps_rel_hold_position.utm_north--; |
460 | if (gps_rel_hold_position.utm_north > GPS_G2T_V_MIN) gps_rel_hold_position.utm_north -= GPS_G2T_V_MIN; |
| 455 | else if (gps_rel_hold_position.utm_north < -1 ) gps_rel_hold_position.utm_north++; |
461 | else if (gps_rel_hold_position.utm_north < - GPS_G2T_V_MIN ) gps_rel_hold_position.utm_north += GPS_G2T_V_MIN; |
| 456 | if ((abs(gps_rel_hold_position.utm_east) <= 1) && (abs(gps_rel_hold_position.utm_north) <=1)) gps_sub_state = GPS_HOME_FINISHED; |
462 | if ((abs(gps_rel_hold_position.utm_east) <= GPS_G2T_V_MIN) && (abs(gps_rel_hold_position.utm_north) <=GPS_G2T_V_MIN)) |
| - | 463 | { |
|
| - | 464 | gps_rel_hold_position.utm_east = 0; |
|
| - | 465 | gps_rel_hold_position.utm_north = 0; |
|
| - | 466 | gps_sub_state = GPS_HOME_FINISHED; |
|
| - | 467 | } |
|
| 457 | } |
468 | } |
| 458 | } |
469 | } |
| 459 | else |
470 | else |
| 460 | { |
471 | { |
| - | 472 | gps_sub_state = GPS_HOME_RMPDWN_OUTOF_TOL; |
|
| 461 | if (gps_g2t_act_v > 0) gps_g2t_act_v--; |
473 | if (gps_g2t_act_v > 0) gps_g2t_act_v--; |
| 462 | } |
474 | } |
| 463 | } |
475 | } |
| 464 | gps_state = GPS_CRTL_HOLD_ACTIVE; //Zwischensprung |
476 | gps_state = GPS_CRTL_HOLD_ACTIVE; //Zwischensprung |
| 465 | return (GPS_STST_OK); |
477 | return (GPS_STST_OK); |
| Line 512... | Line 524... | ||
| 512 | phi = arctan_i(abs(dist_north),abs(dist_east)); |
524 | phi = arctan_i(abs(dist_north),abs(dist_east)); |
| 513 | dist = get_dist(dist_east,dist_north,phi); //Zunaechst Entfernung zum Ziel ermitteln |
525 | dist = get_dist(dist_east,dist_north,phi); //Zunaechst Entfernung zum Ziel ermitteln |
| Line 514... | Line 526... | ||
| 514 | 526 | ||
| 515 | diff_v = (int)((dist * (GPS_DIFF_MAX_V - 10)) / GPS_DIFF_MAX_D) +10; //Verstaerkung * 10 |
527 | diff_v = (int)((dist * (GPS_DIFF_MAX_V - 10)) / GPS_DIFF_MAX_D) +10; //Verstaerkung * 10 |
| 516 | if (diff_v > GPS_DIFF_MAX_V) diff_v = GPS_DIFF_MAX_V; //begrenzen |
528 | if (diff_v > GPS_DIFF_MAX_V) diff_v = GPS_DIFF_MAX_V; //begrenzen |
| Line 517... | Line 529... | ||
| 517 | if (hold_fast > 0) diff_v = 10; //im schnellen Modus schwache Wirkung des Differenzierers |
529 | // if (hold_fast > 0) diff_v = 10; //im schnellen Modus schwache Wirkung des Differenzierers |
| 518 | 530 | ||
| 519 | //I Werte begrenzen |
531 | //I Werte begrenzen |
| 520 | #define INT1_MAX (20 * GPS_V) |
532 | #define INT1_MAX (20 * GPS_V) |
| 521 | int_east1 = ((((long)int_east) * Parameter_UserParam2)/32); |
533 | int_east1 = ((((long)int_east) * Parameter_UserParam2)/32); |
| 522 | int_north = ((((long)int_north) * Parameter_UserParam2)/32); |
534 | int_north = ((((long)int_north) * Parameter_UserParam2)/32); |
| 523 | if (int_east1 > INT1_MAX) int_east1 = INT1_MAX; //begrenzen |
535 | if (int_east1 > INT1_MAX) int_east1 = INT1_MAX; //begrenzen |
| 524 | else if (int_east1 < -INT1_MAX) int_east1 = -INT1_MAX; |
536 | else if (int_east1 < -INT1_MAX) int_east1 = -INT1_MAX; |
| Line -... | Line 537... | ||
| - | 537 | if (int_north1 > INT1_MAX) int_north1 = INT1_MAX; //begrenzen |
|
| - | 538 | else if (int_north1 < -INT1_MAX) int_north1 = -INT1_MAX; |
|
| - | 539 | ||
| - | 540 | int diff_p; |
|
| 525 | if (int_north1 > INT1_MAX) int_north1 = INT1_MAX; //begrenzen |
541 | if (hold_fast > 0) diff_p = 2; //im schnellen Modus Proportionalanteil staerken |
| 526 | else if (int_north1 < -INT1_MAX) int_north1 = -INT1_MAX; |
542 | else diff_p = 1; |
| 527 | 543 | ||
| Line 528... | Line 544... | ||
| 528 | //PID Regler Werte aufsummieren |
544 | //PID Regler Werte aufsummieren |
| 529 | gps_reg_x = ((int)int_east1 + ((dist_east * Parameter_UserParam1)/8)+ ((diff_east * diff_v * Parameter_UserParam3)/10)); // I + P +D Anteil X Achse |
545 | gps_reg_x = ((int)int_east1 + ((dist_east * Parameter_UserParam1 * diff_p)/8)+ ((diff_east * diff_v * Parameter_UserParam3)/10)); // I + P +D Anteil X Achse |
| Line 530... | Line 546... | ||
| 530 | gps_reg_y = ((int)int_north1 + ((dist_north * Parameter_UserParam1)/8)+ ((diff_north * diff_v * Parameter_UserParam3)/10)); // I + P +D Anteil Y Achse |
546 | gps_reg_y = ((int)int_north1 + ((dist_north * Parameter_UserParam1 * diff_p)/8)+ ((diff_north * diff_v * Parameter_UserParam3)/10)); // I + P +D Anteil Y Achse |