Prof Greg Egan / Mbed 2 deprecated UAVXArm-GKE

UAVX Multicopter Flight Controller.

Dependencies:   mbed

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?

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