Diff: outputs.c

Revision:

2:90292f8bd179

Parent:

1:1e3318a30ddd

--- a/outputs.c	Fri Feb 25 01:35:24 2011 +0000
+++ b/outputs.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.
@@ -49,7 +49,9 @@
 } // TC
 
 void DoMulticopterMix(real32 CurrThrottle) {
+#ifndef MULTICOPTER
     static real32 Temp;
+#endif
 
 #ifdef Y6COPTER
     PWM[FrontTC] = PWM[LeftTC] = PWM[RightTC] = CurrThrottle;
@@ -81,10 +83,9 @@
     }
 #else
 #ifdef Y6COPTER
-
     Temp = Pl * 0.5;
-    PWM[FrontTC] -= Pl;             // front motor
-    PWM[LeftTC] += (Temp - Rl);     // right rear
+    PWM[FrontTC] -= Pl;          // front motor
+    PWM[LeftTC] += (Temp - Rl);  // right rear
     PWM[RightTC] += (Temp + Rl); // left rear
 
     PWM[FrontBC] = PWM[FrontTC];
@@ -104,7 +105,6 @@
     PWM[FrontBC] += Temp;
     PWM[LeftBC]  += Temp;
     PWM[RightBC] += Temp;
-
 #else
     PWM[LeftC]  += -Rl + Yl;
     PWM[RightC] +=  Rl + Yl;
@@ -159,8 +159,11 @@
 #endif // MULTICOPTER
 
 void MixAndLimitMotors(void) {
-    static real32 Temp, TempElevon, TempElevator;
+#ifndef MULTICOPTER
+    static TempElevon, TempElevator;
     static uint8 m;
+#endif
+    static real32 Temp;
 
     if ( DesiredThrottle < IdleThrottle )
         CurrThrottle = 0;
@@ -169,7 +172,7 @@
 
 #ifdef MULTICOPTER
     if ( State == InFlight )
-        CurrThrottle += (-Comp[UD] + Comp[Alt]); // vertical compensation not optional
+        CurrThrottle += AltComp; // vertical compensation not optional
 
     Temp = OUT_MAXIMUM * 0.9; // 10% headroom for control
     CurrThrottle = Limit(CurrThrottle, 0, Temp );
@@ -247,7 +250,6 @@
     static uint8 r;
 
 #ifdef MULTICOPTER
-
     if ( P[ESCType] ==  ESCHolger )
         for ( m = 0 ; m < NoOfI2CESCOutputs ; m++ ) {
             I2CESC.start();
@@ -306,7 +308,6 @@
 } // StopMotors
 
 void InitMotors(void) {
-    static uint8 m;
 
     Out0.period_us(PWM_PERIOD_US);
 
@@ -315,9 +316,11 @@
 #ifndef Y6COPTER
 #ifdef TRICOPTER
     PWM[BackC] = OUT_NEUTRAL;
-#endif // !TRICOPTER    
+#endif // !TRICOPTER
     PWM[CamRollC] = OUT_NEUTRAL;
     PWM[CamPitchC] = OUT_NEUTRAL;
+    CamRollPulseWidth = 1000 + (int16)( PWM[CamRollC] * PWMScale );
+    CamPitchPulseWidth = 1000 + (int16)( PWM[CamPitchC] * PWMScale );    
 #endif // Y6COPTER
 
 } // InitMotors