WebSVN - FlightCtrl - Diff - Rev 838 and 966 - /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 "kafi.h"
 #include "mymath.h"
 #include <math.h>
+/*****************************************************************************
+(SYMBOLIC) CONSTANTS
+*****************************************************************************/
+/*****************************************************************************
+  VARIABLES
+*****************************************************************************/
 int GPSTracking = 0;
 int targetPosValid = 0;
 int homePosValid = 0;
-uint8_t gpsState;
+int holdPosValid = 0;
-gpsInfo_t               actualPos;                      // measured position (last gps record)
+volatile gpsInfo_t actualPos;// measured position (last gps record)
-gpsInfo_t               targetPos;                      // measured position (last gps record)
+volatile gpsInfo_t targetPos;// measured position (last gps record)
-gpsInfo_t               homePos;                        // 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_STATUS_t navStatus;
-NAV_POSLLH_t    navPosLlh;
+NAV_POSLLH_t navPosLlh;
-NAV_POSUTM_t    navPosUtm;
+NAV_POSUTM_t navPosUtm;
-NAV_VELNED_t    navVelNed;
+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 = 0;
+int nick_gain_p, nick_gain_d;
-int nick_gain_d = 0;
-int roll_gain_p = 0;
+int roll_gain_p, roll_gain_d;
-int roll_gain_d = 0;
 int Override =  0;
 int TargetGier = 0;
 extern int sollGier;
