WebSVN - FlightCtrl - Diff - Rev 966 and 988 - /branches/KalmanFilter MikeW/gps.c


/*
Copyright 2008, by Michael Walter
 
All functions written by Michael Walter are free software and can be redistributed and/or modified under the terms of the GNU Lesser
General Public License as published by the Free Software Foundation. This program is distributed in the hope that it will be useful, but
WITHOUT ANY WARRANTY, without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
See the GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public
License along with this program. If not, see <http://www.gnu.org/licenses/>.
 
Please note: The software is based on the framework provided by H. Buss and I. Busker in their Mikrokopter projekt. All functions that
are not written by Michael Walter are under the license by H. Buss and I. Busker (license_buss.txt) published by www.mikrokopter.de
unless it is stated otherwise.
*/
 
/*****************************************************************************
  INCLUDES
**************************************************************************** */
#include "main.h"
#include "kafi.h"
#include "mymath.h"
#include <math.h>
 
/*****************************************************************************
(SYMBOLIC) CONSTANTS
*****************************************************************************/
 
/*****************************************************************************
  VARIABLES
*****************************************************************************/
 
int GPSTracking = 0;
int targetPosValid = 0;
int homePosValid = 0;
int holdPosValid = 0;
 
volatile gpsInfo_t actualPos;// measured position (last gps record)
volatile gpsInfo_t targetPos;// measured position (last gps record)
volatile gpsInfo_t homePos;// measured position (last gps record)
volatile gpsInfo_t holdPos;   // measured position (last gps record)
 
NAV_STATUS_t navStatus;
NAV_POSLLH_t navPosLlh;
NAV_POSUTM_t navPosUtm;
NAV_VELNED_t navVelNed;
 
uint8_t gpsState;
 
signed int GPS_Nick = 0;
signed int GPS_Roll = 0;
 
long distanceNS = 0;
long distanceEW = 0;
long velocityNS = 0;
long velocityEW = 0;
unsigned long maxDistance = 0;
 
int roll_gain = 0;
int nick_gain = 0;
int nick_gain_p, nick_gain_d;
int roll_gain_p, roll_gain_d;
int Override =  0;
int TargetGier = 0;
 
extern int sollGier;
extern int RemoteLinkLost;
 
volatile char*ubxP, *ubxEp, *ubxSp;// pointers to packet currently transfered
volatile uint8_t CK_A, CK_B;// UBX checksum bytes
volatile uint8_t ignorePacket;// true when previous packet was not processed
volatile unsigned short msgLen;
volatile uint8_t msgID;
 
void GPSscanData (void);
void GPSupdate(void);
void SetNewHeading(unsigned long  maxDistance);
 
