Rev 167 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 167 | Rev 182 | ||
|---|---|---|---|
| Line 10... | Line 10... | ||
| 10 | Please note: All the other files for the project "Mikrokopter" by H.Buss are under the license (license_buss.txt) published by www.mikrokopter.de |
10 | Please note: All the other files for the project "Mikrokopter" by H.Buss are under the license (license_buss.txt) published by www.mikrokopter.de |
| 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 |
15 | Hold Modus mit PID Regler |
| 16 | Stand 21.9.2007 |
16 | Stand 27.9.2007 |
| 17 | ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
17 | ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 18 | */ |
18 | */ |
| 19 | #include "main.h" |
19 | #include "main.h" |
| 20 | //#include "gps.h" |
20 | //#include "gps.h" |
| Line 378... | Line 378... | ||
| 378 | if (dist_east <-DIST_MAX) dist_east = -DIST_MAX; |
378 | if (dist_east <-DIST_MAX) dist_east = -DIST_MAX; |
| 379 | if (dist_north > DIST_MAX) dist_north = DIST_MAX; |
379 | if (dist_north > DIST_MAX) dist_north = DIST_MAX; |
| 380 | if (dist_north <-DIST_MAX) dist_north = -DIST_MAX; |
380 | if (dist_north <-DIST_MAX) dist_north = -DIST_MAX; |
| Line 381... | Line 381... | ||
| 381 | 381 | ||
| 382 | //PID Regler |
382 | //PID Regler |
| 383 | gps_reg_x = ((int_east * Parameter_UserParam1)/32) + ((dist_east * Parameter_UserParam2)/2)+ ((diff_east * Parameter_UserParam3)/2); // |
383 | gps_reg_x = ((int_east * Parameter_UserParam1)/32) + ((dist_east * Parameter_UserParam2)/8)+ ((diff_east * Parameter_UserParam3)/2); // |
| Line 384... | Line 384... | ||
| 384 | gps_reg_y = ((int_north * Parameter_UserParam1)/32) + ((dist_north * Parameter_UserParam2)/2)+ ((diff_north * Parameter_UserParam3)/2); // I + P +D Anteil Y Achse |
384 | gps_reg_y = ((int_north * Parameter_UserParam1)/32) + ((dist_north * Parameter_UserParam2)/8)+ ((diff_north * Parameter_UserParam3)/2); // I + P +D Anteil Y Achse |
| 385 | 385 | ||
| Line 386... | Line 386... | ||
| 386 | //Richtung bezogen auf Nordpol bestimmen |
386 | //Richtung bezogen auf Nordpol bestimmen |
| Line 399... | Line 399... | ||
| 399 | 399 | ||
| Line 400... | Line 400... | ||
| 400 | // Regelabweichung aus x,y zu Ziel in Distanz umrechnen |
400 | // Regelabweichung aus x,y zu Ziel in Distanz umrechnen |
| 401 | 401 | ||
| 402 | if (abs(gps_reg_x) > abs(gps_reg_y) ) |
402 | if (abs(gps_reg_x) > abs(gps_reg_y) ) |
| 403 | { |
403 | { |
| 404 | dist = (long)gps_reg_x; //Groesseren Wert wegen besserer genauigkeit nehmen |
404 | dist = (long)gps_reg_x; //Groesseren Wert wegen besserer Genauigkeit nehmen |
| 405 | dist = abs((dist *1000) / (long) cos_i(GPS_hdng_abs_2trgt)); |
405 | dist = abs((dist *1000) / (long) cos_i(GPS_hdng_abs_2trgt)); |
| 406 | } |
406 | } |
| 407 | else |
407 | else |
| 408 | { |
408 | { |
| 409 | dist = (long)gps_reg_y; |
409 | dist = (long)gps_reg_y; |
| 410 | dist = abs((dist *1000) / (long) sin_i(GPS_hdng_abs_2trgt)); |
410 | dist = abs((dist *1000) / (long) sin_i(GPS_hdng_abs_2trgt)); |
| 411 | } |
411 | } |
| Line 412... | Line 412... | ||
| 412 | if (dist > 200) dist = 200; |
412 | if (dist > 200) dist = 200; |
| 413 | GPS_dist_2trgt = (int )dist; |
413 | GPS_dist_2trgt = (int )dist; |
| 414 | 414 | ||
| 415 | // Winkel und Distanz in Nick und Roll groessen umrechnen |
415 | // Winkel und Distanz in Nick und Roll groessen umrechnen |
| Line 416... | Line 416... | ||
| 416 | long int a,b; |
416 | long int a,b; |
| 417 | GPS_Roll = (int) +((dist * sin_i(GPS_hdng_rel_2trgt))/(1000*4)); |
417 | GPS_Roll = (int) +((dist * sin_i(GPS_hdng_rel_2trgt))/(1000*8)); |
| 418 | GPS_Nick = (int) -((dist * cos_i(GPS_hdng_rel_2trgt))/(1000*4)); |
418 | GPS_Nick = (int) -((dist * cos_i(GPS_hdng_rel_2trgt))/(1000*8)); |
| 419 | 419 | ||
| 420 | if (GPS_Roll > GPS_NICKROLL_MAX) GPS_Roll = GPS_NICKROLL_MAX; //Auf Maxwerte begrenzen |
- | |
| 421 | else if (GPS_Roll < -GPS_NICKROLL_MAX) GPS_Roll = - GPS_NICKROLL_MAX; |
420 | if (GPS_Roll > GPS_NICKROLL_MAX) GPS_Roll = GPS_NICKROLL_MAX; //Auf Maxwerte begrenzen |
| 422 | if (GPS_Nick > GPS_NICKROLL_MAX) GPS_Nick = GPS_NICKROLL_MAX; |
421 | else if (GPS_Roll < -GPS_NICKROLL_MAX) GPS_Roll = - GPS_NICKROLL_MAX; |
| 423 | else if (GPS_Nick < - GPS_NICKROLL_MAX) GPS_Nick = - GPS_NICKROLL_MAX; |
422 | if (GPS_Nick > GPS_NICKROLL_MAX) GPS_Nick = GPS_NICKROLL_MAX; |
| 424 | 423 | else if (GPS_Nick < - GPS_NICKROLL_MAX) GPS_Nick = - GPS_NICKROLL_MAX; |
|