Subversion Repositories FlightCtrl

Rev

Rev 942 | 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"
938 killagreg 7
#include "uart.h"
886 killagreg 8
#include "rc.h"
9
#include "eeprom.h"
10
 
936 killagreg 11
typedef enum
12
{
13
        GPS_FLIGHT_MODE_UNDEF,
14
        GPS_FLIGHT_MODE_FREE,
15
        GPS_FLIGHT_MODE_AID,
16
        GPS_FLIGHT_MODE_HOME,
17
} FlightMode_t;
886 killagreg 18
 
950 pangu 19
#define GPS_POSINTEGRAL_LIMIT 32000
20
#define GPS_STICK_LIMIT         45              // limit of gps stick control to avoid critical flight attitudes
21
#define GPS_P_LIMIT                     25
886 killagreg 22
 
23
 
24
typedef struct
25
{
26
        int32_t Longitude;
27
        int32_t Latitude;
28
        int32_t Altitude;
936 killagreg 29
        Status_t Status;
886 killagreg 30
} GPS_Pos_t;
31
 
32
// GPS coordinates for hold position
33
GPS_Pos_t HoldPosition  = {0,0,0,INVALID};
34
// GPS coordinates for home position
35
GPS_Pos_t HomePosition  = {0,0,0,INVALID};
936 killagreg 36
// the current flight mode
37
FlightMode_t FlightMode = GPS_FLIGHT_MODE_UNDEF;
886 killagreg 38
 
39
 
40
// ---------------------------------------------------------------------------------
936 killagreg 41
void GPS_UpdateParameter(void)
886 killagreg 42
{
936 killagreg 43
        static FlightMode_t FlightModeOld = GPS_FLIGHT_MODE_UNDEF;
886 killagreg 44
 
936 killagreg 45
        if((RC_Quality < 100) || (MKFlags & MKFLAG_EMERGENCY_LANDING))
886 killagreg 46
        {
936 killagreg 47
                FlightMode = GPS_FLIGHT_MODE_FREE;
886 killagreg 48
        }
49
        else
50
        {
936 killagreg 51
                if     (FCParam.NaviGpsModeControl <  50) FlightMode = GPS_FLIGHT_MODE_AID;
52
                else if(FCParam.NaviGpsModeControl < 180) FlightMode = GPS_FLIGHT_MODE_FREE;
53
                else                                              FlightMode = GPS_FLIGHT_MODE_HOME;
886 killagreg 54
        }
936 killagreg 55
        if (FlightMode != FlightModeOld)
56
        {
57
                BeepTime = 100;
58
        }
59
        FlightModeOld = FlightMode;
886 killagreg 60
}
61
 
936 killagreg 62
 
63
 
64
// ---------------------------------------------------------------------------------
65
// This function defines a good GPS signal condition
66
uint8_t GPS_IsSignalOK(void)
886 killagreg 67
{
936 killagreg 68
        static uint8_t GPSFix = 0;
69
        if( (GPSInfo.status != INVALID)  && (GPSInfo.satfix == SATFIX_3D) && (GPSInfo.flags & FLAG_GPSFIXOK) && ((GPSInfo.satnum >= ParamSet.NaviGpsMinSat) || GPSFix))
886 killagreg 70
        {
936 killagreg 71
                GPSFix = 1;
72
                return(1);
73
 
886 killagreg 74
        }
936 killagreg 75
        else return (0);
76
 
77
}
78
// ---------------------------------------------------------------------------------
79
// rescale xy-vector length to  limit
80
uint8_t GPS_LimitXY(int32_t *x, int32_t *y, int32_t limit)
81
{
82
        uint8_t retval = 0;
83
        int32_t len;
84
        len = (int32_t)c_sqrt(*x * *x + *y * *y);
85
        if (len > limit)
886 killagreg 86
        {
936 killagreg 87
                // normalize control vector components to the limit
88
                *x  = (*x  * limit) / len;
89
                *y  = (*y  * limit) / len;
90
                retval = 1;
886 killagreg 91
        }
936 killagreg 92
        return(retval);
886 killagreg 93
}
94
 
