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

+  }
   /* Set Hold Position */
   if ((actualPos.state > 2) &&
     ((Override == 1) ||
-    (SwitchPos ==  2) )) /* Update the hold position in case we are in target mode */
+    (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 ;
   /* Determine Offset from nominal Position */
   if (actualPos.state > 2 )
+  {
     if ((SwitchPos ==  2) &&
-    (targetPosValid == 1) &&
+      (targetPosValid == 1) &&
-        (RemoteLinkLost == 0) &&
+      (RemoteLinkLost == 0) &&
-        (Override == 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) &&
+      (RemoteLinkLost == 0) &&
-          (Override == 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
+    {
-        distanceNS = 0.0F; //  in 0.1m
+      distanceNS = 0.0F; //  in 0.1m
-        distanceEW = 0.0F; // in 0.1m
+      distanceEW = 0.0F; // in 0.1m
-        velocityNS = 0.0F; // in 0.1m/s
+      velocityNS = 0.0F; // in 0.1m/s
-        velocityEW = 0.0F; // in 0.1m/s
+      velocityEW = 0.0F; // in 0.1m/s
-        maxDistance = 0.0F;
+      maxDistance = 0.0F;
-        GPSTrackingCycles = 0;
+      GPSTrackingCycles = 0;
+    }
     /* Limit GPS_Nick to 25 */
     nick_gain_p = (long) MAX(-maxGainPos, MIN(maxGainPos, ((distanceNS* max_p)/50)));  //m

Subversion Repositories FlightCtrl

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