Prof Greg Egan
/
UAVXArm-GKE
UAVX Multicopter Flight Controller.
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?
User | Revision | Line number | New 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 |