Subversion Repositories FlightCtrl

Rev

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

Rev Author Line No. Line
2039 - 1
// Navigation with a GPS directly attached to the FC's UART1.
2
 
3
#include <inttypes.h>
4
#include <stdlib.h>
5
#include <stddef.h>
6
#include "ubx.h"
7
#include "configuration.h"
8
#include "controlMixer.h"
9
#include "output.h"
10
#include "isqrt.h"
11
#include "attitude.h"
12
#include "dongfangMath.h"
2048 - 13
#include "attitude.h"
2039 - 14
 
15
typedef enum {
2074 - 16
  NAVI_FLIGHT_MODE_UNDEF,
17
  NAVI_FLIGHT_MODE_FREE,
18
  NAVI_FLIGHT_MODE_AID,
19
  NAVI_FLIGHT_MODE_HOME,
2039 - 20
} FlightMode_t;
21
 
2088 - 22
/*
23
 * This is not read from internally. It only serves to let a pilot/debugger
24
 * know the status of the navigation system (thru a data connection).
25
 */
2074 - 26
typedef enum {
2088 - 27
  NAVI_STATUS_NO_COMPASS=-1,
28
  NAVI_STATUS_INVALID_GPS=-2,
29
  NAVI_STATUS_BAD_GPS_SIGNAL=-3,
30
  NAVI_STATUS_RTH_POSITION_INVALID=-4,
31
  NAVI_STATUS_RTH_FALLBACK_ON_HOLD=-5,
32
  NAVI_STATUS_GPS_TIMEOUT=-6,
2074 - 33
  NAVI_STATUS_FREEFLIGHT=0,
2088 - 34
  NAVI_STATUS_POSITION_HOLD=1,
35
  NAVI_STATUS_RTH=2,
36
  NAVI_STATUS_MANUAL_OVERRIDE=3,
37
  NAVI_STATUS_HOLD_POSITION_INVALID=4,
2074 - 38
} NaviStatus_t;
39
 
2039 - 40
#define GPS_POSINTEGRAL_LIMIT 32000
2058 - 41
#define LOG_NAVI_STICK_GAIN 3
2046 - 42
#define GPS_P_LIMIT                     100
2039 - 43
 
44
typedef struct {
45
  int32_t longitude;
46
  int32_t latitude;
47
  int32_t altitude;
48
  Status_t status;
49
} GPS_Pos_t;
50
 
51
// GPS coordinates for hold position
52
GPS_Pos_t holdPosition = { 0, 0, 0, INVALID };
53
// GPS coordinates for home position
54
GPS_Pos_t homePosition = { 0, 0, 0, INVALID };
55
// the current flight mode
2074 - 56
FlightMode_t flightMode = NAVI_FLIGHT_MODE_UNDEF;
2058 - 57
int16_t naviSticks[2] = {0,0};
2039 - 58
 
2088 - 59
int8_t naviStatus;
2074 - 60
 
2039 - 61
// ---------------------------------------------------------------------------------
2046 - 62
void navi_updateFlightMode(void) {
2074 - 63
  static FlightMode_t flightModeOld = NAVI_FLIGHT_MODE_UNDEF;
2039 - 64
 
2058 - 65
  if (MKFlags & MKFLAG_EMERGENCY_FLIGHT) {
2074 - 66
    flightMode = NAVI_FLIGHT_MODE_FREE;
2039 - 67
  } else {
2045 - 68
    if (dynamicParams.naviMode < 50)
2074 - 69
      flightMode = NAVI_FLIGHT_MODE_FREE;
2045 - 70
    else if (dynamicParams.naviMode < 180)
2074 - 71
      flightMode = NAVI_FLIGHT_MODE_AID;
2039 - 72
    else
2074 - 73
      flightMode = NAVI_FLIGHT_MODE_HOME;
2039 - 74
  }
75
 
76
  if (flightMode != flightModeOld) {
77
    beep(100);
78
    flightModeOld = flightMode;
79
  }
80
}
81
 
82
// ---------------------------------------------------------------------------------
83
// This function defines a good GPS signal condition
2046 - 84
uint8_t navi_isGPSSignalOK(void) {
2039 - 85
  static uint8_t GPSFix = 0;
86
  if ((GPSInfo.status != INVALID) && (GPSInfo.satfix == SATFIX_3D)
87
      && (GPSInfo.flags & FLAG_GPSFIXOK)
88
      && ((GPSInfo.satnum >= staticParams.GPSMininumSatellites) || GPSFix)) {
89
    GPSFix = 1;
90
    return 1;
91
  } else
92
    return (0);
93
}
94
 
