| 13,7 → 13,7 |
|---|
| Peter Muehlenbrock |
| Auswertung der Daten vom GPS im ublox Format |
| Hold Modus mit PID Regler |
| Stand 28.9.2007 |
| Stand 29.9.2007 |
| ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| */ |
| #include "main.h" |
| 283,10 → 283,10 |
|---|
| //Zentrale Statemachine fuer alle GPS relevanten Regelungsablauefe |
| short int GPS_CRTL(short int cmd) |
| { |
| static unsigned int cnt; //Zaehler fuer diverse Verzoegerungen |
| static int int_east,int_north; //Integrierer |
| static signed int dist_north,dist_east; |
| int diff_east,diff_north; // Differenzierer (Differenz zum vorhergehenden x bzw. y Wert) |
| static unsigned int cnt; //Zaehler fuer diverse Verzoegerungen |
| static signed int int_east,int_north; //Integrierer |
| static signed int dist_north,dist_east; |
| int diff_east,diff_north; // Differenzierer (Differenz zum vorhergehenden x bzw. y Wert) |
| int n; |
| long int dist; |
| switch (cmd) |
| 312,6 → 312,8 |
|---|
| int_north = 0; |
| gps_reg_x = 0; |
| gps_reg_y = 0; |
| dist_east = 0; |
| dist_north = 0; |
| 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 |
| 357,10 → 359,10 |
|---|
| gps_updte_flag = 0; |
| // ab hier wird geregelt |
| |
| diff_east = -dist_east; //Alten Wert schon mal abziehen |
| diff_north = -dist_north; |
| dist_east = gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east; |
| dist_north = gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north; |
| diff_east = -dist_east; //Alten Wert schon mal abziehen |
| diff_north = -dist_north; |
| dist_east = gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east; |
| dist_north = gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north; |
| int_east += dist_east; |
| int_north += dist_north; |
| diff_east += dist_east; // Differenz zur vorhergehenden East Position |
| 372,16 → 374,10 |
|---|
| int_east -= dist_east; |
| int_north -= dist_north; |
| } |
| |
| #define DIST_MAX 100 //neuer Wert ab 24.9.2007 Begrenzung |
| if (dist_east > DIST_MAX) dist_east = DIST_MAX; |
| if (dist_east <-DIST_MAX) dist_east = -DIST_MAX; |
| if (dist_north > DIST_MAX) dist_north = DIST_MAX; |
| if (dist_north <-DIST_MAX) dist_north = -DIST_MAX; |
| |
| //PID Regler |
| gps_reg_x = ((int_east * Parameter_UserParam1)/32) + ((dist_east * Parameter_UserParam2)/8)+ ((diff_east * Parameter_UserParam3)/2); // |
| gps_reg_y = ((int_north * Parameter_UserParam1)/32) + ((dist_north * Parameter_UserParam2)/8)+ ((diff_north * Parameter_UserParam3)/2); // I + P +D Anteil Y Achse |
| gps_reg_x = ((int_east * Parameter_UserParam2)/32) + ((dist_east * Parameter_UserParam1)/8)+ ((diff_east * Parameter_UserParam3)/2); // |
| gps_reg_y = ((int_north * Parameter_UserParam2)/32) + ((dist_north * Parameter_UserParam1)/8)+ ((diff_north * Parameter_UserParam3)/2); // I + P +D Anteil Y Achse |
| |
| //Richtung bezogen auf Nordpol bestimmen |
| GPS_hdng_abs_2trgt = arctan_i((long)gps_reg_x,(long)gps_reg_y); |
| 397,8 → 393,7 |
|---|
| else if (GPS_hdng_rel_2trgt >180) GPS_hdng_rel_2trgt = 360 - GPS_hdng_rel_2trgt; |
| else if (GPS_hdng_rel_2trgt <-180) GPS_hdng_rel_2trgt = 360 + GPS_hdng_rel_2trgt; |
| |
| // Regelabweichung aus x,y zu Ziel in Distanz umrechnen |
| |
| // Regelabweichung aus x,y zu Ziel in Distanz umrechnen |
| if (abs(gps_reg_x) > abs(gps_reg_y) ) |
| { |
| dist = (long)gps_reg_x; //Groesseren Wert wegen besserer Genauigkeit nehmen |
| 409,18 → 404,41 |
|---|
| dist = (long)gps_reg_y; |
| dist = abs((dist *1000) / (long) sin_i(GPS_hdng_abs_2trgt)); |
| } |
| if (dist > 200) dist = 200; |
| GPS_dist_2trgt = (int )dist; |
| // if (dist > 200) dist = 200; |
| // GPS_dist_2trgt = (int )dist; |
| |
| // Winkel und Distanz in Nick und Roll groessen umrechnen |
| GPS_Roll = (int) +((dist * sin_i(GPS_hdng_rel_2trgt))/(1000*8)); |
| GPS_Nick = (int) -((dist * cos_i(GPS_hdng_rel_2trgt))/(1000*8)); |
| GPS_Roll = (int) +( (dist * (long) sin_i(GPS_hdng_rel_2trgt))/1000); |
| GPS_Nick = (int) -((dist * (long) cos_i(GPS_hdng_rel_2trgt))/1000); |
| |
| if (GPS_Roll > GPS_NICKROLL_MAX) GPS_Roll = GPS_NICKROLL_MAX; //Auf Maxwerte begrenzen |
| else if (GPS_Roll < -GPS_NICKROLL_MAX) GPS_Roll = - GPS_NICKROLL_MAX; |
| if (GPS_Nick > GPS_NICKROLL_MAX) GPS_Nick = GPS_NICKROLL_MAX; |
| else if (GPS_Nick < - GPS_NICKROLL_MAX) GPS_Nick = - GPS_NICKROLL_MAX; |
| return (GPS_STST_OK); |
| #define GPS_V 8 |
| if (GPS_Roll > (GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = (GPS_NICKROLL_MAX * GPS_V); |
| else if (GPS_Roll < -(GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = -(GPS_NICKROLL_MAX * GPS_V); |
| if (GPS_Nick > (GPS_NICKROLL_MAX * GPS_V)) GPS_Nick = (GPS_NICKROLL_MAX * GPS_V); |
| else if (GPS_Nick < -(GPS_NICKROLL_MAX * GPS_V)) GPS_Nick = -(GPS_NICKROLL_MAX * GPS_V); |
| |
| //Kleine Werte verstaerken, Grosse abschwaechen |
| long int nick,roll; |
| roll = (((long) GPS_Roll) * ((long)sin_i(abs((GPS_Roll*90)/(GPS_NICKROLL_MAX * GPS_V)))))/1000; |
| GPS_Roll = (int) (roll / GPS_V); |
| nick = (((long) GPS_Nick) * ((long)sin_i(abs((GPS_Nick*90)/(GPS_NICKROLL_MAX * GPS_V)))))/1000; |
| GPS_Nick = (int) (nick / GPS_V); |
| |
| if ((abs(dist_east) > GPS_DIST_MAX) || (abs(dist_north) > GPS_DIST_MAX)) // bei zu grossem Abstand abbrechen |
| { |
| GPS_Roll = 0; |
| GPS_Nick = 0; |
| gps_state = GPS_CRTL_IDLE; |
| return (GPS_STST_ERR); |
| } |
| else // Distanz ok // Die Abfrage kann noch rausfliegen, weil vorher bereits begrenzung war |
| { |
| if (GPS_Roll > GPS_NICKROLL_MAX) GPS_Roll = GPS_NICKROLL_MAX; //Auf Maxwerte begrenzen |
| else if (GPS_Roll < -GPS_NICKROLL_MAX) GPS_Roll = - GPS_NICKROLL_MAX; |
| if (GPS_Nick > GPS_NICKROLL_MAX) GPS_Nick = GPS_NICKROLL_MAX; |
| else if (GPS_Nick < - GPS_NICKROLL_MAX) GPS_Nick = - GPS_NICKROLL_MAX; |
| return (GPS_STST_OK); |
| } |
| } |
| else return (GPS_STST_OK); |
| break; |