Details | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 21 | user | 1 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 2 | // + Copyright (c) 08.2007 by Christopher Hartmann / Daniel Schmitz |
||
| 3 | // + |
||
| 4 | // + Bitte die read_me Datei beachten! Es handelt sich hierbei um eine erste Probeversion! |
||
| 5 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
| 6 | #include "main.h" |
||
| 7 | #include "math.h" |
||
| 8 | |||
| 9 | // GPS feste Variablen++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
| 37 | hallo2 | 10 | //volatile int loop = 0; |
| 11 | //long gps_northing = 0, gps_easting = 0, gps_altitude = 0; |
||
| 21 | user | 12 | |
| 37 | hallo2 | 13 | long target_x = 0, target_y = 0, target_z = 0; |
| 14 | |||
| 21 | user | 15 | volatile int alpha = 0; |
| 16 | |||
| 17 | long zwn = 0, zwe = 0, zwn1 = 0, zwe1 = 0, zwn2 = 0, zwe2 = 0; |
||
| 18 | volatile int gps_getpos = 5; |
||
| 37 | hallo2 | 19 | long gps_home_x = 0; |
| 20 | long gps_home_y = 0; |
||
| 21 | user | 21 | |
| 22 | // GPS Einstellungen +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
| 23 | volatile int komp_dreh = 0; // verdrehten Kompasseinbau kompensieren (+/-Grad) |
||
| 36 | chris2798 | 24 | volatile int gpsmax = 35; //maximal zulässiger "GPS-Steuerausschlag" |
| 21 | user | 25 | |
| 26 | //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
| 27 | |||
| 28 | signed int GPS_Nick = 0; |
||
| 29 | signed int GPS_Roll = 0; |
||
| 30 | |||
| 31 | void gps_main(void) |
||
| 32 | { |
||
| 36 | chris2798 | 33 | /* |
| 34 | if (MotorenEin = 1 && gps_gethome == 0 && actualPos.state != 0){ //speichert GPS-Home-Position |
||
| 37 | hallo2 | 35 | gps_home_x = actualPos.x; |
| 36 | gps_home_y = actualPos.y; |
||
| 21 | user | 37 | beeptime = 80; |
| 38 | gps_gethome = 1; |
||
| 36 | chris2798 | 39 | }*/ |
| 21 | user | 40 | |
| 41 | |||
| 42 | if (Poti1>0 && actualPos.state != 0){ //Beginn GPS-Position-Hold |
||
| 43 | |||
| 44 | if (gps_getpos != 0){ //Postion mit Schalter loggen |
||
| 37 | hallo2 | 45 | target_x = actualPos.x; |
| 46 | target_y = actualPos.y; |
||
| 47 | target_z = actualPos.z; |
||
| 21 | user | 48 | beeptime = 50; |
| 49 | gps_getpos = 0;} |
||
| 50 | |||
| 51 | //Regler ########################################################################################################################## |
||
| 52 | //P-Regler |
||
| 37 | hallo2 | 53 | zwn = ((sqrt(target_x^2+target_z^2)-sqrt(actualPos.x^2+actualPos.z^2))*gps_p)/10; //8 |
| 54 | zwe = ((target_y-actualPos.y)*gps_p)/10; |
||
| 21 | user | 55 | |
| 56 | //D-Regler |
||
| 37 | hallo2 | 57 | zwn2= (gps_d*actualPos.vx)/-3; //-2 |
| 58 | zwe2= (gps_d*actualPos.vy)/-3; |
||
| 21 | user | 59 | |
| 37 | hallo2 | 60 | GPS_Nick = (zwn+zwn2); // skal; |
| 36 | chris2798 | 61 | GPS_Roll = (zwe+zwe2); // skal; |
| 21 | user | 62 | |
| 63 | //GPS-Mixer######################################################################################################################## |
||
| 64 | if (GPS_Nick>gpsmax){GPS_Nick=gpsmax;} else if (GPS_Nick<(-1*gpsmax)){GPS_Nick=(-1*gpsmax);} //min-max Wert überprüfen |
||
| 65 | if (GPS_Roll>gpsmax){GPS_Roll=gpsmax;} else if (GPS_Roll<(-1*gpsmax)){GPS_Roll=(-1*gpsmax);} |
||
| 66 | |||
| 36 | chris2798 | 67 | /* |
| 21 | user | 68 | //Rotationsmatrix################################################################################################################## |
| 69 | //Kompass ++++++++++++++++++++++++++++ |
||
| 70 | alpha=0; |
||
| 36 | chris2798 | 71 | alpha = komp_dreh+KompassValue; |
| 72 | if (KompassValue>300) {beeptime=50;} |
||
| 73 | if (alpha>359) {alpha=alpha-360;} |
||
| 21 | user | 74 | |
| 36 | chris2798 | 75 | |
| 21 | user | 76 | GPS_Nick=(sin(alpha)*GPS_Roll+cos(alpha)*GPS_Nick); |
| 37 | hallo2 | 77 | GPS_Roll=(cos(alpha)*GPS_Roll-sin(alpha)*GPS_Nick); |
| 36 | chris2798 | 78 | */ |
| 21 | user | 79 | |
| 80 | }else { |
||
| 81 | gps_getpos=5; |
||
| 82 | GPS_Nick=0; |
||
| 83 | GPS_Roll=0; |
||
| 84 | } |
||
| 85 | } |
||
| 86 | |||
| 87 | |||
| 88 | |||
| 89 |