outputs_copter.h

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 
00022 void OutSignals(void) {
00023     static int8 m;
00024     static uint8 r;
00025 
00026 #if !( defined SIMULATE | defined TESTING )
00027 
00028     if ( !F.MotorsArmed )
00029         StopMotors();
00030 
00031     PWM[FrontC] = TC(PWM[FrontC]);
00032     PWM[LeftC] = TC(PWM[LeftC]);
00033     PWM[RightC] = TC(PWM[RightC]);
00034 #ifdef TRICOPTER
00035     PWM[BackC] = Limit(PWM[BackC], 1, OUT_MAXIMUM);
00036 #else
00037     PWM[BackC] = TC(PWM[BackC]);
00038 #endif
00039 
00040     Out0.pulsewidth_us(1000 + (int16)( PWM[FrontC] * PWMScale ) );
00041     Out1.pulsewidth_us(1000 + (int16)( PWM[RightC] * PWMScale ) );
00042     Out2.pulsewidth_us(1000 + (int16)( PWM[BackC] * PWMScale ) );
00043     Out3.pulsewidth_us(1000 + (int16)( PWM[LeftC] * PWMScale ) );
00044                       
00045 #ifdef MULTICOPTER
00046     // in X3D and Holger-Mode, K2 (left motor) is SDA, K3 (right) is SCL.
00047     // ACK (r) not checked as no recovery is possible.
00048     // Octocopters may have ESCs paired with common address so ACK is meaningless.
00049     // All motors driven with fourth motor ignored for Tricopter.
00050 
00051     if ( P[ESCType] ==  ESCHolger )
00052         for ( m = 0 ; m < NoOfI2CESCOutputs ; m++ ) {
00053             I2CESC.start();
00054             r = I2CESC.write(0x52 + ( m*2 ));        // one command, one data byte per motor
00055             r += I2CESC.write( PWM[m] );
00056             ESCI2CFail[m] += r;
00057             I2CESC.stop();
00058         }
00059     else
00060         if ( P[ESCType] == ESCYGEI2C )
00061             for ( m = 0 ; m < NoOfI2CESCOutputs ; m++ ) {
00062                 I2CESC.start();
00063                 r = I2CESC.write(0x62 + ( m*2) );    // one cmd, one data byte per motor
00064                 r += I2CESC.write( PWM[m] * 0.5 );
00065                 ESCI2CFail[m] += r;
00066                 I2CESC.stop();
00067             }
00068         else
00069            if ( P[ESCType] == ESCX3D ) {
00070                 I2CESC.start();
00071                 r = I2CESC.write(0x10);                // one command, 4 data bytes
00072                 r += I2CESC.write( PWM[FrontC] );
00073                 r += I2CESC.write( PWM[RightC] );
00074                 r += I2CESC.write( PWM[BackC] );
00075                 r += I2CESC.write( PWM[LeftC] );
00076                 ESCI2CFail[0] += r;
00077                 //  other ESCs if a Hexacopter
00078                 I2CESC.stop();
00079             }
00080 #endif //  MULTICOPTER
00081 
00082 #else
00083     PWM[FrontC] = Limit(PWM[FrontC], ESCMin, ESCMax);
00084     PWM[LeftC] = Limit(PWM[LeftC], ESCMin, ESCMax);
00085     PWM[RightC] = Limit(PWM[RightC], ESCMin, ESCMax);
00086 #ifdef TRICOPTER
00087     PWM[BackC] = Limit(PWM[BackC], 1, OUT_MAXIMUM);
00088 #else
00089     PWM[BackC] = Limit(PWM[BackC], ESCMin, ESCMax);
00090 #endif
00091 
00092 #endif // !(SIMULATE | TESTING)
00093 
00094 } // OutSignals
00095 
00096 
00097 
00098