| 0,0 → 1,90 |
|---|
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Copyright (c) 08.2007 by Christopher Hartmann / Daniel Schmitz |
| // + |
| // + Bitte die read_me Datei beachten! Es handelt sich hierbei um eine erste Probeversion! |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| #include "main.h" |
| #include "math.h" |
| |
| // GPS feste Variablen++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| volatile int loop = 0; |
| long gps_northing = 0, gps_easting = 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" |
| |
| //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| |
| signed int GPS_Nick = 0; |
| signed int GPS_Roll = 0; |
| |
| void gps_main(void) |
| { |
| 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 |
| |
| if (gps_getpos != 0){ //Postion mit Schalter loggen |
| gps_northing = actualPos.northing; |
| gps_easting = actualPos.easting; |
| beeptime = 50; |
| gps_getpos = 0;} |
| |
| //Regler ########################################################################################################################## |
| //P-Regler |
| zwn = (gps_northing-actualPos.northing)*gps_p; |
| zwe = (gps_easting-actualPos.easting)*gps_p; |
| |
| //I-Regler |
| zwn1=0; |
| zwe1=0; |
| |
| //D-Regler |
| zwn2= gps_d*actualPos.velNorth; |
| zwe2= gps_d*actualPos.velEast; |
| |
| GPS_Nick = -1*(zwn+zwn1-zwn2) / skal; |
| GPS_Roll = (zwe+zwe1-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;} |
| |
| GPS_Nick=(sin(alpha)*GPS_Roll+cos(alpha)*GPS_Nick); |
| GPS_Roll=(cos(alpha)*GPS_Nick-sin(alpha)*GPS_Roll); |
| |
| }else { |
| gps_getpos=5; |
| GPS_Nick=0; |
| GPS_Roll=0; |
| zwn1=0; |
| zwe1=0; |
| } |
| } |
| |
| |
| |
| |