Rev 631 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 631 | Rev 632 | ||
---|---|---|---|
Line 14... | Line 14... | ||
14 | /*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
14 | /*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
15 | von Peter Muehlenbrock alias Salvo |
15 | von Peter Muehlenbrock alias Salvo |
16 | Auswertung der Daten vom GPS im ublox Format |
16 | Auswertung der Daten vom GPS im ublox Format |
17 | Hold Modus mit PID Regler |
17 | Hold Modus mit PID Regler |
18 | Rückstuerz zur Basis Funktion |
18 | Rückstuerz zur Basis Funktion |
19 | Stand 9.1.2008 |
19 | Stand 10.1.2008 |
Line 20... | Line 20... | ||
20 | 20 | ||
21 | ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
21 | ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
22 | */ |
22 | */ |
23 | #include "main.h" |
23 | #include "main.h" |
Line 52... | Line 52... | ||
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 | static 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 | static 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 | static signed int GPS_dist_2trgt; //vorzeichenlose Distanz zum Ziel |
56 | static signed int GPS_dist_2trgt; //vorzeichenlose Distanz zum Ziel |
57 | static signed int gps_int_x,gps_int_y,gps_reg_x,gps_reg_y; |
57 | static signed int gps_int_x,gps_int_y; |
- | 58 | static long signed gps_reg_x,gps_reg_y; |
|
58 | static unsigned int rx_len; |
59 | static unsigned int rx_len; |
59 | static unsigned int ptr_payload_data_end; |
60 | static unsigned int ptr_payload_data_end; |
60 | unsigned int gps_alive_cnt; // Wird bei jedem gueltigen GPS Telegramm hochgezaehlt |
61 | unsigned int gps_alive_cnt; // Wird bei jedem gueltigen GPS Telegramm hochgezaehlt |
61 | static signed int hdng_2home,dist_2home; //Richtung und Entfernung zur home Position |
62 | 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 |
63 | static signed gps_tick; //wird bei jedem Update durch das GPS Modul hochgezaehlt |
Line 302... | Line 303... | ||
302 | static signed int gps_g2t_act_v; // Aktuelle Geschwindigkeitsvorgabe fuer Home Funktion |
303 | static signed int gps_g2t_act_v; // Aktuelle Geschwindigkeitsvorgabe fuer Home Funktion |
303 | long signed int dev; |
304 | long signed int dev; |
304 | signed int dist_frm_start_east,dist_frm_start_north; |
305 | signed int dist_frm_start_east,dist_frm_start_north; |
305 | int amplfy_speed_east,amplfy_speed_north; //Verstaerkungsfaktoren fuer D-Anteil |
306 | int amplfy_speed_east,amplfy_speed_north; //Verstaerkungsfaktoren fuer D-Anteil |
306 | static signed int int_east,int_north; //Integrierer |
307 | static signed int int_east,int_north; //Integrierer |
307 | int speed_east,speed_north; //Aktuelle Geschwindigkeit |
308 | long int speed_east,speed_north; //Aktuelle Geschwindigkeit |
308 | signed long int_east1,int_north1; |
309 | signed long int_east1,int_north1; |
309 | // signed long dist; |
310 | // signed long dist; |
Line 310... | Line 311... | ||
310 | 311 | ||
311 | switch (cmd) |
312 | switch (cmd) |
Line 424... | Line 425... | ||
424 | gps_tick++; |
425 | gps_tick++; |
425 | int d1,d2,d3; |
426 | int d1,d2,d3; |
426 | d1 = abs (gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east ); |
427 | d1 = abs (gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east ); |
427 | d2 = abs (gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north ); |
428 | d2 = abs (gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north ); |
428 | d3 = (dist_2home - dist_flown); // Restdistanz zum Ziel |
429 | d3 = (dist_2home - dist_flown); // Restdistanz zum Ziel |
429 | debug_gp_2 = dist_2home; // zum Debuggen |
430 | // debug_gp_2 = dist_2home; // zum Debuggen |
430 | debug_gp_3 = dist_flown; // zum Debuggen |
431 | // debug_gp_3 = dist_flown; // zum Debuggen |
431 | debug_gp_4 = hdng_2home; // zum Debuggen |
432 | // debug_gp_4 = hdng_2home; // zum Debuggen |
432 | // debug_gp_5 = amplfy_speed_north; // zum Debuggen |
433 | // debug_gp_5 = amplfy_speed_north; // zum Debuggen |
Line 433... | Line 434... | ||
433 | 434 | ||
434 | if (d3 > GPS_G2T_DIST_MAX_STOP) // Schneller Rueckflug, noch weit weg vom Ziel |
435 | if (d3 > GPS_G2T_DIST_MAX_STOP) // Schneller Rueckflug, noch weit weg vom Ziel |
435 | { |
436 | { |
Line 505... | Line 506... | ||
505 | 506 | ||
506 | case GPS_CRTL_HOLD_ACTIVE: // Hier werden die Daten fuer Nick und Roll errechnet |
507 | case GPS_CRTL_HOLD_ACTIVE: // Hier werden die Daten fuer Nick und Roll errechnet |
507 | if (gps_updte_flag >0) // nur wenn neue GPS Daten vorliegen |
508 | if (gps_updte_flag >0) // nur wenn neue GPS Daten vorliegen |
508 | { |
509 | { |
509 | // ab hier wird geregelt |
510 | // ab hier wird geregelt |
510 | dist_east = gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east; |
511 | dist_east = gps_rel_act_position.utm_east - gps_rel_hold_position.utm_east; |
511 | dist_north = gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north; |
512 | dist_north = gps_rel_act_position.utm_north - gps_rel_hold_position.utm_north; |
512 | int_east += dist_east; |
513 | int_east += dist_east; |
513 | int_north += dist_north; |
514 | int_north += dist_north; |
514 | speed_east = (int) (-actual_speed.speed_e); |
515 | speed_east = actual_speed.speed_e; |
515 | speed_north = (int) (-actual_speed.speed_n); |
516 | speed_north = actual_speed.speed_n; |
- | 517 | gps_updte_flag = 0; // Neue Werte koennen vom GPS geholt werden |
|
- | 518 | debug_gp_2 = dist_east; // zum Debuggen |
|
- | 519 | debug_gp_3 = dist_north; // zum Debuggen |
|
Line 516... | Line 520... | ||
516 | gps_updte_flag = 0; // Neue Werte koennen vom GPS geholt werden |
520 | |
517 | 521 | ||
518 | #define GPSINT_MAX 3000 // Neues Verfahren ab 30.12.2007 bei Integratoroverflow |
522 | #define GPSINT_MAX 3000 // Neues Verfahren ab 30.12.2007 bei Integratoroverflow |
519 | if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) |
523 | if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) |
Line 539... | Line 543... | ||
539 | amplfy_speed_north = (((abs(speed_north))*(DIFF_Y_F_MAX-1)*10)/DIFF_X_F_MAX) +10; |
543 | 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 |
544 | 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 |
545 | if (amplfy_speed_north > (DIFF_Y_F_MAX *10)) amplfy_speed_north = DIFF_Y_F_MAX *10; // Begrenzung |
542 | amplfy_speed_east *= (Parameter_UserParam3/GPS_USR_PAR_FKT); |
546 | amplfy_speed_east *= (Parameter_UserParam3/GPS_USR_PAR_FKT); |
543 | amplfy_speed_north *= (Parameter_UserParam3/GPS_USR_PAR_FKT); |
547 | 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 |
- | |
546 | } |
548 | } |
547 | else //langsamer Holdmodus |
549 | else //langsamer Holdmodus |
548 | { |
550 | { |
549 | // Verstaerkungsfaktor fuer D-Anteil ermitteln um exponentielles Verhalten zu erzielen |
551 | // Verstaerkungsfaktor fuer D-Anteil ermitteln um exponentielles Verhalten zu erzielen |
550 | amplfy_speed_east = (((abs(speed_east)) *(DIFF_Y_N_MAX-1)*10)/DIFF_X_N_MAX) +10; |
552 | amplfy_speed_east = (((abs(speed_east)) *(DIFF_Y_N_MAX-1)*10)/DIFF_X_N_MAX) +10; |
551 | amplfy_speed_north = (((abs(speed_north))*(DIFF_Y_N_MAX-1)*10)/DIFF_X_N_MAX) +10; |
553 | amplfy_speed_north = (((abs(speed_north))*(DIFF_Y_N_MAX-1)*10)/DIFF_X_N_MAX) +10; |
552 | if (amplfy_speed_east > (DIFF_Y_N_MAX *10)) amplfy_speed_east = DIFF_Y_N_MAX *10; // Begrenzung |
554 | if (amplfy_speed_east > (DIFF_Y_N_MAX *10)) amplfy_speed_east = DIFF_Y_N_MAX *10; // Begrenzung |
553 | if (amplfy_speed_north > (DIFF_Y_N_MAX *10)) amplfy_speed_north = DIFF_Y_N_MAX *10; // Begrenzung |
555 | if (amplfy_speed_north > (DIFF_Y_N_MAX *10)) amplfy_speed_north = DIFF_Y_N_MAX *10; // Begrenzung |
554 | amplfy_speed_east *= (Parameter_UserParam3/GPS_USR_PAR_FKT); |
556 | amplfy_speed_east *= (Parameter_UserParam3/GPS_USR_PAR_FKT); |
555 | amplfy_speed_north *= (Parameter_UserParam3/GPS_USR_PAR_FKT); |
557 | amplfy_speed_north *= (Parameter_UserParam3/GPS_USR_PAR_FKT); |
556 | amplfy_speed_east /= 10; //Faktor 10 wieder rausnehmen |
558 | speed_east = speed_east * abs(speed_east) /100; //quadrieren |
557 | amplfy_speed_north /= 10; //Faktor 10 wieder rausnehmen |
559 | speed_north = speed_north * abs(speed_north) /100; //quadrieren |
558 | } |
560 | } |
559 | - | ||
560 | - | ||
- | 561 | // amplfy_speed_east /= 10; |
|
561 | #define GPS_SPEED_SCALE 5 |
562 | // amplfy_speed_north /= 10; |
562 | speed_east /= GPS_SPEED_SCALE; // Wegen overflow Gefahr Wert jetzt schon runterskalieren |
563 | debug_gp_4 = (int)speed_east; // zum Debuggen |
563 | speed_north /= GPS_SPEED_SCALE; // Wegen overflow Gefahr Wert jetzt schon runterskalieren |
564 | debug_gp_5 = (int)speed_north; // zum Debuggen |
Line 564... | Line 565... | ||
564 | 565 | ||
565 | int diff_p; //Vom Modus abhaengige zusaetzliche Verstaerkung |
566 | int diff_p; //Vom Modus abhaengige zusaetzliche Verstaerkung |
566 | if (hold_fast > 0) diff_p = GPS_PROP_FAST_V; |
567 | if (hold_fast > 0) diff_p = GPS_PROP_FAST_V; |
Line 574... | Line 575... | ||
574 | else if (int_east1 < -INT1_MAX) int_east1 = -INT1_MAX; |
575 | else if (int_east1 < -INT1_MAX) int_east1 = -INT1_MAX; |
575 | if (int_north1 > INT1_MAX) int_north1 = INT1_MAX; //begrenzen |
576 | if (int_north1 > INT1_MAX) int_north1 = INT1_MAX; //begrenzen |
576 | else if (int_north1 < -INT1_MAX) int_north1 = -INT1_MAX; |
577 | else if (int_north1 < -INT1_MAX) int_north1 = -INT1_MAX; |
Line 577... | Line 578... | ||
577 | 578 | ||
578 | //PID Regler Werte aufsummieren |
579 | //PID Regler Werte aufsummieren |
579 | gps_reg_x = ((int)int_east1 + ((dist_east * (Parameter_UserParam1/GPS_USR_PAR_FKT) * diff_p)/(8*2))+ ((speed_east * amplfy_speed_east )/(10/GPS_SPEED_SCALE))); // I + P +D Anteil X Achse |
580 | gps_reg_x = -(int_east1 + (long)((dist_east * (Parameter_UserParam1/GPS_USR_PAR_FKT) * diff_p)/(8*2))+ ((speed_east * (long)amplfy_speed_east )/(100L))); // I + P +D Anteil X Achse |
580 | gps_reg_y = ((int)int_north1 + ((dist_north * (Parameter_UserParam1/GPS_USR_PAR_FKT) * diff_p)/(8*2))+ ((speed_north * amplfy_speed_north )/(10/GPS_SPEED_SCALE))); // I + P +D Anteil Y Achse |
581 | gps_reg_y = -(int_north1 + (long)((dist_north * (Parameter_UserParam1/GPS_USR_PAR_FKT) * diff_p)/(8*2))+ ((speed_north * (long)amplfy_speed_north )/(100L))); // I + P +D Anteil Y Achse |
581 | debug_gp_0 = gps_reg_x; // zum Debuggen |
582 | debug_gp_0 = (int)gps_reg_x; // zum Debuggen |
Line 582... | Line 583... | ||
582 | debug_gp_1 = gps_reg_y; // zum Debuggen |
583 | debug_gp_1 = (int)gps_reg_y; // zum Debuggen |
583 | 584 | ||
Line 584... | Line 585... | ||
584 | //Ziel-Richtung bezogen auf Nordpol bestimmen |
585 | //Ziel-Richtung bezogen auf Nordpol bestimmen |
585 | GPS_hdng_abs_2trgt = arctan_i(gps_reg_x,gps_reg_y); |
586 | GPS_hdng_abs_2trgt = arctan_i((int)gps_reg_x,(int)gps_reg_y); |
586 | 587 | ||
Line 596... | Line 597... | ||
596 | else if (GPS_hdng_rel_2trgt <-180) GPS_hdng_rel_2trgt = 360 + GPS_hdng_rel_2trgt; |
597 | else if (GPS_hdng_rel_2trgt <-180) GPS_hdng_rel_2trgt = 360 + GPS_hdng_rel_2trgt; |
Line 597... | Line 598... | ||
597 | 598 | ||
598 | // Regelabweichung aus x,y zu Ziel in Distanz umrechnen |
599 | // Regelabweichung aus x,y zu Ziel in Distanz umrechnen |
599 | if (abs(gps_reg_x) > abs(gps_reg_y) ) |
600 | if (abs(gps_reg_x) > abs(gps_reg_y) ) |
600 | { |
601 | { |
601 | dev = (long)gps_reg_x; //Groesseren Wert wegen besserer Genauigkeit nehmen |
602 | dev = gps_reg_x; //Groesseren Wert wegen besserer Genauigkeit nehmen |
602 | dev = abs((dev *1000) / (long) sin_i(GPS_hdng_abs_2trgt)); |
603 | dev = abs((dev *1000) / (long) sin_i(GPS_hdng_abs_2trgt)); |
603 | } |
604 | } |
604 | else |
605 | else |
605 | { |
606 | { |