+extern int RemoteLinkLost;
-char                    *ubxP, *ubxEp, *ubxSp;  // pointers to packet currently transfered
+volatile char*ubxP, *ubxEp, *ubxSp;// pointers to packet currently transfered
-uint8_t         CK_A, CK_B;             // UBX checksum bytes
+volatile uint8_t CK_A, CK_B;// UBX checksum bytes
-uint8_t         ignorePacket;           // true when previous packet was not processed
+volatile uint8_t ignorePacket;// true when previous packet was not processed
-unsigned short  msgLen;
+volatile unsigned short msgLen;
-uint8_t         msgID;
+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)
+{
-#if 0
-        DebugOut.Analog[11] =  max_p;
-        DebugOut.Analog[12] =  max_d;
-#endif
-        static int GPSTrackingCycles = 0;
-        long maxGainPos = 140;
-        long maxGainVel = 140;
-        /* Slowly ramp up gain */
-        if (GPSTrackingCycles < 1000)
-        {
-                GPSTrackingCycles++;
-        }
-        maxGainPos = (maxGainPos * GPSTrackingCycles) / 1000;
-        maxGainVel = (maxGainVel * GPSTrackingCycles) / 1000;
-        if (actualPos.state > 2 )
-        {
-                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);
-                // 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, ((actualPos.velNorth  * 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, ((actualPos.velEast * max_d)/50)));  //m/s
-                TargetGier = c_atan2(-distanceEW / 8, -distanceNS / 8) * 10;
-        }
-        if ((GPSTracking == 1) &&
-                (actualPos.state > 2) &&
-                (Override == 0))
-        {
-                /* Calculate Distance to Target */
-                SIN_H = (float) sin(status.Psi);
-                COS_H =(float) cos(status.Psi);
-                /* PD-Regler */
-                nick_gain = nick_gain_p + nick_gain_d;
-                roll_gain = -(roll_gain_p + roll_gain_d);
-                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));
-                /* Turn the mikrokopter slowly towards the target position */
-                {
-                        int OffsetGier;
-                        if (maxDistance / 10 > 2)
-                        {
-                                OffsetGier = 2;
-                        }
-                        else
-                        {
-                                OffsetGier = 0;
-                        }
-                        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;
-                                }
-        }
-#if 0
-                /* Go round in a square */
+#if 0
-                if (maxDistance / 10 < 10)
+if (maxDistance / 10 < 12)
-                {
+{
-                        static int iState = 0;
+  static int iState = 0;
-                        switch (iState)
+  switch (iState)
-                        {
+  {
-                                case 0:
+  case 0:
-                                        targetPos.north += 400;
+    targetPos.north += 400;
-                                        targetPos.east += 0;
+    targetPos.east += 0;
-                                        GPSTrackingCycles = 0;
+    GPSTrackingCycles = 0;
-                                        iState++;
+    iState++;
-                                        break;
+    break;
-                                case 1:
+  case 1:
-                                        targetPos.north += 0;
+    targetPos.north += 0;
-                                        targetPos.east += 400;
+    targetPos.east += 400;
-                                        GPSTrackingCycles = 0;
+    GPSTrackingCycles = 0;
-                                        iState++;
+    iState++;
-                                        break;
+    break;
-                                case 2:
+  case 2:
-                                        targetPos.north -= 400;
+    targetPos.north -= 400;
-                                        targetPos.east += 0;
+    targetPos.east += 0;
-                                        GPSTrackingCycles = 0;
+    GPSTrackingCycles = 0;
-                                        iState++;
+    iState++;
-                                        break;
+    break;
-                                case 3:
+  case 3:
-                                        targetPos.north += 0;
+    targetPos.north += 0;
-                                        targetPos.east -= 400;
+    targetPos.east -= 400;
-                                        GPSTrackingCycles = 0;
+    GPSTrackingCycles = 0;
-                                        iState=0;
+    iState=0;
-                                        break;
+    break;
-                                default:
+  default:
-                                        iState=0;
+    iState=0;
-                                        break;
+    break;
-                        }
+  }
-                        beeptime = 5000;
+  beeptime = 5000;
-                        BeepMuster = 0x0c00;
+  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;
-#endif
+    }
-        }
+  }
+  else
-        else
+  {
-        {
+    if (sollGier - TargetGier < 1800)
+    {
+      sollGier -= OffsetGier;
+    }
-                GPS_Nick = 0;
+    else
+    {
-                GPS_Roll = 0;
+      sollGier += OffsetGier;
                 GPSTrackingCycles = 0;
+  }
+}
 /* ****************************************************************************
 Functionname:     InitGPS                      */ /*!
 Description:
   @return           void
   @pre              -
   @post             -
   @author
  **************************************************************************** */
 void InitGPS(void)
+{
-        // USART1 Control and Status Register A, B, C and baud rate register
+  // USART1 Control and Status Register A, B, C and baud rate register
-        uint8_t sreg = SREG;
+  uint8_t  sreg = SREG;
-        //uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK/(8 * USART1_BAUD) - 1);
+  //uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK/(8 * USART1_BAUD) - 1);
-        // disable all interrupts before reconfiguration
+  // disable all interrupts before reconfiguration
-        cli();
+  cli();
-        // disable RX-Interrupt
+  // disable RX-Interrupt
-        UCSR1B &= ~(1 << RXCIE1);
+  UCSR1B &= ~(1 << RXCIE1);
-        // disable TX-Interrupt
+  // disable TX-Interrupt
-        UCSR1B &= ~(1 << TXCIE1);
+  UCSR1B &= ~(1 << TXCIE1);
-        // disable DRE-Interrupt
+  // disable DRE-Interrupt
-        UCSR1B &= ~(1 << UDRIE1);
+  UCSR1B &= ~(1 << UDRIE1);
-        // set direction of RXD1 and TXD1 pins
+  // set direction of RXD1 and TXD1 pins
-        // set RXD1 (PD2) as an input pin
+  // set RXD1 (PD2) as an input pin
-        PORTD |= (1 << PORTD2);
+  PORTD |= (1 << PORTD2);
-        DDRD &= ~(1 << DDD2);
+  DDRD &= ~(1 << DDD2);
-        // set TXD1 (PD3) as an output pin
+  // set TXD1 (PD3) as an output pin
-        PORTD |= (1 << PORTD3);
+  PORTD |= (1 << PORTD3);
-        DDRD  |= (1 << DDD3);
+  DDRD  |= (1 << DDD3);
-        // USART0 Baud Rate Register
+  // USART0 Baud Rate Register
-        // set clock divider
+  // set clock divider
-        UBRR1H = 0;
+  UBRR1H = 0;
-        UBRR1L = BAUDRATE;
+  UBRR1L = BAUDRATE;
-        // enable double speed operation
+  // enable double speed operation
-        //UCSR1A |= (1 << U2X1);
+  //UCSR1A |= (1 << U2X1);
-        UCSR1A = _B0(U2X1);
+  UCSR1A = _B0(U2X1);
-        // enable receiver and transmitter
+  // enable receiver and transmitter
-        UCSR1B = (1 << TXEN1) | (1 << RXEN1);
+  UCSR1B = (1 << TXEN1) | (1 << RXEN1);
-        // set asynchronous mode
+  // set asynchronous mode
-        UCSR1C &= ~(1 << UMSEL11);
+  UCSR1C &= ~(1 << UMSEL11);
-        UCSR1C &= ~(1 << UMSEL10);
+  UCSR1C &= ~(1 << UMSEL10);
-        // no parity
+  // no parity
-        UCSR1C &= ~(1 << UPM11);
+  UCSR1C &= ~(1 << UPM11);
-        UCSR1C &= ~(1 << UPM10);
+  UCSR1C &= ~(1 << UPM10);
-        // 1 stop bit
+  // 1 stop bit
-        UCSR1C &= ~(1 << USBS1);
+  UCSR1C &= ~(1 << USBS1);
-        // 8-bit
+  // 8-bit
-        UCSR1B &= ~(1 << UCSZ12);
+  UCSR1B &= ~(1 << UCSZ12);
-        UCSR1C |=  (1 << UCSZ11);
+  UCSR1C |=  (1 << UCSZ11);
-        UCSR1C |=  (1 << UCSZ10);
+  UCSR1C |=  (1 << UCSZ10);
-                // flush receive buffer explicit
+  // flush receive buffer explicit
-        while ( UCSR1A & (1<<RXC1) ) UDR1;
+  while ( UCSR1A & (1<<RXC1) ) UDR1;
-        // enable interrupts at the end
+  // enable interrupts at the end
-        // enable RX-Interrupt
+  // enable RX-Interrupt
-        UCSR1B |= (1 << RXCIE1);
+  UCSR1B |= (1 << RXCIE1);
-        // enable TX-Interrupt
+  // enable TX-Interrupt
-        UCSR1B |= (1 << TXCIE1);
+  UCSR1B |= (1 << TXCIE1);
-        // enable DRE interrupt
+  // enable DRE interrupt
-        //UCSR1B |= (1 << UDRIE1);
+  //UCSR1B |= (1 << UDRIE1);
-        // restore global interrupt flags
+  // restore global interrupt flags
-    SREG = sreg;
+  SREG = sreg;
-        gpsState = GPS_EMPTY;
+  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
+ if (navStatus.packetStatus == 1)// valid packet
-        {
+  {
-                actualPos.state = navStatus.GPSfix;
+    actualPos.state = navStatus.GPSfix;
-                if ((actualPos.state > 1) && (GPSTracking == 0))
+    if ((actualPos.state > 1) && (GPSTracking == 0))
-                {
+    {
-                                GRN_FLASH;
+      GRN_FLASH;
-                }
+    }
-                navStatus.packetStatus = 0;
+    navStatus.packetStatus = 0;
-        }
+  }
-        if (navPosUtm.packetStatus == 1)                        // valid packet
+  if (navPosUtm.packetStatus == 1)// valid packet
-        {
+  {
-                actualPos.north = navPosUtm.NORTH/10L;
+    actualPos.north = navPosUtm.NORTH/10L;
-                actualPos.east = navPosUtm.EAST/10L;
+    actualPos.east = navPosUtm.EAST/10L;
-                actualPos.altitude = navPosUtm.ALT/100;
+    actualPos.altitude = navPosUtm.ALT/100;
-                actualPos.ITOW = navPosUtm.ITOW;
+    actualPos.ITOW = navPosUtm.ITOW;
-                navPosUtm.packetStatus = 0;
+    navPosUtm.packetStatus = 0;
-        }
+  }
-/*
+  /*
-        if (navPosLlh.packetStatus == 1)
+  if (navPosLlh.packetStatus == 1)
-        {
+  {
-                actualPos.longi = navPosLlh.LON;
+    actualPos.longi = navPosLlh.LON;
-                actualPos.lati = navPosLlh.LAT;
+    actualPos.lati = navPosLlh.LAT;
-                actualPos.height = navPosLlh.HEIGHT;
+    actualPos.height = navPosLlh.HEIGHT;
-                navPosLlh.packetStatus = 0;
+    navPosLlh.packetStatus = 0;
-        }
+    }
-*/
+  */
-        if (navVelNed.packetStatus == 1)
+  if (navVelNed.packetStatus == 1)
-        {
+  {
-                actualPos.velNorth = navVelNed.VEL_N;
+    actualPos.velNorth = navVelNed.VEL_N;
-            actualPos.velEast = navVelNed.VEL_E;
+    actualPos.velEast = navVelNed.VEL_E;
-            actualPos.velDown = navVelNed.VEL_D;
+    actualPos.velDown = navVelNed.VEL_D;
-                actualPos.groundSpeed = navVelNed.GSpeed;
+    actualPos.groundSpeed = navVelNed.GSpeed;
-                navVelNed.packetStatus = 0;
+    navVelNed.packetStatus = 0;
-        }
+  }
+}
 /* ****************************************************************************
 Functionname:     SIGNAL                      */ /*!
 Description:
   @return           void
   @pre              -
   @post             -
   @author
  **************************************************************************** */
 SIGNAL (SIG_USART1_RECV)
+{
+}

Subversion Repositories FlightCtrl

(root)/branches/KalmanFilter MikeW/gps.c @ 1354 - Rev 838 → 966