Subversion Repositories FlightCtrl

Rev

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