Prof Greg Egan / Mbed 2 deprecated UAVXArm-GKE

UAVX Multicopter Flight Controller.

Dependencies:   mbed

control.c@0:62a1c91a859a, 2011-02-18 (annotated)

Committer:
gke
Date:
Fri Feb 18 22:28:05 2011 +0000
Revision:
0:62a1c91a859a
Child:
1:1e3318a30ddd
First release

Who changed what in which revision?

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