Prof Greg Egan
/
UAVXArm-GKE
UAVX Multicopter Flight Controller.
control.c@1:1e3318a30ddd, 2011-02-25 (annotated)
- Committer:
- gke
- Date:
- Fri Feb 25 01:35:24 2011 +0000
- Revision:
- 1:1e3318a30ddd
- Parent:
- 0:62a1c91a859a
- Child:
- 2:90292f8bd179
This version has broken I2C - posted for debugging involvement of Simon et al.
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 | 0:62a1c91a859a | 5 | // = http://code.google.com/p/uavp-mods/ http://uavp.ch = |
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 | 1:1e3318a30ddd | 23 | real32 PTerm, ITerm, DTerm; |
gke | 1:1e3318a30ddd | 24 | |
gke | 0:62a1c91a859a | 25 | void DoAltitudeHold(void); |
gke | 0:62a1c91a859a | 26 | void UpdateAltitudeSource(void); |
gke | 0:62a1c91a859a | 27 | void AltitudeHold(void); |
gke | 0:62a1c91a859a | 28 | void InertialDamping(void); |
gke | 0:62a1c91a859a | 29 | void DoOrientationTransform(void); |
gke | 0:62a1c91a859a | 30 | void DoControl(void); |
gke | 0:62a1c91a859a | 31 | |
gke | 0:62a1c91a859a | 32 | void CheckThrottleMoved(void); |
gke | 0:62a1c91a859a | 33 | void LightsAndSirens(void); |
gke | 0:62a1c91a859a | 34 | void InitControl(void); |
gke | 0:62a1c91a859a | 35 | |
gke | 0:62a1c91a859a | 36 | real32 Angle[3], Anglep[3], Rate[3], Ratep[3]; // Milliradians |
gke | 0:62a1c91a859a | 37 | real32 Comp[4]; |
gke | 0:62a1c91a859a | 38 | real32 DescentComp; |
gke | 0:62a1c91a859a | 39 | |
gke | 0:62a1c91a859a | 40 | real32 AngleE[3], AngleIntE[3]; |
gke | 0:62a1c91a859a | 41 | |
gke | 0:62a1c91a859a | 42 | real32 GS; |
gke | 0:62a1c91a859a | 43 | real32 Rl, Pl, Yl, Ylp; |
gke | 0:62a1c91a859a | 44 | int16 HoldYaw; |
gke | 0:62a1c91a859a | 45 | int16 CruiseThrottle, MaxCruiseThrottle, DesiredThrottle, IdleThrottle, InitialThrottle, StickThrottle; |
gke | 0:62a1c91a859a | 46 | int16 DesiredRoll, DesiredPitch, DesiredYaw, DesiredHeading, DesiredCamPitchTrim; |
gke | 0:62a1c91a859a | 47 | real32 ControlRoll, ControlPitch, ControlRollP, ControlPitchP; |
gke | 1:1e3318a30ddd | 48 | real32 CameraRollAngle, CameraPitchAngle; |
gke | 0:62a1c91a859a | 49 | int16 CurrMaxRollPitch; |
gke | 0:62a1c91a859a | 50 | |
gke | 0:62a1c91a859a | 51 | int16 AttitudeHoldResetCount; |
gke | 0:62a1c91a859a | 52 | real32 AltDiffSum, AltD, AltDSum; |
gke | 0:62a1c91a859a | 53 | real32 DesiredAltitude, Altitude, AltitudeP; |
gke | 0:62a1c91a859a | 54 | real32 ROC; |
gke | 0:62a1c91a859a | 55 | |
gke | 0:62a1c91a859a | 56 | uint32 AltuSp; |
gke | 0:62a1c91a859a | 57 | int16 DescentLimiter; |
gke | 0:62a1c91a859a | 58 | |
gke | 0:62a1c91a859a | 59 | real32 GRollKp, GRollKi, GRollKd, GPitchKp, GPitchKi, GPitchKd; |
gke | 0:62a1c91a859a | 60 | |
gke | 0:62a1c91a859a | 61 | boolean FirstPass; |
gke | 0:62a1c91a859a | 62 | int8 BeepTick = 0; |
gke | 0:62a1c91a859a | 63 | |
gke | 0:62a1c91a859a | 64 | void DoAltitudeHold(void) { // Syncronised to baro intervals independant of active altitude source |
gke | 0:62a1c91a859a | 65 | |
gke | 0:62a1c91a859a | 66 | static int16 NewAltComp; |
gke | 0:62a1c91a859a | 67 | static real32 AltP, AltI, AltD; |
gke | 0:62a1c91a859a | 68 | static real32 LimAltE, AltE; |
gke | 0:62a1c91a859a | 69 | static real32 AltdT, AltdTR; |
gke | 0:62a1c91a859a | 70 | static uint32 Now; |
gke | 0:62a1c91a859a | 71 | |
gke | 0:62a1c91a859a | 72 | Now = uSClock(); |
gke | 0:62a1c91a859a | 73 | AltdT = ( Now - AltuSp ) * 0.000001; |
gke | 0:62a1c91a859a | 74 | AltdT = Limit(AltdT, 0.01, 0.1); // limit range for restarts |
gke | 0:62a1c91a859a | 75 | AltdTR = 1.0 / AltdT; |
gke | 0:62a1c91a859a | 76 | AltuSp = Now; |
gke | 0:62a1c91a859a | 77 | |
gke | 0:62a1c91a859a | 78 | AltE = DesiredAltitude - Altitude; |
gke | 0:62a1c91a859a | 79 | LimAltE = Limit(AltE, -ALT_BAND_M, ALT_BAND_M); |
gke | 0:62a1c91a859a | 80 | |
gke | 0:62a1c91a859a | 81 | AltP = LimAltE * K[AltKp]; |
gke | 0:62a1c91a859a | 82 | AltP = Limit(AltP, -ALT_MAX_THR_COMP, ALT_MAX_THR_COMP); |
gke | 0:62a1c91a859a | 83 | |
gke | 0:62a1c91a859a | 84 | AltDiffSum += LimAltE; |
gke | 0:62a1c91a859a | 85 | AltDiffSum = Limit(AltDiffSum, -ALT_INT_WINDUP_LIMIT, ALT_INT_WINDUP_LIMIT); |
gke | 0:62a1c91a859a | 86 | AltI = AltDiffSum * K[AltKi] * AltdT; |
gke | 0:62a1c91a859a | 87 | AltI = Limit(AltDiffSum, -K[AltIntLimit], K[AltIntLimit]); |
gke | 0:62a1c91a859a | 88 | |
gke | 0:62a1c91a859a | 89 | ROC = ( Altitude - AltitudeP ) * AltdTR; // may neeed filtering - noisy |
gke | 0:62a1c91a859a | 90 | AltitudeP = Altitude; |
gke | 0:62a1c91a859a | 91 | |
gke | 0:62a1c91a859a | 92 | AltD = ROC * K[AltKd]; |
gke | 0:62a1c91a859a | 93 | AltD = Limit(AltD, -ALT_MAX_THR_COMP, ALT_MAX_THR_COMP); |
gke | 0:62a1c91a859a | 94 | |
gke | 0:62a1c91a859a | 95 | if ( ROC < ( -K[MaxDescentRateDmpS] * 10.0 ) ) { |
gke | 0:62a1c91a859a | 96 | DescentLimiter += 1; |
gke | 0:62a1c91a859a | 97 | DescentLimiter = Limit(DescentLimiter, 0, ALT_MAX_THR_COMP * 2.0); |
gke | 0:62a1c91a859a | 98 | |
gke | 0:62a1c91a859a | 99 | } else |
gke | 0:62a1c91a859a | 100 | DescentLimiter = DecayX(DescentLimiter, 1); |
gke | 0:62a1c91a859a | 101 | |
gke | 0:62a1c91a859a | 102 | NewAltComp = AltP + AltI + AltD + AltDSum + DescentLimiter; |
gke | 0:62a1c91a859a | 103 | |
gke | 0:62a1c91a859a | 104 | NewAltComp = Limit(NewAltComp, -ALT_MAX_THR_COMP, ALT_MAX_THR_COMP); |
gke | 0:62a1c91a859a | 105 | |
gke | 0:62a1c91a859a | 106 | Comp[Alt] = SlewLimit(Comp[Alt], NewAltComp, 1.0); |
gke | 0:62a1c91a859a | 107 | |
gke | 0:62a1c91a859a | 108 | |
gke | 0:62a1c91a859a | 109 | if ( ROC > Stats[MaxROCS] ) |
gke | 0:62a1c91a859a | 110 | Stats[MaxROCS] = ROC; |
gke | 0:62a1c91a859a | 111 | else |
gke | 0:62a1c91a859a | 112 | if ( ROC < Stats[MinROCS] ) |
gke | 0:62a1c91a859a | 113 | Stats[MinROCS] = ROC; |
gke | 0:62a1c91a859a | 114 | |
gke | 0:62a1c91a859a | 115 | } // DoAltitudeHold |
gke | 0:62a1c91a859a | 116 | |
gke | 0:62a1c91a859a | 117 | void UpdateAltitudeSource(void) { |
gke | 0:62a1c91a859a | 118 | if ( F.UsingRangefinderAlt ) |
gke | 0:62a1c91a859a | 119 | Altitude = RangefinderAltitude; |
gke | 0:62a1c91a859a | 120 | else |
gke | 0:62a1c91a859a | 121 | Altitude = BaroRelAltitude; |
gke | 0:62a1c91a859a | 122 | |
gke | 0:62a1c91a859a | 123 | } // UpdateAltitudeSource |
gke | 0:62a1c91a859a | 124 | |
gke | 0:62a1c91a859a | 125 | void AltitudeHold() { |
gke | 0:62a1c91a859a | 126 | static int16 NewCruiseThrottle; |
gke | 0:62a1c91a859a | 127 | |
gke | 0:62a1c91a859a | 128 | GetBaroAltitude(); |
gke | 0:62a1c91a859a | 129 | GetRangefinderAltitude(); |
gke | 0:62a1c91a859a | 130 | CheckThrottleMoved(); |
gke | 0:62a1c91a859a | 131 | |
gke | 0:62a1c91a859a | 132 | if ( F.AltHoldEnabled ) { |
gke | 0:62a1c91a859a | 133 | if ( F.NewBaroValue ) { // sync on Baro which MUST be working |
gke | 0:62a1c91a859a | 134 | F.NewBaroValue = false; |
gke | 0:62a1c91a859a | 135 | |
gke | 0:62a1c91a859a | 136 | UpdateAltitudeSource(); |
gke | 0:62a1c91a859a | 137 | |
gke | 0:62a1c91a859a | 138 | if ( ( NavState != HoldingStation ) && F.AllowNavAltitudeHold ) { // Navigating - using CruiseThrottle |
gke | 0:62a1c91a859a | 139 | F.HoldingAlt = true; |
gke | 0:62a1c91a859a | 140 | DoAltitudeHold(); |
gke | 0:62a1c91a859a | 141 | } else |
gke | 0:62a1c91a859a | 142 | if ( F.ThrottleMoving ) { |
gke | 0:62a1c91a859a | 143 | F.HoldingAlt = false; |
gke | 0:62a1c91a859a | 144 | DesiredAltitude = Altitude; |
gke | 0:62a1c91a859a | 145 | Comp[Alt] = Decay1(Comp[Alt]); |
gke | 0:62a1c91a859a | 146 | } else { |
gke | 0:62a1c91a859a | 147 | F.HoldingAlt = true; |
gke | 0:62a1c91a859a | 148 | if ( fabs(ROC) < ALT_HOLD_MAX_ROC_MPS ) { |
gke | 0:62a1c91a859a | 149 | NewCruiseThrottle = DesiredThrottle + Comp[Alt]; |
gke | 0:62a1c91a859a | 150 | CruiseThrottle = HardFilter(CruiseThrottle, NewCruiseThrottle); |
gke | 0:62a1c91a859a | 151 | CruiseThrottle = Limit( CruiseThrottle , IdleThrottle, MaxCruiseThrottle ); |
gke | 0:62a1c91a859a | 152 | } |
gke | 0:62a1c91a859a | 153 | DoAltitudeHold(); |
gke | 0:62a1c91a859a | 154 | } |
gke | 0:62a1c91a859a | 155 | } |
gke | 0:62a1c91a859a | 156 | } else { |
gke | 0:62a1c91a859a | 157 | Comp[Alt] = Decay1(Comp[Alt]); |
gke | 0:62a1c91a859a | 158 | ROC = 0.0; |
gke | 0:62a1c91a859a | 159 | F.HoldingAlt = false; |
gke | 0:62a1c91a859a | 160 | } |
gke | 0:62a1c91a859a | 161 | } // AltitudeHold |
gke | 0:62a1c91a859a | 162 | |
gke | 0:62a1c91a859a | 163 | void InertialDamping(void) { // Uses accelerometer to damp disturbances while holding altitude |
gke | 0:62a1c91a859a | 164 | static uint8 i; |
gke | 0:62a1c91a859a | 165 | |
gke | 0:62a1c91a859a | 166 | if ( F.AccelerationsValid && F.NearLevel ) { |
gke | 0:62a1c91a859a | 167 | // Up - Down |
gke | 0:62a1c91a859a | 168 | |
gke | 0:62a1c91a859a | 169 | Vel[UD] += Acc[UD] * dT; |
gke | 0:62a1c91a859a | 170 | Comp[UD] = Vel[UD] * K[VertDampKp]; |
gke | 0:62a1c91a859a | 171 | Comp[UD] = Limit(Comp[UD], DAMP_VERT_LIMIT_LOW, DAMP_VERT_LIMIT_HIGH); |
gke | 0:62a1c91a859a | 172 | |
gke | 0:62a1c91a859a | 173 | Vel[UD] = DecayX(Vel[UD], K[VertDampDecay]); |
gke | 0:62a1c91a859a | 174 | |
gke | 0:62a1c91a859a | 175 | // Lateral compensation only when holding altitude |
gke | 0:62a1c91a859a | 176 | if ( F.HoldingAlt && F.AttitudeHold ) { |
gke | 0:62a1c91a859a | 177 | if ( F.WayPointCentred ) { |
gke | 0:62a1c91a859a | 178 | // Left - Right |
gke | 0:62a1c91a859a | 179 | Vel[LR] += Acc[LR] * dT; |
gke | 0:62a1c91a859a | 180 | Comp[LR] = Vel[LR] * K[HorizDampKp]; |
gke | 0:62a1c91a859a | 181 | Comp[LR] = Limit(Comp[LR], -DAMP_HORIZ_LIMIT, DAMP_HORIZ_LIMIT); |
gke | 0:62a1c91a859a | 182 | Vel[LR] = DecayX(Vel[LR], K[HorizDampDecay]); |
gke | 0:62a1c91a859a | 183 | |
gke | 0:62a1c91a859a | 184 | // Back - Front |
gke | 0:62a1c91a859a | 185 | Vel[BF] += Acc[BF] * dT; |
gke | 0:62a1c91a859a | 186 | Comp[BF] = Vel[BF] * K[HorizDampKp]; |
gke | 0:62a1c91a859a | 187 | Comp[BF] = Limit(Comp[BF], -DAMP_HORIZ_LIMIT, DAMP_HORIZ_LIMIT); |
gke | 0:62a1c91a859a | 188 | Vel[BF] = DecayX(Vel[BF], K[HorizDampDecay]); |
gke | 0:62a1c91a859a | 189 | } else { |
gke | 0:62a1c91a859a | 190 | Vel[LR] = Vel[BF] = 0; |
gke | 0:62a1c91a859a | 191 | Comp[LR] = Decay1(Comp[LR]); |
gke | 0:62a1c91a859a | 192 | Comp[BF] = Decay1(Comp[BF]); |
gke | 0:62a1c91a859a | 193 | } |
gke | 0:62a1c91a859a | 194 | } else { |
gke | 0:62a1c91a859a | 195 | Vel[LR] = Vel[BF] = 0; |
gke | 0:62a1c91a859a | 196 | |
gke | 0:62a1c91a859a | 197 | Comp[LR] = Decay1(Comp[LR]); |
gke | 0:62a1c91a859a | 198 | Comp[BF] = Decay1(Comp[BF]); |
gke | 0:62a1c91a859a | 199 | } |
gke | 0:62a1c91a859a | 200 | } else |
gke | 0:62a1c91a859a | 201 | for ( i = 0; i < (uint8)3; i++ ) |
gke | 0:62a1c91a859a | 202 | Comp[i] = Vel[i] = 0.0; |
gke | 0:62a1c91a859a | 203 | |
gke | 0:62a1c91a859a | 204 | } // InertialDamping |
gke | 0:62a1c91a859a | 205 | |
gke | 0:62a1c91a859a | 206 | void DoOrientationTransform(void) { |
gke | 0:62a1c91a859a | 207 | static real32 OSO, OCO; |
gke | 0:62a1c91a859a | 208 | |
gke | 0:62a1c91a859a | 209 | if ( F.UsingPolar ) { |
gke | 0:62a1c91a859a | 210 | OSO = OSin[PolarOrientation]; |
gke | 0:62a1c91a859a | 211 | OCO = OCos[PolarOrientation]; |
gke | 0:62a1c91a859a | 212 | } else { |
gke | 0:62a1c91a859a | 213 | OSO = OSin[Orientation]; |
gke | 0:62a1c91a859a | 214 | OCO = OCos[Orientation]; |
gke | 0:62a1c91a859a | 215 | } |
gke | 0:62a1c91a859a | 216 | |
gke | 0:62a1c91a859a | 217 | if ( !F.NavigationActive ) |
gke | 0:62a1c91a859a | 218 | NavCorr[Roll] = NavCorr[Pitch] = NavCorr[Yaw] = 0; |
gke | 0:62a1c91a859a | 219 | |
gke | 0:62a1c91a859a | 220 | // -PS+RC |
gke | 0:62a1c91a859a | 221 | ControlRoll = (int16)( -DesiredPitch * OSO + DesiredRoll * OCO ); |
gke | 0:62a1c91a859a | 222 | |
gke | 0:62a1c91a859a | 223 | // PC+RS |
gke | 0:62a1c91a859a | 224 | ControlPitch = (int16)( DesiredPitch * OCO + DesiredRoll * OSO ); |
gke | 1:1e3318a30ddd | 225 | |
gke | 1:1e3318a30ddd | 226 | CameraRollAngle = Angle[Pitch] * OSO + Angle[Roll] * OCO; |
gke | 1:1e3318a30ddd | 227 | CameraPitchAngle = Angle[Pitch] * OCO - Angle[Roll] * OSO; |
gke | 0:62a1c91a859a | 228 | |
gke | 0:62a1c91a859a | 229 | } // DoOrientationTransform |
gke | 0:62a1c91a859a | 230 | |
gke | 0:62a1c91a859a | 231 | void GainSchedule(boolean UseAngle) { |
gke | 0:62a1c91a859a | 232 | |
gke | 0:62a1c91a859a | 233 | /* |
gke | 0:62a1c91a859a | 234 | // rudimentary gain scheduling (linear) |
gke | 0:62a1c91a859a | 235 | static int16 AttDiff, ThrDiff; |
gke | 0:62a1c91a859a | 236 | |
gke | 0:62a1c91a859a | 237 | if ( (!F.NavigationActive) || ( F.NavigationActive && (NavState == HoldingStation ) ) ) |
gke | 0:62a1c91a859a | 238 | { |
gke | 0:62a1c91a859a | 239 | // also density altitude? |
gke | 0:62a1c91a859a | 240 | |
gke | 0:62a1c91a859a | 241 | if ( P[Acro] > 0) // due to Foliage 2009 and Alexinparis 2010 |
gke | 0:62a1c91a859a | 242 | { |
gke | 0:62a1c91a859a | 243 | AttDiff = CurrMaxRollPitch - ATTITUDE_HOLD_LIMIT; |
gke | 0:62a1c91a859a | 244 | GS = GS * ( 1000.0 - (AttDiff * (int16)P[Acro]) ); |
gke | 0:62a1c91a859a | 245 | GS *= 0.001; |
gke | 0:62a1c91a859a | 246 | GS = Limit(GS, 0, 1.0); |
gke | 0:62a1c91a859a | 247 | } |
gke | 0:62a1c91a859a | 248 | |
gke | 0:62a1c91a859a | 249 | if ( P[GSThrottle] > 0 ) |
gke | 0:62a1c91a859a | 250 | { |
gke | 0:62a1c91a859a | 251 | ThrDiff = DesiredThrottle - CruiseThrottle; |
gke | 0:62a1c91a859a | 252 | GS = (int32)GS * ( 1000.0 + (ThrDiff * (int16)P[GSThrottle]) ); |
gke | 0:62a1c91a859a | 253 | GS *= 0.001; |
gke | 0:62a1c91a859a | 254 | } |
gke | 0:62a1c91a859a | 255 | } |
gke | 0:62a1c91a859a | 256 | |
gke | 0:62a1c91a859a | 257 | */ |
gke | 0:62a1c91a859a | 258 | |
gke | 0:62a1c91a859a | 259 | GS = 1.0; // Temp |
gke | 0:62a1c91a859a | 260 | |
gke | 0:62a1c91a859a | 261 | GRollKp = K[RollKp]; |
gke | 0:62a1c91a859a | 262 | GRollKi = K[RollKi]; |
gke | 0:62a1c91a859a | 263 | GRollKd = K[RollKd]; |
gke | 0:62a1c91a859a | 264 | |
gke | 0:62a1c91a859a | 265 | GPitchKp = K[PitchKp]; |
gke | 0:62a1c91a859a | 266 | GPitchKi = K[PitchKi]; |
gke | 0:62a1c91a859a | 267 | GPitchKd = K[PitchKd]; |
gke | 0:62a1c91a859a | 268 | |
gke | 0:62a1c91a859a | 269 | } // GainSchedule |
gke | 0:62a1c91a859a | 270 | |
gke | 0:62a1c91a859a | 271 | void DoControl(void) { |
gke | 1:1e3318a30ddd | 272 | |
gke | 0:62a1c91a859a | 273 | GetAttitude(); |
gke | 0:62a1c91a859a | 274 | AltitudeHold(); |
gke | 0:62a1c91a859a | 275 | InertialDamping(); |
gke | 0:62a1c91a859a | 276 | |
gke | 0:62a1c91a859a | 277 | #ifdef SIMULATE |
gke | 0:62a1c91a859a | 278 | |
gke | 0:62a1c91a859a | 279 | FakeDesiredRoll = DesiredRoll + NavRCorr; |
gke | 0:62a1c91a859a | 280 | FakeDesiredPitch = DesiredPitch + NavPCorr; |
gke | 0:62a1c91a859a | 281 | FakeDesiredYaw = DesiredYaw + NavYCorr; |
gke | 0:62a1c91a859a | 282 | Angle[Roll] = SlewLimit(Angle[Roll], FakeDesiredRoll * 16.0, 4.0); |
gke | 0:62a1c91a859a | 283 | Angle[Pitch] = SlewLimit(Angle[Pitch], FakeDesiredPitch * 16.0, 4.0); |
gke | 0:62a1c91a859a | 284 | Angle[Yaw] = SlewLimit(Angle[Yaw], FakeDesiredYaw, 4.0); |
gke | 0:62a1c91a859a | 285 | Rl = FakeDesiredRoll; |
gke | 0:62a1c91a859a | 286 | Pl = FakeDesiredPitch; |
gke | 0:62a1c91a859a | 287 | Yl = DesiredYaw; |
gke | 0:62a1c91a859a | 288 | |
gke | 0:62a1c91a859a | 289 | #else |
gke | 0:62a1c91a859a | 290 | |
gke | 0:62a1c91a859a | 291 | DoOrientationTransform(); |
gke | 0:62a1c91a859a | 292 | |
gke | 0:62a1c91a859a | 293 | GainSchedule(F.UsingAngleControl); |
gke | 0:62a1c91a859a | 294 | |
gke | 0:62a1c91a859a | 295 | #ifdef DISABLE_EXTRAS |
gke | 0:62a1c91a859a | 296 | // for commissioning |
gke | 0:62a1c91a859a | 297 | Comp[BF] = Comp[LR] = Comp[UD] = Comp[Alt] = 0; |
gke | 0:62a1c91a859a | 298 | NavCorr[Roll] = NavCorr[Pitch] = NavCorr[Yaw] = 0; |
gke | 0:62a1c91a859a | 299 | F.UsingAngleControl = false; |
gke | 0:62a1c91a859a | 300 | #endif // DISABLE_EXTRAS |
gke | 0:62a1c91a859a | 301 | |
gke | 0:62a1c91a859a | 302 | if ( F.UsingAngleControl ) { |
gke | 0:62a1c91a859a | 303 | // Roll |
gke | 0:62a1c91a859a | 304 | |
gke | 0:62a1c91a859a | 305 | AngleE[Roll] = ( ControlRoll * ATTITUDE_SCALE ) - Angle[Roll]; |
gke | 0:62a1c91a859a | 306 | AngleIntE[Roll] += AngleE[Roll] * dT; |
gke | 0:62a1c91a859a | 307 | AngleIntE[Roll] = Limit(AngleIntE[Roll], -K[RollIntLimit], K[RollIntLimit]); |
gke | 0:62a1c91a859a | 308 | Rl = -(AngleE[Roll] * GRollKp + AngleIntE[Roll] * GRollKi + Rate[Roll] * GRollKd * dTR); |
gke | 0:62a1c91a859a | 309 | Rl -= NavCorr[Roll] - Comp[LR]; |
gke | 0:62a1c91a859a | 310 | |
gke | 0:62a1c91a859a | 311 | // Pitch |
gke | 0:62a1c91a859a | 312 | |
gke | 0:62a1c91a859a | 313 | AngleE[Pitch] = ( ControlPitch * ATTITUDE_SCALE ) - Angle[Pitch]; |
gke | 0:62a1c91a859a | 314 | AngleIntE[Pitch] += AngleE[Pitch] * dT; |
gke | 0:62a1c91a859a | 315 | AngleIntE[Pitch] = Limit(AngleIntE[Pitch], -K[PitchIntLimit], K[PitchIntLimit]); |
gke | 0:62a1c91a859a | 316 | Pl = -(AngleE[Pitch] * GPitchKp + AngleIntE[Pitch] * GPitchKi + Rate[Pitch] * GPitchKd * dTR); |
gke | 0:62a1c91a859a | 317 | Pl -= NavCorr[Pitch] - Comp[BF]; |
gke | 0:62a1c91a859a | 318 | |
gke | 0:62a1c91a859a | 319 | } else { |
gke | 0:62a1c91a859a | 320 | // Roll |
gke | 0:62a1c91a859a | 321 | |
gke | 0:62a1c91a859a | 322 | AngleE[Roll] = Limit(Angle[Roll], -K[RollIntLimit], K[RollIntLimit]); |
gke | 0:62a1c91a859a | 323 | Rl = Rate[Roll] * GRollKp + AngleE[Roll] * GRollKi + (Rate[Roll]-Ratep[Roll]) * GRollKd * dTR; |
gke | 0:62a1c91a859a | 324 | Rl -= NavCorr[Roll] - Comp[LR]; |
gke | 1:1e3318a30ddd | 325 | |
gke | 0:62a1c91a859a | 326 | Rl *= GS; |
gke | 0:62a1c91a859a | 327 | |
gke | 0:62a1c91a859a | 328 | Rl -= ControlRoll; |
gke | 0:62a1c91a859a | 329 | |
gke | 0:62a1c91a859a | 330 | ControlRollP = ControlRoll; |
gke | 0:62a1c91a859a | 331 | Ratep[Roll] = Rate[Roll]; |
gke | 0:62a1c91a859a | 332 | |
gke | 0:62a1c91a859a | 333 | // Pitch |
gke | 0:62a1c91a859a | 334 | |
gke | 0:62a1c91a859a | 335 | AngleE[Pitch] = Limit(Angle[Pitch], -K[PitchIntLimit], K[PitchIntLimit]); |
gke | 0:62a1c91a859a | 336 | Pl = Rate[Pitch] * GPitchKp + AngleE[Pitch] * GPitchKi + (Rate[Pitch]-Ratep[Pitch]) * GPitchKd * dTR; |
gke | 0:62a1c91a859a | 337 | Pl -= NavCorr[Pitch] - Comp[BF]; |
gke | 1:1e3318a30ddd | 338 | |
gke | 0:62a1c91a859a | 339 | Pl *= GS; |
gke | 0:62a1c91a859a | 340 | |
gke | 0:62a1c91a859a | 341 | Pl -= ControlPitch; |
gke | 0:62a1c91a859a | 342 | |
gke | 0:62a1c91a859a | 343 | ControlPitchP = ControlPitch; |
gke | 0:62a1c91a859a | 344 | Ratep[Pitch] = Rate[Pitch]; |
gke | 0:62a1c91a859a | 345 | } |
gke | 0:62a1c91a859a | 346 | |
gke | 0:62a1c91a859a | 347 | // Yaw |
gke | 0:62a1c91a859a | 348 | |
gke | 0:62a1c91a859a | 349 | Rate[Yaw] -= NavCorr[Yaw]; |
gke | 0:62a1c91a859a | 350 | if ( abs(DesiredYaw) > 5 ) |
gke | 0:62a1c91a859a | 351 | Rate[Yaw] -= DesiredYaw; |
gke | 0:62a1c91a859a | 352 | |
gke | 0:62a1c91a859a | 353 | Yl = Rate[Yaw] * K[YawKp] + Angle[Yaw] * K[YawKi] + (Rate[Yaw]-Ratep[Yaw]) * K[YawKd] * dTR; |
gke | 0:62a1c91a859a | 354 | Ratep[Yaw] = Rate[Yaw]; |
gke | 0:62a1c91a859a | 355 | |
gke | 0:62a1c91a859a | 356 | #ifdef TRICOPTER |
gke | 0:62a1c91a859a | 357 | Yl = SlewLimit(Ylp, Yl, 2.0); |
gke | 0:62a1c91a859a | 358 | Ylp = Yl; |
gke | 0:62a1c91a859a | 359 | Yl = Limit(Yl, -K[YawLimit] * 4.0, K[YawLimit] * 4.0); |
gke | 0:62a1c91a859a | 360 | #else |
gke | 0:62a1c91a859a | 361 | Yl = Limit(Yl, -K[YawLimit], K[YawLimit]); |
gke | 0:62a1c91a859a | 362 | #endif // TRICOPTER |
gke | 0:62a1c91a859a | 363 | |
gke | 0:62a1c91a859a | 364 | #endif // SIMULATE |
gke | 0:62a1c91a859a | 365 | |
gke | 0:62a1c91a859a | 366 | } // DoControl |
gke | 0:62a1c91a859a | 367 | |
gke | 0:62a1c91a859a | 368 | static int8 RCStart = RC_INIT_FRAMES; |
gke | 0:62a1c91a859a | 369 | |
gke | 0:62a1c91a859a | 370 | void LightsAndSirens(void) { |
gke | 0:62a1c91a859a | 371 | static int24 Ch5Timeout; |
gke | 0:62a1c91a859a | 372 | |
gke | 0:62a1c91a859a | 373 | LEDYellow_TOG; |
gke | 0:62a1c91a859a | 374 | if ( F.Signal ) LEDGreen_ON; |
gke | 0:62a1c91a859a | 375 | else LEDGreen_OFF; |
gke | 0:62a1c91a859a | 376 | |
gke | 0:62a1c91a859a | 377 | Beeper_OFF; |
gke | 0:62a1c91a859a | 378 | Ch5Timeout = mSClock() + 500; // mS. |
gke | 0:62a1c91a859a | 379 | |
gke | 0:62a1c91a859a | 380 | do { |
gke | 0:62a1c91a859a | 381 | |
gke | 0:62a1c91a859a | 382 | ProcessCommand(); |
gke | 0:62a1c91a859a | 383 | |
gke | 0:62a1c91a859a | 384 | if ( F.Signal ) { |
gke | 0:62a1c91a859a | 385 | LEDGreen_ON; |
gke | 0:62a1c91a859a | 386 | if ( F.RCNewValues ) { |
gke | 0:62a1c91a859a | 387 | UpdateControls(); |
gke | 0:62a1c91a859a | 388 | if ( --RCStart == 0 ) { // wait until RC filters etc. have settled |
gke | 0:62a1c91a859a | 389 | UpdateParamSetChoice(); |
gke | 0:62a1c91a859a | 390 | MixAndLimitCam(); |
gke | 0:62a1c91a859a | 391 | RCStart = 1; |
gke | 0:62a1c91a859a | 392 | } |
gke | 0:62a1c91a859a | 393 | |
gke | 0:62a1c91a859a | 394 | InitialThrottle = StickThrottle; |
gke | 0:62a1c91a859a | 395 | StickThrottle = 0; |
gke | 0:62a1c91a859a | 396 | OutSignals(); |
gke | 0:62a1c91a859a | 397 | if ( mSClock() > Ch5Timeout ) { |
gke | 0:62a1c91a859a | 398 | if ( F.Navigate || F.ReturnHome || !F.ParametersValid ) { |
gke | 0:62a1c91a859a | 399 | Beeper_TOG; |
gke | 0:62a1c91a859a | 400 | LEDRed_TOG; |
gke | 0:62a1c91a859a | 401 | } else |
gke | 0:62a1c91a859a | 402 | if ( Armed ) |
gke | 0:62a1c91a859a | 403 | LEDRed_TOG; |
gke | 0:62a1c91a859a | 404 | |
gke | 0:62a1c91a859a | 405 | Ch5Timeout += 500; |
gke | 0:62a1c91a859a | 406 | } |
gke | 0:62a1c91a859a | 407 | } |
gke | 0:62a1c91a859a | 408 | } else { |
gke | 0:62a1c91a859a | 409 | LEDRed_ON; |
gke | 0:62a1c91a859a | 410 | LEDGreen_OFF; |
gke | 0:62a1c91a859a | 411 | } |
gke | 0:62a1c91a859a | 412 | ReadParameters(); |
gke | 0:62a1c91a859a | 413 | GetIRAttitude(); // only active if IRSensors selected |
gke | 0:62a1c91a859a | 414 | } while ((!F.Signal) || (Armed && FirstPass) || F.Ch5Active || F.GyroFailure || (!F.AccelerationsValid) || |
gke | 0:62a1c91a859a | 415 | ( InitialThrottle >= RC_THRES_START ) || (!F.ParametersValid) ); |
gke | 0:62a1c91a859a | 416 | |
gke | 0:62a1c91a859a | 417 | FirstPass = false; |
gke | 0:62a1c91a859a | 418 | |
gke | 0:62a1c91a859a | 419 | Beeper_OFF; |
gke | 0:62a1c91a859a | 420 | LEDRed_OFF; |
gke | 0:62a1c91a859a | 421 | LEDGreen_ON; |
gke | 0:62a1c91a859a | 422 | LEDYellow_ON; |
gke | 0:62a1c91a859a | 423 | |
gke | 0:62a1c91a859a | 424 | mS[LastBattery] = mSClock(); |
gke | 0:62a1c91a859a | 425 | mS[FailsafeTimeout] = mSClock() + FAILSAFE_TIMEOUT_MS; |
gke | 0:62a1c91a859a | 426 | |
gke | 0:62a1c91a859a | 427 | F.LostModel = false; |
gke | 0:62a1c91a859a | 428 | FailState = MonitoringRx; |
gke | 0:62a1c91a859a | 429 | |
gke | 0:62a1c91a859a | 430 | } // LightsAndSirens |
gke | 0:62a1c91a859a | 431 | |
gke | 0:62a1c91a859a | 432 | void InitControl(void) { |
gke | 0:62a1c91a859a | 433 | static uint8 i; |
gke | 0:62a1c91a859a | 434 | |
gke | 0:62a1c91a859a | 435 | AltuSp = DescentLimiter = 0; |
gke | 0:62a1c91a859a | 436 | |
gke | 0:62a1c91a859a | 437 | for ( i = 0; i < (uint8)3; i++ ) |
gke | 0:62a1c91a859a | 438 | AngleE[i] = AngleIntE[i] = Angle[i] = Anglep[i] = Rate[i] = Trim[i] = Vel[i] = Comp[i] = 0.0; |
gke | 0:62a1c91a859a | 439 | |
gke | 0:62a1c91a859a | 440 | Comp[Alt] = AltSum = Ylp = ControlRollP = ControlPitchP = AltitudeP = 0.0; |
gke | 0:62a1c91a859a | 441 | |
gke | 0:62a1c91a859a | 442 | } // InitControl |