Subversion Repositories FlightCtrl

Rev

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++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
37 hallo2 10
//volatile int loop = 0;
11
//long gps_northing = 0, gps_easting = 0, gps_altitude = 0;
21 user 12
 
37 hallo2 13
long target_x = 0, target_y = 0, target_z = 0;
14
 
21 user 15
volatile int  alpha = 0;
16
 
17
long zwn = 0, zwe = 0, zwn1 = 0, zwe1 = 0, zwn2 = 0, zwe2 = 0;
18
volatile int  gps_getpos = 5;
37 hallo2 19
long gps_home_x = 0;
20
long gps_home_y = 0;
21 user 21
 
22
// GPS Einstellungen +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
23
volatile int komp_dreh = 0;             // verdrehten Kompasseinbau kompensieren (+/-Grad)
36 chris2798 24
volatile int gpsmax = 35;                       //maximal zulässiger "GPS-Steuerausschlag"
21 user 25
 
26
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
27
 
28
signed int GPS_Nick = 0;
29
signed int GPS_Roll = 0;
30
 
31
void gps_main(void)
32
{
36 chris2798 33
/*
34
if (MotorenEin = 1 && gps_gethome == 0 && actualPos.state != 0){                //speichert GPS-Home-Position
37 hallo2 35
gps_home_x = actualPos.x;
36
gps_home_y = actualPos.y;
21 user 37
beeptime = 80;
38
gps_gethome  = 1;
36 chris2798 39
}*/
21 user 40
 
41
 
42
if (Poti1>0 && actualPos.state != 0){                           //Beginn GPS-Position-Hold
43
 
44
if (gps_getpos != 0){                                                           //Postion mit Schalter loggen
37 hallo2 45
target_x = actualPos.x;
46
target_y = actualPos.y;
47
target_z = actualPos.z;
21 user 48
beeptime = 50;
49
gps_getpos = 0;}
50
 
51
//Regler ##########################################################################################################################
52
//P-Regler
37 hallo2 53
zwn = ((sqrt(target_x^2+target_z^2)-sqrt(actualPos.x^2+actualPos.z^2))*gps_p)/10; //8
54
zwe = ((target_y-actualPos.y)*gps_p)/10;
21 user 55
 
56
//D-Regler
37 hallo2 57
zwn2= (gps_d*actualPos.vx)/-3; //-2
58
zwe2= (gps_d*actualPos.vy)/-3; 
21 user 59
 
37 hallo2 60
GPS_Nick = (zwn+zwn2); // skal;
36 chris2798 61
GPS_Roll = (zwe+zwe2); // skal; 
21 user 62
 
63
//GPS-Mixer########################################################################################################################
64
if (GPS_Nick>gpsmax){GPS_Nick=gpsmax;} else if (GPS_Nick<(-1*gpsmax)){GPS_Nick=(-1*gpsmax);} //min-max Wert überprüfen
65
if (GPS_Roll>gpsmax){GPS_Roll=gpsmax;} else if (GPS_Roll<(-1*gpsmax)){GPS_Roll=(-1*gpsmax);}
66
 
36 chris2798 67
/*
21 user 68
//Rotationsmatrix##################################################################################################################
69
//Kompass ++++++++++++++++++++++++++++
70
alpha=0;
36 chris2798 71
alpha = komp_dreh+KompassValue;                
72
if (KompassValue>300) {beeptime=50;}
73
if (alpha>359) {alpha=alpha-360;}
21 user 74
 
36 chris2798 75
 
21 user 76
GPS_Nick=(sin(alpha)*GPS_Roll+cos(alpha)*GPS_Nick);
37 hallo2 77
GPS_Roll=(cos(alpha)*GPS_Roll-sin(alpha)*GPS_Nick);
36 chris2798 78
*/
21 user 79
 
80
}else {
81
gps_getpos=5;
82
GPS_Nick=0;
83
GPS_Roll=0;
84
}
85
}
86
 
87
 
88
 
89