Rev 266 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 266 | Rev 291 | ||
---|---|---|---|
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 8.10.2007 |
19 | Stand 12.10.2007 |
20 | ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
20 | ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
21 | */ |
21 | */ |
22 | #include "main.h" |
22 | #include "main.h" |
23 | //#include "gps.h" |
23 | //#include "gps.h" |
Line 326... | Line 326... | ||
326 | gps_rel_start_position.utm_north= gps_rel_act_position.utm_north; |
326 | gps_rel_start_position.utm_north= gps_rel_act_position.utm_north; |
327 | gps_rel_start_position.status = 1; // gueltige Positionsdaten |
327 | gps_rel_start_position.status = 1; // gueltige Positionsdaten |
328 | gps_rel_hold_position.utm_east = gps_rel_act_position.utm_east; |
328 | gps_rel_hold_position.utm_east = gps_rel_act_position.utm_east; |
329 | gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north; |
329 | gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north; |
330 | gps_rel_hold_position.status = 1; // gueltige Positionsdaten |
330 | gps_rel_hold_position.status = 1; // gueltige Positionsdaten |
331 | //Richtung zur Home Positionbezogen auf Nordpol bestimmen |
331 | //Richtung zur Home Position bezogen auf Nordpol bestimmen |
332 | hdng_2home = arctan_i(-gps_rel_start_position.utm_east,-gps_rel_start_position.utm_north); |
332 | hdng_2home = arctan_i(-gps_rel_start_position.utm_east,-gps_rel_start_position.utm_north); |
333 | // in Winkel 0...360 Grad umrechnen |
333 | // in Winkel 0...360 Grad umrechnen |
334 | if (( gps_rel_start_position.utm_east < 0)) hdng_2home = ( 90-hdng_2home); |
334 | if (( gps_rel_start_position.utm_east < 0)) hdng_2home = ( 90-hdng_2home); |
335 | else hdng_2home = (270 - hdng_2home); |
335 | else hdng_2home = (270 - hdng_2home); |
336 | dist_2home = (int)get_dist(gps_rel_start_position.utm_east,gps_rel_start_position.utm_north,hdng_2home); //Entfernung zur Home Position bestimmen |
336 | dist_2home = (int) get_dist(gps_rel_start_position.utm_east,gps_rel_start_position.utm_north,hdng_2home); //Entfernung zur Home Position bestimmen |
337 | gps_state = GPS_CRTL_HOME_ACTIVE; |
337 | gps_state = GPS_CRTL_HOME_ACTIVE; |
338 | return (GPS_STST_OK); |
338 | return (GPS_STST_OK); |
339 | } |
339 | } |
340 | else |
340 | else |
341 | { |
341 | { |
Line 421... | Line 421... | ||
421 | if (d3 > GPS_G2T_DIST_MAX_STOP) // Schneller Rueckflug, noch weit weg vom Ziel |
421 | if (d3 > GPS_G2T_DIST_MAX_STOP) // Schneller Rueckflug, noch weit weg vom Ziel |
422 | { |
422 | { |
423 | if ((d1 < GPS_G2T_FAST_TOL) && (d2 < GPS_G2T_FAST_TOL)) //nur weiter wenn Lage innerhalb der Toleranz |
423 | if ((d1 < GPS_G2T_FAST_TOL) && (d2 < GPS_G2T_FAST_TOL)) //nur weiter wenn Lage innerhalb der Toleranz |
424 | { |
424 | { |
425 | if (gps_g2t_act_v < GPS_G2T_V_MAX) gps_g2t_act_v++; //Geschwindigkeit langsam erhoehen |
425 | if (gps_g2t_act_v < GPS_G2T_V_MAX) gps_g2t_act_v++; //Geschwindigkeit langsam erhoehen |
426 | dist_flown +=(long)gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit |
426 | dist_flown +=(long)gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit |
427 | gps_sub_state = GPS_HOME_FAST_IN_TOL; |
427 | gps_sub_state = GPS_HOME_FAST_IN_TOL; |
428 | } |
428 | } |
429 | else //Den Lageregler in Ruhe arbeiten lassen weil ausserhalb der Toleranz |
429 | else //Den Lageregler in Ruhe arbeiten lassen weil ausserhalb der Toleranz |
430 | { |
430 | { |
431 | if (gps_g2t_act_v > 1) gps_g2t_act_v--; // Geschwindigkeit reduzieren |
431 | if (gps_g2t_act_v > 1) gps_g2t_act_v--; // Geschwindigkeit reduzieren |
432 | dist_flown++; //Auch ausserhalb der Toleranz langsam erhoehen |
432 | dist_flown++; //Auch ausserhalb der Toleranz langsam erhoehen |
Line 501... | Line 501... | ||
501 | dist_north = gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north; |
501 | dist_north = gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north; |
502 | int_east += dist_east; |
502 | int_east += dist_east; |
503 | int_north += dist_north; |
503 | int_north += dist_north; |
504 | diff_east += dist_east; // Differenz zur vorhergehenden East Position |
504 | diff_east += dist_east; // Differenz zur vorhergehenden East Position |
505 | diff_north += dist_north; // Differenz zur vorhergehenden North Position |
505 | diff_north += dist_north; // Differenz zur vorhergehenden North Position |
506 | /* |
506 | |
- | 507 | if (hold_fast > 0) // wegen Sollpositionsspruengen im Fast Mode Differenzierer daempfen |
|
- | 508 | { |
|
- | 509 | diff_east_f = ((diff_east_f *3)/4) + (diff_east *1/4); //Differenzierer filtern |
|
- | 510 | diff_north_f = ((diff_north_f *3)/4) + (diff_north*1/4); //Differenzierer filtern |
|
- | 511 | } |
|
- | 512 | else // schwache Filterung |
|
- | 513 | { |
|
- | 514 | // diff_east_f = diff_east; |
|
- | 515 | // diff_north_f = diff_north; |
|
507 | diff_east_f = ((diff_east_f )/4) + (diff_east*3/4); //Differenzierer filtern |
516 | diff_east_f = ((diff_east_f *2)/4) + (diff_east *2/4); //Differenzierer filtern |
508 | diff_north_f = ((diff_north_f)/4) + (diff_north*3/4); //Differenzierer filtern |
517 | diff_north_f = ((diff_north_f *2)/4) + (diff_north*2/4); //Differenzierer filtern |
- | 518 | } |
|
509 | */ |
519 | |
510 | #define GPSINT_MAX 30000 //neuer Wert ab 7.10.2007 Begrenzung |
520 | #define GPSINT_MAX 30000 //neuer Wert ab 7.10.2007 Begrenzung |
511 | if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) //Bei zu hohem Wert Integrator auf Wert halten |
521 | if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) //Bei zu hohem Wert Integrator auf Wert halten |
512 | { |
522 | { |
513 | int_east -= dist_east; |
523 | int_east -= dist_east; |
514 | int_north -= dist_north; |
524 | int_north -= dist_north; |
Line 549... | Line 559... | ||
549 | else if (int_east1 < -INT1_MAX) int_east1 = -INT1_MAX; |
559 | else if (int_east1 < -INT1_MAX) int_east1 = -INT1_MAX; |
550 | if (int_north1 > INT1_MAX) int_north1 = INT1_MAX; //begrenzen |
560 | if (int_north1 > INT1_MAX) int_north1 = INT1_MAX; //begrenzen |
551 | else if (int_north1 < -INT1_MAX) int_north1 = -INT1_MAX; |
561 | else if (int_north1 < -INT1_MAX) int_north1 = -INT1_MAX; |
Line 552... | Line 562... | ||
552 | 562 | ||
553 | //PID Regler Werte aufsummieren |
563 | //PID Regler Werte aufsummieren |
554 | gps_reg_x = ((int)int_east1 + ((dist_east * Parameter_UserParam1 * diff_p)/(8*2))+ ((diff_east * diff_v * Parameter_UserParam3)/10)); // I + P +D Anteil X Achse |
564 | gps_reg_x = ((int)int_east1 + ((dist_east * Parameter_UserParam1 * diff_p)/(8*2))+ ((diff_east_f * diff_v * Parameter_UserParam3)/10)); // I + P +D Anteil X Achse |
Line 555... | Line 565... | ||
555 | gps_reg_y = ((int)int_north1 + ((dist_north * Parameter_UserParam1 * diff_p)/(8*2))+ ((diff_north * diff_v * Parameter_UserParam3)/10)); // I + P +D Anteil Y Achse |
565 | gps_reg_y = ((int)int_north1 + ((dist_north * Parameter_UserParam1 * diff_p)/(8*2))+ ((diff_north_f * diff_v * Parameter_UserParam3)/10)); // I + P +D Anteil Y Achse |
556 | 566 | ||
Line 557... | Line 567... | ||
557 | //Ziel-Richtung bezogen auf Nordpol bestimmen |
567 | //Ziel-Richtung bezogen auf Nordpol bestimmen |