Prof Greg Egan
/
UAVXArm-GKE
UAVX Multicopter Flight Controller.
control.c
- Committer:
- gke
- Date:
- 2011-04-26
- Revision:
- 2:90292f8bd179
- Parent:
- 1:1e3318a30ddd
File content as of revision 2:90292f8bd179:
// =============================================================================================== // = 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/ = // =============================================================================================== // This is part of UAVXArm. // UAVXArm is free software: you can redistribute it and/or modify it under the terms of the GNU // General Public License as published by the Free Software Foundation, either version 3 of the // License, or (at your option) any later version. // UAVXArm is distributed in the hope that it will be useful,but WITHOUT ANY WARRANTY; without // even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. // See the GNU General Public License for more details. // You should have received a copy of the GNU General Public License along with this program. // If not, see http://www.gnu.org/licenses/ #include "UAVXArm.h" void DoAltitudeHold(void); void UpdateAltitudeSource(void); real32 AltitudeCF( real32, real32); void AltitudeHold(void); void DoOrientationTransform(void); void DoControl(void); void CheckThrottleMoved(void); void LightsAndSirens(void); void InitControl(void); real32 Angle[3], Anglep[3], Rate[3], Ratep[3], YawRateIntE; real32 AccAltComp, AltComp; real32 DescentComp; real32 GS; real32 Rl, Pl, Yl, Ylp; int16 HoldYaw; int16 CruiseThrottle, MaxCruiseThrottle, DesiredThrottle, IdleThrottle, InitialThrottle, StickThrottle; int16 DesiredRoll, DesiredPitch, DesiredYaw, DesiredCamPitchTrim; real32 DesiredHeading; real32 ControlRoll, ControlPitch; real32 CameraRollAngle, CameraPitchAngle; int16 CurrMaxRollPitch; int16 Trim[3]; int16 AttitudeHoldResetCount; real32 AltDiffSum, AltD, AltDSum; real32 DesiredAltitude, Altitude, AltitudeP; real32 ROC; uint32 AltuSp; int16 DescentLimiter; uint32 ControlUpdateTimeuS; real32 GRollKp, GRollKi, GRollKd, GPitchKp, GPitchKi, GPitchKd; boolean FirstPass; int8 BeepTick = 0; real32 AltCF = 0.0; real32 AltF[3] = { 0.0, 0.0, 0.0}; real32 AltitudeCF( real32 AltE, real32 dT ) { // Complementary Filter originally authored by RoyLB originally for attitude estimation // http://www.rcgroups.com/forums/showpost.php?p=12082524&postcount=1286 // Using acceleration as an alias for vertical displacement to avoid integration "issues" static real32 TauCF; TauCF = K[VertDamp]; if ( F.AccelerationsValid && F.NearLevel ) { AltF[0] = (AltE - AltCF) * Sqr(TauCF); AltF[2] += AltF[0] * dT; AltF[1] = AltF[2] + (AltE - AltCF) * 2.0 * TauCF + ( 0.5 * Acc[UD] * Sqr(dT) ); // ??? some scaling on Acc perhaps AltCF += AltF[1] * dT; } else AltCF = AltE; AccAltComp = AltCF - AltE; return( AltCF ); } // AltitudeCF void DoAltitudeHold(void) { // Syncronised to baro intervals independant of active altitude source static int16 NewAltComp; static real32 AltP, AltI, AltD; static real32 LimAltE, AltE; static real32 AltdT, AltdTR; static uint32 Now; Now = uSClock(); AltdT = ( Now - AltuSp ) * 0.000001; AltdT = Limit(AltdT, 0.01, 0.1); // limit range for restarts AltdTR = 1.0 / AltdT; AltuSp = Now; AltE = AltitudeCF( DesiredAltitude - Altitude, AltdT ); LimAltE = Limit1(AltE, ALT_BAND_M); AltP = LimAltE * K[AltKp]; AltP = Limit1(AltP, ALT_MAX_THR_COMP); AltDiffSum += LimAltE; AltDiffSum = Limit1(AltDiffSum, ALT_INT_WINDUP_LIMIT); AltI = AltDiffSum * K[AltKi] * AltdT; AltI = Limit1(AltDiffSum, K[AltIntLimit]); ROC = ( Altitude - AltitudeP ) * AltdTR; // may neeed filtering - noisy AltitudeP = Altitude; AltD = ROC * K[AltKd]; AltD = Limit1(AltD, ALT_MAX_THR_COMP); if ( ROC < ( -K[MaxDescentRateDmpS] * 10.0 ) ) { DescentLimiter += 1; DescentLimiter = Limit(DescentLimiter, 0, ALT_MAX_THR_COMP * 2.0); } else DescentLimiter = DecayX(DescentLimiter, 1); NewAltComp = AltP + AltI + AltD + AltDSum + DescentLimiter; NewAltComp = Limit1(NewAltComp, ALT_MAX_THR_COMP); AltComp = SlewLimit(AltComp, NewAltComp, 1.0); if ( ROC > Stats[MaxROCS] ) Stats[MaxROCS] = ROC; else if ( ROC < Stats[MinROCS] ) Stats[MinROCS] = ROC; } // DoAltitudeHold void UpdateAltitudeSource(void) { if ( F.UsingRangefinderAlt ) Altitude = RangefinderAltitude; else Altitude = BaroRelAltitude; } // UpdateAltitudeSource void AltitudeHold() { static int16 NewCruiseThrottle; GetBaroAltitude(); GetRangefinderAltitude(); CheckThrottleMoved(); if ( F.AltHoldEnabled ) { if ( F.NewBaroValue ) { // sync on Baro which MUST be working F.NewBaroValue = false; UpdateAltitudeSource(); if ( ( NavState != HoldingStation ) && F.AllowNavAltitudeHold ) { // Navigating - using CruiseThrottle F.HoldingAlt = true; DoAltitudeHold(); } else if ( F.ThrottleMoving ) { F.HoldingAlt = false; DesiredAltitude = Altitude; AltComp = Decay1(AltComp); } else { F.HoldingAlt = true; if ( fabs(ROC) < ALT_HOLD_MAX_ROC_MPS ) { NewCruiseThrottle = DesiredThrottle + AltComp; CruiseThrottle = HardFilter(CruiseThrottle, NewCruiseThrottle); CruiseThrottle = Limit( CruiseThrottle , IdleThrottle, MaxCruiseThrottle ); } DoAltitudeHold(); } } } else { AltComp = Decay1(AltComp); ROC = 0.0; F.HoldingAlt = false; } } // AltitudeHold void DoOrientationTransform(void) { static real32 OSO, OCO; if ( F.UsingPolar ) { OSO = OSin[PolarOrientation]; OCO = OCos[PolarOrientation]; } else { OSO = OSin[Orientation]; OCO = OCos[Orientation]; } if ( !F.NavigationActive ) NavCorr[Roll] = NavCorr[Pitch] = NavCorr[Yaw] = 0; // -PS+RC ControlRoll = (int16)( -DesiredPitch * OSO + DesiredRoll * OCO ); // PC+RS ControlPitch = (int16)( DesiredPitch * OCO + DesiredRoll * OSO ); CameraRollAngle = Angle[Pitch] * OSO + Angle[Roll] * OCO; CameraPitchAngle = Angle[Pitch] * OCO - Angle[Roll] * OSO; } // DoOrientationTransform void GainSchedule(void) { /* // rudimentary gain scheduling (linear) static int16 AttDiff, ThrDiff; if ( (!F.NavigationActive) || ( F.NavigationActive && (NavState == HoldingStation ) ) ) { // also density altitude? if ( P[Acro] > 0) // due to Foliage 2009 and Alexinparis 2010 { AttDiff = CurrMaxRollPitch - ATTITUDE_HOLD_LIMIT; GS = GS * ( 1000.0 - (AttDiff * (int16)P[Acro]) ); GS *= 0.001; GS = Limit(GS, 0, 1.0); } if ( P[GSThrottle] > 0 ) { ThrDiff = DesiredThrottle - CruiseThrottle; GS = (int32)GS * ( 1000.0 + (ThrDiff * (int16)P[GSThrottle]) ); GS *= 0.001; } } */ GS = 1.0; // Temp } // GainSchedule const real32 RelayKcu = ( 4.0 * 10 )/ ( PI * 0.8235 ); // stimulus 10 => Kcu 15.5 const real32 RelayPd = 2.0 * 1.285; const real32 RelayStim = 3.0; real32 RelayA = 0.0; real32 RelayTau = 0.0; uint32 RelayIteration = 0; real32 RelayP, RelayW; void DoRelayTuning(void) { static real32 Temp; Temp = fabs(Angle[Roll]); if ( Temp > RelayA ) RelayA = Temp; if ( ( RelayP < 0.0 ) && ( Angle[Roll] >= 0.0 ) ) { RelayTau = RelayIteration * dT; RelayP = - (PI * RelayA) / (4.0 * RelayStim); RelayW = (2.0 * PI) / RelayTau; #ifndef PID_RAW SendPIDTuning(); #endif // PID_RAW RelayA = 0.0; RelayIteration = 0; } RelayIteration++; RelayP = Angle[Roll]; } // DoRelayTuning void Relay(void) { if ( Angle[Roll] < 0.0 ) Rl = -RelayStim; else Rl = +RelayStim; DesiredRoll = Rl; } // Relay void DoControl(void) { static real32 RateE; GetAttitude(); AltitudeHold(); #ifdef SIMULATE FakeDesiredRoll = DesiredRoll + NavRCorr; FakeDesiredPitch = DesiredPitch + NavPCorr; FakeDesiredYaw = DesiredYaw + NavYCorr; Angle[Roll] = SlewLimit(Angle[Roll], FakeDesiredRoll * 16.0, 4.0); Angle[Pitch] = SlewLimit(Angle[Pitch], FakeDesiredPitch * 16.0, 4.0); Angle[Yaw] = SlewLimit(Angle[Yaw], FakeDesiredYaw, 4.0); Rl = FakeDesiredRoll; Pl = FakeDesiredPitch; Yl = DesiredYaw; #else DoOrientationTransform(); GainSchedule(); #ifdef DISABLE_EXTRAS // for commissioning AccAltComp = AltComp = 0.0; NavCorr[Roll] = NavCorr[Pitch] = NavCorr[Yaw] = 0; #endif // DISABLE_EXTRAS // Roll #ifdef USE_ANGLE_DERIVED_RATE // Gyro rate noisy so compute from angle RateE = ( Angle[Roll] - Anglep[Roll] ) * dTR; #else RateE = Rate[Roll]; #endif // USE_ANGLE_DERIVED_RATE Rl = RateE * GRollKp + Angle[Roll] * GRollKi + ( RateE - Ratep[Roll] ) * GRollKd * dTR; Rl += ControlRoll + NavCorr[Roll]; #ifdef USE_ANGLE_DERIVED_RATE Ratep[Roll] = RateE; Anglep[Roll] = Angle[Roll]; #else Ratep[Roll] = Rate[Roll]; #endif // USE_ANGLE_DERIVED_RATE // Pitch #ifdef USE_ANGLE_DERIVED_RATE // Gyro rate noisy so compute from angle RateE = ( Angle[Pitch] - Anglep[Pitch] ) * dTR; #else RateE = Rate[Pitch]; #endif // USE_ANGLE_DERIVED_RATE Pl = RateE * GPitchKp + Angle[Pitch] * GPitchKi + ( RateE - Ratep[Pitch] ) * GPitchKd * dTR; Pl += ControlPitch + NavCorr[Pitch]; #ifdef USE_ANGLE_DERIVED_RATE Ratep[Pitch] = RateE; Anglep[Pitch] = Angle[Pitch]; #else Ratep[Pitch] = Rate[Pitch]; #endif // USE_ANGLE_DERIVED_RATE // Yaw #define MAX_YAW_RATE (HALFPI / RC_NEUTRAL) // Radians/Sec e.g. HalfPI is 90deg/sec DoLegacyYawComp(AttitudeMethod); // returns Angle as heading error along with compensated rate RateE = Rate[Yaw] + ( DesiredYaw + NavCorr[Yaw] ) * MAX_YAW_RATE; YawRateIntE += RateE; YawRateIntE = Limit1(YawRateIntE, YawIntLimit); Yl = RateE * K[YawKp] + YawRateIntE * K[YawKi]; #ifdef TRICOPTER Yl = SlewLimit(Ylp, Yl, 2.0); Ylp = Yl; Yl = Limit1(Yl, K[YawLimit] * 4.0); #else Yl = Limit1(Yl, K[YawLimit]); // currently 25 default #endif // TRICOPTER #endif // SIMULATE } // DoControl static int8 RCStart = RC_INIT_FRAMES; void LightsAndSirens(void) { static int24 Ch5Timeout; LEDYellow_TOG; if ( F.Signal ) LEDGreen_ON; else LEDGreen_OFF; Beeper_OFF; Ch5Timeout = mSClock() + 500; // mS. do { ProcessCommand(); if ( F.Signal ) { LEDGreen_ON; if ( F.RCNewValues ) { UpdateControls(); if ( --RCStart == 0 ) { // wait until RC filters etc. have settled UpdateParamSetChoice(); MixAndLimitCam(); RCStart = 1; } InitialThrottle = StickThrottle; StickThrottle = 0; OutSignals(); if ( mSClock() > Ch5Timeout ) { if ( F.Navigate || F.ReturnHome || !F.ParametersValid ) { Beeper_TOG; LEDRed_TOG; } else if ( Armed ) LEDRed_TOG; Ch5Timeout += 500; } } } else { LEDRed_ON; LEDGreen_OFF; } ReadParameters(); GetIRAttitude(); // only active if IRSensors selected } while ((!F.Signal) || (Armed && FirstPass) || F.Ch5Active || F.GyroFailure || (!F.AccelerationsValid) || ( InitialThrottle >= RC_THRES_START ) || (!F.ParametersValid) ); FirstPass = false; Beeper_OFF; LEDRed_OFF; LEDGreen_ON; LEDYellow_ON; mS[LastBattery] = mSClock(); mS[FailsafeTimeout] = mSClock() + FAILSAFE_TIMEOUT_MS; F.LostModel = false; FailState = MonitoringRx; } // LightsAndSirens void InitControl(void) { static uint8 i; AltuSp = DescentLimiter = 0; for ( i = 0; i < (uint8)3; i++ ) Angle[i] = Anglep[i] = Rate[i] = Vel[i] = 0.0; AccAltComp = AltComp = 0.0; YawRateIntE = 0.0; AltSum = Ylp = AltitudeP = 0.0; ControlUpdateTimeuS = 0; } // InitControl