Prof Greg Egan / Mbed 2 deprecated UAVXArm-GKE

UAVX Multicopter Flight Controller.

Dependencies:   mbed

ir.c@0:62a1c91a859a, 2011-02-18 (annotated)

Committer:
gke
Date:
Fri Feb 18 22:28:05 2011 +0000
Revision:
0:62a1c91a859a
Child:
1:1e3318a30ddd
First release

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 0:62a1c91a859a 5 // = http://code.google.com/p/uavp-mods/ http://uavp.ch =
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 0:62a1c91a859a 52 IR[Pitch] = -PitchADC.read() * 2.0 - IR_NEUTRAL;
gke 0:62a1c91a859a 53 IR[Roll] = RollADC.read() * 2.0 - IR_NEUTRAL;
gke 0:62a1c91a859a 54 IR[Yaw] = YawADC.read() * 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