95
// ---------------------------------------------------------------------------------
96
// rescale xy-vector length to  limit
2046 - 97
uint8_t navi_limitXY(int32_t *x, int32_t *y, int32_t limit) {
2039 - 98
  int32_t len;
99
  len = isqrt32(*x * *x + *y * *y);
100
  if (len > limit) {
101
    // normalize control vector components to the limit
102
    *x = (*x * limit) / len;
103
    *y = (*y * limit) / len;
104
    return 1;
105
  }
106
  return 0;
107
}
108
 
109
// checks nick and roll sticks for manual control
2048 - 110
uint8_t navi_isManuallyControlled(int16_t* PRTY) {
2076 - 111
  debugOut.analog[26] = PRTY[CONTROL_PITCH];
112
  debugOut.analog[27] = PRTY[CONTROL_ROLL];
113
  if (abs(PRTY[CONTROL_PITCH]) < staticParams.naviStickThreshold
114
      && abs(PRTY[CONTROL_ROLL]) < staticParams.naviStickThreshold)
2039 - 115
    return 0;
116
  else
117
    return 1;
118
}
119
 
120
// set given position to current gps position
2046 - 121
uint8_t navi_writeCurrPositionTo(GPS_Pos_t * pGPSPos) {
2039 - 122
  if (pGPSPos == NULL)
2044 - 123
    return 0; // bad pointer
2039 - 124
 
2046 - 125
  if (navi_isGPSSignalOK()) { // is GPS signal condition is fine
2039 - 126
    pGPSPos->longitude = GPSInfo.longitude;
127
    pGPSPos->latitude = GPSInfo.latitude;
128
    pGPSPos->altitude = GPSInfo.altitude;
129
    pGPSPos->status = NEWDATA;
2044 - 130
    return 1;
2039 - 131
  } else { // bad GPS signal condition
132
    pGPSPos->status = INVALID;
2044 - 133
    return 0;
2039 - 134
  }
135
}
136
 
137
// clear position
2046 - 138
uint8_t navi_clearPosition(GPS_Pos_t * pGPSPos) {
2039 - 139
  if (pGPSPos == NULL)
2044 - 140
    return 0; // bad pointer
2039 - 141
  else {
142
    pGPSPos->longitude = 0;
143
    pGPSPos->latitude = 0;
144
    pGPSPos->altitude = 0;
145
    pGPSPos->status = INVALID;
146
  }
2044 - 147
  return 1;
2039 - 148
}
149
 
2058 - 150
void navi_setNeutral(void) {
151
  naviSticks[CONTROL_PITCH] = naviSticks[CONTROL_ROLL] = 0;
152
}
153
 
