Rev 622 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 622 | Rev 626 | ||
---|---|---|---|
Line 14... | Line 14... | ||
14 | /*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
14 | /*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
15 | Peter Muehlenbrock |
15 | Peter Muehlenbrock |
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 24.10.2007 |
19 | Stand 7.1.2008 |
20 | Anederung: 24.10. Altitude in relativer Position jetzt auch drin |
- | |
- | 20 | ||
21 | ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
21 | ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
22 | */ |
22 | */ |
23 | #include "main.h" |
23 | #include "main.h" |
24 | #include "math.h" |
24 | #include "math.h" |
25 | //#include "gps.h" |
25 | //#include "gps.h" |
Line 296... | Line 296... | ||
296 | //Zentrale Statemachine fuer alle GPS relevanten Regelungsablauefe |
296 | //Zentrale Statemachine fuer alle GPS relevanten Regelungsablauefe |
297 | short int GPS_CRTL(short int cmd) |
297 | short int GPS_CRTL(short int cmd) |
298 | { |
298 | { |
299 | static unsigned int cnt; //Zaehler fuer diverse Verzoegerungen |
299 | static unsigned int cnt; //Zaehler fuer diverse Verzoegerungen |
300 | static signed int dist_north,dist_east; // Distanz zur Sollpoistion |
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) |
- | |
302 | static signed int diff_east_f,diff_north_f; // Differenzierer, gefiltert |
- | |
303 | signed int n,n1; |
301 | signed int n,n1; |
304 | static signed int gps_g2t_act_v; // Aktuelle Geschwindigkeitsvorgabe fuer Home Funktion |
302 | static signed int gps_g2t_act_v; // Aktuelle Geschwindigkeitsvorgabe fuer Home Funktion |
305 | long signed int dev; |
303 | long signed int dev; |
306 | signed int dist_frm_start_east,dist_frm_start_north; |
304 | signed int dist_frm_start_east,dist_frm_start_north; |
307 | int diff_v_east,diff_v_north; //Verstaerkungsfaktoren fuer Differenzierer |
305 | int amplfy_speed_east,amplfy_speed_north; //Verstaerkungsfaktoren fuer D-Anteil |
308 | static signed int int_east,int_north; //Integrierer |
306 | static signed int int_east,int_north; //Integrierer |
309 | int speed_east,speed_north; //Aktuelle Geschwindigkeit |
307 | int speed_east,speed_north; //Aktuelle Geschwindigkeit |
310 | signed long int_east1,int_north1; |
308 | signed long int_east1,int_north1; |
311 | // signed long dist; |
309 | // signed long dist; |
Line 325... | Line 323... | ||
325 | hold_fast = 0; |
323 | hold_fast = 0; |
326 | hold_reset_int = 0; // Integrator enablen |
324 | hold_reset_int = 0; // Integrator enablen |
327 | int_east = 0, int_north = 0; |
325 | int_east = 0, int_north = 0; |
328 | gps_reg_x = 0, gps_reg_y = 0; |
326 | gps_reg_x = 0, gps_reg_y = 0; |
329 | dist_east = 0, dist_north = 0; |
327 | dist_east = 0, dist_north = 0; |
330 | diff_east_f = 0, diff_north_f= 0; |
- | |
331 | diff_east = 0, diff_north = 0; |
- | |
332 | dist_flown = 0; |
328 | dist_flown = 0; |
333 | gps_g2t_act_v = 0; |
329 | gps_g2t_act_v = 0; |
334 | gps_sub_state = GPS_CRTL_IDLE; |
330 | gps_sub_state = GPS_CRTL_IDLE; |
335 | // aktuelle positionsdaten abspeichern |
331 | // aktuelle positionsdaten abspeichern |
336 | if (gps_rel_act_position.status > 0) |
332 | if (gps_rel_act_position.status > 0) |
Line 375... | Line 371... | ||
375 | hold_fast = 0; |
371 | hold_fast = 0; |
376 | hold_reset_int = 0; // Integrator enablen |
372 | hold_reset_int = 0; // Integrator enablen |
377 | int_east = 0, int_north = 0; |
373 | int_east = 0, int_north = 0; |
378 | gps_reg_x = 0, gps_reg_y = 0; |
374 | gps_reg_x = 0, gps_reg_y = 0; |
379 | dist_east = 0, dist_north = 0; |
375 | dist_east = 0, dist_north = 0; |
380 | diff_east_f = 0, diff_north_f= 0; |
- | |
381 | diff_east = 0, diff_north = 0; |
- | |
382 | speed_east = 0; speed_north= 0; |
376 | speed_east = 0; speed_north= 0; |
383 | int_ovfl_cnt = 0; |
377 | int_ovfl_cnt = 0; |
384 | gps_rel_hold_position.utm_east = gps_rel_act_position.utm_east; |
378 | gps_rel_hold_position.utm_east = gps_rel_act_position.utm_east; |
385 | gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north; |
379 | gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north; |
386 | gps_rel_hold_position.status = 1; // gueltige Positionsdaten |
380 | gps_rel_hold_position.status = 1; // gueltige Positionsdaten |
Line 507... | Line 501... | ||
507 | 501 | ||
508 | case GPS_CRTL_HOLD_ACTIVE: // Hier werden die Daten fuer Nick und Roll errechnet |
502 | case GPS_CRTL_HOLD_ACTIVE: // Hier werden die Daten fuer Nick und Roll errechnet |
509 | if (gps_updte_flag >0) // nur wenn neue GPS Daten vorliegen |
503 | if (gps_updte_flag >0) // nur wenn neue GPS Daten vorliegen |
510 | { |
504 | { |
511 | // ab hier wird geregelt |
- | |
512 | diff_east = dist_east; //Alten Wert fuer Differenzierer schon mal addieren |
- | |
513 | diff_north = dist_north; |
505 | // ab hier wird geregelt |
514 | dist_east = gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east; |
506 | dist_east = gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east; |
515 | dist_north = gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north; |
507 | dist_north = gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north; |
516 | int_east += dist_east; |
508 | int_east += dist_east; |
517 | int_north += dist_north; |
- | |
518 | diff_east -= dist_east; // Differenz zur vorhergehenden East Position |
- | |
519 | diff_north -= dist_north; // Differenz zur vorhergehenden North Position |
509 | int_north += dist_north; |
520 | speed_east = (int) actual_speed.speed_e; |
510 | speed_east = (int) (-actual_speed.speed_e); |
521 | speed_north = (int) actual_speed.speed_n; |
511 | speed_north = (int) (-actual_speed.speed_n); |
Line 522... | Line -... | ||
522 | gps_updte_flag = 0; // Neue Werte koennen vom GPS geholt werden |
- | |
523 | - | ||
524 | debug_gp_2 = speed_east; // zum Debuggen |
- | |
525 | debug_gp_3 = speed_north; // zum Debuggen |
- | |
526 | - | ||
527 | if (hold_fast > 0) // wegen Sollpositionsspruengen im Fast Mode Differenzierer daempfen |
- | |
528 | { |
- | |
529 | diff_east_f = (((diff_east_f *2)/3) + ((diff_east *1*10)/3)); //Differenzierer filtern |
- | |
530 | diff_north_f = (((diff_north_f *2)/3) + ((diff_north*1*10)/3)); //Differenzierer filtern |
- | |
531 | } |
- | |
532 | else // schwache Filterung |
- | |
533 | { |
- | |
534 | diff_east_f = (((diff_east_f * 2)/4) + ((diff_east *2*10)/4)); //Differenzierer filtern |
- | |
535 | diff_north_f = (((diff_north_f * 2)/4) + ((diff_north*2*10)/4)); //Differenzierer filtern |
- | |
536 | } |
- | |
537 | - | ||
538 | // Differenz aus Distanz durch eche geschwindigkeit aus VELNED Msg ersetzten |
- | |
539 | diff_east_f = -speed_east; |
- | |
540 | diff_north_f = speed_north; |
512 | gps_updte_flag = 0; // Neue Werte koennen vom GPS geholt werden |
541 | 513 | ||
542 | #define GPSINT_MAX 3000 // Neues Verfahren ab 30.12.2007 bei Integratoroverflow |
514 | #define GPSINT_MAX 3000 // Neues Verfahren ab 30.12.2007 bei Integratoroverflow |
543 | if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) |
515 | if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) |
544 | { |
516 | { |
Line 557... | Line 529... | ||
557 | { |
529 | { |
558 | int_east = 0; |
530 | int_east = 0; |
559 | int_north = 0; |
531 | int_north = 0; |
560 | } |
532 | } |
Line -... | Line 533... | ||
- | 533 | ||
- | 534 | // Verstaerkungsfaktor fuer Geschwindigkeit ermitteln um exponentielles Verhalten zu erzielen |
|
- | 535 | amplfy_speed_east = (((abs(speed_east)) *(DIFF_Y_MAX-1)*10)/DIFF_X_MAX) +10; |
|
- | 536 | amplfy_speed_north = (((abs(speed_north))*(DIFF_Y_MAX-1)*10)/DIFF_X_MAX) +10; |
|
- | 537 | if (amplfy_speed_east > (DIFF_Y_MAX *10)) amplfy_speed_east = DIFF_Y_MAX *10; // Begrenzung |
|
- | 538 | if (amplfy_speed_north > (DIFF_Y_MAX *10)) amplfy_speed_north = DIFF_Y_MAX *10; // Begrenzung |
|
- | 539 | amplfy_speed_east *= (Parameter_UserParam3/GPS_USR_PAR_FKT); |
|
- | 540 | amplfy_speed_north *= (Parameter_UserParam3/GPS_USR_PAR_FKT); |
|
- | 541 | amplfy_speed_east /= 10; //Faktor 10 wieder rausnehmen |
|
561 | 542 | amplfy_speed_north /= 10; //Faktor 10 wieder rausnehmen |
|
- | 543 | #define GPS_SPEED_SCALE 5 |
|
- | 544 | speed_east /= GPS_SPEED_SCALE; // Wegen overflow Gefahr Wert jetzt schon runterskalieren |
|
- | 545 | speed_north /= GPS_SPEED_SCALE; // Wegen overflow Gefahr Wert jetzt schon runterskalieren |
|
562 | // int phi; |
546 | debug_gp_2 = speed_east; // zum Debuggen |
- | 547 | debug_gp_3 = speed_north; // zum Debuggen |
|
563 | // phi = arctan_i(abs(dist_north),abs(dist_east)); |
548 | debug_gp_4 = amplfy_speed_east; // zum Debuggen |
Line 564... | Line -... | ||
564 | // dist = get_dist(dist_east,dist_north,phi); //Zunaechst Entfernung zum Ziel ermitteln |
- | |
565 | - | ||
566 | if (hold_fast == 0) // je Nach Modus andere Verstaerkungskurve fuer Differenzierer |
- | |
567 | { |
- | |
568 | // diff_v = (int)((dist * (GPS_DIFF_NRML_MAX_V - 10)) / GPS_DIFF_NRML_MAX_D) +10; //Verstaerkung * 10 |
- | |
569 | // if (diff_v > GPS_DIFF_NRML_MAX_V) diff_v = GPS_DIFF_NRML_MAX_V; //begrenzen |
- | |
570 | // diff_v = GPS_DIFF_NRML_MAX_V ; //variable Versterkung raus 31.12.2007 |
- | |
571 | } |
- | |
572 | else |
- | |
573 | { |
- | |
574 | // diff_v = (int)((dist * (GPS_DIFF_FAST_MAX_V - 10)) / GPS_DIFF_FAST_MAX_D) +10; //Verstaerkung * 10 |
- | |
575 | // if (diff_v > GPS_DIFF_FAST_MAX_V) diff_v = GPS_DIFF_FAST_MAX_V; //begrenzen |
- | |
576 | // diff_v = GPS_DIFF_FAST_MAX_V ; //variable Versterkung raus 31.12.2007 |
- | |
577 | } |
- | |
578 | diff_v_east = (((abs(diff_east_f)) *(DIFF_Y_MAX-1))/DIFF_X_MAX) +10; |
- | |
579 | diff_v_north = (((abs(diff_north_f))*(DIFF_Y_MAX-1))/DIFF_X_MAX) +10; |
- | |
580 | if (diff_v_east > (DIFF_Y_MAX *10)) diff_v_east = DIFF_Y_MAX *10; // Begrenzung |
- | |
581 | if (diff_v_north > (DIFF_Y_MAX *10)) diff_v_north = DIFF_Y_MAX *10; // Begrenzung |
- | |
582 | // diff_v_east *= 2; |
- | |
583 | // diff_v_north *= 2; |
- | |
584 | - | ||
585 | // debug_gp_2 = diff_v_east; // zum Debuggen |
- | |
586 | // debug_gp_3 = diff_v_north; // zum Debuggen |
- | |
Line 587... | Line 549... | ||
587 | // debug_gp_4 = diff_east_f; // zum Debuggen |
549 | debug_gp_5 = amplfy_speed_north; // zum Debuggen |
588 | // debug_gp_5 = diff_north_f; // zum Debuggen |
550 | |
589 | 551 | ||
Line 599... | Line 561... | ||
599 | else if (int_east1 < -INT1_MAX) int_east1 = -INT1_MAX; |
561 | else if (int_east1 < -INT1_MAX) int_east1 = -INT1_MAX; |
600 | if (int_north1 > INT1_MAX) int_north1 = INT1_MAX; //begrenzen |
562 | if (int_north1 > INT1_MAX) int_north1 = INT1_MAX; //begrenzen |
601 | else if (int_north1 < -INT1_MAX) int_north1 = -INT1_MAX; |
563 | else if (int_north1 < -INT1_MAX) int_north1 = -INT1_MAX; |
Line 602... | Line 564... | ||
602 | 564 | ||
603 | //PID Regler Werte aufsummieren |
565 | //PID Regler Werte aufsummieren |
604 | gps_reg_x = ((int)int_east1 + ((dist_east * (Parameter_UserParam1/GPS_USR_PAR_FKT) * diff_p)/(8*2))+ ((diff_east_f * diff_v_east * (Parameter_UserParam3/GPS_USR_PAR_FKT))/100)); // I + P +D Anteil X Achse |
566 | 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 |
605 | gps_reg_y = ((int)int_north1 + ((dist_north * (Parameter_UserParam1/GPS_USR_PAR_FKT) * diff_p)/(8*2))+ ((diff_north_f * diff_v_north * (Parameter_UserParam3/GPS_USR_PAR_FKT))/100)); // I + P +D Anteil Y Achse |
567 | 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 |
606 | debug_gp_0 = gps_reg_x; // zum Debuggen |
568 | debug_gp_0 = gps_reg_x; // zum Debuggen |
Line 607... | Line 569... | ||
607 | debug_gp_1 = gps_reg_y; // zum Debuggen |
569 | debug_gp_1 = gps_reg_y; // zum Debuggen |
608 | 570 |