| 16,7 → 16,7 |
|---|
| Auswertung der Daten vom GPS im ublox Format |
| Hold Modus mit PID Regler |
| Rückstuerz zur Basis Funktion |
| Stand 8.10.2007 |
| Stand 12.10.2007 |
| ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| */ |
| #include "main.h" |
| 328,12 → 328,12 |
|---|
| gps_rel_hold_position.utm_east = gps_rel_act_position.utm_east; |
| gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north; |
| gps_rel_hold_position.status = 1; // gueltige Positionsdaten |
| //Richtung zur Home Positionbezogen auf Nordpol bestimmen |
| //Richtung zur Home Position bezogen auf Nordpol bestimmen |
| hdng_2home = arctan_i(-gps_rel_start_position.utm_east,-gps_rel_start_position.utm_north); |
| // in Winkel 0...360 Grad umrechnen |
| if (( gps_rel_start_position.utm_east < 0)) hdng_2home = ( 90-hdng_2home); |
| else hdng_2home = (270 - hdng_2home); |
| dist_2home = (int)get_dist(gps_rel_start_position.utm_east,gps_rel_start_position.utm_north,hdng_2home); //Entfernung zur Home Position bestimmen |
| dist_2home = (int) get_dist(gps_rel_start_position.utm_east,gps_rel_start_position.utm_north,hdng_2home); //Entfernung zur Home Position bestimmen |
| gps_state = GPS_CRTL_HOME_ACTIVE; |
| return (GPS_STST_OK); |
| } |
| 423,8 → 423,8 |
|---|
| if ((d1 < GPS_G2T_FAST_TOL) && (d2 < GPS_G2T_FAST_TOL)) //nur weiter wenn Lage innerhalb der Toleranz |
| { |
| if (gps_g2t_act_v < GPS_G2T_V_MAX) gps_g2t_act_v++; //Geschwindigkeit langsam erhoehen |
| dist_flown +=(long)gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit |
| gps_sub_state = GPS_HOME_FAST_IN_TOL; |
| dist_flown +=(long)gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit |
| gps_sub_state = GPS_HOME_FAST_IN_TOL; |
| } |
| else //Den Lageregler in Ruhe arbeiten lassen weil ausserhalb der Toleranz |
| { |
| 503,10 → 503,20 |
|---|
| int_north += dist_north; |
| diff_east += dist_east; // Differenz zur vorhergehenden East Position |
| diff_north += dist_north; // Differenz zur vorhergehenden North Position |
| /* |
| diff_east_f = ((diff_east_f )/4) + (diff_east*3/4); //Differenzierer filtern |
| diff_north_f = ((diff_north_f)/4) + (diff_north*3/4); //Differenzierer filtern |
| */ |
| |
| if (hold_fast > 0) // wegen Sollpositionsspruengen im Fast Mode Differenzierer daempfen |
| { |
| diff_east_f = ((diff_east_f *3)/4) + (diff_east *1/4); //Differenzierer filtern |
| diff_north_f = ((diff_north_f *3)/4) + (diff_north*1/4); //Differenzierer filtern |
| } |
| else // schwache Filterung |
| { |
| // diff_east_f = diff_east; |
| // diff_north_f = diff_north; |
| diff_east_f = ((diff_east_f *2)/4) + (diff_east *2/4); //Differenzierer filtern |
| diff_north_f = ((diff_north_f *2)/4) + (diff_north*2/4); //Differenzierer filtern |
| } |
| |
| #define GPSINT_MAX 30000 //neuer Wert ab 7.10.2007 Begrenzung |
| if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) //Bei zu hohem Wert Integrator auf Wert halten |
| { |
| 551,8 → 561,8 |
|---|
| else if (int_north1 < -INT1_MAX) int_north1 = -INT1_MAX; |
| |
| //PID Regler Werte aufsummieren |
| 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 |
| 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 |
| 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 |
| 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 |
| |
| //Ziel-Richtung bezogen auf Nordpol bestimmen |
| GPS_hdng_abs_2trgt = arctan_i(gps_reg_x,gps_reg_y); |