| 8,19 → 8,18 |
|---|
| |
| // GPS feste Variablen++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| volatile int loop = 0; |
| long gps_northing = 0, gps_easting = 0; |
| long gps_northing = 0, gps_easting = 0, gps_altitude = 0; |
| |
| volatile int alpha = 0; |
| |
| long zwn = 0, zwe = 0, zwn1 = 0, zwe1 = 0, zwn2 = 0, zwe2 = 0; |
| volatile int gps_getpos = 5; |
| volatile int gps_gethome = 0; |
| long gps_home_n = 0; |
| long gps_home_e = 0; |
| |
| // GPS Einstellungen +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| volatile int komp_dreh = 0; // verdrehten Kompasseinbau kompensieren (+/-Grad) |
| volatile int gpsmax = 30; //maximal zulässiger "GPS-Steuerausschlag" |
| volatile int gpsmax = 35; //maximal zulässiger "GPS-Steuerausschlag" |
| |
| //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| |
| 29,12 → 28,13 |
|---|
| |
| void gps_main(void) |
| { |
| if (MotorenEin > 1 && gps_gethome == 0 && actualPos.state != 0){ //speichert GPS-Home-Position |
| /* |
| if (MotorenEin = 1 && gps_gethome == 0 && actualPos.state != 0){ //speichert GPS-Home-Position |
| gps_home_n = actualPos.northing; |
| gps_home_e = actualPos.easting; |
| beeptime = 80; |
| gps_gethome = 1; |
| } |
| }*/ |
| |
| |
| if (Poti1>0 && actualPos.state != 0){ //Beginn GPS-Position-Hold |
| 42,39 → 42,38 |
|---|
| if (gps_getpos != 0){ //Postion mit Schalter loggen |
| gps_northing = actualPos.northing; |
| gps_easting = actualPos.easting; |
| gps_altitude = actualPos.altitude; |
| beeptime = 50; |
| gps_getpos = 0;} |
| |
| //Regler ########################################################################################################################## |
| //P-Regler |
| zwn = (gps_northing-actualPos.northing)*gps_p; |
| zwe = (gps_easting-actualPos.easting)*gps_p; |
| zwn = ((sqrt(gps_northing^2+gps_altitude^2)-sqrt(actualPos.northing^2+actualPos.altitude^2))*gps_p)/8; |
| zwe = ((gps_easting-actualPos.easting)*gps_p)/8; |
| |
| //I-Regler |
| zwn1=0; |
| zwe1=0; |
| |
| //D-Regler |
| zwn2= gps_d*actualPos.velNorth; |
| zwe2= gps_d*actualPos.velEast; |
| zwn2= (gps_d*actualPos.velNorth)/-2; |
| zwe2= (gps_d*actualPos.velEast)/-2; |
| |
| GPS_Nick = (-zwn+zwn1-zwn2) / skal; |
| GPS_Roll = (zwe+zwe1-zwe2) / skal; |
| GPS_Nick = -1*(zwn+zwn2); // skal; |
| GPS_Roll = (zwe+zwe2); // skal; |
| |
| //GPS-Mixer######################################################################################################################## |
| if (GPS_Nick>gpsmax){GPS_Nick=gpsmax;} else if (GPS_Nick<(-1*gpsmax)){GPS_Nick=(-1*gpsmax);} //min-max Wert überprüfen |
| if (GPS_Roll>gpsmax){GPS_Roll=gpsmax;} else if (GPS_Roll<(-1*gpsmax)){GPS_Roll=(-1*gpsmax);} |
| |
| /* |
| //Rotationsmatrix################################################################################################################## |
| |
| //Kompass ++++++++++++++++++++++++++++ |
| alpha=0; |
| //alpha = komp_dreh+KompassValue; |
| //if (KompassValue>300) {beeptime=50;} |
| //if (alpha>359) {alpha=alpha-360;} |
| alpha = komp_dreh+KompassValue; |
| if (KompassValue>300) {beeptime=50;} |
| if (alpha>359) {alpha=alpha-360;} |
| |
| |
| GPS_Nick=(sin(alpha)*GPS_Roll+cos(alpha)*GPS_Nick); |
| GPS_Roll=(cos(alpha)*GPS_Nick-sin(alpha)*GPS_Roll); |
| */ |
| |
| }else { |
| gps_getpos=5; |