Subversion Repositories FlightCtrl

Rev

Rev 733 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 733 Rev 741
Line 2... Line 2...
2
 
2
 
3
#include "fc.h"
3
#include "fc.h"
4
#include "ubx.h"
4
#include "ubx.h"
5
#include "mymath.h"
5
#include "mymath.h"
-
 
6
#include "timer0.h"
-
 
7
#include "uart.h"
-
 
8
 
-
 
9
#define TSK_IDLE                0
-
 
10
#define TSK_HOLD                1
-
 
11
#define TSK_HOME                2
Line 6... Line 12...
6
#include "timer0.h"
12
 
7
 
13
 
Line 8... Line 14...
8
int16_t GPS_Pitch = 0;
14
int16_t GPS_Pitch = 0;
Line 18... Line 24...
18
        uint8_t Status;
24
        uint8_t Status;
Line 19... Line 25...
19
 
25
 
Line 20... Line 26...
20
} GPS_Pos_t;
26
} GPS_Pos_t;
21
 
27
 
22
// GPS coordinates for hold position
-
 
-
 
28
// GPS coordinates for hold position
-
 
29
GPS_Pos_t HoldPosition  = {0,0, INVALID};
Line 23... Line 30...
23
GPS_Pos_t GPSHoldPoint  = {0,0, INVALID};
30
// GPS coordinates for home position
Line 31... Line 38...
31
{
38
{
32
        int32_t coscompass, sincompass;
39
        int32_t coscompass, sincompass;
33
        int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0;
40
        int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0;
34
        int32_t PD_North = 0, PD_East = 0;
41
        int32_t PD_North = 0, PD_East = 0;
35
        int32_t GPSPosDev_North = 0, GPSPosDev_East = 0;
42
        int32_t GPSPosDev_North = 0, GPSPosDev_East = 0;
-
 
43
        static uint8_t GPS_Task = TSK_IDLE;
-
 
44
 
Line 36... Line 45...
36
 
45
 
37
        // poti2 enables the gps feature
46
        // poti2 enables the gps feature
-
 
47
        if(Poti2 < 70) GPS_Task = TSK_IDLE;
-
 
48
        else if (Poti2 < 160) GPS_Task = TSK_HOLD;
-
 
49
        //else GPS_Task = TSK_HOME;
-
 
50
 
-
 
51
 
38
        if((Poti2 > 70) && (!EmergencyLanding)) // run GPS function only if Poti 2 is larger than 70 (switch on)
52
        switch(GPSInfo.status)
-
 
53
        {
-
 
54
        case INVALID:  // invalid gps data
-
 
55
                GPS_Pitch = 0;
-
 
56
                GPS_Roll = 0;
-
 
57
                if(GPS_Task != TSK_IDLE) BeepTime = 50; // beep if signal is neccesary
-
 
58
                break;
39
        {
59
        case PROCESSED: // if gps data are already processed
-
 
60
                // downcount timeout
-
 
61
                if(GPSTimeout) GPSTimeout--;
-
 
62
                // if no new data arrived within timeout set current data invalid
-
 
63
                // and therefore disable GPS
40
                switch(GPSInfo.status)
64
                else
41
                {
-
 
42
                case INVALID:  // invalid gps data
65
                {
43
                        GPS_Pitch = 0;
66
                        GPS_Pitch = 0;
44
                        GPS_Roll = 0;
-
 
45
                        BeepTime = 50;
-
 
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;
67
                        GPS_Roll = 0;
56
                                GPSInfo.status = INVALID;
68
                        GPSInfo.status = INVALID;
57
                        }
69
                }
58
                        break;
70
                break;
59
                case VALID: // new valid data from gps device
71
        case VALID: // new valid data from gps device
60
                        // if the gps data quality is sufficient
72
                // if the gps data quality is sufficient
-
 
73
                if (GPSInfo.satfix == SATFIX_3D)
-
 
74
                {
61
                        if (GPSInfo.satfix == SATFIX_3D)
75
                        switch(GPS_Task) // check what's to do
62
                        {
-
 
63
                                // if sticks are centered  and hold position is valid enable position hold control
-
 
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;
-
 
69
 
-
 
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;
-
 
75
 
76
                        {
76
                                        // PD-controller
-
 
77
                                        PD_North = P_North + D_North;
-
 
78
                                        PD_East  = P_East  + D_East;
-
 
79
 
-
 
80
                                        // GPS to pitch and roll settings
-
 
81
 
-
 
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
 
-
 
103
                                        // limit GPS controls
-
 
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;
-
 
109
                                }
-
 
110
                                else // MK controlled by user
-
 
111
                                {
77
                                case TSK_IDLE:
112
                                        // update hold point to current gps position
78
                                        // update hold point to current gps position
113
                                        GPSHoldPoint.Northing = GPSInfo.utmnorth;
79
                                        HoldPosition.Northing = GPSInfo.utmnorth;
114
                                        GPSHoldPoint.Easting  = GPSInfo.utmeast;
80
                                        HoldPosition.Easting  = GPSInfo.utmeast;
115
                                        GPSHoldPoint.Status = VALID;
81
                                        HoldPosition.Status = VALID;
116
                                        // disable gps control
82
                                        // disable gps control
117
                                        GPS_Pitch = 0;
83
                                        GPS_Pitch = 0;
-
 
84
                                        GPS_Roll = 0;
-
 
85
                                        break; // eof TSK_IDLE
-
 
86
                                case TSK_HOLD:
-
 
87
                                        // if sticks are centered and hold position is valid enable position hold control
-
 
88
                                        if ((MaxStickPitch < 20) && (MaxStickRoll < 20) && (HoldPosition.Status == VALID))
-
 
89
                                        {
-
 
90
                                                // Calculate deviation from hold position
-
 
91
                                                GPSPosDev_North = GPSInfo.utmnorth - HoldPosition.Northing;
-
 
92
                                                GPSPosDev_East  = GPSInfo.utmeast  - HoldPosition.Easting;
-
 
93
                                                DebugOut.Analog[12] = GPSPosDev_North;
-
 
94
                                                DebugOut.Analog[13] = GPSPosDev_East;
-
 
95
 
-
 
96
                                                //Calculate PD-components of the controller (negative sign for compensation)
-
 
97
                                                P_North = -(GPS_P_Factor * GPSPosDev_North)/2048;
-
 
98
                                                D_North = -(GPS_D_Factor * GPSInfo.velnorth)/512;
-
 
99
                                                P_East =  -(GPS_P_Factor * GPSPosDev_East)/2048;
-
 
100
                                                D_East =  -(GPS_D_Factor * GPSInfo.veleast)/512;
-
 
101
 
-
 
102
                                                // PD-controller
-
 
103
                                                PD_North = P_North + D_North;
-
 
104
                                                PD_East  = P_East  + D_East;
-
 
105
 
-
 
106
                                                // GPS to pitch and roll settings
-
 
107
 
-
 
108
                                                //A positive pitch angle moves head downwards (flying forward).
-
 
109
                                                //A positive roll angle tilts left side downwards (flying left).
-
 
110
 
-
 
111
                                                // If compass heading is 0 the head of the copter is in north direction.
-
 
112
                                                // A positive pitch angle will fly to north and a positive roll angle will fly to west.
-
 
113
                                                // In case of a positive north deviation/velocity the
-
 
114
                                                // copter should fly to south (negative pitch).
-
 
115
                                                // In case of a positive east position deviation and a positive east velocity the
-
 
116
                                                // copter should fly to west (positive roll).
-
 
117
 
-
 
118
                                                // The influence of the GPS_Pitch and GPS_Roll variable is contrarily to the stick values
-
 
119
                                                // in the fc.c. Therefore a positive north deviation/velocity should result in a positive
-
 
120
                                                // GPS_Pitch and a positive east deviation/velocity should result in a negative GPS_Roll.
-
 
121
 
-
 
122
                                                // rotation transformation to compass heading to match any copter orientation
-
 
123
                                                if (CompassHeading < 0) // no valid compass data
-
 
124
                                                { // disable GPS
-
 
125
                                                        GPS_Task = TSK_IDLE;
-
 
126
                                                        GPS_Pitch = 0;
-
 
127
                                                        GPS_Roll = 0;
-
 
128
                                                }
-
 
129
                                                else
-
 
130
                                                {
-
 
131
                                                        coscompass = (int32_t)c_cos_8192(CompassHeading);
-
 
132
                                                        sincompass = (int32_t)c_sin_8192(CompassHeading);
-
 
133
                                                        GPS_Roll  =    (int16_t)((coscompass * PD_East - sincompass * PD_North) / 8192);
-
 
134
                                                        GPS_Pitch = -1*(int16_t)((sincompass * PD_East + coscompass * PD_North) / 8192);
-
 
135
                                                }
-
 
136
                                        }
-
 
137
                                        else // MK controlled by user
-
 
138
                                        {
-
 
139
                                                // update hold point to current gps position
-
 
140
                                                HoldPosition.Northing = GPSInfo.utmnorth;
-
 
141
                                                HoldPosition.Easting  = GPSInfo.utmeast;
-
 
142
                                                HoldPosition.Status = VALID;
-
 
143
                                                // disable gps control
-
 
144
                                                GPS_Pitch = 0;
118
                                        GPS_Roll = 0;
145
                                                GPS_Roll = 0;
-
 
146
                                        }
-
 
147
                                        break; // eof TSK_HOLD
-
 
148
                                default: // unhandled task
-
 
149
                                        GPS_Task = TSK_IDLE;
-
 
150
                                        GPS_Pitch = 0;
-
 
151
                                        GPS_Roll = 0;
-
 
152
                                        break; // eof default
-
 
153
                        } // eof switch GPS_Task
-
 
154
 
-
 
155
                        // limit GPS controls
-
 
156
                        #define GPS_CTRL_LIMIT 35
-
 
157
                        if (GPS_Pitch >  GPS_CTRL_LIMIT) GPS_Pitch =  GPS_CTRL_LIMIT;
-
 
158
                        if (GPS_Pitch < -GPS_CTRL_LIMIT) GPS_Pitch = -GPS_CTRL_LIMIT;
-
 
159
                        if (GPS_Roll  >  GPS_CTRL_LIMIT)  GPS_Roll =  GPS_CTRL_LIMIT;
119
                                }
160
                        if (GPS_Roll  < -GPS_CTRL_LIMIT)  GPS_Roll = -GPS_CTRL_LIMIT;
-
 
161
                } // eof 3D-FIX
120
                        } // eof 3D-FIX
162
 
121
                        else // no 3D-SATFIX
163
                else // no 3D-SATFIX
122
                        {       // diable gps control
164
                {       // disable gps control
123
                                GPS_Pitch = 0;
165
                        GPS_Pitch = 0;
124
                                GPS_Roll = 0;
166
                        GPS_Roll = 0;
125
                                BeepTime = 50;
167
                        if(GPS_Task != TSK_IDLE) BeepTime = 50;
-
 
168
                }
126
                        }
169
 
127
                        // set current data as processed to avoid further calculations on the same gps data
170
                // set current data as processed to avoid further calculations on the same gps data
128
                        GPSInfo.status = PROCESSED;
171
                GPSInfo.status = PROCESSED;
129
                        break;
172
                break;
130
                } // eof GPSInfo.status
-
 
-
 
173
        } // eof GPSInfo.status
131
        }
174
        DebugOut.Analog[14] = GPS_Pitch;
132
        return;
175
        DebugOut.Analog[15] = GPS_Roll;