Subversion Repositories FlightCtrl

Rev

Rev 32 | Rev 37 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 32 Rev 36
Line 6... Line 6...
6 
#include "main.h"
 6 
#include "main.h"
7 
#include "math.h"
 7 
#include "math.h"
Line 8... Line 8...
8 
 
 8 
 
9 
// GPS feste Variablen++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 9 
// GPS feste Variablen++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
10 
volatile int loop = 0;
 10 
volatile int loop = 0;
Line 11... Line 11...
11 
long gps_northing = 0, gps_easting = 0;
 11 
long gps_northing = 0, gps_easting = 0, gps_altitude = 0;
Line 12... Line 12...
12 
 
 12 
 
13 
volatile int  alpha = 0;
 13 
volatile int  alpha = 0;
14 
 
 - 
 
15 
long zwn = 0, zwe = 0, zwn1 = 0, zwe1 = 0, zwn2 = 0, zwe2 = 0;
 14 
 
16 
volatile int  gps_getpos = 5;
 15 
long zwn = 0, zwe = 0, zwn1 = 0, zwe1 = 0, zwn2 = 0, zwe2 = 0;
Line 17... Line 16...
17 
volatile int  gps_gethome = 0;
 16 
volatile int  gps_getpos = 5;
18 
long gps_home_n = 0;
 17 
long gps_home_n = 0;
19 
long gps_home_e = 0;
 18 
long gps_home_e = 0;
Line 20... Line 19...
20 
 
 19 
 
Line 21... Line 20...
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)
Line 23... Line 22...
23 
volatile int gpsmax = 30;                       //maximal zulässiger "GPS-Steuerausschlag"
 22 
volatile int gpsmax = 35;                       //maximal zulässiger "GPS-Steuerausschlag"
24 
 
 23 
 
- 
 
 24 
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
25 
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 25 
 
26 
 
 26 
signed int GPS_Nick = 0;
27 
signed int GPS_Nick = 0;
 27 
signed int GPS_Roll = 0;
28 
signed int GPS_Roll = 0;
 28 
 
29 
 
 29 
void gps_main(void)
30 
void gps_main(void)
 30 
{
Line 31... Line 31...
31 
{
 31 
/*
Line 32... Line 32...
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 
}*/
Line 37... Line 38...
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
 - 
 
43 
gps_northing = actualPos.northing;
 - 
 
44 
gps_easting = actualPos.easting;
 - 
 
Line 45... Line 42...
45 
beeptime = 50;
 42 
if (gps_getpos != 0){                                                           //Postion mit Schalter loggen
46 
gps_getpos = 0;}
 43 
gps_northing = actualPos.northing;
47 
 
 44 
gps_easting = actualPos.easting;
Line 48... Line 45...
48 
//Regler ##########################################################################################################################
 45 
gps_altitude = actualPos.altitude;
49 
//P-Regler
 46 
beeptime = 50;
Line 50... Line 47...
50 
zwn = (gps_northing-actualPos.northing)*gps_p;
 47 
gps_getpos = 0;}
51 
zwe = (gps_easting-actualPos.easting)*gps_p;
 48 
 
52 
 
 49 
//Regler ##########################################################################################################################
Line -... Line 50...
- 
 
 50 
//P-Regler
53 
//I-Regler
 51 
zwn = ((sqrt(gps_northing^2+gps_altitude^2)-sqrt(actualPos.northing^2+actualPos.altitude^2))*gps_p)/8;
54 
zwn1=0;
 - 
 
55 
zwe1=0;
 52 
zwe = ((gps_easting-actualPos.easting)*gps_p)/8;
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;    
- 
 
 57 
 
Line 60... Line 58...
60 
 
 58 
GPS_Nick = -1*(zwn+zwn2); // skal;
61 
GPS_Nick = (-zwn+zwn1-zwn2) / skal;
 59 
GPS_Roll = (zwe+zwe2); // skal;
- 
 
 60 
 
Line 62... Line 61...
62 
GPS_Roll = (zwe+zwe1-zwe2) / skal;
 61 
//GPS-Mixer########################################################################################################################
63 
 
 62 
if (GPS_Nick>gpsmax){GPS_Nick=gpsmax;} else if (GPS_Nick<(-1*gpsmax)){GPS_Nick=(-1*gpsmax);} //min-max Wert überprüfen
64 
//GPS-Mixer########################################################################################################################
 63 
if (GPS_Roll>gpsmax){GPS_Roll=gpsmax;} else if (GPS_Roll<(-1*gpsmax)){GPS_Roll=(-1*gpsmax);}
65 
if (GPS_Nick>gpsmax){GPS_Nick=gpsmax;} else if (GPS_Nick<(-1*gpsmax)){GPS_Nick=(-1*gpsmax);} //min-max Wert überprüfen
 64