Diff: control.c

Revision:

1:1e3318a30ddd

Parent:

0:62a1c91a859a

Child:

2:90292f8bd179

--- a/control.c	Fri Feb 18 22:28:05 2011 +0000
+++ b/control.c	Fri Feb 25 01:35:24 2011 +0000
@@ -20,6 +20,8 @@
 
 #include "UAVXArm.h"
 
+real32 PTerm, ITerm, DTerm;
+
 void DoAltitudeHold(void);
 void UpdateAltitudeSource(void);
 void AltitudeHold(void);
@@ -43,6 +45,7 @@
 int16 CruiseThrottle, MaxCruiseThrottle, DesiredThrottle, IdleThrottle, InitialThrottle, StickThrottle;
 int16 DesiredRoll, DesiredPitch, DesiredYaw, DesiredHeading, DesiredCamPitchTrim;
 real32 ControlRoll, ControlPitch, ControlRollP, ControlPitchP;
+real32 CameraRollAngle, CameraPitchAngle;
 int16 CurrMaxRollPitch;
 
 int16 AttitudeHoldResetCount;
@@ -219,6 +222,9 @@
 
     // PC+RS
     ControlPitch = (int16)( DesiredPitch * OCO + DesiredRoll * OSO );
+    
+    CameraRollAngle = Angle[Pitch] * OSO + Angle[Roll] * OCO;
+    CameraPitchAngle = Angle[Pitch] * OCO - Angle[Roll] * OSO;
 
 } // DoOrientationTransform
 
@@ -263,6 +269,7 @@
 } // GainSchedule
 
 void DoControl(void) {
+
     GetAttitude();
     AltitudeHold();
     InertialDamping();
@@ -289,9 +296,7 @@
     // for commissioning
     Comp[BF] = Comp[LR] = Comp[UD] = Comp[Alt] = 0;
     NavCorr[Roll] = NavCorr[Pitch] = NavCorr[Yaw] = 0;
-
     F.UsingAngleControl = false;
-
 #endif // DISABLE_EXTRAS
 
     if ( F.UsingAngleControl ) {
@@ -317,7 +322,7 @@
         AngleE[Roll] = Limit(Angle[Roll],  -K[RollIntLimit], K[RollIntLimit]);
         Rl  = Rate[Roll] * GRollKp + AngleE[Roll] * GRollKi + (Rate[Roll]-Ratep[Roll]) * GRollKd * dTR;
         Rl -=  NavCorr[Roll] - Comp[LR];
-
+          
         Rl *= GS;
 
         Rl -= ControlRoll;
@@ -330,7 +335,7 @@
         AngleE[Pitch] = Limit(Angle[Pitch],  -K[PitchIntLimit], K[PitchIntLimit]);
         Pl  = Rate[Pitch] * GPitchKp + AngleE[Pitch] * GPitchKi + (Rate[Pitch]-Ratep[Pitch]) * GPitchKd * dTR;
         Pl -= NavCorr[Pitch] - Comp[BF];
-
+        
         Pl *= GS;
 
         Pl -= ControlPitch;