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