2039 - 154
// calculates the GPS control stick values from the deviation to target position
155
// if the pointer to the target positin is NULL or is the target position invalid
156
// then the P part of the controller is deactivated.
2058 - 157
void navi_PIDController(GPS_Pos_t *pTargetPos) {
158
  static int32_t PID_Pitch, PID_Roll;
2039 - 159
  int32_t coscompass, sincompass;
160
  int32_t GPSPosDev_North, GPSPosDev_East; // Position deviation in cm
2044 - 161
  int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0, I_North = 0, I_East = 0;
2039 - 162
  int32_t PID_North = 0, PID_East = 0;
163
  static int32_t cos_target_latitude = 1;
164
  static int32_t GPSPosDevIntegral_North = 0, GPSPosDevIntegral_East = 0;
165
  static GPS_Pos_t *pLastTargetPos = 0;
166
 
167
  // if GPS data and Compass are ok
2046 - 168
  if (navi_isGPSSignalOK() && (magneticHeading >= 0)) {
2044 - 169
    if (pTargetPos != NULL) { // if there is a target position
170
      if (pTargetPos->status != INVALID) { // and the position data are valid
2039 - 171
        // if the target data are updated or the target pointer has changed
2086 - 172
        if ((pTargetPos->status != PROCESSED) || (pTargetPos != pLastTargetPos)) {
2039 - 173
          // reset error integral
174
          GPSPosDevIntegral_North = 0;
175
          GPSPosDevIntegral_East = 0;
176
          // recalculate latitude projection
2045 - 177
          cos_target_latitude = cos_360(pTargetPos->latitude / 10000000L);
2039 - 178
          // remember last target pointer
179
          pLastTargetPos = pTargetPos;
180
          // mark data as processed
181
          pTargetPos->status = PROCESSED;
182
        }
183
        // calculate position deviation from latitude and longitude differences
184
        GPSPosDev_North = (GPSInfo.latitude - pTargetPos->latitude); // to calculate real cm we would need *111/100 additionally
185
        GPSPosDev_East = (GPSInfo.longitude - pTargetPos->longitude); // to calculate real cm we would need *111/100 additionally
186
        // calculate latitude projection
187
        GPSPosDev_East *= cos_target_latitude;
2047 - 188
        GPSPosDev_East >>= LOG_MATH_UNIT_FACTOR;
2039 - 189
      } else { // no valid target position available
190
        // reset error
191
        GPSPosDev_North = 0;
192
        GPSPosDev_East = 0;
193
        // reset error integral
194
        GPSPosDevIntegral_North = 0;
195
        GPSPosDevIntegral_East = 0;
196
      }
197
    } else { // no target position available
198
      // reset error
199
      GPSPosDev_North = 0;
200
      GPSPosDev_East = 0;
201
      // reset error integral
202
      GPSPosDevIntegral_North = 0;
203
      GPSPosDevIntegral_East = 0;
204
    }
205
 
206
    //Calculate PID-components of the controller
207
    // D-Part
2046 - 208
    D_North = ((int32_t) staticParams.naviD * GPSInfo.velnorth) >> 9;
209
    D_East = ((int32_t) staticParams.naviD * GPSInfo.veleast) >> 9;
2039 - 210
 
211
    // P-Part
2046 - 212
    P_North = ((int32_t) staticParams.naviP * GPSPosDev_North) >> 11;
213
    P_East = ((int32_t) staticParams.naviP * GPSPosDev_East) >> 11;
2039 - 214
 
215
    // I-Part
2046 - 216
    I_North = ((int32_t) staticParams.naviI * GPSPosDevIntegral_North) >> 13;
217
    I_East = ((int32_t) staticParams.naviI * GPSPosDevIntegral_East) >> 13;
2039 - 218
 
219
    // combine P & I
220
    PID_North = P_North + I_North;
221
    PID_East = P_East + I_East;
2058 - 222
 
2046 - 223
    if (!navi_limitXY(&PID_North, &PID_East, GPS_P_LIMIT)) {
224
      // within limit
225
      GPSPosDevIntegral_North += GPSPosDev_North >> 4;
226
      GPSPosDevIntegral_East += GPSPosDev_East >> 4;
227
      navi_limitXY(&GPSPosDevIntegral_North, &GPSPosDevIntegral_East, GPS_POSINTEGRAL_LIMIT);
2039 - 228
    }
229
 
230
    // combine PI- and D-Part
231
    PID_North += D_North;
232
    PID_East += D_East;
233
 
234
    // scale combination with gain.
235
    // dongfang: Lets not do that. P I and D can be scaled instead.
236
    // PID_North = (PID_North * (int32_t) staticParams.NaviGpsGain) / 100;
237
    // PID_East = (PID_East * (int32_t) staticParams.NaviGpsGain) / 100;
238
 
239
    // GPS to nick and roll settings
240
    // A positive nick angle moves head downwards (flying forward).
241
    // A positive roll angle tilts left side downwards (flying left).
242
    // If compass heading is 0 the head of the copter is in north direction.
243
    // A positive nick angle will fly to north and a positive roll angle will fly to west.
244
    // In case of a positive north deviation/velocity the
245
    // copter should fly to south (negative nick).
246
    // In case of a positive east position deviation and a positive east velocity the
247
    // copter should fly to west (positive roll).
248
    // The influence of the GPSStickNick and GPSStickRoll variable is contrarily to the stick values
249
    // in the flight.c. Therefore a positive north deviation/velocity should result in a positive
250
    // GPSStickNick and a positive east deviation/velocity should result in a negative GPSStickRoll.
251
 
2048 - 252
    coscompass = -cos_360(heading / GYRO_DEG_FACTOR_YAW);
253
    sincompass = -sin_360(heading / GYRO_DEG_FACTOR_YAW);
2044 - 254
 
2058 - 255
    PID_Pitch = (coscompass * PID_North + sincompass * PID_East) >> (LOG_MATH_UNIT_FACTOR-LOG_NAVI_STICK_GAIN);
2047 - 256
    PID_Roll = (sincompass * PID_North - coscompass * PID_East) >> (LOG_MATH_UNIT_FACTOR-LOG_NAVI_STICK_GAIN);
2039 - 257
 
258
    // limit resulting GPS control vector
2058 - 259
    navi_limitXY(&PID_Pitch, &PID_Roll, staticParams.naviStickLimit << LOG_NAVI_STICK_GAIN);
2039 - 260
 
2058 - 261
    naviSticks[CONTROL_PITCH] = PID_Pitch;
262
    naviSticks[CONTROL_ROLL] = PID_Roll;
2039 - 263
  } else { // invalid GPS data or bad compass reading
264
    // reset error integral
2058 - 265
    navi_setNeutral();
2039 - 266
    GPSPosDevIntegral_North = 0;
267
    GPSPosDevIntegral_East = 0;
268
  }
269
}
270
 
