From f2b7dc74d7bfbc9004324615ccac085999f1b141 Mon Sep 17 00:00:00 2001 From: liorosh Date: Sat, 2 Sep 2017 17:37:48 +0300 Subject: [PATCH] AP_Baro: Adding a new LPS25H Barometer driver The following is a Barometer sensor driver for the LPS25H Barometer that is integrated in the 96Boards STM32 Sensor mezzanine board. the update includes the .cpp and .h files of the driver as well as the updates required in AP_Baro.cpp. --- libraries/AP_Baro/AP_Baro.cpp | 4 + libraries/AP_Baro/AP_Baro_LPS25H.cpp | 136 +++++++++++++++++++++++++++ libraries/AP_Baro/AP_Baro_LPS25H.h | 37 ++++++++ 3 files changed, 177 insertions(+) create mode 100644 libraries/AP_Baro/AP_Baro_LPS25H.cpp create mode 100644 libraries/AP_Baro/AP_Baro_LPS25H.h diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp index 5f57a57095..70f0b58f9a 100644 --- a/libraries/AP_Baro/AP_Baro.cpp +++ b/libraries/AP_Baro/AP_Baro.cpp @@ -34,6 +34,7 @@ #include "AP_Baro_HIL.h" #include "AP_Baro_KellerLD.h" #include "AP_Baro_MS5611.h" +#include "AP_Baro_LPS25H.h" #include "AP_Baro_qflight.h" #include "AP_Baro_QURT.h" #if HAL_WITH_UAVCAN @@ -479,6 +480,9 @@ void AP_Baro::init(void) #elif HAL_BARO_DEFAULT == HAL_BARO_QURT drivers[0] = new AP_Baro_QURT(*this); _num_drivers = 1; +#elif HAL_BARO_DEFAULT == HAL_BARO_LPS25H + ADD_BACKEND(AP_Baro_LPS25H::probe(*this, + std::move(hal.i2c_mgr->get_device(HAL_BARO_LPS25H_I2C_BUS, HAL_BARO_LPS25H_I2C_ADDR)))); #endif // can optionally have baro on I2C too diff --git a/libraries/AP_Baro/AP_Baro_LPS25H.cpp b/libraries/AP_Baro/AP_Baro_LPS25H.cpp new file mode 100644 index 0000000000..12b62c5800 --- /dev/null +++ b/libraries/AP_Baro/AP_Baro_LPS25H.cpp @@ -0,0 +1,136 @@ +/* + 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 . + */ +#include "AP_Baro_LPS25H.h" + +#include +#include +#include +extern const AP_HAL::HAL &hal; + + +#define LPS25H_ID 0xBD +#define LPS25H_REG_ID 0x0F +#define LPS25H_CTRL_REG1_ADDR 0x20 +#define LPS25H_CTRL_REG2_ADDR 0x21 +#define LPS25H_CTRL_REG3_ADDR 0x22 +#define LPS25H_CTRL_REG4_ADDR 0x23 +#define LPS25H_FIFO_CTRL 0x2E +#define LPS25H_TEMP_OUT_ADDR 0xAB //Regsiter address is 0x2B in 2's compliment. +#define PRESS_OUT_XL_ADDR 0xA8 //Regsiter address is 0x28 in 2's compliment. +//putting 1 in the MSB of those two registers turns on Auto increment for faster reading. + +AP_Baro_LPS25H::AP_Baro_LPS25H(AP_Baro &baro, AP_HAL::OwnPtr dev) + : AP_Baro_Backend(baro) + , _dev(std::move(dev)) +{ +} + +AP_Baro_Backend *AP_Baro_LPS25H::probe(AP_Baro &baro, + AP_HAL::OwnPtr dev) +{ + if (!dev) { + return nullptr; + } + AP_Baro_LPS25H *sensor = new AP_Baro_LPS25H(baro, std::move(dev)); + if (!sensor || !sensor->_init()) { + delete sensor; + return nullptr; + } + return sensor; +} + +bool AP_Baro_LPS25H::_init() +{ + if (!_dev || !_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { + return false; + } + _has_sample = false; + + _dev->set_speed(AP_HAL::Device::SPEED_HIGH); + + uint8_t whoami; + if (!_dev->read_registers(LPS25H_REG_ID, &whoami, 1) || + whoami != LPS25H_ID) { + // not a LPS25H + _dev->get_semaphore()->give(); + return false; + } + + //init control registers. + _dev->write_register(LPS25H_CTRL_REG1_ADDR,0x00); // turn off for config + _dev->write_register(LPS25H_CTRL_REG2_ADDR,0x00); //FIFO Disabled + _dev->write_register(LPS25H_FIFO_CTRL, 0x01); + _dev->write_register(LPS25H_CTRL_REG1_ADDR,0xc0); + + _instance = _frontend.register_sensor(); + + _dev->get_semaphore()->give(); + + // request 25Hz update (maximum refresh Rate according to datasheet) + _dev->register_periodic_callback(40 * AP_USEC_PER_MSEC, FUNCTOR_BIND_MEMBER(&AP_Baro_LPS25H::_timer, void)); + + return true; +} + + + +// acumulate a new sensor reading +void AP_Baro_LPS25H::_timer(void) +{ + _update_temperature(); + _update_pressure(); + _has_sample = true; +} + +// transfer data to the frontend +void AP_Baro_LPS25H::update(void) +{ + if (_sem->take_nonblocking()) { + if (!_has_sample) { + _sem->give(); + return; + } + + _copy_to_frontend(_instance, _pressure, _temperature); + _has_sample = false; + _sem->give(); + } +} + +// calculate temperature +void AP_Baro_LPS25H::_update_temperature(void) +{ + uint8_t pu8[2]; + _dev->read_registers(LPS25H_TEMP_OUT_ADDR, pu8, 2); + int16_t Temp_Reg_s16 = (uint16_t)(pu8[1]<<8) | pu8[0]; + if (_sem->take_nonblocking()) { + _temperature=((float)(Temp_Reg_s16/480)+42.5); + _sem->give(); + } + +} + +// calculate pressure +void AP_Baro_LPS25H::_update_pressure(void) +{ + uint8_t pressure[3]; + _dev->read_registers(PRESS_OUT_XL_ADDR, pressure, 3); + int32_t Pressure_Reg_s32 = ((uint32_t)pressure[2]<<16)|((uint32_t)pressure[1]<<8)|(uint32_t)pressure[0]; + int32_t Pressure_mb = Pressure_Reg_s32 / 4096; // scale + if (_sem->take_nonblocking()) { + _pressure=Pressure_mb; + _sem->give(); + } +} diff --git a/libraries/AP_Baro/AP_Baro_LPS25H.h b/libraries/AP_Baro/AP_Baro_LPS25H.h new file mode 100644 index 0000000000..911efd98fe --- /dev/null +++ b/libraries/AP_Baro/AP_Baro_LPS25H.h @@ -0,0 +1,37 @@ +#pragma once + +#include +#include +#include + +#include "AP_Baro_Backend.h" + +#define HAL_BARO_LPS25H_I2C_BUS 0 +#define HAL_BARO_LPS25H_I2C_ADDR 0x5D + + +class AP_Baro_LPS25H : public AP_Baro_Backend +{ +public: + AP_Baro_LPS25H(AP_Baro &baro, AP_HAL::OwnPtr dev); + + /* AP_Baro public interface: */ + void update(); + + static AP_Baro_Backend *probe(AP_Baro &baro, AP_HAL::OwnPtr dev); + +private: + virtual ~AP_Baro_LPS25H(void) {}; + + bool _init(void); + void _timer(void); + void _update_temperature(void); + void _update_pressure(void); + + AP_HAL::OwnPtr _dev; + + bool _has_sample; + uint8_t _instance; + float _pressure; + float _temperature; +};