/* ****************************************************************************
Functionname:     GPSupdate                      */ /*!
Description:
 
  @param[in]        
 
  @return           void
  @pre              -
  @post             -
  @author           Michael Walter
**************************************************************************** */
void GPSupdate(void)
{
  float SIN_H, COS_H;
  long max_p = 0;
  long max_d = 0;
  int SwitchPos = 0;
 
  /* Determine Selector Switch Position */
  if (PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] < -100)
  {
    SwitchPos = 0;
  }
  else if (PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] > 100)
  {
    SwitchPos = 2;/* Target Mode */
  }
  else
  {
    SwitchPos = 1;/* Position Hold */
  }
 
  /* Overide On / Off */
  if (abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]])> 10 ||
    abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]])> 10 ||
    abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]])> 10)
  {
    Override = 1;
  }
  else
  {
    Override = 0;
  }
 
  /* Set Home Position */
  if ((actualPos.state > 2) &&
    (homePosValid == 0))
  {
    homePos.north = actualPos.north;
    homePos.east = actualPos.east;
    homePos.altitude = actualPos.altitude ;
    homePosValid = 1;
  }
 
  /* Set Target Position */
  if ((actualPos.state > 2) &&
    (SwitchPos == 0))
  {
    targetPos.north = actualPos.north;
    targetPos.east = actualPos.east;
    targetPos.altitude = actualPos.altitude ;
    targetPosValid = 1;
  }
  if ((actualPos.state < 3) &&
    (SwitchPos == 0))
  {
    targetPosValid = 0;
  }
 
  /* Set Hold Position */
  if ((actualPos.state > 2) &&
    ((Override == 1) ||
    (SwitchPos == 2) )) /* Update the hold position in case we are in target mode */
  {
    holdPos.north = actualPos.north;
    holdPos.east = actualPos.east;
    holdPos.altitude = actualPos.altitude ;
    holdPosValid = 1;
  }
  if ((actualPos.state < 3) &&
    (Override == 1))
  {
    holdPosValid = 0;
  }
 
  /* Indicate Valid GPS Position */
  if ((actualPos.state > 2) &&
    (SwitchPos == 0))
  {
    if(BeepMuster == 0xffff)
    {
      beeptime = 5000;
      BeepMuster = 0x0100;
    }
  }
 
  if (RemoteLinkLost == 0) /* Disable Heading Update in case we lost the RC - Link */
  {
    max_p = (MAX(0, (PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 170)) / 20);
    max_d = (MAX(0, (PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 170)) / 40);  
  }
  else
  {
    max_p = 8;
    max_d = 4;
  }
 
  /* Those seem to be good values */
  max_p = 8;
  max_d = 4;
 
 //DebugOut.Analog[11] = max_p;
 //DebugOut.Analog[12] = max_d;
 
  static int GPSTrackingCycles = 0;
  long maxGainPos = 140;
  long maxGainVel = 140;
 
  /* Ramp up gain */
  if (GPSTrackingCycles < 1000)
  {
    GPSTrackingCycles++;
  }
  maxGainPos = (maxGainPos * GPSTrackingCycles) / 1000;
  maxGainVel = (maxGainVel * GPSTrackingCycles) / 1000;
 
  /* Determine Offset from nominal Position */
  if (actualPos.state > 2 )
  {
    if ((SwitchPos ==  2) &&
      (targetPosValid == 1) &&
      (RemoteLinkLost == 0) &&
      (Override == 0))
    {  /* determine distance from target position */
      distanceNS = actualPos.north - targetPos.north; //  in 0.1m
      distanceEW = actualPos.east - targetPos.east; // in 0.1m
      velocityNS = actualPos.velNorth; // in 0.1m/s
      velocityEW = actualPos.velEast; // in 0.1m/s
      maxDistance = sqrt(distanceNS * distanceNS + distanceEW * distanceEW);
    }
    else if ((SwitchPos ==  1)&&
      (holdPosValid == 1) &&
      (RemoteLinkLost == 0) &&
      (Override == 0))
    {  /* determine distance from hold position */
      distanceNS = actualPos.north - holdPos.north; //  in 0.1m
      distanceEW = actualPos.east - holdPos.east; // in 0.1m
      velocityNS = actualPos.velNorth; // in 0.1m/s
      velocityEW = actualPos.velEast; // in 0.1m/s
      maxDistance = sqrt(distanceNS * distanceNS + distanceEW * distanceEW);
    }
    else if ((RemoteLinkLost == 1) &&
      (homePosValid ==1)) /* determine distance from target position */
    {/* Overide in case the remote link got lost */
      distanceNS = actualPos.north - homePos.north; //  in 0.1m
      distanceEW = actualPos.east - homePos.east; // in 0.1m
      velocityNS = actualPos.velNorth; // in 0.1m/s
      velocityEW = actualPos.velEast; // in 0.1m/s
      maxDistance = sqrt(distanceNS * distanceNS + distanceEW * distanceEW);
    }
    else
    {
      distanceNS = 0.0F; //  in 0.1m
      distanceEW = 0.0F; // in 0.1m
      velocityNS = 0.0F; // in 0.1m/s
      velocityEW = 0.0F; // in 0.1m/s
      maxDistance = 0.0F;
      GPSTrackingCycles = 0;
    }
   
    /* Limit GPS_Nick to 25 */
    nick_gain_p = (long) MAX(-maxGainPos, MIN(maxGainPos, ((distanceNS* max_p)/50)));  //m
    nick_gain_d = (long) MAX(-maxGainVel, MIN(maxGainVel, ((velocityNS  * max_d)/50)));  //m/s
    roll_gain_p = (long) MAX(-maxGainPos, MIN(maxGainPos, ((distanceEW * max_p)/50))); //m
    roll_gain_d = (long) MAX(-maxGainVel, MIN(maxGainVel, ((velocityEW * max_d)/50)));  //m/s
    TargetGier = c_atan2(-distanceEW / 8, -distanceNS / 8) * 10;
   
    /* PD-Control */
    nick_gain = nick_gain_p + nick_gain_d;
    roll_gain = -(roll_gain_p + roll_gain_d);
   
    /* Calculate Distance to Target */
    SIN_H = (float) sin(status.Psi);
    COS_H =(float) cos(status.Psi);
   
    GPS_Nick = (int) (COS_H * (float) nick_gain - SIN_H * (float) roll_gain);
    GPS_Roll = (int) (SIN_H * (float) nick_gain + COS_H * (float) roll_gain);
   
    GPS_Nick = MAX(-maxGainPos, MIN(maxGainPos, GPS_Nick));
    GPS_Roll = MAX(-maxGainPos, MIN(maxGainPos, GPS_Roll));
   
    /* Set New Heading */
    SetNewHeading(maxDistance);
  }
  else
  {
    GPS_Nick = 0;
    GPS_Roll = 0;
    maxDistance = 0.0F;
  }
}
 
 
#if 0
if (maxDistance / 10 < 12)
{
  static int iState = 0;
  switch (iState)
  {
  case 0:
    targetPos.north += 400;
    targetPos.east += 0;
    GPSTrackingCycles = 0;
    iState++;
    break;
  case 1:
    targetPos.north += 0;
    targetPos.east += 400;
    GPSTrackingCycles = 0;
    iState++;
    break;
  case 2:
    targetPos.north -= 400;
    targetPos.east += 0;
    GPSTrackingCycles = 0;
    iState++;
    break;
  case 3:
    targetPos.north += 0;
    targetPos.east -= 400;
    GPSTrackingCycles = 0;
    iState=0;
    break;
  default:
    iState=0;
    break;
  }
  beeptime = 5000;
  BeepMuster = 0x0c00;
}
#endif
 
 
void SetNewHeading(unsigned long  maxDistance)
/* Set New Heading */
{
  int OffsetGier = 0;
  if (maxDistance / 10 > 8)
  {
    OffsetGier = 4;
  }
 
  if (TargetGier < 0)
  {
    TargetGier  += 3600;
  }
  if (TargetGier > sollGier)
  {
    if (TargetGier - sollGier < 1800)
    {
      sollGier += OffsetGier;
    }
    else
    {
      sollGier -= OffsetGier;
    }
  }
  else
  {
    if (sollGier - TargetGier < 1800)
    {
      sollGier -= OffsetGier;
    }
    else
    {
      sollGier += OffsetGier;
    }
  }
}
 
/* ****************************************************************************
Functionname:     InitGPS                      */ /*!
Description:
 
  @return           void
  @pre              -
  @post             -
  @author          
 **************************************************************************** */
void InitGPS(void)
{
  // USART1 Control and Status Register A, B, C and baud rate register
  uint8_t  sreg = SREG;
  //uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK/(8 * USART1_BAUD) - 1);
 
  // disable all interrupts before reconfiguration
  cli();
 
  // disable RX-Interrupt
  UCSR1B &= ~(1 << RXCIE1);
  // disable TX-Interrupt
  UCSR1B &= ~(1 << TXCIE1);
  // disable DRE-Interrupt
  UCSR1B &= ~(1 << UDRIE1);
 
  // set direction of RXD1 and TXD1 pins
  // set RXD1 (PD2) as an input pin
  PORTD |= (1 << PORTD2);
  DDRD &= ~(1 << DDD2);
 
  // set TXD1 (PD3) as an output pin
  PORTD |= (1 << PORTD3);
  DDRD  |= (1 << DDD3);
 
  // USART0 Baud Rate Register
  // set clock divider
  UBRR1H = 0;
  UBRR1L = BAUDRATE;
 
  // enable double speed operation
  //UCSR1A |= (1 << U2X1);
  UCSR1A = _B0(U2X1);
 
  // enable receiver and transmitter
  UCSR1B = (1 << TXEN1) | (1 << RXEN1);
  // set asynchronous mode
  UCSR1C &= ~(1 << UMSEL11);
  UCSR1C &= ~(1 << UMSEL10);
  // no parity
  UCSR1C &= ~(1 << UPM11);
  UCSR1C &= ~(1 << UPM10);
  // 1 stop bit
  UCSR1C &= ~(1 << USBS1);
  // 8-bit
  UCSR1B &= ~(1 << UCSZ12);
  UCSR1C |=  (1 << UCSZ11);
  UCSR1C |=  (1 << UCSZ10);
 
  // flush receive buffer explicit
  while ( UCSR1A & (1<<RXC1) ) UDR1;
 
  // enable interrupts at the end
  // enable RX-Interrupt
  UCSR1B |= (1 << RXCIE1);
  // enable TX-Interrupt
  UCSR1B |= (1 << TXCIE1);
  // enable DRE interrupt
  //UCSR1B |= (1 << UDRIE1);
 
  // restore global interrupt flags
  SREG = sreg;
 
  gpsState = GPS_EMPTY;
}
 
 
/* ****************************************************************************
Functionname:     InitGPS                      */ /*!
Description:  Copy GPS paket data to actualPos
 
  @return           void
  @pre              -
  @post             -
  @author          
 **************************************************************************** */
