Diff: outputs_copter.h

Revision:

0:62a1c91a859a

Child:

1:1e3318a30ddd

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/outputs_copter.h	Fri Feb 18 22:28:05 2011 +0000
@@ -0,0 +1,99 @@
+// ===============================================================================================
+// =                              UAVXArm Quadrocopter Controller                                =
+// =                           Copyright (c) 2008 by Prof. Greg Egan                             =
+// =                 Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer                   =
+// =                     http://code.google.com/p/uavp-mods/ http://uavp.ch                      =
+// ===============================================================================================
+
+//    This is part of UAVXArm.
+
+//    UAVXArm is free software: you can redistribute it and/or modify it under the terms of the GNU
+//    General Public License as published by the Free Software Foundation, either version 3 of the
+//    License, or (at your option) any later version.
+
+//    UAVXArm is distributed in the hope that it will be useful,but WITHOUT ANY WARRANTY; without
+//    even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+//    See the GNU General Public License for more details.
+
+//    You should have received a copy of the GNU General Public License along with this program.
+//    If not, see http://www.gnu.org/licenses/
+
+
+void OutSignals(void) {
+    static int8 m;
+    static uint8 r;
+
+#if !( defined SIMULATE | defined TESTING )
+
+    if ( !F.MotorsArmed )
+        StopMotors();
+
+    PWM[FrontC] = TC(PWM[FrontC]);
+    PWM[LeftC] = TC(PWM[LeftC]);
+    PWM[RightC] = TC(PWM[RightC]);
+#ifdef TRICOPTER
+    PWM[BackC] = Limit(PWM[BackC], 1, OUT_MAXIMUM);
+#else
+    PWM[BackC] = TC(PWM[BackC]);
+#endif
+
+    Out0.pulsewidth_us(1000 + (int16)( PWM[FrontC] * PWMScale ) );
+    Out1.pulsewidth_us(1000 + (int16)( PWM[LeftC] * PWMScale ) );
+    Out2.pulsewidth_us(1000 + (int16)( PWM[RightC] * PWMScale ) );
+    Out3.pulsewidth_us(1000 + (int16)( PWM[BackC] * PWMScale ) );
+                      
+#ifdef MULTICOPTER
+    // in X3D and Holger-Mode, K2 (left motor) is SDA, K3 (right) is SCL.
+    // ACK (r) not checked as no recovery is possible.
+    // Octocopters may have ESCs paired with common address so ACK is meaningless.
+    // All motors driven with fourth motor ignored for Tricopter.
+
+    if ( P[ESCType] ==  ESCHolger )
+        for ( m = 0 ; m < NoOfI2CESCOutputs ; m++ ) {
+            I2CESC.start();
+            r = I2CESC.write(0x52 + ( m*2 ));        // one command, one data byte per motor
+            r += I2CESC.write( PWM[m] );
+            ESCI2CFail[m] += r;
+            I2CESC.stop();
+        }
+    else
+        if ( P[ESCType] == ESCYGEI2C )
+            for ( m = 0 ; m < NoOfI2CESCOutputs ; m++ ) {
+                I2CESC.start();
+                r = I2CESC.write(0x62 + ( m*2) );    // one cmd, one data byte per motor
+                r += I2CESC.write( PWM[m] * 0.5 );
+                ESCI2CFail[m] += r;
+                I2CESC.stop();
+            }
+        else
+            if ( P[ESCType] == ESCX3D ) {
+                I2CESC.start();
+                r = I2CESC.write(0x10);                // one command, 4 data bytes
+                r += I2CESC.write( PWM[FrontC] );
+                r += I2CESC.write( PWM[BackC] );
+                r += I2CESC.write( PWM[LeftC] );
+                r += I2CESC.write( PWM[RightC] );
+                ESCI2CFail[0] += r;
+                //  other ESCs if a Hexacopter
+                I2CESC.stop();
+            }
+#endif //  MULTICOPTER
+
+#else
+
+    PWM[FrontC] = Limit(PWM[FrontC], ESCMin, ESCMax);
+    PWM[LeftC] = Limit(PWM[LeftC], ESCMin, ESCMax);
+    PWM[RightC] = Limit(PWM[RightC], ESCMin, ESCMax);
+#ifdef TRICOPTER
+    PWM[BackC] = Limit(PWM[BackC], 1, OUT_MAXIMUM);
+#else
+    PWM[BackC] = Limit(PWM[BackC], ESCMin, ESCMax);
+#endif
+
+#endif // !(SIMULATE | TESTING)
+
+} // OutSignals
+
+
+
+