Subversion Repositories FlightCtrl

Rev

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

Rev 130 Rev 243
Line 1... Line 1...
1 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2 
// + Copyright (c) 08.2007 by Christopher Hartmann / Daniel Schmitz
 2 
// + Copyright (c) 10.2007 by Christopher Hartmann / Daniel Schmitz
3 
// +
 3 
// +
4 
// + Bitte die read_me Datei beachten! Es handelt sich hierbei um eine erste Probeversion!
 4 
// + Bitte die read_me Datei beachten! Es handelt sich hierbei um eine BETAVersion!
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++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 - 
 
10 
//volatile int loop = 0;
 - 
 
11 
//long gps_northing = 0, gps_easting = 0, gps_altitude = 0;
 - 
 
12 
 
 - 
 
13 
long target_x = 0, target_y = 0, target_z = 0;
 - 
 
14 
 
 9 
// GPS feste Variablen++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
Line 15... Line 10...
15 
volatile int  alpha = 0;
 10 
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;
 - 
 
19 
long gps_home_x = 0;
 - 
 
20 
long gps_home_y = 0;
 - 
 
21 
 
 - 
 
22 
// GPS Einstellungen +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 - 
 
Line -... Line 11...
- 
 
 11 
 
23 
volatile int komp_dreh = 0;             // verdrehten Kompasseinbau kompensieren (+/-Grad)
 12 
long zwn = 0, zwe = 0, zwn1 = 0, zwe1 = 0, zwn2 = 0, zwe2 = 0, entf_x = 0, entf_y = 0;
- 
 
 13 
 
- 
 
 14 
int x=0;
- 
 
 15 
int hoehedurchlauf=0;
Line 24... Line 16...
24 
volatile int gpsmax = 35;                       //maximal zulässiger "GPS-Steuerausschlag"
 16 
 
25 
 
 17 
float gps_sin=0;
- 
 
 18 
float gps_cos=0;
- 
 
 19 
 
- 
 
 20 
signed int GPS_Nick = 0;
- 
 
 21 
signed int GPS_Roll = 0;
Line 26... Line 22...
26 
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 22 
 
27 
 
 23 
long alpha2 = 900;
28 
signed int GPS_Nick = 0;
 - 
 
29 
signed int GPS_Roll = 0;
 24 
long GPS_Nick2 = 0;
- 
 
 25 
long GPS_Roll2 = 0;
30 
 
 26 
 
31 
void gps_main(void)
 27 
