Rev 616 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 616 | Rev 621 | ||
|---|---|---|---|
| Line 49... | Line 49... | ||
| 49 | short int ublox_msg_state = UBLOX_IDLE; |
49 | short int ublox_msg_state = UBLOX_IDLE; |
| 50 | static uint8_t chk_a =0; //Checksum |
50 | static uint8_t chk_a =0; //Checksum |
| 51 | static uint8_t chk_b =0; |
51 | static uint8_t chk_b =0; |
| 52 | short int gps_state,gps_sub_state; //Zustaende der Statemachine |
52 | short int gps_state,gps_sub_state; //Zustaende der Statemachine |
| 53 | short int gps_updte_flag; |
53 | short int gps_updte_flag; |
| 54 | signed int GPS_hdng_abs_2trgt; //Winkel zum Ziel bezogen auf Nordpol |
54 | static signed int GPS_hdng_abs_2trgt; //Winkel zum Ziel bezogen auf Nordpol |
| 55 | signed int GPS_hdng_rel_2trgt; //Winkel zum Ziel bezogen auf Nordachse des Kopters |
55 | static signed int GPS_hdng_rel_2trgt; //Winkel zum Ziel bezogen auf Nordachse des Kopters |
| 56 | signed int GPS_dist_2trgt; //vorzeichenlose Distanz zum Ziel |
56 | static signed int GPS_dist_2trgt; //vorzeichenlose Distanz zum Ziel |
| 57 | signed int gps_int_x,gps_int_y,gps_reg_x,gps_reg_y; |
57 | static signed int gps_int_x,gps_int_y,gps_reg_x,gps_reg_y; |
| 58 | static unsigned int rx_len; |
58 | static unsigned int rx_len; |
| 59 | static unsigned int ptr_payload_data_end; |
59 | static unsigned int ptr_payload_data_end; |
| 60 | unsigned int gps_alive_cnt; // Wird bei jedem gueltigen GPS Telegramm hochgezaehlt |
60 | unsigned int gps_alive_cnt; // Wird bei jedem gueltigen GPS Telegramm hochgezaehlt |
| 61 | signed int hdng_2home,dist_2home; //Richtung und Entfernung zur home Position |
61 | static signed int hdng_2home,dist_2home; //Richtung und Entfernung zur home Position |
| 62 | static signed gps_tick; //wird bei jedem Update durch das GPS Modul hochgezaehlt |
62 | static signed gps_tick; //wird bei jedem Update durch das GPS Modul hochgezaehlt |
| 63 | static short int hold_fast,hold_reset_int; //Flags fuer Hold Regler |
63 | static short int hold_fast,hold_reset_int; //Flags fuer Hold Regler |
| 64 | static uint8_t *ptr_payload_data; |
64 | static uint8_t *ptr_payload_data; |
| 65 | static uint8_t *ptr_pac_status; |
65 | static uint8_t *ptr_pac_status; |
| 66 | long int dist_flown; |
66 | static long int dist_flown; |
| 67 | unsigned int int_ovfl_cnt; // Zaehler fuer Overflows des Integrators |
67 | static unsigned int int_ovfl_cnt; // Zaehler fuer Overflows des Integrators |
| 68 | signed int int_east,int_north; //Integrierer |
- | |
| 69 | signed int diff_east_f,diff_north_f; // Differenzierer, gefiltert |
- | |
| 70 | //signed int diff_v; |
- | |
| 71 | signed long dist; |
- | |
| - | 68 | ||
| Line 72... | Line 69... | ||
| 72 | 69 | ||
| Line 73... | Line 70... | ||
| 73 | short int Get_GPS_data(void); |
70 | short int Get_GPS_data(void); |
| 74 | 71 | ||
| Line 297... | Line 294... | ||
| 297 | } |
294 | } |
| Line 298... | Line 295... | ||
| 298 | 295 | ||
| 299 | //Zentrale Statemachine fuer alle GPS relevanten Regelungsablauefe |
296 | //Zentrale Statemachine fuer alle GPS relevanten Regelungsablauefe |
| 300 | short int GPS_CRTL(short int cmd) |
297 | short int GPS_CRTL(short int cmd) |
| 301 | { |
298 | { |
| 302 | static unsigned int cnt; //Zaehler fuer diverse Verzoegerungen |
299 | static unsigned int cnt; //Zaehler fuer diverse Verzoegerungen |
| 303 | static signed int dist_north,dist_east; |
300 | static signed int dist_north,dist_east; // Distanz zur Sollpoistion |
| - | 301 | static signed int diff_east,diff_north; // Differenzierer (Differenz zum vorhergehenden x bzw. y Wert) |
|
| 304 | static signed int diff_east,diff_north; // Differenzierer (Differenz zum vorhergehenden x bzw. y Wert) |
302 | static signed int diff_east_f,diff_north_f; // Differenzierer, gefiltert |
| 305 | signed int n; |
303 | signed int n,n1; |
| 306 | static signed int gps_g2t_act_v; // Aktuelle Geschwindigkeitsvorgabe fuer Home Funktion |
304 | static signed int gps_g2t_act_v; // Aktuelle Geschwindigkeitsvorgabe fuer Home Funktion |
| 307 | long signed int dev,n_l; |
305 | long signed int dev; |
| 308 | signed int dist_frm_start_east,dist_frm_start_north; |
306 | signed int dist_frm_start_east,dist_frm_start_north; |
| - | 307 | int diff_v_east,diff_v_north; //Verstaerkungsfaktoren fuer Differenzierer |
|
| - | 308 | static signed int int_east,int_north; //Integrierer |
|
| Line 309... | Line 309... | ||
| 309 | int diff_v_east,diff_v_north; //Verstaerkungsfaktoren fuer Differenzierer |
309 | // signed long dist; |
| 310 | 310 | ||
| Line 311... | Line 311... | ||
| 311 | switch (cmd) |
311 | switch (cmd) |
| Line 362... | Line 362... | ||
| 362 | 362 | ||
| 363 | case GPS_CMD_REQ_HOLD: // Die Lageregelung soll aktiviert werden. |
363 | case GPS_CMD_REQ_HOLD: // Die Lageregelung soll aktiviert werden. |
| 364 | if (gps_state != GPS_CRTL_HOLD_ACTIVE) |
364 | if (gps_state != GPS_CRTL_HOLD_ACTIVE) |
| 365 | { |
365 | { |
| 366 | cnt++; |
366 | cnt++; |
| 367 | if (cnt > 400) // erst nach Verzoegerung |
367 | if (cnt > 500) // erst nach Verzoegerung |
| 368 | { |
368 | { |
| 369 | cnt = 0; |
369 | cnt = 0; |
| 370 | // aktuelle positionsdaten abspeichern |
370 | // aktuelle positionsdaten abspeichern |
| 371 | if (gps_rel_act_position.status > 0) |
371 | if (gps_rel_act_position.status > 0) |
| Line 505... | Line 505... | ||
| 505 | case GPS_CRTL_HOLD_ACTIVE: // Hier werden die Daten fuer Nick und Roll errechnet |
505 | case GPS_CRTL_HOLD_ACTIVE: // Hier werden die Daten fuer Nick und Roll errechnet |
| 506 | if (gps_updte_flag >0) // nur wenn neue GPS Daten vorliegen |
506 | if (gps_updte_flag >0) // nur wenn neue GPS Daten vorliegen |
| 507 | { |
507 | { |
| 508 | gps_updte_flag = 0; |
508 | gps_updte_flag = 0; |
| 509 | // ab hier wird geregelt |
509 | // ab hier wird geregelt |
| 510 | diff_east = -dist_east; //Alten Wert fuer Differenzierer schon mal abziehen |
510 | diff_east = dist_east; //Alten Wert fuer Differenzierer schon mal addieren |
| 511 | diff_north = -dist_north; |
511 | diff_north = dist_north; |
| 512 | dist_east = gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east; |
512 | dist_east = gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east; |
| 513 | dist_north = gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north; |
513 | dist_north = gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north; |
| 514 | int_east += dist_east; |
514 | int_east += dist_east; |
| 515 | int_north += dist_north; |
515 | int_north += dist_north; |
| 516 | diff_east += dist_east; // Differenz zur vorhergehenden East Position |
516 | diff_east -= dist_east; // Differenz zur vorhergehenden East Position |
| 517 | diff_north += dist_north; // Differenz zur vorhergehenden North Position |
517 | diff_north -= dist_north; // Differenz zur vorhergehenden North Position |
| Line 518... | Line 518... | ||
| 518 | 518 | ||
| 519 | if (hold_fast > 0) // wegen Sollpositionsspruengen im Fast Mode Differenzierer daempfen |
519 | if (hold_fast > 0) // wegen Sollpositionsspruengen im Fast Mode Differenzierer daempfen |
| 520 | { |
520 | { |
| 521 | diff_east_f = (((diff_east_f *2)/3) + ((diff_east *1*10)/3)); //Differenzierer filtern |
521 | diff_east_f = (((diff_east_f *2)/3) + ((diff_east *1*10)/3)); //Differenzierer filtern |
| 522 | diff_north_f = (((diff_north_f *2)/3) + ((diff_north*1*10)/3)); //Differenzierer filtern |
522 | diff_north_f = (((diff_north_f *2)/3) + ((diff_north*1*10)/3)); //Differenzierer filtern |
| 523 | } |
523 | } |
| 524 | else // schwache Filterung |
524 | else // schwache Filterung |
| 525 | { |
525 | { |
| 526 | diff_east_f = (((diff_east_f * 3)/4) + ((diff_east *1*10)/4)); //Differenzierer filtern |
526 | diff_east_f = (((diff_east_f * 1)/4) + ((diff_east *3*10)/4)); //Differenzierer filtern |
| 527 | diff_north_f = (((diff_north_f * 3)/4) + ((diff_north*1*10)/4)); //Differenzierer filtern |
527 | diff_north_f = (((diff_north_f * 1)/4) + ((diff_north*3*10)/4)); //Differenzierer filtern |
| Line 528... | Line 528... | ||
| 528 | } |
528 | } |
| 529 | 529 | ||
| 530 | #define GPSINT_MAX 3000 // Neues Verfahren ab 30.12.2007 bei Integratoroverflow |
530 | #define GPSINT_MAX 3000 // Neues Verfahren ab 30.12.2007 bei Integratoroverflow |
| 531 | if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) |
531 | if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) |
| 532 | { |
532 | { |
| 533 | if (int_ovfl_cnt < 40) int_ovfl_cnt += 20; // Zahl der Overflows zaehlen |
533 | if (int_ovfl_cnt < 40) int_ovfl_cnt += 1; // Zahl der Overflows zaehlen |
| 534 | // int_east -= dist_east; auf alten Wert halten |
534 | // int_east -= dist_east; auf alten Wert halten |
| 535 | // int_north -= dist_north; |
535 | // int_north -= dist_north; |
| 536 | } |
536 | } |
| 537 | if (int_ovfl_cnt > 0) //bei Overflow Wert Inetgratorwert reduzieren |
537 | if (int_ovfl_cnt > 0) //bei Overflow Wert Integratorwert reduzieren |
| 538 | { |
538 | { |
| 539 | int_ovfl_cnt -= 1; |
539 | int_ovfl_cnt -= 1; |
| 540 | int_east = (int_east*7)/8; // Wert reduzieren |
540 | int_east = (int_east*7)/8; // Wert reduzieren |
| Line 549... | Line 549... | ||
| 549 | 549 | ||
| 550 | // Variable Verstarkung fuer Differenzierer ermitteln. Je weiter vom Ziel wir entfernt sind |
550 | // Variable Verstarkung fuer Differenzierer ermitteln. Je weiter vom Ziel wir entfernt sind |
| 551 | // desto groesser wird der Faktor. Es gibt aber einen Maximalwert. Bei 0 ist die Verstaerkung immer 1 |
551 | // desto groesser wird der Faktor. Es gibt aber einen Maximalwert. Bei 0 ist die Verstaerkung immer 1 |
| Line 552... | Line 552... | ||
| 552 | signed long int_east1,int_north1; |
552 | signed long int_east1,int_north1; |
| 553 | 553 | ||
| 554 | int phi; |
554 | // int phi; |
| Line 555... | Line 555... | ||
| 555 | phi = arctan_i(abs(dist_north),abs(dist_east)); |
555 | // phi = arctan_i(abs(dist_north),abs(dist_east)); |
| 556 | dist = get_dist(dist_east,dist_north,phi); //Zunaechst Entfernung zum Ziel ermitteln |
556 | // dist = get_dist(dist_east,dist_north,phi); //Zunaechst Entfernung zum Ziel ermitteln |
| 557 | 557 | ||
| 558 | if (hold_fast == 0) // je Nach Modus andere Verstaerkungskurve fuer Differenzierer |
558 | if (hold_fast == 0) // je Nach Modus andere Verstaerkungskurve fuer Differenzierer |
| Line 623... | Line 623... | ||
| 623 | dev = (long)gps_reg_y; |
623 | dev = (long)gps_reg_y; |
| 624 | dev = abs((dev *1000) / (long) cos_i(GPS_hdng_abs_2trgt)); |
624 | dev = abs((dev *1000) / (long) cos_i(GPS_hdng_abs_2trgt)); |
| 625 | } |
625 | } |
| 626 | GPS_dist_2trgt = (int) dev; |
626 | GPS_dist_2trgt = (int) dev; |
| 627 | // Winkel und Distanz in Nick und Rollgroessen umrechnen |
627 | // Winkel und Distanz in Nick und Rollgroessen umrechnen |
| 628 | GPS_Roll = (int) +( (dev * (long) sin_i(GPS_hdng_rel_2trgt))/1000); |
628 | n = (int) +( (dev * (long) sin_i(GPS_hdng_rel_2trgt))/1000); //Rollwert bestimmen |
| 629 | GPS_Nick = (int) -( (dev * (long) cos_i(GPS_hdng_rel_2trgt))/1000); |
629 | n1 = (int) -( (dev * (long) cos_i(GPS_hdng_rel_2trgt))/1000); //Nickwert bestimmen |
| 630 | - | ||
| 631 | if (GPS_Roll > (GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = (GPS_NICKROLL_MAX * GPS_V); |
- | |
| 632 | else if (GPS_Roll < -(GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = -(GPS_NICKROLL_MAX * GPS_V); |
- | |
| 633 | if (GPS_Nick > (GPS_NICKROLL_MAX * GPS_V)) GPS_Nick = (GPS_NICKROLL_MAX * GPS_V); |
- | |
| 634 | else if (GPS_Nick < -(GPS_NICKROLL_MAX * GPS_V)) GPS_Nick = -(GPS_NICKROLL_MAX * GPS_V); |
- | |
| Line -... | Line 630... | ||
| - | 630 | ||
| - | 631 | // GPS_Roll = (int) +( (dev * (long) sin_i(GPS_hdng_rel_2trgt))/1000); |
|
| - | 632 | // GPS_Nick = (int) -( (dev * (long) cos_i(GPS_hdng_rel_2trgt))/1000); |
|
| - | 633 | ||
| - | 634 | if (n > (GPS_NICKROLL_MAX * GPS_V)) n = (GPS_NICKROLL_MAX * GPS_V); |
|
| - | 635 | else if (n < -(GPS_NICKROLL_MAX * GPS_V)) n = -(GPS_NICKROLL_MAX * GPS_V); |
|
| - | 636 | if (n1 > (GPS_NICKROLL_MAX * GPS_V)) n1 = (GPS_NICKROLL_MAX * GPS_V); |
|
| - | 637 | else if (n1 < -(GPS_NICKROLL_MAX * GPS_V)) n1 = -(GPS_NICKROLL_MAX * GPS_V); |
|
| - | 638 | n = n/GPS_V; |
|
| 635 | 639 | n1 = n1/GPS_V; |
|
| 636 | //Kleine Werte verstaerken, Grosse abschwaechen |
640 | //Kleine Werte verstaerken, Grosse abschwaechen |
| 637 | n = sin_i((GPS_Roll*90)/(GPS_NICKROLL_MAX * GPS_V)); |
641 | /* n = sin_i((GPS_Roll*90)/(GPS_NICKROLL_MAX * GPS_V)); |
| 638 | n_l = ((long) GPS_NICKROLL_MAX * (long) n)/1000; |
642 | n_l = ((long) GPS_NICKROLL_MAX * (long) n)/1000; |
| 639 | GPS_Roll = (int) n_l; |
643 | GPS_Roll = (int) n_l; |
| 640 | n = sin_i((GPS_Nick*90)/(GPS_NICKROLL_MAX * GPS_V)); |
644 | n = sin_i((GPS_Nick*90)/(GPS_NICKROLL_MAX * GPS_V)); |
| 641 | n_l = ((long) GPS_NICKROLL_MAX * (long) n)/1000; |
645 | n_l = ((long) GPS_NICKROLL_MAX * (long) n)/1000; |
| - | 646 | GPS_Nick = (int) n_l; |
|
| 642 | GPS_Nick = (int) n_l; |
647 | */ |
| 643 | 648 | ||
| 644 | if ((abs(dist_east) > GPS_DIST_MAX) || (abs(dist_north) > GPS_DIST_MAX)) // bei zu grossem Abstand abbrechen |
649 | if ((abs(dist_east) > GPS_DIST_MAX) || (abs(dist_north) > GPS_DIST_MAX)) // bei zu grossem Abstand abbrechen |
| 645 | { |
650 | { |
| 646 | GPS_Roll = 0; |
651 | GPS_Roll = 0; |
| 647 | GPS_Nick = 0; |
652 | GPS_Nick = 0; |
| 648 | gps_state = GPS_CRTL_IDLE; |
653 | gps_state = GPS_CRTL_IDLE; |
| 649 | return (GPS_STST_ERR); |
654 | return (GPS_STST_ERR); |
| 650 | break; |
655 | break; |
| 651 | } |
656 | } |
| 652 | else |
657 | else |
| - | 658 | { |
|
| - | 659 | GPS_Roll = n; |
|
| 653 | { |
660 | GPS_Nick = n1; |
| 654 | if ( cmd == GPS_CMD_REQ_HOME ) gps_state = GPS_CRTL_HOME_ACTIVE; // State umsetzen |
661 | if ( cmd == GPS_CMD_REQ_HOME ) gps_state = GPS_CRTL_HOME_ACTIVE; // State umsetzen |
| 655 | return (GPS_STST_OK); |
662 | return (GPS_STST_OK); |
| 656 | } |
663 | } |
| 657 | } |
664 | } |