Subversion Repositories FlightCtrl

Rev

Rev 1612 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 1612 Rev 1775
Line 49... Line 49...
49
// +  POSSIBILITY OF SUCH DAMAGE.
49
// +  POSSIBILITY OF SUCH DAMAGE.
50
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
50
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
51
#include <inttypes.h>
51
#include <inttypes.h>
52
#include <stdlib.h>
52
#include <stdlib.h>
53
#include "ubx.h"
53
#include "ubx.h"
54
#include "mymath.h"
54
//#include "mymath.h"
55
#include "timer0.h"
55
#include "timer0.h"
56
#include "uart0.h"
56
#include "uart0.h"
57
#include "rc.h"
57
#include "rc.h"
58
#include "eeprom.h"
58
#include "eeprom.h"
Line 85... Line 85...
85
// the current flight mode
85
// the current flight mode
86
FlightMode_t FlightMode = GPS_FLIGHT_MODE_UNDEF;
86
FlightMode_t FlightMode = GPS_FLIGHT_MODE_UNDEF;
Line 87... Line 87...
87
 
87
 
88
 
88
 
89
// ---------------------------------------------------------------------------------
-
 
90
void GPS_UpdateParameter(void)
89
// ---------------------------------------------------------------------------------
91
{
90
void GPS_UpdateParameter(void) {
92
  static FlightMode_t FlightModeOld = GPS_FLIGHT_MODE_UNDEF;
91
  static FlightMode_t FlightModeOld = GPS_FLIGHT_MODE_UNDEF;
93
 
-
 
94
  if((RC_Quality < 100) || (MKFlags & MKFLAG_EMERGENCY_LANDING))
92
 
95
    {
-
 
96
      FlightMode = GPS_FLIGHT_MODE_FREE;
93
  if((RC_Quality < 100) || (MKFlags & MKFLAG_EMERGENCY_LANDING)) {
97
    }
-
 
98
  else
94
    FlightMode = GPS_FLIGHT_MODE_FREE;
99
    {
95
  } else {
100
      if     (dynamicParams.NaviGpsModeControl <  50) FlightMode = GPS_FLIGHT_MODE_AID;
96
    if     (dynamicParams.NaviGpsModeControl <  50) FlightMode = GPS_FLIGHT_MODE_AID;
101
      else if(dynamicParams.NaviGpsModeControl < 180) FlightMode = GPS_FLIGHT_MODE_FREE;
97
    else if(dynamicParams.NaviGpsModeControl < 180) FlightMode = GPS_FLIGHT_MODE_FREE;
-
 
98
    else                                          FlightMode = GPS_FLIGHT_MODE_HOME;
102
      else                                                FlightMode = GPS_FLIGHT_MODE_HOME;
99
  }
103
    }
-
 
104
  if (FlightMode != FlightModeOld)
100
 
105
    {
101
  if (FlightMode != FlightModeOld) {
106
      BeepTime = 100;
102
    BeepTime = 100;
107
    }
103
  }
Line 108... Line -...
108
  FlightModeOld = FlightMode;
-
 
109
}
-
 
110
 
104
  FlightModeOld = FlightMode;
111
 
105
}
112
 
106
 
113
// ---------------------------------------------------------------------------------
-
 
114
// This function defines a good GPS signal condition
107
// ---------------------------------------------------------------------------------
115
uint8_t GPS_IsSignalOK(void)
108
// This function defines a good GPS signal condition
116
{
-
 
117
  static uint8_t GPSFix = 0;
109
uint8_t GPS_IsSignalOK(void) {
118
  if( (GPSInfo.status != INVALID)  && (GPSInfo.satfix == SATFIX_3D) && (GPSInfo.flags & FLAG_GPSFIXOK) && ((GPSInfo.satnum >= staticParams.NaviGpsMinSat) || GPSFix))
110
  static uint8_t GPSFix = 0;
119
    {
-
 
120
      GPSFix = 1;
111
  if( (GPSInfo.status != INVALID)  && (GPSInfo.satfix == SATFIX_3D) && (GPSInfo.flags & FLAG_GPSFIXOK) && ((GPSInfo.satnum >= staticParams.NaviGpsMinSat) || GPSFix)) {
121
      return(1);
112
    GPSFix = 1;
122
 
-
 
123
    }
113
    return 1;
-
 
114
  }
124
  else return (0);
115
  else return (0);
125
 
116
}
126
}
117
 
