outputs.c

00001 // ===============================================================================================
00002 // =                              UAVXArm Quadrocopter Controller                                =
00003 // =                           Copyright (c) 2008 by Prof. Greg Egan                             =
00004 // =                 Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer                   =
00005 // =                           http://code.google.com/p/uavp-mods/                               =
00006 // ===============================================================================================
00007 
00008 //    This is part of UAVXArm.
00009 
00010 //    UAVXArm is free software: you can redistribute it and/or modify it under the terms of the GNU
00011 //    General Public License as published by the Free Software Foundation, either version 3 of the
00012 //    License, or (at your option) any later version.
00013 
00014 //    UAVXArm is distributed in the hope that it will be useful,but WITHOUT ANY WARRANTY; without
00015 //    even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
00016 //    See the GNU General Public License for more details.
00017 
00018 //    You should have received a copy of the GNU General Public License along with this program.
00019 //    If not, see http://www.gnu.org/licenses/
00020 
00021 #include "UAVXArm.h"
00022 
00023 const real32 PWMScale = 1000.0 / OUT_MAXIMUM;
00024 
00025 void DoMulticopterMix(real32);
00026 void CheckDemand(real32);
00027 void MixAndLimitMotors(void);
00028 void MixAndLimitCam(void);
00029 void OutSignals(void);
00030 void InitI2CESCs(void);
00031 void StopMotors(void);
00032 void ExercisePWM(void);
00033 void InitMotors(void);
00034 
00035 boolean OutToggle;
00036 real32 PWM[8];
00037 real32 PWMSense[8];
00038 int16 ESCI2CFail[6];
00039 int16 CurrThrottle;
00040 int16 CamRollPulseWidth, CamPitchPulseWidth;
00041 int16 ESCMin, ESCMax;
00042 
00043 #define PWM_PERIOD_US         (1000000/PWM_UPDATE_HZ)
00044 
00045 #ifdef MULTICOPTER
00046 
00047 uint8 TC(int16 T) {
00048     return ( Limit(T, ESCMin, ESCMax) );
00049 } // TC
00050 
00051 void DoMulticopterMix(real32 CurrThrottle) {
00052 #ifndef MULTICOPTER
00053     static real32 Temp;
00054 #endif
00055 
00056 #ifdef Y6COPTER
00057     PWM[FrontTC] = PWM[LeftTC] = PWM[RightTC] = CurrThrottle;
00058 #else
00059     PWM[FrontC] = PWM[LeftC] = PWM[RightC] = PWM[BackC] = CurrThrottle;
00060 #endif
00061 
00062 #ifdef TRICOPTER // usually flown K1 motor to the rear - use orientation of 24
00063     Temp = Pl * 0.5;
00064     PWM[FrontC] -= Pl;            // front motor
00065     PWM[LeftC] += (Temp - Rl);    // right rear
00066     PWM[RightC] += (Temp + Rl);   // left rear
00067 
00068     PWM[BackC] = -PWMSense[RudderC] * Yl + OUT_NEUTRAL;    // yaw servo
00069     if ( fabs(K[Balance]) > 0.5 )
00070         PWM[FrontC] =  PWM[FrontC] * K[Balance];
00071 #else
00072 #ifdef VTCOPTER     // usually flown VTail (K1+K4) to the rear - use orientation of 24
00073     Temp = Pl * 0.5;
00074 
00075     PWM[LeftC] += (Temp - Rl);    // right rear
00076     PWM[RightC] += (Temp + Rl); // left rear
00077 
00078     PWM[FrontLeftC] -= Pl - PWMSense[RudderC] * Yl;
00079     PWM[FrontRightC] -= Pl + PWMSense[RudderC] * Yl;
00080     if ( fabs(K[Balance]) > 0.01 ) {
00081         PWM[FrontLeftC] = PWM[FrontLeftC] * K[Balance];
00082         PWM[FrontRightC] = PWM[FrontRightC] * K[Balance];
00083     }
00084 #else
00085 #ifdef Y6COPTER
00086     Temp = Pl * 0.5;
00087     PWM[FrontTC] -= Pl;          // front motor
00088     PWM[LeftTC] += (Temp - Rl);  // right rear
00089     PWM[RightTC] += (Temp + Rl); // left rear
00090 
00091     PWM[FrontBC] = PWM[FrontTC];
00092     PWM[LeftBC]  = PWM[LeftTC];
00093     PWM[RightBC] = PWM[RightTC];
00094 
00095     if ( fabs(K[Balance]) > 0.01 ) {
00096         PWM[FrontTC] =  PWM[FrontTC] * K[Balance];
00097         PWM[FrontBC] = PWM[FrontTC];
00098     }
00099 
00100     Temp = Yl * 0.5;
00101     PWM[FrontTC] -= Temp;
00102     PWM[LeftTC]  -= Temp;
00103     PWM[RightTC] -= Temp;
00104 
00105     PWM[FrontBC] += Temp;
00106     PWM[LeftBC]  += Temp;
00107     PWM[RightBC] += Temp;
00108 #else
00109     PWM[LeftC]  += -Rl + Yl;
00110     PWM[RightC] +=  Rl + Yl;
00111     PWM[FrontC] += -Pl - Yl;
00112     PWM[BackC]  +=  Pl - Yl;
00113 #endif
00114 #endif
00115 #endif
00116 
00117 } // DoMulticopterMix
00118 
00119 boolean     MotorDemandRescale;
00120 
00121 void CheckDemand(real32 CurrThrottle) {
00122     static real32 Scale, ScaleHigh, ScaleLow, MaxMotor, DemandSwing;
00123 
00124 #ifdef Y6COPTER
00125     MaxMotor = Max(PWM[FrontTC], PWM[LeftTC]);
00126     MaxMotor = Max(MaxMotor, PWM[RightTC]);
00127     MaxMotor = Max(MaxMotor, PWM[FrontBC]);
00128     MaxMotor = Max(MaxMotor, PWM[LeftBC]);
00129     MaxMotor = Max(MaxMotor, PWM[RightBC]);
00130 #else
00131     MaxMotor = Max(PWM[FrontC], PWM[LeftC]);
00132     MaxMotor = Max(MaxMotor, PWM[RightC]);
00133 #ifndef TRICOPTER
00134     MaxMotor = Max(MaxMotor, PWM[BackC]);
00135 #endif // TRICOPTER
00136 #endif // Y6COPTER
00137 
00138     DemandSwing = MaxMotor - CurrThrottle;
00139 
00140     if ( DemandSwing > 0.0 ) {
00141         ScaleHigh = (OUT_MAXIMUM - CurrThrottle) / DemandSwing;
00142         ScaleLow = (CurrThrottle - IdleThrottle) / DemandSwing;
00143         Scale = Min(ScaleHigh, ScaleLow); // just in case!
00144         if ( Scale < 0.0 ) Scale = 1.0 / OUT_MAXIMUM;
00145         if ( Scale < 1.0 ) {
00146             MotorDemandRescale = true;
00147             Rl *= Scale;  // could get rid of the divides
00148             Pl *= Scale;
00149 #ifndef TRICOPTER
00150             Yl *= Scale;
00151 #endif // TRICOPTER 
00152         } else
00153             MotorDemandRescale = false;
00154     } else
00155         MotorDemandRescale = false;
00156 
00157 } // CheckDemand
00158 
00159 #endif // MULTICOPTER
00160 
00161 void MixAndLimitMotors(void) {
00162 #ifndef MULTICOPTER
00163     static TempElevon, TempElevator;
00164     static uint8 m;
00165 #endif
00166     static real32 Temp;
00167 
00168     if ( DesiredThrottle < IdleThrottle )
00169         CurrThrottle = 0;
00170     else
00171         CurrThrottle = DesiredThrottle;
00172 
00173 #ifdef MULTICOPTER
00174     if ( State == InFlight )
00175         CurrThrottle += AltComp; // vertical compensation not optional
00176 
00177     Temp = OUT_MAXIMUM * 0.9; // 10% headroom for control
00178     CurrThrottle = Limit(CurrThrottle, 0, Temp );
00179 
00180     if ( CurrThrottle > IdleThrottle ) {
00181         DoMulticopterMix(CurrThrottle);
00182 
00183         CheckDemand(CurrThrottle);
00184 
00185         if ( MotorDemandRescale )
00186             DoMulticopterMix(CurrThrottle);
00187     } else {
00188 #ifdef Y6COPTER
00189         for ( m = 0; m < (uint8)6; m++ )
00190             PWM[m] = CurrThrottle;
00191 #else
00192         PWM[FrontC] = PWM[LeftC] = PWM[RightC] = CurrThrottle;
00193 #ifdef TRICOPTER
00194         PWM[BackC] = -PWMSense[RudderC] * Yl + OUT_NEUTRAL;    // yaw servo
00195 #else
00196         PWM[BackC] = CurrThrottle;
00197 #endif // !TRICOPTER
00198 #endif // Y6COPTER
00199     }
00200 #else
00201     CurrThrottle += Comp[Alt]; // simple - faster to climb with no elevator yet
00202 
00203     PWM[ThrottleC] = CurrThrottle;
00204     PWM[RudderC] = -PWMSense[RudderC] * Yl + OUT_NEUTRAL;
00205 
00206 #if ( defined AILERON | defined HELICOPTER )
00207     PWM[AileronC] = PWMSense[AileronC] * Rl + OUT_NEUTRAL;
00208     PWM[ElevatorC] = PWMSense[ElevatorC] * Pl + OUT_NEUTRAL;
00209 #else // ELEVON
00210     TempElevator = PWMSense[2] * Pl + OUT_NEUTRAL;
00211     PWM[RightElevonC] = PWMSense[RightElevonC] * (TempElevator + Rl);
00212     PWM[LeftElevonC] = PWMSense[LeftElevonC] * (TempElevator -  Rl);
00213 #endif
00214 #endif
00215 
00216 } // MixAndLimitMotors
00217 
00218 void MixAndLimitCam(void) {
00219 
00220     static real32 NewCamRoll, NewCamPitch;
00221     
00222     PWMSense[CamRollC] = 1.0; 
00223     PWMSense[CamPitchC] = 1.0;
00224  
00225     NewCamRoll = CameraRollAngle * K[CamRollKp] + K[CamRollTrim];
00226     NewCamRoll = PWMSense[CamRollC] * NewCamRoll + OUT_NEUTRAL;
00227     PWM[CamRollC] = SlewLimit( PWM[CamRollC], NewCamRoll, 2); // change to 10Hz filter
00228 
00229     NewCamPitch = CameraPitchAngle * K[CamPitchKp] + DesiredCamPitchTrim;
00230     NewCamPitch = PWMSense[CamPitchC] * NewCamPitch + OUT_NEUTRAL; 
00231     PWM[CamPitchC] = SlewLimit( PWM[CamPitchC], NewCamPitch, 2.0); // change to 10Hz filter
00232  
00233     CamRollPulseWidth = 1000 + (int16)( PWM[CamRollC] * PWMScale );
00234     CamPitchPulseWidth = 1000 + (int16)( PWM[CamPitchC] * PWMScale );
00235 
00236 } // MixAndLimitCam
00237 
00238 #if ( defined Y6COPTER )
00239 #include "outputs_y6.h"
00240 #else
00241 #if ( defined TRICOPTER | defined MULTICOPTER | defined VTCOPTER )
00242 #include "outputs_copter.h"
00243 #else
00244 #include "outputs_conventional.h"
00245 #endif // Y6COPTER
00246 #endif // TRICOPTER | MULTICOPTER
00247 
00248 void InitI2CESCs(void) {
00249     static int8 m;
00250     static uint8 r;
00251 
00252 #ifdef MULTICOPTER
00253     if ( P[ESCType] ==  ESCHolger )
00254         for ( m = 0 ; m < NoOfI2CESCOutputs ; m++ ) {
00255             I2CESC.start();
00256             r = I2CESC.write(0x52 + ( m*2 ));        // one cmd, one data byte per motor
00257             r += I2CESC.write(0);
00258             ESCI2CFail[m] += r;
00259             I2CESC.stop();
00260         }
00261     else
00262         if ( P[ESCType] == ESCYGEI2C )
00263             for ( m = 0 ; m < NoOfI2CESCOutputs ; m++ ) {
00264                 I2CESC.start();
00265                 r = I2CESC.write(0x62 + ( m*2 ));    // one cmd, one data byte per motor
00266                 r += I2CESC.write(0);
00267                 ESCI2CFail[m] += r;
00268                 I2CESC.stop();
00269             }
00270         else
00271             if ( P[ESCType] == ESCX3D ) {
00272                 I2CESC.start();
00273                 r = I2CESC.write(0x10);            // one command, 4 data bytes
00274                 r += I2CESC.write(0);
00275                 r += I2CESC.write(0);
00276                 r += I2CESC.write(0);
00277                 r += I2CESC.write(0);
00278                 ESCI2CFail[0] += r;
00279                 I2CESC.stop();
00280             }
00281 #endif // MULTICOPTER
00282 } // InitI2CESCs
00283 
00284 void StopMotors(void) {
00285 #ifdef MULTICOPTER
00286 #ifdef Y6COPTER
00287     PWM[FrontTC] = PWM[LeftTC] = PWM[RightTC] =
00288     PWM[FrontBC] = PWM[LeftBC] = PWM[RightBC] = ESCMin;
00289 #else
00290     PWM[FrontC] = PWM[LeftC] = PWM[RightC] = ESCMin;
00291 #ifndef TRICOPTER
00292     PWM[BackC] = ESCMin;
00293 #endif // !TRICOPTER
00294 #endif // Y6COPTER    
00295 #else
00296     PWM[ThrottleC] = ESCMin;
00297 #endif // MULTICOPTER
00298 
00299     Out0.pulsewidth_us(1000 + (int16)( PWM[0] * PWMScale ) );
00300     Out1.pulsewidth_us(1000 + (int16)( PWM[1] * PWMScale ) );
00301     Out2.pulsewidth_us(1000 + (int16)( PWM[2] * PWMScale ) );
00302     Out3.pulsewidth_us(1000 + (int16)( PWM[3] * PWMScale ) );
00303     
00304  //   Out4.pulsewidth_us(1000 + (int16)( PWM[4] * PWMScale ) );
00305  //   Out5.pulsewidth_us(1000 + (int16)( PWM[5] * PWMScale ) );
00306 
00307     F.MotorsArmed = false;
00308 } // StopMotors
00309 
00310 void InitMotors(void) {
00311 
00312     Out0.period_us(PWM_PERIOD_US);
00313 
00314     StopMotors();
00315 
00316 #ifndef Y6COPTER
00317 #ifdef TRICOPTER
00318     PWM[BackC] = OUT_NEUTRAL;
00319 #endif // !TRICOPTER
00320     PWM[CamRollC] = OUT_NEUTRAL;
00321     PWM[CamPitchC] = OUT_NEUTRAL;
00322     CamRollPulseWidth = 1000 + (int16)( PWM[CamRollC] * PWMScale );
00323     CamPitchPulseWidth = 1000 + (int16)( PWM[CamPitchC] * PWMScale );    
00324 #endif // Y6COPTER
00325 
00326 } // InitMotors
00327