Prof Greg Egan
/
UAVXArm-GKE
UAVX Multicopter Flight Controller.
analog.c@1:1e3318a30ddd, 2011-02-25 (annotated)
- Committer:
- gke
- Date:
- Fri Feb 25 01:35:24 2011 +0000
- Revision:
- 1:1e3318a30ddd
- Parent:
- 0:62a1c91a859a
- Child:
- 2:90292f8bd179
This version has broken I2C - posted for debugging involvement of Simon et al.
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 | 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 | 1:1e3318a30ddd | 23 | real32 ADC(uint8); |
gke | 0:62a1c91a859a | 24 | void GetBattery(void); |
gke | 0:62a1c91a859a | 25 | void BatteryTest(void); |
gke | 0:62a1c91a859a | 26 | void InitBattery(void); |
gke | 0:62a1c91a859a | 27 | |
gke | 0:62a1c91a859a | 28 | real32 BatteryVolts, BatteryCurrentADCEstimated, BatteryChargeUsedAH; |
gke | 0:62a1c91a859a | 29 | real32 BatteryCharge, BatteryCurrent; |
gke | 0:62a1c91a859a | 30 | real32 BatteryVoltsScale; |
gke | 0:62a1c91a859a | 31 | |
gke | 1:1e3318a30ddd | 32 | void DirectAnalog(void) { |
gke | 1:1e3318a30ddd | 33 | // Framework Simon Ford |
gke | 1:1e3318a30ddd | 34 | |
gke | 1:1e3318a30ddd | 35 | static uint32 data; |
gke | 1:1e3318a30ddd | 36 | |
gke | 1:1e3318a30ddd | 37 | // Select channel and start conversion |
gke | 1:1e3318a30ddd | 38 | LPC_ADC->ADCR &= ~0xFF; |
gke | 1:1e3318a30ddd | 39 | LPC_ADC->ADCR |= 1 << 5; // ADC0[5] |
gke | 1:1e3318a30ddd | 40 | LPC_ADC->ADCR |= 1 << 24; |
gke | 1:1e3318a30ddd | 41 | |
gke | 1:1e3318a30ddd | 42 | // Repeatedly get the sample data until DONE bit |
gke | 1:1e3318a30ddd | 43 | do { |
gke | 1:1e3318a30ddd | 44 | data = LPC_ADC->ADGDR; |
gke | 1:1e3318a30ddd | 45 | } while ((data & ((uint32)1 << 31)) == 0); |
gke | 1:1e3318a30ddd | 46 | |
gke | 1:1e3318a30ddd | 47 | // Stop conversion |
gke | 1:1e3318a30ddd | 48 | LPC_ADC->ADCR &= ~(1 << 24); |
gke | 1:1e3318a30ddd | 49 | |
gke | 1:1e3318a30ddd | 50 | } // DirectAnalog |
gke | 1:1e3318a30ddd | 51 | |
gke | 1:1e3318a30ddd | 52 | void InitDirectAnalog(void) { |
gke | 1:1e3318a30ddd | 53 | |
gke | 1:1e3318a30ddd | 54 | // power on, clk divider /4 |
gke | 1:1e3318a30ddd | 55 | LPC_SC->PCONP |= (1 << 12); |
gke | 1:1e3318a30ddd | 56 | LPC_SC->PCLKSEL0 &= ~(0x3 << 24); |
gke | 1:1e3318a30ddd | 57 | |
gke | 1:1e3318a30ddd | 58 | // software-controlled ADC settings |
gke | 1:1e3318a30ddd | 59 | LPC_ADC->ADCR = (0 << 0) // SEL: 0 = no channels selected |
gke | 1:1e3318a30ddd | 60 | | (25 << 8) // CLKDIV: PCLK max ~= 25MHz, /25 to give safe 1MHz |
gke | 1:1e3318a30ddd | 61 | | (0 << 16) // BURST: 0 = software control |
gke | 1:1e3318a30ddd | 62 | | (0 << 17) // CLKS: not applicable |
gke | 1:1e3318a30ddd | 63 | | (1 << 21) // PDN: 1 = operational |
gke | 1:1e3318a30ddd | 64 | | (0 << 24) // START: 0 = no start |
gke | 1:1e3318a30ddd | 65 | | (0 << 27); // EDGE: not applicable |
gke | 1:1e3318a30ddd | 66 | |
gke | 1:1e3318a30ddd | 67 | // setup P1_31 as sel 3 (ADC), mode 2 (no pull) |
gke | 1:1e3318a30ddd | 68 | LPC_PINCON->PINSEL3 &= ~((uint32)0x3 << 30); |
gke | 1:1e3318a30ddd | 69 | LPC_PINCON->PINSEL3 |= (uint32)0x3 << 30; |
gke | 1:1e3318a30ddd | 70 | |
gke | 1:1e3318a30ddd | 71 | LPC_PINCON->PINMODE3 &= ~((uint32)0x3 << 30); |
gke | 1:1e3318a30ddd | 72 | LPC_PINCON->PINMODE3 |= (uint32)0x2 << 30; |
gke | 1:1e3318a30ddd | 73 | |
gke | 1:1e3318a30ddd | 74 | } // InitDirectAnalog |
gke | 1:1e3318a30ddd | 75 | |
gke | 1:1e3318a30ddd | 76 | real32 ADC(uint8 p) { |
gke | 1:1e3318a30ddd | 77 | |
gke | 1:1e3318a30ddd | 78 | static real32 r; |
gke | 1:1e3318a30ddd | 79 | |
gke | 1:1e3318a30ddd | 80 | DebugPin = 1; |
gke | 1:1e3318a30ddd | 81 | switch (p) { |
gke | 1:1e3318a30ddd | 82 | case ADCPitch: |
gke | 1:1e3318a30ddd | 83 | r = PitchADC.read(); |
gke | 1:1e3318a30ddd | 84 | break; |
gke | 1:1e3318a30ddd | 85 | case ADCRoll: |
gke | 1:1e3318a30ddd | 86 | r = RollADC.read(); |
gke | 1:1e3318a30ddd | 87 | break; |
gke | 1:1e3318a30ddd | 88 | case ADCYaw: |
gke | 1:1e3318a30ddd | 89 | r = YawADC.read(); |
gke | 1:1e3318a30ddd | 90 | break; |
gke | 1:1e3318a30ddd | 91 | case ADCRangefinder: |
gke | 1:1e3318a30ddd | 92 | r = RangefinderADC.read(); |
gke | 1:1e3318a30ddd | 93 | break; |
gke | 1:1e3318a30ddd | 94 | case ADCBatteryCurrent: |
gke | 1:1e3318a30ddd | 95 | r = BatteryCurrentADC.read(); |
gke | 1:1e3318a30ddd | 96 | break; |
gke | 1:1e3318a30ddd | 97 | case ADCBatteryVolts: |
gke | 1:1e3318a30ddd | 98 | r = BatteryVoltsADC.read(); |
gke | 1:1e3318a30ddd | 99 | break; |
gke | 1:1e3318a30ddd | 100 | } |
gke | 1:1e3318a30ddd | 101 | |
gke | 1:1e3318a30ddd | 102 | DebugPin = 0; |
gke | 1:1e3318a30ddd | 103 | |
gke | 1:1e3318a30ddd | 104 | return ( r ); |
gke | 1:1e3318a30ddd | 105 | |
gke | 1:1e3318a30ddd | 106 | } // ADC |
gke | 1:1e3318a30ddd | 107 | |
gke | 0:62a1c91a859a | 108 | void GetBattery(void) { |
gke | 0:62a1c91a859a | 109 | // AttoPilot Voltage and Current Sense Breakout SparkFun Part: SEN-09028 |
gke | 0:62a1c91a859a | 110 | |
gke | 0:62a1c91a859a | 111 | const real32 BatteryCurrentScale = 90.15296; // Amps FS |
gke | 0:62a1c91a859a | 112 | |
gke | 0:62a1c91a859a | 113 | if ( F.HaveBatterySensor ) { |
gke | 1:1e3318a30ddd | 114 | BatteryCurrent = ADC(ADCBatteryCurrent) * BatteryCurrentScale; |
gke | 0:62a1c91a859a | 115 | BatteryChargeUsedAH += BatteryCurrent * (real32)(mSClock() - mS[LastBattery]) * 2.777777e-7; |
gke | 0:62a1c91a859a | 116 | mS[LastBattery] = mSClock(); |
gke | 0:62a1c91a859a | 117 | } else |
gke | 0:62a1c91a859a | 118 | BatteryCurrent = BatteryChargeUsedAH = 0; |
gke | 0:62a1c91a859a | 119 | |
gke | 1:1e3318a30ddd | 120 | BatteryVolts = ADC(ADCBatteryVolts) * BatteryVoltsScale; |
gke | 0:62a1c91a859a | 121 | F.LowBatt = BatteryVolts < K[LowVoltThres]; |
gke | 0:62a1c91a859a | 122 | |
gke | 0:62a1c91a859a | 123 | } // GetBattery |
gke | 0:62a1c91a859a | 124 | |
gke | 0:62a1c91a859a | 125 | void BatteryTest(void) { |
gke | 0:62a1c91a859a | 126 | |
gke | 0:62a1c91a859a | 127 | TxString("\r\nBattery test\r\n"); |
gke | 0:62a1c91a859a | 128 | |
gke | 0:62a1c91a859a | 129 | GetBattery(); |
gke | 0:62a1c91a859a | 130 | |
gke | 0:62a1c91a859a | 131 | // Battery |
gke | 0:62a1c91a859a | 132 | TxString("Volts :\t"); |
gke | 0:62a1c91a859a | 133 | TxVal32(BatteryVolts * 10.0, 1, 'V'); |
gke | 0:62a1c91a859a | 134 | TxString(" Limit > "); |
gke | 0:62a1c91a859a | 135 | TxVal32( K[LowVoltThres] * 10.0, 1, 'V'); |
gke | 0:62a1c91a859a | 136 | |
gke | 0:62a1c91a859a | 137 | if ( F.HaveBatterySensor ) { |
gke | 0:62a1c91a859a | 138 | TxString("\r\nCurrent:\t"); |
gke | 0:62a1c91a859a | 139 | TxVal32(BatteryCurrent * 10.0, 1, 'A'); |
gke | 0:62a1c91a859a | 140 | TxString(" ( "); |
gke | 0:62a1c91a859a | 141 | TxVal32(BatteryChargeUsedAH * 1000.0, 0, 0 ); |
gke | 0:62a1c91a859a | 142 | TxString(" mAH )\r\n"); |
gke | 0:62a1c91a859a | 143 | } else |
gke | 0:62a1c91a859a | 144 | TxString("\r\nCurrent:\tnot available - no battery sensor\r\n"); |
gke | 0:62a1c91a859a | 145 | |
gke | 0:62a1c91a859a | 146 | } // BatteryTest |
gke | 0:62a1c91a859a | 147 | |
gke | 0:62a1c91a859a | 148 | void InitBattery() { |
gke | 0:62a1c91a859a | 149 | |
gke | 0:62a1c91a859a | 150 | F.HaveBatterySensor = true; |
gke | 0:62a1c91a859a | 151 | GetBattery(); |
gke | 0:62a1c91a859a | 152 | F.HaveBatterySensor = BatteryCurrent < 2.0; |
gke | 0:62a1c91a859a | 153 | if ( F.HaveBatterySensor ) |
gke | 0:62a1c91a859a | 154 | BatteryVoltsScale = 51.8144; // Volts FS |
gke | 0:62a1c91a859a | 155 | else |
gke | 0:62a1c91a859a | 156 | BatteryVoltsScale = BATTERY_VOLTS_SCALE; |
gke | 0:62a1c91a859a | 157 | |
gke | 0:62a1c91a859a | 158 | } // InitBattery |
gke | 0:62a1c91a859a | 159 | |
gke | 0:62a1c91a859a | 160 | //_____________________________________________________________________ |
gke | 0:62a1c91a859a | 161 | |
gke | 0:62a1c91a859a | 162 | void GetRangefinderAltitude(void); |
gke | 0:62a1c91a859a | 163 | void InitRangefinder(void); |
gke | 0:62a1c91a859a | 164 | |
gke | 0:62a1c91a859a | 165 | real32 RangefinderAltitude; |
gke | 0:62a1c91a859a | 166 | |
gke | 0:62a1c91a859a | 167 | const real32 RangefinderScale = 10.24; // Metres FS |
gke | 0:62a1c91a859a | 168 | |
gke | 0:62a1c91a859a | 169 | void GetRangefinderAltitude(void) { |
gke | 0:62a1c91a859a | 170 | |
gke | 0:62a1c91a859a | 171 | if ( F.RangefinderAltitudeValid ) { |
gke | 1:1e3318a30ddd | 172 | RangefinderAltitude = ADC(ADCRangefinder) * RangefinderScale; |
gke | 0:62a1c91a859a | 173 | if ( F.RFInInches ) |
gke | 0:62a1c91a859a | 174 | RangefinderAltitude *= 2.54; |
gke | 0:62a1c91a859a | 175 | |
gke | 0:62a1c91a859a | 176 | if (( RangefinderAltitude < ALT_RF_ENABLE_M ) && !F.UsingRangefinderAlt) |
gke | 0:62a1c91a859a | 177 | F.UsingRangefinderAlt = true; |
gke | 0:62a1c91a859a | 178 | else |
gke | 0:62a1c91a859a | 179 | if (( RangefinderAltitude > ALT_RF_DISABLE_M ) && F.UsingRangefinderAlt) |
gke | 0:62a1c91a859a | 180 | F.UsingRangefinderAlt = false; |
gke | 0:62a1c91a859a | 181 | } else { |
gke | 0:62a1c91a859a | 182 | RangefinderAltitude = 0.0; |
gke | 0:62a1c91a859a | 183 | F.UsingRangefinderAlt = false; |
gke | 0:62a1c91a859a | 184 | } |
gke | 0:62a1c91a859a | 185 | } // GetRangefinderAltitude |
gke | 0:62a1c91a859a | 186 | |
gke | 0:62a1c91a859a | 187 | void InitRangefinder(void) { |
gke | 0:62a1c91a859a | 188 | |
gke | 0:62a1c91a859a | 189 | F.RangefinderAltitudeValid = true; |
gke | 0:62a1c91a859a | 190 | GetRangefinderAltitude(); |
gke | 0:62a1c91a859a | 191 | F.RangefinderAltitudeValid = RangefinderAltitude < 1.0; // => supply not RF |
gke | 0:62a1c91a859a | 192 | GetRangefinderAltitude(); |
gke | 0:62a1c91a859a | 193 | |
gke | 0:62a1c91a859a | 194 | } // InitRangefinder |