936 killagreg 95
// checks nick and roll sticks for manual control
96
uint8_t GPS_IsManualControlled(void)
886 killagreg 97
{
936 killagreg 98
        if ( (abs(PPM_in[ParamSet.ChannelAssignment[CH_NICK]]) < ParamSet.NaviStickThreshold) && (abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) < ParamSet.NaviStickThreshold)) return 0;
99
        else return 1;
886 killagreg 100
}
101
 
936 killagreg 102
// set given position to current gps position
103
uint8_t GPS_SetCurrPosition(GPS_Pos_t * pGPSPos)
104
{
105
        uint8_t retval = 0;
106
        if(pGPSPos == NULL) return(retval);     // bad pointer
107
 
108
        if(GPS_IsSignalOK())
109
        {       // is GPS signal condition is fine
110
                pGPSPos->Longitude      = GPSInfo.longitude;
111
                pGPSPos->Latitude       = GPSInfo.latitude;
112
                pGPSPos->Altitude       = GPSInfo.altitude;
113
                pGPSPos->Status         = NEWDATA;
114
                retval = 1;
115
        }
116
        else
117
        {       // bad GPS signal condition
118
                pGPSPos->Status = INVALID;
119
                retval = 0;
120
        }
121
        return(retval);
122
}
123
 
124
// clear position
125
uint8_t GPS_ClearPosition(GPS_Pos_t * pGPSPos)
126
{
127
        uint8_t retval = 0;
128
        if(pGPSPos == NULL) return(retval);     // bad pointer
129
        else
130
        {
131
                pGPSPos->Longitude      = 0;
132
                pGPSPos->Latitude       = 0;
133
                pGPSPos->Altitude       = 0;
134
                pGPSPos->Status         = INVALID;
135
                retval = 1;
136
        }
137
        return (retval);
138
}
139
 
886 killagreg 140
// disable GPS control sticks
141
void GPS_Neutral(void)
142
{
911 killagreg 143
        GPS_Nick = 0;
886 killagreg 144
        GPS_Roll = 0;
145
}
146
 
