Subversion Repositories FlightCtrl

Rev

Rev 966 | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
838 MikeW 1
/*
2
Copyright 2008, by Michael Walter
3
 
4
All functions written by Michael Walter are free software and can be redistributed and/or modified under the terms of the GNU Lesser
5
General Public License as published by the Free Software Foundation. This program is distributed in the hope that it will be useful, but
6
WITHOUT ANY WARRANTY, without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
7
See the GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public
8
License along with this program. If not, see <http://www.gnu.org/licenses/>.
9
 
10
Please note: The software is based on the framework provided by H. Buss and I. Busker in their Mikrokopter projekt. All functions that
11
are not written by Michael Walter are under the license by H. Buss and I. Busker (license_buss.txt) published by www.mikrokopter.de
12
unless it is stated otherwise.
13
*/
14
 
966 MikeW 15
/*****************************************************************************
16
  INCLUDES
17
**************************************************************************** */
838 MikeW 18
#include "main.h"
19
#include "kafi.h"
20
#include "mymath.h"
21
#include <math.h>
22
 
966 MikeW 23
/*****************************************************************************
24
(SYMBOLIC) CONSTANTS
25
*****************************************************************************/
26
 
27
/*****************************************************************************
28
  VARIABLES
29
*****************************************************************************/
30
 
838 MikeW 31
int GPSTracking = 0;
32
int targetPosValid = 0;
33
int homePosValid = 0;
966 MikeW 34
int holdPosValid = 0;
838 MikeW 35
 
966 MikeW 36
volatile gpsInfo_t actualPos;// measured position (last gps record)
37
volatile gpsInfo_t targetPos;// measured position (last gps record)
38
volatile gpsInfo_t homePos;// measured position (last gps record)
39
volatile gpsInfo_t holdPos;   // measured position (last gps record)
40
 
41
NAV_STATUS_t navStatus;
42
NAV_POSLLH_t navPosLlh;
43
NAV_POSUTM_t navPosUtm;
44
NAV_VELNED_t navVelNed;
45
 
46
uint8_t gpsState;
47
 
838 MikeW 48
signed int GPS_Nick = 0;
49
signed int GPS_Roll = 0;
966 MikeW 50
 
838 MikeW 51
long distanceNS = 0;
966 MikeW 52
long distanceEW = 0;
838 MikeW 53
long velocityNS = 0;
54
long velocityEW = 0;
55
unsigned long maxDistance = 0;
56
 
57
int roll_gain = 0;
966 MikeW 58
int nick_gain = 0;
59
int nick_gain_p, nick_gain_d;
60
int roll_gain_p, roll_gain_d;
838 MikeW 61
int Override =  0;
966 MikeW 62
int TargetGier = 0;
63
 
838 MikeW 64
extern int sollGier;
966 MikeW 65
extern int RemoteLinkLost;
838 MikeW 66
 
966 MikeW 67
volatile char*ubxP, *ubxEp, *ubxSp;// pointers to packet currently transfered
68
volatile uint8_t CK_A, CK_B;// UBX checksum bytes
69
volatile uint8_t ignorePacket;// true when previous packet was not processed
70
volatile unsigned short msgLen;
71
volatile uint8_t msgID;
72
 
838 MikeW 73
void GPSscanData (void);
74
void GPSupdate(void);
966 MikeW 75
void SetNewHeading(unsigned long  maxDistance);
838 MikeW 76
 
77
/* ****************************************************************************
78
Functionname:     GPSupdate                      */ /*!
79
Description:
80
 
966 MikeW 81
  @param[in]        
82
 
838 MikeW 83
  @return           void
84
  @pre              -
85
  @post             -
86
  @author           Michael Walter
87
**************************************************************************** */
88
void GPSupdate(void)
89
{
966 MikeW 90
  float SIN_H, COS_H;
91
  long max_p = 0;
92
  long max_d = 0;
93
  int SwitchPos = 0;
94
 
95
  /* Determine Selector Switch Position */
96
  if (PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] < -100)
97
  {
98
    SwitchPos = 0;
99
  }
100
  else if (PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] > 100)
101
  {
102
    SwitchPos = 2;/* Target Mode */
103
  }
104
  else
105
  {
106
    SwitchPos = 1;/* Position Hold */
107
  }
108
 
109
  /* Overide On / Off */
110
  if (abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]])> 10 ||
111
    abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]])> 10 ||
112
    abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]])> 10)
113
  {
114
    Override = 1;
115
  }
116
  else
117
  {
118
    Override = 0;
119
  }
120
 
121
  /* Set Home Position */
