Prof Greg Egan / Mbed 2 deprecated UAVXArm-GKE

UAVX Multicopter Flight Controller.

Dependencies:   mbed

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?

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 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