Daniel Agar
5 years ago
committed by
GitHub
6 changed files with 463 additions and 485 deletions
@ -0,0 +1,177 @@ |
|||||||
|
/****************************************************************************
|
||||||
|
* |
||||||
|
* Copyright (c) 2012-2019 PX4 Development Team. All rights reserved. |
||||||
|
* |
||||||
|
* Redistribution and use in source and binary forms, with or without |
||||||
|
* modification, are permitted provided that the following conditions |
||||||
|
* are met: |
||||||
|
* |
||||||
|
* 1. Redistributions of source code must retain the above copyright |
||||||
|
* notice, this list of conditions and the following disclaimer. |
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright |
||||||
|
* notice, this list of conditions and the following disclaimer in |
||||||
|
* the documentation and/or other materials provided with the |
||||||
|
* distribution. |
||||||
|
* 3. Neither the name PX4 nor the names of its contributors may be |
||||||
|
* used to endorse or promote products derived from this software |
||||||
|
* without specific prior written permission. |
||||||
|
* |
||||||
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||||||
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||||||
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||||||
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||||||
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||||||
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||||||
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
||||||
|
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
||||||
|
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||||||
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||||||
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||||
|
* POSSIBILITY OF SUCH DAMAGE. |
||||||
|
* |
||||||
|
****************************************************************************/ |
||||||
|
|
||||||
|
#pragma once |
||||||
|
|
||||||
|
#include <drivers/device/i2c.h> |
||||||
|
#include <drivers/device/device.h> |
||||||
|
#include <drivers/device/ringbuffer.h> |
||||||
|
#include <drivers/device/spi.h> |
||||||
|
#include <drivers/drv_baro.h> |
||||||
|
#include <lib/cdev/CDev.hpp> |
||||||
|
#include <lib/perf/perf_counter.h> |
||||||
|
#include <px4_platform_common/getopt.h> |
||||||
|
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp> |
||||||
|
#include <systemlib/err.h> |
||||||
|
#include <uORB/uORB.h> |
||||||
|
|
||||||
|
#include "ms5611.h" |
||||||
|
|
||||||
|
enum MS56XX_DEVICE_TYPES { |
||||||
|
MS56XX_DEVICE = 0, |
||||||
|
MS5611_DEVICE = 5611, |
||||||
|
MS5607_DEVICE = 5607, |
||||||
|
}; |
||||||
|
|
||||||
|
/* helper macro for handling report buffer indices */ |
||||||
|
#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0) |
||||||
|
|
||||||
|
/* helper macro for arithmetic - returns the square of the argument */ |
||||||
|
#define POW2(_x) ((_x) * (_x)) |
||||||
|
|
||||||
|
/*
|
||||||
|
* MS5611/MS5607 internal constants and data structures. |
||||||
|
*/ |
||||||
|
#define ADDR_CMD_CONVERT_D1_OSR256 0x40 /* write to this address to start pressure conversion */ |
||||||
|
#define ADDR_CMD_CONVERT_D1_OSR512 0x42 /* write to this address to start pressure conversion */ |
||||||
|
#define ADDR_CMD_CONVERT_D1_OSR1024 0x44 /* write to this address to start pressure conversion */ |
||||||
|
#define ADDR_CMD_CONVERT_D1_OSR2048 0x46 /* write to this address to start pressure conversion */ |
||||||
|
#define ADDR_CMD_CONVERT_D1_OSR4096 0x48 /* write to this address to start pressure conversion */ |
||||||
|
#define ADDR_CMD_CONVERT_D2_OSR256 0x50 /* write to this address to start temperature conversion */ |
||||||
|
#define ADDR_CMD_CONVERT_D2_OSR512 0x52 /* write to this address to start temperature conversion */ |
||||||
|
#define ADDR_CMD_CONVERT_D2_OSR1024 0x54 /* write to this address to start temperature conversion */ |
||||||
|
#define ADDR_CMD_CONVERT_D2_OSR2048 0x56 /* write to this address to start temperature conversion */ |
||||||
|
#define ADDR_CMD_CONVERT_D2_OSR4096 0x58 /* write to this address to start temperature conversion */ |
||||||
|
|
||||||
|
/*
|
||||||
|
use an OSR of 1024 to reduce the self-heating effect of the |
||||||
|
sensor. Information from MS tells us that some individual sensors |
||||||
|
are quite sensitive to this effect and that reducing the OSR can |
||||||
|
make a big difference |
||||||
|
*/ |
||||||
|
#define ADDR_CMD_CONVERT_D1 ADDR_CMD_CONVERT_D1_OSR1024 |
||||||
|
#define ADDR_CMD_CONVERT_D2 ADDR_CMD_CONVERT_D2_OSR1024 |
||||||
|
|
||||||
|
/*
|
||||||
|
* Maximum internal conversion time for OSR 1024 is 2.28 ms. We set an update |
||||||
|
* rate of 100Hz which is be very safe not to read the ADC before the |
||||||
|
* conversion finished |
||||||
|
*/ |
||||||
|
#define MS5611_CONVERSION_INTERVAL 10000 /* microseconds */ |
||||||
|
#define MS5611_MEASUREMENT_RATIO 3 /* pressure measurements per temperature measurement */ |
||||||
|
#define MS5611_BARO_DEVICE_PATH_EXT "/dev/ms5611_ext" |
||||||
|
#define MS5611_BARO_DEVICE_PATH_INT "/dev/ms5611_int" |
||||||
|
|
||||||
|
class MS5611 : public cdev::CDev, public px4::ScheduledWorkItem |
||||||
|
{ |
||||||
|
public: |
||||||
|
MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char *path, enum MS56XX_DEVICE_TYPES device_type); |
||||||
|
~MS5611(); |
||||||
|
|
||||||
|
virtual int init(); |
||||||
|
|
||||||
|
virtual ssize_t read(cdev::file_t *filp, char *buffer, size_t buflen); |
||||||
|
virtual int ioctl(cdev::file_t *filp, int cmd, unsigned long arg); |
||||||
|
|
||||||
|
/**
|
||||||
|
* Diagnostics - print some basic information about the driver. |
||||||
|
*/ |
||||||
|
void print_info(); |
||||||
|
|
||||||
|
protected: |
||||||
|
device::Device *_interface; |
||||||
|
|
||||||
|
ms5611::prom_s _prom; |
||||||
|
|
||||||
|
unsigned _measure_interval{0}; |
||||||
|
|
||||||
|
ringbuffer::RingBuffer *_reports; |
||||||
|
enum MS56XX_DEVICE_TYPES _device_type; |
||||||
|
bool _collect_phase; |
||||||
|
unsigned _measure_phase; |
||||||
|
|
||||||
|
/* intermediate temperature values per MS5611/MS5607 datasheet */ |
||||||
|
int32_t _TEMP; |
||||||
|
int64_t _OFF; |
||||||
|
int64_t _SENS; |
||||||
|
float _P; |
||||||
|
float _T; |
||||||
|
|
||||||
|
orb_advert_t _baro_topic; |
||||||
|
int _orb_class_instance; |
||||||
|
int _class_instance; |
||||||
|
|
||||||
|
perf_counter_t _sample_perf; |
||||||
|
perf_counter_t _measure_perf; |
||||||
|
perf_counter_t _comms_errors; |
||||||
|
|
||||||
|
/**
|
||||||
|
* Initialize the automatic measurement state machine and start it. |
||||||
|
* |
||||||
|
* @note This function is called at open and error time. It might make sense |
||||||
|
* to make it more aggressive about resetting the bus in case of errors. |
||||||
|
*/ |
||||||
|
void start(); |
||||||
|
|
||||||
|
/**
|
||||||
|
* Stop the automatic measurement state machine. |
||||||
|
*/ |
||||||
|
void stop(); |
||||||
|
|
||||||
|
/**
|
||||||
|
* Perform a poll cycle; collect from the previous measurement |
||||||
|
* and start a new one. |
||||||
|
* |
||||||
|
* This is the heart of the measurement state machine. This function |
||||||
|
* alternately starts a measurement, or collects the data from the |
||||||
|
* previous measurement. |
||||||
|
* |
||||||
|
* When the interval between measurements is greater than the minimum |
||||||
|
* measurement interval, a gap is inserted between collection |
||||||
|
* and measurement to provide the most recent measurement possible |
||||||
|
* at the next interval. |
||||||
|
*/ |
||||||
|
void Run() override; |
||||||
|
|
||||||
|
/**
|
||||||
|
* Issue a measurement command for the current state. |
||||||
|
* |
||||||
|
* @return OK if the measurement command was successful. |
||||||
|
*/ |
||||||
|
virtual int measure(); |
||||||
|
|
||||||
|
/**
|
||||||
|
* Collect the result of the most recent measurement. |
||||||
|
*/ |
||||||
|
virtual int collect(); |
||||||
|
}; |
||||||
@ -0,0 +1,277 @@ |
|||||||
|
/****************************************************************************
|
||||||
|
* |
||||||
|
* Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. |
||||||
|
* |
||||||
|
* Redistribution and use in source and binary forms, with or without |
||||||
|
* modification, are permitted provided that the following conditions |
||||||
|
* are met: |
||||||
|
* |
||||||
|
* 1. Redistributions of source code must retain the above copyright |
||||||
|
* notice, this list of conditions and the following disclaimer. |
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright |
||||||
|
* notice, this list of conditions and the following disclaimer in |
||||||
|
* the documentation and/or other materials provided with the |
||||||
|
* distribution. |
||||||
|
* 3. Neither the name PX4 nor the names of its contributors may be |
||||||
|
* used to endorse or promote products derived from this software |
||||||
|
* without specific prior written permission. |
||||||
|
* |
||||||
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||||||
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||||||
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||||||
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||||||
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||||||
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||||||
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
||||||
|
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
||||||
|
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||||||
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||||||
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||||
|
* POSSIBILITY OF SUCH DAMAGE. |
||||||
|
* |
||||||
|
****************************************************************************/ |
||||||
|
|
||||||
|
/**
|
||||||
|
* @file ms5611.cpp |
||||||
|
* Driver for the MS5611 and MS5607 barometric pressure sensor connected via I2C or SPI. |
||||||
|
*/ |
||||||
|
|
||||||
|
#include "MS5611.hpp" |
||||||
|
#include "ms5611.h" |
||||||
|
|
||||||
|
enum MS5611_BUS { |
||||||
|
MS5611_BUS_ALL = 0, |
||||||
|
MS5611_BUS_I2C_INTERNAL, |
||||||
|
MS5611_BUS_I2C_EXTERNAL, |
||||||
|
MS5611_BUS_SPI_INTERNAL, |
||||||
|
MS5611_BUS_SPI_EXTERNAL |
||||||
|
}; |
||||||
|
|
||||||
|
/*
|
||||||
|
* Driver 'main' command. |
||||||
|
*/ |
||||||
|
extern "C" __EXPORT int ms5611_main(int argc, char *argv[]); |
||||||
|
|
||||||
|
/**
|
||||||
|
* Local functions in support of the shell command. |
||||||
|
*/ |
||||||
|
namespace ms5611 |
||||||
|
{ |
||||||
|
|
||||||
|
/*
|
||||||
|
list of supported bus configurations |
||||||
|
*/ |
||||||
|
struct ms5611_bus_option { |
||||||
|
enum MS5611_BUS busid; |
||||||
|
const char *devpath; |
||||||
|
MS5611_constructor interface_constructor; |
||||||
|
uint8_t busnum; |
||||||
|
MS5611 *dev; |
||||||
|
} bus_options[] = { |
||||||
|
#if defined(PX4_SPIDEV_EXT_BARO) && defined(PX4_SPI_BUS_EXT) |
||||||
|
{ MS5611_BUS_SPI_EXTERNAL, "/dev/ms5611_spi_ext", &MS5611_spi_interface, PX4_SPI_BUS_EXT, NULL }, |
||||||
|
#endif |
||||||
|
#ifdef PX4_SPIDEV_BARO |
||||||
|
{ MS5611_BUS_SPI_INTERNAL, "/dev/ms5611_spi_int", &MS5611_spi_interface, PX4_SPI_BUS_BARO, NULL }, |
||||||
|
#endif |
||||||
|
#ifdef PX4_I2C_BUS_ONBOARD |
||||||
|
{ MS5611_BUS_I2C_INTERNAL, "/dev/ms5611_int", &MS5611_i2c_interface, PX4_I2C_BUS_ONBOARD, NULL }, |
||||||
|
#endif |
||||||
|
#ifdef PX4_I2C_BUS_EXPANSION |
||||||
|
{ MS5611_BUS_I2C_EXTERNAL, "/dev/ms5611_ext", &MS5611_i2c_interface, PX4_I2C_BUS_EXPANSION, NULL }, |
||||||
|
#endif |
||||||
|
#ifdef PX4_I2C_BUS_EXPANSION1 |
||||||
|
{ MS5611_BUS_I2C_EXTERNAL, "/dev/ms5611_ext1", &MS5611_i2c_interface, PX4_I2C_BUS_EXPANSION1, NULL }, |
||||||
|
#endif |
||||||
|
#ifdef PX4_I2C_BUS_EXPANSION2 |
||||||
|
{ MS5611_BUS_I2C_EXTERNAL, "/dev/ms5611_ext2", &MS5611_i2c_interface, PX4_I2C_BUS_EXPANSION2, NULL }, |
||||||
|
#endif |
||||||
|
}; |
||||||
|
#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0])) |
||||||
|
|
||||||
|
/**
|
||||||
|
* Start the driver. |
||||||
|
*/ |
||||||
|
static bool |
||||||
|
start_bus(struct ms5611_bus_option &bus, enum MS56XX_DEVICE_TYPES device_type) |
||||||
|
{ |
||||||
|
if (bus.dev != nullptr) { |
||||||
|
PX4_ERR("bus option already started"); |
||||||
|
return false; |
||||||
|
} |
||||||
|
|
||||||
|
prom_u prom_buf; |
||||||
|
device::Device *interface = bus.interface_constructor(prom_buf, bus.busnum); |
||||||
|
|
||||||
|
if (interface->init() != OK) { |
||||||
|
delete interface; |
||||||
|
PX4_WARN("no device on bus %u", (unsigned)bus.busid); |
||||||
|
return false; |
||||||
|
} |
||||||
|
|
||||||
|
bus.dev = new MS5611(interface, prom_buf, bus.devpath, device_type); |
||||||
|
|
||||||
|
if (bus.dev != nullptr && OK != bus.dev->init()) { |
||||||
|
delete bus.dev; |
||||||
|
bus.dev = NULL; |
||||||
|
return false; |
||||||
|
} |
||||||
|
|
||||||
|
int fd = px4_open(bus.devpath, O_RDONLY); |
||||||
|
|
||||||
|
/* set the poll rate to default, starts automatic data collection */ |
||||||
|
if (fd == -1) { |
||||||
|
PX4_ERR("can't open baro device"); |
||||||
|
return false; |
||||||
|
} |
||||||
|
|
||||||
|
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { |
||||||
|
px4_close(fd); |
||||||
|
PX4_ERR("failed setting default poll rate"); |
||||||
|
return false; |
||||||
|
} |
||||||
|
|
||||||
|
px4_close(fd); |
||||||
|
return true; |
||||||
|
} |
||||||
|
|
||||||
|
/**
|
||||||
|
* Start the driver. |
||||||
|
* |
||||||
|
* This function call only returns once the driver |
||||||
|
* is either successfully up and running or failed to start. |
||||||
|
*/ |
||||||
|
static int |
||||||
|
start(enum MS5611_BUS busid, enum MS56XX_DEVICE_TYPES device_type) |
||||||
|
{ |
||||||
|
uint8_t i; |
||||||
|
bool started = false; |
||||||
|
|
||||||
|
for (i = 0; i < NUM_BUS_OPTIONS; i++) { |
||||||
|
if (busid == MS5611_BUS_ALL && bus_options[i].dev != NULL) { |
||||||
|
// this device is already started
|
||||||
|
continue; |
||||||
|
} |
||||||
|
|
||||||
|
if (busid != MS5611_BUS_ALL && bus_options[i].busid != busid) { |
||||||
|
// not the one that is asked for
|
||||||
|
continue; |
||||||
|
} |
||||||
|
|
||||||
|
started = started | start_bus(bus_options[i], device_type); |
||||||
|
} |
||||||
|
|
||||||
|
if (!started) { |
||||||
|
return PX4_ERROR; |
||||||
|
} |
||||||
|
|
||||||
|
// one or more drivers started OK
|
||||||
|
return PX4_OK; |
||||||
|
} |
||||||
|
|
||||||
|
static int |
||||||
|
info() |
||||||
|
{ |
||||||
|
for (uint8_t i = 0; i < NUM_BUS_OPTIONS; i++) { |
||||||
|
struct ms5611_bus_option &bus = bus_options[i]; |
||||||
|
|
||||||
|
if (bus.dev != nullptr) { |
||||||
|
PX4_INFO("%s", bus.devpath); |
||||||
|
bus.dev->print_info(); |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
return 0; |
||||||
|
} |
||||||
|
|
||||||
|
static int |
||||||
|
usage() |
||||||
|
{ |
||||||
|
PX4_INFO("missing command: try 'start', 'info',"); |
||||||
|
PX4_INFO("options:"); |
||||||
|
PX4_INFO(" -X (external I2C bus)"); |
||||||
|
PX4_INFO(" -I (intternal I2C bus)"); |
||||||
|
PX4_INFO(" -S (external SPI bus)"); |
||||||
|
PX4_INFO(" -s (internal SPI bus)"); |
||||||
|
PX4_INFO(" -T 5611|5607 (default 5611)"); |
||||||
|
PX4_INFO(" -T 0 (autodetect version)"); |
||||||
|
|
||||||
|
return 0; |
||||||
|
} |
||||||
|
|
||||||
|
} // namespace
|
||||||
|
|
||||||
|
int |
||||||
|
ms5611_main(int argc, char *argv[]) |
||||||
|
{ |
||||||
|
enum MS5611_BUS busid = MS5611_BUS_ALL; |
||||||
|
enum MS56XX_DEVICE_TYPES device_type = MS5611_DEVICE; |
||||||
|
int ch; |
||||||
|
int myoptind = 1; |
||||||
|
const char *myoptarg = NULL; |
||||||
|
|
||||||
|
/* jump over start/off/etc and look at options first */ |
||||||
|
while ((ch = px4_getopt(argc, argv, "T:XISs", &myoptind, &myoptarg)) != EOF) { |
||||||
|
switch (ch) { |
||||||
|
case 'X': |
||||||
|
busid = MS5611_BUS_I2C_EXTERNAL; |
||||||
|
break; |
||||||
|
|
||||||
|
case 'I': |
||||||
|
busid = MS5611_BUS_I2C_INTERNAL; |
||||||
|
break; |
||||||
|
|
||||||
|
case 'S': |
||||||
|
busid = MS5611_BUS_SPI_EXTERNAL; |
||||||
|
break; |
||||||
|
|
||||||
|
case 's': |
||||||
|
busid = MS5611_BUS_SPI_INTERNAL; |
||||||
|
break; |
||||||
|
|
||||||
|
case 'T': { |
||||||
|
int val = atoi(myoptarg); |
||||||
|
|
||||||
|
if (val == 5611) { |
||||||
|
device_type = MS5611_DEVICE; |
||||||
|
break; |
||||||
|
|
||||||
|
} else if (val == 5607) { |
||||||
|
device_type = MS5607_DEVICE; |
||||||
|
break; |
||||||
|
|
||||||
|
} else if (val == 0) { |
||||||
|
device_type = MS56XX_DEVICE; |
||||||
|
break; |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
/* FALLTHROUGH */ |
||||||
|
|
||||||
|
default: |
||||||
|
return ms5611::usage(); |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
if (myoptind >= argc) { |
||||||
|
return ms5611::usage(); |
||||||
|
} |
||||||
|
|
||||||
|
const char *verb = argv[myoptind]; |
||||||
|
|
||||||
|
/*
|
||||||
|
* Start/load the driver. |
||||||
|
*/ |
||||||
|
if (!strcmp(verb, "start")) { |
||||||
|
return ms5611::start(busid, device_type); |
||||||
|
} |
||||||
|
|
||||||
|
/*
|
||||||
|
* Print driver information. |
||||||
|
*/ |
||||||
|
if (!strcmp(verb, "info")) { |
||||||
|
return ms5611::info(); |
||||||
|
} |
||||||
|
|
||||||
|
return ms5611::usage(); |
||||||
|
} |
||||||
Loading…
Reference in new issue