zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_Baro: support mini-pix using LPS25H SPI barometer 

allow LPS22H and LPS25H to share a driver
master
ljwang 7 years ago committed by Andrew Tridgell
parent
348a757cc6
commit
88effef51a
4 changed files with 209 additions and 139 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 7
      libraries/AP_Baro/AP_Baro.cpp
  2. 135
      libraries/AP_Baro/AP_Baro_LPS25H.cpp
  3. 185
      libraries/AP_Baro/AP_Baro_LPS2XH.cpp
  4. 21
      libraries/AP_Baro/AP_Baro_LPS2XH.h

7
libraries/AP_Baro/AP_Baro.cpp
Unescape View File

@ -34,8 +34,8 @@ @@ -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) @@ -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) @@ -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
Unescape View File

@ -1,135 +0,0 @@ @@ -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
Unescape View File

@ -0,0 +1,185 @@ @@ -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
Unescape View File

@ -10,27 +10,44 @@ @@ -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
};
Write Preview
Loading…
Cancel
Save