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

 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
-#include <math.h>
+*****************************************************************************/
 int GPSTracking = 0;
+int targetPosValid = 0;
-int targetPosValid = 0;
+int homePosValid = 0;
-int homePosValid = 0;
+int holdPosValid = 0;
-uint8_t gpsState;
+volatile gpsInfo_t actualPos;// measured position (last gps record)
-gpsInfo_t               actualPos;                      // measured position (last gps record)
+volatile gpsInfo_t targetPos;// measured position (last gps record)
-gpsInfo_t               targetPos;                      // measured position (last gps record)
+volatile gpsInfo_t homePos;// measured position (last gps record)
-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_STATUS_t    navStatus;
+NAV_POSUTM_t navPosUtm;
-NAV_POSLLH_t    navPosLlh;
+NAV_VELNED_t navVelNed;
 NAV_POSUTM_t    navPosUtm;
-NAV_VELNED_t    navVelNed;
+uint8_t gpsState;
 long velocityEW = 0;
 unsigned long maxDistance = 0;
 int roll_gain = 0;
 int nick_gain = 0;
-int nick_gain_p = 0;
-int nick_gain_d = 0;
+int nick_gain_p, nick_gain_d;
-int roll_gain_p = 0;
-int roll_gain_d = 0;
+int roll_gain_p, roll_gain_d;
 int Override =  0;
 int TargetGier = 0;
+extern int sollGier;
-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:
 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 */
-**************************************************************************** */
+  }
-void GPSupdate(void)
+  else
-{
+  {
-        float SIN_H, COS_H;
+    SwitchPos = 1;/* Position Hold */
         long max_p = 0;
         long max_d = 0;
   /* Overide On / Off */
-        if (abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]])> 15 ||
+  if (abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]])> 10 ||
-    /* Set Home Position */
+  /* Set Home Position */
-        if ((actualPos.state > 2) &&
+  if ((actualPos.state > 2) &&
-                (homePosValid == 0))
+    (homePosValid == 0))
-        {
                 /* Save Current Position */
-                homePos.north = actualPos.north;
+    homePos.north = actualPos.north;
-                homePos.east = actualPos.east;
+    homePos.east = actualPos.east;
-                homePos.altitude = actualPos.altitude ;
+    homePos.altitude = actualPos.altitude ;
-                homePosValid = 1;
+    homePosValid = 1;
+  }
-    /* Set Target Position */
+  /* Set Target Position */
-        if ((actualPos.state > 2) &&
+  if ((actualPos.state > 2) &&
-                (PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] < -100))
     (SwitchPos == 0))
                 /* Save Current Position */
-                targetPos.north = actualPos.north;
+    targetPos.north = actualPos.north;
-                targetPos.east = actualPos.east;
+    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 */
-                targetPos.altitude = actualPos.altitude ;
+  if ((actualPos.state > 2) &&
-                targetPosValid = 1;
+    (SwitchPos == 0))
+  {
-        if(BeepMuster == 0xffff)
+    if(BeepMuster == 0xffff)
+    {
-            beeptime = 5000;
+      beeptime = 5000;
-            BeepMuster = 0x0100;
-        }
+      BeepMuster = 0x0100;
+    }
-        if ((GPSTracking == 1) &&
+  }
-                (PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] < 100))
   if (RemoteLinkLost == 0) /* Disable Heading Update in case we lost the RC - Link */
-                GPSTracking = 0;
+  {
-                beeptime = 5000;
-                BeepMuster = 0x0080;
-        }
-        /* The System is in Tracking State*/
-        if ((GPSTracking == 0) &&
+    max_p = (MAX(0, (PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 170)) / 20);
-            (targetPosValid == 1) &&
+    max_d = (MAX(0, (PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 170)) / 40);
-                (PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] > 100) &&
+  }
-                (actualPos.state > 2))
   else
-                GPSTracking = 1;
+  {
+    max_p = 8;
-                beeptime = 5000;
+    max_d = 4;
-                BeepMuster = 0x0c00;
-        }
+  }
-        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);
+  /* Those seem to be good values */
   max_p = 8;
-#if 0
+  max_d = 4;
-        DebugOut.Analog[11] =  max_p;
-        DebugOut.Analog[12] =  max_d;
+ //DebugOut.Analog[11] = max_p;
-#endif
+ //DebugOut.Analog[12] = max_d;
-        static int GPSTrackingCycles = 0;
+  static int GPSTrackingCycles = 0;
-        long maxGainPos = 140;
+  long maxGainPos = 140;
-        long maxGainVel = 140;
+  long maxGainVel = 140;
   /* Ramp up gain */