122
  if ((actualPos.state > 2) &&
123
    (homePosValid == 0))
124
  {
125
    homePos.north = actualPos.north;
126
    homePos.east = actualPos.east;
127
    homePos.altitude = actualPos.altitude ;
128
    homePosValid = 1;
129
  }
130
 
131
  /* Set Target Position */
132
  if ((actualPos.state > 2) &&
133
    (SwitchPos == 0))
134
  {
135
    targetPos.north = actualPos.north;
136
    targetPos.east = actualPos.east;
137
    targetPos.altitude = actualPos.altitude ;
138
    targetPosValid = 1;
139
  }
140
  if ((actualPos.state < 3) &&
141
    (SwitchPos == 0))
142
  {
143
    targetPosValid = 0;
144
  }
145
 
146
  /* Set Hold Position */
147
  if ((actualPos.state > 2) &&
148
    ((Override == 1) ||
988 mikew 149
    (SwitchPos == 2) )) /* Update the hold position in case we are in target mode */
966 MikeW 150
  {
151
    holdPos.north = actualPos.north;
152
    holdPos.east = actualPos.east;
153
    holdPos.altitude = actualPos.altitude ;
154
    holdPosValid = 1;
155
  }
156
  if ((actualPos.state < 3) &&
157
    (Override == 1))
158
  {
159
    holdPosValid = 0;
160
  }
161
 
162
  /* Indicate Valid GPS Position */
163
  if ((actualPos.state > 2) &&
164
    (SwitchPos == 0))
165
  {
166
    if(BeepMuster == 0xffff)
167
    {
168
      beeptime = 5000;
169
      BeepMuster = 0x0100;
170
    }
171
  }
172
 
173
  if (RemoteLinkLost == 0) /* Disable Heading Update in case we lost the RC - Link */
174
  {
175
    max_p = (MAX(0, (PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 170)) / 20);
176
    max_d = (MAX(0, (PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 170)) / 40);  
177
  }
178
  else
179
  {
180
    max_p = 8;
181
    max_d = 4;
182
  }
183
 
184
  /* Those seem to be good values */
185
  max_p = 8;
186
  max_d = 4;
187
 
188
 //DebugOut.Analog[11] = max_p;
189
 //DebugOut.Analog[12] = max_d;
190
 
191
  static int GPSTrackingCycles = 0;
192
  long maxGainPos = 140;
193
  long maxGainVel = 140;
194
 
195
  /* Ramp up gain */
196
  if (GPSTrackingCycles < 1000)
197
  {
198
    GPSTrackingCycles++;
199
  }
200
  maxGainPos = (maxGainPos * GPSTrackingCycles) / 1000;
201
  maxGainVel = (maxGainVel * GPSTrackingCycles) / 1000;
202
 
203
  /* Determine Offset from nominal Position */
204
  if (actualPos.state > 2 )
