Prof Greg Egan / Mbed 2 deprecated UAVXArm-GKE

UAVX Multicopter Flight Controller.

Dependencies:   mbed

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?

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 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