147
// calculates the GPS control stick values from the deviation to target position
148
// if the pointer to the target positin is NULL or is the target position invalid
149
// then the P part of the controller is deactivated.
150
void GPS_PIDController(GPS_Pos_t *pTargetPos)
151
{
938 killagreg 152
        static int32_t PID_Nick, PID_Roll;
886 killagreg 153
        int32_t coscompass, sincompass;
154
        int32_t GPSPosDev_North, GPSPosDev_East; // Position deviation in cm
155
        int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0, I_North = 0, I_East = 0;
156
        int32_t PID_North = 0, PID_East = 0;
157
        static int32_t cos_target_latitude = 1;
158
        static int32_t GPSPosDevIntegral_North = 0, GPSPosDevIntegral_East = 0;
159
        static GPS_Pos_t *pLastTargetPos = 0;
160
 
161
        // if GPS data and Compass are ok
936 killagreg 162
        if( GPS_IsSignalOK() && (CompassHeading >= 0) )
886 killagreg 163
        {
164
 
165
                if(pTargetPos != NULL) // if there is a target position
166
                {
167
                        if(pTargetPos->Status != INVALID) // and the position data are valid
168
                        {
169
                                // if the target data are updated or the target pointer has changed
170
                                if ((pTargetPos->Status != PROCESSED) || (pTargetPos != pLastTargetPos) )
171
                                {
172
                                        // reset error integral
173
                                        GPSPosDevIntegral_North = 0;
174
                                        GPSPosDevIntegral_East = 0;
175
                                        // recalculate latitude projection
176
                                        cos_target_latitude = (int32_t)c_cos_8192((int16_t)(pTargetPos->Latitude/10000000L));
177
                                        // remember last target pointer
178
                                        pLastTargetPos = pTargetPos;
179
                                        // mark data as processed
180
                                        pTargetPos->Status = PROCESSED;
181
                                }
182
                                // calculate position deviation from latitude and longitude differences
183
                                GPSPosDev_North = (GPSInfo.latitude - pTargetPos->Latitude); // to calculate real cm we would need *111/100 additionally
184
                                GPSPosDev_East  = (GPSInfo.longitude - pTargetPos->Longitude); // to calculate real cm we would need *111/100 additionally
185
                                // calculate latitude projection
186
                                GPSPosDev_East  *= cos_target_latitude;
187
                                GPSPosDev_East /= 8192;
188
                        }
189
                        else // no valid target position available
190
                        {
191
                                // reset error
192
                                GPSPosDev_North = 0;
193
                                GPSPosDev_East = 0;
194
                                // reset error integral
195
                                GPSPosDevIntegral_North = 0;
196
                                GPSPosDevIntegral_East = 0;
197
                        }
198
                }
199
                else // no target position available
200
                {
201
                        // reset error
202
                        GPSPosDev_North = 0;
203
                        GPSPosDev_East = 0;
204
                        // reset error integral
205
                        GPSPosDevIntegral_North = 0;
206
                        GPSPosDevIntegral_East = 0;
207
                }
208
 
938 killagreg 209
                //Calculate PID-components of the controller
886 killagreg 210
 
939 killagreg 211
                // D-Part
950 pangu 212
                D_North = ((int32_t)FCParam.NaviGpsD * GPSInfo.velnorth)/512;
213
                D_East =  ((int32_t)FCParam.NaviGpsD * GPSInfo.veleast)/512;
939 killagreg 214
 
886 killagreg 215
                // P-Part
950 pangu 216
                P_North = ((int32_t)FCParam.NaviGpsP * GPSPosDev_North)/2048;
217
                P_East =  ((int32_t)FCParam.NaviGpsP * GPSPosDev_East)/2048;
886 killagreg 218
 
219
                // I-Part
939 killagreg 220
                I_North = ((int32_t)FCParam.NaviGpsI * GPSPosDevIntegral_North)/8192;
221
                I_East =  ((int32_t)FCParam.NaviGpsI * GPSPosDevIntegral_East)/8192;
886 killagreg 222
 
223
 
950 pangu 224
                // combine P & I
225
                PID_North = P_North + I_North;
226
                PID_East  = P_East  + I_East;
227
                if(!GPS_LimitXY(&PID_North, &PID_East, GPS_P_LIMIT))
228
                {
229
                        GPSPosDevIntegral_North += GPSPosDev_North/16;
230
                        GPSPosDevIntegral_East  += GPSPosDev_East/16;
231
                        GPS_LimitXY(&GPSPosDevIntegral_North, &GPSPosDevIntegral_North, GPS_POSINTEGRAL_LIMIT);
232
                }
233
 
234
                // combine PI- and D-Part
235
                PID_North += D_North;
236
                PID_East  += D_East;
237
 
238
 
936 killagreg 239
                // scale combination with gain.
937 killagreg 240
                PID_North = (PID_North * (int32_t)FCParam.NaviGpsGain) / 100;
241
                PID_East  = (PID_East  * (int32_t)FCParam.NaviGpsGain) / 100;
936 killagreg 242
 
911 killagreg 243
                // GPS to nick and roll settings
886 killagreg 244
 
911 killagreg 245
                // A positive nick angle moves head downwards (flying forward).
886 killagreg 246
                // A positive roll angle tilts left side downwards (flying left).
247
                // If compass heading is 0 the head of the copter is in north direction.
911 killagreg 248
                // A positive nick angle will fly to north and a positive roll angle will fly to west.
886 killagreg 249
                // In case of a positive north deviation/velocity the
911 killagreg 250
                // copter should fly to south (negative nick).
886 killagreg 251
                // In case of a positive east position deviation and a positive east velocity the
252
                // copter should fly to west (positive roll).
911 killagreg 253
                // The influence of the GPS_Nick and GPS_Roll variable is contrarily to the stick values
886 killagreg 254
                // in the fc.c. Therefore a positive north deviation/velocity should result in a positive
911 killagreg 255
                // GPS_Nick and a positive east deviation/velocity should result in a negative GPS_Roll.
886 killagreg 256
 
936 killagreg 257
                coscompass = (int32_t)c_cos_8192(YawGyroHeading / YAW_GYRO_DEG_FACTOR);
258
                sincompass = (int32_t)c_sin_8192(YawGyroHeading / YAW_GYRO_DEG_FACTOR);
950 pangu 259
                PID_Nick =   (coscompass * PID_North + sincompass * PID_East) / 8192;
260
                PID_Roll  =  (sincompass * PID_North - coscompass * PID_East) / 8192;
886 killagreg 261
 
938 killagreg 262
 
886 killagreg 263
                // limit resulting GPS control vector
936 killagreg 264
                GPS_LimitXY(&PID_Nick, &PID_Roll, GPS_STICK_LIMIT);
886 killagreg 265
 
911 killagreg 266
                GPS_Nick = (int16_t)PID_Nick;
936 killagreg 267
                GPS_Roll = (int16_t)PID_Roll;
886 killagreg 268
        }
269
        else // invalid GPS data or bad compass reading
270
        {
271
                GPS_Neutral(); // do nothing
272
                // reset error integral
273
                GPSPosDevIntegral_North = 0;
274
                GPSPosDevIntegral_East = 0;
275
        }
276
}
277
 
