Daniel Agar
5 years ago
committed by
GitHub
6 changed files with 463 additions and 485 deletions
@ -0,0 +1,177 @@
@@ -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 @@
@@ -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