Rev 130 | Details | Compare with Previous | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 21 | user | 1 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 243 | hallo2 | 2 | // + Copyright (c) 10.2007 by Christopher Hartmann / Daniel Schmitz |
| 21 | user | 3 | // + |
| 243 | hallo2 | 4 | // + Bitte die read_me Datei beachten! Es handelt sich hierbei um eine BETAVersion! |
| 21 | user | 5 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 6 | #include "main.h" |
||
| 7 | #include "math.h" |
||
| 8 | |||
| 9 | // GPS feste Variablen++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
| 10 | volatile int alpha = 0; |
||
| 11 | |||
| 243 | hallo2 | 12 | long zwn = 0, zwe = 0, zwn1 = 0, zwe1 = 0, zwn2 = 0, zwe2 = 0, entf_x = 0, entf_y = 0; |
| 21 | user | 13 | |
| 243 | hallo2 | 14 | int x=0; |
| 15 | int hoehedurchlauf=0; |
||
| 21 | user | 16 | |
| 243 | hallo2 | 17 | float gps_sin=0; |
| 18 | float gps_cos=0; |
||
| 21 | user | 19 | |
| 20 | signed int GPS_Nick = 0; |
||
| 21 | signed int GPS_Roll = 0; |
||
| 243 | hallo2 | 22 | |
| 23 | long alpha2 = 900; |
||
| 24 | long GPS_Nick2 = 0; |
||
| 25 | long GPS_Roll2 = 0; |
||
| 21 | user | 26 | |
| 27 | void gps_main(void) |
||
| 28 | { |
||
| 243 | hallo2 | 29 | if (StickNick>-15 && StickNick<15 && StickRoll>-15 && StickRoll<15){ |
| 30 | stick_time++; |
||
| 31 | if (stick_time==2){ |
||
| 32 | sticks_centered=1;} |
||
| 33 | else if (stick_time>35){ |
||
| 34 | stick_time=0; |
||
| 35 | }} |
||
| 36 | else |
||
| 37 | {stick_time=0; sticks_centered=0;} |
||
| 21 | user | 38 | |
| 39 | |||
| 243 | hallo2 | 40 | if (actualPos.state != 0 && gps_new==1 && Poti1>0){ //Beginn GPS-Position-Hold |
| 21 | user | 41 | |
| 243 | hallo2 | 42 | |
| 43 | |||
| 44 | if (gps_getpos != 0 && (Poti2 == 0||gps_gethome==5) && sticks_centered==1){ //PH aktivieren |
||
| 45 | target_x = actualPos.northing; |
||
| 46 | target_y = actualPos.easting; |
||
| 47 | if (gps_gethome!=5){beeptime = 50;} |
||
| 21 | user | 48 | gps_getpos = 0;} |
| 49 | |||
| 243 | hallo2 | 50 | |
| 51 | if (Poti2>0 && gps_gethome==1){ // Home anfliegen |
||
| 52 | |||
| 53 | target_x = gps_home_x; |
||
| 54 | target_y = gps_home_y; |
||
| 55 | |||
| 56 | if ((actualPos.northing < target_x+200 && actualPos.northing > target_x-200) && (actualPos.easting < target_y+200 && actualPos.easting > target_y-200) && homing_state !=1) |
||
| 57 | {beeptime = 100; homing_state=1; gps_getpos = 0;}else {beeptime = 50;} |
||
| 58 | |||
| 59 | //Höhenreglung für Home-Anflug |
||
| 60 | if (SollHoehe > home_height && hoehedurchlauf==2){SollHoehe=SollHoehe-1; |
||
| 61 | hoehedurchlauf=0;} |
||
| 62 | if (SollHoehe < home_height && hoehedurchlauf==2){SollHoehe=SollHoehe+1; |
||
| 63 | hoehedurchlauf=0;} |
||
| 64 | else if ((SollHoehe > home_height || SollHoehe < home_height) && hoehedurchlauf<2) {hoehedurchlauf++;}} |
||
| 65 | |||
| 66 | |||
| 67 | if (sticks_centered==1){ |
||
| 21 | user | 68 | //Regler ########################################################################################################################## |
| 69 | //P-Regler |
||
| 70 | |||
| 243 | hallo2 | 71 | zwn = (target_x-actualPos.northing)*(gps_p); |
| 72 | zwe = (target_y-actualPos.easting)*(gps_p); |
||
| 73 | |||
| 21 | user | 74 | //D-Regler |
| 243 | hallo2 | 75 | zwn2 = (actualPos.velNorth)*(-gps_d); |
| 76 | zwe2 = (actualPos.velEast)*(-gps_d); |
||
| 21 | user | 77 | |
| 243 | hallo2 | 78 | //Rotationsmatrix + Kompass######################################################################################################## |
| 21 | user | 79 | |
| 243 | hallo2 | 80 | if (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV){ |
| 81 | alpha = komp_dreh+KompassStartwert; |
||
| 36 | chris2798 | 82 | if (alpha>359) {alpha=alpha-360;} |
| 21 | user | 83 | |
| 243 | hallo2 | 84 | if (alpha != alpha2){ |
| 85 | gps_sin =floor(sin(alpha*M_PI/180)*100)/100; |
||
| 86 | gps_cos =floor(cos(alpha*M_PI/180)*100)/100; |
||
| 87 | alpha2 = alpha; |
||
| 88 | } |
||
| 36 | chris2798 | 89 | |
| 243 | hallo2 | 90 | GPS_Nick2 = (zwn+zwn2)/skal; |
| 91 | GPS_Roll2 = (zwe+zwe2)/skal; |
||
| 21 | user | 92 | |
| 243 | hallo2 | 93 | GPS_Nick= (gps_sin*GPS_Roll2)+(gps_cos*GPS_Nick2); |
| 94 | GPS_Roll= (gps_cos*GPS_Roll2)-(gps_sin*GPS_Nick2); |
||
| 95 | |||
| 96 | }else{ |
||
| 97 | GPS_Nick = (zwn+zwn2)/skal; |
||
| 98 | GPS_Roll = (zwe+zwe2)/skal; |
||
| 99 | } |
||
| 100 | |||
| 101 | GPS_Nick=GPS_Nick*-1; |
||
| 102 | |||
| 103 | //GPS-Min-Max Prüfung############################################################################################################### |
||
| 104 | if (GPS_Nick>gpsmax){GPS_Nick=gpsmax;} else if (GPS_Nick<(-1*gpsmax)){GPS_Nick=(-1*gpsmax);} |
||
| 105 | if (GPS_Roll>gpsmax){GPS_Roll=gpsmax;} else if (GPS_Roll<(-1*gpsmax)){GPS_Roll=(-1*gpsmax);} |
||
| 106 | |||
| 107 | gps_new=0; |
||
| 108 | }else{ |
||
| 21 | user | 109 | gps_getpos=5; |
| 110 | GPS_Nick=0; |
||
| 243 | hallo2 | 111 | GPS_Roll=0;}} |
| 21 | user | 112 | } |
| 113 | |||
| 114 |