Prof Greg Egan / Mbed 2 deprecated UAVXArm-GKE

UAVX Multicopter Flight Controller.

Dependencies:   mbed

outputs.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 const real32 PWMScale = 1000.0 / OUT_MAXIMUM;
gke 0:62a1c91a859a 24
gke 0:62a1c91a859a 25 void DoMulticopterMix(real32);
gke 0:62a1c91a859a 26 void CheckDemand(real32);
gke 0:62a1c91a859a 27 void MixAndLimitMotors(void);
gke 0:62a1c91a859a 28 void MixAndLimitCam(void);
gke 0:62a1c91a859a 29 void OutSignals(void);
gke 0:62a1c91a859a 30 void InitI2CESCs(void);
gke 0:62a1c91a859a 31 void StopMotors(void);
gke 0:62a1c91a859a 32 void ExercisePWM(void);
gke 0:62a1c91a859a 33 void InitMotors(void);
gke 0:62a1c91a859a 34
gke 0:62a1c91a859a 35 boolean OutToggle;
gke 0:62a1c91a859a 36 real32 PWM[8];
gke 0:62a1c91a859a 37 real32 PWMSense[8];
gke 0:62a1c91a859a 38 int16 ESCI2CFail[6];
gke 0:62a1c91a859a 39 int16 CurrThrottle;
gke 0:62a1c91a859a 40 int16 CamRollPulseWidth, CamPitchPulseWidth;
gke 0:62a1c91a859a 41 int16 ESCMin, ESCMax;
gke 0:62a1c91a859a 42
gke 0:62a1c91a859a 43 #define PWM_PERIOD_US (1000000/PWM_UPDATE_HZ)
gke 0:62a1c91a859a 44
gke 0:62a1c91a859a 45 #ifdef MULTICOPTER
gke 0:62a1c91a859a 46
gke 0:62a1c91a859a 47 uint8 TC(int16 T) {
gke 0:62a1c91a859a 48 return ( Limit(T, ESCMin, ESCMax) );
gke 0:62a1c91a859a 49 } // TC
gke 0:62a1c91a859a 50
gke 0:62a1c91a859a 51 void DoMulticopterMix(real32 CurrThrottle) {
gke 0:62a1c91a859a 52 static real32 Temp;
gke 0:62a1c91a859a 53
gke 0:62a1c91a859a 54 #ifdef Y6COPTER
gke 0:62a1c91a859a 55 PWM[FrontTC] = PWM[LeftTC] = PWM[RightTC] = CurrThrottle;
gke 0:62a1c91a859a 56 #else
gke 0:62a1c91a859a 57 PWM[FrontC] = PWM[LeftC] = PWM[RightC] = PWM[BackC] = CurrThrottle;
gke 0:62a1c91a859a 58 #endif
gke 0:62a1c91a859a 59
gke 0:62a1c91a859a 60 #ifdef TRICOPTER // usually flown K1 motor to the rear - use orientation of 24
gke 0:62a1c91a859a 61 Temp = Pl * 0.5;
gke 0:62a1c91a859a 62 PWM[FrontC] -= Pl; // front motor
gke 0:62a1c91a859a 63 PWM[LeftC] += (Temp - Rl); // right rear
gke 0:62a1c91a859a 64 PWM[RightC] += (Temp + Rl); // left rear
gke 0:62a1c91a859a 65
gke 0:62a1c91a859a 66 PWM[BackC] = -PWMSense[RudderC] * Yl + OUT_NEUTRAL; // yaw servo
gke 0:62a1c91a859a 67 if ( fabs(K[Balance]) > 0.5 )
gke 0:62a1c91a859a 68 PWM[FrontC] = PWM[FrontC] * K[Balance];
gke 0:62a1c91a859a 69 #else
gke 0:62a1c91a859a 70 #ifdef VTCOPTER // usually flown VTail (K1+K4) to the rear - use orientation of 24
gke 0:62a1c91a859a 71 Temp = Pl * 0.5;
gke 0:62a1c91a859a 72
gke 0:62a1c91a859a 73 PWM[LeftC] += (Temp - Rl); // right rear
gke 0:62a1c91a859a 74 PWM[RightC] += (Temp + Rl); // left rear
gke 0:62a1c91a859a 75
gke 0:62a1c91a859a 76 PWM[FrontLeftC] -= Pl - PWMSense[RudderC] * Yl;
gke 0:62a1c91a859a 77 PWM[FrontRightC] -= Pl + PWMSense[RudderC] * Yl;
gke 0:62a1c91a859a 78 if ( fabs(K[Balance]) > 0.01 ) {
gke 0:62a1c91a859a 79 PWM[FrontLeftC] = PWM[FrontLeftC] * K[Balance];
gke 0:62a1c91a859a 80 PWM[FrontRightC] = PWM[FrontRightC] * K[Balance];
gke 0:62a1c91a859a 81 }
gke 0:62a1c91a859a 82 #else
gke 0:62a1c91a859a 83 #ifdef Y6COPTER
gke 0:62a1c91a859a 84
gke 0:62a1c91a859a 85 Temp = Pl * 0.5;
gke 0:62a1c91a859a 86 PWM[FrontTC] -= Pl; // front motor
gke 0:62a1c91a859a 87 PWM[LeftTC] += (Temp - Rl); // right rear
gke 0:62a1c91a859a 88 PWM[RightTC] += (Temp + Rl); // left rear
gke 0:62a1c91a859a 89
gke 0:62a1c91a859a 90 PWM[FrontBC] = PWM[FrontTC];
gke 0:62a1c91a859a 91 PWM[LeftBC] = PWM[LeftTC];
gke 0:62a1c91a859a 92 PWM[RightBC] = PWM[RightTC];
gke 0:62a1c91a859a 93
gke 0:62a1c91a859a 94 if ( fabs(K[Balance]) > 0.01 ) {
gke 0:62a1c91a859a 95 PWM[FrontTC] = PWM[FrontTC] * K[Balance];
gke 0:62a1c91a859a 96 PWM[FrontBC] = PWM[FrontTC];
gke 0:62a1c91a859a 97 }
gke 0:62a1c91a859a 98
gke 0:62a1c91a859a 99 Temp = Yl * 0.5;
gke 0:62a1c91a859a 100 PWM[FrontTC] -= Temp;
gke 0:62a1c91a859a 101 PWM[LeftTC] -= Temp;
gke 0:62a1c91a859a 102 PWM[RightTC] -= Temp;
gke 0:62a1c91a859a 103
gke 0:62a1c91a859a 104 PWM[FrontBC] += Temp;
gke 0:62a1c91a859a 105 PWM[LeftBC] += Temp;
gke 0:62a1c91a859a 106 PWM[RightBC] += Temp;
gke 0:62a1c91a859a 107
gke 0:62a1c91a859a 108 #else
gke 0:62a1c91a859a 109 PWM[LeftC] += -Rl + Yl;
gke 0:62a1c91a859a 110 PWM[RightC] += Rl + Yl;
gke 0:62a1c91a859a 111 PWM[FrontC] += -Pl - Yl;
gke 0:62a1c91a859a 112 PWM[BackC] += Pl - Yl;
gke 0:62a1c91a859a 113 #endif
gke 0:62a1c91a859a 114 #endif
gke 0:62a1c91a859a 115 #endif
gke 0:62a1c91a859a 116
gke 0:62a1c91a859a 117 } // DoMulticopterMix
gke 0:62a1c91a859a 118
gke 0:62a1c91a859a 119 boolean MotorDemandRescale;
gke 0:62a1c91a859a 120
gke 0:62a1c91a859a 121 void CheckDemand(real32 CurrThrottle) {
gke 0:62a1c91a859a 122 static real32 Scale, ScaleHigh, ScaleLow, MaxMotor, DemandSwing;
gke 0:62a1c91a859a 123
gke 0:62a1c91a859a 124 #ifdef Y6COPTER
gke 0:62a1c91a859a 125 MaxMotor = Max(PWM[FrontTC], PWM[LeftTC]);
gke 0:62a1c91a859a 126 MaxMotor = Max(MaxMotor, PWM[RightTC]);
gke 0:62a1c91a859a 127 MaxMotor = Max(MaxMotor, PWM[FrontBC]);
gke 0:62a1c91a859a 128 MaxMotor = Max(MaxMotor, PWM[LeftBC]);
gke 0:62a1c91a859a 129 MaxMotor = Max(MaxMotor, PWM[RightBC]);
gke 0:62a1c91a859a 130 #else
gke 0:62a1c91a859a 131 MaxMotor = Max(PWM[FrontC], PWM[LeftC]);
gke 0:62a1c91a859a 132 MaxMotor = Max(MaxMotor, PWM[RightC]);
gke 0:62a1c91a859a 133 #ifndef TRICOPTER
gke 0:62a1c91a859a 134 MaxMotor = Max(MaxMotor, PWM[BackC]);
gke 0:62a1c91a859a 135 #endif // TRICOPTER
gke 0:62a1c91a859a 136 #endif // Y6COPTER
gke 0:62a1c91a859a 137
gke 0:62a1c91a859a 138 DemandSwing = MaxMotor - CurrThrottle;
gke 0:62a1c91a859a 139
gke 0:62a1c91a859a 140 if ( DemandSwing > 0.0 ) {
gke 0:62a1c91a859a 141 ScaleHigh = (OUT_MAXIMUM - CurrThrottle) / DemandSwing;
gke 0:62a1c91a859a 142 ScaleLow = (CurrThrottle - IdleThrottle) / DemandSwing;
gke 0:62a1c91a859a 143 Scale = Min(ScaleHigh, ScaleLow); // just in case!
gke 0:62a1c91a859a 144 if ( Scale < 0.0 ) Scale = 1.0 / OUT_MAXIMUM;
gke 0:62a1c91a859a 145 if ( Scale < 1.0 ) {
gke 0:62a1c91a859a 146 MotorDemandRescale = true;
gke 0:62a1c91a859a 147 Rl *= Scale; // could get rid of the divides
gke 0:62a1c91a859a 148 Pl *= Scale;
gke 0:62a1c91a859a 149 #ifndef TRICOPTER
gke 0:62a1c91a859a 150 Yl *= Scale;
gke 0:62a1c91a859a 151 #endif // TRICOPTER
gke 0:62a1c91a859a 152 } else
gke 0:62a1c91a859a 153 MotorDemandRescale = false;
gke 0:62a1c91a859a 154 } else
gke 0:62a1c91a859a 155 MotorDemandRescale = false;
gke 0:62a1c91a859a 156
gke 0:62a1c91a859a 157 } // CheckDemand
gke 0:62a1c91a859a 158
gke 0:62a1c91a859a 159 #endif // MULTICOPTER
gke 0:62a1c91a859a 160
gke 0:62a1c91a859a 161 void MixAndLimitMotors(void) {
gke 0:62a1c91a859a 162 static real32 Temp, TempElevon, TempElevator;
gke 0:62a1c91a859a 163 static uint8 m;
gke 0:62a1c91a859a 164
gke 0:62a1c91a859a 165 if ( DesiredThrottle < IdleThrottle )
gke 0:62a1c91a859a 166 CurrThrottle = 0;
gke 0:62a1c91a859a 167 else
gke 0:62a1c91a859a 168 CurrThrottle = DesiredThrottle;
gke 0:62a1c91a859a 169
gke 0:62a1c91a859a 170 #ifdef MULTICOPTER
gke 0:62a1c91a859a 171 if ( State == InFlight )
gke 0:62a1c91a859a 172 CurrThrottle += (-Comp[UD] + Comp[Alt]); // vertical compensation not optional
gke 0:62a1c91a859a 173
gke 0:62a1c91a859a 174 Temp = OUT_MAXIMUM * 0.9; // 10% headroom for control
gke 0:62a1c91a859a 175 CurrThrottle = Limit(CurrThrottle, 0, Temp );
gke 0:62a1c91a859a 176
gke 0:62a1c91a859a 177 if ( CurrThrottle > IdleThrottle ) {
gke 0:62a1c91a859a 178 DoMulticopterMix(CurrThrottle);
gke 0:62a1c91a859a 179
gke 0:62a1c91a859a 180 CheckDemand(CurrThrottle);
gke 0:62a1c91a859a 181
gke 0:62a1c91a859a 182 if ( MotorDemandRescale )
gke 0:62a1c91a859a 183 DoMulticopterMix(CurrThrottle);
gke 0:62a1c91a859a 184 } else {
gke 0:62a1c91a859a 185 #ifdef Y6COPTER
gke 0:62a1c91a859a 186 for ( m = 0; m < (uint8)6; m++ )
gke 0:62a1c91a859a 187 PWM[m] = CurrThrottle;
gke 0:62a1c91a859a 188 #else
gke 0:62a1c91a859a 189 PWM[FrontC] = PWM[LeftC] = PWM[RightC] = CurrThrottle;
gke 0:62a1c91a859a 190 #ifdef TRICOPTER
gke 0:62a1c91a859a 191 PWM[BackC] = -PWMSense[RudderC] * Yl + OUT_NEUTRAL; // yaw servo
gke 0:62a1c91a859a 192 #else
gke 0:62a1c91a859a 193 PWM[BackC] = CurrThrottle;
gke 0:62a1c91a859a 194 #endif // !TRICOPTER
gke 0:62a1c91a859a 195 #endif // Y6COPTER
gke 0:62a1c91a859a 196 }
gke 0:62a1c91a859a 197 #else
gke 0:62a1c91a859a 198 CurrThrottle += Comp[Alt]; // simple - faster to climb with no elevator yet
gke 0:62a1c91a859a 199
gke 0:62a1c91a859a 200 PWM[ThrottleC] = CurrThrottle;
gke 0:62a1c91a859a 201 PWM[RudderC] = -PWMSense[RudderC] * Yl + OUT_NEUTRAL;
gke 0:62a1c91a859a 202
gke 0:62a1c91a859a 203 #if ( defined AILERON | defined HELICOPTER )
gke 0:62a1c91a859a 204 PWM[AileronC] = PWMSense[AileronC] * Rl + OUT_NEUTRAL;
gke 0:62a1c91a859a 205 PWM[ElevatorC] = PWMSense[ElevatorC] * Pl + OUT_NEUTRAL;
gke 0:62a1c91a859a 206 #else // ELEVON
gke 0:62a1c91a859a 207 TempElevator = PWMSense[2] * Pl + OUT_NEUTRAL;
gke 0:62a1c91a859a 208 PWM[RightElevonC] = PWMSense[RightElevonC] * (TempElevator + Rl);
gke 0:62a1c91a859a 209 PWM[LeftElevonC] = PWMSense[LeftElevonC] * (TempElevator - Rl);
gke 0:62a1c91a859a 210 #endif
gke 0:62a1c91a859a 211 #endif
gke 0:62a1c91a859a 212
gke 0:62a1c91a859a 213 } // MixAndLimitMotors
gke 0:62a1c91a859a 214
gke 0:62a1c91a859a 215 void MixAndLimitCam(void) {
gke 0:62a1c91a859a 216 static real32 Temp;
gke 0:62a1c91a859a 217
gke 0:62a1c91a859a 218 // use only roll/pitch angle estimates
gke 0:62a1c91a859a 219 Temp = Angle[Roll] * K[CamRollKp];
gke 0:62a1c91a859a 220 PWM[CamRollC] = Temp + K[CamRollTrim];
gke 0:62a1c91a859a 221 PWM[CamRollC] = PWM[CamRollC] + OUT_NEUTRAL; // PWMSense[CamRollC] *
gke 0:62a1c91a859a 222
gke 0:62a1c91a859a 223 Temp = Angle[Pitch] * K[CamPitchKp];
gke 0:62a1c91a859a 224 PWM[CamPitchC] = Temp + DesiredCamPitchTrim;
gke 0:62a1c91a859a 225 PWM[CamPitchC] = PWM[CamPitchC] + OUT_NEUTRAL; //PWMSense[CamPitchC] *
gke 0:62a1c91a859a 226
gke 0:62a1c91a859a 227 CamRollPulseWidth = 1000 + (int16)( PWM[CamRollC] * PWMScale );
gke 0:62a1c91a859a 228 CamPitchPulseWidth = 1000 + (int16)( PWM[CamPitchC] * PWMScale );
gke 0:62a1c91a859a 229
gke 0:62a1c91a859a 230 } // MixAndLimitCam
gke 0:62a1c91a859a 231
gke 0:62a1c91a859a 232 #if ( defined Y6COPTER )
gke 0:62a1c91a859a 233 #include "outputs_y6.h"
gke 0:62a1c91a859a 234 #else
gke 0:62a1c91a859a 235 #if ( defined TRICOPTER | defined MULTICOPTER | defined VTCOPTER )
gke 0:62a1c91a859a 236 #include "outputs_copter.h"
gke 0:62a1c91a859a 237 #else
gke 0:62a1c91a859a 238 #include "outputs_conventional.h"
gke 0:62a1c91a859a 239 #endif // Y6COPTER
gke 0:62a1c91a859a 240 #endif // TRICOPTER | MULTICOPTER
gke 0:62a1c91a859a 241
gke 0:62a1c91a859a 242 void InitI2CESCs(void) {
gke 0:62a1c91a859a 243 static int8 m;
gke 0:62a1c91a859a 244 static uint8 r;
gke 0:62a1c91a859a 245
gke 0:62a1c91a859a 246 #ifdef MULTICOPTER
gke 0:62a1c91a859a 247
gke 0:62a1c91a859a 248 if ( P[ESCType] == ESCHolger )
gke 0:62a1c91a859a 249 for ( m = 0 ; m < NoOfI2CESCOutputs ; m++ ) {
gke 0:62a1c91a859a 250 I2CESC.start();
gke 0:62a1c91a859a 251 r = I2CESC.write(0x52 + ( m*2 )); // one cmd, one data byte per motor
gke 0:62a1c91a859a 252 r += I2CESC.write(0);
gke 0:62a1c91a859a 253 ESCI2CFail[m] += r;
gke 0:62a1c91a859a 254 I2CESC.stop();
gke 0:62a1c91a859a 255 }
gke 0:62a1c91a859a 256 else
gke 0:62a1c91a859a 257 if ( P[ESCType] == ESCYGEI2C )
gke 0:62a1c91a859a 258 for ( m = 0 ; m < NoOfI2CESCOutputs ; m++ ) {
gke 0:62a1c91a859a 259 I2CESC.start();
gke 0:62a1c91a859a 260 r = I2CESC.write(0x62 + ( m*2 )); // one cmd, one data byte per motor
gke 0:62a1c91a859a 261 r += I2CESC.write(0);
gke 0:62a1c91a859a 262 ESCI2CFail[m] += r;
gke 0:62a1c91a859a 263 I2CESC.stop();
gke 0:62a1c91a859a 264 }
gke 0:62a1c91a859a 265 else
gke 0:62a1c91a859a 266 if ( P[ESCType] == ESCX3D ) {
gke 0:62a1c91a859a 267 I2CESC.start();
gke 0:62a1c91a859a 268 r = I2CESC.write(0x10); // one command, 4 data bytes
gke 0:62a1c91a859a 269 r += I2CESC.write(0);
gke 0:62a1c91a859a 270 r += I2CESC.write(0);
gke 0:62a1c91a859a 271 r += I2CESC.write(0);
gke 0:62a1c91a859a 272 r += I2CESC.write(0);
gke 0:62a1c91a859a 273 ESCI2CFail[0] += r;
gke 0:62a1c91a859a 274 I2CESC.stop();
gke 0:62a1c91a859a 275 }
gke 0:62a1c91a859a 276 #endif // MULTICOPTER
gke 0:62a1c91a859a 277 } // InitI2CESCs
gke 0:62a1c91a859a 278
gke 0:62a1c91a859a 279 void StopMotors(void) {
gke 0:62a1c91a859a 280 #ifdef MULTICOPTER
gke 0:62a1c91a859a 281 #ifdef Y6COPTER
gke 0:62a1c91a859a 282 PWM[FrontTC] = PWM[LeftTC] = PWM[RightTC] =
gke 0:62a1c91a859a 283 PWM[FrontBC] = PWM[LeftBC] = PWM[RightBC] = ESCMin;
gke 0:62a1c91a859a 284 #else
gke 0:62a1c91a859a 285 PWM[FrontC] = PWM[LeftC] = PWM[RightC] = ESCMin;
gke 0:62a1c91a859a 286 #ifndef TRICOPTER
gke 0:62a1c91a859a 287 PWM[BackC] = ESCMin;
gke 0:62a1c91a859a 288 #endif // !TRICOPTER
gke 0:62a1c91a859a 289 #endif // Y6COPTER
gke 0:62a1c91a859a 290 #else
gke 0:62a1c91a859a 291 PWM[ThrottleC] = ESCMin;
gke 0:62a1c91a859a 292 #endif // MULTICOPTER
gke 0:62a1c91a859a 293
gke 0:62a1c91a859a 294 Out0.pulsewidth_us(1000 + (int16)( PWM[FrontC] * PWMScale ) );
gke 0:62a1c91a859a 295 Out1.pulsewidth_us(1000 + (int16)( PWM[LeftC] * PWMScale ) );
gke 0:62a1c91a859a 296 Out2.pulsewidth_us(1000 + (int16)( PWM[RightC] * PWMScale ) );
gke 0:62a1c91a859a 297 Out3.pulsewidth_us(1000 + (int16)( PWM[BackC] * PWMScale ) );
gke 0:62a1c91a859a 298
gke 0:62a1c91a859a 299 CamRollPulseWidth = 1000 + (int16)( PWM[CamRollC] * PWMScale );
gke 0:62a1c91a859a 300 CamPitchPulseWidth = 1000 + (int16)( PWM[CamPitchC] * PWMScale );
gke 0:62a1c91a859a 301
gke 0:62a1c91a859a 302 #ifndef SOFTWARE_CAM_PWM
gke 0:62a1c91a859a 303 Out4.pulsewidth_us(CamRollPulseWidth);
gke 0:62a1c91a859a 304 Out5.pulsewidth_us(CamPitchPulseWidth);
gke 0:62a1c91a859a 305 #endif // !SOFTWARE_CAM_PWM
gke 0:62a1c91a859a 306
gke 0:62a1c91a859a 307 F.MotorsArmed = false;
gke 0:62a1c91a859a 308 } // StopMotors
gke 0:62a1c91a859a 309
gke 0:62a1c91a859a 310 void InitMotors(void) {
gke 0:62a1c91a859a 311 static uint8 m;
gke 0:62a1c91a859a 312
gke 0:62a1c91a859a 313 Out0.period_us(PWM_PERIOD_US);
gke 0:62a1c91a859a 314
gke 0:62a1c91a859a 315 StopMotors();
gke 0:62a1c91a859a 316
gke 0:62a1c91a859a 317 #ifndef Y6COPTER
gke 0:62a1c91a859a 318 #ifdef TRICOPTER
gke 0:62a1c91a859a 319 PWM[BackC] = OUT_NEUTRAL;
gke 0:62a1c91a859a 320 #endif // !TRICOPTER
gke 0:62a1c91a859a 321 PWM[CamRollC] = OUT_NEUTRAL;
gke 0:62a1c91a859a 322 PWM[CamPitchC] = OUT_NEUTRAL;
gke 0:62a1c91a859a 323 #endif // Y6COPTER
gke 0:62a1c91a859a 324
gke 0:62a1c91a859a 325 } // InitMotors
gke 0:62a1c91a859a 326