Diff: autonomous.c

Revision:

2:90292f8bd179

Parent:

0:62a1c91a859a

--- a/autonomous.c	Fri Feb 25 01:35:24 2011 +0000
+++ b/autonomous.c	Tue Apr 26 12:12:29 2011 +0000
@@ -2,7 +2,7 @@
 // =                              UAVXArm Quadrocopter Controller                                =
 // =                           Copyright (c) 2008 by Prof. Greg Egan                             =
 // =                 Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer                   =
-// =                     http://code.google.com/p/uavp-mods/ http://uavp.ch                      =
+// =                           http://code.google.com/p/uavp-mods/                               =
 // ===============================================================================================
 
 //    This is part of UAVXArm.
@@ -26,7 +26,6 @@
 void SetDesiredAltitude(int16);
 void DoFailsafeLanding(void);
 void AcquireHoldPosition(void);
-void NavGainSchedule(int16);
 void DoNavigation(void);
 void DoPPMFailsafe(void);
 void UAVXNavCommand(void);
@@ -217,7 +216,7 @@
         // Just use simple rudder only for now.
         if ( !F.WayPointAchieved ) {
             NavCorr[Yaw] = -(RelHeading * NAV_YAW_LIMIT) / HALFPI;
-            NavCorr[Yaw] = Limit(NavCorr[Yaw], -NAV_YAW_LIMIT, NAV_YAW_LIMIT); // gently!
+            NavCorr[Yaw] = Limit1(NavCorr[Yaw], NAV_YAW_LIMIT); // gently!
         }
 
 #else // MULTICOPTER
@@ -233,24 +232,24 @@
         // Roll & Pitch
 
         for ( a = 0; a < (uint8)2 ; a++ ) {
-            NavP = Limit(NavE[a], -NAV_MAX_ROLL_PITCH, NAV_MAX_ROLL_PITCH);
+            NavP = Limit1(NavE[a], NAV_MAX_ROLL_PITCH);
 
             NavIntE[a] += NavE[a];
-            NavIntE[a] = Limit(NavIntE[a], -K[NavIntLimit], K[NavIntLimit]);
+            NavIntE[a] = Limit1(NavIntE[a], K[NavIntLimit]);
             NavI = NavIntE[a] * K[NavKi] * GPSdT;
             NavIntE[a] = Decay1(NavIntE[a]);
 
             Diff = (NavEp[a] - NavE[a]);
-            Diff = Limit(Diff, -256, 256);
+            Diff = Limit1(Diff, 256);
             NavD = Diff * K[NavKd] * GPSdTR;
-            NavD = Limit(NavD, -NAV_DIFF_LIMIT, NAV_DIFF_LIMIT);
+            NavD = Limit1(NavD, NAV_DIFF_LIMIT);
 
             NavEp[a] = NavE[a];
 
             NavCorr[a] = (NavP + NavI + NavD ) * NavSensitivity;
 
             NavCorr[a] = SlewLimit(NavCorrp[a], NavCorr[a], NAV_CORR_SLEW_LIMIT);
-            NavCorr[a] = Limit(NavCorr[a], -NAV_MAX_ROLL_PITCH, NAV_MAX_ROLL_PITCH);
+            NavCorr[a] = Limit1(NavCorr[a], NAV_MAX_ROLL_PITCH);
 
             NavCorrp[a] = NavCorr[a];
         }
@@ -259,7 +258,7 @@
         if ( F.AllowTurnToWP && !F.WayPointAchieved ) {
             RelHeading = MakePi(WayHeading - Heading); // make +/- MilliPi
             NavCorr[Yaw] = -(RelHeading * NavYCorrLimit) / HALFPI;
-            NavCorr[Yaw] = Limit(NavCorr[Yaw], -NavYCorrLimit, NavYCorrLimit); // gently!
+            NavCorr[Yaw] = Limit1(NavCorr[Yaw], NavYCorrLimit); // gently!
         } else
             NavCorr[Yaw] = 0;
 
@@ -362,12 +361,9 @@
                         if ( F.AcquireNewPosition && !( F.Ch5Active & F.UsingPositionHoldLock ) ) {
                             F.AllowTurnToWP = SaveAllowTurnToWP;
                             AcquireHoldPosition();
-#ifdef NAV_ACQUIRE_BEEPER
-                            if ( !F.BeeperInUse ) {
-                                mS[BeeperTimeout] = mSClock() + 500L;
-                                Beeper_ON;
-                            }
-#endif // NAV_ACQUIRE_BEEPER
+                            #ifdef NAV_ACQUIRE_BEEPER
+                            DoBeeperPulse1mS(500);
+                            #endif // NAV_ACQUIRE_BEEPER
                         }
                     } else
                         F.AcquireNewPosition = true;