control.c

00001 // ===============================================================================================
00002 // =                              UAVXArm Quadrocopter Controller                                =
00003 // =                           Copyright (c) 2008 by Prof. Greg Egan                             =
00004 // =                 Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer                   =
00005 // =                           http://code.google.com/p/uavp-mods/                               =
00006 // ===============================================================================================
00007 
00008 //    This is part of UAVXArm.
00009 
00010 //    UAVXArm is free software: you can redistribute it and/or modify it under the terms of the GNU
00011 //    General Public License as published by the Free Software Foundation, either version 3 of the
00012 //    License, or (at your option) any later version.
00013 
00014 //    UAVXArm is distributed in the hope that it will be useful,but WITHOUT ANY WARRANTY; without
00015 //    even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
00016 //    See the GNU General Public License for more details.
00017 
00018 //    You should have received a copy of the GNU General Public License along with this program.
00019 //    If not, see http://www.gnu.org/licenses/
00020 
00021 #include "UAVXArm.h"
00022 
00023 void DoAltitudeHold(void);
00024 void UpdateAltitudeSource(void);
00025 real32 AltitudeCF( real32, real32);
00026 void AltitudeHold(void);
00027 void DoOrientationTransform(void);
00028 void DoControl(void);
00029 
00030 void CheckThrottleMoved(void);
00031 void LightsAndSirens(void);
00032 void InitControl(void);
00033 
00034 real32 Angle[3], Anglep[3], Rate[3], Ratep[3], YawRateIntE;
00035 real32 AccAltComp, AltComp;
00036 real32 DescentComp;
00037 
00038 real32 GS;
00039 real32 Rl, Pl, Yl, Ylp;
00040 int16 HoldYaw;
00041 int16 CruiseThrottle, MaxCruiseThrottle, DesiredThrottle, IdleThrottle, InitialThrottle, StickThrottle;
00042 int16 DesiredRoll, DesiredPitch, DesiredYaw, DesiredCamPitchTrim;
00043 real32 DesiredHeading;
00044 real32 ControlRoll, ControlPitch;
00045 real32 CameraRollAngle, CameraPitchAngle;
00046 int16 CurrMaxRollPitch;
00047 int16 Trim[3];
00048 
00049 int16 AttitudeHoldResetCount;
00050 real32 AltDiffSum, AltD, AltDSum;
00051 real32 DesiredAltitude, Altitude, AltitudeP;
00052 real32 ROC;
00053 
00054 uint32 AltuSp;
00055 int16 DescentLimiter;
00056 uint32 ControlUpdateTimeuS;
00057 
00058 real32 GRollKp, GRollKi, GRollKd, GPitchKp, GPitchKi, GPitchKd;
00059 
00060 boolean FirstPass;
00061 int8 BeepTick = 0;
00062 
00063 
00064 
00065 real32 AltCF = 0.0;
00066 real32 AltF[3] = { 0.0, 0.0, 0.0};
00067 
00068 real32 AltitudeCF( real32 AltE, real32 dT ) {
00069 
00070     // Complementary Filter originally authored by RoyLB originally for attitude estimation
00071     // http://www.rcgroups.com/forums/showpost.php?p=12082524&postcount=1286
00072     
00073     // Using acceleration as an alias for vertical displacement to avoid integration "issues"
00074     
00075     static real32 TauCF;
00076      
00077     TauCF = K[VertDamp];
00078 
00079     if ( F.AccelerationsValid && F.NearLevel ) {
00080 
00081         AltF[0] = (AltE - AltCF) * Sqr(TauCF);
00082         AltF[2] += AltF[0] * dT;
00083         
00084         AltF[1] =  AltF[2] + (AltE - AltCF) * 2.0 * TauCF + ( 0.5 *  Acc[UD] * Sqr(dT) ); // ??? some scaling on Acc perhaps
00085         AltCF += AltF[1] * dT;
00086 
00087     } else
00088         AltCF = AltE;
00089         
00090     AccAltComp = AltCF - AltE;
00091 
00092     return( AltCF );
00093 
00094 } // AltitudeCF
00095 
00096 
00097 void DoAltitudeHold(void) { // Syncronised to baro intervals independant of active altitude source
00098 
00099     static int16 NewAltComp;
00100     static real32 AltP, AltI, AltD;
00101     static real32 LimAltE, AltE;
00102     static real32 AltdT, AltdTR;
00103     static uint32 Now;
00104 
00105     Now = uSClock();
00106     AltdT = ( Now - AltuSp ) * 0.000001;
00107     AltdT = Limit(AltdT, 0.01, 0.1); // limit range for restarts
00108     AltdTR = 1.0 / AltdT;
00109     AltuSp = Now;
00110 
00111     AltE = AltitudeCF( DesiredAltitude - Altitude, AltdT );
00112     LimAltE = Limit1(AltE, ALT_BAND_M);
00113 
00114     AltP = LimAltE * K[AltKp];
00115     AltP = Limit1(AltP, ALT_MAX_THR_COMP);
00116 
00117     AltDiffSum += LimAltE;
00118     AltDiffSum = Limit1(AltDiffSum, ALT_INT_WINDUP_LIMIT);
00119     AltI = AltDiffSum * K[AltKi] * AltdT;
00120     AltI = Limit1(AltDiffSum, K[AltIntLimit]);
00121 
00122     ROC = ( Altitude - AltitudeP ) * AltdTR; // may neeed filtering - noisy
00123     AltitudeP = Altitude;
00124 
00125     AltD = ROC * K[AltKd];
00126     AltD = Limit1(AltD, ALT_MAX_THR_COMP);
00127 
00128     if ( ROC < ( -K[MaxDescentRateDmpS] * 10.0 ) ) {
00129         DescentLimiter += 1;
00130         DescentLimiter = Limit(DescentLimiter, 0, ALT_MAX_THR_COMP * 2.0);
00131     } else
00132         DescentLimiter = DecayX(DescentLimiter, 1);
00133 
00134     NewAltComp = AltP + AltI + AltD + AltDSum + DescentLimiter;
00135     NewAltComp = Limit1(NewAltComp, ALT_MAX_THR_COMP);
00136     AltComp = SlewLimit(AltComp, NewAltComp, 1.0);
00137 
00138     if ( ROC > Stats[MaxROCS] )
00139         Stats[MaxROCS] = ROC;
00140     else
00141         if ( ROC < Stats[MinROCS] )
00142             Stats[MinROCS] = ROC;
00143 
00144 } // DoAltitudeHold
00145 
00146 void UpdateAltitudeSource(void) {
00147     if ( F.UsingRangefinderAlt )
00148         Altitude = RangefinderAltitude;
00149     else
00150         Altitude = BaroRelAltitude;
00151 
00152 } // UpdateAltitudeSource
00153 
00154 void AltitudeHold() {
00155     static int16 NewCruiseThrottle;
00156 
00157     GetBaroAltitude();
00158     GetRangefinderAltitude();
00159     CheckThrottleMoved();
00160 
00161     if ( F.AltHoldEnabled ) {
00162         if ( F.NewBaroValue  ) { // sync on Baro which MUST be working
00163             F.NewBaroValue = false;
00164 
00165             UpdateAltitudeSource();
00166 
00167             if ( ( NavState != HoldingStation ) && F.AllowNavAltitudeHold ) { // Navigating - using CruiseThrottle
00168                 F.HoldingAlt = true;
00169                 DoAltitudeHold();
00170             } else
00171                 if ( F.ThrottleMoving ) {
00172                     F.HoldingAlt = false;
00173                     DesiredAltitude = Altitude;
00174                     AltComp = Decay1(AltComp);
00175                 } else {
00176                     F.HoldingAlt = true;
00177                     if ( fabs(ROC) < ALT_HOLD_MAX_ROC_MPS  ) {
00178                         NewCruiseThrottle = DesiredThrottle + AltComp;
00179                         CruiseThrottle = HardFilter(CruiseThrottle, NewCruiseThrottle);
00180                         CruiseThrottle = Limit( CruiseThrottle , IdleThrottle, MaxCruiseThrottle );
00181                     }
00182                     DoAltitudeHold();
00183                 }
00184         }
00185     } else {
00186         AltComp = Decay1(AltComp);
00187         ROC = 0.0;
00188         F.HoldingAlt = false;
00189     }
00190 } // AltitudeHold
00191 
00192 
00193 void DoOrientationTransform(void) {
00194     static real32 OSO, OCO;
00195 
00196     if ( F.UsingPolar ) {
00197         OSO = OSin[PolarOrientation];
00198         OCO = OCos[PolarOrientation];
00199     } else {
00200         OSO = OSin[Orientation];
00201         OCO = OCos[Orientation];
00202     }
00203 
00204     if ( !F.NavigationActive )
00205         NavCorr[Roll] = NavCorr[Pitch] = NavCorr[Yaw] = 0;
00206 
00207     // -PS+RC
00208     ControlRoll = (int16)( -DesiredPitch * OSO + DesiredRoll * OCO );
00209 
00210     // PC+RS
00211     ControlPitch = (int16)( DesiredPitch * OCO + DesiredRoll * OSO );
00212 
00213     CameraRollAngle = Angle[Pitch] * OSO + Angle[Roll] * OCO;
00214     CameraPitchAngle = Angle[Pitch] * OCO - Angle[Roll] * OSO;
00215 
00216 } // DoOrientationTransform
00217 
00218 void GainSchedule(void) {
00219 
00220     /*
00221     // rudimentary gain scheduling (linear)
00222     static int16 AttDiff, ThrDiff;
00223 
00224     if ( (!F.NavigationActive) || ( F.NavigationActive && (NavState == HoldingStation ) ) )
00225     {
00226         // also density altitude?
00227 
00228         if ( P[Acro] > 0) // due to Foliage 2009 and Alexinparis 2010
00229         {
00230              AttDiff = CurrMaxRollPitch  - ATTITUDE_HOLD_LIMIT;
00231             GS = GS * ( 1000.0 - (AttDiff * (int16)P[Acro]) );
00232             GS *= 0.001;
00233             GS = Limit(GS, 0, 1.0);
00234         }
00235 
00236         if ( P[GSThrottle] > 0 )
00237         {
00238              ThrDiff = DesiredThrottle - CruiseThrottle;
00239             GS = (int32)GS * ( 1000.0 + (ThrDiff * (int16)P[GSThrottle]) );
00240             GS *= 0.001;
00241         }
00242     }
00243 
00244     */
00245 
00246     GS = 1.0; // Temp
00247 
00248 
00249 
00250 } // GainSchedule
00251 
00252 const real32 RelayKcu = ( 4.0 * 10 )/ ( PI * 0.8235 ); // stimulus 10 => Kcu 15.5
00253 const real32 RelayPd  = 2.0 * 1.285;
00254 const real32 RelayStim = 3.0;
00255 
00256 real32 RelayA = 0.0;
00257 real32 RelayTau = 0.0;
00258 uint32 RelayIteration = 0;
00259 real32 RelayP, RelayW;
00260 
00261 void DoRelayTuning(void) {
00262 
00263     static real32 Temp;
00264 
00265     Temp = fabs(Angle[Roll]);
00266     if ( Temp > RelayA ) RelayA = Temp;
00267 
00268     if ( ( RelayP < 0.0 ) && ( Angle[Roll] >= 0.0 ) ) {
00269 
00270         RelayTau = RelayIteration * dT;
00271 
00272         RelayP = - (PI * RelayA) / (4.0 * RelayStim);
00273         RelayW = (2.0 * PI) / RelayTau;
00274 
00275 #ifndef PID_RAW
00276         SendPIDTuning();
00277 #endif // PID_RAW
00278 
00279         RelayA = 0.0;
00280         RelayIteration = 0;
00281     }
00282 
00283     RelayIteration++;
00284     RelayP = Angle[Roll];
00285 
00286 } // DoRelayTuning
00287 
00288 void Relay(void) {
00289 
00290     if ( Angle[Roll] < 0.0 )
00291         Rl = -RelayStim;
00292     else
00293         Rl = +RelayStim;
00294 
00295     DesiredRoll = Rl;
00296 
00297 } // Relay
00298 
00299 void DoControl(void) {
00300 
00301     static real32 RateE;
00302 
00303 
00304     GetAttitude();
00305     AltitudeHold();
00306 
00307 #ifdef SIMULATE
00308 
00309     FakeDesiredRoll = DesiredRoll + NavRCorr;
00310     FakeDesiredPitch = DesiredPitch + NavPCorr;
00311     FakeDesiredYaw =  DesiredYaw + NavYCorr;
00312     Angle[Roll] = SlewLimit(Angle[Roll], FakeDesiredRoll * 16.0, 4.0);
00313     Angle[Pitch] = SlewLimit(Angle[Pitch], FakeDesiredPitch * 16.0, 4.0);
00314     Angle[Yaw] = SlewLimit(Angle[Yaw], FakeDesiredYaw, 4.0);
00315     Rl = FakeDesiredRoll;
00316     Pl = FakeDesiredPitch;
00317     Yl = DesiredYaw;
00318 
00319 #else
00320 
00321     DoOrientationTransform();
00322 
00323     GainSchedule();
00324 
00325 #ifdef DISABLE_EXTRAS
00326     // for commissioning
00327     AccAltComp = AltComp = 0.0;
00328     NavCorr[Roll] = NavCorr[Pitch] = NavCorr[Yaw] = 0;
00329 #endif // DISABLE_EXTRAS
00330 
00331     // Roll
00332 
00333 #ifdef USE_ANGLE_DERIVED_RATE  // Gyro rate noisy so compute from angle
00334     RateE = ( Angle[Roll] - Anglep[Roll] ) * dTR;
00335 #else
00336     RateE = Rate[Roll];
00337 #endif // USE_ANGLE_DERIVED_RATE
00338     Rl  = RateE * GRollKp + Angle[Roll] * GRollKi + ( RateE - Ratep[Roll] ) * GRollKd * dTR;
00339     Rl += ControlRoll + NavCorr[Roll];
00340 
00341 #ifdef USE_ANGLE_DERIVED_RATE
00342     Ratep[Roll] = RateE;
00343     Anglep[Roll] = Angle[Roll];
00344 #else
00345     Ratep[Roll] = Rate[Roll];
00346 #endif // USE_ANGLE_DERIVED_RATE
00347 
00348     // Pitch
00349 
00350 #ifdef USE_ANGLE_DERIVED_RATE // Gyro rate noisy so compute from angle
00351     RateE = ( Angle[Pitch] - Anglep[Pitch] ) * dTR;
00352 #else
00353     RateE = Rate[Pitch];
00354 #endif // USE_ANGLE_DERIVED_RATE
00355     Pl  = RateE * GPitchKp + Angle[Pitch] * GPitchKi + ( RateE - Ratep[Pitch] ) * GPitchKd * dTR;
00356     Pl += ControlPitch + NavCorr[Pitch];
00357 
00358 #ifdef USE_ANGLE_DERIVED_RATE
00359     Ratep[Pitch] = RateE;
00360     Anglep[Pitch] = Angle[Pitch];
00361 #else
00362     Ratep[Pitch] = Rate[Pitch];
00363 #endif // USE_ANGLE_DERIVED_RATE    
00364 
00365     // Yaw
00366 
00367 #define MAX_YAW_RATE  (HALFPI / RC_NEUTRAL)  // Radians/Sec e.g. HalfPI is 90deg/sec
00368 
00369     DoLegacyYawComp(AttitudeMethod); // returns Angle as heading error along with compensated rate
00370 
00371     RateE =  Rate[Yaw] + ( DesiredYaw + NavCorr[Yaw] ) * MAX_YAW_RATE;
00372 
00373     YawRateIntE += RateE;
00374     YawRateIntE = Limit1(YawRateIntE, YawIntLimit);
00375 
00376     Yl =  RateE * K[YawKp] + YawRateIntE * K[YawKi];
00377 
00378 #ifdef TRICOPTER
00379     Yl = SlewLimit(Ylp, Yl, 2.0);
00380     Ylp = Yl;
00381     Yl = Limit1(Yl, K[YawLimit] * 4.0);
00382 #else
00383     Yl = Limit1(Yl, K[YawLimit]); // currently 25 default
00384 #endif // TRICOPTER
00385 
00386 #endif // SIMULATE 
00387 
00388 } // DoControl
00389 
00390 static int8 RCStart = RC_INIT_FRAMES;
00391 
00392 void LightsAndSirens(void) {
00393     static int24 Ch5Timeout;
00394 
00395     LEDYellow_TOG;
00396     if ( F.Signal ) LEDGreen_ON;
00397     else LEDGreen_OFF;
00398 
00399     Beeper_OFF;
00400     Ch5Timeout = mSClock() + 500;                     // mS.
00401 
00402     do {
00403 
00404         ProcessCommand();
00405 
00406         if ( F.Signal ) {
00407             LEDGreen_ON;
00408             if ( F.RCNewValues ) {
00409                 UpdateControls();
00410                 if ( --RCStart == 0 ) { // wait until RC filters etc. have settled
00411                     UpdateParamSetChoice();
00412                     MixAndLimitCam();
00413                     RCStart = 1;
00414                 }
00415 
00416                 InitialThrottle = StickThrottle;
00417                 StickThrottle = 0;
00418                 OutSignals();
00419                 if ( mSClock() > Ch5Timeout ) {
00420                     if ( F.Navigate || F.ReturnHome || !F.ParametersValid ) {
00421                         Beeper_TOG;
00422                         LEDRed_TOG;
00423                     } else
00424                         if ( Armed )
00425                             LEDRed_TOG;
00426 
00427                     Ch5Timeout += 500;
00428                 }
00429             }
00430         } else {
00431             LEDRed_ON;
00432             LEDGreen_OFF;
00433         }
00434         ReadParameters();
00435         GetIRAttitude(); // only active if IRSensors selected
00436     } while ((!F.Signal) || (Armed && FirstPass) || F.Ch5Active || F.GyroFailure || (!F.AccelerationsValid) ||
00437              ( InitialThrottle >= RC_THRES_START ) || (!F.ParametersValid) );
00438 
00439     FirstPass = false;
00440 
00441     Beeper_OFF;
00442     LEDRed_OFF;
00443     LEDGreen_ON;
00444     LEDYellow_ON;
00445 
00446     mS[LastBattery] = mSClock();
00447     mS[FailsafeTimeout] = mSClock() + FAILSAFE_TIMEOUT_MS;
00448 
00449     F.LostModel = false;
00450     FailState = MonitoringRx;
00451 
00452 } // LightsAndSirens
00453 
00454 void InitControl(void) {
00455     static uint8 i;
00456 
00457     AltuSp = DescentLimiter = 0;
00458 
00459     for ( i = 0; i < (uint8)3; i++ )
00460         Angle[i] = Anglep[i] = Rate[i] = Vel[i] =  0.0;
00461        
00462     AccAltComp = AltComp = 0.0;
00463 
00464     YawRateIntE = 0.0;
00465 
00466     AltSum = Ylp =  AltitudeP = 0.0;
00467     ControlUpdateTimeuS = 0;
00468 
00469 } // InitControl
00470