Subversion Repositories FlightCtrl

Rev

Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
726 killagreg 1
#include <inttypes.h>
1 ingob 2
 
726 killagreg 3
#include "fc.h"
4
#include "ubx.h"
5
#include "mymath.h"
727 killagreg 6
#include "timer0.h"
701 killagreg 7
 
726 killagreg 8
int16_t GPS_Pitch = 0;
9
int16_t GPS_Roll = 0;
10
 
727 killagreg 11
int32_t GPS_P_Factor = 0;
12
int32_t GPS_D_Factor = 0;
726 killagreg 13
 
14
typedef struct
1 ingob 15
{
726 killagreg 16
        int32_t Northing;
17
        int32_t Easting;
18
        uint8_t Status;
1 ingob 19
 
726 killagreg 20
} GPS_Pos_t;
21
 
22
// GPS coordinates for hold position
23
GPS_Pos_t GPSHoldPoint  = {0,0, INVALID};
24
 
25
 
26
 
27
// ---------------------------------------------------------------------------------
28
 
29
 
30
void GPS_Main(void)
1 ingob 31
{
726 killagreg 32
        int32_t coscompass, sincompass;
33
        int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0;
34
        int32_t PD_North = 0, PD_East = 0;
35
        int32_t GPSPosDev_North = 0, GPSPosDev_East = 0;
1 ingob 36
 
726 killagreg 37
        // poti2 enables the gps feature
733 killagreg 38
        if((Poti2 > 70) && (!EmergencyLanding)) // run GPS function only if Poti 2 is larger than 70 (switch on)
726 killagreg 39
        {
40
                switch(GPSInfo.status)
41
                {
42
                case INVALID:  // invalid gps data
43
                        GPS_Pitch = 0;
44
                        GPS_Roll = 0;
727 killagreg 45
                        BeepTime = 50;
726 killagreg 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
                        {
727 killagreg 63
                                // if sticks are centered  and hold position is valid enable position hold control
726 killagreg 64
                                if ((MaxStickPitch < 20) && (MaxStickRoll < 20) && (GPSHoldPoint.Status == VALID))
65
                                {
66
                                        // Calculate deviation from hold position
67
                                        GPSPosDev_North = GPSInfo.utmnorth - GPSHoldPoint.Northing;
68
                                        GPSPosDev_East  = GPSInfo.utmeast  - GPSHoldPoint.Easting;
1 ingob 69
 
727 killagreg 70
                                        //Calculate PD-components of the controller (negative sign for compensation)
71
                                        P_North = -(GPS_P_Factor * GPSPosDev_North)/2048;
72
                                        D_North = -(GPS_D_Factor * GPSInfo.velnorth)/512;
73
                                        P_East =  -(GPS_P_Factor * GPSPosDev_East)/2048;
74
                                        D_East =  -(GPS_D_Factor * GPSInfo.veleast)/512;
1 ingob 75
 
726 killagreg 76
                                        // PD-controller
727 killagreg 77
                                        PD_North = P_North + D_North;
78
                                        PD_East  = P_East  + D_East;
726 killagreg 79
 
727 killagreg 80
                                        // GPS to pitch and roll settings
726 killagreg 81
 
727 killagreg 82
                                        //A positive pitch angle moves head downwards (flying forward).
83
                                        //A positive roll angle tilts left side downwards (flying left).
84
 
85
                                        // If compass heading is 0 the head of the copter is in north direction.
86
                                        // A positive pitch angle will fly to north and a positive roll angle will fly to west.
87
                                        // In case of a positive north deviation/velocity the
88
                                        // copter should fly to south (negative pitch).
89
                                        // In case of a positive east position deviation and a positive east velocity the
90
                                        // copter should fly to west (positive roll).
91
 
92
                                        // The influence of the GPS_Pitch and GPS_Roll variable is contrarily to the stick values
93
                                        // in the fc.c. Therefore a positive north deviation/velocity should result in a positive
94
                                        // GPS_Pitch and a positive east deviation/velocity should result in a negative GPS_Roll.
95
 
96
                                        // rotation transformation to compass heading to match any copter orientation
97
                                        coscompass = (int32_t)c_cos_8192(CompassHeading);
98
                                        sincompass = (int32_t)c_sin_8192(CompassHeading);
99
 
100
                                        GPS_Roll  =    (int16_t)((coscompass * PD_East - sincompass * PD_North) / 8192);
101
                                        GPS_Pitch = -1*(int16_t)((sincompass * PD_East + coscompass * PD_North) / 8192);
102
 
726 killagreg 103
                                        // limit GPS controls
727 killagreg 104
                                        #define GPS_CTRL_LIMIT 35
105
                                        if (GPS_Pitch >  GPS_CTRL_LIMIT) GPS_Pitch =  GPS_CTRL_LIMIT;
106
                                        if (GPS_Pitch < -GPS_CTRL_LIMIT) GPS_Pitch = -GPS_CTRL_LIMIT;
107
                                        if (GPS_Roll  >  GPS_CTRL_LIMIT)  GPS_Roll =  GPS_CTRL_LIMIT;
108
                                        if (GPS_Roll  < -GPS_CTRL_LIMIT)  GPS_Roll = -GPS_CTRL_LIMIT;
726 killagreg 109
                                }
110
                                else // MK controlled by user
111
                                {
112
                                        // update hold point to current gps position
113
                                        GPSHoldPoint.Northing = GPSInfo.utmnorth;
114
                                        GPSHoldPoint.Easting  = GPSInfo.utmeast;
115
                                        GPSHoldPoint.Status = VALID;
116
                                        // disable gps control
117
                                        GPS_Pitch = 0;
118
                                        GPS_Roll = 0;
119
                                }
120
                        } // eof 3D-FIX
121
                        else // no 3D-SATFIX
122
                        {       // diable gps control
123
                                GPS_Pitch = 0;
124
                                GPS_Roll = 0;
727 killagreg 125
                                BeepTime = 50;
726 killagreg 126
                        }
127
                        // set current data as processed to avoid further calculations on the same gps data
128
                        GPSInfo.status = PROCESSED;
129
                        break;
130
                } // eof GPSInfo.status
131
        }
132
        return;
133
}
134