Subversion Repositories FlightCtrl

Rev

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