diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp index dc861b6f53..65d50197e3 100644 --- a/libraries/AP_Baro/AP_Baro.cpp +++ b/libraries/AP_Baro/AP_Baro.cpp @@ -241,6 +241,11 @@ void AP_Baro::init(void) #elif HAL_BARO_DEFAULT == HAL_BARO_HIL drivers[0] = new AP_Baro_HIL(*this); _num_drivers = 1; +#elif HAL_BARO_DEFAULT == HAL_BARO_BMP085 + { + drivers[0] = new AP_Baro_BMP085(*this); + _num_drivers = 1; + } #elif HAL_BARO_DEFAULT == HAL_BARO_MS5611 { drivers[0] = new AP_Baro_MS5611(*this, new AP_SerialBus_I2C(MS5611_I2C_ADDR)); diff --git a/libraries/AP_Baro/AP_Baro.h b/libraries/AP_Baro/AP_Baro.h index b65426c135..9825b835b2 100644 --- a/libraries/AP_Baro/AP_Baro.h +++ b/libraries/AP_Baro/AP_Baro.h @@ -149,7 +149,7 @@ private: #include "AP_Baro_Backend.h" #include "AP_Baro_MS5611.h" -//#include "AP_Baro_BMP085.h" +#include "AP_Baro_BMP085.h" #include "AP_Baro_HIL.h" #include "AP_Baro_PX4.h" diff --git a/libraries/AP_Baro/AP_Baro_BMP085.cpp b/libraries/AP_Baro/AP_Baro_BMP085.cpp index 6efbf79a9e..c52e591531 100644 --- a/libraries/AP_Baro/AP_Baro_BMP085.cpp +++ b/libraries/AP_Baro/AP_Baro_BMP085.cpp @@ -1,5 +1,4 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#if 0 /* 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 @@ -15,42 +14,16 @@ along with this program. If not, see . */ /* - * APM_BMP085.cpp - Arduino Library for BMP085 absolute pressure sensor - * Code by Jordi Mu�oz and Jose Julio. DIYDrones.com - * Sensor is conected to I2C port - * Sensor End of Conversion (EOC) pin is PC7 (30) - * - * Variables: - * RawTemp : Raw temperature data - * RawPress : Raw pressure data - * - * Temp : Calculated temperature (in 0.1�C units) - * Press : Calculated pressure (in Pa units) - * - * Methods: - * Init() : Initialization of I2C and read sensor calibration data - * Read() : Read sensor data and calculate Temperature and Pressure - * This function is optimized so the main host don�t need to wait - * You can call this function in your main loop - * It returns a 1 if there are new data. - * - * Internal functions: - * Command_ReadTemp(): Send commando to read temperature - * Command_ReadPress(): Send commando to read Pressure - * ReadTemp() : Read temp register - * ReadPress() : Read press register - * - * - */ + BMP085 barometer driver. Based on original code by Jordi Munoz and + Jose Julio -// AVR LibC Includes -#include - -#include -#include // ArduPilot Mega Vector/Matrix math Library + Substantially modified by Andrew Tridgell +*/ #include -#include "AP_Baro_BMP085.h" +#include + +#include "AP_Baro.h" extern const AP_HAL::HAL& hal; @@ -74,25 +47,36 @@ extern const AP_HAL::HAL& hal; // oversampling 3 gives 26ms conversion time. We then average #define OVERSAMPLING 3 -// Public Methods ////////////////////////////////////////////////////////////// -bool AP_Baro_BMP085::init() +/* + constructor + */ +AP_Baro_BMP085::AP_Baro_BMP085(AP_Baro &baro) : + AP_Baro_Backend(baro), + _instance(0), + _temp_sum(0), + _press_sum(0), + _count(0), + BMP085_State(0), + ac1(0), ac2(0), ac3(0), b1(0), b2(0), mb(0), mc(0), md(0), + ac4(0), ac5(0), ac6(0), + _retry_time(0) { uint8_t buff[22]; // get pointer to i2c bus semaphore - AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore(); + AP_HAL::Semaphore *i2c_sem = hal.i2c->get_semaphore(); // take i2c bus sempahore - if (!i2c_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) - return false; + if (!i2c_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { + hal.scheduler->panic(PSTR("BMP085: unable to get semaphore")); + } - hal.gpio->pinMode(BMP085_EOC, HAL_GPIO_INPUT);// End Of Conversion (PC7) input + // End Of Conversion (PC7) input + hal.gpio->pinMode(BMP085_EOC, HAL_GPIO_INPUT); // We read the calibration data registers if (hal.i2c->readRegisters(BMP085_ADDRESS, 0xAA, 22, buff) != 0) { - _flags.healthy = false; - i2c_sem->give(); - return false; + hal.scheduler->panic(PSTR("BMP085: bad calibration registers")); } ac1 = ((int16_t)buff[0] << 8) | buff[1]; @@ -110,17 +94,14 @@ bool AP_Baro_BMP085::init() _last_press_read_command_time = 0; _last_temp_read_command_time = 0; + _instance = _frontend.register_sensor(); + //Send a command to read Temp Command_ReadTemp(); BMP085_State = 0; - // init raw temo - RawTemp = 0; - - _flags.healthy = true; i2c_sem->give(); - return true; } // Read the sensor. This is a state machine @@ -141,8 +122,9 @@ void AP_Baro_BMP085::accumulate(void) if (BMP085_State == 0) { ReadTemp(); } else { - ReadPress(); - Calculate(); + if (ReadPress()) { + Calculate(); + } } BMP085_State++; if (BMP085_State == 5) { @@ -156,76 +138,58 @@ void AP_Baro_BMP085::accumulate(void) } -// Read the sensor using accumulated data -uint8_t AP_Baro_BMP085::read() +/* + transfer data to the frontend + */ +void AP_Baro_BMP085::update(void) { if (_count == 0 && BMP_DATA_READY()) { accumulate(); } if (_count == 0) { - return 0; + return; } - _last_update = hal.scheduler->millis(); - Temp = 0.1f * _temp_sum / _count; - Press = _press_sum / _count; + float temperature = 0.1f * _temp_sum / _count; + float pressure = _press_sum / _count; _count = 0; _temp_sum = 0; _press_sum = 0; - return 1; + _copy_to_frontend(_instance, pressure, temperature); } -float AP_Baro_BMP085::get_pressure() { - return Press; -} - -float AP_Baro_BMP085::get_temperature() const { - return Temp; -} - -// Private functions: ///////////////////////////////////////////////////////// - // Send command to Read Pressure void AP_Baro_BMP085::Command_ReadPress() { // Mode 0x34+(OVERSAMPLING << 6) is osrs=3 when OVERSAMPLING=3 => 25.5ms conversion time - uint8_t res = hal.i2c->writeRegister(BMP085_ADDRESS, 0xF4, - 0x34+(OVERSAMPLING << 6)); + hal.i2c->writeRegister(BMP085_ADDRESS, 0xF4, + 0x34+(OVERSAMPLING << 6)); _last_press_read_command_time = hal.scheduler->millis(); - if (res != 0) { - _flags.healthy = false; - } } // Read Raw Pressure values -void AP_Baro_BMP085::ReadPress() +bool AP_Baro_BMP085::ReadPress() { uint8_t buf[3]; - if (!_flags.healthy && hal.scheduler->millis() < _retry_time) { - return; - } - if (hal.i2c->readRegisters(BMP085_ADDRESS, 0xF6, 3, buf) != 0) { _retry_time = hal.scheduler->millis() + 1000; hal.i2c->setHighSpeed(false); - _flags.healthy = false; - return; + return false; } RawPress = (((uint32_t)buf[0] << 16) | ((uint32_t)buf[1] << 8) | ((uint32_t)buf[2])) >> (8 - OVERSAMPLING); + return true; } // Send Command to Read Temperature void AP_Baro_BMP085::Command_ReadTemp() { - if (hal.i2c->writeRegister(BMP085_ADDRESS, 0xF4, 0x2E) != 0) { - _flags.healthy = false; - } + hal.i2c->writeRegister(BMP085_ADDRESS, 0xF4, 0x2E); _last_temp_read_command_time = hal.scheduler->millis(); } @@ -235,20 +199,14 @@ void AP_Baro_BMP085::ReadTemp() uint8_t buf[2]; int32_t _temp_sensor; - if (!_flags.healthy && hal.scheduler->millis() < _retry_time) { - return; - } - if (hal.i2c->readRegisters(BMP085_ADDRESS, 0xF6, 2, buf) != 0) { - _retry_time = hal.scheduler->millis() + 1000; hal.i2c->setHighSpeed(false); - _flags.healthy = false; return; } _temp_sensor = buf[0]; _temp_sensor = (_temp_sensor << 8) | buf[1]; - RawTemp = _temp_filter.apply(_temp_sensor); + RawTemp = _temp_sensor; } @@ -296,5 +254,3 @@ void AP_Baro_BMP085::Calculate() _count /= 2; } } - -#endif diff --git a/libraries/AP_Baro/AP_Baro_BMP085.h b/libraries/AP_Baro/AP_Baro_BMP085.h index 85ee991b45..065ca71797 100644 --- a/libraries/AP_Baro/AP_Baro_BMP085.h +++ b/libraries/AP_Baro/AP_Baro_BMP085.h @@ -2,65 +2,42 @@ #ifndef __AP_BARO_BMP085_H__ #define __AP_BARO_BMP085_H__ -#define PRESS_FILTER_SIZE 2 - #include "AP_Baro.h" -#include -class AP_Baro_BMP085 : public AP_Baro +class AP_Baro_BMP085 : public AP_Baro_Backend { public: - AP_Baro_BMP085() : - RawPress(0), - RawTemp(0), - _temp_sum(0.0f), - _press_sum(0.0f), - _count(0), - Temp(0.0f), - Press(0.0f), - _last_press_read_command_time(0), - _last_temp_read_command_time(0), - BMP085_State(0), - ac1(0), ac2(0), ac3(0), b1(0), b2(0), mb(0), mc(0), md(0), - ac4(0), ac5(0), ac6(0), - _retry_time(0) - { - }; // Constructor - + // Constructor + AP_Baro_BMP085(AP_Baro &baro); /* AP_Baro public interface: */ - bool init(); - uint8_t read(); - void accumulate(void); - float get_pressure(); - float get_temperature() const; + void update(); + void accumulate(void); private: - int32_t RawPress; - int32_t RawTemp; + uint8_t _instance; float _temp_sum; float _press_sum; uint8_t _count; - float Temp; - float Press; + // Flymaple has no EOC pin, so use times instead uint32_t _last_press_read_command_time; uint32_t _last_temp_read_command_time; - // State machine uint8_t BMP085_State; + // Internal calibration registers int16_t ac1, ac2, ac3, b1, b2, mb, mc, md; uint16_t ac4, ac5, ac6; - AverageFilterInt32_Size4 _temp_filter; - uint32_t _retry_time; + int32_t RawPress; + int32_t RawTemp; void Command_ReadPress(); void Command_ReadTemp(); - void ReadPress(); + bool ReadPress(); void ReadTemp(); void Calculate(); };