Diff: outputs.c

Revision:

1:1e3318a30ddd

Parent:

0:62a1c91a859a

Child:

2:90292f8bd179

--- a/outputs.c	Fri Feb 18 22:28:05 2011 +0000
+++ b/outputs.c	Fri Feb 25 01:35:24 2011 +0000
@@ -213,17 +213,20 @@
 } // MixAndLimitMotors
 
 void MixAndLimitCam(void) {
-    static real32 Temp; 
 
-    // use only roll/pitch angle estimates
-    Temp = Angle[Roll] * K[CamRollKp];
-    PWM[CamRollC] = Temp + K[CamRollTrim];
-    PWM[CamRollC] = PWM[CamRollC] + OUT_NEUTRAL; // PWMSense[CamRollC] * 
+    static real32 NewCamRoll, NewCamPitch;
+    
+    PWMSense[CamRollC] = 1.0; 
+    PWMSense[CamPitchC] = 1.0;
+ 
+    NewCamRoll = CameraRollAngle * K[CamRollKp] + K[CamRollTrim];
+    NewCamRoll = PWMSense[CamRollC] * NewCamRoll + OUT_NEUTRAL;
+    PWM[CamRollC] = SlewLimit( PWM[CamRollC], NewCamRoll, 2); // change to 10Hz filter
 
-    Temp = Angle[Pitch] * K[CamPitchKp];
-    PWM[CamPitchC] = Temp + DesiredCamPitchTrim;
-    PWM[CamPitchC] = PWM[CamPitchC] + OUT_NEUTRAL; //PWMSense[CamPitchC] * 
-    
+    NewCamPitch = CameraPitchAngle * K[CamPitchKp] + DesiredCamPitchTrim;
+    NewCamPitch = PWMSense[CamPitchC] * NewCamPitch + OUT_NEUTRAL; 
+    PWM[CamPitchC] = SlewLimit( PWM[CamPitchC], NewCamPitch, 2.0); // change to 10Hz filter
+ 
     CamRollPulseWidth = 1000 + (int16)( PWM[CamRollC] * PWMScale );
     CamPitchPulseWidth = 1000 + (int16)( PWM[CamPitchC] * PWMScale );
 
@@ -291,18 +294,13 @@
     PWM[ThrottleC] = ESCMin;
 #endif // MULTICOPTER
 
-    Out0.pulsewidth_us(1000 + (int16)( PWM[FrontC] * PWMScale ) );
-    Out1.pulsewidth_us(1000 + (int16)( PWM[LeftC] * PWMScale ) );
-    Out2.pulsewidth_us(1000 + (int16)( PWM[RightC] * PWMScale ) );
-    Out3.pulsewidth_us(1000 + (int16)( PWM[BackC] * PWMScale ) );
-
-    CamRollPulseWidth = 1000 + (int16)( PWM[CamRollC] * PWMScale );
-    CamPitchPulseWidth = 1000 + (int16)( PWM[CamPitchC] * PWMScale );
+    Out0.pulsewidth_us(1000 + (int16)( PWM[0] * PWMScale ) );
+    Out1.pulsewidth_us(1000 + (int16)( PWM[1] * PWMScale ) );
+    Out2.pulsewidth_us(1000 + (int16)( PWM[2] * PWMScale ) );
+    Out3.pulsewidth_us(1000 + (int16)( PWM[3] * PWMScale ) );
     
-#ifndef SOFTWARE_CAM_PWM 
-    Out4.pulsewidth_us(CamRollPulseWidth);
-    Out5.pulsewidth_us(CamPitchPulseWidth);
-#endif // !SOFTWARE_CAM_PWM
+ //   Out4.pulsewidth_us(1000 + (int16)( PWM[4] * PWMScale ) );
+ //   Out5.pulsewidth_us(1000 + (int16)( PWM[5] * PWMScale ) );
 
     F.MotorsArmed = false;
 } // StopMotors