205
  {
206
    if ((SwitchPos ==  2) &&
988 mikew 207
      (targetPosValid == 1) &&
208
      (RemoteLinkLost == 0) &&
209
      (Override == 0))
966 MikeW 210
    {  /* determine distance from target position */
211
      distanceNS = actualPos.north - targetPos.north; //  in 0.1m
212
      distanceEW = actualPos.east - targetPos.east; // in 0.1m
213
      velocityNS = actualPos.velNorth; // in 0.1m/s
214
      velocityEW = actualPos.velEast; // in 0.1m/s
215
      maxDistance = sqrt(distanceNS * distanceNS + distanceEW * distanceEW);
216
    }
217
    else if ((SwitchPos ==  1)&&
218
      (holdPosValid == 1) &&
988 mikew 219
      (RemoteLinkLost == 0) &&
220
      (Override == 0))
966 MikeW 221
    {  /* determine distance from hold position */
222
      distanceNS = actualPos.north - holdPos.north; //  in 0.1m
223
      distanceEW = actualPos.east - holdPos.east; // in 0.1m
224
      velocityNS = actualPos.velNorth; // in 0.1m/s
225
      velocityEW = actualPos.velEast; // in 0.1m/s
226
      maxDistance = sqrt(distanceNS * distanceNS + distanceEW * distanceEW);
227
    }
228
    else if ((RemoteLinkLost == 1) &&
229
      (homePosValid ==1)) /* determine distance from target position */
230
    {/* Overide in case the remote link got lost */
231
      distanceNS = actualPos.north - homePos.north; //  in 0.1m
232
      distanceEW = actualPos.east - homePos.east; // in 0.1m
233
      velocityNS = actualPos.velNorth; // in 0.1m/s
234
      velocityEW = actualPos.velEast; // in 0.1m/s
235
      maxDistance = sqrt(distanceNS * distanceNS + distanceEW * distanceEW);
236
    }
237
    else
238
    {
988 mikew 239
      distanceNS = 0.0F; //  in 0.1m
240
      distanceEW = 0.0F; // in 0.1m
241
      velocityNS = 0.0F; // in 0.1m/s
242
      velocityEW = 0.0F; // in 0.1m/s
243
      maxDistance = 0.0F;
244
      GPSTrackingCycles = 0;
966 MikeW 245
    }
246
 
247
    /* Limit GPS_Nick to 25 */
248
    nick_gain_p = (long) MAX(-maxGainPos, MIN(maxGainPos, ((distanceNS* max_p)/50)));  //m
249
    nick_gain_d = (long) MAX(-maxGainVel, MIN(maxGainVel, ((velocityNS  * max_d)/50)));  //m/s
250
    roll_gain_p = (long) MAX(-maxGainPos, MIN(maxGainPos, ((distanceEW * max_p)/50))); //m
251
    roll_gain_d = (long) MAX(-maxGainVel, MIN(maxGainVel, ((velocityEW * max_d)/50)));  //m/s
252
    TargetGier = c_atan2(-distanceEW / 8, -distanceNS / 8) * 10;
253
 
254
    /* PD-Control */
255
    nick_gain = nick_gain_p + nick_gain_d;
256
    roll_gain = -(roll_gain_p + roll_gain_d);
257
 
258
    /* Calculate Distance to Target */
259
    SIN_H = (float) sin(status.Psi);
260
    COS_H =(float) cos(status.Psi);
261
 
262
    GPS_Nick = (int) (COS_H * (float) nick_gain - SIN_H * (float) roll_gain);
263
    GPS_Roll = (int) (SIN_H * (float) nick_gain + COS_H * (float) roll_gain);
264
 
265
    GPS_Nick = MAX(-maxGainPos, MIN(maxGainPos, GPS_Nick));
266
    GPS_Roll = MAX(-maxGainPos, MIN(maxGainPos, GPS_Roll));
267
 
268
    /* Set New Heading */
269
    SetNewHeading(maxDistance);
270
  }
271
  else
272
  {
273
    GPS_Nick = 0;
274
    GPS_Roll = 0;
275
    maxDistance = 0.0F;
276
  }
277
}
838 MikeW 278
 
279
 
280
#if 0
966 MikeW 281
if (maxDistance / 10 < 12)
282
{
283
  static int iState = 0;
284
  switch (iState)
285
  {
286
  case 0:
287
    targetPos.north += 400;
288
    targetPos.east += 0;
289
    GPSTrackingCycles = 0;
290
    iState++;
291
    break;
292
  case 1:
293
    targetPos.north += 0;
294
    targetPos.east += 400;
295
    GPSTrackingCycles = 0;
296
    iState++;
297
    break;
298
  case 2:
299
    targetPos.north -= 400;
300
    targetPos.east += 0;
301
    GPSTrackingCycles = 0;
302
    iState++;
303
    break;
304
  case 3:
305
    targetPos.north += 0;
306
    targetPos.east -= 400;
307
    GPSTrackingCycles = 0;
308
    iState=0;
309
    break;
310
  default:
311
    iState=0;
312
    break;
313
  }
314
  beeptime = 5000;
315
  BeepMuster = 0x0c00;
316
}
838 MikeW 317
#endif
318
 
319
 
966 MikeW 320
void SetNewHeading(unsigned long  maxDistance)
321
/* Set New Heading */
322
{
323
  int OffsetGier = 0;
324
  if (maxDistance / 10 > 8)
325
  {
326
    OffsetGier = 4;
327
  }
328
 
329
  if (TargetGier < 0)
330
  {
331
    TargetGier  += 3600;
332
  }
333
  if (TargetGier > sollGier)
334
  {
335
    if (TargetGier - sollGier < 1800)
336
    {
337
      sollGier += OffsetGier;
338
    }
339
    else
340
    {
341
      sollGier -= OffsetGier;
342
    }
343
  }
344
  else
345
  {
346
    if (sollGier - TargetGier < 1800)
347
    {
348
      sollGier -= OffsetGier;
349
    }
350
    else
351
    {
352
      sollGier += OffsetGier;
353
    }
354
  }
838 MikeW 355
}
356
 
357
/* ****************************************************************************
358
Functionname:     InitGPS                      */ /*!
359
Description:
360
 
361
  @return           void
362
  @pre              -
363
  @post             -
364
  @author          
365
 **************************************************************************** */
