Rev 258 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 258 | Rev 266 | ||
|---|---|---|---|
| Line 78... | Line 78... | ||
| 78 | // Initialisierung |
78 | // Initialisierung |
| 79 | void GPS_Neutral(void) |
79 | void GPS_Neutral(void) |
| 80 | { |
80 | { |
| 81 | ublox_msg_state = UBLOX_IDLE; |
81 | ublox_msg_state = UBLOX_IDLE; |
| 82 | gps_state = GPS_CRTL_IDLE; |
82 | gps_state = GPS_CRTL_IDLE; |
| - | 83 | gps_sub_state = GPS_CRTL_IDLE; |
|
| 83 | actual_pos.status = 0; |
84 | actual_pos.status = 0; |
| 84 | actual_speed.status = 0; |
85 | actual_speed.status = 0; |
| 85 | actual_status.status = 0; |
86 | actual_status.status = 0; |
| 86 | gps_home_position.status = 0; // Noch keine gueltige Home Position |
87 | gps_home_position.status = 0; // Noch keine gueltige Home Position |
| 87 | gps_act_position.status = 0; |
88 | gps_act_position.status = 0; |
| Line 330... | Line 331... | ||
| 330 | //Richtung zur Home Positionbezogen auf Nordpol bestimmen |
331 | //Richtung zur Home Positionbezogen auf Nordpol bestimmen |
| 331 | hdng_2home = arctan_i(-gps_rel_start_position.utm_east,-gps_rel_start_position.utm_north); |
332 | hdng_2home = arctan_i(-gps_rel_start_position.utm_east,-gps_rel_start_position.utm_north); |
| 332 | // in Winkel 0...360 Grad umrechnen |
333 | // in Winkel 0...360 Grad umrechnen |
| 333 | if (( gps_rel_start_position.utm_east < 0)) hdng_2home = ( 90-hdng_2home); |
334 | if (( gps_rel_start_position.utm_east < 0)) hdng_2home = ( 90-hdng_2home); |
| 334 | else hdng_2home = (270 - hdng_2home); |
335 | else hdng_2home = (270 - hdng_2home); |
| 335 | dist_2home = get_dist(gps_rel_start_position.utm_east,gps_rel_start_position.utm_north,hdng_2home); //Entfernung zur Home Position bestimmen |
336 | dist_2home = (int)get_dist(gps_rel_start_position.utm_east,gps_rel_start_position.utm_north,hdng_2home); //Entfernung zur Home Position bestimmen |
| 336 | gps_state = GPS_CRTL_HOME_ACTIVE; |
337 | gps_state = GPS_CRTL_HOME_ACTIVE; |
| 337 | return (GPS_STST_OK); |
338 | return (GPS_STST_OK); |
| 338 | } |
339 | } |
| 339 | else |
340 | else |
| 340 | { |
341 | { |
| Line 386... | Line 387... | ||
| 386 | cnt = 0; |
387 | cnt = 0; |
| 387 | GPS_Nick = 0; |
388 | GPS_Nick = 0; |
| 388 | GPS_Roll = 0; |
389 | GPS_Roll = 0; |
| 389 | gps_int_x = 0; |
390 | gps_int_x = 0; |
| 390 | gps_int_y = 0; |
391 | gps_int_y = 0; |
| - | 392 | gps_sub_state = GPS_CRTL_IDLE; |
|
| 391 | gps_state = GPS_CRTL_IDLE; |
393 | gps_state = GPS_CRTL_IDLE; |
| 392 | return (GPS_STST_OK); |
394 | return (GPS_STST_OK); |
| 393 | break; |
395 | break; |
| Line 394... | Line 396... | ||
| 394 | 396 | ||
| 395 | default: |
397 | default: |
| Line 416... | Line 418... | ||
| 416 | d2 = abs (gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north ); |
418 | d2 = abs (gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north ); |
| 417 | d3 = (dist_2home - (int)dist_flown); // Restdistanz zum Ziel |
419 | d3 = (dist_2home - (int)dist_flown); // Restdistanz zum Ziel |
| Line 418... | Line 420... | ||
| 418 | 420 | ||
| 419 | if (d3 > GPS_G2T_DIST_MAX_STOP) // Schneller Rueckflug, noch weit weg vom Ziel |
421 | if (d3 > GPS_G2T_DIST_MAX_STOP) // Schneller Rueckflug, noch weit weg vom Ziel |
| 420 | { |
- | |
| 421 | // hold_fast = 1; // Schnell Regeln |
- | |
| 422 | hold_reset_int = 1; // Integrator ausschalten |
422 | { |
| 423 | if ((d1 < GPS_G2T_FAST_TOL) && (d2 < GPS_G2T_FAST_TOL)) //nur weiter wenn Lage innerhalb der Toleranz |
423 | if ((d1 < GPS_G2T_FAST_TOL) && (d2 < GPS_G2T_FAST_TOL)) //nur weiter wenn Lage innerhalb der Toleranz |
| 424 | { |
424 | { |
| 425 | if (gps_g2t_act_v < GPS_G2T_V_MAX) gps_g2t_act_v++; //Geschwindigkeit langsam erhoehen |
425 | if (gps_g2t_act_v < GPS_G2T_V_MAX) gps_g2t_act_v++; //Geschwindigkeit langsam erhoehen |
| 426 | dist_flown +=(long)gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit |
- | |
| 427 | dist_frm_start_east = (int)((dist_flown * (long)sin_i(hdng_2home))/1000); |
- | |
| 428 | dist_frm_start_north = (int)((dist_flown * (long)cos_i(hdng_2home))/1000); |
- | |
| 429 | gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + dist_frm_start_east; //naechster Zielpunkt |
- | |
| 430 | gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + dist_frm_start_north; //naechster Zielpunkt |
426 | dist_flown +=(long)gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit |
| 431 | gps_sub_state = GPS_HOME_FAST_IN_TOL; |
427 | gps_sub_state = GPS_HOME_FAST_IN_TOL; |
| 432 | } |
428 | } |
| 433 | else //Den Lageregler in Ruhe arbeiten lassen weil ausserhalb der Toleranz |
429 | else //Den Lageregler in Ruhe arbeiten lassen weil ausserhalb der Toleranz |
| 434 | { |
430 | { |
| - | 431 | if (gps_g2t_act_v > 1) gps_g2t_act_v--; // Geschwindigkeit reduzieren |
|
| 435 | if (gps_g2t_act_v > 1) gps_g2t_act_v--; // Geschwindigkeit reduzieren |
432 | dist_flown++; //Auch ausserhalb der Toleranz langsam erhoehen |
| 436 | gps_sub_state = GPS_HOME_FAST_OUTOF_TOL; |
433 | gps_sub_state = GPS_HOME_FAST_OUTOF_TOL; |
| - | 434 | } |
|
| - | 435 | hold_fast = 1; // Regler fuer schnellen Flug |
|
| - | 436 | hold_reset_int = 1; // Integrator ausschalten |
|
| - | 437 | dist_frm_start_east = (int)((dist_flown * (long)sin_i(hdng_2home))/1000); |
|
| - | 438 | dist_frm_start_north = (int)((dist_flown * (long)cos_i(hdng_2home))/1000); |
|
| - | 439 | gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + dist_frm_start_east; //naechster Zielpunkt |
|
| 437 | } |
440 | gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + dist_frm_start_north; //naechster Zielpunkt |
| 438 | } |
441 | } |
| 439 | else if (d3 > GPS_G2T_DIST_HOLD) //Das Ziel naehert sich, deswegen abbremsen |
442 | else if (d3 > GPS_G2T_DIST_HOLD) //Das Ziel naehert sich, deswegen abbremsen |
| 440 | { |
- | |
| 441 | hold_reset_int = 0; // Integrator einschalten |
443 | { |
| 442 | if ((d1 < GPS_G2T_NRML_TOL) && (d2 < GPS_G2T_NRML_TOL)) //nur weiter wenn Lage innerhalb der Toleranz |
444 | if ((d1 < GPS_G2T_NRML_TOL) && (d2 < GPS_G2T_NRML_TOL)) |
| 443 | { |
- | |
| 444 | if (gps_g2t_act_v > GPS_G2T_V_RAMP_DWN) gps_g2t_act_v = GPS_G2T_V_RAMP_DWN; // Geschwindigkeit zu hoch |
- | |
| 445 | else gps_g2t_act_v++; //Geschwindigkeit langsam erhoehen |
445 | { |
| 446 | dist_flown +=(long)gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit |
- | |
| 447 | dist_frm_start_east = (int)((dist_flown * (long)sin_i(hdng_2home))/1000); |
- | |
| 448 | dist_frm_start_north = (int)((dist_flown * (long)cos_i(hdng_2home))/1000); |
- | |
| 449 | gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + dist_frm_start_east; //naechster Zielpunkt |
- | |
| 450 | gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + dist_frm_start_north; //naechster Zielpunkt |
446 | dist_flown += GPS_G2T_V_RAMP_DWN; // Vorgabe der Strecke anhand der Geschwindigkeit |
| 451 | gps_sub_state = GPS_HOME_RMPDWN_IN_TOL; |
447 | gps_sub_state = GPS_HOME_RMPDWN_IN_TOL; |
| 452 | } |
448 | } |
| 453 | else |
449 | else |
| 454 | { |
450 | { |
| 455 | if (gps_g2t_act_v > 1) gps_g2t_act_v--; |
451 | dist_flown++; //Auch ausserhalb der Toleranz langsam erhoehen |
| 456 | gps_sub_state = GPS_HOME_RMPDWN_OUTOF_TOL; |
452 | gps_sub_state = GPS_HOME_RMPDWN_OUTOF_TOL; |
| - | 453 | } |
|
| - | 454 | hold_fast = 0; // Wieder normal regeln |
|
| - | 455 | hold_reset_int = 1; // Integrator ausschalten |
|
| - | 456 | dist_frm_start_east = (int)((dist_flown * (long)sin_i(hdng_2home))/1000); |
|
| - | 457 | dist_frm_start_north = (int)((dist_flown * (long)cos_i(hdng_2home))/1000); |
|
| - | 458 | gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + dist_frm_start_east; //naechster Zielpunkt |
|
| 457 | } |
459 | gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + dist_frm_start_north; //naechster Zielpunkt |
| 458 | } |
460 | } |
| 459 | else //Soll-Ziel fast erreicht, Jetzt noch Reste ausgleichen, weil Zielpunkt nicht exakt bestimmt werden konnte (Fehler in Winkelfkt) |
461 | else //Soll-Ziel fast erreicht, Jetzt noch Reste ausgleichen, weil Zielpunkt nicht exakt bestimmt werden konnte (Fehler in Winkelfkt) |
| 460 | { |
462 | { |
| 461 | if ((d1 < GPS_G2T_NRML_TOL) && (d2 < GPS_G2T_NRML_TOL)) // Jetzt bis zum Zielpunkt regeln |
463 | if ((d1 < GPS_G2T_NRML_TOL) && (d2 < GPS_G2T_NRML_TOL)) // Jetzt bis zum Zielpunkt regeln |
| Line 472... | Line 474... | ||
| 472 | gps_rel_hold_position.utm_east = 0; |
474 | gps_rel_hold_position.utm_east = 0; |
| 473 | gps_rel_hold_position.utm_north = 0; |
475 | gps_rel_hold_position.utm_north = 0; |
| 474 | gps_sub_state = GPS_HOME_FINISHED; |
476 | gps_sub_state = GPS_HOME_FINISHED; |
| 475 | } |
477 | } |
| 476 | } |
478 | } |
| - | 479 | else gps_sub_state = GPS_HOME_OUTOF_TOL; |
|
| 477 | } |
480 | } |
| 478 | } |
481 | } |
| 479 | gps_state = GPS_CRTL_HOLD_ACTIVE; //Zwischensprung |
482 | gps_state = GPS_CRTL_HOLD_ACTIVE; //Zwischensprung |
| 480 | return (GPS_STST_OK); |
483 | return (GPS_STST_OK); |
| 481 | } |
484 | } |
| Line 518... | Line 521... | ||
| 518 | 521 | ||
| 519 | // Variable Verstarkung fuer Differenzierer ermitteln. Je weiter vom Ziel wir entfernt sind |
522 | // Variable Verstarkung fuer Differenzierer ermitteln. Je weiter vom Ziel wir entfernt sind |
| 520 | // desto groesser wird der Faktor. Es gibt aber einen Maximalwert. Bei 0 ist die Verstaerkung immer 1 |
523 | // desto groesser wird der Faktor. Es gibt aber einen Maximalwert. Bei 0 ist die Verstaerkung immer 1 |
| 521 | signed long dist,int_east1,int_north1; |
524 | signed long dist,int_east1,int_north1; |
| 522 | int phi; |
525 | int phi; |
| 523 | phi = arctan_i(abs(dist_north),abs(dist_east)); |
526 | phi = arctan_i(abs(dist_north),abs(dist_east)); |
| Line 524... | Line 527... | ||
| 524 | dist = (long) (get_dist(dist_east,dist_north,phi)); //Zunaechst Entfernung zum Ziel ermitteln |
527 | dist = get_dist(dist_east,dist_north,phi); //Zunaechst Entfernung zum Ziel ermitteln |
| 525 | 528 | ||
| 526 | if (hold_fast == 0) // je Nach Modus andere Verstaerkungskurve fuer Differenzierer |
529 | if (hold_fast == 0) // je Nach Modus andere Verstaerkungskurve fuer Differenzierer |
| 527 | { |
530 | { |
| Line 532... | Line 535... | ||
| 532 | { |
535 | { |
| 533 | diff_v = (int)((dist * (GPS_DIFF_FAST_MAX_V - 10)) / GPS_DIFF_FAST_MAX_D) +10; //Verstaerkung * 10 |
536 | diff_v = (int)((dist * (GPS_DIFF_FAST_MAX_V - 10)) / GPS_DIFF_FAST_MAX_D) +10; //Verstaerkung * 10 |
| 534 | if (diff_v > GPS_DIFF_FAST_MAX_V) diff_v = GPS_DIFF_FAST_MAX_V; //begrenzen |
537 | if (diff_v > GPS_DIFF_FAST_MAX_V) diff_v = GPS_DIFF_FAST_MAX_V; //begrenzen |
| 535 | } |
538 | } |
| Line -... | Line 539... | ||
| - | 539 | ||
| - | 540 | int diff_p; //Vom Modus abhaengige zusaetzliche Verstaerkung |
|
| - | 541 | if (hold_fast > 0) diff_p = GPS_PROP_FAST_V; |
|
| - | 542 | else diff_p = GPS_PROP_NRML_V; |
|
| 536 | 543 | ||
| 537 | //I Werte begrenzen |
544 | //I Werte begrenzen |
| 538 | #define INT1_MAX (20 * GPS_V) |
545 | #define INT1_MAX (20 * GPS_V) |
| 539 | int_east1 = ((((long)int_east) * Parameter_UserParam2)/32); |
546 | int_east1 = ((((long)int_east) * Parameter_UserParam2)/32); |
| 540 | int_north = ((((long)int_north) * Parameter_UserParam2)/32); |
547 | int_north = ((((long)int_north) * Parameter_UserParam2)/32); |
| 541 | if (int_east1 > INT1_MAX) int_east1 = INT1_MAX; //begrenzen |
548 | if (int_east1 > INT1_MAX) int_east1 = INT1_MAX; //begrenzen |
| 542 | else if (int_east1 < -INT1_MAX) int_east1 = -INT1_MAX; |
549 | else if (int_east1 < -INT1_MAX) int_east1 = -INT1_MAX; |
| 543 | if (int_north1 > INT1_MAX) int_north1 = INT1_MAX; //begrenzen |
550 | if (int_north1 > INT1_MAX) int_north1 = INT1_MAX; //begrenzen |
| Line 544... | Line -... | ||
| 544 | else if (int_north1 < -INT1_MAX) int_north1 = -INT1_MAX; |
- | |
| 545 | - | ||
| 546 | int diff_p; |
- | |
| 547 | if (hold_fast == 0) diff_p = 1; |
- | |
| 548 | else diff_p = 2; //im schnellen Modus Proportionalanteil staerken |
551 | else if (int_north1 < -INT1_MAX) int_north1 = -INT1_MAX; |
| 549 | 552 | ||
| 550 | //PID Regler Werte aufsummieren |
553 | //PID Regler Werte aufsummieren |
| Line 551... | Line 554... | ||
| 551 | 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 |
554 | gps_reg_x = ((int)int_east1 + ((dist_east * Parameter_UserParam1 * diff_p)/(8*2))+ ((diff_east * diff_v * Parameter_UserParam3)/10)); // I + P +D Anteil X Achse |
| 552 | 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 |
555 | gps_reg_y = ((int)int_north1 + ((dist_north * Parameter_UserParam1 * diff_p)/(8*2))+ ((diff_north * diff_v * Parameter_UserParam3)/10)); // I + P +D Anteil Y Achse |
| Line 553... | Line 556... | ||
| 553 | 556 | ||