Prof Greg Egan / Mbed 2 deprecated UAVXArm-GKE

UAVX Multicopter Flight Controller.

Dependencies:   mbed

outputs_copter.h@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
gke 0:62a1c91a859a 22 void OutSignals(void) {
gke 0:62a1c91a859a 23 static int8 m;
gke 0:62a1c91a859a 24 static uint8 r;
gke 0:62a1c91a859a 25
gke 0:62a1c91a859a 26 #if !( defined SIMULATE | defined TESTING )
gke 0:62a1c91a859a 27
gke 0:62a1c91a859a 28 if ( !F.MotorsArmed )
gke 0:62a1c91a859a 29 StopMotors();
gke 0:62a1c91a859a 30
gke 0:62a1c91a859a 31 PWM[FrontC] = TC(PWM[FrontC]);
gke 0:62a1c91a859a 32 PWM[LeftC] = TC(PWM[LeftC]);
gke 0:62a1c91a859a 33 PWM[RightC] = TC(PWM[RightC]);
gke 0:62a1c91a859a 34 #ifdef TRICOPTER
gke 0:62a1c91a859a 35 PWM[BackC] = Limit(PWM[BackC], 1, OUT_MAXIMUM);
gke 0:62a1c91a859a 36 #else
gke 0:62a1c91a859a 37 PWM[BackC] = TC(PWM[BackC]);
gke 0:62a1c91a859a 38 #endif
gke 0:62a1c91a859a 39
gke 0:62a1c91a859a 40 Out0.pulsewidth_us(1000 + (int16)( PWM[FrontC] * PWMScale ) );
gke 1:1e3318a30ddd 41 Out1.pulsewidth_us(1000 + (int16)( PWM[RightC] * PWMScale ) );
gke 1:1e3318a30ddd 42 Out2.pulsewidth_us(1000 + (int16)( PWM[BackC] * PWMScale ) );
gke 1:1e3318a30ddd 43 Out3.pulsewidth_us(1000 + (int16)( PWM[LeftC] * PWMScale ) );
gke 0:62a1c91a859a 44
gke 0:62a1c91a859a 45 #ifdef MULTICOPTER
gke 0:62a1c91a859a 46 // in X3D and Holger-Mode, K2 (left motor) is SDA, K3 (right) is SCL.
gke 0:62a1c91a859a 47 // ACK (r) not checked as no recovery is possible.
gke 0:62a1c91a859a 48 // Octocopters may have ESCs paired with common address so ACK is meaningless.
gke 0:62a1c91a859a 49 // All motors driven with fourth motor ignored for Tricopter.
gke 0:62a1c91a859a 50
gke 0:62a1c91a859a 51 if ( P[ESCType] == ESCHolger )
gke 0:62a1c91a859a 52 for ( m = 0 ; m < NoOfI2CESCOutputs ; m++ ) {
gke 0:62a1c91a859a 53 I2CESC.start();
gke 0:62a1c91a859a 54 r = I2CESC.write(0x52 + ( m*2 )); // one command, one data byte per motor
gke 0:62a1c91a859a 55 r += I2CESC.write( PWM[m] );
gke 0:62a1c91a859a 56 ESCI2CFail[m] += r;
gke 0:62a1c91a859a 57 I2CESC.stop();
gke 0:62a1c91a859a 58 }
gke 0:62a1c91a859a 59 else
gke 0:62a1c91a859a 60 if ( P[ESCType] == ESCYGEI2C )
gke 0:62a1c91a859a 61 for ( m = 0 ; m < NoOfI2CESCOutputs ; m++ ) {
gke 0:62a1c91a859a 62 I2CESC.start();
gke 0:62a1c91a859a 63 r = I2CESC.write(0x62 + ( m*2) ); // one cmd, one data byte per motor
gke 0:62a1c91a859a 64 r += I2CESC.write( PWM[m] * 0.5 );
gke 0:62a1c91a859a 65 ESCI2CFail[m] += r;
gke 0:62a1c91a859a 66 I2CESC.stop();
gke 0:62a1c91a859a 67 }
gke 0:62a1c91a859a 68 else
gke 1:1e3318a30ddd 69 if ( P[ESCType] == ESCX3D ) {
gke 0:62a1c91a859a 70 I2CESC.start();
gke 0:62a1c91a859a 71 r = I2CESC.write(0x10); // one command, 4 data bytes
gke 0:62a1c91a859a 72 r += I2CESC.write( PWM[FrontC] );
gke 1:1e3318a30ddd 73 r += I2CESC.write( PWM[RightC] );
gke 0:62a1c91a859a 74 r += I2CESC.write( PWM[BackC] );
gke 0:62a1c91a859a 75 r += I2CESC.write( PWM[LeftC] );
gke 0:62a1c91a859a 76 ESCI2CFail[0] += r;
gke 0:62a1c91a859a 77 // other ESCs if a Hexacopter
gke 0:62a1c91a859a 78 I2CESC.stop();
gke 0:62a1c91a859a 79 }
gke 0:62a1c91a859a 80 #endif // MULTICOPTER
gke 0:62a1c91a859a 81
gke 0:62a1c91a859a 82 #else
gke 0:62a1c91a859a 83 PWM[FrontC] = Limit(PWM[FrontC], ESCMin, ESCMax);
gke 0:62a1c91a859a 84 PWM[LeftC] = Limit(PWM[LeftC], ESCMin, ESCMax);
gke 0:62a1c91a859a 85 PWM[RightC] = Limit(PWM[RightC], ESCMin, ESCMax);
gke 0:62a1c91a859a 86 #ifdef TRICOPTER
gke 0:62a1c91a859a 87 PWM[BackC] = Limit(PWM[BackC], 1, OUT_MAXIMUM);
gke 0:62a1c91a859a 88 #else
gke 0:62a1c91a859a 89 PWM[BackC] = Limit(PWM[BackC], ESCMin, ESCMax);
gke 0:62a1c91a859a 90 #endif
gke 0:62a1c91a859a 91
gke 0:62a1c91a859a 92 #endif // !(SIMULATE | TESTING)
gke 0:62a1c91a859a 93
gke 0:62a1c91a859a 94 } // OutSignals
gke 0:62a1c91a859a 95
gke 0:62a1c91a859a 96
gke 0:62a1c91a859a 97
gke 0:62a1c91a859a 98