366
void InitGPS(void)
367
{
966 MikeW 368
  // USART1 Control and Status Register A, B, C and baud rate register
369
  uint8_t  sreg = SREG;
370
  //uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK/(8 * USART1_BAUD) - 1);
371
 
372
  // disable all interrupts before reconfiguration
373
  cli();
374
 
375
  // disable RX-Interrupt
376
  UCSR1B &= ~(1 << RXCIE1);
377
  // disable TX-Interrupt
378
  UCSR1B &= ~(1 << TXCIE1);
379
  // disable DRE-Interrupt
380
  UCSR1B &= ~(1 << UDRIE1);
381
 
382
  // set direction of RXD1 and TXD1 pins
383
  // set RXD1 (PD2) as an input pin
384
  PORTD |= (1 << PORTD2);
385
  DDRD &= ~(1 << DDD2);
386
 
387
  // set TXD1 (PD3) as an output pin
388
  PORTD |= (1 << PORTD3);
389
  DDRD  |= (1 << DDD3);
390
 
391
  // USART0 Baud Rate Register
392
  // set clock divider
393
  UBRR1H = 0;
394
  UBRR1L = BAUDRATE;
395
 
396
  // enable double speed operation
397
  //UCSR1A |= (1 << U2X1);
398
  UCSR1A = _B0(U2X1);
399
 
400
  // enable receiver and transmitter
401
  UCSR1B = (1 << TXEN1) | (1 << RXEN1);
402
  // set asynchronous mode
403
  UCSR1C &= ~(1 << UMSEL11);
404
  UCSR1C &= ~(1 << UMSEL10);
405
  // no parity
406
  UCSR1C &= ~(1 << UPM11);
407
  UCSR1C &= ~(1 << UPM10);
408
  // 1 stop bit
409
  UCSR1C &= ~(1 << USBS1);
410
  // 8-bit
411
  UCSR1B &= ~(1 << UCSZ12);
412
  UCSR1C |=  (1 << UCSZ11);
413
  UCSR1C |=  (1 << UCSZ10);
414
 
415
  // flush receive buffer explicit
416
  while ( UCSR1A & (1<<RXC1) ) UDR1;
417
 
418
  // enable interrupts at the end
419
  // enable RX-Interrupt
420
  UCSR1B |= (1 << RXCIE1);
421
  // enable TX-Interrupt
422
  UCSR1B |= (1 << TXCIE1);
423
  // enable DRE interrupt
424
  //UCSR1B |= (1 << UDRIE1);
425
 
426
  // restore global interrupt flags
427
  SREG = sreg;
428
 
429
  gpsState = GPS_EMPTY;
838 MikeW 430
}
431
 
432
 
433
/* ****************************************************************************
434
Functionname:     InitGPS                      */ /*!
435
Description:  Copy GPS paket data to actualPos
436
 
437
  @return           void
438
  @pre              -
439
  @post             -
440
  @author          
441
 **************************************************************************** */
442
void GPSscanData (void)
443
{
966 MikeW 444
 if (navStatus.packetStatus == 1)// valid packet
445
  {
446
    actualPos.state = navStatus.GPSfix;
447
    if ((actualPos.state > 1) && (GPSTracking == 0))
448
    {
449
      GRN_FLASH;
450
    }
451
    navStatus.packetStatus = 0;
452
  }
453
 
454
  if (navPosUtm.packetStatus == 1)// valid packet
455
  {
456
    actualPos.north = navPosUtm.NORTH/10L;
457
    actualPos.east = navPosUtm.EAST/10L;
458
    actualPos.altitude = navPosUtm.ALT/100;
459
    actualPos.ITOW = navPosUtm.ITOW;
460
    navPosUtm.packetStatus = 0;
461
  }
462
  /*
463
  if (navPosLlh.packetStatus == 1)
464
  {
465
    actualPos.longi = navPosLlh.LON;    
466
    actualPos.lati = navPosLlh.LAT;      
467
    actualPos.height = navPosLlh.HEIGHT;
468
    navPosLlh.packetStatus = 0;
469
    }
470
  */
471
  if (navVelNed.packetStatus == 1)
472
  {
473
    actualPos.velNorth = navVelNed.VEL_N;
474
    actualPos.velEast = navVelNed.VEL_E;
475
    actualPos.velDown = navVelNed.VEL_D;
476
    actualPos.groundSpeed = navVelNed.GSpeed;
477
    navVelNed.packetStatus = 0;
478
  }
838 MikeW 479
}
480
 
481
/* ****************************************************************************
482
Functionname:     SIGNAL                      */ /*!
483
Description:  
484
 
485
  @return           void
486
  @pre              -
487
  @post             -
488
  @author          
489
 **************************************************************************** */
