Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 : #include "AP_Baro_BMP085.h"
16 :
17 : #if AP_BARO_BMP085_ENABLED
18 :
19 : #include <stdio.h>
20 :
21 : #include <AP_Common/AP_Common.h>
22 : #include <AP_HAL/AP_HAL.h>
23 :
24 : extern const AP_HAL::HAL &hal;
25 :
26 : #define BMP085_OVERSAMPLING_ULTRALOWPOWER 0
27 : #define BMP085_OVERSAMPLING_STANDARD 1
28 : #define BMP085_OVERSAMPLING_HIGHRES 2
29 : #define BMP085_OVERSAMPLING_ULTRAHIGHRES 3
30 :
31 : #ifndef BMP085_EOC
32 : #define BMP085_EOC -1
33 : #define OVERSAMPLING BMP085_OVERSAMPLING_ULTRAHIGHRES
34 : #else
35 : #define OVERSAMPLING BMP085_OVERSAMPLING_HIGHRES
36 : #endif
37 :
38 0 : AP_Baro_BMP085::AP_Baro_BMP085(AP_Baro &baro, AP_HAL::Device &dev)
39 : : AP_Baro_Backend(baro)
40 0 : , _dev(&dev)
41 0 : { }
42 :
43 0 : AP_Baro_Backend * AP_Baro_BMP085::probe(AP_Baro &baro, AP_HAL::Device &dev)
44 : {
45 0 : AP_Baro_BMP085 *sensor = NEW_NOTHROW AP_Baro_BMP085(baro, dev);
46 0 : if (!sensor || !sensor->_init()) {
47 0 : delete sensor;
48 0 : return nullptr;
49 : }
50 0 : return sensor;
51 :
52 : }
53 :
54 0 : bool AP_Baro_BMP085::_init()
55 : {
56 0 : if (!_dev) {
57 0 : return false;
58 : }
59 : union {
60 : uint8_t buff[22];
61 : uint16_t wb[11];
62 : } bb;
63 :
64 : // get pointer to i2c bus semaphore
65 0 : AP_HAL::Semaphore *sem = _dev->get_semaphore();
66 :
67 : // take i2c bus semaphore
68 0 : WITH_SEMAPHORE(sem);
69 :
70 : if (BMP085_EOC >= 0) {
71 : _eoc = hal.gpio->channel(BMP085_EOC);
72 : _eoc->mode(HAL_GPIO_INPUT);
73 : }
74 :
75 :
76 : uint8_t id;
77 :
78 0 : if (!_dev->read_registers(0xD0, &id, 1)) {
79 0 : return false;
80 : }
81 :
82 0 : if (id!=0x55) {
83 0 : return false; // not BMP180
84 : }
85 :
86 :
87 0 : _dev->read_registers(0xD1, &_vers, 1);
88 :
89 0 : bool prom_ok=false;
90 0 : _type=0;
91 :
92 : // We read the calibration data registers
93 0 : if (_dev->read_registers(0xAA, bb.buff, sizeof(bb.buff))) {
94 0 : prom_ok=true;
95 :
96 : }
97 :
98 0 : if (!prom_ok) {
99 0 : if (_read_prom((uint16_t *)&bb.wb[0])) { // BMP180 requires reads by 2 bytes
100 0 : prom_ok=true;
101 0 : _type=1;
102 : }
103 : }
104 0 : if (!prom_ok) {
105 0 : return false;
106 : }
107 :
108 0 : ac1 = ((int16_t)bb.buff[0] << 8) | bb.buff[1];
109 0 : ac2 = ((int16_t)bb.buff[2] << 8) | bb.buff[3];
110 0 : ac3 = ((int16_t)bb.buff[4] << 8) | bb.buff[5];
111 0 : ac4 = ((int16_t)bb.buff[6] << 8) | bb.buff[7];
112 0 : ac5 = ((int16_t)bb.buff[8] << 8) | bb.buff[9];
113 0 : ac6 = ((int16_t)bb.buff[10]<< 8) | bb.buff[11];
114 0 : b1 = ((int16_t)bb.buff[12] << 8) | bb.buff[13];
115 0 : b2 = ((int16_t)bb.buff[14] << 8) | bb.buff[15];
116 0 : mb = ((int16_t)bb.buff[16] << 8) | bb.buff[17];
117 0 : mc = ((int16_t)bb.buff[18] << 8) | bb.buff[19];
118 0 : md = ((int16_t)bb.buff[20] << 8) | bb.buff[21];
119 :
120 0 : if ((ac1==0 || ac1==-1) ||
121 0 : (ac2==0 || ac2==-1) ||
122 0 : (ac3==0 || ac3==-1) ||
123 0 : (ac4==0 || ac4==0xFFFF) ||
124 0 : (ac5==0 || ac5==0xFFFF) ||
125 0 : (ac6==0 || ac6==0xFFFF)) {
126 0 : return false;
127 : }
128 :
129 0 : _last_press_read_command_time = 0;
130 0 : _last_temp_read_command_time = 0;
131 :
132 : // Send a command to read temperature
133 0 : _cmd_read_temp();
134 :
135 0 : _state = 0;
136 :
137 0 : _instance = _frontend.register_sensor();
138 :
139 0 : _dev->set_device_type(DEVTYPE_BARO_BMP085);
140 0 : set_bus_id(_instance, _dev->get_bus_id());
141 :
142 0 : _dev->register_periodic_callback(20000, FUNCTOR_BIND_MEMBER(&AP_Baro_BMP085::_timer, void));
143 0 : return true;
144 0 : }
145 :
146 0 : uint16_t AP_Baro_BMP085::_read_prom_word(uint8_t word)
147 : {
148 0 : const uint8_t reg = 0xAA + (word << 1);
149 : uint8_t val[2];
150 0 : if (!_dev->transfer(®, 1, val, sizeof(val))) {
151 0 : return 0;
152 : }
153 0 : return (val[0] << 8) | val[1];
154 : }
155 :
156 0 : bool AP_Baro_BMP085::_read_prom(uint16_t *prom)
157 : {
158 0 : bool all_zero = true;
159 0 : for (uint8_t i = 0; i < 11; i++) {
160 0 : prom[i] = _read_prom_word(i);
161 0 : if (prom[i] != 0) {
162 0 : all_zero = false;
163 : }
164 : }
165 :
166 0 : if (all_zero) {
167 0 : return false;
168 : }
169 :
170 0 : return true;
171 : }
172 :
173 : /*
174 : This is a state machine. Accumulate a new sensor reading.
175 : */
176 0 : void AP_Baro_BMP085::_timer(void)
177 : {
178 0 : if (!_data_ready()) {
179 0 : return;
180 : }
181 :
182 0 : if (_state == 0) {
183 0 : _read_temp();
184 0 : } else if (_read_pressure()) {
185 0 : _calculate();
186 : }
187 :
188 0 : _state++;
189 0 : if (_state == 25) {
190 0 : _state = 0;
191 0 : _cmd_read_temp();
192 : } else {
193 0 : _cmd_read_pressure();
194 : }
195 : }
196 :
197 : /*
198 : transfer data to the frontend
199 : */
200 0 : void AP_Baro_BMP085::update(void)
201 : {
202 0 : WITH_SEMAPHORE(_sem);
203 :
204 0 : if (!_has_sample) {
205 0 : return;
206 : }
207 :
208 0 : float temperature = 0.1f * _temp;
209 0 : float pressure = _pressure_filter.getf();
210 :
211 0 : _copy_to_frontend(_instance, pressure, temperature);
212 0 : }
213 :
214 : // Send command to Read Pressure
215 0 : void AP_Baro_BMP085::_cmd_read_pressure()
216 : {
217 0 : _dev->write_register(0xF4, 0x34 + (OVERSAMPLING << 6));
218 0 : _last_press_read_command_time = AP_HAL::millis();
219 0 : }
220 :
221 : // Read raw pressure values
222 0 : bool AP_Baro_BMP085::_read_pressure()
223 : {
224 : uint8_t buf[3];
225 0 : if (_dev->read_registers(0xF6, buf, sizeof(buf))) {
226 0 : _raw_pressure = (((uint32_t)buf[0] << 16)
227 0 : | ((uint32_t)buf[1] << 8)
228 0 : | ((uint32_t)buf[2])) >> (8 - OVERSAMPLING);
229 0 : return true;
230 : }
231 :
232 : uint8_t xlsb;
233 0 : if (_dev->read_registers(0xF6, buf, 2) && _dev->read_registers(0xF8, &xlsb, 1)) {
234 0 : _raw_pressure = (((uint32_t)buf[0] << 16)
235 0 : | ((uint32_t)buf[1] << 8)
236 0 : | ((uint32_t)xlsb)) >> (8 - OVERSAMPLING);
237 0 : return true;
238 : }
239 :
240 0 : _last_press_read_command_time = AP_HAL::millis() + 1000;
241 0 : _dev->set_speed(AP_HAL::Device::SPEED_LOW);
242 0 : return false;
243 : }
244 :
245 : // Send Command to Read Temperature
246 0 : void AP_Baro_BMP085::_cmd_read_temp()
247 : {
248 0 : _dev->write_register(0xF4, 0x2E);
249 0 : _last_temp_read_command_time = AP_HAL::millis();
250 0 : }
251 :
252 : // Read raw temperature values
253 0 : void AP_Baro_BMP085::_read_temp()
254 : {
255 : uint8_t buf[2];
256 : int32_t _temp_sensor;
257 :
258 0 : if (!_dev->read_registers(0xF6, buf, sizeof(buf))) {
259 0 : _dev->set_speed(AP_HAL::Device::SPEED_LOW);
260 0 : return;
261 : }
262 0 : _temp_sensor = buf[0];
263 0 : _temp_sensor = (_temp_sensor << 8) | buf[1];
264 :
265 0 : _raw_temp = _temp_sensor;
266 : }
267 :
268 : // _calculate Temperature and Pressure in real units.
269 0 : void AP_Baro_BMP085::_calculate()
270 : {
271 : int32_t x1, x2, x3, b3, b5, b6, p;
272 : uint32_t b4, b7;
273 : int32_t tmp;
274 :
275 : // See Datasheet page 13 for this formulas
276 : // Based also on Jee Labs BMP085 example code. Thanks for share.
277 : // Temperature calculations
278 0 : x1 = ((int32_t)_raw_temp - ac6) * ac5 >> 15;
279 0 : x2 = ((int32_t) mc << 11) / (x1 + md);
280 0 : b5 = x1 + x2;
281 0 : _temp = (b5 + 8) >> 4;
282 :
283 : // Pressure calculations
284 0 : b6 = b5 - 4000;
285 0 : x1 = (b2 * (b6 * b6 >> 12)) >> 11;
286 0 : x2 = ac2 * b6 >> 11;
287 0 : x3 = x1 + x2;
288 : //b3 = (((int32_t) ac1 * 4 + x3)<<OVERSAMPLING + 2) >> 2; // BAD
289 : //b3 = ((int32_t) ac1 * 4 + x3 + 2) >> 2; //OK for OVERSAMPLING=0
290 0 : tmp = ac1;
291 0 : tmp = (tmp*4 + x3)<<OVERSAMPLING;
292 0 : b3 = (tmp+2)/4;
293 0 : x1 = ac3 * b6 >> 13;
294 0 : x2 = (b1 * (b6 * b6 >> 12)) >> 16;
295 0 : x3 = ((x1 + x2) + 2) >> 2;
296 0 : b4 = (ac4 * (uint32_t)(x3 + 32768)) >> 15;
297 0 : b7 = ((uint32_t) _raw_pressure - b3) * (50000 >> OVERSAMPLING);
298 0 : p = b7 < 0x80000000 ? (b7 * 2) / b4 : (b7 / b4) * 2;
299 :
300 0 : x1 = (p >> 8) * (p >> 8);
301 0 : x1 = (x1 * 3038) >> 16;
302 0 : x2 = (-7357 * p) >> 16;
303 0 : p += ((x1 + x2 + 3791) >> 4);
304 :
305 0 : if (!pressure_ok(p)) {
306 0 : return;
307 : }
308 :
309 0 : WITH_SEMAPHORE(_sem);
310 :
311 0 : _pressure_filter.apply(p);
312 0 : _has_sample = true;
313 0 : }
314 :
315 0 : bool AP_Baro_BMP085::_data_ready()
316 : {
317 : if (BMP085_EOC >= 0) {
318 : return _eoc->read();
319 : }
320 :
321 : // No EOC pin: use time from last read instead.
322 0 : if (_state == 0) {
323 0 : return AP_HAL::millis() - _last_temp_read_command_time > 5u;
324 : }
325 :
326 : uint32_t conversion_time_msec;
327 :
328 : switch (OVERSAMPLING) {
329 : case BMP085_OVERSAMPLING_ULTRALOWPOWER:
330 : conversion_time_msec = 5;
331 : break;
332 : case BMP085_OVERSAMPLING_STANDARD:
333 : conversion_time_msec = 8;
334 : break;
335 : case BMP085_OVERSAMPLING_HIGHRES:
336 : conversion_time_msec = 14;
337 : break;
338 0 : case BMP085_OVERSAMPLING_ULTRAHIGHRES:
339 0 : conversion_time_msec = 26;
340 0 : break;
341 : default:
342 : break;
343 : }
344 :
345 0 : return AP_HAL::millis() - _last_press_read_command_time > conversion_time_msec;
346 : }
347 :
348 : #endif // AP_BARO_BMP085_ENABLED
|
