![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
UAVX Multicopter Flight Controller.
Diff: outputs.c
- Revision:
- 1:1e3318a30ddd
- Parent:
- 0:62a1c91a859a
- Child:
- 2:90292f8bd179
diff -r 62a1c91a859a -r 1e3318a30ddd outputs.c --- 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