analog.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 real32 ADC(uint8);
00024 void GetBattery(void);
00025 void BatteryTest(void);
00026 void InitBattery(void);
00027 
00028 real32 BatteryVolts, BatteryCurrentADCEstimated, BatteryChargeUsedAH;
00029 real32 BatteryCharge, BatteryCurrent;
00030 real32 BatteryVoltsScale;
00031 
00032 void DirectAnalog(void) {
00033 // Framework Simon Ford
00034 
00035     static uint32 data;
00036 
00037     // Select channel and start conversion
00038     LPC_ADC->ADCR &= ~0xff;
00039     LPC_ADC->ADCR |= 1 << 5; // ADC0[5]
00040     LPC_ADC->ADCR |= 1 << 24;
00041 
00042     // Repeatedly get the sample data until DONE bit
00043     do {
00044         data = LPC_ADC->ADGDR;
00045     } while ((data & ((uint32)1 << 31)) == 0);
00046 
00047     // Stop conversion
00048     LPC_ADC->ADCR &= ~(1 << 24);
00049 
00050 } // DirectAnalog
00051 
00052 void InitDirectAnalog(void) {
00053 
00054     // power on, clk divider /4
00055     LPC_SC->PCONP |= (1 << 12);
00056     LPC_SC->PCLKSEL0 &= ~(0x3 << 24);
00057 
00058     // software-controlled ADC settings
00059     LPC_ADC->ADCR = (0 << 0) // SEL: 0 = no channels selected
00060                     | (25 << 8)    // CLKDIV: PCLK max ~= 25MHz, /25 to give safe 1MHz
00061                     | (0 << 16)    // BURST: 0 = software control
00062                     | (0 << 17)    // CLKS: not applicable
00063                     | (1 << 21)    // PDN: 1 = operational
00064                     | (0 << 24)    // START: 0 = no start
00065                     | (0 << 27);   // EDGE: not applicable
00066 
00067     // setup P1_31 as sel 3 (ADC), mode 2 (no pull)
00068     LPC_PINCON->PINSEL3 &= ~((uint32)0x3 << 30);
00069     LPC_PINCON->PINSEL3 |= (uint32)0x3 << 30;
00070 
00071     LPC_PINCON->PINMODE3 &= ~((uint32)0x3 << 30);
00072     LPC_PINCON->PINMODE3 |= (uint32)0x2 << 30;
00073 
00074 } // InitDirectAnalog
00075 
00076 real32 ADC(uint8 p) {
00077 
00078     static real32 r;
00079 
00080     switch (p) {
00081         case ADCPitch:
00082             r = PitchADC.read();
00083             break;
00084         case ADCRoll:
00085             r = RollADC.read();
00086             break;
00087         case ADCYaw:
00088             r = YawADC.read();
00089             break;
00090         case ADCRangefinder:
00091             r = RangefinderADC.read();
00092             break;
00093         case ADCBatteryCurrent:
00094             r = BatteryCurrentADC.read();
00095             break;
00096         case ADCBatteryVolts:
00097             r =  BatteryVoltsADC.read();
00098             break;
00099     }
00100 
00101     return ( r );
00102 
00103 } // ADC
00104 
00105 void GetBattery(void) {
00106     //  AttoPilot Voltage and Current Sense Breakout SparkFun Part: SEN-09028
00107 
00108     const real32 BatteryCurrentScale = 90.15296;    // Amps FS
00109 
00110     if ( F.HaveBatterySensor ) {
00111         BatteryCurrent = ADC(ADCBatteryCurrent) * BatteryCurrentScale;
00112         BatteryChargeUsedAH  += BatteryCurrent * (real32)(mSClock() - mS[LastBattery]) * 2.777777e-7;
00113         mS[LastBattery] = mSClock();
00114     } else
00115         BatteryCurrent =  BatteryChargeUsedAH = 0;
00116 
00117     BatteryVolts = ADC(ADCBatteryVolts) *  BatteryVoltsScale;
00118     F.LowBatt = BatteryVolts < K[LowVoltThres];
00119 
00120 } // GetBattery
00121 
00122 void BatteryTest(void) {
00123 
00124     TxString("\r\nBattery test\r\n");
00125 
00126     GetBattery();
00127 
00128     // Battery
00129     TxString("Volts  :\t");
00130     TxVal32(BatteryVolts * 10.0, 1, 'V');
00131     TxString(" Limit > ");
00132     TxVal32( K[LowVoltThres] * 10.0, 1, 'V');
00133 
00134     if ( F.HaveBatterySensor ) {
00135         TxString("\r\nCurrent:\t");
00136         TxVal32(BatteryCurrent * 10.0, 1, 'A');
00137         TxString(" ( ");
00138         TxVal32(BatteryChargeUsedAH * 1000.0, 0, 0 );
00139         TxString(" mAH )\r\n");
00140     } else
00141         TxString("\r\nCurrent:\tnot available - no battery sensor\r\n");
00142 
00143 } // BatteryTest
00144 
00145 void InitBattery() {
00146 
00147     F.HaveBatterySensor = true;
00148     GetBattery();
00149     F.HaveBatterySensor =  BatteryCurrent < 2.0;
00150     if ( F.HaveBatterySensor )
00151         BatteryVoltsScale = 51.8144;       // Volts FS
00152     else
00153         BatteryVoltsScale = BATTERY_VOLTS_SCALE;
00154 
00155 } // InitBattery
00156 
00157 //_____________________________________________________________________
00158 
00159 void GetRangefinderAltitude(void);
00160 void InitRangefinder(void);
00161 
00162 real32 RangefinderAltitude;
00163 
00164 const real32 RangefinderScale = 10.24; // Metres FS
00165 
00166 void GetRangefinderAltitude(void) {
00167 
00168     if ( F.RangefinderAltitudeValid ) {
00169         RangefinderAltitude = ADC(ADCRangefinder) * RangefinderScale;
00170         if ( F.RFInInches )
00171             RangefinderAltitude *= 2.54;
00172 
00173         if (( RangefinderAltitude < ALT_RF_ENABLE_M ) && !F.UsingRangefinderAlt)
00174             F.UsingRangefinderAlt = true;
00175         else
00176             if (( RangefinderAltitude > ALT_RF_DISABLE_M ) && F.UsingRangefinderAlt)
00177                 F.UsingRangefinderAlt = false;
00178     } else {
00179         RangefinderAltitude = 0.0;
00180         F.UsingRangefinderAlt = false;
00181     }
00182 } // GetRangefinderAltitude
00183 
00184 void InitRangefinder(void) {
00185 
00186     F.RangefinderAltitudeValid = true;
00187     GetRangefinderAltitude();
00188     F.RangefinderAltitudeValid = RangefinderAltitude < 1.0; // => supply not RF
00189     GetRangefinderAltitude();
00190 
00191 } // InitRangefinder