Prof Greg Egan / Mbed 2 deprecated UAVXArm-GKE

UAVX Multicopter Flight Controller.

Dependencies:   mbed

outputs.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 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 2:90292f8bd179 52 #ifndef MULTICOPTER
gke 0:62a1c91a859a 53 static real32 Temp;
gke 2:90292f8bd179 54 #endif
gke 0:62a1c91a859a 55
gke 0:62a1c91a859a 56 #ifdef Y6COPTER
gke 0:62a1c91a859a 57 PWM[FrontTC] = PWM[LeftTC] = PWM[RightTC] = CurrThrottle;
gke 0:62a1c91a859a 58 #else
gke 0:62a1c91a859a 59 PWM[FrontC] = PWM[LeftC] = PWM[RightC] = PWM[BackC] = CurrThrottle;
gke 0:62a1c91a859a 60 #endif
gke 0:62a1c91a859a 61
gke 0:62a1c91a859a 62 #ifdef TRICOPTER // usually flown K1 motor to the rear - use orientation of 24
gke 0:62a1c91a859a 63 Temp = Pl * 0.5;
gke 0:62a1c91a859a 64 PWM[FrontC] -= Pl; // front motor
gke 0:62a1c91a859a 65 PWM[LeftC] += (Temp - Rl); // right rear
gke 0:62a1c91a859a 66 PWM[RightC] += (Temp + Rl); // left rear
gke 0:62a1c91a859a 67
gke 0:62a1c91a859a 68 PWM[BackC] = -PWMSense[RudderC] * Yl + OUT_NEUTRAL; // yaw servo
gke 0:62a1c91a859a 69 if ( fabs(K[Balance]) > 0.5 )
gke 0:62a1c91a859a 70 PWM[FrontC] = PWM[FrontC] * K[Balance];
gke 0:62a1c91a859a 71 #else
gke 0:62a1c91a859a 72 #ifdef VTCOPTER // usually flown VTail (K1+K4) to the rear - use orientation of 24
gke 0:62a1c91a859a 73 Temp = Pl * 0.5;
gke 0:62a1c91a859a 74
gke 0:62a1c91a859a 75 PWM[LeftC] += (Temp - Rl); // right rear
gke 0:62a1c91a859a 76 PWM[RightC] += (Temp + Rl); // left rear
gke 0:62a1c91a859a 77
gke 0:62a1c91a859a 78 PWM[FrontLeftC] -= Pl - PWMSense[RudderC] * Yl;
gke 0:62a1c91a859a 79 PWM[FrontRightC] -= Pl + PWMSense[RudderC] * Yl;
gke 0:62a1c91a859a 80 if ( fabs(K[Balance]) > 0.01 ) {
gke 0:62a1c91a859a 81 PWM[FrontLeftC] = PWM[FrontLeftC] * K[Balance];
gke 0:62a1c91a859a 82 PWM[FrontRightC] = PWM[FrontRightC] * K[Balance];
gke 0:62a1c91a859a 83 }
gke 0:62a1c91a859a 84 #else
gke 0:62a1c91a859a 85 #ifdef Y6COPTER
gke 0:62a1c91a859a 86 Temp = Pl * 0.5;
gke 2:90292f8bd179 87 PWM[FrontTC] -= Pl; // front motor
gke 2:90292f8bd179 88 PWM[LeftTC] += (Temp - Rl); // right rear
gke 0:62a1c91a859a 89 PWM[RightTC] += (Temp + Rl); // left rear
gke 0:62a1c91a859a 90
gke 0:62a1c91a859a 91 PWM[FrontBC] = PWM[FrontTC];
gke 0:62a1c91a859a 92 PWM[LeftBC] = PWM[LeftTC];
gke 0:62a1c91a859a 93 PWM[RightBC] = PWM[RightTC];
gke 0:62a1c91a859a 94
gke 0:62a1c91a859a 95 if ( fabs(K[Balance]) > 0.01 ) {
gke 0:62a1c91a859a 96 PWM[FrontTC] = PWM[FrontTC] * K[Balance];
gke 0:62a1c91a859a 97 PWM[FrontBC] = PWM[FrontTC];
gke 0:62a1c91a859a 98 }
gke 0:62a1c91a859a 99
gke 0:62a1c91a859a 100 Temp = Yl * 0.5;
gke 0:62a1c91a859a 101 PWM[FrontTC] -= Temp;
gke 0:62a1c91a859a 102 PWM[LeftTC] -= Temp;
gke 0:62a1c91a859a 103 PWM[RightTC] -= Temp;
gke 0:62a1c91a859a 104
gke 0:62a1c91a859a 105 PWM[FrontBC] += Temp;
gke 0:62a1c91a859a 106 PWM[LeftBC] += Temp;
gke 0:62a1c91a859a 107 PWM[RightBC] += Temp;
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 2:90292f8bd179 162 #ifndef MULTICOPTER
gke 2:90292f8bd179 163 static TempElevon, TempElevator;
gke 0:62a1c91a859a 164 static uint8 m;
gke 2:90292f8bd179 165 #endif
gke 2:90292f8bd179 166 static real32 Temp;
gke 0:62a1c91a859a 167
gke 0:62a1c91a859a 168 if ( DesiredThrottle < IdleThrottle )
gke 0:62a1c91a859a 169 CurrThrottle = 0;
gke 0:62a1c91a859a 170 else
gke 0:62a1c91a859a 171 CurrThrottle = DesiredThrottle;
gke 0:62a1c91a859a 172
gke 0:62a1c91a859a 173 #ifdef MULTICOPTER
gke 0:62a1c91a859a 174 if ( State == InFlight )
gke 2:90292f8bd179 175 CurrThrottle += AltComp; // vertical compensation not optional
gke 0:62a1c91a859a 176
gke 0:62a1c91a859a 177 Temp = OUT_MAXIMUM * 0.9; // 10% headroom for control
gke 0:62a1c91a859a 178 CurrThrottle = Limit(CurrThrottle, 0, Temp );
gke 0:62a1c91a859a 179
gke 0:62a1c91a859a 180 if ( CurrThrottle > IdleThrottle ) {
gke 0:62a1c91a859a 181 DoMulticopterMix(CurrThrottle);
gke 0:62a1c91a859a 182
gke 0:62a1c91a859a 183 CheckDemand(CurrThrottle);
gke 0:62a1c91a859a 184
gke 0:62a1c91a859a 185 if ( MotorDemandRescale )
gke 0:62a1c91a859a 186 DoMulticopterMix(CurrThrottle);
gke 0:62a1c91a859a 187 } else {
gke 0:62a1c91a859a 188 #ifdef Y6COPTER
gke 0:62a1c91a859a 189 for ( m = 0; m < (uint8)6; m++ )
gke 0:62a1c91a859a 190 PWM[m] = CurrThrottle;
gke 0:62a1c91a859a 191 #else
gke 0:62a1c91a859a 192 PWM[FrontC] = PWM[LeftC] = PWM[RightC] = CurrThrottle;
gke 0:62a1c91a859a 193 #ifdef TRICOPTER
gke 0:62a1c91a859a 194 PWM[BackC] = -PWMSense[RudderC] * Yl + OUT_NEUTRAL; // yaw servo
gke 0:62a1c91a859a 195 #else
gke 0:62a1c91a859a 196 PWM[BackC] = CurrThrottle;
gke 0:62a1c91a859a 197 #endif // !TRICOPTER
gke 0:62a1c91a859a 198 #endif // Y6COPTER
gke 0:62a1c91a859a 199 }
gke 0:62a1c91a859a 200 #else
gke 0:62a1c91a859a 201 CurrThrottle += Comp[Alt]; // simple - faster to climb with no elevator yet
gke 0:62a1c91a859a 202
gke 0:62a1c91a859a 203 PWM[ThrottleC] = CurrThrottle;
gke 0:62a1c91a859a 204 PWM[RudderC] = -PWMSense[RudderC] * Yl + OUT_NEUTRAL;
gke 0:62a1c91a859a 205
gke 0:62a1c91a859a 206 #if ( defined AILERON | defined HELICOPTER )
gke 0:62a1c91a859a 207 PWM[AileronC] = PWMSense[AileronC] * Rl + OUT_NEUTRAL;
gke 0:62a1c91a859a 208 PWM[ElevatorC] = PWMSense[ElevatorC] * Pl + OUT_NEUTRAL;
gke 0:62a1c91a859a 209 #else // ELEVON
gke 0:62a1c91a859a 210 TempElevator = PWMSense[2] * Pl + OUT_NEUTRAL;
gke 0:62a1c91a859a 211 PWM[RightElevonC] = PWMSense[RightElevonC] * (TempElevator + Rl);
gke 0:62a1c91a859a 212 PWM[LeftElevonC] = PWMSense[LeftElevonC] * (TempElevator - Rl);
gke 0:62a1c91a859a 213 #endif
gke 0:62a1c91a859a 214 #endif
gke 0:62a1c91a859a 215
gke 0:62a1c91a859a 216 } // MixAndLimitMotors
gke 0:62a1c91a859a 217
gke 0:62a1c91a859a 218 void MixAndLimitCam(void) {
gke 0:62a1c91a859a 219
gke 1:1e3318a30ddd 220 static real32 NewCamRoll, NewCamPitch;
gke 1:1e3318a30ddd 221
gke 1:1e3318a30ddd 222 PWMSense[CamRollC] = 1.0;
gke 1:1e3318a30ddd 223 PWMSense[CamPitchC] = 1.0;
gke 1:1e3318a30ddd 224
gke 1:1e3318a30ddd 225 NewCamRoll = CameraRollAngle * K[CamRollKp] + K[CamRollTrim];
gke 1:1e3318a30ddd 226 NewCamRoll = PWMSense[CamRollC] * NewCamRoll + OUT_NEUTRAL;
gke 1:1e3318a30ddd 227 PWM[CamRollC] = SlewLimit( PWM[CamRollC], NewCamRoll, 2); // change to 10Hz filter
gke 0:62a1c91a859a 228
gke 1:1e3318a30ddd 229 NewCamPitch = CameraPitchAngle * K[CamPitchKp] + DesiredCamPitchTrim;
gke 1:1e3318a30ddd 230 NewCamPitch = PWMSense[CamPitchC] * NewCamPitch + OUT_NEUTRAL;
gke 1:1e3318a30ddd 231 PWM[CamPitchC] = SlewLimit( PWM[CamPitchC], NewCamPitch, 2.0); // change to 10Hz filter
gke 1:1e3318a30ddd 232
gke 0:62a1c91a859a 233 CamRollPulseWidth = 1000 + (int16)( PWM[CamRollC] * PWMScale );
gke 0:62a1c91a859a 234 CamPitchPulseWidth = 1000 + (int16)( PWM[CamPitchC] * PWMScale );
gke 0:62a1c91a859a 235
gke 0:62a1c91a859a 236 } // MixAndLimitCam
gke 0:62a1c91a859a 237
gke 0:62a1c91a859a 238 #if ( defined Y6COPTER )
gke 0:62a1c91a859a 239 #include "outputs_y6.h"
gke 0:62a1c91a859a 240 #else
gke 0:62a1c91a859a 241 #if ( defined TRICOPTER | defined MULTICOPTER | defined VTCOPTER )
gke 0:62a1c91a859a 242 #include "outputs_copter.h"
gke 0:62a1c91a859a 243 #else
gke 0:62a1c91a859a 244 #include "outputs_conventional.h"
gke 0:62a1c91a859a 245 #endif // Y6COPTER
gke 0:62a1c91a859a 246 #endif // TRICOPTER | MULTICOPTER
gke 0:62a1c91a859a 247
gke 0:62a1c91a859a 248 void InitI2CESCs(void) {
gke 0:62a1c91a859a 249 static int8 m;
gke 0:62a1c91a859a 250 static uint8 r;
gke 0:62a1c91a859a 251
gke 0:62a1c91a859a 252 #ifdef MULTICOPTER
gke 0:62a1c91a859a 253 if ( P[ESCType] == ESCHolger )
gke 0:62a1c91a859a 254 for ( m = 0 ; m < NoOfI2CESCOutputs ; m++ ) {
gke 0:62a1c91a859a 255 I2CESC.start();
gke 0:62a1c91a859a 256 r = I2CESC.write(0x52 + ( m*2 )); // one cmd, one data byte per motor
gke 0:62a1c91a859a 257 r += I2CESC.write(0);
gke 0:62a1c91a859a 258 ESCI2CFail[m] += r;
gke 0:62a1c91a859a 259 I2CESC.stop();
gke 0:62a1c91a859a 260 }
gke 0:62a1c91a859a 261 else
gke 0:62a1c91a859a 262 if ( P[ESCType] == ESCYGEI2C )
gke 0:62a1c91a859a 263 for ( m = 0 ; m < NoOfI2CESCOutputs ; m++ ) {
gke 0:62a1c91a859a 264 I2CESC.start();
gke 0:62a1c91a859a 265 r = I2CESC.write(0x62 + ( m*2 )); // one cmd, one data byte per motor
gke 0:62a1c91a859a 266 r += I2CESC.write(0);
gke 0:62a1c91a859a 267 ESCI2CFail[m] += r;
gke 0:62a1c91a859a 268 I2CESC.stop();
gke 0:62a1c91a859a 269 }
gke 0:62a1c91a859a 270 else
gke 0:62a1c91a859a 271 if ( P[ESCType] == ESCX3D ) {
gke 0:62a1c91a859a 272 I2CESC.start();
gke 0:62a1c91a859a 273 r = I2CESC.write(0x10); // one command, 4 data bytes
gke 0:62a1c91a859a 274 r += I2CESC.write(0);
gke 0:62a1c91a859a 275 r += I2CESC.write(0);
gke 0:62a1c91a859a 276 r += I2CESC.write(0);
gke 0:62a1c91a859a 277 r += I2CESC.write(0);
gke 0:62a1c91a859a 278 ESCI2CFail[0] += r;
gke 0:62a1c91a859a 279 I2CESC.stop();
gke 0:62a1c91a859a 280 }
gke 0:62a1c91a859a 281 #endif // MULTICOPTER
gke 0:62a1c91a859a 282 } // InitI2CESCs
gke 0:62a1c91a859a 283
gke 0:62a1c91a859a 284 void StopMotors(void) {
gke 0:62a1c91a859a 285 #ifdef MULTICOPTER
gke 0:62a1c91a859a 286 #ifdef Y6COPTER
gke 0:62a1c91a859a 287 PWM[FrontTC] = PWM[LeftTC] = PWM[RightTC] =
gke 0:62a1c91a859a 288 PWM[FrontBC] = PWM[LeftBC] = PWM[RightBC] = ESCMin;
gke 0:62a1c91a859a 289 #else
gke 0:62a1c91a859a 290 PWM[FrontC] = PWM[LeftC] = PWM[RightC] = ESCMin;
gke 0:62a1c91a859a 291 #ifndef TRICOPTER
gke 0:62a1c91a859a 292 PWM[BackC] = ESCMin;
gke 0:62a1c91a859a 293 #endif // !TRICOPTER
gke 0:62a1c91a859a 294 #endif // Y6COPTER
gke 0:62a1c91a859a 295 #else
gke 0:62a1c91a859a 296 PWM[ThrottleC] = ESCMin;
gke 0:62a1c91a859a 297 #endif // MULTICOPTER
gke 0:62a1c91a859a 298
gke 1:1e3318a30ddd 299 Out0.pulsewidth_us(1000 + (int16)( PWM[0] * PWMScale ) );
gke 1:1e3318a30ddd 300 Out1.pulsewidth_us(1000 + (int16)( PWM[1] * PWMScale ) );
gke 1:1e3318a30ddd 301 Out2.pulsewidth_us(1000 + (int16)( PWM[2] * PWMScale ) );
gke 1:1e3318a30ddd 302 Out3.pulsewidth_us(1000 + (int16)( PWM[3] * PWMScale ) );
gke 0:62a1c91a859a 303
gke 1:1e3318a30ddd 304 // Out4.pulsewidth_us(1000 + (int16)( PWM[4] * PWMScale ) );
gke 1:1e3318a30ddd 305 // Out5.pulsewidth_us(1000 + (int16)( PWM[5] * PWMScale ) );
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
gke 0:62a1c91a859a 312 Out0.period_us(PWM_PERIOD_US);
gke 0:62a1c91a859a 313
gke 0:62a1c91a859a 314 StopMotors();
gke 0:62a1c91a859a 315
gke 0:62a1c91a859a 316 #ifndef Y6COPTER
gke 0:62a1c91a859a 317 #ifdef TRICOPTER
gke 0:62a1c91a859a 318 PWM[BackC] = OUT_NEUTRAL;
gke 2:90292f8bd179 319 #endif // !TRICOPTER
gke 0:62a1c91a859a 320 PWM[CamRollC] = OUT_NEUTRAL;
gke 0:62a1c91a859a 321 PWM[CamPitchC] = OUT_NEUTRAL;
gke 2:90292f8bd179 322 CamRollPulseWidth = 1000 + (int16)( PWM[CamRollC] * PWMScale );
gke 2:90292f8bd179 323 CamPitchPulseWidth = 1000 + (int16)( PWM[CamPitchC] * PWMScale );
gke 0:62a1c91a859a 324 #endif // Y6COPTER
gke 0:62a1c91a859a 325
gke 0:62a1c91a859a 326 } // InitMotors
gke 0:62a1c91a859a 327