Subversion Repositories FlightCtrl

Rev

Details | Last modification | View Log | RSS feed

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