Subversion Repositories FlightCtrl

Rev

Rev 886 | Rev 909 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
886 killagreg 1
#include <inttypes.h>
2
#include <stdlib.h>
3
#include "fc.h"
4
#include "ubx.h"
5
#include "mymath.h"
6
#include "timer0.h"
7
#include "uart.h"
8
#include "rc.h"
9
#include "eeprom.h"
10
 
11
#define TSK_IDLE                0
12
#define TSK_HOLD                1
13
#define TSK_HOME                2
14
 
15
#define GPS_STICK_SENSE         15              // must be at least in a range where 90% of the trimming does not switch of the GPS function
16
#define GPS_STICK_LIMIT         45              // limit of gps stick control to avoid critical flight attitudes
17
#define GPS_POSDEV_INTEGRAL_LIMIT  32000  // limit for the position error integral
18
#define GPS_P_LIMIT                     25
19
 
20
 
21
uint8_t GPS_P_Factor = 0, GPS_I_Factor = 0, GPS_D_Factor = 0;
22
 
23
 
24
 
25
typedef struct
26
{
27
        int32_t Longitude;
28
        int32_t Latitude;
29
        int32_t Altitude;
30
        uint8_t Status;
31
} GPS_Pos_t;
32
 
33
// GPS coordinates for hold position
34
GPS_Pos_t HoldPosition  = {0,0,0,INVALID};
35
// GPS coordinates for home position
36
GPS_Pos_t HomePosition  = {0,0,0,INVALID};
37
 
38
 
39
// ---------------------------------------------------------------------------------
40
 
41
// checks pitch and roll sticks for manual control
42
uint8_t IsManualControlled(void)
43
{
44
        if ( (abs(PPM_in[ParamSet.ChannelAssignment[CH_PITCH]]) < GPS_STICK_SENSE) && (abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) < GPS_STICK_SENSE)) return 0;
45
        else return 1;
46
}
47
 
48
// set home position to current positon
49
void GPS_SetHomePosition(void)
50
{
51
        if( ((GPSInfo.status == VALID) || (GPSInfo.status == PROCESSED)) && GPSInfo.satfix == SATFIX_3D)
52
        {
53
                HomePosition.Longitude = GPSInfo.longitude;
54
                HomePosition.Latitude = GPSInfo.latitude;
55
                HomePosition.Altitude = GPSInfo.altitude;
56
                HomePosition.Status = VALID;
57
                BeepTime = 1000; // signal if new home position was set
58
        }
59
        else
60
        {
61
                HomePosition.Status = INVALID;
62
        }
63
}
64
 
65
// set hold position to current positon
66
void GPS_SetHoldPosition(void)
67
{
68
        if( ((GPSInfo.status == VALID) || (GPSInfo.status == PROCESSED)) && GPSInfo.satfix == SATFIX_3D)
69
        {
70
                HoldPosition.Longitude = GPSInfo.longitude;
71
                HoldPosition.Latitude = GPSInfo.latitude;
72
                HoldPosition.Altitude = GPSInfo.altitude;
73
                HoldPosition.Status = VALID;
74
        }
75
        else
76
        {
77
                HoldPosition.Status = INVALID;
78
        }
79
}
80
 
81
// clear home position
82
void GPS_ClearHomePosition(void)
83
{
84
                HomePosition.Status = INVALID;
85
}
86
 
87
// disable GPS control sticks
88
void GPS_Neutral(void)
89
{
90
        GPS_Pitch = 0;
91
        GPS_Roll = 0;
92
}
93
 
