Subversion Repositories FlightCtrl

Rev

Rev 36 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 36 Rev 37
Line 5... Line 5...
5
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
5
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
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;
-
 
11
//long gps_northing = 0, gps_easting = 0, gps_altitude = 0;
-
 
12
 
Line 11... Line 13...
11
long gps_northing = 0, gps_easting = 0, gps_altitude = 0;
13
long target_x = 0, target_y = 0, target_z = 0;
Line 12... Line 14...
12
 
14
 
13
volatile int  alpha = 0;
15
volatile int  alpha = 0;
14
 
16
 
15
long zwn = 0, zwe = 0, zwn1 = 0, zwe1 = 0, zwn2 = 0, zwe2 = 0;
17
long zwn = 0, zwe = 0, zwn1 = 0, zwe1 = 0, zwn2 = 0, zwe2 = 0;
Line 16... Line 18...
16
volatile int  gps_getpos = 5;
18
volatile int  gps_getpos = 5;
17
long gps_home_n = 0;
19
long gps_home_x = 0;
18
long gps_home_e = 0;
20
long gps_home_y = 0;
Line 28... Line 30...
28
 
30
 
29
void gps_main(void)
31
void gps_main(void)
30
{
32
{
31
/*
33
/*
32
if (MotorenEin = 1 && gps_gethome == 0 && actualPos.state != 0){                //speichert GPS-Home-Position
34
if (MotorenEin = 1 && gps_gethome == 0 && actualPos.state != 0){                //speichert GPS-Home-Position
33
gps_home_n = actualPos.northing;
35
gps_home_x = actualPos.x;
34
gps_home_e = actualPos.easting;
36
gps_home_y = actualPos.y;
35
beeptime = 80;
37
beeptime = 80;
36
gps_gethome  = 1;
38
gps_gethome  = 1;
Line 37... Line 39...
37
}*/
39
}*/
Line 38... Line 40...
38
 
40
 
39
 
41
 
40
if (Poti1>0 && actualPos.state != 0){                           //Beginn GPS-Position-Hold
42
if (Poti1>0 && actualPos.state != 0){                           //Beginn GPS-Position-Hold
41
 
43
 
42
if (gps_getpos != 0){                                                           //Postion mit Schalter loggen
44
if (gps_getpos != 0){                                                           //Postion mit Schalter loggen
43
gps_northing = actualPos.northing;
45
target_x = actualPos.x;
Line 44... Line 46...
44
gps_easting = actualPos.easting;
46
target_y = actualPos.y;
45
gps_altitude = actualPos.altitude;
47
target_z = actualPos.z;
46
beeptime = 50;
48
beeptime = 50;
47
gps_getpos = 0;}
49
gps_getpos = 0;}
Line 48... Line 50...
48
 
50
 
49
//Regler ##########################################################################################################################
51
//Regler ##########################################################################################################################
50
//P-Regler
52
//P-Regler
Line 51... Line 53...
51
zwn = ((sqrt(gps_northing^2+gps_altitude^2)-sqrt(actualPos.northing^2+actualPos.altitude^2))*gps_p)/8;
53
zwn = ((sqrt(target_x^2+target_z^2)-sqrt(actualPos.x^2+actualPos.z^2))*gps_p)/10; //8
52
zwe = ((gps_easting-actualPos.easting)*gps_p)/8;
54
zwe = ((target_y-actualPos.y)*gps_p)/10;
Line 53... Line 55...
53
 
55
 
54
//D-Regler
56
//D-Regler
55
zwn2= (gps_d*actualPos.velNorth)/-2;
57
zwn2= (gps_d*actualPos.vx)/-3; //-2
Line 70... Line 72...
70
if (KompassValue>300) {beeptime=50;}
72
if (KompassValue>300) {beeptime=50;}
71
if (alpha>359) {alpha=alpha-360;}
73
if (alpha>359) {alpha=alpha-360;}
Line 72... Line 74...
72
 
74
 
73
 
75
 
74
GPS_Nick=(sin(alpha)*GPS_Roll+cos(alpha)*GPS_Nick);
76
GPS_Nick=(sin(alpha)*GPS_Roll+cos(alpha)*GPS_Nick);
Line 75... Line 77...
75
GPS_Roll=(cos(alpha)*GPS_Nick-sin(alpha)*GPS_Roll);
77
GPS_Roll=(cos(alpha)*GPS_Roll-sin(alpha)*GPS_Nick);
76
*/
78
*/
77
 
79
 
78
}else {
80
}else {
79
gps_getpos=5;
-
 
80
GPS_Nick=0;
-
 
81
GPS_Roll=0;
81
gps_getpos=5;
82
zwn1=0;
82
GPS_Nick=0;