127
// ---------------------------------------------------------------------------------
-
 
128
// rescale xy-vector length to  limit
118
// ---------------------------------------------------------------------------------
129
uint8_t GPS_LimitXY(int32_t *x, int32_t *y, int32_t limit)
119
// rescale xy-vector length to  limit
130
{
120
uint8_t GPS_LimitXY(int32_t *x, int32_t *y, int32_t limit) {
131
  uint8_t retval = 0;
121
  uint8_t retval = 0;
132
  int32_t len;
-
 
133
  len = (int32_t)c_sqrt(*x * *x + *y * *y);
122
  int32_t len;
134
  if (len > limit)
123
  len = (int32_t)c_sqrt(*x * *x + *y * *y);
135
    {
124
  if (len > limit) {
136
      // normalize control vector components to the limit
125
    // normalize control vector components to the limit
137
      *x  = (*x  * limit) / len;
126
    *x  = (*x  * limit) / len;
138
      *y  = (*y  * limit) / len;
127
    *y  = (*y  * limit) / len;
139
      retval = 1;
128
    retval = 1;
Line 140... Line 129...
140
    }
129
  }
141
  return(retval);
130
  return(retval);
142
}
-
 
143
 
131
}
144
// checks nick and roll sticks for manual control
132
 
145
uint8_t GPS_IsManualControlled(void)
133
// checks nick and roll sticks for manual control
Line 146... Line 134...
146
{
134
uint8_t GPS_IsManualControlled(void) {
147
  if ( (abs(PPM_in[staticParams.ChannelAssignment[CH_NICK]]) < staticParams.NaviStickThreshold) && (abs(PPM_in[staticParams.ChannelAssignment[CH_ROLL]]) < staticParams.NaviStickThreshold)) return 0;
135
  if ((abs(PPM_in[staticParams.ChannelAssignment[CH_NICK]]) < staticParams.NaviStickThreshold) && (abs(PPM_in[staticParams.ChannelAssignment[CH_ROLL]]) < staticParams.NaviStickThreshold)) return 0;
148
  else return 1;
-
 
149
}
136
  else return 1;
150
 
137
}
Line 151... Line -...
151
// set given position to current gps position
-
 
152
uint8_t GPS_SetCurrPosition(GPS_Pos_t * pGPSPos)
138
 
153
{
139
// set given position to current gps position
154
  uint8_t retval = 0;
140
uint8_t GPS_SetCurrPosition(GPS_Pos_t * pGPSPos) {
155
  if(pGPSPos == NULL) return(retval);   // bad pointer
141
  uint8_t retval = 0;
156
 
142
  if(pGPSPos == NULL) return(retval);   // bad pointer
157
  if(GPS_IsSignalOK())
143
 
158
    {   // is GPS signal condition is fine
-
 
159
      pGPSPos->Longitude        = GPSInfo.longitude;
-
 
160
      pGPSPos->Latitude = GPSInfo.latitude;
144
  if(GPS_IsSignalOK()) {        // is GPS signal condition is fine
161
      pGPSPos->Altitude = GPSInfo.altitude;
145
    pGPSPos->Longitude  = GPSInfo.longitude;
162
      pGPSPos->Status   = NEWDATA;
146
    pGPSPos->Latitude   = GPSInfo.latitude;
163
      retval = 1;
147
    pGPSPos->Altitude   = GPSInfo.altitude;
164
    }
148
    pGPSPos->Status     = NEWDATA;
165
  else
149
    retval = 1;
Line 166... Line 150...
166
    {   // bad GPS signal condition
150
  } else {      // bad GPS signal condition
167
      pGPSPos->Status = INVALID;
151
    pGPSPos->Status = INVALID;
168
      retval = 0;
-
 
169
    }
152
    retval = 0;
-
 
153
  }
170
  return(retval);
154
  return(retval);
171
}
155
}
172
 
-
 
173
// clear position
156
 