490
SIGNAL (SIG_USART1_RECV)
491
{
966 MikeW 492
  uint8_t  c;
493
  uint8_t  re;
494
 
495
  re = (UCSR1A & (_B1(FE1) | _B1(DOR1)));// any error occured ?
496
  c = UDR1;
497
 
498
  if (re == 0)
499
  {
500
    switch (gpsState)
501
    {
502
    case GPS_EMPTY:
503
      if (c == SYNC_CHAR1)
504
      {
505
        gpsState = GPS_SYNC1;
506
      }
507
      break;
508
    case GPS_SYNC1:
509
      if (c == SYNC_CHAR2)
510
      {
511
        gpsState = GPS_SYNC2;
512
      }
513
      else if (c != SYNC_CHAR1)
514
      {
515
        gpsState = GPS_EMPTY;
516
      }
517
      break;
518
    case GPS_SYNC2:
519
      if (c == CLASS_NAV)
520
      {
521
        gpsState = GPS_CLASS;
522
      }
523
      else
524
      {
525
        gpsState = GPS_EMPTY;
526
      }
527
      break;
528
    case GPS_CLASS:// msg ID seen: init packed receive
529
      msgID = c;
530
      CK_A = CLASS_NAV + c;
531
      CK_B = CLASS_NAV + CK_A;
532
      gpsState = GPS_LEN1;
533
 
534
      switch (msgID)
535
      {
536
      case MSGID_STATUS:
537
        ubxP = (char*)&navStatus;
538
        ubxEp = (char*)(&navStatus + sizeof(NAV_STATUS_t));
539
        ubxSp = (char*)&navStatus.packetStatus;
540
        ignorePacket = navStatus.packetStatus;
541
        break;
542
        /*
543
        case MSGID_POSLLH:
544
        ubxP = (char*)&navPosLlh;
545
        ubxEp = (char*)(&navPosLlh + sizeof(NAV_POSLLH_t));
546
        ubxSp = (char*)&navPosLlh.packetStatus;
547
        ignorePacket = navPosLlh.packetStatus;
548
        break;
549
        */
550
      case MSGID_POSUTM:
551
        ubxP = (char*)&navPosUtm;
552
        ubxEp = (char*)(&navPosUtm + sizeof(NAV_POSUTM_t));
553
        ubxSp = (char*)&navPosUtm.packetStatus;
554
        ignorePacket = navPosUtm.packetStatus;
555
        break;
556
      case MSGID_VELNED:
557
        ubxP = (char*)&navVelNed;
558
        ubxEp = (char*)(&navVelNed + sizeof(NAV_VELNED_t));
559
        ubxSp = (char*)&navVelNed.packetStatus;
560
        ignorePacket = navVelNed.packetStatus;
561
        break;
562
      default:
563
        ignorePacket = 1;
564
        ubxSp = (char*)0;
565
      }
566
      break;
567
      case GPS_LEN1:// first len byte
568
        msgLen = c;
569
        CK_A += c;
570
        CK_B += CK_A;
571
        gpsState = GPS_LEN2;
572
        break;
573
      case GPS_LEN2:// second len byte
574
        msgLen = msgLen + (c * 256);
575
        CK_A += c;
576
        CK_B += CK_A;
577
        gpsState = GPS_FILLING;// next data will be stored in packet struct
578
        break;
579
      case GPS_FILLING:
580
        CK_A += c;
581
        CK_B += CK_A;
582
 
583
        if ( !ignorePacket && ubxP < ubxEp)
584
        {
585
          *ubxP++ = c;
586
        }
587
 
588
        if (--msgLen == 0)
589
        {
590
          gpsState = GPS_CKA;
591
        }
592
        break;
593
      case GPS_CKA:
594
        if (c == CK_A)
595
        {
596
          gpsState = GPS_CKB;
597
        }
598
        else
599
        {
600
          gpsState = GPS_EMPTY;
601
        }
602
        break;
603
      case GPS_CKB:
604
        if (c == CK_B && ubxSp)// No error -> packet received successfully
605
        {
606
          *ubxSp = 1;// set packetStatus in struct
607
          GPSscanData();
608
        }
609
        gpsState = GPS_EMPTY;// ready for next packet
610
        break;
611
      default:
612
        gpsState = GPS_EMPTY;// ready for next packet
613
    }
614
  }
615
  else// discard any data if error occured
616
  {
617
    gpsState = GPS_EMPTY;
618
  }
838 MikeW 619
}
620
 
621
 
622
 
623