Subversion Repositories FlightCtrl

Rev

Rev 32 | Rev 37 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

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