Subversion Repositories FlightCtrl

Rev

Go to most recent revision | Details | Last modification | View Log | RSS feed

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