2048 - 271
void navigation_periodicTaskAndPRTY(int16_t* PRTY) {
2039 - 272
  static uint8_t GPS_P_Delay = 0;
273
  static uint16_t beep_rythm = 0;
2086 - 274
  static uint16_t navi_testOscPrescaler = 0;
275
  static uint8_t  navi_testOscTimer = 0;
276
 
2088 - 277
  if (!(staticParams.bitConfig & CFG_COMPASS_ENABLED)) {
278
    navi_setNeutral();
279
    naviStatus = NAVI_STATUS_NO_COMPASS;
280
    return;
281
  }
282
 
283
  navi_updateFlightMode();
284
 
2086 - 285
  navi_testOscPrescaler++;
286
  if (navi_testOscPrescaler == 488) {
2088 - 287
      navi_testOscPrescaler = 0;
288
      navi_testOscTimer++;
289
      if (navi_testOscTimer == staticParams.naviTestOscPeriod) {
290
          navi_testOscTimer = 0;
291
          if (staticParams.naviTestOscAmplitude) {
292
              holdPosition.status = NEWDATA;
293
              holdPosition.latitude += staticParams.naviTestOscAmplitude * 90L; // there are about 90 units per meter.
294
          }
295
      } else if (navi_testOscTimer == staticParams.naviTestOscPeriod/2) {
296
          if (staticParams.naviTestOscAmplitude) {
297
              holdPosition.status = NEWDATA;
298
              holdPosition.latitude -= staticParams.naviTestOscAmplitude * 90L;
299
          }
300
      }
2086 - 301
  }
302
 
2039 - 303
  // store home position if start of flight flag is set
304
  if (MKFlags & MKFLAG_CALIBRATE) {
2044 - 305
    MKFlags &= ~(MKFLAG_CALIBRATE);
2046 - 306
    if (navi_writeCurrPositionTo(&homePosition)) {
2074 - 307
        // shift north to simulate an offset.
2058 - 308
        // homePosition.latitude += 10000L;
2045 - 309
        beep(500);
310
      }
311
    }
2039 - 312
 
313
  switch (GPSInfo.status) {
314
  case INVALID: // invalid gps data
2058 - 315
    navi_setNeutral();
2074 - 316
    naviStatus = NAVI_STATUS_INVALID_GPS;
317
    if (flightMode != NAVI_FLIGHT_MODE_FREE) {
2071 - 318
      beep(1); // beep if signal is neccesary
2039 - 319
    }
320
    break;
321
  case PROCESSED: // if gps data are already processed do nothing
322
    // downcount timeout
323
    if (GPSTimeout)
324
      GPSTimeout--;
325
    // if no new data arrived within timeout set current data invalid
326
    // and therefore disable GPS
327
    else {
2058 - 328
      navi_setNeutral();
2039 - 329
      GPSInfo.status = INVALID;
2074 - 330
      naviStatus = NAVI_STATUS_GPS_TIMEOUT;
2039 - 331
    }
332
    break;
333
  case NEWDATA: // new valid data from gps device
334
    // if the gps data quality is good
335
    beep_rythm++;
2046 - 336
    if (navi_isGPSSignalOK()) {
2039 - 337
      switch (flightMode) { // check what's to do
2074 - 338
      case NAVI_FLIGHT_MODE_FREE:
2039 - 339
        // update hold position to current gps position
2046 - 340
        navi_writeCurrPositionTo(&holdPosition); // can get invalid if gps signal is bad
2039 - 341
        // disable gps control
2058 - 342
        navi_setNeutral();
2074 - 343
        naviStatus = NAVI_STATUS_FREEFLIGHT;
2039 - 344
        break;
345
 
2074 - 346
      case NAVI_FLIGHT_MODE_AID:
2039 - 347
        if (holdPosition.status != INVALID) {
2048 - 348
          if (navi_isManuallyControlled(PRTY)) { // MK controlled by user
2039 - 349
            // update hold point to current gps position
2046 - 350
            navi_writeCurrPositionTo(&holdPosition);
2039 - 351
            // disable gps control
2058 - 352
            navi_setNeutral();
2039 - 353
            GPS_P_Delay = 0;
2074 - 354
            naviStatus = NAVI_STATUS_MANUAL_OVERRIDE;
2039 - 355
          } else { // GPS control active
356
            if (GPS_P_Delay < 7) {
357
              // delayed activation of P-Part for 8 cycles (8*0.25s = 2s)
358
              GPS_P_Delay++;
2046 - 359
              navi_writeCurrPositionTo(&holdPosition); // update hold point to current gps position
2058 - 360
              navi_PIDController(NULL); // activates only the D-Part
2074 - 361
              naviStatus = NAVI_STATUS_POSITION_HOLD;
362
            } else {
2058 - 363
              navi_PIDController(&holdPosition); // activates the P&D-Part
2074 - 364
              naviStatus = NAVI_STATUS_POSITION_HOLD;
365
            }
2039 - 366
          }
2058 - 367
        } else { // invalid Hold Position
368
        // try to catch a valid hold position from gps data input
2046 - 369
          navi_writeCurrPositionTo(&holdPosition);
2058 - 370
          navi_setNeutral();
2074 - 371
          naviStatus = NAVI_STATUS_HOLD_POSITION_INVALID;
2039 - 372
        }
373
        break;
374
 
2074 - 375
      case NAVI_FLIGHT_MODE_HOME:
2039 - 376
        if (homePosition.status != INVALID) {
377
          // update hold point to current gps position
378
          // to avoid a flight back if home comming is deactivated
2046 - 379
          navi_writeCurrPositionTo(&holdPosition);
2058 - 380
          if (navi_isManuallyControlled(PRTY)) { // MK controlled by user
381
            navi_setNeutral();
2076 - 382
            naviStatus = NAVI_STATUS_MANUAL_OVERRIDE;
2039 - 383
          } else {// GPS control active
2058 - 384
            navi_PIDController(&homePosition);
2039 - 385
          }
2074 - 386
          naviStatus = NAVI_STATUS_RTH;
2039 - 387
        } else {
388
          // bad home position
389
          beep(50); // signal invalid home position
390
          // try to hold at least the position as a fallback option
391
          if (holdPosition.status != INVALID) {
2048 - 392
            if (navi_isManuallyControlled(PRTY)) {
2039 - 393
              // MK controlled by user
2058 - 394
              navi_setNeutral();
2074 - 395
              naviStatus = NAVI_STATUS_MANUAL_OVERRIDE;
2039 - 396
            } else {
397
              // GPS control active
2058 - 398
              navi_PIDController(&holdPosition);
2074 - 399
              naviStatus = NAVI_STATUS_RTH_FALLBACK_ON_HOLD;
2039 - 400
            }
401
          } else { // try to catch a valid hold position
2046 - 402
            navi_writeCurrPositionTo(&holdPosition);
2074 - 403
            naviStatus = NAVI_STATUS_RTH_POSITION_INVALID;
2058 - 404
            navi_setNeutral();
2039 - 405
          }
406
        }
407
        break; // eof TSK_HOME
408
      default: // unhandled task
2058 - 409
        navi_setNeutral();
2039 - 410
        break; // eof default
411
      } // eof switch GPS_Task
412
    } // eof gps data quality is good
2058 - 413
    else { // gps data quality is bad
414
      // disable gps control
415
      navi_setNeutral();
2074 - 416
      naviStatus = NAVI_STATUS_BAD_GPS_SIGNAL;
417
      if (flightMode != NAVI_FLIGHT_MODE_FREE) {
2039 - 418
        // beep if signal is not sufficient
419
        if (!(GPSInfo.flags & FLAG_GPSFIXOK) && !(beep_rythm % 5))
420
          beep(100);
421
        else if (GPSInfo.satnum < staticParams.GPSMininumSatellites
422
            && !(beep_rythm % 5))
423
          beep(10);
424
      }
425
    }
426
    // set current data as processed to avoid further calculations on the same gps data
427
    GPSInfo.status = PROCESSED;
428
    break;
429
  } // eof GPSInfo.status
2058 - 430
 
431
  PRTY[CONTROL_PITCH] += naviSticks[CONTROL_PITCH];
432
  PRTY[CONTROL_ROLL] += naviSticks[CONTROL_ROLL];
2074 - 433
 
434
  debugOut.analog[16] = flightMode;
435
  debugOut.analog[17] = naviStatus;
436
 
437
  debugOut.analog[18] = naviSticks[CONTROL_PITCH];
438
  debugOut.analog[19] = naviSticks[CONTROL_ROLL];
2039 - 439
}