Statistics
| Branch: | Revision:

root / quad1 / AeroQuad / Altitude.h @ 9240aaa3

History | View | Annotate | Download (7.68 KB)

1
/*
2
  AeroQuad v2.1 - October 2010
3
  www.AeroQuad.com
4
  Copyright (c) 2010 Ted Carancho.  All rights reserved.
5
  An Open Source Arduino based multicopter.
6
 
7
  This program is free software: you can redistribute it and/or modify 
8
  it under the terms of the GNU General Public License as published by 
9
  the Free Software Foundation, either version 3 of the License, or 
10
  (at your option) any later version. 
11

12
  This program is distributed in the hope that it will be useful, 
13
  but WITHOUT ANY WARRANTY; without even the implied warranty of 
14
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 
15
  GNU General Public License for more details. 
16

17
  You should have received a copy of the GNU General Public License 
18
  along with this program. If not, see <http://www.gnu.org/licenses/>. 
19
*/
20

    
21
// Class to define sensors that can determine altitude
22

    
23
// ***********************************************************************
24
// ************************** Altitude Class *****************************
25
// ***********************************************************************
26

    
27
class Altitude {
28
public:
29
  double altitude, rawAltitude;
30
  float groundTemperature; // remove later
31
  float groundPressure; // remove later
32
  float groundAltitude;
33
  float smoothFactor;
34
  
35
  Altitude (void) { 
36
    altitude = 0;
37
    smoothFactor = 0.1;
38
  }
39

    
40
  // **********************************************************************
41
  // The following function calls must be defined inside any new subclasses
42
  // **********************************************************************
43
  virtual void initialize(void); 
44
  virtual void measure(void);
45
  
46
  // *********************************************************
47
  // The following functions are common between all subclasses
48
  // *********************************************************
49
  const float getData(void) {
50
    return altitude - groundAltitude;
51
  }
52
  
53
  const float getRawData(void) {
54
    return rawAltitude;
55
  }
56
  
57
  void setStartAltitude(float value) {
58
    altitude = value;
59
  }
60
  
61
  void measureGround(void) {
62
    // measure initial ground pressure (multiple samples)
63
    groundAltitude = 0;
64
    for (int i=0; i < 25; i++) {
65
      measure();
66
      delay(26);
67
      groundAltitude += rawAltitude;
68
    }
69
    groundAltitude = groundAltitude / 25.0;
70
  }
71
  
72
  void setGroundAltitude(float value) {
73
    groundAltitude = value;
74
  }
75
  
76
  const float getGroundAltitude(void) {
77
    return groundAltitude;
78
  }
79
  
80
  void setSmoothFactor(float value) {
81
    smoothFactor = value;
82
  }
83
  
84
  const float getSmoothFactor(void) {
85
    return smoothFactor;
86
  }
87
};
88

    
89
// ***********************************************************************
90
// ************************* BMP085 Subclass *****************************
91
// ***********************************************************************
92
class Altitude_AeroQuad_v2 : public Altitude {
93
// This sets up the BMP085 from Sparkfun
94
// Code from http://wiring.org.co/learning/libraries/bmp085.html
95
// Also made bug fixes based on BMP085 library from Jordi Munoz and Jose Julio
96
private:
97
  byte overSamplingSetting;
98
  int ac1, ac2, ac3;
99
  unsigned int ac4, ac5, ac6;
100
  int b1, b2, mb, mc, md;
101
  long pressure;
102
  long temperature;
103
  int altitudeAddress;
104
  long rawPressure, rawTemperature;
105
  byte select, pressureCount;
106
  float pressureFactor;
107
  
108
  void requestRawPressure(void) {
109
    updateRegisterI2C(altitudeAddress, 0xF4, 0x34+(overSamplingSetting<<6));
110
  }
111
  
112
  long readRawPressure(void) {
113
    unsigned char msb, lsb, xlsb;
114
    sendByteI2C(altitudeAddress, 0xF6);
115
    Wire.requestFrom(altitudeAddress, 3); // request three bytes
116
    while(!Wire.available()); // wait until data available
117
    msb = Wire.receive();
118
    while(!Wire.available()); // wait until data available
119
    lsb |= Wire.receive();
120
    while(!Wire.available()); // wait until data available
121
    xlsb |= Wire.receive();
122
    return (((long)msb<<16) | ((long)lsb<<8) | ((long)xlsb)) >>(8-overSamplingSetting);
123
  }
124

    
125
  void requestRawTemperature(void) {
126
    updateRegisterI2C(altitudeAddress, 0xF4, 0x2E);
127
  }
128
  
129
  unsigned int readRawTemperature(void) {
130
    sendByteI2C(altitudeAddress, 0xF6);
131
    return readWordWaitI2C(altitudeAddress);
132
  }
133

    
134
public: 
135
  Altitude_AeroQuad_v2() : Altitude(){
136
    altitudeAddress = 0x77;
137
    // oversampling setting
138
    // 0 = ultra low power
139
    // 1 = standard
140
    // 2 = high
141
    // 3 = ultra high resolution
142
    overSamplingSetting = 3;
143
    pressure = 0;
144
    groundPressure = 0;
145
    temperature = 0;
146
    groundTemperature = 0;
147
    groundAltitude = 0;
148
    pressureFactor = 1/5.255;
149
  }
150

    
151
  // ***********************************************************
152
  // Define all the virtual functions declared in the main class
153
  // ***********************************************************
154
  void initialize(void) {
155
    float verifyGroundAltitude;
156
    
157
    sendByteI2C(altitudeAddress, 0xAA);
158
    ac1 = readWordWaitI2C(altitudeAddress);
159
    sendByteI2C(altitudeAddress, 0xAC);
160
    ac2 = readWordWaitI2C(altitudeAddress);
161
    sendByteI2C(altitudeAddress, 0xAE);
162
    ac3 = readWordWaitI2C(altitudeAddress);
163
    sendByteI2C(altitudeAddress, 0xB0);
164
    ac4 = readWordWaitI2C(altitudeAddress);
165
    sendByteI2C(altitudeAddress, 0xB2);
166
    ac5 = readWordWaitI2C(altitudeAddress);
167
    sendByteI2C(altitudeAddress, 0xB4);
168
    ac6 = readWordWaitI2C(altitudeAddress);
169
    sendByteI2C(altitudeAddress, 0xB6);
170
    b1 = readWordWaitI2C(altitudeAddress);
171
    sendByteI2C(altitudeAddress, 0xB8);
172
    b2 = readWordWaitI2C(altitudeAddress);
173
    sendByteI2C(altitudeAddress, 0xBA);
174
    mb = readWordWaitI2C(altitudeAddress);
175
    sendByteI2C(altitudeAddress, 0xBC);
176
    mc = readWordWaitI2C(altitudeAddress);
177
    sendByteI2C(altitudeAddress, 0xBE);
178
    md = readWordWaitI2C(altitudeAddress);
179
    requestRawTemperature(); // setup up next measure() for temperature
180
    select = TEMPERATURE;
181
    pressureCount = 0;
182
    measure();
183
    delay(5); // delay for temperature
184
    measure();
185
    delay(26); // delay for pressure
186
    measureGround();
187
    // check if measured ground altitude is valid
188
    while (abs(getRawData() - getGroundAltitude()) > 10) {
189
      delay(26);
190
      measureGround();
191
    }
192
    setStartAltitude(getGroundAltitude());
193
  }
194
  
195
  void measure(void) {
196
    long x1, x2, x3, b3, b5, b6, p;
197
    unsigned long b4, b7;
198
    int32_t tmp;
199

    
200
    // switch between pressure and tempature measurements
201
    // each loop, since it's slow to measure pressure
202
    if (select == PRESSURE) {
203
      rawPressure = readRawPressure();
204
      if (pressureCount == 3) {
205
        requestRawTemperature();
206
        pressureCount = 0;
207
       select = TEMPERATURE;
208
      }
209
      else
210
        requestRawPressure();
211
      pressureCount++;
212
    }
213
    else { // select must equal TEMPERATURE
214
      rawTemperature = (long)readRawTemperature();
215
      requestRawPressure();
216
      select = PRESSURE;
217
    }
218
    
219
    //calculate true temperature
220
    x1 = ((long)rawTemperature - ac6) * ac5 >> 15;
221
    x2 = ((long) mc << 11) / (x1 + md);
222
    b5 = x1 + x2;
223
    temperature = ((b5 + 8) >> 4);
224
  
225
    //calculate true pressure
226
    b6 = b5 - 4000;
227
    x1 = (b2 * (b6 * b6 >> 12)) >> 11; 
228
    x2 = ac2 * b6 >> 11;
229
    x3 = x1 + x2;
230
    tmp = ac1;
231
    tmp = (tmp*4 + x3)<<overSamplingSetting;
232
    b3 = (tmp+2)/4;
233
    x1 = ac3 * b6 >> 13;
234
    x2 = (b1 * (b6 * b6 >> 12)) >> 16;
235
    x3 = ((x1 + x2) + 2) >> 2;
236
    b4 = (ac4 * (uint32_t) (x3 + 32768)) >> 15;
237
    b7 = ((uint32_t) rawPressure - b3) * (50000 >> overSamplingSetting);
238
    p = b7 < 0x80000000 ? (b7 * 2) / b4 : (b7 / b4) * 2;
239
    
240
    x1 = (p >> 8) * (p >> 8);
241
    x1 = (x1 * 3038) >> 16;
242
    x2 = (-7357 * p) >> 16;
243
    pressure = (p + ((x1 + x2 + 3791) >> 4));
244
    
245
    rawAltitude = 44330 * (1 - pow(pressure/101325.0, pressureFactor)); // returns absolute altitude in meters
246
    altitude = smooth(rawAltitude, altitude, 0.1);//smoothFactor); // smoothFactor defined in main class
247
  }
248
};