void GPSscanData (void)
{
 if (navStatus.packetStatus == 1)// valid packet
  {
    actualPos.state = navStatus.GPSfix;
    if ((actualPos.state > 1) && (GPSTracking == 0))
    {
      GRN_FLASH;
    }
    navStatus.packetStatus = 0;
  }
 
  if (navPosUtm.packetStatus == 1)// valid packet
  {
    actualPos.north = navPosUtm.NORTH/10L;
    actualPos.east = navPosUtm.EAST/10L;
    actualPos.altitude = navPosUtm.ALT/100;
    actualPos.ITOW = navPosUtm.ITOW;
    navPosUtm.packetStatus = 0;
  }
  /*
  if (navPosLlh.packetStatus == 1)
  {
    actualPos.longi = navPosLlh.LON;    
    actualPos.lati = navPosLlh.LAT;      
    actualPos.height = navPosLlh.HEIGHT;
    navPosLlh.packetStatus = 0;
    }
  */
  if (navVelNed.packetStatus == 1)
  {
    actualPos.velNorth = navVelNed.VEL_N;
    actualPos.velEast = navVelNed.VEL_E;
    actualPos.velDown = navVelNed.VEL_D;
    actualPos.groundSpeed = navVelNed.GSpeed;
    navVelNed.packetStatus = 0;
  }
}
 
/* ****************************************************************************
Functionname:     SIGNAL                      */ /*!
Description:  
 
  @return           void
  @pre              -
  @post             -
  @author          
 **************************************************************************** */
SIGNAL (SIG_USART1_RECV)
{
  uint8_t  c;
  uint8_t  re;
 
  re = (UCSR1A & (_B1(FE1) | _B1(DOR1)));// any error occured ?
  c = UDR1;
 
  if (re == 0)
  {
    switch (gpsState)
    {
    case GPS_EMPTY:
      if (c == SYNC_CHAR1)
      {
        gpsState = GPS_SYNC1;
      }
      break;
    case GPS_SYNC1:
      if (c == SYNC_CHAR2)
      {
        gpsState = GPS_SYNC2;
      }
      else if (c != SYNC_CHAR1)
      {
        gpsState = GPS_EMPTY;
      }
      break;
    case GPS_SYNC2:
      if (c == CLASS_NAV)
      {
        gpsState = GPS_CLASS;
      }
      else
      {
        gpsState = GPS_EMPTY;
      }
      break;
    case GPS_CLASS:// msg ID seen: init packed receive
      msgID = c;
      CK_A = CLASS_NAV + c;
      CK_B = CLASS_NAV + CK_A;
      gpsState = GPS_LEN1;
     
      switch (msgID)
      {
      case MSGID_STATUS:
        ubxP = (char*)&navStatus;
        ubxEp = (char*)(&navStatus + sizeof(NAV_STATUS_t));
        ubxSp = (char*)&navStatus.packetStatus;
        ignorePacket = navStatus.packetStatus;
        break;
        /*
        case MSGID_POSLLH:
        ubxP = (char*)&navPosLlh;
        ubxEp = (char*)(&navPosLlh + sizeof(NAV_POSLLH_t));
        ubxSp = (char*)&navPosLlh.packetStatus;
        ignorePacket = navPosLlh.packetStatus;
        break;
        */
      case MSGID_POSUTM:
        ubxP = (char*)&navPosUtm;
        ubxEp = (char*)(&navPosUtm + sizeof(NAV_POSUTM_t));
        ubxSp = (char*)&navPosUtm.packetStatus;
        ignorePacket = navPosUtm.packetStatus;
        break;
      case MSGID_VELNED:
        ubxP = (char*)&navVelNed;
        ubxEp = (char*)(&navVelNed + sizeof(NAV_VELNED_t));
        ubxSp = (char*)&navVelNed.packetStatus;
        ignorePacket = navVelNed.packetStatus;
        break;
      default:
        ignorePacket = 1;
        ubxSp = (char*)0;
      }
      break;
      case GPS_LEN1:// first len byte
        msgLen = c;
        CK_A += c;
        CK_B += CK_A;
        gpsState = GPS_LEN2;
        break;
      case GPS_LEN2:// second len byte
        msgLen = msgLen + (c * 256);
        CK_A += c;
        CK_B += CK_A;
        gpsState = GPS_FILLING;// next data will be stored in packet struct
        break;
      case GPS_FILLING:
        CK_A += c;
        CK_B += CK_A;
       
        if ( !ignorePacket && ubxP < ubxEp)
        {
          *ubxP++ = c;
        }
       
        if (--msgLen == 0)
        {
          gpsState = GPS_CKA;
        }
        break;
      case GPS_CKA:
        if (c == CK_A)
        {
          gpsState = GPS_CKB;
        }
        else
        {
          gpsState = GPS_EMPTY;
        }
        break;
      case GPS_CKB:
        if (c == CK_B && ubxSp)// No error -> packet received successfully
        {
          *ubxSp = 1;// set packetStatus in struct
          GPSscanData();
        }
        gpsState = GPS_EMPTY;// ready for next packet
        break;
      default:
        gpsState = GPS_EMPTY;// ready for next packet
    }
  }
  else// discard any data if error occured
  {
    gpsState = GPS_EMPTY;
  }
}
 
 
 
 
 

