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