void gps_main(void)
32 
{
 28 
{
33 
/*
 29 
if (StickNick>-15 && StickNick<15 && StickRoll>-15 && StickRoll<15){
34 
if (MotorenEin = 1 && gps_gethome == 0 && actualPos.state != 0){                //speichert GPS-Home-Position
 30 
stick_time++;
- 
 
 31 
 if (stick_time==2){
- 
 
 32 
 sticks_centered=1;}
35 
gps_home_x = actualPos.x;
 33 
 else if (stick_time>35){
36 
gps_home_y = actualPos.y;
 34 
 stick_time=0;
37 
beeptime = 80;
 35 
 }}
- 
 
 36 
 else
- 
 
 37 
{stick_time=0; sticks_centered=0;}
38 
gps_gethome  = 1;
 38 
 
39 
}*/
 39 
 
40 
 
 40 
if (actualPos.state != 0 && gps_new==1 && Poti1>0){                             //Beginn GPS-Position-Hold
41 
 
 41 
 
42 
if (Poti1>0 && actualPos.state != 0){                           //Beginn GPS-Position-Hold
 - 
 
43 
 
 42 
 
44 
if (gps_getpos != 0){                                                           //Postion mit Schalter loggen
 43 
 
Line -... Line 44...
- 
 
 44 
if (gps_getpos != 0 && (Poti2 == 0||gps_gethome==5) && sticks_centered==1){                                                             //PH aktivieren
- 
 
 45 
target_x = actualPos.northing;
- 
 
 46 
target_y = actualPos.easting;
- 
 
 47 
if (gps_gethome!=5){beeptime = 50;}
- 
 
 48 
gps_getpos = 0;}
- 
 
 49 
 
- 
 
 50 
 
- 
 
 51 
if (Poti2>0 && gps_gethome==1){                   // Home anfliegen
- 
 
 52 
 
- 
 
 53 
target_x = gps_home_x;
- 
 
 54 
target_y = gps_home_y;
- 
 
 55 
 
- 
 
 56 
if ((actualPos.northing < target_x+200 && actualPos.northing > target_x-200) && (actualPos.easting < target_y+200 && actualPos.easting > target_y-200) && homing_state !=1)
- 
 
 57 
{beeptime = 100; homing_state=1; gps_getpos = 0;}else {beeptime = 50;}
- 
 
 58 
 
- 
 
 59 
//Höhenreglung für Home-Anflug
- 
 
 60 
if (SollHoehe > home_height && hoehedurchlauf==2){SollHoehe=SollHoehe-1;
- 
 
 61 
hoehedurchlauf=0;}
45 
target_x = actualPos.x;
 62 
if (SollHoehe < home_height && hoehedurchlauf==2){SollHoehe=SollHoehe+1;
46 
target_y = actualPos.y;
 63 
hoehedurchlauf=0;}
47 
target_z = actualPos.z;
 - 
 
48 
beeptime = 50;
 - 
 
Line 49... Line -...
49 
gps_getpos = 0;}
 - 
 
50 
 
 64 
else if ((SollHoehe > home_height || SollHoehe < home_height) && hoehedurchlauf<2) {hoehedurchlauf++;}}
51 
//Regler ##########################################################################################################################
 65 
 
Line -... Line 66...
- 
 
 66 
 
52 
//P-Regler
 67 
if (sticks_centered==1){
53 
zwn = ((sqrt(target_x^2+target_z^2)-sqrt(actualPos.x^2+actualPos.z^2))*gps_p)/10; //8
 68 
//Regler ##########################################################################################################################
Line 54... Line 69...
54 
zwe = ((target_y-actualPos.y)*gps_p)/10;
 69 
//P-Regler
55 
 
 - 
 
56 
//D-Regler
 - 
 
Line 57... Line -...
57 
zwn2= (gps_d*actualPos.vx)/-3; //-2
 - 
 
58 
zwe2= (gps_d*actualPos.vy)/-3; 
 - 
 
59 
 
 70 
 
60 
GPS_Nick = (zwn+zwn2); // skal;
 - 
 
61 
GPS_Roll = (zwe+zwe2); // skal;
71 
zwn = (target_x-actualPos.northing)*(gps_p);
62 
 
 - 
 
63 
//GPS-Mixer########################################################################################################################
 72 
zwe = (target_y-actualPos.easting)*(gps_p);
Line -... Line 73...
- 
 
 73 
 
- 
 
 74 
//D-Regler
- 
 
 75 
zwn2 = (actualPos.velNorth)*(-gps_d);
- 
 
 76 
zwe2 = (actualPos.velEast)*(-gps_d);
- 
 
 77 
 
Line 64... Line 78...
64 
if (GPS_Nick>gpsmax){GPS_Nick=gpsmax;} else if (GPS_Nick<(-1*gpsmax)){GPS_Nick=(-1*gpsmax);} //min-max Wert überprüfen
 78 
//Rotationsmatrix + Kompass########################################################################################################
65 
if (GPS_Roll>gpsmax){GPS_Roll=gpsmax;} else if (GPS_Roll<(-1*gpsmax)){GPS_Roll=(-1*gpsmax);}
 79 
 
66 
 
 - 
 
Line -... Line 80...
- 
 
 80 
if (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV){
- 
 
 81 
alpha = komp_dreh+KompassStartwert;                    
- 
 
 82 
if (alpha>359) {alpha=alpha-360;}
67 
/*
 83 
 
68 
//Rotationsmatrix##################################################################################################################
 - 
 
69 
//Kompass ++++++++++++++++++++++++++++
 84 
if (alpha != alpha2){
70 
alpha=0;
 85 
gps_sin =floor(sin(alpha*M_PI/180)*100)/100;
71 
alpha = komp_dreh+KompassValue;                
 - 
 
72 
if (KompassValue>300) {beeptime=50;}
 86 
gps_cos =floor(cos(alpha*M_PI/180)*100)/100;
Line -... Line 87...
- 
 
 87 
alpha2 = alpha;
- 
 
 88 
}
- 
 
 89 
 
- 
 
 90 
GPS_Nick2 = (zwn+zwn2)/skal;
- 
 
 91 
GPS_Roll2 = (zwe+zwe2)/skal;
Line -... Line 92...
- 
 
 92 
 
- 
 
 93 
GPS_Nick= (gps_sin*GPS_Roll2)+(gps_cos*GPS_Nick2);
- 
 
 94 
GPS_Roll= (gps_cos*GPS_Roll2)-(gps_sin*GPS_Nick2);
- 
 
 95 
 
- 
 
 96 
}else{
- 
 
 97 
GPS_Nick = (zwn+zwn2)/skal;