174
uint8_t GPS_ClearPosition(GPS_Pos_t * pGPSPos)
157
// clear position
175
{
158
uint8_t GPS_ClearPosition(GPS_Pos_t * pGPSPos) {
176
  uint8_t retval = 0;
159
  uint8_t retval = 0;
177
  if(pGPSPos == NULL) return(retval);   // bad pointer
160
  if(pGPSPos == NULL)
178
  else
161
    return retval;      // bad pointer
179
    {
162
  else {
180
      pGPSPos->Longitude        = 0;
163
    pGPSPos->Longitude  = 0;
Line 181... Line 164...
181
      pGPSPos->Latitude = 0;
164
    pGPSPos->Latitude   = 0;
182
      pGPSPos->Altitude = 0;
165
    pGPSPos->Altitude   = 0;
183
      pGPSPos->Status   = INVALID;
-
 
184
      retval = 1;
166
    pGPSPos->Status     = INVALID;
185
    }
167
    retval = 1;
186
  return (retval);
168
  }
Line 187... Line 169...
187
}
169
  return (retval);
188
 
170
}
189
// disable GPS control sticks
171
 
190
void GPS_Neutral(void)
172
// disable GPS control sticks
191
{
-
 
192
  GPSStickNick = 0;
173
void GPS_Neutral(void) {
193
  GPSStickRoll = 0;
174
  GPSStickNick = 0;
194
}
175
  GPSStickRoll = 0;
195
 
176
}
196
// calculates the GPS control stick values from the deviation to target position
177
 
197
// if the pointer to the target positin is NULL or is the target position invalid
178
// calculates the GPS control stick values from the deviation to target position
198
// then the P part of the controller is deactivated.
179
// if the pointer to the target positin is NULL or is the target position invalid
199
void GPS_PIDController(GPS_Pos_t *pTargetPos)
180
// then the P part of the controller is deactivated.
Line 200... Line 181...
200
{
181
void GPS_PIDController(GPS_Pos_t *pTargetPos) {
308
      PID_Nick =   (coscompass * PID_North + sincompass * PID_East) / 8192;
283
    sincompass = (int32_t)c_sin_8192(YawGyroHeading / GYRO_DEG_FACTOR);
309
      PID_Roll  =  (sincompass * PID_North - coscompass * PID_East) / 8192;
284
    PID_Nick =   (coscompass * PID_North + sincompass * PID_East) / 8192;
310
 
285
    PID_Roll  =  (sincompass * PID_North - coscompass * PID_East) / 8192;
311
 
286
   
312
      // limit resulting GPS control vector
287
    // limit resulting GPS control vector
313
      GPS_LimitXY(&PID_Nick, &PID_Roll, GPS_STICK_LIMIT);
288
    GPS_LimitXY(&PID_Nick, &PID_Roll, GPS_STICK_LIMIT);
314
 
289
   
315
      GPSStickNick = (int16_t)PID_Nick;
290
    GPSStickNick = (int16_t)PID_Nick;
Line 316... Line -...
316
      GPSStickRoll = (int16_t)PID_Roll;
-
 
317
    }
-
 
318
  else // invalid GPS data or bad compass reading
-
 
319
    {
291
    GPSStickRoll = (int16_t)PID_Roll;
320
      GPS_Neutral(); // do nothing
-
 
321
      // reset error integral
292
  }
322
      GPSPosDevIntegral_North = 0;
293
  else // invalid GPS data or bad compass reading
Line 323... Line 294...
323
      GPSPosDevIntegral_East = 0;
294
    {
Line 324... Line 295...
324
    }
295
      GPS_Neutral(); // do nothing
325
}
296
      // reset error integral
326
 
-
 
327
 
297
      GPSPosDevIntegral_North = 0;
328
 
298
      GPSPosDevIntegral_East = 0;
329
 
299
    }