Rev 966	Rev 988
1	/*	1	/*
2	Copyright 2008, by Michael Walter	2	Copyright 2008, by Michael Walter
3		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	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	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.	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	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/>.	8	License along with this program. If not, see <http://www.gnu.org/licenses/>.
9		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	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	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.	12	unless it is stated otherwise.
13	*/	13	*/
14		14
15	/*****************************************************************************	15	/*****************************************************************************
16	INCLUDES	16	INCLUDES
17	**************************************************************************** */	17	**************************************************************************** */
18	#include "main.h"	18	#include "main.h"
19	#include "kafi.h"	19	#include "kafi.h"
20	#include "mymath.h"	20	#include "mymath.h"
21	#include <math.h>	21	#include <math.h>
22		22
23	/*****************************************************************************	23	/*****************************************************************************
24	(SYMBOLIC) CONSTANTS	24	(SYMBOLIC) CONSTANTS
25	*****************************************************************************/	25	*****************************************************************************/
26		26
27	/*****************************************************************************	27	/*****************************************************************************
28	VARIABLES	28	VARIABLES
29	*****************************************************************************/	29	*****************************************************************************/
30		30
31	int GPSTracking = 0;	31	int GPSTracking = 0;
32	int targetPosValid = 0;	32	int targetPosValid = 0;
33	int homePosValid = 0;	33	int homePosValid = 0;
34	int holdPosValid = 0;	34	int holdPosValid = 0;
35		35
36	volatile gpsInfo_t actualPos;// measured position (last gps record)	36	volatile gpsInfo_t actualPos;// measured position (last gps record)
37	volatile gpsInfo_t targetPos;// 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)	38	volatile gpsInfo_t homePos;// measured position (last gps record)
39	volatile gpsInfo_t holdPos; // measured position (last gps record)	39	volatile gpsInfo_t holdPos; // measured position (last gps record)
40		40
41	NAV_STATUS_t navStatus;	41	NAV_STATUS_t navStatus;
42	NAV_POSLLH_t navPosLlh;	42	NAV_POSLLH_t navPosLlh;
43	NAV_POSUTM_t navPosUtm;	43	NAV_POSUTM_t navPosUtm;
44	NAV_VELNED_t navVelNed;	44	NAV_VELNED_t navVelNed;
45		45
46	uint8_t gpsState;	46	uint8_t gpsState;
47		47
48	signed int GPS_Nick = 0;	48	signed int GPS_Nick = 0;
49	signed int GPS_Roll = 0;	49	signed int GPS_Roll = 0;
50		50
51	long distanceNS = 0;	51	long distanceNS = 0;
52	long distanceEW = 0;	52	long distanceEW = 0;
53	long velocityNS = 0;	53	long velocityNS = 0;
54	long velocityEW = 0;	54	long velocityEW = 0;
55	unsigned long maxDistance = 0;	55	unsigned long maxDistance = 0;
56		56
57	int roll_gain = 0;	57	int roll_gain = 0;
58	int nick_gain = 0;	58	int nick_gain = 0;
59	int nick_gain_p, nick_gain_d;	59	int nick_gain_p, nick_gain_d;
60	int roll_gain_p, roll_gain_d;	60	int roll_gain_p, roll_gain_d;
61	int Override = 0;	61	int Override = 0;
62	int TargetGier = 0;	62	int TargetGier = 0;
63		63
64	extern int sollGier;	64	extern int sollGier;
65	extern int RemoteLinkLost;	65	extern int RemoteLinkLost;
66		66
67	volatile charubxP, ubxEp, *ubxSp;// pointers to packet currently transfered	67	volatile charubxP, ubxEp, *ubxSp;// pointers to packet currently transfered
68	volatile uint8_t CK_A, CK_B;// UBX checksum bytes	68	volatile uint8_t CK_A, CK_B;// UBX checksum bytes
69	volatile uint8_t ignorePacket;// true when previous packet was not processed	69	volatile uint8_t ignorePacket;// true when previous packet was not processed
70	volatile unsigned short msgLen;	70	volatile unsigned short msgLen;
71	volatile uint8_t msgID;	71	volatile uint8_t msgID;
72		72
73	void GPSscanData (void);	73	void GPSscanData (void);
74	void GPSupdate(void);	74	void GPSupdate(void);
75	void SetNewHeading(unsigned long maxDistance);	75	void SetNewHeading(unsigned long maxDistance);
76		76
77	/* ****************************************************************************	77	/* ****************************************************************************
78	Functionname: GPSupdate / /!	78	Functionname: GPSupdate / /!
79	Description:	79	Description:
80		80
81	@param[in]	81	@param[in]
82		82
83	@return void	83	@return void
84	@pre -	84	@pre -
85	@post -	85	@post -
86	@author Michael Walter	86	@author Michael Walter
87	**************************************************************************** */	87	**************************************************************************** */
88	void GPSupdate(void)	88	void GPSupdate(void)
89	{	89	{
90	float SIN_H, COS_H;	90	float SIN_H, COS_H;
91	long max_p = 0;	91	long max_p = 0;
92	long max_d = 0;	92	long max_d = 0;
93	int SwitchPos = 0;	93	int SwitchPos = 0;
94		94
95	/* Determine Selector Switch Position */	95	/* Determine Selector Switch Position */
96	if (PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] < -100)	96	if (PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] < -100)
97	{	97	{
98	SwitchPos = 0;	98	SwitchPos = 0;
99	}	99	}
100	else if (PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] > 100)	100	else if (PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] > 100)
101	{	101	{
102	SwitchPos = 2;/* Target Mode */	102	SwitchPos = 2;/* Target Mode */
103	}	103	}
104	else	104	else
105	{	105	{
106	SwitchPos = 1;/* Position Hold */	106	SwitchPos = 1;/* Position Hold */
107	}	107	}
108		108
109	/* Overide On / Off */	109	/* Overide On / Off */
110	if (abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]])> 10 \|\|	110	if (abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]])> 10 \|\|
111	abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]])> 10 \|\|	111	abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]])> 10 \|\|
112	abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]])> 10)	112	abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]])> 10)
113	{	113	{
114	Override = 1;	114	Override = 1;
115	}	115	}
116	else	116	else
117	{	117	{
118	Override = 0;	118	Override = 0;
119	}	119	}
120		120
121	/* Set Home Position */	121	/* Set Home Position */
122	if ((actualPos.state > 2) &&	122	if ((actualPos.state > 2) &&
123	(homePosValid == 0))	123	(homePosValid == 0))
124	{	124	{
125	homePos.north = actualPos.north;	125	homePos.north = actualPos.north;
126	homePos.east = actualPos.east;	126	homePos.east = actualPos.east;
127	homePos.altitude = actualPos.altitude ;	127	homePos.altitude = actualPos.altitude ;
128	homePosValid = 1;	128	homePosValid = 1;
129	}	129	}
130		130
131	/* Set Target Position */	131	/* Set Target Position */
132	if ((actualPos.state > 2) &&	132	if ((actualPos.state > 2) &&
133	(SwitchPos == 0))	133	(SwitchPos == 0))
134	{	134	{
135	targetPos.north = actualPos.north;	135	targetPos.north = actualPos.north;
136	targetPos.east = actualPos.east;	136	targetPos.east = actualPos.east;
137	targetPos.altitude = actualPos.altitude ;	137	targetPos.altitude = actualPos.altitude ;
138	targetPosValid = 1;	138	targetPosValid = 1;
139	}	139	}
140	if ((actualPos.state < 3) &&	140	if ((actualPos.state < 3) &&
141	(SwitchPos == 0))	141	(SwitchPos == 0))
142	{	142	{
143	targetPosValid = 0;	143	targetPosValid = 0;
144	}	144	}
145		145
146	/* Set Hold Position */	146	/* Set Hold Position */
147	if ((actualPos.state > 2) &&	147	if ((actualPos.state > 2) &&
148	((Override == 1) \|\|	148	((Override == 1) \|\|
149	(SwitchPos == 2) )) /* Update the hold position in case we are in target mode */	149	(SwitchPos == 2) )) /* Update the hold position in case we are in target mode */
150	{	150	{
151	holdPos.north = actualPos.north;	151	holdPos.north = actualPos.north;
152	holdPos.east = actualPos.east;	152	holdPos.east = actualPos.east;
153	holdPos.altitude = actualPos.altitude ;	153	holdPos.altitude = actualPos.altitude ;
154	holdPosValid = 1;	154	holdPosValid = 1;
155	}	155	}
156	if ((actualPos.state < 3) &&	156	if ((actualPos.state < 3) &&
157	(Override == 1))	157	(Override == 1))
158	{	158	{
159	holdPosValid = 0;	159	holdPosValid = 0;
160	}	160	}
161		161
162	/* Indicate Valid GPS Position */	162	/* Indicate Valid GPS Position */
163	if ((actualPos.state > 2) &&	163	if ((actualPos.state > 2) &&
164	(SwitchPos == 0))	164	(SwitchPos == 0))
165	{	165	{
166	if(BeepMuster == 0xffff)	166	if(BeepMuster == 0xffff)
167	{	167	{
168	beeptime = 5000;	168	beeptime = 5000;
169	BeepMuster = 0x0100;	169	BeepMuster = 0x0100;
170	}	170	}
171	}	171	}
172		172
173	if (RemoteLinkLost == 0) /* Disable Heading Update in case we lost the RC - Link */	173	if (RemoteLinkLost == 0) /* Disable Heading Update in case we lost the RC - Link */
174	{	174	{
175	max_p = (MAX(0, (PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 170)) / 20);	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);	176	max_d = (MAX(0, (PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 170)) / 40);
177	}	177	}
178	else	178	else
179	{	179	{
180	max_p = 8;	180	max_p = 8;
181	max_d = 4;	181	max_d = 4;
182	}	182	}
183		183
184	/* Those seem to be good values */	184	/* Those seem to be good values */
185	max_p = 8;	185	max_p = 8;
186	max_d = 4;	186	max_d = 4;
187		187
188	//DebugOut.Analog[11] = max_p;	188	//DebugOut.Analog[11] = max_p;
189	//DebugOut.Analog[12] = max_d;	189	//DebugOut.Analog[12] = max_d;
190		190
191	static int GPSTrackingCycles = 0;	191	static int GPSTrackingCycles = 0;
192	long maxGainPos = 140;	192	long maxGainPos = 140;
193	long maxGainVel = 140;	193	long maxGainVel = 140;
194		194
195	/* Ramp up gain */	195	/* Ramp up gain */
196	if (GPSTrackingCycles < 1000)	196	if (GPSTrackingCycles < 1000)
197	{	197	{
198	GPSTrackingCycles++;	198	GPSTrackingCycles++;
199	}	199	}
200	maxGainPos = (maxGainPos * GPSTrackingCycles) / 1000;	200	maxGainPos = (maxGainPos * GPSTrackingCycles) / 1000;
201	maxGainVel = (maxGainVel * GPSTrackingCycles) / 1000;	201	maxGainVel = (maxGainVel * GPSTrackingCycles) / 1000;
202		202
203	/* Determine Offset from nominal Position */	203	/* Determine Offset from nominal Position */
204	if (actualPos.state > 2 )	204	if (actualPos.state > 2 )
205	{	205	{
206	if ((SwitchPos == 2) &&	206	if ((SwitchPos == 2) &&
207	(targetPosValid == 1) &&	207	(targetPosValid == 1) &&
208	(RemoteLinkLost == 0) &&	208	(RemoteLinkLost == 0) &&
209	(Override == 0))	209	(Override == 0))
210	{ /* determine distance from target position */	210	{ /* determine distance from target position */
211	distanceNS = actualPos.north - targetPos.north; // in 0.1m	211	distanceNS = actualPos.north - targetPos.north; // in 0.1m
212	distanceEW = actualPos.east - targetPos.east; // in 0.1m	212	distanceEW = actualPos.east - targetPos.east; // in 0.1m
213	velocityNS = actualPos.velNorth; // in 0.1m/s	213	velocityNS = actualPos.velNorth; // in 0.1m/s
214	velocityEW = actualPos.velEast; // in 0.1m/s	214	velocityEW = actualPos.velEast; // in 0.1m/s
215	maxDistance = sqrt(distanceNS * distanceNS + distanceEW * distanceEW);	215	maxDistance = sqrt(distanceNS * distanceNS + distanceEW * distanceEW);
216	}	216	}
217	else if ((SwitchPos == 1)&&	217	else if ((SwitchPos == 1)&&
218	(holdPosValid == 1) &&	218	(holdPosValid == 1) &&
219	(RemoteLinkLost == 0) &&	219	(RemoteLinkLost == 0) &&
220	(Override == 0))	220	(Override == 0))
221	{ /* determine distance from hold position */	221	{ /* determine distance from hold position */
222	distanceNS = actualPos.north - holdPos.north; // in 0.1m	222	distanceNS = actualPos.north - holdPos.north; // in 0.1m
223	distanceEW = actualPos.east - holdPos.east; // in 0.1m	223	distanceEW = actualPos.east - holdPos.east; // in 0.1m
224	velocityNS = actualPos.velNorth; // in 0.1m/s	224	velocityNS = actualPos.velNorth; // in 0.1m/s
225	velocityEW = actualPos.velEast; // in 0.1m/s	225	velocityEW = actualPos.velEast; // in 0.1m/s
226	maxDistance = sqrt(distanceNS * distanceNS + distanceEW * distanceEW);	226	maxDistance = sqrt(distanceNS * distanceNS + distanceEW * distanceEW);
227	}	227	}
228	else if ((RemoteLinkLost == 1) &&	228	else if ((RemoteLinkLost == 1) &&
229	(homePosValid ==1)) /* determine distance from target position */	229	(homePosValid ==1)) /* determine distance from target position */
230	{/* Overide in case the remote link got lost */	230	{/* Overide in case the remote link got lost */
231	distanceNS = actualPos.north - homePos.north; // in 0.1m	231	distanceNS = actualPos.north - homePos.north; // in 0.1m
232	distanceEW = actualPos.east - homePos.east; // in 0.1m	232	distanceEW = actualPos.east - homePos.east; // in 0.1m
233	velocityNS = actualPos.velNorth; // in 0.1m/s	233	velocityNS = actualPos.velNorth; // in 0.1m/s
234	velocityEW = actualPos.velEast; // in 0.1m/s	234	velocityEW = actualPos.velEast; // in 0.1m/s
235	maxDistance = sqrt(distanceNS * distanceNS + distanceEW * distanceEW);	235	maxDistance = sqrt(distanceNS * distanceNS + distanceEW * distanceEW);
236	}	236	}
237	else	237	else
238	{	238	{
239	distanceNS = 0.0F; // in 0.1m	239	distanceNS = 0.0F; // in 0.1m
240	distanceEW = 0.0F; // in 0.1m	240	distanceEW = 0.0F; // in 0.1m
241	velocityNS = 0.0F; // in 0.1m/s	241	velocityNS = 0.0F; // in 0.1m/s
242	velocityEW = 0.0F; // in 0.1m/s	242	velocityEW = 0.0F; // in 0.1m/s
243	maxDistance = 0.0F;	243	maxDistance = 0.0F;
244	GPSTrackingCycles = 0;	244	GPSTrackingCycles = 0;
245	}	245	}
246		246
247	/* Limit GPS_Nick to 25 */	247	/* Limit GPS_Nick to 25 */
248	nick_gain_p = (long) MAX(-maxGainPos, MIN(maxGainPos, ((distanceNS* max_p)/50))); //m	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	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	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	251	roll_gain_d = (long) MAX(-maxGainVel, MIN(maxGainVel, ((velocityEW * max_d)/50))); //m/s
252	TargetGier = c_atan2(-distanceEW / 8, -distanceNS / 8) * 10;	252	TargetGier = c_atan2(-distanceEW / 8, -distanceNS / 8) * 10;
253		253
254	/* PD-Control */	254	/* PD-Control */
255	nick_gain = nick_gain_p + nick_gain_d;	255	nick_gain = nick_gain_p + nick_gain_d;
256	roll_gain = -(roll_gain_p + roll_gain_d);	256	roll_gain = -(roll_gain_p + roll_gain_d);
257		257
258	/* Calculate Distance to Target */	258	/* Calculate Distance to Target */
259	SIN_H = (float) sin(status.Psi);	259	SIN_H = (float) sin(status.Psi);
260	COS_H =(float) cos(status.Psi);	260	COS_H =(float) cos(status.Psi);
261		261
262	GPS_Nick = (int) (COS_H * (float) nick_gain - SIN_H * (float) roll_gain);	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);	263	GPS_Roll = (int) (SIN_H * (float) nick_gain + COS_H * (float) roll_gain);
264		264
265	GPS_Nick = MAX(-maxGainPos, MIN(maxGainPos, GPS_Nick));	265	GPS_Nick = MAX(-maxGainPos, MIN(maxGainPos, GPS_Nick));
266	GPS_Roll = MAX(-maxGainPos, MIN(maxGainPos, GPS_Roll));	266	GPS_Roll = MAX(-maxGainPos, MIN(maxGainPos, GPS_Roll));
267		267
268	/* Set New Heading */	268	/* Set New Heading */
269	SetNewHeading(maxDistance);	269	SetNewHeading(maxDistance);
270	}	270	}
271	else	271	else
272	{	272	{
273	GPS_Nick = 0;	273	GPS_Nick = 0;
274	GPS_Roll = 0;	274	GPS_Roll = 0;
275	maxDistance = 0.0F;	275	maxDistance = 0.0F;
276	}	276	}
277	}	277	}
278		278
279		279
280	#if 0	280	#if 0
281	if (maxDistance / 10 < 12)	281	if (maxDistance / 10 < 12)
282	{	282	{
283	static int iState = 0;	283	static int iState = 0;
284	switch (iState)	284	switch (iState)
285	{	285	{
286	case 0:	286	case 0:
287	targetPos.north += 400;	287	targetPos.north += 400;
288	targetPos.east += 0;	288	targetPos.east += 0;
289	GPSTrackingCycles = 0;	289	GPSTrackingCycles = 0;
290	iState++;	290	iState++;
291	break;	291	break;
292	case 1:	292	case 1:
293	targetPos.north += 0;	293	targetPos.north += 0;
294	targetPos.east += 400;	294	targetPos.east += 400;
295	GPSTrackingCycles = 0;	295	GPSTrackingCycles = 0;
296	iState++;	296	iState++;
297	break;	297	break;
298	case 2:	298	case 2:
299	targetPos.north -= 400;	299	targetPos.north -= 400;
300	targetPos.east += 0;	300	targetPos.east += 0;
301	GPSTrackingCycles = 0;	301	GPSTrackingCycles = 0;
302	iState++;	302	iState++;
303	break;	303	break;
304	case 3:	304	case 3:
305	targetPos.north += 0;	305	targetPos.north += 0;
306	targetPos.east -= 400;	306	targetPos.east -= 400;
307	GPSTrackingCycles = 0;	307	GPSTrackingCycles = 0;
308	iState=0;	308	iState=0;
309	break;	309	break;
310	default:	310	default:
311	iState=0;	311	iState=0;
312	break;	312	break;
313	}	313	}
314	beeptime = 5000;	314	beeptime = 5000;
315	BeepMuster = 0x0c00;	315	BeepMuster = 0x0c00;
316	}	316	}
317	#endif	317	#endif
318		318
319		319
320	void SetNewHeading(unsigned long maxDistance)	320	void SetNewHeading(unsigned long maxDistance)
321	/* Set New Heading */	321	/* Set New Heading */
322	{	322	{
323	int OffsetGier = 0;	323	int OffsetGier = 0;
324	if (maxDistance / 10 > 8)	324	if (maxDistance / 10 > 8)
325	{	325	{
326	OffsetGier = 4;	326	OffsetGier = 4;
327	}	327	}
328		328
329	if (TargetGier < 0)	329	if (TargetGier < 0)
330	{	330	{
331	TargetGier += 3600;	331	TargetGier += 3600;
332	}	332	}
333	if (TargetGier > sollGier)	333	if (TargetGier > sollGier)
334	{	334	{
335	if (TargetGier - sollGier < 1800)	335	if (TargetGier - sollGier < 1800)
336	{	336	{
337	sollGier += OffsetGier;	337	sollGier += OffsetGier;
338	}	338	}
339	else	339	else
340	{	340	{
341	sollGier -= OffsetGier;	341	sollGier -= OffsetGier;
342	}	342	}
343	}	343	}
344	else	344	else
345	{	345	{
346	if (sollGier - TargetGier < 1800)	346	if (sollGier - TargetGier < 1800)
347	{	347	{
348	sollGier -= OffsetGier;	348	sollGier -= OffsetGier;
349	}	349	}
350	else	350	else
351	{	351	{
352	sollGier += OffsetGier;	352	sollGier += OffsetGier;
353	}	353	}
354	}	354	}
355	}	355	}
356		356
357	/* ****************************************************************************	357	/* ****************************************************************************
358	Functionname: InitGPS / /!	358	Functionname: InitGPS / /!
359	Description:	359	Description:
360		360
361	@return void	361	@return void
362	@pre -	362	@pre -
363	@post -	363	@post -
364	@author	364	@author
365	**************************************************************************** */	365	**************************************************************************** */
366	void InitGPS(void)	366	void InitGPS(void)
367	{	367	{
368	// USART1 Control and Status Register A, B, C and baud rate register	368	// USART1 Control and Status Register A, B, C and baud rate register
369	uint8_t sreg = SREG;	369	uint8_t sreg = SREG;
370	//uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK/(8 * USART1_BAUD) - 1);	370	//uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK/(8 * USART1_BAUD) - 1);
371		371
372	// disable all interrupts before reconfiguration	372	// disable all interrupts before reconfiguration
373	cli();	373	cli();
374		374
375	// disable RX-Interrupt	375	// disable RX-Interrupt
376	UCSR1B &= ~(1 << RXCIE1);	376	UCSR1B &= ~(1 << RXCIE1);
377	// disable TX-Interrupt	377	// disable TX-Interrupt
378	UCSR1B &= ~(1 << TXCIE1);	378	UCSR1B &= ~(1 << TXCIE1);
379	// disable DRE-Interrupt	379	// disable DRE-Interrupt
380	UCSR1B &= ~(1 << UDRIE1);	380	UCSR1B &= ~(1 << UDRIE1);
381		381
382	// set direction of RXD1 and TXD1 pins	382	// set direction of RXD1 and TXD1 pins
383	// set RXD1 (PD2) as an input pin	383	// set RXD1 (PD2) as an input pin
384	PORTD \|= (1 << PORTD2);	384	PORTD \|= (1 << PORTD2);
385	DDRD &= ~(1 << DDD2);	385	DDRD &= ~(1 << DDD2);
386		386
387	// set TXD1 (PD3) as an output pin	387	// set TXD1 (PD3) as an output pin
388	PORTD \|= (1 << PORTD3);	388	PORTD \|= (1 << PORTD3);
389	DDRD \|= (1 << DDD3);	389	DDRD \|= (1 << DDD3);
390		390
391	// USART0 Baud Rate Register	391	// USART0 Baud Rate Register
392	// set clock divider	392	// set clock divider
393	UBRR1H = 0;	393	UBRR1H = 0;
394	UBRR1L = BAUDRATE;	394	UBRR1L = BAUDRATE;
395		395
396	// enable double speed operation	396	// enable double speed operation
397	//UCSR1A \|= (1 << U2X1);	397	//UCSR1A \|= (1 << U2X1);
398	UCSR1A = _B0(U2X1);	398	UCSR1A = _B0(U2X1);
399		399
400	// enable receiver and transmitter	400	// enable receiver and transmitter
401	UCSR1B = (1 << TXEN1) \| (1 << RXEN1);	401	UCSR1B = (1 << TXEN1) \| (1 << RXEN1);
402	// set asynchronous mode	402	// set asynchronous mode
403	UCSR1C &= ~(1 << UMSEL11);	403	UCSR1C &= ~(1 << UMSEL11);
404	UCSR1C &= ~(1 << UMSEL10);	404	UCSR1C &= ~(1 << UMSEL10);
405	// no parity	405	// no parity
406	UCSR1C &= ~(1 << UPM11);	406	UCSR1C &= ~(1 << UPM11);
407	UCSR1C &= ~(1 << UPM10);	407	UCSR1C &= ~(1 << UPM10);
408	// 1 stop bit	408	// 1 stop bit
409	UCSR1C &= ~(1 << USBS1);	409	UCSR1C &= ~(1 << USBS1);
410	// 8-bit	410	// 8-bit
411	UCSR1B &= ~(1 << UCSZ12);	411	UCSR1B &= ~(1 << UCSZ12);
412	UCSR1C \|= (1 << UCSZ11);	412	UCSR1C \|= (1 << UCSZ11);
413	UCSR1C \|= (1 << UCSZ10);	413	UCSR1C \|= (1 << UCSZ10);
414		414
415	// flush receive buffer explicit	415	// flush receive buffer explicit
416	while ( UCSR1A & (1<<RXC1) ) UDR1;	416	while ( UCSR1A & (1<<RXC1) ) UDR1;
417		417
418	// enable interrupts at the end	418	// enable interrupts at the end
419	// enable RX-Interrupt	419	// enable RX-Interrupt
420	UCSR1B \|= (1 << RXCIE1);	420	UCSR1B \|= (1 << RXCIE1);
421	// enable TX-Interrupt	421	// enable TX-Interrupt
422	UCSR1B \|= (1 << TXCIE1);	422	UCSR1B \|= (1 << TXCIE1);
423	// enable DRE interrupt	423	// enable DRE interrupt
424	//UCSR1B \|= (1 << UDRIE1);	424	//UCSR1B \|= (1 << UDRIE1);
425		425
426	// restore global interrupt flags	426	// restore global interrupt flags
427	SREG = sreg;	427	SREG = sreg;
428		428
429	gpsState = GPS_EMPTY;	429	gpsState = GPS_EMPTY;
430	}	430	}
431		431
432		432
433	/* ****************************************************************************	433	/* ****************************************************************************
434	Functionname: InitGPS / /!	434	Functionname: InitGPS / /!
435	Description: Copy GPS paket data to actualPos	435	Description: Copy GPS paket data to actualPos
436		436
437	@return void	437	@return void
438	@pre -	438	@pre -
439	@post -	439	@post -
440	@author	440	@author
441	**************************************************************************** */	441	**************************************************************************** */
442	void GPSscanData (void)	442	void GPSscanData (void)
443	{	443	{
444	if (navStatus.packetStatus == 1)// valid packet	444	if (navStatus.packetStatus == 1)// valid packet
445	{	445	{
446	actualPos.state = navStatus.GPSfix;	446	actualPos.state = navStatus.GPSfix;
447	if ((actualPos.state > 1) && (GPSTracking == 0))	447	if ((actualPos.state > 1) && (GPSTracking == 0))
448	{	448	{
449	GRN_FLASH;	449	GRN_FLASH;
450	}	450	}
451	navStatus.packetStatus = 0;	451	navStatus.packetStatus = 0;
452	}	452	}
453		453
454	if (navPosUtm.packetStatus == 1)// valid packet	454	if (navPosUtm.packetStatus == 1)// valid packet
455	{	455	{
456	actualPos.north = navPosUtm.NORTH/10L;	456	actualPos.north = navPosUtm.NORTH/10L;
457	actualPos.east = navPosUtm.EAST/10L;	457	actualPos.east = navPosUtm.EAST/10L;
458	actualPos.altitude = navPosUtm.ALT/100;	458	actualPos.altitude = navPosUtm.ALT/100;
459	actualPos.ITOW = navPosUtm.ITOW;	459	actualPos.ITOW = navPosUtm.ITOW;
460	navPosUtm.packetStatus = 0;	460	navPosUtm.packetStatus = 0;
461	}	461	}
462	/*	462	/*
463	if (navPosLlh.packetStatus == 1)	463	if (navPosLlh.packetStatus == 1)
464	{	464	{
465	actualPos.longi = navPosLlh.LON;	465	actualPos.longi = navPosLlh.LON;
466	actualPos.lati = navPosLlh.LAT;	466	actualPos.lati = navPosLlh.LAT;
467	actualPos.height = navPosLlh.HEIGHT;	467	actualPos.height = navPosLlh.HEIGHT;
468	navPosLlh.packetStatus = 0;	468	navPosLlh.packetStatus = 0;
469	}	469	}
470	*/	470	*/
471	if (navVelNed.packetStatus == 1)	471	if (navVelNed.packetStatus == 1)
472	{	472	{
473	actualPos.velNorth = navVelNed.VEL_N;	473	actualPos.velNorth = navVelNed.VEL_N;
474	actualPos.velEast = navVelNed.VEL_E;	474	actualPos.velEast = navVelNed.VEL_E;
475	actualPos.velDown = navVelNed.VEL_D;	475	actualPos.velDown = navVelNed.VEL_D;
476	actualPos.groundSpeed = navVelNed.GSpeed;	476	actualPos.groundSpeed = navVelNed.GSpeed;
477	navVelNed.packetStatus = 0;	477	navVelNed.packetStatus = 0;
478	}	478	}
479	}	479	}
480		480
481	/* ****************************************************************************	481	/* ****************************************************************************
482	Functionname: SIGNAL / /!	482	Functionname: SIGNAL / /!
483	Description:	483	Description:
484		484
485	@return void	485	@return void
486	@pre -	486	@pre -
487	@post -	487	@post -
488	@author	488	@author
489	**************************************************************************** */	489	**************************************************************************** */
490	SIGNAL (SIG_USART1_RECV)	490	SIGNAL (SIG_USART1_RECV)
491	{	491	{
492	uint8_t c;	492	uint8_t c;
493	uint8_t re;	493	uint8_t re;
494		494
495	re = (UCSR1A & (_B1(FE1) \| _B1(DOR1)));// any error occured ?	495	re = (UCSR1A & (_B1(FE1) \| _B1(DOR1)));// any error occured ?
496	c = UDR1;	496	c = UDR1;
497		497
498	if (re == 0)	498	if (re == 0)
499	{	499	{
500	switch (gpsState)	500	switch (gpsState)

Subversion Repositories FlightCtrl

(root)/branches/KalmanFilter MikeW/gps.c @ 861 - Rev 966 → 988