2013-08-29 02:34:34 -03:00
|
|
|
/*
|
|
|
|
This program is free software: you can redistribute it and/or modify
|
|
|
|
it under the terms of the GNU General Public License as published by
|
|
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
|
|
(at your option) any later version.
|
|
|
|
|
|
|
|
This program is distributed in the hope that it will be useful,
|
|
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
|
|
GNU General Public License for more details.
|
|
|
|
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
|
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
|
|
*/
|
2015-12-01 12:07:15 -04:00
|
|
|
#include "AP_Baro_BMP085.h"
|
2011-11-27 00:42:52 -04:00
|
|
|
|
2015-11-30 15:25:00 -04:00
|
|
|
#include <utility>
|
|
|
|
|
2015-08-11 03:28:42 -03:00
|
|
|
#include <AP_Common/AP_Common.h>
|
2015-12-01 12:07:15 -04:00
|
|
|
#include <AP_HAL/AP_HAL.h>
|
2011-11-27 00:42:52 -04:00
|
|
|
|
2015-11-23 10:11:40 -04:00
|
|
|
extern const AP_HAL::HAL &hal;
|
2012-10-11 14:53:21 -03:00
|
|
|
|
2015-07-11 16:12:01 -03:00
|
|
|
#define BMP085_OVERSAMPLING_ULTRALOWPOWER 0
|
|
|
|
#define BMP085_OVERSAMPLING_STANDARD 1
|
|
|
|
#define BMP085_OVERSAMPLING_HIGHRES 2
|
|
|
|
#define BMP085_OVERSAMPLING_ULTRAHIGHRES 3
|
|
|
|
|
2015-11-03 09:46:29 -04:00
|
|
|
#ifndef BMP085_EOC
|
|
|
|
#define BMP085_EOC -1
|
2015-07-11 16:12:01 -03:00
|
|
|
#define OVERSAMPLING BMP085_OVERSAMPLING_ULTRAHIGHRES
|
2015-07-11 16:37:22 -03:00
|
|
|
#else
|
|
|
|
#define OVERSAMPLING BMP085_OVERSAMPLING_HIGHRES
|
|
|
|
#endif
|
2011-11-12 22:45:23 -04:00
|
|
|
|
2015-11-30 15:25:00 -04:00
|
|
|
AP_Baro_BMP085::AP_Baro_BMP085(AP_Baro &baro, AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev)
|
2015-11-23 10:11:40 -04:00
|
|
|
: AP_Baro_Backend(baro)
|
2015-11-30 15:25:00 -04:00
|
|
|
, _dev(std::move(dev))
|
2011-11-27 00:42:52 -04:00
|
|
|
{
|
2012-10-11 14:53:21 -03:00
|
|
|
uint8_t buff[22];
|
2012-08-17 03:09:23 -03:00
|
|
|
|
2013-10-07 21:22:58 -03:00
|
|
|
// get pointer to i2c bus semaphore
|
2015-11-30 15:25:00 -04:00
|
|
|
AP_HAL::Semaphore *sem = _dev->get_semaphore();
|
2013-10-07 21:22:58 -03:00
|
|
|
|
|
|
|
// take i2c bus sempahore
|
2015-11-30 15:25:00 -04:00
|
|
|
if (!sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
|
2015-11-19 23:07:59 -04:00
|
|
|
AP_HAL::panic("BMP085: unable to get semaphore");
|
2015-01-05 07:00:32 -04:00
|
|
|
}
|
2013-10-07 21:22:58 -03:00
|
|
|
|
2015-11-03 09:46:29 -04:00
|
|
|
if (BMP085_EOC >= 0) {
|
2015-07-11 17:00:57 -03:00
|
|
|
_eoc = hal.gpio->channel(BMP085_EOC);
|
|
|
|
_eoc->mode(HAL_GPIO_INPUT);
|
2015-11-03 09:46:29 -04:00
|
|
|
}
|
2012-08-17 03:09:23 -03:00
|
|
|
|
|
|
|
// We read the calibration data registers
|
2015-11-30 15:25:00 -04:00
|
|
|
if (!_dev->read_registers(0xAA, buff, 22)) {
|
2015-11-19 23:07:59 -04:00
|
|
|
AP_HAL::panic("BMP085: bad calibration registers");
|
2012-08-17 03:09:23 -03:00
|
|
|
}
|
|
|
|
|
2013-01-09 08:44:26 -04:00
|
|
|
ac1 = ((int16_t)buff[0] << 8) | buff[1];
|
|
|
|
ac2 = ((int16_t)buff[2] << 8) | buff[3];
|
|
|
|
ac3 = ((int16_t)buff[4] << 8) | buff[5];
|
|
|
|
ac4 = ((int16_t)buff[6] << 8) | buff[7];
|
|
|
|
ac5 = ((int16_t)buff[8] << 8) | buff[9];
|
|
|
|
ac6 = ((int16_t)buff[10] << 8) | buff[11];
|
|
|
|
b1 = ((int16_t)buff[12] << 8) | buff[13];
|
|
|
|
b2 = ((int16_t)buff[14] << 8) | buff[15];
|
|
|
|
mb = ((int16_t)buff[16] << 8) | buff[17];
|
|
|
|
mc = ((int16_t)buff[18] << 8) | buff[19];
|
|
|
|
md = ((int16_t)buff[20] << 8) | buff[21];
|
2012-08-17 03:09:23 -03:00
|
|
|
|
2013-09-23 00:12:09 -03:00
|
|
|
_last_press_read_command_time = 0;
|
|
|
|
_last_temp_read_command_time = 0;
|
|
|
|
|
2015-01-05 07:00:32 -04:00
|
|
|
_instance = _frontend.register_sensor();
|
|
|
|
|
2015-11-23 10:11:40 -04:00
|
|
|
// Send a command to read temperature
|
|
|
|
_cmd_read_temp();
|
|
|
|
|
2016-05-04 21:39:39 -03:00
|
|
|
_state = 0;
|
2012-08-17 03:09:23 -03:00
|
|
|
|
2015-11-30 15:25:00 -04:00
|
|
|
sem->give();
|
2016-11-03 22:05:12 -03:00
|
|
|
|
|
|
|
_dev->register_periodic_callback(20000, FUNCTOR_BIND_MEMBER(&AP_Baro_BMP085::_timer, bool));
|
2011-11-27 00:42:52 -04:00
|
|
|
}
|
|
|
|
|
2015-11-23 10:11:40 -04:00
|
|
|
/*
|
|
|
|
This is a state machine. Acumulate a new sensor reading.
|
|
|
|
*/
|
2016-11-03 22:05:12 -03:00
|
|
|
bool AP_Baro_BMP085::_timer(void)
|
2013-01-09 08:05:17 -04:00
|
|
|
{
|
2015-11-23 10:25:10 -04:00
|
|
|
if (!_data_ready()) {
|
2016-11-03 22:05:12 -03:00
|
|
|
return true;
|
2015-11-23 10:11:40 -04:00
|
|
|
}
|
2013-10-07 21:22:58 -03:00
|
|
|
|
2016-05-04 21:39:39 -03:00
|
|
|
if (_state == 0) {
|
2015-11-23 10:11:40 -04:00
|
|
|
_read_temp();
|
|
|
|
} else if (_read_pressure()) {
|
|
|
|
_calculate();
|
2013-01-09 08:44:26 -04:00
|
|
|
}
|
2015-11-23 10:11:40 -04:00
|
|
|
|
2016-05-04 21:39:39 -03:00
|
|
|
_state++;
|
2015-07-11 19:14:41 -03:00
|
|
|
if (_state == 25) {
|
2016-05-04 21:39:39 -03:00
|
|
|
_state = 0;
|
2015-11-23 10:11:40 -04:00
|
|
|
_cmd_read_temp();
|
2013-01-09 08:44:26 -04:00
|
|
|
} else {
|
2015-11-23 10:11:40 -04:00
|
|
|
_cmd_read_pressure();
|
2013-01-09 08:05:17 -04:00
|
|
|
}
|
2016-11-03 22:05:12 -03:00
|
|
|
return true;
|
2013-01-09 08:05:17 -04:00
|
|
|
}
|
|
|
|
|
2015-01-05 07:00:32 -04:00
|
|
|
/*
|
|
|
|
transfer data to the frontend
|
|
|
|
*/
|
|
|
|
void AP_Baro_BMP085::update(void)
|
2011-11-27 00:42:52 -04:00
|
|
|
{
|
2016-11-03 22:05:12 -03:00
|
|
|
if (_sem->take_nonblocking()) {
|
|
|
|
if (!_has_sample) {
|
|
|
|
_sem->give();
|
|
|
|
return;
|
|
|
|
}
|
2013-01-09 08:05:17 -04:00
|
|
|
|
2016-11-03 22:05:12 -03:00
|
|
|
float temperature = 0.1f * _temp;
|
|
|
|
float pressure = _pressure_filter.getf();
|
2013-01-09 08:05:17 -04:00
|
|
|
|
2016-11-03 22:05:12 -03:00
|
|
|
_copy_to_frontend(_instance, pressure, temperature);
|
|
|
|
_sem->give();
|
|
|
|
}
|
2011-11-30 00:33:53 -04:00
|
|
|
}
|
|
|
|
|
2011-11-27 00:42:52 -04:00
|
|
|
// Send command to Read Pressure
|
2015-11-23 10:11:40 -04:00
|
|
|
void AP_Baro_BMP085::_cmd_read_pressure()
|
2011-11-27 00:42:52 -04:00
|
|
|
{
|
2015-11-30 15:25:00 -04:00
|
|
|
_dev->write_register(0xF4, 0x34 + (OVERSAMPLING << 6));
|
2015-11-19 23:07:59 -04:00
|
|
|
_last_press_read_command_time = AP_HAL::millis();
|
2011-11-27 00:42:52 -04:00
|
|
|
}
|
|
|
|
|
2015-11-23 10:11:40 -04:00
|
|
|
// Read raw pressure values
|
|
|
|
bool AP_Baro_BMP085::_read_pressure()
|
2011-11-27 00:42:52 -04:00
|
|
|
{
|
2012-08-17 03:09:23 -03:00
|
|
|
uint8_t buf[3];
|
2011-11-27 00:42:52 -04:00
|
|
|
|
2015-11-30 15:25:00 -04:00
|
|
|
if (!_dev->read_registers(0xF6, buf, 3)) {
|
2015-11-19 23:07:59 -04:00
|
|
|
_retry_time = AP_HAL::millis() + 1000;
|
2015-11-30 15:25:00 -04:00
|
|
|
_dev->set_speed(AP_HAL::Device::SPEED_LOW);
|
2015-01-05 07:00:32 -04:00
|
|
|
return false;
|
2012-08-17 03:09:23 -03:00
|
|
|
}
|
2011-11-27 00:42:52 -04:00
|
|
|
|
2015-11-23 10:11:40 -04:00
|
|
|
_raw_pressure = (((uint32_t)buf[0] << 16)
|
|
|
|
| ((uint32_t)buf[1] << 8)
|
|
|
|
| ((uint32_t)buf[2])) >> (8 - OVERSAMPLING);
|
2015-01-05 07:00:32 -04:00
|
|
|
return true;
|
2011-11-27 00:42:52 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
// Send Command to Read Temperature
|
2015-11-23 10:11:40 -04:00
|
|
|
void AP_Baro_BMP085::_cmd_read_temp()
|
2011-11-27 00:42:52 -04:00
|
|
|
{
|
2015-11-30 15:25:00 -04:00
|
|
|
_dev->write_register(0xF4, 0x2E);
|
2015-11-19 23:07:59 -04:00
|
|
|
_last_temp_read_command_time = AP_HAL::millis();
|
2011-11-27 00:42:52 -04:00
|
|
|
}
|
|
|
|
|
2015-11-23 10:11:40 -04:00
|
|
|
// Read raw temperature values
|
|
|
|
void AP_Baro_BMP085::_read_temp()
|
2011-11-27 00:42:52 -04:00
|
|
|
{
|
2012-08-17 03:09:23 -03:00
|
|
|
uint8_t buf[2];
|
|
|
|
int32_t _temp_sensor;
|
2011-11-27 00:42:52 -04:00
|
|
|
|
2015-11-30 15:25:00 -04:00
|
|
|
if (!_dev->read_registers(0xF6, buf, 2)) {
|
|
|
|
_dev->set_speed(AP_HAL::Device::SPEED_LOW);
|
2012-08-17 03:09:23 -03:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
_temp_sensor = buf[0];
|
|
|
|
_temp_sensor = (_temp_sensor << 8) | buf[1];
|
2011-11-27 00:42:52 -04:00
|
|
|
|
2015-11-23 10:11:40 -04:00
|
|
|
_raw_temp = _temp_sensor;
|
2011-11-27 00:42:52 -04:00
|
|
|
}
|
|
|
|
|
2015-11-23 10:11:40 -04:00
|
|
|
// _calculate Temperature and Pressure in real units.
|
|
|
|
void AP_Baro_BMP085::_calculate()
|
2011-11-27 00:42:52 -04:00
|
|
|
{
|
2012-08-17 03:09:23 -03:00
|
|
|
int32_t x1, x2, x3, b3, b5, b6, p;
|
|
|
|
uint32_t b4, b7;
|
|
|
|
int32_t tmp;
|
|
|
|
|
|
|
|
// See Datasheet page 13 for this formulas
|
|
|
|
// Based also on Jee Labs BMP085 example code. Thanks for share.
|
|
|
|
// Temperature calculations
|
2015-11-23 10:11:40 -04:00
|
|
|
x1 = ((int32_t)_raw_temp - ac6) * ac5 >> 15;
|
2012-08-17 03:09:23 -03:00
|
|
|
x2 = ((int32_t) mc << 11) / (x1 + md);
|
|
|
|
b5 = x1 + x2;
|
2015-07-11 19:14:41 -03:00
|
|
|
_temp = (b5 + 8) >> 4;
|
2012-08-17 03:09:23 -03:00
|
|
|
|
|
|
|
// Pressure calculations
|
|
|
|
b6 = b5 - 4000;
|
|
|
|
x1 = (b2 * (b6 * b6 >> 12)) >> 11;
|
|
|
|
x2 = ac2 * b6 >> 11;
|
|
|
|
x3 = x1 + x2;
|
|
|
|
//b3 = (((int32_t) ac1 * 4 + x3)<<OVERSAMPLING + 2) >> 2; // BAD
|
|
|
|
//b3 = ((int32_t) ac1 * 4 + x3 + 2) >> 2; //OK for OVERSAMPLING=0
|
|
|
|
tmp = ac1;
|
|
|
|
tmp = (tmp*4 + x3)<<OVERSAMPLING;
|
|
|
|
b3 = (tmp+2)/4;
|
|
|
|
x1 = ac3 * b6 >> 13;
|
|
|
|
x2 = (b1 * (b6 * b6 >> 12)) >> 16;
|
|
|
|
x3 = ((x1 + x2) + 2) >> 2;
|
|
|
|
b4 = (ac4 * (uint32_t) (x3 + 32768)) >> 15;
|
2015-11-23 10:11:40 -04:00
|
|
|
b7 = ((uint32_t) _raw_pressure - b3) * (50000 >> OVERSAMPLING);
|
2012-08-17 03:09:23 -03:00
|
|
|
p = b7 < 0x80000000 ? (b7 * 2) / b4 : (b7 / b4) * 2;
|
|
|
|
|
|
|
|
x1 = (p >> 8) * (p >> 8);
|
|
|
|
x1 = (x1 * 3038) >> 16;
|
|
|
|
x2 = (-7357 * p) >> 16;
|
2015-07-12 22:40:36 -03:00
|
|
|
p += ((x1 + x2 + 3791) >> 4);
|
2013-01-09 08:05:17 -04:00
|
|
|
|
2016-11-03 22:05:12 -03:00
|
|
|
if (_sem->take(0)) {
|
|
|
|
_pressure_filter.apply(p);
|
|
|
|
_has_sample = true;
|
|
|
|
_sem->give();
|
|
|
|
}
|
2011-11-12 22:45:23 -04:00
|
|
|
}
|
2015-11-23 10:25:10 -04:00
|
|
|
|
|
|
|
bool AP_Baro_BMP085::_data_ready()
|
|
|
|
{
|
|
|
|
if (BMP085_EOC >= 0) {
|
2015-07-11 17:00:57 -03:00
|
|
|
return _eoc->read();
|
2015-11-23 10:25:10 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
// No EOC pin: use time from last read instead.
|
2016-05-04 21:39:39 -03:00
|
|
|
if (_state == 0) {
|
2015-11-23 10:25:10 -04:00
|
|
|
return AP_HAL::millis() > _last_temp_read_command_time + 5;
|
|
|
|
}
|
|
|
|
|
2015-07-11 16:12:01 -03:00
|
|
|
uint32_t conversion_time_msec;
|
|
|
|
|
|
|
|
switch (OVERSAMPLING) {
|
|
|
|
case BMP085_OVERSAMPLING_ULTRALOWPOWER:
|
|
|
|
conversion_time_msec = 5;
|
|
|
|
break;
|
|
|
|
case BMP085_OVERSAMPLING_STANDARD:
|
|
|
|
conversion_time_msec = 8;
|
|
|
|
break;
|
|
|
|
case BMP085_OVERSAMPLING_HIGHRES:
|
|
|
|
conversion_time_msec = 14;
|
|
|
|
break;
|
|
|
|
case BMP085_OVERSAMPLING_ULTRAHIGHRES:
|
|
|
|
conversion_time_msec = 26;
|
|
|
|
default:
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
|
|
|
return AP_HAL::millis() > _last_press_read_command_time + conversion_time_msec;
|
2015-11-23 10:25:10 -04:00
|
|
|
}
|