Rev 621 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 621 | Rev 622 | ||
|---|---|---|---|
| Line 153... | Line 153... | ||
| 153 | actual_status.status = 0; |
153 | actual_status.status = 0; |
| 154 | gps_act_position.utm_east = actual_pos.utm_east/10; |
154 | gps_act_position.utm_east = actual_pos.utm_east/10; |
| 155 | gps_act_position.utm_north = actual_pos.utm_north/10; |
155 | gps_act_position.utm_north = actual_pos.utm_north/10; |
| 156 | gps_act_position.utm_alt = actual_pos.utm_alt/10; |
156 | gps_act_position.utm_alt = actual_pos.utm_alt/10; |
| 157 | actual_pos.status = 0; //neue ublox Messages anfordern |
157 | actual_pos.status = 0; //neue ublox Messages anfordern |
| 158 | // gps_act_position.speed_gnd = actual_speed.speed_gnd/10; |
158 | gps_act_position.speed_gnd = actual_speed.speed_gnd; |
| 159 | // gps_act_position.speed_gnd = actual_speed.speed_gnd/10; |
159 | gps_act_position.speed_gnd = actual_speed.speed_gnd; |
| 160 | // gps_act_position.heading = actual_speed.heading/100000; |
160 | gps_act_position.heading = actual_speed.heading/100000; |
| 161 | actual_speed.status = 0; |
161 | actual_speed.status = 0; |
| 162 | gps_act_position.status = 1; |
162 | gps_act_position.status = 1; |
| 163 | n = 0; //Daten gueltig |
163 | n = 0; //Daten gueltig |
| 164 | } |
164 | } |
| 165 | else |
165 | else |
| Line 304... | Line 304... | ||
| 304 | 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 |
| 305 | long signed int dev; |
305 | long signed int dev; |
| 306 | 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 |
307 | int diff_v_east,diff_v_north; //Verstaerkungsfaktoren fuer Differenzierer |
| 308 | static signed int int_east,int_north; //Integrierer |
308 | static signed int int_east,int_north; //Integrierer |
| - | 309 | int speed_east,speed_north; //Aktuelle Geschwindigkeit |
|
| - | 310 | signed long int_east1,int_north1; |
|
| 309 | // signed long dist; |
311 | // signed long dist; |
| Line 310... | Line 312... | ||
| 310 | 312 | ||
| 311 | switch (cmd) |
313 | switch (cmd) |
| Line 375... | Line 377... | ||
| 375 | int_east = 0, int_north = 0; |
377 | int_east = 0, int_north = 0; |
| 376 | gps_reg_x = 0, gps_reg_y = 0; |
378 | gps_reg_x = 0, gps_reg_y = 0; |
| 377 | dist_east = 0, dist_north = 0; |
379 | dist_east = 0, dist_north = 0; |
| 378 | diff_east_f = 0, diff_north_f= 0; |
380 | diff_east_f = 0, diff_north_f= 0; |
| 379 | diff_east = 0, diff_north = 0; |
381 | diff_east = 0, diff_north = 0; |
| - | 382 | speed_east = 0; speed_north= 0; |
|
| 380 | int_ovfl_cnt = 0; |
383 | int_ovfl_cnt = 0; |
| 381 | gps_rel_hold_position.utm_east = gps_rel_act_position.utm_east; |
384 | gps_rel_hold_position.utm_east = gps_rel_act_position.utm_east; |
| 382 | gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north; |
385 | gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north; |
| 383 | gps_rel_hold_position.status = 1; // gueltige Positionsdaten |
386 | gps_rel_hold_position.status = 1; // gueltige Positionsdaten |
| 384 | gps_state = GPS_CRTL_HOLD_ACTIVE; |
387 | gps_state = GPS_CRTL_HOLD_ACTIVE; |
| Line 503... | Line 506... | ||
| 503 | 506 | ||
| 504 | 507 | ||
| 505 | case GPS_CRTL_HOLD_ACTIVE: // Hier werden die Daten fuer Nick und Roll errechnet |
508 | 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 |
- | |
| 507 | { |
509 | if (gps_updte_flag >0) // nur wenn neue GPS Daten vorliegen |
| 508 | gps_updte_flag = 0; |
510 | { |
| 509 | // ab hier wird geregelt |
511 | // ab hier wird geregelt |
| 510 | diff_east = dist_east; //Alten Wert fuer Differenzierer schon mal addieren |
512 | diff_east = dist_east; //Alten Wert fuer Differenzierer schon mal addieren |
| 511 | diff_north = dist_north; |
513 | diff_north = dist_north; |
| 512 | dist_east = gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east; |
514 | 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; |
515 | dist_north = gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north; |
| 514 | int_east += dist_east; |
516 | int_east += dist_east; |
| 515 | int_north += dist_north; |
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 |
|
| - | 520 | speed_east = (int) actual_speed.speed_e; |
|
| 516 | diff_east -= dist_east; // Differenz zur vorhergehenden East Position |
521 | speed_north = (int) actual_speed.speed_n; |
| - | 522 | gps_updte_flag = 0; // Neue Werte koennen vom GPS geholt werden |
|
| - | 523 | ||
| - | 524 | debug_gp_2 = speed_east; // zum Debuggen |
|
| 517 | diff_north -= dist_north; // Differenz zur vorhergehenden North Position |
525 | debug_gp_3 = speed_north; // zum Debuggen |
| 518 | 526 | ||
| 519 | if (hold_fast > 0) // wegen Sollpositionsspruengen im Fast Mode Differenzierer daempfen |
527 | if (hold_fast > 0) // wegen Sollpositionsspruengen im Fast Mode Differenzierer daempfen |
| 520 | { |
528 | { |
| 521 | diff_east_f = (((diff_east_f *2)/3) + ((diff_east *1*10)/3)); //Differenzierer filtern |
529 | 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 |
530 | diff_north_f = (((diff_north_f *2)/3) + ((diff_north*1*10)/3)); //Differenzierer filtern |
| 523 | } |
531 | } |
| 524 | else // schwache Filterung |
532 | else // schwache Filterung |
| 525 | { |
533 | { |
| 526 | diff_east_f = (((diff_east_f * 1)/4) + ((diff_east *3*10)/4)); //Differenzierer filtern |
534 | diff_east_f = (((diff_east_f * 2)/4) + ((diff_east *2*10)/4)); //Differenzierer filtern |
| Line -... | Line 535... | ||
| - | 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 |
|
| 527 | diff_north_f = (((diff_north_f * 1)/4) + ((diff_north*3*10)/4)); //Differenzierer filtern |
539 | diff_east_f = -speed_east; |
| 528 | } |
540 | diff_north_f = speed_north; |
| 529 | 541 | ||
| 530 | #define GPSINT_MAX 3000 // Neues Verfahren ab 30.12.2007 bei Integratoroverflow |
542 | #define GPSINT_MAX 3000 // Neues Verfahren ab 30.12.2007 bei Integratoroverflow |
| 531 | if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) |
543 | if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) |
| Line 545... | Line 557... | ||
| 545 | { |
557 | { |
| 546 | int_east = 0; |
558 | int_east = 0; |
| 547 | int_north = 0; |
559 | int_north = 0; |
| 548 | } |
560 | } |
| Line 549... | Line -... | ||
| 549 | - | ||
| 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 |
- | |
| 552 | signed long int_east1,int_north1; |
- | |
| 553 | 561 | ||
| 554 | // int phi; |
562 | // int phi; |
| 555 | // phi = arctan_i(abs(dist_north),abs(dist_east)); |
563 | // phi = arctan_i(abs(dist_north),abs(dist_east)); |
| Line 556... | Line 564... | ||
| 556 | // dist = get_dist(dist_east,dist_north,phi); //Zunaechst Entfernung zum Ziel ermitteln |
564 | // dist = get_dist(dist_east,dist_north,phi); //Zunaechst Entfernung zum Ziel ermitteln |
| Line 569... | Line 577... | ||
| 569 | } |
577 | } |
| 570 | diff_v_east = (((abs(diff_east_f)) *(DIFF_Y_MAX-1))/DIFF_X_MAX) +10; |
578 | diff_v_east = (((abs(diff_east_f)) *(DIFF_Y_MAX-1))/DIFF_X_MAX) +10; |
| 571 | diff_v_north = (((abs(diff_north_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; |
| 572 | if (diff_v_east > (DIFF_Y_MAX *10)) diff_v_east = DIFF_Y_MAX *10; // Begrenzung |
580 | if (diff_v_east > (DIFF_Y_MAX *10)) diff_v_east = DIFF_Y_MAX *10; // Begrenzung |
| 573 | if (diff_v_north > (DIFF_Y_MAX *10)) diff_v_north = DIFF_Y_MAX *10; // Begrenzung |
581 | if (diff_v_north > (DIFF_Y_MAX *10)) diff_v_north = DIFF_Y_MAX *10; // Begrenzung |
| 574 | diff_v_east *= 2; |
582 | // diff_v_east *= 2; |
| 575 | diff_v_north *= 2; |
583 | // diff_v_north *= 2; |
| Line 576... | Line 584... | ||
| 576 | 584 | ||
| 577 | // debug_gp_2 = diff_v_east; // zum Debuggen |
585 | // debug_gp_2 = diff_v_east; // zum Debuggen |
| 578 | // debug_gp_3 = diff_v_north; // zum Debuggen |
586 | // debug_gp_3 = diff_v_north; // zum Debuggen |
| 579 | // debug_gp_4 = diff_east_f; // zum Debuggen |
587 | // debug_gp_4 = diff_east_f; // zum Debuggen |