mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-08 00:48:30 -04:00
f2b7dc74d7
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.
137 lines
3.9 KiB
C++
137 lines
3.9 KiB
C++
/*
|
|
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/>.
|
|
*/
|
|
#include "AP_Baro_LPS25H.h"
|
|
|
|
#include <unistd.h>
|
|
#include <utility>
|
|
#include <stdio.h>
|
|
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<AP_HAL::Device> dev)
|
|
: AP_Baro_Backend(baro)
|
|
, _dev(std::move(dev))
|
|
{
|
|
}
|
|
|
|
AP_Baro_Backend *AP_Baro_LPS25H::probe(AP_Baro &baro,
|
|
AP_HAL::OwnPtr<AP_HAL::Device> 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();
|
|
}
|
|
}
|