Diff: rc.c

Revision:

2:90292f8bd179

Parent:

0:62a1c91a859a

--- a/rc.c	Fri Feb 25 01:35:24 2011 +0000
+++ b/rc.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.
@@ -92,7 +92,6 @@
 int16x8x4Q PPMQ;
 int16 PPMQSum[CONTROLS];
 int16 RC[CONTROLS], RCp[CONTROLS];
-int16 Trim[3];
 int16 ThrLow, ThrNeutral, ThrHigh;
 
 boolean RCPositiveEdge;
@@ -115,7 +114,7 @@
 }  // DoRxPolarity
 
 void InitRC(void) {
-    static int8 c, i, q;
+    static int8 c, q;
 
     DoRxPolarity();
 
@@ -133,9 +132,7 @@
     PPMQ.Head = 0;
 
     DesiredRoll = DesiredPitch = DesiredYaw = DesiredThrottle = StickThrottle = 0;
-
-    for ( i = 0; i < 3; i++ )
-        Trim[i] = 0;
+    Trim[Roll] = Trim[Pitch] = Trim[Yaw] = 0;
 
     PPM_Index = PrevEdge = RCGlitches = 0;
 } // InitRC
@@ -288,11 +285,11 @@
     DesiredPitch = SRS16((RC[PitchRC] - RC_NEUTRAL) * RollPitchScale, 7);
 #endif // ATTITUDE_NO_LIMITS
     DesiredYaw = RC[YawRC] - RC_NEUTRAL;
+    
+    AdaptiveYawLPFreq();
 
-    HoldRoll = DesiredRoll - Trim[Roll];
-    HoldRoll = abs(HoldRoll);
-    HoldPitch = DesiredPitch - Trim[Pitch];
-    HoldPitch = abs(HoldPitch);
+    HoldRoll = abs(DesiredRoll - Trim[Roll]);
+    HoldPitch = abs(DesiredPitch - Trim[Pitch]);
     CurrMaxRollPitch = Max(HoldRoll, HoldPitch);
 
     if ( CurrMaxRollPitch > ATTITUDE_HOLD_LIMIT )
@@ -318,11 +315,14 @@
 
 } // UpdateControls
 
-void CaptureTrims(void) {    // only used in detecting movement from neutral in hold GPS position
+void CaptureTrims(void)
+{     // only used in detecting movement from neutral in hold GPS position
     // Trims are invalidated if Nav sensitivity is changed - Answer do not use trims ?
-    Trim[Roll] = Limit(DesiredRoll, -NAV_MAX_TRIM, NAV_MAX_TRIM);
-    Trim[Pitch] = Limit(DesiredPitch, -NAV_MAX_TRIM, NAV_MAX_TRIM);
-    Trim[Yaw] = Limit(DesiredYaw, -NAV_MAX_TRIM, NAV_MAX_TRIM);
+    #ifndef TESTING
+    Trim[Roll] = Limit1(DesiredRoll, NAV_MAX_TRIM);
+    Trim[Yaw] = Limit1(DesiredPitch, NAV_MAX_TRIM);
+    Trim[Yaw] = Limit1(DesiredYaw, NAV_MAX_TRIM);
+    #endif // TESTING
 
     HoldYaw = 0;
 } // CaptureTrims