94
// calculates the GPS control stick values from the deviation to target position
95
// if the pointer to the target positin is NULL or is the target position invalid
96
// then the P part of the controller is deactivated.
97
void GPS_PIDController(GPS_Pos_t *pTargetPos)
98
{
99
        int32_t temp, temp1, PID_Pitch, PID_Roll;
100
        int32_t coscompass, sincompass;
101
        int32_t GPSPosDev_North, GPSPosDev_East; // Position deviation in cm
102
        int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0, I_North = 0, I_East = 0;
103
        int32_t PID_North = 0, PID_East = 0;
104
        static int32_t cos_target_latitude = 1;
105
        static int32_t GPSPosDevIntegral_North = 0, GPSPosDevIntegral_East = 0;
106
        static GPS_Pos_t *pLastTargetPos = 0;
107
 
108
        // if GPS data and Compass are ok
109
        if((GPSInfo.status == VALID) && (GPSInfo.satfix == SATFIX_3D) && (CompassHeading >= 0) )
110
        {
111
 
112
                if(pTargetPos != NULL) // if there is a target position
113
                {
114
                        if(pTargetPos->Status != INVALID) // and the position data are valid
115
                        {
116
                                // if the target data are updated or the target pointer has changed
117
                                if ((pTargetPos->Status != PROCESSED) || (pTargetPos != pLastTargetPos) )
118
                                {
119
                                        // reset error integral
120
                                        GPSPosDevIntegral_North = 0;
121
                                        GPSPosDevIntegral_East = 0;
122
                                        // recalculate latitude projection
123
                                        cos_target_latitude = (int32_t)c_cos_8192((int16_t)(pTargetPos->Latitude/10000000L));
124
                                        // remember last target pointer
125
                                        pLastTargetPos = pTargetPos;
126
                                        // mark data as processed
127
                                        pTargetPos->Status = PROCESSED;
128
                                }
129
                                // calculate position deviation from latitude and longitude differences
130
                                GPSPosDev_North = (GPSInfo.latitude - pTargetPos->Latitude); // to calculate real cm we would need *111/100 additionally
131
                                GPSPosDev_East  = (GPSInfo.longitude - pTargetPos->Longitude); // to calculate real cm we would need *111/100 additionally
132
                                // calculate latitude projection
133
                                GPSPosDev_East  *= cos_target_latitude;
134
                                GPSPosDev_East /= 8192;
135
                        }
136
                        else // no valid target position available
137
                        {
138
                                // reset error
139
                                GPSPosDev_North = 0;
140
                                GPSPosDev_East = 0;
141
                                // reset error integral
142
                                GPSPosDevIntegral_North = 0;
143
                                GPSPosDevIntegral_East = 0;
144
                        }
145
                }
146
                else // no target position available
147
                {
148
                        // reset error
149
                        GPSPosDev_North = 0;
150
                        GPSPosDev_East = 0;
151
                        // reset error integral
152
                        GPSPosDevIntegral_North = 0;
153
                        GPSPosDevIntegral_East = 0;
154
                }
155
 
156
                //Calculate PID-components of the controller (negative sign for compensation)
157
 
158
                // P-Part
159
                P_North = -((int32_t)GPS_P_Factor * GPSPosDev_North)/2048;
160
                P_East =  -((int32_t)GPS_P_Factor * GPSPosDev_East)/2048;
161
 
162
                // I-Part
163
                I_North = -((int32_t)GPS_I_Factor * GPSPosDevIntegral_North)/8192;
164
                I_East =  -((int32_t)GPS_I_Factor * GPSPosDevIntegral_East)/8192;
165
 
166
                // combine P- & I-Part
167
                PID_North = P_North + I_North;
168
                PID_East  = P_East  + I_East;
169
 
170
                //limit PI-Part to limit the max velocity
171
                //temp1 = ((int32_t)GPS_D_Factor * MAX_VELOCITY)/512; // the PI-Part limit
172
                temp = (int32_t)c_sqrt(PID_North*PID_North + PID_East*PID_East); // the current PI-Part
173
                if(temp > GPS_P_LIMIT) // P-Part limit is reached
174
                {
175
                        // normalize P-part components to the P-Part limit
176
                        PID_North  = (PID_North  * GPS_P_LIMIT)/temp;
177
                        PID_East   = (PID_East   * GPS_P_LIMIT)/temp;
178
                }
179
                else // PI-Part under its limit
180
                {
181
                        // update position error integrals
182
                        GPSPosDevIntegral_North += GPSPosDev_North/16;
183
                        if( GPSPosDevIntegral_North > GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_North = GPS_POSDEV_INTEGRAL_LIMIT;
184
                        else if (GPSPosDevIntegral_North < -GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_North = -GPS_POSDEV_INTEGRAL_LIMIT;
185
                        GPSPosDevIntegral_East += GPSPosDev_East/16;
186
                        if( GPSPosDevIntegral_East > GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_East = GPS_POSDEV_INTEGRAL_LIMIT;
187
                        else if (GPSPosDevIntegral_East < -GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_East = -GPS_POSDEV_INTEGRAL_LIMIT;
188
                }
189
 
190
                // D-Part
191
                D_North = -((int32_t)GPS_D_Factor * GPSInfo.velnorth)/512;
192
                D_East =  -((int32_t)GPS_D_Factor * GPSInfo.veleast)/512;
193
 
194
 
195
                // combine PI- and D-Part
196
                PID_North += D_North;
197
                PID_East  += D_East;
198
 
199
                // GPS to pitch and roll settings
200
 
201
                // A positive pitch angle moves head downwards (flying forward).
202
                // A positive roll angle tilts left side downwards (flying left).
203
                // If compass heading is 0 the head of the copter is in north direction.
204
                // A positive pitch angle will fly to north and a positive roll angle will fly to west.
205
                // In case of a positive north deviation/velocity the
206
                // copter should fly to south (negative pitch).
207
                // In case of a positive east position deviation and a positive east velocity the
208
                // copter should fly to west (positive roll).
209
                // The influence of the GPS_Pitch and GPS_Roll variable is contrarily to the stick values
210
                // in the fc.c. Therefore a positive north deviation/velocity should result in a positive
211
                // GPS_Pitch and a positive east deviation/velocity should result in a negative GPS_Roll.
212
 
213
                coscompass = (int32_t)c_cos_8192(CompassHeading);
214
                sincompass = (int32_t)c_sin_8192(CompassHeading);
215
                PID_Roll  =  (coscompass * PID_East - sincompass * PID_North) / 8192;
216
                PID_Pitch =   -1*((sincompass * PID_East + coscompass * PID_North) / 8192);
217
 
218
                // limit resulting GPS control vector
219
                temp = (int32_t)c_sqrt(PID_Roll*PID_Roll + PID_Pitch*PID_Pitch);
220
                if (temp > GPS_STICK_LIMIT)
221
                {
222
                        // normalize control vector components to the limit
223
                        PID_Roll  = (PID_Roll  * GPS_STICK_LIMIT)/temp;
224
                        PID_Pitch = (PID_Pitch * GPS_STICK_LIMIT)/temp;
225
                }
226
 
227
                GPS_Roll  = (int16_t)PID_Roll;
228
                GPS_Pitch = (int16_t)PID_Pitch;
229
 
230
        }
231
        else // invalid GPS data or bad compass reading
232
        {
233
                GPS_Neutral(); // do nothing
234
                // reset error integral
235
                GPSPosDevIntegral_North = 0;
236
                GPSPosDevIntegral_East = 0;
237
        }
238
}
239
 
240
 
241
 
242
 
243
void GPS_Main(uint8_t ctrl)
244
{
245
        static uint8_t GPS_Task = TSK_IDLE;
246
        static uint8_t GPS_P_Delay = 0;
247
        int16_t satbeep;
248
 
249
        // ctrl enables the gps feature
250
        if(ctrl < 70) GPS_Task = TSK_IDLE;
251
        else if (ctrl < 160) GPS_Task = TSK_HOLD;
252
        else GPS_Task = TSK_HOME; // ctrl >= 160
253
 
254
 
255
        switch(GPSInfo.status)
256
        {
257
        case INVALID:  // invalid gps data
258
                GPS_Neutral();
259
                if(GPS_Task != TSK_IDLE)
260
                {
261
                        BeepTime = 100; // beep if signal is neccesary
262
                }
263
                break;
264
        case PROCESSED: // if gps data are already processed do nothing
265
                // downcount timeout
266
                if(GPSTimeout) GPSTimeout--;
267
                // if no new data arrived within timeout set current data invalid
268
                // and therefore disable GPS
269
                else
270
                {
271
                        GPS_Neutral();
272
                        GPSInfo.status = INVALID;
273
                }
274
                break;
275
        case VALID: // new valid data from gps device
276
                // if the gps data quality is good
277
                DebugOut.Analog[29] = (int16_t)GPSInfo.updatetime;
278
                if (GPSInfo.satfix == SATFIX_3D)
279
                {
280
                        switch(GPS_Task) // check what's to do
281
                        {
282
                                case TSK_IDLE:
283
                                        // update hold position to current gps position
284
                                        GPS_SetHoldPosition(); // can get invalid if gps signal is bad
285
                                        // disable gps control
286
                                        GPS_Neutral();
287
                                        break; // eof TSK_IDLE
288
                                case TSK_HOLD:
289
                                        if(HoldPosition.Status != INVALID)
290
                                        {
291
                                                if( IsManualControlled() ) // MK controlled by user
292
                                                {
293
                                                        // update hold point to current gps position
294
                                                        GPS_SetHoldPosition();
295
                                                        // disable gps control
296
                                                        GPS_Neutral();
297
                                                        GPS_P_Delay = 0;
298
                                                }
299
                                                else // GPS control active
300
                                                {
301
                                                        if(GPS_P_Delay<7)
302
                                                        { // delayed activation of P-Part for 8 cycles (8*0.25s = 2s)
303
                                                                GPS_P_Delay++;
304
                                                                GPS_SetHoldPosition();  // update hold point to current gps position
305
                                                                GPS_PIDController(NULL); // activates only the D-Part
306
                                                        }
307
                                                        else GPS_PIDController(&HoldPosition);// activates the P&D-Part
308
                                                }
309
                                        }
310
                                        else // invalid Hold Position
311
                                        {  // try to catch a valid hold position from gps data input
312
                                                GPS_SetHoldPosition();
313
                                                GPS_Neutral();
314
                                        }
315
                                        break; // eof TSK_HOLD
316
                                case TSK_HOME:
317
                                        if(HomePosition.Status != INVALID)
318
                                        {
319
                                                // update hold point to current gps position
320
                                                // to avoid a flight back if home comming is deactivated
321
                                                GPS_SetHoldPosition();
322
                                                if( IsManualControlled() ) // MK controlled by user
323
                                                {
324
                                                        GPS_Neutral();
325
                                                }
326
                                                else // GPS control active
327
                                                {
328
                                                        GPS_PIDController(&HomePosition);
329
                                                }
330
                                        }
331
                                        else // bad home position
332
                                        {
333
                                                BeepTime = 50; // signal invalid home position
334
                                                // try to hold at least the position as a fallback option
335
 
336
                                                if (HoldPosition.Status != INVALID)
337
                                                {
338
                                                        if( IsManualControlled() ) // MK controlled by user
339
                                                        {
340
                                                                GPS_Neutral();
341
                                                        }
342
                                                        else // GPS control active
343
                                                        {
344
                                                                GPS_PIDController(&HoldPosition);
345
                                                        }
346
                                                }
347
                                                else
348
                                                { // try to catch a valid hold position
349
                                                        GPS_SetHoldPosition();
350
                                                        GPS_Neutral();
351
                                                }
352
                                        }
353
                                        break; // eof TSK_HOME
354
                                default: // unhandled task
355
                                        GPS_Neutral();
356
                                        break; // eof default
357
                        } // eof switch GPS_Task
358
                } // eof 3D-FIX
359
                else // no 3D-SATFIX
360
                {       // disable gps control
361
                        GPS_Neutral();
362
                        if(GPS_Task != TSK_IDLE)
363
                        {
364
                                satbeep = 1600 - (int16_t)GPSInfo.satnum * 200; // is zero at 8 sats
365
                                if (satbeep < 0) satbeep = 0;
366
                                BeepTime = 50 + (uint16_t)satbeep; // max 1650 * 0.1 ms =
367
                        }
368
                }
369
                // set current data as processed to avoid further calculations on the same gps data
370
                GPSInfo.status = PROCESSED;
371
                break;
372
        } // eof GPSInfo.status
373
}
374