330
void GPS_Main(void)
300
}
331
{
-
 
332
  static uint8_t GPS_P_Delay = 0;
301
 
333
  static uint16_t beep_rythm = 0;
302
void GPS_Main(void) {
334
 
303
  static uint8_t GPS_P_Delay = 0;
335
  GPS_UpdateParameter();
-
 
336
 
304
  static uint16_t beep_rythm = 0;
337
  // store home position if start of flight flag is set
305
 
338
  if(MKFlags & MKFLAG_CALIBRATE)
306
  GPS_UpdateParameter();
339
    {
307
 
340
      if(GPS_SetCurrPosition(&HomePosition)) BeepTime = 700;
308
  // store home position if start of flight flag is set
341
    }
309
  if(MKFlags & MKFLAG_CALIBRATE) {
342
 
310
    if(GPS_SetCurrPosition(&HomePosition)) BeepTime = 700;
343
  switch(GPSInfo.status)
311
  }
344
    {
312
 
345
    case INVALID:  // invalid gps data
313
  switch(GPSInfo.status) {
346
      GPS_Neutral();
314
  case INVALID:  // invalid gps data
347
      if(FlightMode != GPS_FLIGHT_MODE_FREE)
315
    GPS_Neutral();
348
        {
316
    if(FlightMode != GPS_FLIGHT_MODE_FREE) {
349
          BeepTime = 100; // beep if signal is neccesary
317
      BeepTime = 100; // beep if signal is neccesary
350
        }
318
    }
351
      break;
319
    break;
352
    case PROCESSED: // if gps data are already processed do nothing
320
  case PROCESSED: // if gps data are already processed do nothing
353
      // downcount timeout
321
    // downcount timeout
354
      if(GPSTimeout) GPSTimeout--;
322
    if(GPSTimeout) GPSTimeout--;
355
      // if no new data arrived within timeout set current data invalid
323
    // if no new data arrived within timeout set current data invalid
356
      // and therefore disable GPS
324
    // and therefore disable GPS
357
      else
325
    else
358
        {
326
      {
359
          GPS_Neutral();
327
        GPS_Neutral();
360
          GPSInfo.status = INVALID;
328
          GPSInfo.status = INVALID;
361
        }
329
      }
362
      break;
330
    break;
363
    case NEWDATA: // new valid data from gps device
331
  case NEWDATA: // new valid data from gps device
364
      // if the gps data quality is good
332
    // if the gps data quality is good
365
      beep_rythm++;
333
    beep_rythm++;
366
 
334
   
367
      if (GPS_IsSignalOK())
335
    if (GPS_IsSignalOK())
368
        {
336
      {
369
          switch(FlightMode) // check what's to do
337
        switch(FlightMode) // check what's to do
370
            {
338
          {
371
            case GPS_FLIGHT_MODE_FREE:
339
          case GPS_FLIGHT_MODE_FREE:
372
              // update hold position to current gps position
340
            // update hold position to current gps position
373
              GPS_SetCurrPosition(&HoldPosition); // can get invalid if gps signal is bad
341
            GPS_SetCurrPosition(&HoldPosition); // can get invalid if gps signal is bad
374
              // disable gps control
342
            // disable gps control
375
              GPS_Neutral();
343
            GPS_Neutral();
376
              break;
344
            break;
377
 
345
           
378
            case GPS_FLIGHT_MODE_AID:
346
          case GPS_FLIGHT_MODE_AID:
379
              if(HoldPosition.Status != INVALID)
347
            if(HoldPosition.Status != INVALID)
380
                {
348
              {
381
                  if( GPS_IsManualControlled() ) // MK controlled by user
349
                if( GPS_IsManualControlled() ) // MK controlled by user
382
                    {
350
                  {
383
                      // update hold point to current gps position
351
                    // update hold point to current gps position
384
                      GPS_SetCurrPosition(&HoldPosition);
352
                    GPS_SetCurrPosition(&HoldPosition);
385
                      // disable gps control
353
                    // disable gps control
386
                      GPS_Neutral();
354
                    GPS_Neutral();
387
                      GPS_P_Delay = 0;
355
                    GPS_P_Delay = 0;
388
                    }
356
                  }
389
                  else // GPS control active
357
                else // GPS control active
390
                    {
358
                  {
391
                      if(GPS_P_Delay < 7)
359
                    if(GPS_P_Delay < 7)
392
                        { // delayed activation of P-Part for 8 cycles (8*0.25s = 2s)
360
                      { // delayed activation of P-Part for 8 cycles (8*0.25s = 2s)
393
                          GPS_P_Delay++;
361
                        GPS_P_Delay++;
394
                          GPS_SetCurrPosition(&HoldPosition); // update hold point to current gps position
362
                        GPS_SetCurrPosition(&HoldPosition); // update hold point to current gps position
395
                          GPS_PIDController(NULL); // activates only the D-Part
363
                        GPS_PIDController(NULL); // activates only the D-Part
396
                        }
364
                      }
397
                      else GPS_PIDController(&HoldPosition);// activates the P&D-Part
365
                    else GPS_PIDController(&HoldPosition);// activates the P&D-Part
398
                    }
366
                  }
399
                }
367
              }
400
              else // invalid Hold Position
368
            else // invalid Hold Position
401
                {  // try to catch a valid hold position from gps data input
369
              {  // try to catch a valid hold position from gps data input
402
                  GPS_SetCurrPosition(&HoldPosition);
370
                GPS_SetCurrPosition(&HoldPosition);
403
                  GPS_Neutral();
371
                GPS_Neutral();
404
                }
372
              }
405
              break;
373
            break;
406
 
374
           
407
            case GPS_FLIGHT_MODE_HOME:
375
          case GPS_FLIGHT_MODE_HOME:
408
              if(HomePosition.Status != INVALID)
376
            if(HomePosition.Status != INVALID)
409
                {
377
              {
410
                  // update hold point to current gps position
378
                // update hold point to current gps position
411
                  // to avoid a flight back if home comming is deactivated
379
                // to avoid a flight back if home comming is deactivated
412
                  GPS_SetCurrPosition(&HoldPosition);
380
                GPS_SetCurrPosition(&HoldPosition);
413
                  if( GPS_IsManualControlled() ) // MK controlled by user
381
                if( GPS_IsManualControlled() ) // MK controlled by user
414
                    {
382
                  {
415
                      GPS_Neutral();
383
                    GPS_Neutral();
416
                    }
384
                  }
417
                  else // GPS control active
385
                else // GPS control active
418
                    {
386
                  {
419
                      GPS_PIDController(&HomePosition);
387
                    GPS_PIDController(&HomePosition);
420
                    }
388
                  }
421
                }
389
              }
422
              else // bad home position
390
            else // bad home position
423
                {
391
              {
424
                  BeepTime = 50; // signal invalid home position
392
                BeepTime = 50; // signal invalid home position
425
                  // try to hold at least the position as a fallback option
393
                // try to hold at least the position as a fallback option
426
 
394
               
427
                  if (HoldPosition.Status != INVALID)
395
                if (HoldPosition.Status != INVALID)
428
                    {
396
                  {
429
                      if( GPS_IsManualControlled() ) // MK controlled by user
397
                    if( GPS_IsManualControlled() ) // MK controlled by user
430
                        {
398
                      {
431
                          GPS_Neutral();
399
                        GPS_Neutral();
432
                        }
400
                      }
433
                      else // GPS control active
401
                    else // GPS control active
434
                        {
402
                      {
435
                          GPS_PIDController(&HoldPosition);
403
                        GPS_PIDController(&HoldPosition);
436
                        }
404
                      }
437
                    }
405
                  }
438
                  else
406
                else
439
                    { // try to catch a valid hold position
407
                  { // try to catch a valid hold position
440
                      GPS_SetCurrPosition(&HoldPosition);
408
                    GPS_SetCurrPosition(&HoldPosition);
441
                      GPS_Neutral();
409
                    GPS_Neutral();
442
                    }
410
                  }
443
                }
411
              }
444
              break; // eof TSK_HOME
412
            break; // eof TSK_HOME
445
            default: // unhandled task
413
          default: // unhandled task
446
              GPS_Neutral();
414
            GPS_Neutral();
447
              break; // eof default
415
            break; // eof default
448
            } // eof switch GPS_Task
416
          } // eof switch GPS_Task
449
        } // eof gps data quality is good
417
      } // eof gps data quality is good
450
      else // gps data quality is bad
418
    else // gps data quality is bad
451
        {       // disable gps control
419
      { // disable gps control