Prof Greg Egan
/
UAVXArm-GKE
UAVX Multicopter Flight Controller.
ir.c@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 | #include "UAVXArm.h" |
gke | 0:62a1c91a859a | 22 | |
gke | 0:62a1c91a859a | 23 | // IR Sensors ( Scheme from "Leveller" Autopilot ~2005 ) |
gke | 0:62a1c91a859a | 24 | |
gke | 0:62a1c91a859a | 25 | void GetIRAttitude(void); |
gke | 0:62a1c91a859a | 26 | void TrackIRMaxMin(real32); |
gke | 0:62a1c91a859a | 27 | void InitIRSensors(void); |
gke | 0:62a1c91a859a | 28 | |
gke | 0:62a1c91a859a | 29 | // FMA Roll/Pitch connector Pin1 Gnd, Pin 2 3.3V, Pin3 -Pitch, Pin4 Roll. |
gke | 0:62a1c91a859a | 30 | // FMA Z-Axis connector Pin1 Gnd, Pin 2 3.3V, Pin3 Z , Pin4 Unused. |
gke | 0:62a1c91a859a | 31 | // DIYDrones connector Pin1 Gnd, Pin 2 3.3V, Pin3 Z, Pin4 -Pitch, Pin5 Roll. |
gke | 0:62a1c91a859a | 32 | |
gke | 0:62a1c91a859a | 33 | real32 IR[3], IRMax, IRMin, IRSwing; |
gke | 0:62a1c91a859a | 34 | |
gke | 0:62a1c91a859a | 35 | |
gke | 0:62a1c91a859a | 36 | void TrackIRMaxMin(real32 m) { |
gke | 0:62a1c91a859a | 37 | if ( m > IRMax ) IRMax = m; |
gke | 0:62a1c91a859a | 38 | else |
gke | 0:62a1c91a859a | 39 | if ( m < IRMin ) IRMin = m; |
gke | 0:62a1c91a859a | 40 | |
gke | 0:62a1c91a859a | 41 | IRSwing = Max( IRSwing, fabs(m) ); |
gke | 0:62a1c91a859a | 42 | IRSwing -= 0.00001; // zzz |
gke | 0:62a1c91a859a | 43 | |
gke | 0:62a1c91a859a | 44 | } // TrackIRMaxMin |
gke | 0:62a1c91a859a | 45 | |
gke | 0:62a1c91a859a | 46 | void GetIRAttitude() { |
gke | 0:62a1c91a859a | 47 | #define IR_NEUTRAL 1.0 |
gke | 0:62a1c91a859a | 48 | static uint8 i; |
gke | 0:62a1c91a859a | 49 | |
gke | 0:62a1c91a859a | 50 | if ( GyroType == IRSensors ) { |
gke | 0:62a1c91a859a | 51 | |
gke | 1:1e3318a30ddd | 52 | IR[Pitch] = -ADC(ADCPitch) * 2.0 - IR_NEUTRAL; |
gke | 1:1e3318a30ddd | 53 | IR[Roll] = ADC(ADCRoll) * 2.0 - IR_NEUTRAL; |
gke | 1:1e3318a30ddd | 54 | IR[Yaw] = ADC(YawADC) * 2.0 - IR_NEUTRAL; |
gke | 0:62a1c91a859a | 55 | |
gke | 0:62a1c91a859a | 56 | for ( i = 0; i < (uint8)3; i++ ) |
gke | 0:62a1c91a859a | 57 | TrackIRMaxMin(IR[i]); |
gke | 0:62a1c91a859a | 58 | |
gke | 0:62a1c91a859a | 59 | for ( i = 0; i < (uint8)2; i++ ) |
gke | 0:62a1c91a859a | 60 | Angle[i] = asin ( Limit( IR[i] / IRSwing, -1.0, 1.0 ) ); |
gke | 0:62a1c91a859a | 61 | |
gke | 0:62a1c91a859a | 62 | Angle[Yaw] = 0.0; |
gke | 0:62a1c91a859a | 63 | Rate[Yaw] = 0.0; |
gke | 0:62a1c91a859a | 64 | |
gke | 0:62a1c91a859a | 65 | } |
gke | 0:62a1c91a859a | 66 | |
gke | 0:62a1c91a859a | 67 | } // GetIRAttitude |
gke | 0:62a1c91a859a | 68 | |
gke | 0:62a1c91a859a | 69 | void InitIRSensors() { |
gke | 0:62a1c91a859a | 70 | static uint8 i; |
gke | 0:62a1c91a859a | 71 | |
gke | 0:62a1c91a859a | 72 | LEDYellow_ON; |
gke | 0:62a1c91a859a | 73 | IRSwing = 0.5; |
gke | 0:62a1c91a859a | 74 | |
gke | 0:62a1c91a859a | 75 | IRMax = -1.0; |
gke | 0:62a1c91a859a | 76 | IRMin= 1.0; |
gke | 0:62a1c91a859a | 77 | |
gke | 0:62a1c91a859a | 78 | for ( i = 0; i < 20; i++ ) { |
gke | 0:62a1c91a859a | 79 | GetIRAttitude(); |
gke | 0:62a1c91a859a | 80 | Delay1mS(100); |
gke | 0:62a1c91a859a | 81 | } |
gke | 0:62a1c91a859a | 82 | |
gke | 0:62a1c91a859a | 83 | LEDYellow_OFF; |
gke | 0:62a1c91a859a | 84 | |
gke | 0:62a1c91a859a | 85 | } // InitIRSensors |