Prof Greg Egan
/
UAVXArm-GKE
UAVX Multicopter Flight Controller.
analog.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 | 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 | 2:90292f8bd179 | 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 | 2:90292f8bd179 | 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 | switch (p) { |
gke | 1:1e3318a30ddd | 81 | case ADCPitch: |
gke | 1:1e3318a30ddd | 82 | r = PitchADC.read(); |
gke | 1:1e3318a30ddd | 83 | break; |
gke | 1:1e3318a30ddd | 84 | case ADCRoll: |
gke | 1:1e3318a30ddd | 85 | r = RollADC.read(); |
gke | 1:1e3318a30ddd | 86 | break; |
gke | 1:1e3318a30ddd | 87 | case ADCYaw: |
gke | 1:1e3318a30ddd | 88 | r = YawADC.read(); |
gke | 1:1e3318a30ddd | 89 | break; |
gke | 1:1e3318a30ddd | 90 | case ADCRangefinder: |
gke | 1:1e3318a30ddd | 91 | r = RangefinderADC.read(); |
gke | 1:1e3318a30ddd | 92 | break; |
gke | 1:1e3318a30ddd | 93 | case ADCBatteryCurrent: |
gke | 1:1e3318a30ddd | 94 | r = BatteryCurrentADC.read(); |
gke | 1:1e3318a30ddd | 95 | break; |
gke | 1:1e3318a30ddd | 96 | case ADCBatteryVolts: |
gke | 1:1e3318a30ddd | 97 | r = BatteryVoltsADC.read(); |
gke | 1:1e3318a30ddd | 98 | break; |
gke | 1:1e3318a30ddd | 99 | } |
gke | 1:1e3318a30ddd | 100 | |
gke | 1:1e3318a30ddd | 101 | return ( r ); |
gke | 1:1e3318a30ddd | 102 | |
gke | 1:1e3318a30ddd | 103 | } // ADC |
gke | 1:1e3318a30ddd | 104 | |
gke | 0:62a1c91a859a | 105 | void GetBattery(void) { |
gke | 0:62a1c91a859a | 106 | // AttoPilot Voltage and Current Sense Breakout SparkFun Part: SEN-09028 |
gke | 0:62a1c91a859a | 107 | |
gke | 0:62a1c91a859a | 108 | const real32 BatteryCurrentScale = 90.15296; // Amps FS |
gke | 0:62a1c91a859a | 109 | |
gke | 0:62a1c91a859a | 110 | if ( F.HaveBatterySensor ) { |
gke | 1:1e3318a30ddd | 111 | BatteryCurrent = ADC(ADCBatteryCurrent) * BatteryCurrentScale; |
gke | 0:62a1c91a859a | 112 | BatteryChargeUsedAH += BatteryCurrent * (real32)(mSClock() - mS[LastBattery]) * 2.777777e-7; |
gke | 0:62a1c91a859a | 113 | mS[LastBattery] = mSClock(); |
gke | 0:62a1c91a859a | 114 | } else |
gke | 0:62a1c91a859a | 115 | BatteryCurrent = BatteryChargeUsedAH = 0; |
gke | 0:62a1c91a859a | 116 | |
gke | 1:1e3318a30ddd | 117 | BatteryVolts = ADC(ADCBatteryVolts) * BatteryVoltsScale; |
gke | 0:62a1c91a859a | 118 | F.LowBatt = BatteryVolts < K[LowVoltThres]; |
gke | 0:62a1c91a859a | 119 | |
gke | 0:62a1c91a859a | 120 | } // GetBattery |
gke | 0:62a1c91a859a | 121 | |
gke | 0:62a1c91a859a | 122 | void BatteryTest(void) { |
gke | 0:62a1c91a859a | 123 | |
gke | 0:62a1c91a859a | 124 | TxString("\r\nBattery test\r\n"); |
gke | 0:62a1c91a859a | 125 | |
gke | 0:62a1c91a859a | 126 | GetBattery(); |
gke | 0:62a1c91a859a | 127 | |
gke | 0:62a1c91a859a | 128 | // Battery |
gke | 0:62a1c91a859a | 129 | TxString("Volts :\t"); |
gke | 0:62a1c91a859a | 130 | TxVal32(BatteryVolts * 10.0, 1, 'V'); |
gke | 0:62a1c91a859a | 131 | TxString(" Limit > "); |
gke | 0:62a1c91a859a | 132 | TxVal32( K[LowVoltThres] * 10.0, 1, 'V'); |
gke | 0:62a1c91a859a | 133 | |
gke | 0:62a1c91a859a | 134 | if ( F.HaveBatterySensor ) { |
gke | 0:62a1c91a859a | 135 | TxString("\r\nCurrent:\t"); |
gke | 0:62a1c91a859a | 136 | TxVal32(BatteryCurrent * 10.0, 1, 'A'); |
gke | 0:62a1c91a859a | 137 | TxString(" ( "); |
gke | 0:62a1c91a859a | 138 | TxVal32(BatteryChargeUsedAH * 1000.0, 0, 0 ); |
gke | 0:62a1c91a859a | 139 | TxString(" mAH )\r\n"); |
gke | 0:62a1c91a859a | 140 | } else |
gke | 0:62a1c91a859a | 141 | TxString("\r\nCurrent:\tnot available - no battery sensor\r\n"); |
gke | 0:62a1c91a859a | 142 | |
gke | 0:62a1c91a859a | 143 | } // BatteryTest |
gke | 0:62a1c91a859a | 144 | |
gke | 0:62a1c91a859a | 145 | void InitBattery() { |
gke | 0:62a1c91a859a | 146 | |
gke | 0:62a1c91a859a | 147 | F.HaveBatterySensor = true; |
gke | 0:62a1c91a859a | 148 | GetBattery(); |
gke | 0:62a1c91a859a | 149 | F.HaveBatterySensor = BatteryCurrent < 2.0; |
gke | 0:62a1c91a859a | 150 | if ( F.HaveBatterySensor ) |
gke | 0:62a1c91a859a | 151 | BatteryVoltsScale = 51.8144; // Volts FS |
gke | 0:62a1c91a859a | 152 | else |
gke | 0:62a1c91a859a | 153 | BatteryVoltsScale = BATTERY_VOLTS_SCALE; |
gke | 0:62a1c91a859a | 154 | |
gke | 0:62a1c91a859a | 155 | } // InitBattery |
gke | 0:62a1c91a859a | 156 | |
gke | 0:62a1c91a859a | 157 | //_____________________________________________________________________ |
gke | 0:62a1c91a859a | 158 | |
gke | 0:62a1c91a859a | 159 | void GetRangefinderAltitude(void); |
gke | 0:62a1c91a859a | 160 | void InitRangefinder(void); |
gke | 0:62a1c91a859a | 161 | |
gke | 0:62a1c91a859a | 162 | real32 RangefinderAltitude; |
gke | 0:62a1c91a859a | 163 | |
gke | 0:62a1c91a859a | 164 | const real32 RangefinderScale = 10.24; // Metres FS |
gke | 0:62a1c91a859a | 165 | |
gke | 0:62a1c91a859a | 166 | void GetRangefinderAltitude(void) { |
gke | 0:62a1c91a859a | 167 | |
gke | 0:62a1c91a859a | 168 | if ( F.RangefinderAltitudeValid ) { |
gke | 1:1e3318a30ddd | 169 | RangefinderAltitude = ADC(ADCRangefinder) * RangefinderScale; |
gke | 0:62a1c91a859a | 170 | if ( F.RFInInches ) |
gke | 0:62a1c91a859a | 171 | RangefinderAltitude *= 2.54; |
gke | 0:62a1c91a859a | 172 | |
gke | 0:62a1c91a859a | 173 | if (( RangefinderAltitude < ALT_RF_ENABLE_M ) && !F.UsingRangefinderAlt) |
gke | 0:62a1c91a859a | 174 | F.UsingRangefinderAlt = true; |
gke | 0:62a1c91a859a | 175 | else |
gke | 0:62a1c91a859a | 176 | if (( RangefinderAltitude > ALT_RF_DISABLE_M ) && F.UsingRangefinderAlt) |
gke | 0:62a1c91a859a | 177 | F.UsingRangefinderAlt = false; |
gke | 0:62a1c91a859a | 178 | } else { |
gke | 0:62a1c91a859a | 179 | RangefinderAltitude = 0.0; |
gke | 0:62a1c91a859a | 180 | F.UsingRangefinderAlt = false; |
gke | 0:62a1c91a859a | 181 | } |
gke | 0:62a1c91a859a | 182 | } // GetRangefinderAltitude |
gke | 0:62a1c91a859a | 183 | |
gke | 0:62a1c91a859a | 184 | void InitRangefinder(void) { |
gke | 0:62a1c91a859a | 185 | |
gke | 0:62a1c91a859a | 186 | F.RangefinderAltitudeValid = true; |
gke | 0:62a1c91a859a | 187 | GetRangefinderAltitude(); |
gke | 0:62a1c91a859a | 188 | F.RangefinderAltitudeValid = RangefinderAltitude < 1.0; // => supply not RF |
gke | 0:62a1c91a859a | 189 | GetRangefinderAltitude(); |
gke | 0:62a1c91a859a | 190 | |
gke | 0:62a1c91a859a | 191 | } // InitRangefinder |