Prof Greg Egan
/
UAVXArm-GKE
UAVX Multicopter Flight Controller.
control.c@2:90292f8bd179, 2011-04-26 (annotated)
- Committer:
- gke
- Date:
- Tue Apr 26 12:12:29 2011 +0000
- Revision:
- 2:90292f8bd179
- Parent:
- 1:1e3318a30ddd
Not flightworthy. Posted for others to make use of the I2C SW code.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
gke | 0:62a1c91a859a | 1 | // =============================================================================================== |
gke | 0:62a1c91a859a | 2 | // = UAVXArm Quadrocopter Controller = |
gke | 0:62a1c91a859a | 3 | // = Copyright (c) 2008 by Prof. Greg Egan = |
gke | 0:62a1c91a859a | 4 | // = Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer = |
gke | 2:90292f8bd179 | 5 | // = http://code.google.com/p/uavp-mods/ = |
gke | 0:62a1c91a859a | 6 | // =============================================================================================== |
gke | 0:62a1c91a859a | 7 | |
gke | 0:62a1c91a859a | 8 | // This is part of UAVXArm. |
gke | 0:62a1c91a859a | 9 | |
gke | 0:62a1c91a859a | 10 | // UAVXArm is free software: you can redistribute it and/or modify it under the terms of the GNU |
gke | 0:62a1c91a859a | 11 | // General Public License as published by the Free Software Foundation, either version 3 of the |
gke | 0:62a1c91a859a | 12 | // License, or (at your option) any later version. |
gke | 0:62a1c91a859a | 13 | |
gke | 0:62a1c91a859a | 14 | // UAVXArm is distributed in the hope that it will be useful,but WITHOUT ANY WARRANTY; without |
gke | 0:62a1c91a859a | 15 | // even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. |
gke | 0:62a1c91a859a | 16 | // See the GNU General Public License for more details. |
gke | 0:62a1c91a859a | 17 | |
gke | 0:62a1c91a859a | 18 | // You should have received a copy of the GNU General Public License along with this program. |
gke | 0:62a1c91a859a | 19 | // If not, see http://www.gnu.org/licenses/ |
gke | 0:62a1c91a859a | 20 | |
gke | 0:62a1c91a859a | 21 | #include "UAVXArm.h" |
gke | 0:62a1c91a859a | 22 | |
gke | 0:62a1c91a859a | 23 | void DoAltitudeHold(void); |
gke | 0:62a1c91a859a | 24 | void UpdateAltitudeSource(void); |
gke | 2:90292f8bd179 | 25 | real32 AltitudeCF( real32, real32); |
gke | 0:62a1c91a859a | 26 | void AltitudeHold(void); |
gke | 0:62a1c91a859a | 27 | void DoOrientationTransform(void); |
gke | 0:62a1c91a859a | 28 | void DoControl(void); |
gke | 0:62a1c91a859a | 29 | |
gke | 0:62a1c91a859a | 30 | void CheckThrottleMoved(void); |
gke | 0:62a1c91a859a | 31 | void LightsAndSirens(void); |
gke | 0:62a1c91a859a | 32 | void InitControl(void); |
gke | 0:62a1c91a859a | 33 | |
gke | 2:90292f8bd179 | 34 | real32 Angle[3], Anglep[3], Rate[3], Ratep[3], YawRateIntE; |
gke | 2:90292f8bd179 | 35 | real32 AccAltComp, AltComp; |
gke | 0:62a1c91a859a | 36 | real32 DescentComp; |
gke | 0:62a1c91a859a | 37 | |
gke | 0:62a1c91a859a | 38 | real32 GS; |
gke | 0:62a1c91a859a | 39 | real32 Rl, Pl, Yl, Ylp; |
gke | 0:62a1c91a859a | 40 | int16 HoldYaw; |
gke | 0:62a1c91a859a | 41 | int16 CruiseThrottle, MaxCruiseThrottle, DesiredThrottle, IdleThrottle, InitialThrottle, StickThrottle; |
gke | 2:90292f8bd179 | 42 | int16 DesiredRoll, DesiredPitch, DesiredYaw, DesiredCamPitchTrim; |
gke | 2:90292f8bd179 | 43 | real32 DesiredHeading; |
gke | 2:90292f8bd179 | 44 | real32 ControlRoll, ControlPitch; |
gke | 1:1e3318a30ddd | 45 | real32 CameraRollAngle, CameraPitchAngle; |
gke | 0:62a1c91a859a | 46 | int16 CurrMaxRollPitch; |
gke | 2:90292f8bd179 | 47 | int16 Trim[3]; |
gke | 0:62a1c91a859a | 48 | |
gke | 0:62a1c91a859a | 49 | int16 AttitudeHoldResetCount; |
gke | 0:62a1c91a859a | 50 | real32 AltDiffSum, AltD, AltDSum; |
gke | 0:62a1c91a859a | 51 | real32 DesiredAltitude, Altitude, AltitudeP; |
gke | 0:62a1c91a859a | 52 | real32 ROC; |
gke | 0:62a1c91a859a | 53 | |
gke | 0:62a1c91a859a | 54 | uint32 AltuSp; |
gke | 0:62a1c91a859a | 55 | int16 DescentLimiter; |
gke | 2:90292f8bd179 | 56 | uint32 ControlUpdateTimeuS; |
gke | 0:62a1c91a859a | 57 | |
gke | 0:62a1c91a859a | 58 | real32 GRollKp, GRollKi, GRollKd, GPitchKp, GPitchKi, GPitchKd; |
gke | 0:62a1c91a859a | 59 | |
gke | 2:90292f8bd179 | 60 | boolean FirstPass; |
gke | 0:62a1c91a859a | 61 | int8 BeepTick = 0; |
gke | 0:62a1c91a859a | 62 | |
gke | 2:90292f8bd179 | 63 | |
gke | 2:90292f8bd179 | 64 | |
gke | 2:90292f8bd179 | 65 | real32 AltCF = 0.0; |
gke | 2:90292f8bd179 | 66 | real32 AltF[3] = { 0.0, 0.0, 0.0}; |
gke | 2:90292f8bd179 | 67 | |
gke | 2:90292f8bd179 | 68 | real32 AltitudeCF( real32 AltE, real32 dT ) { |
gke | 2:90292f8bd179 | 69 | |
gke | 2:90292f8bd179 | 70 | // Complementary Filter originally authored by RoyLB originally for attitude estimation |
gke | 2:90292f8bd179 | 71 | // http://www.rcgroups.com/forums/showpost.php?p=12082524&postcount=1286 |
gke | 2:90292f8bd179 | 72 | |
gke | 2:90292f8bd179 | 73 | // Using acceleration as an alias for vertical displacement to avoid integration "issues" |
gke | 2:90292f8bd179 | 74 | |
gke | 2:90292f8bd179 | 75 | static real32 TauCF; |
gke | 2:90292f8bd179 | 76 | |
gke | 2:90292f8bd179 | 77 | TauCF = K[VertDamp]; |
gke | 2:90292f8bd179 | 78 | |
gke | 2:90292f8bd179 | 79 | if ( F.AccelerationsValid && F.NearLevel ) { |
gke | 2:90292f8bd179 | 80 | |
gke | 2:90292f8bd179 | 81 | AltF[0] = (AltE - AltCF) * Sqr(TauCF); |
gke | 2:90292f8bd179 | 82 | AltF[2] += AltF[0] * dT; |
gke | 2:90292f8bd179 | 83 | |
gke | 2:90292f8bd179 | 84 | AltF[1] = AltF[2] + (AltE - AltCF) * 2.0 * TauCF + ( 0.5 * Acc[UD] * Sqr(dT) ); // ??? some scaling on Acc perhaps |
gke | 2:90292f8bd179 | 85 | AltCF += AltF[1] * dT; |
gke | 2:90292f8bd179 | 86 | |
gke | 2:90292f8bd179 | 87 | } else |
gke | 2:90292f8bd179 | 88 | AltCF = AltE; |
gke | 2:90292f8bd179 | 89 | |
gke | 2:90292f8bd179 | 90 | AccAltComp = AltCF - AltE; |
gke | 2:90292f8bd179 | 91 | |
gke | 2:90292f8bd179 | 92 | return( AltCF ); |
gke | 2:90292f8bd179 | 93 | |
gke | 2:90292f8bd179 | 94 | } // AltitudeCF |
gke | 2:90292f8bd179 | 95 | |
gke | 2:90292f8bd179 | 96 | |
gke | 0:62a1c91a859a | 97 | void DoAltitudeHold(void) { // Syncronised to baro intervals independant of active altitude source |
gke | 0:62a1c91a859a | 98 | |
gke | 0:62a1c91a859a | 99 | static int16 NewAltComp; |
gke | 0:62a1c91a859a | 100 | static real32 AltP, AltI, AltD; |
gke | 0:62a1c91a859a | 101 | static real32 LimAltE, AltE; |
gke | 0:62a1c91a859a | 102 | static real32 AltdT, AltdTR; |
gke | 0:62a1c91a859a | 103 | static uint32 Now; |
gke | 0:62a1c91a859a | 104 | |
gke | 0:62a1c91a859a | 105 | Now = uSClock(); |
gke | 0:62a1c91a859a | 106 | AltdT = ( Now - AltuSp ) * 0.000001; |
gke | 0:62a1c91a859a | 107 | AltdT = Limit(AltdT, 0.01, 0.1); // limit range for restarts |
gke | 0:62a1c91a859a | 108 | AltdTR = 1.0 / AltdT; |
gke | 0:62a1c91a859a | 109 | AltuSp = Now; |
gke | 0:62a1c91a859a | 110 | |
gke | 2:90292f8bd179 | 111 | AltE = AltitudeCF( DesiredAltitude - Altitude, AltdT ); |
gke | 2:90292f8bd179 | 112 | LimAltE = Limit1(AltE, ALT_BAND_M); |
gke | 0:62a1c91a859a | 113 | |
gke | 0:62a1c91a859a | 114 | AltP = LimAltE * K[AltKp]; |
gke | 2:90292f8bd179 | 115 | AltP = Limit1(AltP, ALT_MAX_THR_COMP); |
gke | 0:62a1c91a859a | 116 | |
gke | 0:62a1c91a859a | 117 | AltDiffSum += LimAltE; |
gke | 2:90292f8bd179 | 118 | AltDiffSum = Limit1(AltDiffSum, ALT_INT_WINDUP_LIMIT); |
gke | 0:62a1c91a859a | 119 | AltI = AltDiffSum * K[AltKi] * AltdT; |
gke | 2:90292f8bd179 | 120 | AltI = Limit1(AltDiffSum, K[AltIntLimit]); |
gke | 0:62a1c91a859a | 121 | |
gke | 0:62a1c91a859a | 122 | ROC = ( Altitude - AltitudeP ) * AltdTR; // may neeed filtering - noisy |
gke | 0:62a1c91a859a | 123 | AltitudeP = Altitude; |
gke | 0:62a1c91a859a | 124 | |
gke | 0:62a1c91a859a | 125 | AltD = ROC * K[AltKd]; |
gke | 2:90292f8bd179 | 126 | AltD = Limit1(AltD, ALT_MAX_THR_COMP); |
gke | 0:62a1c91a859a | 127 | |
gke | 0:62a1c91a859a | 128 | if ( ROC < ( -K[MaxDescentRateDmpS] * 10.0 ) ) { |
gke | 0:62a1c91a859a | 129 | DescentLimiter += 1; |
gke | 0:62a1c91a859a | 130 | DescentLimiter = Limit(DescentLimiter, 0, ALT_MAX_THR_COMP * 2.0); |
gke | 0:62a1c91a859a | 131 | } else |
gke | 0:62a1c91a859a | 132 | DescentLimiter = DecayX(DescentLimiter, 1); |
gke | 0:62a1c91a859a | 133 | |
gke | 0:62a1c91a859a | 134 | NewAltComp = AltP + AltI + AltD + AltDSum + DescentLimiter; |
gke | 2:90292f8bd179 | 135 | NewAltComp = Limit1(NewAltComp, ALT_MAX_THR_COMP); |
gke | 2:90292f8bd179 | 136 | AltComp = SlewLimit(AltComp, NewAltComp, 1.0); |
gke | 0:62a1c91a859a | 137 | |
gke | 0:62a1c91a859a | 138 | if ( ROC > Stats[MaxROCS] ) |
gke | 0:62a1c91a859a | 139 | Stats[MaxROCS] = ROC; |
gke | 0:62a1c91a859a | 140 | else |
gke | 0:62a1c91a859a | 141 | if ( ROC < Stats[MinROCS] ) |
gke | 0:62a1c91a859a | 142 | Stats[MinROCS] = ROC; |
gke | 0:62a1c91a859a | 143 | |
gke | 0:62a1c91a859a | 144 | } // DoAltitudeHold |
gke | 0:62a1c91a859a | 145 | |
gke | 0:62a1c91a859a | 146 | void UpdateAltitudeSource(void) { |
gke | 0:62a1c91a859a | 147 | if ( F.UsingRangefinderAlt ) |
gke | 0:62a1c91a859a | 148 | Altitude = RangefinderAltitude; |
gke | 0:62a1c91a859a | 149 | else |
gke | 0:62a1c91a859a | 150 | Altitude = BaroRelAltitude; |
gke | 0:62a1c91a859a | 151 | |
gke | 0:62a1c91a859a | 152 | } // UpdateAltitudeSource |
gke | 0:62a1c91a859a | 153 | |
gke | 0:62a1c91a859a | 154 | void AltitudeHold() { |
gke | 0:62a1c91a859a | 155 | static int16 NewCruiseThrottle; |
gke | 0:62a1c91a859a | 156 | |
gke | 0:62a1c91a859a | 157 | GetBaroAltitude(); |
gke | 0:62a1c91a859a | 158 | GetRangefinderAltitude(); |
gke | 0:62a1c91a859a | 159 | CheckThrottleMoved(); |
gke | 0:62a1c91a859a | 160 | |
gke | 0:62a1c91a859a | 161 | if ( F.AltHoldEnabled ) { |
gke | 0:62a1c91a859a | 162 | if ( F.NewBaroValue ) { // sync on Baro which MUST be working |
gke | 0:62a1c91a859a | 163 | F.NewBaroValue = false; |
gke | 0:62a1c91a859a | 164 | |
gke | 0:62a1c91a859a | 165 | UpdateAltitudeSource(); |
gke | 0:62a1c91a859a | 166 | |
gke | 0:62a1c91a859a | 167 | if ( ( NavState != HoldingStation ) && F.AllowNavAltitudeHold ) { // Navigating - using CruiseThrottle |
gke | 0:62a1c91a859a | 168 | F.HoldingAlt = true; |
gke | 0:62a1c91a859a | 169 | DoAltitudeHold(); |
gke | 0:62a1c91a859a | 170 | } else |
gke | 0:62a1c91a859a | 171 | if ( F.ThrottleMoving ) { |
gke | 0:62a1c91a859a | 172 | F.HoldingAlt = false; |
gke | 0:62a1c91a859a | 173 | DesiredAltitude = Altitude; |
gke | 2:90292f8bd179 | 174 | AltComp = Decay1(AltComp); |
gke | 0:62a1c91a859a | 175 | } else { |
gke | 0:62a1c91a859a | 176 | F.HoldingAlt = true; |
gke | 0:62a1c91a859a | 177 | if ( fabs(ROC) < ALT_HOLD_MAX_ROC_MPS ) { |
gke | 2:90292f8bd179 | 178 | NewCruiseThrottle = DesiredThrottle + AltComp; |
gke | 0:62a1c91a859a | 179 | CruiseThrottle = HardFilter(CruiseThrottle, NewCruiseThrottle); |
gke | 0:62a1c91a859a | 180 | CruiseThrottle = Limit( CruiseThrottle , IdleThrottle, MaxCruiseThrottle ); |
gke | 0:62a1c91a859a | 181 | } |
gke | 0:62a1c91a859a | 182 | DoAltitudeHold(); |
gke | 0:62a1c91a859a | 183 | } |
gke | 0:62a1c91a859a | 184 | } |
gke | 0:62a1c91a859a | 185 | } else { |
gke | 2:90292f8bd179 | 186 | AltComp = Decay1(AltComp); |
gke | 0:62a1c91a859a | 187 | ROC = 0.0; |
gke | 0:62a1c91a859a | 188 | F.HoldingAlt = false; |
gke | 0:62a1c91a859a | 189 | } |
gke | 0:62a1c91a859a | 190 | } // AltitudeHold |
gke | 0:62a1c91a859a | 191 | |
gke | 0:62a1c91a859a | 192 | |
gke | 0:62a1c91a859a | 193 | void DoOrientationTransform(void) { |
gke | 0:62a1c91a859a | 194 | static real32 OSO, OCO; |
gke | 0:62a1c91a859a | 195 | |
gke | 0:62a1c91a859a | 196 | if ( F.UsingPolar ) { |
gke | 0:62a1c91a859a | 197 | OSO = OSin[PolarOrientation]; |
gke | 0:62a1c91a859a | 198 | OCO = OCos[PolarOrientation]; |
gke | 0:62a1c91a859a | 199 | } else { |
gke | 0:62a1c91a859a | 200 | OSO = OSin[Orientation]; |
gke | 0:62a1c91a859a | 201 | OCO = OCos[Orientation]; |
gke | 0:62a1c91a859a | 202 | } |
gke | 0:62a1c91a859a | 203 | |
gke | 0:62a1c91a859a | 204 | if ( !F.NavigationActive ) |
gke | 0:62a1c91a859a | 205 | NavCorr[Roll] = NavCorr[Pitch] = NavCorr[Yaw] = 0; |
gke | 0:62a1c91a859a | 206 | |
gke | 0:62a1c91a859a | 207 | // -PS+RC |
gke | 0:62a1c91a859a | 208 | ControlRoll = (int16)( -DesiredPitch * OSO + DesiredRoll * OCO ); |
gke | 0:62a1c91a859a | 209 | |
gke | 0:62a1c91a859a | 210 | // PC+RS |
gke | 0:62a1c91a859a | 211 | ControlPitch = (int16)( DesiredPitch * OCO + DesiredRoll * OSO ); |
gke | 2:90292f8bd179 | 212 | |
gke | 1:1e3318a30ddd | 213 | CameraRollAngle = Angle[Pitch] * OSO + Angle[Roll] * OCO; |
gke | 1:1e3318a30ddd | 214 | CameraPitchAngle = Angle[Pitch] * OCO - Angle[Roll] * OSO; |
gke | 0:62a1c91a859a | 215 | |
gke | 0:62a1c91a859a | 216 | } // DoOrientationTransform |
gke | 0:62a1c91a859a | 217 | |
gke | 2:90292f8bd179 | 218 | void GainSchedule(void) { |
gke | 0:62a1c91a859a | 219 | |
gke | 0:62a1c91a859a | 220 | /* |
gke | 0:62a1c91a859a | 221 | // rudimentary gain scheduling (linear) |
gke | 0:62a1c91a859a | 222 | static int16 AttDiff, ThrDiff; |
gke | 0:62a1c91a859a | 223 | |
gke | 0:62a1c91a859a | 224 | if ( (!F.NavigationActive) || ( F.NavigationActive && (NavState == HoldingStation ) ) ) |
gke | 0:62a1c91a859a | 225 | { |
gke | 0:62a1c91a859a | 226 | // also density altitude? |
gke | 0:62a1c91a859a | 227 | |
gke | 0:62a1c91a859a | 228 | if ( P[Acro] > 0) // due to Foliage 2009 and Alexinparis 2010 |
gke | 0:62a1c91a859a | 229 | { |
gke | 0:62a1c91a859a | 230 | AttDiff = CurrMaxRollPitch - ATTITUDE_HOLD_LIMIT; |
gke | 0:62a1c91a859a | 231 | GS = GS * ( 1000.0 - (AttDiff * (int16)P[Acro]) ); |
gke | 0:62a1c91a859a | 232 | GS *= 0.001; |
gke | 0:62a1c91a859a | 233 | GS = Limit(GS, 0, 1.0); |
gke | 0:62a1c91a859a | 234 | } |
gke | 0:62a1c91a859a | 235 | |
gke | 0:62a1c91a859a | 236 | if ( P[GSThrottle] > 0 ) |
gke | 0:62a1c91a859a | 237 | { |
gke | 0:62a1c91a859a | 238 | ThrDiff = DesiredThrottle - CruiseThrottle; |
gke | 0:62a1c91a859a | 239 | GS = (int32)GS * ( 1000.0 + (ThrDiff * (int16)P[GSThrottle]) ); |
gke | 0:62a1c91a859a | 240 | GS *= 0.001; |
gke | 0:62a1c91a859a | 241 | } |
gke | 0:62a1c91a859a | 242 | } |
gke | 0:62a1c91a859a | 243 | |
gke | 0:62a1c91a859a | 244 | */ |
gke | 0:62a1c91a859a | 245 | |
gke | 0:62a1c91a859a | 246 | GS = 1.0; // Temp |
gke | 0:62a1c91a859a | 247 | |
gke | 0:62a1c91a859a | 248 | |
gke | 0:62a1c91a859a | 249 | |
gke | 0:62a1c91a859a | 250 | } // GainSchedule |
gke | 0:62a1c91a859a | 251 | |
gke | 2:90292f8bd179 | 252 | const real32 RelayKcu = ( 4.0 * 10 )/ ( PI * 0.8235 ); // stimulus 10 => Kcu 15.5 |
gke | 2:90292f8bd179 | 253 | const real32 RelayPd = 2.0 * 1.285; |
gke | 2:90292f8bd179 | 254 | const real32 RelayStim = 3.0; |
gke | 2:90292f8bd179 | 255 | |
gke | 2:90292f8bd179 | 256 | real32 RelayA = 0.0; |
gke | 2:90292f8bd179 | 257 | real32 RelayTau = 0.0; |
gke | 2:90292f8bd179 | 258 | uint32 RelayIteration = 0; |
gke | 2:90292f8bd179 | 259 | real32 RelayP, RelayW; |
gke | 2:90292f8bd179 | 260 | |
gke | 2:90292f8bd179 | 261 | void DoRelayTuning(void) { |
gke | 2:90292f8bd179 | 262 | |
gke | 2:90292f8bd179 | 263 | static real32 Temp; |
gke | 2:90292f8bd179 | 264 | |
gke | 2:90292f8bd179 | 265 | Temp = fabs(Angle[Roll]); |
gke | 2:90292f8bd179 | 266 | if ( Temp > RelayA ) RelayA = Temp; |
gke | 2:90292f8bd179 | 267 | |
gke | 2:90292f8bd179 | 268 | if ( ( RelayP < 0.0 ) && ( Angle[Roll] >= 0.0 ) ) { |
gke | 2:90292f8bd179 | 269 | |
gke | 2:90292f8bd179 | 270 | RelayTau = RelayIteration * dT; |
gke | 2:90292f8bd179 | 271 | |
gke | 2:90292f8bd179 | 272 | RelayP = - (PI * RelayA) / (4.0 * RelayStim); |
gke | 2:90292f8bd179 | 273 | RelayW = (2.0 * PI) / RelayTau; |
gke | 2:90292f8bd179 | 274 | |
gke | 2:90292f8bd179 | 275 | #ifndef PID_RAW |
gke | 2:90292f8bd179 | 276 | SendPIDTuning(); |
gke | 2:90292f8bd179 | 277 | #endif // PID_RAW |
gke | 2:90292f8bd179 | 278 | |
gke | 2:90292f8bd179 | 279 | RelayA = 0.0; |
gke | 2:90292f8bd179 | 280 | RelayIteration = 0; |
gke | 2:90292f8bd179 | 281 | } |
gke | 2:90292f8bd179 | 282 | |
gke | 2:90292f8bd179 | 283 | RelayIteration++; |
gke | 2:90292f8bd179 | 284 | RelayP = Angle[Roll]; |
gke | 2:90292f8bd179 | 285 | |
gke | 2:90292f8bd179 | 286 | } // DoRelayTuning |
gke | 2:90292f8bd179 | 287 | |
gke | 2:90292f8bd179 | 288 | void Relay(void) { |
gke | 2:90292f8bd179 | 289 | |
gke | 2:90292f8bd179 | 290 | if ( Angle[Roll] < 0.0 ) |
gke | 2:90292f8bd179 | 291 | Rl = -RelayStim; |
gke | 2:90292f8bd179 | 292 | else |
gke | 2:90292f8bd179 | 293 | Rl = +RelayStim; |
gke | 2:90292f8bd179 | 294 | |
gke | 2:90292f8bd179 | 295 | DesiredRoll = Rl; |
gke | 2:90292f8bd179 | 296 | |
gke | 2:90292f8bd179 | 297 | } // Relay |
gke | 2:90292f8bd179 | 298 | |
gke | 0:62a1c91a859a | 299 | void DoControl(void) { |
gke | 1:1e3318a30ddd | 300 | |
gke | 2:90292f8bd179 | 301 | static real32 RateE; |
gke | 2:90292f8bd179 | 302 | |
gke | 2:90292f8bd179 | 303 | |
gke | 0:62a1c91a859a | 304 | GetAttitude(); |
gke | 0:62a1c91a859a | 305 | AltitudeHold(); |
gke | 0:62a1c91a859a | 306 | |
gke | 0:62a1c91a859a | 307 | #ifdef SIMULATE |
gke | 0:62a1c91a859a | 308 | |
gke | 0:62a1c91a859a | 309 | FakeDesiredRoll = DesiredRoll + NavRCorr; |
gke | 0:62a1c91a859a | 310 | FakeDesiredPitch = DesiredPitch + NavPCorr; |
gke | 0:62a1c91a859a | 311 | FakeDesiredYaw = DesiredYaw + NavYCorr; |
gke | 0:62a1c91a859a | 312 | Angle[Roll] = SlewLimit(Angle[Roll], FakeDesiredRoll * 16.0, 4.0); |
gke | 0:62a1c91a859a | 313 | Angle[Pitch] = SlewLimit(Angle[Pitch], FakeDesiredPitch * 16.0, 4.0); |
gke | 0:62a1c91a859a | 314 | Angle[Yaw] = SlewLimit(Angle[Yaw], FakeDesiredYaw, 4.0); |
gke | 0:62a1c91a859a | 315 | Rl = FakeDesiredRoll; |
gke | 0:62a1c91a859a | 316 | Pl = FakeDesiredPitch; |
gke | 0:62a1c91a859a | 317 | Yl = DesiredYaw; |
gke | 0:62a1c91a859a | 318 | |
gke | 0:62a1c91a859a | 319 | #else |
gke | 0:62a1c91a859a | 320 | |
gke | 0:62a1c91a859a | 321 | DoOrientationTransform(); |
gke | 0:62a1c91a859a | 322 | |
gke | 2:90292f8bd179 | 323 | GainSchedule(); |
gke | 0:62a1c91a859a | 324 | |
gke | 0:62a1c91a859a | 325 | #ifdef DISABLE_EXTRAS |
gke | 0:62a1c91a859a | 326 | // for commissioning |
gke | 2:90292f8bd179 | 327 | AccAltComp = AltComp = 0.0; |
gke | 0:62a1c91a859a | 328 | NavCorr[Roll] = NavCorr[Pitch] = NavCorr[Yaw] = 0; |
gke | 0:62a1c91a859a | 329 | #endif // DISABLE_EXTRAS |
gke | 0:62a1c91a859a | 330 | |
gke | 2:90292f8bd179 | 331 | // Roll |
gke | 0:62a1c91a859a | 332 | |
gke | 2:90292f8bd179 | 333 | #ifdef USE_ANGLE_DERIVED_RATE // Gyro rate noisy so compute from angle |
gke | 2:90292f8bd179 | 334 | RateE = ( Angle[Roll] - Anglep[Roll] ) * dTR; |
gke | 2:90292f8bd179 | 335 | #else |
gke | 2:90292f8bd179 | 336 | RateE = Rate[Roll]; |
gke | 2:90292f8bd179 | 337 | #endif // USE_ANGLE_DERIVED_RATE |
gke | 2:90292f8bd179 | 338 | Rl = RateE * GRollKp + Angle[Roll] * GRollKi + ( RateE - Ratep[Roll] ) * GRollKd * dTR; |
gke | 2:90292f8bd179 | 339 | Rl += ControlRoll + NavCorr[Roll]; |
gke | 0:62a1c91a859a | 340 | |
gke | 2:90292f8bd179 | 341 | #ifdef USE_ANGLE_DERIVED_RATE |
gke | 2:90292f8bd179 | 342 | Ratep[Roll] = RateE; |
gke | 2:90292f8bd179 | 343 | Anglep[Roll] = Angle[Roll]; |
gke | 2:90292f8bd179 | 344 | #else |
gke | 2:90292f8bd179 | 345 | Ratep[Roll] = Rate[Roll]; |
gke | 2:90292f8bd179 | 346 | #endif // USE_ANGLE_DERIVED_RATE |
gke | 0:62a1c91a859a | 347 | |
gke | 2:90292f8bd179 | 348 | // Pitch |
gke | 0:62a1c91a859a | 349 | |
gke | 2:90292f8bd179 | 350 | #ifdef USE_ANGLE_DERIVED_RATE // Gyro rate noisy so compute from angle |
gke | 2:90292f8bd179 | 351 | RateE = ( Angle[Pitch] - Anglep[Pitch] ) * dTR; |
gke | 2:90292f8bd179 | 352 | #else |
gke | 2:90292f8bd179 | 353 | RateE = Rate[Pitch]; |
gke | 2:90292f8bd179 | 354 | #endif // USE_ANGLE_DERIVED_RATE |
gke | 2:90292f8bd179 | 355 | Pl = RateE * GPitchKp + Angle[Pitch] * GPitchKi + ( RateE - Ratep[Pitch] ) * GPitchKd * dTR; |
gke | 2:90292f8bd179 | 356 | Pl += ControlPitch + NavCorr[Pitch]; |
gke | 0:62a1c91a859a | 357 | |
gke | 2:90292f8bd179 | 358 | #ifdef USE_ANGLE_DERIVED_RATE |
gke | 2:90292f8bd179 | 359 | Ratep[Pitch] = RateE; |
gke | 2:90292f8bd179 | 360 | Anglep[Pitch] = Angle[Pitch]; |
gke | 2:90292f8bd179 | 361 | #else |
gke | 2:90292f8bd179 | 362 | Ratep[Pitch] = Rate[Pitch]; |
gke | 2:90292f8bd179 | 363 | #endif // USE_ANGLE_DERIVED_RATE |
gke | 0:62a1c91a859a | 364 | |
gke | 0:62a1c91a859a | 365 | // Yaw |
gke | 0:62a1c91a859a | 366 | |
gke | 2:90292f8bd179 | 367 | #define MAX_YAW_RATE (HALFPI / RC_NEUTRAL) // Radians/Sec e.g. HalfPI is 90deg/sec |
gke | 2:90292f8bd179 | 368 | |
gke | 2:90292f8bd179 | 369 | DoLegacyYawComp(AttitudeMethod); // returns Angle as heading error along with compensated rate |
gke | 0:62a1c91a859a | 370 | |
gke | 2:90292f8bd179 | 371 | RateE = Rate[Yaw] + ( DesiredYaw + NavCorr[Yaw] ) * MAX_YAW_RATE; |
gke | 2:90292f8bd179 | 372 | |
gke | 2:90292f8bd179 | 373 | YawRateIntE += RateE; |
gke | 2:90292f8bd179 | 374 | YawRateIntE = Limit1(YawRateIntE, YawIntLimit); |
gke | 2:90292f8bd179 | 375 | |
gke | 2:90292f8bd179 | 376 | Yl = RateE * K[YawKp] + YawRateIntE * K[YawKi]; |
gke | 0:62a1c91a859a | 377 | |
gke | 0:62a1c91a859a | 378 | #ifdef TRICOPTER |
gke | 0:62a1c91a859a | 379 | Yl = SlewLimit(Ylp, Yl, 2.0); |
gke | 0:62a1c91a859a | 380 | Ylp = Yl; |
gke | 2:90292f8bd179 | 381 | Yl = Limit1(Yl, K[YawLimit] * 4.0); |
gke | 0:62a1c91a859a | 382 | #else |
gke | 2:90292f8bd179 | 383 | Yl = Limit1(Yl, K[YawLimit]); // currently 25 default |
gke | 0:62a1c91a859a | 384 | #endif // TRICOPTER |
gke | 0:62a1c91a859a | 385 | |
gke | 2:90292f8bd179 | 386 | #endif // SIMULATE |
gke | 0:62a1c91a859a | 387 | |
gke | 0:62a1c91a859a | 388 | } // DoControl |
gke | 0:62a1c91a859a | 389 | |
gke | 0:62a1c91a859a | 390 | static int8 RCStart = RC_INIT_FRAMES; |
gke | 0:62a1c91a859a | 391 | |
gke | 0:62a1c91a859a | 392 | void LightsAndSirens(void) { |
gke | 0:62a1c91a859a | 393 | static int24 Ch5Timeout; |
gke | 0:62a1c91a859a | 394 | |
gke | 0:62a1c91a859a | 395 | LEDYellow_TOG; |
gke | 0:62a1c91a859a | 396 | if ( F.Signal ) LEDGreen_ON; |
gke | 0:62a1c91a859a | 397 | else LEDGreen_OFF; |
gke | 0:62a1c91a859a | 398 | |
gke | 0:62a1c91a859a | 399 | Beeper_OFF; |
gke | 0:62a1c91a859a | 400 | Ch5Timeout = mSClock() + 500; // mS. |
gke | 0:62a1c91a859a | 401 | |
gke | 0:62a1c91a859a | 402 | do { |
gke | 0:62a1c91a859a | 403 | |
gke | 0:62a1c91a859a | 404 | ProcessCommand(); |
gke | 0:62a1c91a859a | 405 | |
gke | 0:62a1c91a859a | 406 | if ( F.Signal ) { |
gke | 0:62a1c91a859a | 407 | LEDGreen_ON; |
gke | 0:62a1c91a859a | 408 | if ( F.RCNewValues ) { |
gke | 0:62a1c91a859a | 409 | UpdateControls(); |
gke | 0:62a1c91a859a | 410 | if ( --RCStart == 0 ) { // wait until RC filters etc. have settled |
gke | 0:62a1c91a859a | 411 | UpdateParamSetChoice(); |
gke | 0:62a1c91a859a | 412 | MixAndLimitCam(); |
gke | 0:62a1c91a859a | 413 | RCStart = 1; |
gke | 0:62a1c91a859a | 414 | } |
gke | 0:62a1c91a859a | 415 | |
gke | 0:62a1c91a859a | 416 | InitialThrottle = StickThrottle; |
gke | 0:62a1c91a859a | 417 | StickThrottle = 0; |
gke | 0:62a1c91a859a | 418 | OutSignals(); |
gke | 0:62a1c91a859a | 419 | if ( mSClock() > Ch5Timeout ) { |
gke | 0:62a1c91a859a | 420 | if ( F.Navigate || F.ReturnHome || !F.ParametersValid ) { |
gke | 0:62a1c91a859a | 421 | Beeper_TOG; |
gke | 0:62a1c91a859a | 422 | LEDRed_TOG; |
gke | 0:62a1c91a859a | 423 | } else |
gke | 0:62a1c91a859a | 424 | if ( Armed ) |
gke | 0:62a1c91a859a | 425 | LEDRed_TOG; |
gke | 0:62a1c91a859a | 426 | |
gke | 0:62a1c91a859a | 427 | Ch5Timeout += 500; |
gke | 0:62a1c91a859a | 428 | } |
gke | 0:62a1c91a859a | 429 | } |
gke | 0:62a1c91a859a | 430 | } else { |
gke | 0:62a1c91a859a | 431 | LEDRed_ON; |
gke | 0:62a1c91a859a | 432 | LEDGreen_OFF; |
gke | 0:62a1c91a859a | 433 | } |
gke | 0:62a1c91a859a | 434 | ReadParameters(); |
gke | 0:62a1c91a859a | 435 | GetIRAttitude(); // only active if IRSensors selected |
gke | 0:62a1c91a859a | 436 | } while ((!F.Signal) || (Armed && FirstPass) || F.Ch5Active || F.GyroFailure || (!F.AccelerationsValid) || |
gke | 0:62a1c91a859a | 437 | ( InitialThrottle >= RC_THRES_START ) || (!F.ParametersValid) ); |
gke | 0:62a1c91a859a | 438 | |
gke | 0:62a1c91a859a | 439 | FirstPass = false; |
gke | 0:62a1c91a859a | 440 | |
gke | 0:62a1c91a859a | 441 | Beeper_OFF; |
gke | 0:62a1c91a859a | 442 | LEDRed_OFF; |
gke | 0:62a1c91a859a | 443 | LEDGreen_ON; |
gke | 0:62a1c91a859a | 444 | LEDYellow_ON; |
gke | 0:62a1c91a859a | 445 | |
gke | 0:62a1c91a859a | 446 | mS[LastBattery] = mSClock(); |
gke | 0:62a1c91a859a | 447 | mS[FailsafeTimeout] = mSClock() + FAILSAFE_TIMEOUT_MS; |
gke | 0:62a1c91a859a | 448 | |
gke | 0:62a1c91a859a | 449 | F.LostModel = false; |
gke | 0:62a1c91a859a | 450 | FailState = MonitoringRx; |
gke | 0:62a1c91a859a | 451 | |
gke | 0:62a1c91a859a | 452 | } // LightsAndSirens |
gke | 0:62a1c91a859a | 453 | |
gke | 0:62a1c91a859a | 454 | void InitControl(void) { |
gke | 0:62a1c91a859a | 455 | static uint8 i; |
gke | 0:62a1c91a859a | 456 | |
gke | 0:62a1c91a859a | 457 | AltuSp = DescentLimiter = 0; |
gke | 0:62a1c91a859a | 458 | |
gke | 0:62a1c91a859a | 459 | for ( i = 0; i < (uint8)3; i++ ) |
gke | 2:90292f8bd179 | 460 | Angle[i] = Anglep[i] = Rate[i] = Vel[i] = 0.0; |
gke | 2:90292f8bd179 | 461 | |
gke | 2:90292f8bd179 | 462 | AccAltComp = AltComp = 0.0; |
gke | 0:62a1c91a859a | 463 | |
gke | 2:90292f8bd179 | 464 | YawRateIntE = 0.0; |
gke | 2:90292f8bd179 | 465 | |
gke | 2:90292f8bd179 | 466 | AltSum = Ylp = AltitudeP = 0.0; |
gke | 2:90292f8bd179 | 467 | ControlUpdateTimeuS = 0; |
gke | 0:62a1c91a859a | 468 | |
gke | 0:62a1c91a859a | 469 | } // InitControl |
gke | 2:90292f8bd179 | 470 |