Subversion Repositories FlightCtrl

Rev

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

Rev 701 Rev 726
Line 1... Line -...
1
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
 
2
// + Copyright (c) 04.2007 Holger Buss
-
 
3
// + only for non-profit use
-
 
4
// + www.MikroKopter.com
-
 
5
// + see the File "License.txt" for further Informations
-
 
6
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
 
7
#include "main.h"
1
#include <inttypes.h>
8
 
-
 
9
signed int GPS_Pitch = 0;
-
 
10
signed int GPS_Roll = 0;
-
 
11
long GpsReading_X = 0;
-
 
12
long GpsReading_Y = 0;
-
 
13
long GpsTarget_X = 0;
-
 
14
long GpsTarget_Y = 0;
-
 
Line -... Line 2...
-
 
2
 
-
 
3
#include "fc.h"
15
 
4
#include "ubx.h"
16
void GPS_Neutral(void)
5
#include "mymath.h"
17
{
6
 
18
 GpsTarget_X = GpsReading_X;
7
int16_t GPS_Pitch = 0;
19
 GpsTarget_Y = GpsReading_Y;
-
 
Line -... Line 8...
-
 
8
int16_t GPS_Roll = 0;
20
}
9
 
-
 
10
uint8_t GPS_P_Factor = 0;
-
 
11
uint8_t GPS_D_Factor = 0;
21
 
12
 
-
 
13
typedef struct
22
void GPS_CalcTargetDirection(void)
14
{
23
{
15
        int32_t Northing;
24
 GPS_Pitch = 0;
16
        int32_t Easting;
-
 
17
        uint8_t Status;
Line -... Line 18...
-
 
18
 
-
 
19
} GPS_Pos_t;
-
 
20
 
-
 
21
// GPS coordinates for hold position
Line -... Line 22...
-
 
22
GPS_Pos_t GPSHoldPoint  = {0,0, INVALID};
-
 
23
// GPS coordinates for flying home
-
 
24
GPS_Pos_t GPSHomePoint = {0,0, INVALID};;
-
 
25
 
-
 
26
 
-
 
27
 
-
 
28
// ---------------------------------------------------------------------------------
-
 
29
 
-
 
30
 
-
 
31
void GPS_Main(void)
-
 
32
{
-
 
33
        int32_t coscompass, sincompass;
-
 
34
        int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0;
-
 
35
        int32_t PD_North = 0, PD_East = 0;
-
 
36
        int32_t GPSPosDev_North = 0, GPSPosDev_East = 0;
-
 
37
 
-
 
38
        // poti2 enables the gps feature
-
 
39
        if(Poti2 > 70) // run GPS function only if Poti 2 is larger than 70 (switch on)
-
 
40
        {
-
 
41
                switch(GPSInfo.status)
-
 
42
                {
-
 
43
                case INVALID:  // invalid gps data
-
 
44
                        GPS_Pitch = 0;
-
 
45
                        GPS_Roll = 0;
-
 
46
                        break;
-
 
47
                case PROCESSED: // if gps data are already processed
-
 
48
                        // downcount timeout
-
 
49
                        if(GPSTimeout) GPSTimeout--;
-
 
50
                        // if no new data arrived within timeout set current data invalid
-
 
51
                        // and therefore disable GPS
-
 
52
                        else
-
 
53
                        {
-
 
54
                                GPS_Pitch = 0;
-
 
55
                                GPS_Roll = 0;
-
 
56
                                GPSInfo.status = INVALID;
-
 
57
                        }
-
 
58
                        break;
-
 
59
                case VALID: // new valid data from gps device
-
 
60
                        // if the gps data quality is sufficient
-
 
61
                        if (GPSInfo.satfix == SATFIX_3D)
-
 
62
                        {
-
 
63
                                // if sticks are centered  and hold position is valid enable position hold
-
 
64
                                if ((MaxStickPitch < 20) && (MaxStickRoll < 20) && (GPSHoldPoint.Status == VALID))
-
 
65
                                {
-
 
66
                                        coscompass = (int32_t)c_cos_8192(CompassHeading);
-
 
67
                                        sincompass = (int32_t)c_sin_8192(CompassHeading);
-
 
68
 
-
 
69
                                        // Calculate deviation from hold position
-
 
70
                                        GPSPosDev_North = GPSInfo.utmnorth - GPSHoldPoint.Northing;
-
 
71
                                        GPSPosDev_East  = GPSInfo.utmeast  - GPSHoldPoint.Easting;
-
 
72
 
-
 
73
                                        //Calculate PD-components of the controller
-
 
74
                                        P_North = (GPS_P_Factor * GPSPosDev_North)/2048;
-
 
75
                                        D_North = (GPS_D_Factor * GPSInfo.velnorth)/255;
-
 
76
                                        P_East =  (GPS_P_Factor * GPSPosDev_East)/2048;
-
 
77
                                        D_East =  (GPS_D_Factor * GPSInfo.veleast)/255;
-
 
78
 
-
 
79
                                        // PD-controller
-
 
80
                                        PD_North = (-P_North + D_North);
-
 
81
                                        PD_East = (P_East - D_East);
-
 
82
 
-
 
83
                                        // GPS to pitch and roll
-
 
84
                                        GPS_Pitch = (int16_t)((coscompass * PD_North - sincompass * PD_East) / 8192);
-
 
85
                                        GPS_Roll  = (int16_t)((sincompass * PD_North + coscompass * PD_East) / 8192);
-
 
86
 
-
 
87
                                        // limit GPS controls
-
 
88
                                        if (GPS_Pitch >  35) GPS_Pitch =  35;
-
 
89
                                        if (GPS_Pitch < -35) GPS_Pitch = -35;
-
 
90
                                        if (GPS_Roll >  35) GPS_Roll =  35;
-
 
91
                                        if (GPS_Roll < -35) GPS_Roll = -35;
-
 
92
                                }
-
 
93
                                else // MK controlled by user
-
 
94
                                {
-
 
95
                                        // update hold point to current gps position
-
 
96
                                        GPSHoldPoint.Northing = GPSInfo.utmnorth;
-
 
97
                                        GPSHoldPoint.Easting  = GPSInfo.utmeast;
-
 
98
                                        GPSHoldPoint.Status = VALID;
-
 
99
                                        // disable gps control
-
 
100
                                        GPS_Pitch = 0;
-
 
101
                                        GPS_Roll = 0;
-
 
102
                                }
-
 
103
                        } // eof 3D-FIX
-
 
104
                        else // no 3D-SATFIX
-
 
105
                        {       // diable gps control
-
 
106
                                GPS_Pitch = 0;
-
 
107
                                GPS_Roll = 0;
-
 
108
                        }
-
 
109
                        // set current data as processed to avoid further calculations on the same gps data
-
 
110
                        GPSInfo.status = PROCESSED;