-        /* Slowly ramp up gain */
+  if (GPSTrackingCycles < 1000)
+  {
+    GPSTrackingCycles++;
+  }
+  maxGainPos = (maxGainPos * GPSTrackingCycles) / 1000;
+  maxGainVel = (maxGainVel * GPSTrackingCycles) / 1000;
         if (GPSTrackingCycles < 1000)
   /* Determine Offset from nominal Position */
-                GPSTrackingCycles++;
+  if (actualPos.state > 2 )
+  {
-        maxGainPos = (maxGainPos * GPSTrackingCycles) / 1000;
+    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);
+    }
-        maxGainVel = (maxGainVel * GPSTrackingCycles) / 1000;
+    else
+    {
-        if (actualPos.state > 2 )
+        distanceNS = 0.0F; //  in 0.1m
         distanceEW = 0.0F; // in 0.1m
-                distanceNS = actualPos.north - targetPos.north; //  in 0.1m
+        velocityNS = 0.0F; // in 0.1m/s
-                distanceEW = actualPos.east - targetPos.east; // in 0.1m
-                velocityNS = actualPos.velNorth; // in 0.1m/s
+        velocityEW = 0.0F; // in 0.1m/s
-                velocityEW = actualPos.velEast; // in 0.1m/s
-                maxDistance = sqrt(distanceNS * distanceNS + distanceEW * distanceEW);
+        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, ((actualPos.velNorth  * max_d)/50)));  //m/s
+    /* Limit GPS_Nick to 25 */
-                roll_gain_p = (long) MAX(-maxGainPos, MIN(maxGainPos, ((distanceEW * max_p)/50))); //m
+    nick_gain_p = (long) MAX(-maxGainPos, MIN(maxGainPos, ((distanceNS* max_p)/50)));  //m
-                roll_gain_d = (long) MAX(-maxGainVel, MIN(maxGainVel, ((actualPos.velEast * max_d)/50)));  //m/s
+    nick_gain_d = (long) MAX(-maxGainVel, MIN(maxGainVel, ((velocityNS  * max_d)/50)));  //m/s
-                TargetGier = c_atan2(-distanceEW / 8, -distanceNS / 8) * 10;
-        }
-        if ((GPSTracking == 1) &&
+    roll_gain_p = (long) MAX(-maxGainPos, MIN(maxGainPos, ((distanceEW * max_p)/50))); //m
-                (actualPos.state > 2) &&
+    roll_gain_d = (long) MAX(-maxGainVel, MIN(maxGainVel, ((velocityEW * max_d)/50)));  //m/s
-                (Override == 0))
+    TargetGier = c_atan2(-distanceEW / 8, -distanceNS / 8) * 10;
-                /* 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 */
+    /* PD-Control */
-                {
-                        int OffsetGier;
-                        if (maxDistance / 10 > 2)
-                        {
-                                OffsetGier = 2;
-                        }
+    nick_gain = nick_gain_p + nick_gain_d;
-                        else
+    roll_gain = -(roll_gain_p + roll_gain_d);
-                                OffsetGier = 0;
+    /* Calculate Distance to Target */
-                        }
+    SIN_H = (float) sin(status.Psi);
-                        if (TargetGier < 0)
-                        {
-                                TargetGier  += 3600;
-                        }
-                        if (TargetGier > sollGier)
-                        {
+    COS_H =(float) cos(status.Psi);
-                                if (TargetGier - sollGier < 1800)
-                                {
-                                        sollGier += OffsetGier;
-                                }
-                                else
     GPS_Nick = (int) (COS_H * (float) nick_gain - SIN_H * (float) roll_gain);
-                                        sollGier -= OffsetGier;
+    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));
-                        }
-                        else
     /* Set New Heading */
-                                if (sollGier - TargetGier < 1800)
+    SetNewHeading(maxDistance);
+  }
-                                        sollGier -= OffsetGier;
+  else
+  {
+  }
-                        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
+    else
+    {
-                GPS_Nick = 0;
+      sollGier -= OffsetGier;
+    }
+  }
+  else
+  {
+    if (sollGier - TargetGier < 1800)
+    {
-                GPS_Roll = 0;
+      sollGier -= OffsetGier;
+    }
+    else
+    {
-                GPSTrackingCycles = 0;
+      sollGier += OffsetGier;
+    }
+  }
+}
 /* ****************************************************************************

Subversion Repositories FlightCtrl

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