Subversion Repositories FlightCtrl

/*

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;

  }

}