Rev 621 | 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 |