Subversion Repositories FlightCtrl

Rev

Rev 36 | Go to most recent revision | 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;