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; |