Rev 194 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 194 | Rev 197 | ||
---|---|---|---|
Line 11... | Line 11... | ||
11 | */ |
11 | */ |
12 | /*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
12 | /*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
13 | Peter Muehlenbrock |
13 | Peter Muehlenbrock |
14 | Auswertung der Daten vom GPS im ublox Format |
14 | Auswertung der Daten vom GPS im ublox Format |
15 | Hold Modus mit PID Regler |
15 | Hold Modus mit PID Regler |
16 | Stand 29.9.2007 |
16 | Stand 2.10.2007 |
17 | ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
17 | ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
18 | */ |
18 | */ |
19 | #include "main.h" |
19 | #include "main.h" |
20 | //#include "gps.h" |
20 | //#include "gps.h" |
Line 289... | Line 289... | ||
289 | static unsigned int cnt; //Zaehler fuer diverse Verzoegerungen |
289 | static unsigned int cnt; //Zaehler fuer diverse Verzoegerungen |
290 | static signed int int_east,int_north; //Integrierer |
290 | static signed int int_east,int_north; //Integrierer |
291 | static signed int dist_north,dist_east; |
291 | static signed int dist_north,dist_east; |
292 | static signed int diff_east,diff_north; // Differenzierer (Differenz zum vorhergehenden x bzw. y Wert) |
292 | static signed int diff_east,diff_north; // Differenzierer (Differenz zum vorhergehenden x bzw. y Wert) |
293 | static signed int diff_east_f,diff_north_f; // Differenzierer, gefiltert |
293 | static signed int diff_east_f,diff_north_f; // Differenzierer, gefiltert |
294 | signed int n; |
294 | signed int n,diff_v; |
295 | long signed int dist,d,n_l; |
295 | long signed int dev,n_l; |
296 | switch (cmd) |
296 | switch (cmd) |
297 | { |
297 | { |
Line 298... | Line 298... | ||
298 | 298 | ||
299 | case GPS_CMD_REQ_HOME: // Es soll zum Startpunkt zurueckgeflogen werden. |
299 | case GPS_CMD_REQ_HOME: // Es soll zum Startpunkt zurueckgeflogen werden. |
Line 382... | Line 382... | ||
382 | if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) //Bei zu hohem Wert Integrator auf Wert halten |
382 | if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) //Bei zu hohem Wert Integrator auf Wert halten |
383 | { |
383 | { |
384 | int_east -= dist_east; |
384 | int_east -= dist_east; |
385 | int_north -= dist_north; |
385 | int_north -= dist_north; |
386 | } |
386 | } |
- | 387 | //Variable Verstarkung fuer Differenzierer ermitteln. Je weiter vom Ziel wir entfernt sind |
|
387 | // P-Anteil Kleine Werte verstaerken, grosse abschwaechen |
388 | // desto groesser wird der Faktor. Es gibt aber einen Maximalwert. bei 0 ist die Verstaerkung 1 |
388 | #define GPS_DIST_P_MAX 200 //maximal 20m |
389 | #define GPS_DIFF_MAX_V 20 //maximale Verstaerkung * 10 |
389 | int dist_east_p,dist_north_p; |
390 | #define GPS_DIFF_MAX_D 40 //Entfernung bei der maximale Verstaerkung erreicht wird in (dm) |
390 | dist_east_p = dist_east; |
391 | signed long dist; |
391 | dist_north_p = dist_north; |
392 | int phi; |
392 | if (dist_east > GPS_DIST_P_MAX) dist_east_p = GPS_DIST_P_MAX; // P-Anteil begrenzen |
393 | phi = arctan_i(abs(dist_north),abs(dist_east)); |
393 | else if (dist_east < - GPS_DIST_P_MAX) dist_east_p = -GPS_DIST_P_MAX; |
394 | if (abs(dist_east) > abs(dist_north) ) //Zunaechst Entfernung zum Ziel ermitteln |
- | 395 | { |
|
394 | if (dist_north > GPS_DIST_P_MAX) dist_north_p = GPS_DIST_P_MAX; // P-Anteil begrenzen |
396 | dist = (long)dist_east; //Groesseren Wert wegen besserer Genauigkeit nehmen |
395 | else if (dist_north < - GPS_DIST_P_MAX) dist_north_p = -GPS_DIST_P_MAX; |
397 | dist = abs((dist *1000) / (long) sin_i(phi)); |
- | 398 | } |
|
- | 399 | else |
|
396 | 400 | { |
|
397 | n = sin_i((dist_east_p*90)/(GPS_DIST_P_MAX)); |
401 | dist = (long)dist_north; |
398 | d = (GPS_DIST_P_MAX * (long)n) /1000; |
402 | dist = abs((dist *1000) / (long) cos_i(phi)); |
399 | dist_east_p = (int) d; |
403 | } |
400 | n = sin_i((dist_north_p*90)/(GPS_DIST_P_MAX)); |
404 | diff_v = (int)((dist * (GPS_DIFF_MAX_V - 10)) / GPS_DIFF_MAX_D) +10; //Verstaerkung * 10 |
401 | d = (GPS_DIST_P_MAX * (long)n) /1000; |
405 | if (diff_v > GPS_DIFF_MAX_V) diff_v = GPS_DIFF_MAX_V; |
402 | dist_north_p = (int) d; |
- | |
Line 403... | Line 406... | ||
403 | 406 | ||
404 | //PID Regler Werte aufsummieren |
407 | //PID Regler Werte aufsummieren |
405 | gps_reg_x = ((int_east * Parameter_UserParam2)/32) + ((dist_east_p * Parameter_UserParam1)/8)+ ((diff_east_f * Parameter_UserParam3)); // I + P +D Anteil X Achse |
408 | gps_reg_x = ((int_east * Parameter_UserParam2)/32) + ((dist_east * Parameter_UserParam1)/8)+ ((diff_east_f * diff_v * Parameter_UserParam3)/10); // I + P +D Anteil X Achse |
Line 406... | Line 409... | ||
406 | gps_reg_y = ((int_north * Parameter_UserParam2)/32) + ((dist_north_p * Parameter_UserParam1)/8)+ ((diff_north_f * Parameter_UserParam3)); // I + P +D Anteil Y Achse |
409 | gps_reg_y = ((int_north * Parameter_UserParam2)/32) + ((dist_north * Parameter_UserParam1)/8)+ ((diff_north_f * diff_v * Parameter_UserParam3)/10); // I + P +D Anteil Y Achse |
407 | 410 | ||
Line 408... | Line 411... | ||
408 | //Ziel-Richtung bezogen auf Nordpol bestimmen |
411 | //Ziel-Richtung bezogen auf Nordpol bestimmen |
Line 420... | Line 423... | ||
420 | else if (GPS_hdng_rel_2trgt <-180) GPS_hdng_rel_2trgt = 360 + GPS_hdng_rel_2trgt; |
423 | else if (GPS_hdng_rel_2trgt <-180) GPS_hdng_rel_2trgt = 360 + GPS_hdng_rel_2trgt; |
Line 421... | Line 424... | ||
421 | 424 | ||
422 | // Regelabweichung aus x,y zu Ziel in Distanz umrechnen |
425 | // Regelabweichung aus x,y zu Ziel in Distanz umrechnen |
423 | if (abs(gps_reg_x) > abs(gps_reg_y) ) |
426 | if (abs(gps_reg_x) > abs(gps_reg_y) ) |
424 | { |
427 | { |
425 | dist = (long)gps_reg_x; //Groesseren Wert wegen besserer Genauigkeit nehmen |
428 | dev = (long)gps_reg_x; //Groesseren Wert wegen besserer Genauigkeit nehmen |
426 | dist = abs((dist *1000) / (long) sin_i(GPS_hdng_abs_2trgt)); |
429 | dev = abs((dev *1000) / (long) sin_i(GPS_hdng_abs_2trgt)); |
427 | } |
430 | } |
428 | else |
431 | else |
429 | { |
432 | { |
430 | dist = (long)gps_reg_y; |
433 | dev = (long)gps_reg_y; |
431 | dist = abs((dist *1000) / (long) cos_i(GPS_hdng_abs_2trgt)); |
434 | dev = abs((dev *1000) / (long) cos_i(GPS_hdng_abs_2trgt)); |
432 | } |
435 | } |
433 | GPS_dist_2trgt = (int) dist; |
436 | GPS_dist_2trgt = (int) dev; |
434 | // Winkel und Distanz in Nick und Rollgroessen umrechnen |
437 | // Winkel und Distanz in Nick und Rollgroessen umrechnen |
435 | GPS_Roll = (int) +( (dist * (long) sin_i(GPS_hdng_rel_2trgt))/1000); |
438 | GPS_Roll = (int) +( (dev * (long) sin_i(GPS_hdng_rel_2trgt))/1000); |
Line 436... | Line 439... | ||
436 | GPS_Nick = (int) -( (dist * (long) cos_i(GPS_hdng_rel_2trgt))/1000); |
439 | GPS_Nick = (int) -( (dev * (long) cos_i(GPS_hdng_rel_2trgt))/1000); |
437 | 440 | ||
438 | #define GPS_V 8 // auf Maximalwert normieren. Teilerfaktor ist 8 |
441 | #define GPS_V 8 // auf Maximalwert normieren. Teilerfaktor ist 8 |
439 | if (GPS_Roll > (GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = (GPS_NICKROLL_MAX * GPS_V); |
442 | if (GPS_Roll > (GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = (GPS_NICKROLL_MAX * GPS_V); |