aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Baro: support mini-pix using LPS25H SPI barometer

Browse Source 
allow LPS22H and LPS25H to share a driver
This commit is contained in:
ljwang 2018-02-06 21:39:55 +11:00 committed by Andrew Tridgell
parent 348a757cc6
commit 88effef51a
4 changed files with 209 additions and 139 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AP_Baro/AP_Baro.cpp
View File

@ -34,8 +34,8 @@
#include "AP_Baro_HIL.h"
#include "AP_Baro_KellerLD.h"
#include "AP_Baro_MS5611.h"
#include "AP_Baro_LPS25H.h"
#include "AP_Baro_ICM20789.h"
#include "AP_Baro_LPS2XH.h"
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT
#include "AP_Baro_qflight.h"
#endif
@ -519,7 +519,7 @@ void AP_Baro::init(void)
drivers[0] = new AP_Baro_QURT(*this);
_num_drivers = 1;
#elif HAL_BARO_DEFAULT == HAL_BARO_LPS25H
ADD_BACKEND(AP_Baro_LPS25H::probe(*this,
ADD_BACKEND(AP_Baro_LPS2XH::probe(*this,
std::move(hal.i2c_mgr->get_device(HAL_BARO_LPS25H_I2C_BUS, HAL_BARO_LPS25H_I2C_ADDR))));
#elif HAL_BARO_DEFAULT == HAL_BARO_20789_I2C_I2C
ADD_BACKEND(AP_Baro_ICM20789::probe(*this,
@ -529,6 +529,9 @@ void AP_Baro::init(void)
ADD_BACKEND(AP_Baro_ICM20789::probe(*this,
std::move(hal.i2c_mgr->get_device(HAL_BARO_20789_I2C_BUS, HAL_BARO_20789_I2C_ADDR_PRESS)),
std::move(hal.spi->get_device("icm20789"))));
#elif HAL_BARO_DEFAULT == HAL_BARO_LPS22H_SPI
ADD_BACKEND(AP_Baro_LPS2XH::probe(*this,
std::move(hal.spi->get_device(HAL_BARO_LPS22H_NAME))));
#endif
// can optionally have baro on I2C too

135
libraries/AP_Baro/AP_Baro_LPS25H.cpp
View File

@ -1,135 +0,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
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 <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();
}
}

185
libraries/AP_Baro/AP_Baro_LPS2XH.cpp Normal file
View File

@ -0,0 +1,185 @@
/*
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 <utility>
#include <stdio.h>
#include "AP_Baro_LPS2XH.h"
extern const AP_HAL::HAL &hal;
#define REG_ID 0x0F
#define LPS22H_ID 0xB1
#define LPS22H_CTRL_REG1 0x10
#define LPS22H_CTRL_REG2 0x11
#define LPS22H_CTRL_REG3 0x12
#define LPS22H_CTRL_REG1_SIM (1 << 0)
#define LPS22H_CTRL_REG1_BDU (1 << 1)
#define LPS22H_CTRL_REG1_LPFP_CFG (1 << 2)
#define LPS22H_CTRL_REG1_EN_LPFP (1 << 3)
#define LPS22H_CTRL_REG1_PD (0 << 4)
#define LPS22H_CTRL_REG1_ODR_1HZ (1 << 4)
#define LPS22H_CTRL_REG1_ODR_10HZ (2 << 4)
#define LPS22H_CTRL_REG1_ODR_25HZ (3 << 4)
#define LPS22H_CTRL_REG1_ODR_50HZ (4 << 4)
#define LPS22H_CTRL_REG1_ODR_75HZ (5 << 4)
#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 TEMP_OUT_ADDR 0x2B
#define PRESS_OUT_XL_ADDR 0x28
//putting 1 in the MSB of those two registers turns on Auto increment for faster reading.
AP_Baro_LPS2XH::AP_Baro_LPS2XH(AP_Baro &baro, AP_HAL::OwnPtr<AP_HAL::Device> dev)
: AP_Baro_Backend(baro)
, _dev(std::move(dev))
{
}
AP_Baro_Backend *AP_Baro_LPS2XH::probe(AP_Baro &baro,
AP_HAL::OwnPtr<AP_HAL::Device> dev)
{
if (!dev) {
return nullptr;
}
AP_Baro_LPS2XH *sensor = new AP_Baro_LPS2XH(baro, std::move(dev));
if (!sensor || !sensor->_init()) {
delete sensor;
return nullptr;
}
return sensor;
}
bool AP_Baro_LPS2XH::_init()
{
if (!_dev || !_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
return false;
}
_has_sample = false;
_dev->set_speed(AP_HAL::Device::SPEED_HIGH);
// top bit is for read on SPI
_dev->set_read_flag(0x80);
if(!_check_whoami()){
_dev->get_semaphore()->give();
return false;
}
//init control registers.
if(_lps2xh_type == BARO_LPS25H){
_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);
// request 25Hz update (maximum refresh Rate according to datasheet)
CallTime = 40 * AP_USEC_PER_MSEC;
}
if(_lps2xh_type == BARO_LPS22H){
_dev->write_register(LPS22H_CTRL_REG1,LPS22H_CTRL_REG1_ODR_75HZ|LPS22H_CTRL_REG1_BDU|LPS22H_CTRL_REG1_EN_LPFP|LPS22H_CTRL_REG1_LPFP_CFG);
_dev->write_register(LPS22H_CTRL_REG2,0x18);
// request 75Hz update
CallTime = 1000000/75;
}
_instance = _frontend.register_sensor();
_dev->get_semaphore()->give();
_dev->register_periodic_callback(CallTime, FUNCTOR_BIND_MEMBER(&AP_Baro_LPS2XH::_timer, void));
return true;
}
//check ID
bool AP_Baro_LPS2XH::_check_whoami(void)
{
uint8_t whoami;
if (! _dev->read_registers(REG_ID, &whoami, 1)) {
return false;
}
hal.console->printf("LPS2XH whoami 0x%02x\n", whoami);
switch(whoami){
case LPS22HB_WHOAMI:
_lps2xh_type = BARO_LPS22H;
return true;
case LPS25HB_WHOAMI:
_lps2xh_type = BARO_LPS25H;
return true;
}
return false;
}
// acumulate a new sensor reading
void AP_Baro_LPS2XH::_timer(void)
{
_update_temperature();
_update_pressure();
_has_sample = true;
}
// transfer data to the frontend
void AP_Baro_LPS2XH::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_LPS2XH::_update_temperature(void)
{
uint8_t pu8[2];
_dev->read_registers(TEMP_OUT_ADDR, pu8, 2);
int16_t Temp_Reg_s16 = (uint16_t)(pu8[1]<<8) | pu8[0];
if (_sem->take_nonblocking()) {
if(_lps2xh_type == BARO_LPS25H){
_temperature=((float)(Temp_Reg_s16/480)+42.5);
}
if(_lps2xh_type == BARO_LPS22H){
_temperature=(float)(Temp_Reg_s16/100);
}
_sem->give();
}
}
// calculate pressure
void AP_Baro_LPS2XH::_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 *100; // scale for pa
if (_sem->take_nonblocking()) {
_pressure=Pressure_mb;
_sem->give();
}
}

21
libraries/AP_Baro/AP_Baro_LPS25H.h → libraries/AP_Baro/AP_Baro_LPS2XH.h
View File

@ -10,27 +10,44 @@
#define HAL_BARO_LPS25H_I2C_ADDR 0x5D
class AP_Baro_LPS25H : public AP_Baro_Backend
class AP_Baro_LPS2XH : public AP_Baro_Backend
{
public:
AP_Baro_LPS25H(AP_Baro &baro, AP_HAL::OwnPtr<AP_HAL::Device> dev);
enum LPS2XH_TYPE {
BARO_LPS22H = 0,
BARO_LPS25H = 1,
};
AP_Baro_LPS2XH(AP_Baro &baro, AP_HAL::OwnPtr<AP_HAL::Device> dev);
/* AP_Baro public interface: */
void update();
static AP_Baro_Backend *probe(AP_Baro &baro, AP_HAL::OwnPtr<AP_HAL::Device> dev);
private:
virtual ~AP_Baro_LPS2XH(void) {};
bool _init(void);
void _timer(void);
void _update_temperature(void);
void _update_pressure(void);
bool _check_whoami(void);
AP_HAL::OwnPtr<AP_HAL::Device> _dev;
bool _has_sample;
uint8_t _instance;
float _pressure;
float _temperature;
uint32_t CallTime = 0;
enum LPS2XH_TYPE _lps2xh_type;
// WHOAMI values
#define LPS22HB_WHOAMI 0xB1
#define LPS25HB_WHOAMI 0xBD
};