278
 
279
 
280
 
936 killagreg 281
void GPS_Main(void)
886 killagreg 282
{
283
        static uint8_t GPS_P_Delay = 0;
938 killagreg 284
        static uint16_t beep_rythm = 0;
886 killagreg 285
 
936 killagreg 286
        GPS_UpdateParameter();
886 killagreg 287
 
936 killagreg 288
        // store home position if start of flight flag is set
289
        if(MKFlags & MKFLAG_CALIBRATE)
290
        {
938 killagreg 291
                if(GPS_SetCurrPosition(&HomePosition)) BeepTime = 700;
936 killagreg 292
        }
886 killagreg 293
 
294
        switch(GPSInfo.status)
295
        {
296
        case INVALID:  // invalid gps data
297
                GPS_Neutral();
936 killagreg 298
                if(FlightMode != GPS_FLIGHT_MODE_FREE)
886 killagreg 299
                {
300
                        BeepTime = 100; // beep if signal is neccesary
301
                }
302
                break;
303
        case PROCESSED: // if gps data are already processed do nothing
304
                // downcount timeout
305
                if(GPSTimeout) GPSTimeout--;
306
                // if no new data arrived within timeout set current data invalid
307
                // and therefore disable GPS
308
                else
309
                {
310
                        GPS_Neutral();
311
                        GPSInfo.status = INVALID;
312
                }
313
                break;
936 killagreg 314
        case NEWDATA: // new valid data from gps device
886 killagreg 315
                // if the gps data quality is good
936 killagreg 316
                beep_rythm++;
317
 
318
                if (GPS_IsSignalOK())
886 killagreg 319
                {
936 killagreg 320
                        switch(FlightMode) // check what's to do
886 killagreg 321
                        {
936 killagreg 322
                                case GPS_FLIGHT_MODE_FREE:
886 killagreg 323
                                        // update hold position to current gps position
936 killagreg 324
                                        GPS_SetCurrPosition(&HoldPosition); // can get invalid if gps signal is bad
886 killagreg 325
                                        // disable gps control
326
                                        GPS_Neutral();
936 killagreg 327
                                        break;
328
 
329
                                case GPS_FLIGHT_MODE_AID:
886 killagreg 330
                                        if(HoldPosition.Status != INVALID)
331
                                        {
936 killagreg 332
                                                if( GPS_IsManualControlled() ) // MK controlled by user
886 killagreg 333
                                                {
334
                                                        // update hold point to current gps position
936 killagreg 335
                                                        GPS_SetCurrPosition(&HoldPosition);
886 killagreg 336
                                                        // disable gps control
337
                                                        GPS_Neutral();
338
                                                        GPS_P_Delay = 0;
339
                                                }
340
                                                else // GPS control active
341
                                                {
936 killagreg 342
                                                        if(GPS_P_Delay < 7)
886 killagreg 343
                                                        { // delayed activation of P-Part for 8 cycles (8*0.25s = 2s)
344
                                                                GPS_P_Delay++;
936 killagreg 345
                                                                GPS_SetCurrPosition(&HoldPosition); // update hold point to current gps position
886 killagreg 346
                                                                GPS_PIDController(NULL); // activates only the D-Part
347
                                                        }
348
                                                        else GPS_PIDController(&HoldPosition);// activates the P&D-Part
349
                                                }
350
                                        }
351
                                        else // invalid Hold Position
352
                                        {  // try to catch a valid hold position from gps data input
936 killagreg 353
                                                GPS_SetCurrPosition(&HoldPosition);
886 killagreg 354
                                                GPS_Neutral();
355
                                        }
936 killagreg 356
                                        break;
357
 
358
                                case GPS_FLIGHT_MODE_HOME:
886 killagreg 359
                                        if(HomePosition.Status != INVALID)
360
                                        {
361
                                                // update hold point to current gps position
362
                                                // to avoid a flight back if home comming is deactivated
936 killagreg 363
                                                GPS_SetCurrPosition(&HoldPosition);
364
                                                if( GPS_IsManualControlled() ) // MK controlled by user
886 killagreg 365
                                                {
366
                                                        GPS_Neutral();
367
                                                }
368
                                                else // GPS control active
369
                                                {
370
                                                        GPS_PIDController(&HomePosition);
371
                                                }
372
                                        }
373
                                        else // bad home position
374
                                        {
375
                                                BeepTime = 50; // signal invalid home position
376
                                                // try to hold at least the position as a fallback option
377
 
378
                                                if (HoldPosition.Status != INVALID)
379
                                                {
936 killagreg 380
                                                        if( GPS_IsManualControlled() ) // MK controlled by user
886 killagreg 381
                                                        {
382
                                                                GPS_Neutral();
383
                                                        }
384
                                                        else // GPS control active
385
                                                        {
386
                                                                GPS_PIDController(&HoldPosition);
387
                                                        }
388
                                                }
389
                                                else
390
                                                { // try to catch a valid hold position
936 killagreg 391
                                                        GPS_SetCurrPosition(&HoldPosition);
886 killagreg 392
                                                        GPS_Neutral();
393
                                                }
394
                                        }
395
                                        break; // eof TSK_HOME
396
                                default: // unhandled task
397
                                        GPS_Neutral();
398
                                        break; // eof default
399
                        } // eof switch GPS_Task
936 killagreg 400
                } // eof gps data quality is good
401
                else // gps data quality is bad
886 killagreg 402
                {       // disable gps control
403
                        GPS_Neutral();
939 killagreg 404
                        if(FlightMode != GPS_FLIGHT_MODE_FREE)
405
                        {
406
                                // beep if signal is not sufficient
407
                                if(!(GPSInfo.flags & FLAG_GPSFIXOK) && !(beep_rythm % 5)) BeepTime = 100;
408
                                else if (GPSInfo.satnum < ParamSet.NaviGpsMinSat && !(beep_rythm % 5)) BeepTime = 10;
409
                        }
886 killagreg 410
                }
411
                // set current data as processed to avoid further calculations on the same gps data
412
                GPSInfo.status = PROCESSED;
413
                break;
414
        } // eof GPSInfo.status
415
}
416