ir.c

00001 // ===============================================================================================
00002 // =                              UAVXArm Quadrocopter Controller                                =
00003 // =                           Copyright (c) 2008 by Prof. Greg Egan                             =
00004 // =                 Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer                   =
00005 // =                           http://code.google.com/p/uavp-mods/                               =
00006 // ===============================================================================================
00007 
00008 //    This is part of UAVXArm.
00009 
00010 //    UAVXArm is free software: you can redistribute it and/or modify it under the terms of the GNU
00011 //    General Public License as published by the Free Software Foundation, either version 3 of the
00012 //    License, or (at your option) any later version.
00013 
00014 //    UAVXArm is distributed in the hope that it will be useful,but WITHOUT ANY WARRANTY; without
00015 //    even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
00016 //    See the GNU General Public License for more details.
00017 
00018 //    You should have received a copy of the GNU General Public License along with this program.
00019 //    If not, see http://www.gnu.org/licenses/
00020 
00021 #include "UAVXArm.h"
00022 
00023 // IR Sensors ( Scheme from "Leveller" Autopilot ~2005 )
00024 
00025 void GetIRAttitude(void);
00026 void TrackIRMaxMin(real32);
00027 void InitIRSensors(void);
00028 
00029 // FMA Roll/Pitch connector Pin1 Gnd, Pin 2 3.3V, Pin3 -Pitch, Pin4 Roll.
00030 // FMA Z-Axis     connector Pin1 Gnd, Pin 2 3.3V, Pin3 Z , Pin4 Unused.
00031 // DIYDrones      connector Pin1 Gnd, Pin 2 3.3V, Pin3 Z, Pin4 -Pitch, Pin5 Roll.
00032 
00033 real32 IR[3], IRMax, IRMin, IRSwing;
00034 
00035 
00036 void TrackIRMaxMin(real32 m) {
00037     if ( m > IRMax ) IRMax = m;
00038     else
00039         if ( m < IRMin ) IRMin = m;
00040         
00041     IRSwing = Max( IRSwing, fabs(m) );
00042     IRSwing -= 0.00001; // zzz
00043 
00044 } // TrackIRMaxMin
00045 
00046 void GetIRAttitude() {
00047     #define IR_NEUTRAL 1.0
00048     static uint8 i;
00049 
00050     if ( GyroType == IRSensors ) {
00051     
00052     IR[Pitch] = -ADC(ADCPitch) * 2.0 - IR_NEUTRAL;
00053     IR[Roll] = ADC(ADCRoll) * 2.0 - IR_NEUTRAL;
00054     IR[Yaw] = ADC(YawADC) * 2.0 - IR_NEUTRAL;
00055 
00056     for ( i = 0; i < (uint8)3; i++ )
00057         TrackIRMaxMin(IR[i]);
00058 
00059     for ( i = 0; i < (uint8)2; i++ )
00060         Angle[i] = asin ( Limit( IR[i] / IRSwing, -1.0, 1.0 ) );
00061 
00062     Angle[Yaw] = 0.0;
00063     Rate[Yaw] = 0.0;
00064     
00065     }
00066 
00067 } // GetIRAttitude
00068 
00069 void InitIRSensors() {
00070     static uint8 i;
00071 
00072     LEDYellow_ON;
00073     IRSwing = 0.5;
00074 
00075     IRMax = -1.0;
00076     IRMin= 1.0;
00077 
00078     for ( i = 0; i < 20; i++ ) {
00079         GetIRAttitude();
00080         Delay1mS(100);
00081     }
00082 
00083     LEDYellow_OFF;
00084 
00085 } // InitIRSensors