Rev 194 | Go to most recent revision | 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); |