mirror of
https://gitee.com/xiaohuolufeihua/bizhang_-obav.git
synced 2026-05-22 01:12:31 +00:00
MS5611 code style fix
This commit is contained in:
@@ -83,4 +83,4 @@ extern bool crc4(uint16_t *n_prom);
|
||||
extern device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf, uint8_t busnum) __attribute__((weak));
|
||||
extern device::Device *MS5611_i2c_interface(ms5611::prom_u &prom_buf, uint8_t busnum) __attribute__((weak));
|
||||
extern device::Device *MS5611_sim_interface(ms5611::prom_u &prom_buf, uint8_t busnum) __attribute__((weak));
|
||||
typedef device::Device* (*MS5611_constructor)(ms5611::prom_u &prom_buf, uint8_t busnum);
|
||||
typedef device::Device *(*MS5611_constructor)(ms5611::prom_u &prom_buf, uint8_t busnum);
|
||||
|
||||
@@ -31,7 +31,7 @@
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
/**
|
||||
* @file ms5611_i2c.cpp
|
||||
*
|
||||
* I2C interface for MS5611
|
||||
@@ -118,11 +118,11 @@ MS5611_i2c_interface(ms5611::prom_u &prom_buf, uint8_t busnum)
|
||||
MS5611_I2C::MS5611_I2C(uint8_t bus, ms5611::prom_u &prom) :
|
||||
I2C("MS5611_I2C",
|
||||
#ifdef __PX4_NUTTX
|
||||
nullptr, bus, 0, 400000
|
||||
nullptr, bus, 0, 400000
|
||||
#else
|
||||
"/dev/MS5611_I2C", bus, 0
|
||||
"/dev/MS5611_I2C", bus, 0
|
||||
#endif
|
||||
),
|
||||
),
|
||||
_prom(prom)
|
||||
{
|
||||
}
|
||||
@@ -150,6 +150,7 @@ MS5611_I2C::read(device::file_t *handlep, char *data, size_t buflen)
|
||||
/* read the most recent measurement */
|
||||
uint8_t cmd = 0;
|
||||
int ret = transfer(&cmd, 1, &buf[0], 3);
|
||||
|
||||
if (ret == PX4_OK) {
|
||||
/* fetch the raw value */
|
||||
cvt->b[0] = buf[2];
|
||||
@@ -208,12 +209,14 @@ MS5611_I2C::_probe_address(uint8_t address)
|
||||
set_address(address);
|
||||
|
||||
/* send reset command */
|
||||
if (PX4_OK != _reset())
|
||||
if (PX4_OK != _reset()) {
|
||||
return -EIO;
|
||||
}
|
||||
|
||||
/* read PROM */
|
||||
if (PX4_OK != _read_prom())
|
||||
if (PX4_OK != _read_prom()) {
|
||||
return -EIO;
|
||||
}
|
||||
|
||||
return PX4_OK;
|
||||
}
|
||||
@@ -267,8 +270,9 @@ MS5611_I2C::_read_prom()
|
||||
for (int i = 0; i < 8; i++) {
|
||||
uint8_t cmd = ADDR_PROM_SETUP + (i * 2);
|
||||
|
||||
if (PX4_OK != transfer(&cmd, 1, &prom_buf[0], 2))
|
||||
if (PX4_OK != transfer(&cmd, 1, &prom_buf[0], 2)) {
|
||||
break;
|
||||
}
|
||||
|
||||
/* assemble 16 bit value and convert from big endian (sensor) to little endian (MCU) */
|
||||
cvt.b[0] = prom_buf[1];
|
||||
|
||||
@@ -106,7 +106,7 @@ static const int ERROR = -1;
|
||||
class MS5611 : public device::CDev
|
||||
{
|
||||
public:
|
||||
MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char* path);
|
||||
MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char *path);
|
||||
~MS5611();
|
||||
|
||||
virtual int init();
|
||||
@@ -214,7 +214,7 @@ protected:
|
||||
*/
|
||||
extern "C" __EXPORT int ms5611_main(int argc, char *argv[]);
|
||||
|
||||
MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char* path) :
|
||||
MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char *path) :
|
||||
CDev("MS5611", path),
|
||||
_interface(interface),
|
||||
_prom(prom_buf.s),
|
||||
@@ -243,12 +243,14 @@ MS5611::~MS5611()
|
||||
/* make sure we are truly inactive */
|
||||
stop_cycle();
|
||||
|
||||
if (_class_instance != -1)
|
||||
if (_class_instance != -1) {
|
||||
unregister_class_devname(get_devname(), _class_instance);
|
||||
}
|
||||
|
||||
/* free any existing reports */
|
||||
if (_reports != nullptr)
|
||||
if (_reports != nullptr) {
|
||||
delete _reports;
|
||||
}
|
||||
|
||||
// free perf counters
|
||||
perf_free(_sample_perf);
|
||||
@@ -265,6 +267,7 @@ MS5611::init()
|
||||
int ret;
|
||||
|
||||
ret = CDev::init();
|
||||
|
||||
if (ret != OK) {
|
||||
DEVICE_DEBUG("CDev init failed");
|
||||
goto out;
|
||||
@@ -342,8 +345,9 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen)
|
||||
int ret = 0;
|
||||
|
||||
/* buffer must be large enough */
|
||||
if (count < 1)
|
||||
if (count < 1) {
|
||||
return -ENOSPC;
|
||||
}
|
||||
|
||||
/* if automatic measurement is enabled */
|
||||
if (_measure_ticks > 0) {
|
||||
@@ -396,8 +400,9 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen)
|
||||
}
|
||||
|
||||
/* state machine will have generated a report, copy it out */
|
||||
if (_reports->get(brp))
|
||||
if (_reports->get(brp)) {
|
||||
ret = sizeof(*brp);
|
||||
}
|
||||
|
||||
} while (0);
|
||||
|
||||
@@ -435,8 +440,9 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
|
||||
_measure_ticks = USEC2TICK(MS5611_CONVERSION_INTERVAL);
|
||||
|
||||
/* if we need to start the poll state machine, do it */
|
||||
if (want_start)
|
||||
if (want_start) {
|
||||
start_cycle();
|
||||
}
|
||||
|
||||
return OK;
|
||||
}
|
||||
@@ -450,15 +456,17 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
|
||||
unsigned ticks = USEC2TICK(1000000 / arg);
|
||||
|
||||
/* check against maximum rate */
|
||||
if (ticks < USEC2TICK(MS5611_CONVERSION_INTERVAL))
|
||||
if (ticks < USEC2TICK(MS5611_CONVERSION_INTERVAL)) {
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
/* update interval for next measurement */
|
||||
_measure_ticks = ticks;
|
||||
|
||||
/* if we need to start the poll state machine, do it */
|
||||
if (want_start)
|
||||
if (want_start) {
|
||||
start_cycle();
|
||||
}
|
||||
|
||||
return OK;
|
||||
}
|
||||
@@ -466,21 +474,25 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
|
||||
}
|
||||
|
||||
case SENSORIOCGPOLLRATE:
|
||||
if (_measure_ticks == 0)
|
||||
if (_measure_ticks == 0) {
|
||||
return SENSOR_POLLRATE_MANUAL;
|
||||
}
|
||||
|
||||
return (1000 / _measure_ticks);
|
||||
|
||||
case SENSORIOCSQUEUEDEPTH: {
|
||||
/* lower bound is mandatory, upper bound is a sanity check */
|
||||
if ((arg < 1) || (arg > 100))
|
||||
if ((arg < 1) || (arg > 100)) {
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
irqstate_t flags = irqsave();
|
||||
|
||||
if (!_reports->resize(arg)) {
|
||||
irqrestore(flags);
|
||||
return -ENOMEM;
|
||||
}
|
||||
|
||||
irqrestore(flags);
|
||||
return OK;
|
||||
}
|
||||
@@ -498,8 +510,9 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
|
||||
case BAROIOCSMSLPRESSURE:
|
||||
|
||||
/* range-check for sanity */
|
||||
if ((arg < 80000) || (arg > 120000))
|
||||
if ((arg < 80000) || (arg > 120000)) {
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
_msl_pressure = arg;
|
||||
return OK;
|
||||
@@ -554,6 +567,7 @@ MS5611::cycle()
|
||||
|
||||
/* perform collection */
|
||||
ret = collect();
|
||||
|
||||
if (ret != OK) {
|
||||
if (ret == -6) {
|
||||
/*
|
||||
@@ -564,6 +578,7 @@ MS5611::cycle()
|
||||
} else {
|
||||
//DEVICE_LOG("collection error %d", ret);
|
||||
}
|
||||
|
||||
/* issue a reset command to the sensor */
|
||||
_interface->ioctl(IOCTL_RESET, dummy);
|
||||
/* reset the collection state machine and try again - we need
|
||||
@@ -598,6 +613,7 @@ MS5611::cycle()
|
||||
|
||||
/* measurement phase */
|
||||
ret = measure();
|
||||
|
||||
if (ret != OK) {
|
||||
/* issue a reset command to the sensor */
|
||||
_interface->ioctl(IOCTL_RESET, dummy);
|
||||
@@ -633,8 +649,10 @@ MS5611::measure()
|
||||
* Send the command to begin measuring.
|
||||
*/
|
||||
ret = _interface->ioctl(IOCTL_MEASURE, addr);
|
||||
if (OK != ret)
|
||||
|
||||
if (OK != ret) {
|
||||
perf_count(_comms_errors);
|
||||
}
|
||||
|
||||
perf_end(_measure_perf);
|
||||
|
||||
@@ -656,6 +674,7 @@ MS5611::collect()
|
||||
|
||||
/* read the most recent measurement - read offset/size are hardcoded in the interface */
|
||||
ret = _interface->read(0, (void *)&raw, 0);
|
||||
|
||||
if (ret < 0) {
|
||||
perf_count(_comms_errors);
|
||||
perf_end(_sample_perf);
|
||||
@@ -883,11 +902,13 @@ crc4(uint16_t *n_prom)
|
||||
bool
|
||||
start_bus(struct ms5611_bus_option &bus)
|
||||
{
|
||||
if (bus.dev != nullptr)
|
||||
errx(1,"bus option already started");
|
||||
if (bus.dev != nullptr) {
|
||||
errx(1, "bus option already started");
|
||||
}
|
||||
|
||||
prom_u prom_buf;
|
||||
device::Device *interface = bus.interface_constructor(prom_buf, bus.busnum);
|
||||
|
||||
if (interface->init() != OK) {
|
||||
delete interface;
|
||||
warnx("no device on bus %u", (unsigned)bus.busid);
|
||||
@@ -895,6 +916,7 @@ start_bus(struct ms5611_bus_option &bus)
|
||||
}
|
||||
|
||||
bus.dev = new MS5611(interface, prom_buf, bus.devpath);
|
||||
|
||||
if (bus.dev != nullptr && OK != bus.dev->init()) {
|
||||
delete bus.dev;
|
||||
bus.dev = NULL;
|
||||
@@ -907,6 +929,7 @@ start_bus(struct ms5611_bus_option &bus)
|
||||
if (fd == -1) {
|
||||
errx(1, "can't open baro device");
|
||||
}
|
||||
|
||||
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
|
||||
close(fd);
|
||||
errx(1, "failed setting default poll rate");
|
||||
@@ -929,20 +952,23 @@ start(enum MS5611_BUS busid)
|
||||
uint8_t i;
|
||||
bool started = false;
|
||||
|
||||
for (i=0; i<NUM_BUS_OPTIONS; i++) {
|
||||
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 |= start_bus(bus_options[i]);
|
||||
}
|
||||
|
||||
if (!started)
|
||||
if (!started) {
|
||||
errx(1, "driver start failed");
|
||||
}
|
||||
|
||||
// one or more drivers started OK
|
||||
exit(0);
|
||||
@@ -954,13 +980,14 @@ start(enum MS5611_BUS busid)
|
||||
*/
|
||||
struct ms5611_bus_option &find_bus(enum MS5611_BUS busid)
|
||||
{
|
||||
for (uint8_t i=0; i<NUM_BUS_OPTIONS; i++) {
|
||||
for (uint8_t i = 0; i < NUM_BUS_OPTIONS; i++) {
|
||||
if ((busid == MS5611_BUS_ALL ||
|
||||
busid == bus_options[i].busid) && bus_options[i].dev != NULL) {
|
||||
return bus_options[i];
|
||||
}
|
||||
}
|
||||
errx(1,"bus %u not started", (unsigned)busid);
|
||||
|
||||
errx(1, "bus %u not started", (unsigned)busid);
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -979,14 +1006,17 @@ test(enum MS5611_BUS busid)
|
||||
int fd;
|
||||
|
||||
fd = open(bus.devpath, O_RDONLY);
|
||||
if (fd < 0)
|
||||
|
||||
if (fd < 0) {
|
||||
err(1, "open failed (try 'ms5611 start' if the driver is not running)");
|
||||
}
|
||||
|
||||
/* do a simple demand read */
|
||||
sz = read(fd, &report, sizeof(report));
|
||||
|
||||
if (sz != sizeof(report))
|
||||
if (sz != sizeof(report)) {
|
||||
err(1, "immediate read failed");
|
||||
}
|
||||
|
||||
warnx("single read");
|
||||
warnx("pressure: %10.4f", (double)report.pressure);
|
||||
@@ -995,12 +1025,14 @@ test(enum MS5611_BUS busid)
|
||||
warnx("time: %lld", report.timestamp);
|
||||
|
||||
/* set the queue depth to 10 */
|
||||
if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10))
|
||||
if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10)) {
|
||||
errx(1, "failed to set queue depth");
|
||||
}
|
||||
|
||||
/* start the sensor polling at 2Hz */
|
||||
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2))
|
||||
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
|
||||
errx(1, "failed to set 2Hz poll rate");
|
||||
}
|
||||
|
||||
/* read the sensor 5x and report each value */
|
||||
for (unsigned i = 0; i < 5; i++) {
|
||||
@@ -1011,14 +1043,16 @@ test(enum MS5611_BUS busid)
|
||||
fds.events = POLLIN;
|
||||
ret = poll(&fds, 1, 2000);
|
||||
|
||||
if (ret != 1)
|
||||
if (ret != 1) {
|
||||
errx(1, "timed out waiting for sensor data");
|
||||
}
|
||||
|
||||
/* now go get it */
|
||||
sz = read(fd, &report, sizeof(report));
|
||||
|
||||
if (sz != sizeof(report))
|
||||
if (sz != sizeof(report)) {
|
||||
err(1, "periodic read failed");
|
||||
}
|
||||
|
||||
warnx("periodic read %u", i);
|
||||
warnx("pressure: %10.4f", (double)report.pressure);
|
||||
@@ -1041,14 +1075,18 @@ reset(enum MS5611_BUS busid)
|
||||
int fd;
|
||||
|
||||
fd = open(bus.devpath, O_RDONLY);
|
||||
if (fd < 0)
|
||||
|
||||
if (fd < 0) {
|
||||
err(1, "failed ");
|
||||
}
|
||||
|
||||
if (ioctl(fd, SENSORIOCRESET, 0) < 0)
|
||||
if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
|
||||
err(1, "driver reset failed");
|
||||
}
|
||||
|
||||
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
|
||||
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
|
||||
err(1, "driver poll restart failed");
|
||||
}
|
||||
|
||||
exit(0);
|
||||
}
|
||||
@@ -1059,13 +1097,15 @@ reset(enum MS5611_BUS busid)
|
||||
void
|
||||
info()
|
||||
{
|
||||
for (uint8_t i=0; i<NUM_BUS_OPTIONS; i++) {
|
||||
for (uint8_t i = 0; i < NUM_BUS_OPTIONS; i++) {
|
||||
struct ms5611_bus_option &bus = bus_options[i];
|
||||
|
||||
if (bus.dev != nullptr) {
|
||||
warnx("%s", bus.devpath);
|
||||
bus.dev->print_info();
|
||||
}
|
||||
}
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
@@ -1084,12 +1124,14 @@ calibrate(unsigned altitude, enum MS5611_BUS busid)
|
||||
|
||||
fd = open(bus.devpath, O_RDONLY);
|
||||
|
||||
if (fd < 0)
|
||||
if (fd < 0) {
|
||||
err(1, "open failed (try 'ms5611 start' if the driver is not running)");
|
||||
}
|
||||
|
||||
/* start the sensor polling at max */
|
||||
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX))
|
||||
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX)) {
|
||||
errx(1, "failed to set poll rate");
|
||||
}
|
||||
|
||||
/* average a few measurements */
|
||||
pressure = 0.0f;
|
||||
@@ -1104,14 +1146,16 @@ calibrate(unsigned altitude, enum MS5611_BUS busid)
|
||||
fds.events = POLLIN;
|
||||
ret = poll(&fds, 1, 1000);
|
||||
|
||||
if (ret != 1)
|
||||
if (ret != 1) {
|
||||
errx(1, "timed out waiting for sensor data");
|
||||
}
|
||||
|
||||
/* now go get it */
|
||||
sz = read(fd, &report, sizeof(report));
|
||||
|
||||
if (sz != sizeof(report))
|
||||
if (sz != sizeof(report)) {
|
||||
err(1, "sensor read failed");
|
||||
}
|
||||
|
||||
pressure += report.pressure;
|
||||
}
|
||||
@@ -1134,8 +1178,9 @@ calibrate(unsigned altitude, enum MS5611_BUS busid)
|
||||
/* save as integer Pa */
|
||||
p1 *= 1000.0f;
|
||||
|
||||
if (ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)p1) != OK)
|
||||
if (ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)p1) != OK) {
|
||||
err(1, "BAROIOCSMSLPRESSURE");
|
||||
}
|
||||
|
||||
close(fd);
|
||||
exit(0);
|
||||
@@ -1166,15 +1211,19 @@ ms5611_main(int argc, char *argv[])
|
||||
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;
|
||||
|
||||
default:
|
||||
ms5611::usage();
|
||||
exit(0);
|
||||
@@ -1215,10 +1264,11 @@ ms5611_main(int argc, char *argv[])
|
||||
* Perform MSL pressure calibration given an altitude in metres
|
||||
*/
|
||||
if (!strcmp(verb, "calibrate")) {
|
||||
if (argc < 2)
|
||||
if (argc < 2) {
|
||||
errx(1, "missing altitude");
|
||||
}
|
||||
|
||||
long altitude = strtol(argv[optind+1], nullptr, 10);
|
||||
long altitude = strtol(argv[optind + 1], nullptr, 10);
|
||||
|
||||
ms5611::calibrate(altitude, busid);
|
||||
}
|
||||
|
||||
@@ -105,7 +105,7 @@ static const int ERROR = -1;
|
||||
class MS5611 : public device::VDev
|
||||
{
|
||||
public:
|
||||
MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char* path);
|
||||
MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char *path);
|
||||
~MS5611();
|
||||
|
||||
virtual int init();
|
||||
@@ -211,7 +211,7 @@ protected:
|
||||
*/
|
||||
extern "C" __EXPORT int ms5611_main(int argc, char *argv[]);
|
||||
|
||||
MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char* path) :
|
||||
MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char *path) :
|
||||
VDev("MS5611", path),
|
||||
_interface(interface),
|
||||
_prom(prom_buf.s),
|
||||
@@ -240,12 +240,14 @@ MS5611::~MS5611()
|
||||
/* make sure we are truly inactive */
|
||||
stop_cycle();
|
||||
|
||||
if (_class_instance != -1)
|
||||
if (_class_instance != -1) {
|
||||
unregister_class_devname(get_devname(), _class_instance);
|
||||
}
|
||||
|
||||
/* free any existing reports */
|
||||
if (_reports != nullptr)
|
||||
if (_reports != nullptr) {
|
||||
delete _reports;
|
||||
}
|
||||
|
||||
// free perf counters
|
||||
perf_free(_sample_perf);
|
||||
@@ -263,6 +265,7 @@ MS5611::init()
|
||||
warnx("MS5611::init");
|
||||
|
||||
ret = VDev::init();
|
||||
|
||||
if (ret != OK) {
|
||||
DEVICE_DEBUG("VDev init failed");
|
||||
goto out;
|
||||
@@ -328,6 +331,7 @@ MS5611::init()
|
||||
if (_baro_topic == nullptr) {
|
||||
warnx("failed to create sensor_baro publication");
|
||||
}
|
||||
|
||||
//warnx("sensor_baro publication %ld", _baro_topic);
|
||||
|
||||
} while (0);
|
||||
@@ -344,8 +348,9 @@ MS5611::read(device::file_t *handlep, char *buffer, size_t buflen)
|
||||
int ret = 0;
|
||||
|
||||
/* buffer must be large enough */
|
||||
if (count < 1)
|
||||
if (count < 1) {
|
||||
return -ENOSPC;
|
||||
}
|
||||
|
||||
/* if automatic measurement is enabled */
|
||||
if (_measure_ticks > 0) {
|
||||
@@ -398,8 +403,9 @@ MS5611::read(device::file_t *handlep, char *buffer, size_t buflen)
|
||||
}
|
||||
|
||||
/* state machine will have generated a report, copy it out */
|
||||
if (_reports->get(brp))
|
||||
if (_reports->get(brp)) {
|
||||
ret = sizeof(*brp);
|
||||
}
|
||||
|
||||
} while (0);
|
||||
|
||||
@@ -437,8 +443,9 @@ MS5611::ioctl(device::file_t *handlep, int cmd, unsigned long arg)
|
||||
_measure_ticks = USEC2TICK(MS5611_CONVERSION_INTERVAL);
|
||||
|
||||
/* if we need to start the poll state machine, do it */
|
||||
if (want_start)
|
||||
if (want_start) {
|
||||
start_cycle();
|
||||
}
|
||||
|
||||
return OK;
|
||||
}
|
||||
@@ -452,15 +459,17 @@ MS5611::ioctl(device::file_t *handlep, int cmd, unsigned long arg)
|
||||
unsigned ticks = USEC2TICK(1000000 / arg);
|
||||
|
||||
/* check against maximum rate */
|
||||
if ((unsigned long)ticks < USEC2TICK(MS5611_CONVERSION_INTERVAL))
|
||||
if ((unsigned long)ticks < USEC2TICK(MS5611_CONVERSION_INTERVAL)) {
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
/* update interval for next measurement */
|
||||
_measure_ticks = ticks;
|
||||
|
||||
/* if we need to start the poll state machine, do it */
|
||||
if (want_start)
|
||||
if (want_start) {
|
||||
start_cycle();
|
||||
}
|
||||
|
||||
return OK;
|
||||
}
|
||||
@@ -468,19 +477,22 @@ MS5611::ioctl(device::file_t *handlep, int cmd, unsigned long arg)
|
||||
}
|
||||
|
||||
case SENSORIOCGPOLLRATE:
|
||||
if (_measure_ticks == 0)
|
||||
if (_measure_ticks == 0) {
|
||||
return SENSOR_POLLRATE_MANUAL;
|
||||
}
|
||||
|
||||
return (1000 / _measure_ticks);
|
||||
|
||||
case SENSORIOCSQUEUEDEPTH: {
|
||||
/* lower bound is mandatory, upper bound is a sanity check */
|
||||
if ((arg < 1) || (arg > 100))
|
||||
if ((arg < 1) || (arg > 100)) {
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
if (!_reports->resize(arg)) {
|
||||
return -ENOMEM;
|
||||
}
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
@@ -497,8 +509,9 @@ MS5611::ioctl(device::file_t *handlep, int cmd, unsigned long arg)
|
||||
case BAROIOCSMSLPRESSURE:
|
||||
|
||||
/* range-check for sanity */
|
||||
if ((arg < 80000) || (arg > 120000))
|
||||
if ((arg < 80000) || (arg > 120000)) {
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
_msl_pressure = arg;
|
||||
return OK;
|
||||
@@ -553,6 +566,7 @@ MS5611::cycle()
|
||||
|
||||
/* perform collection */
|
||||
ret = collect();
|
||||
|
||||
if (ret != OK) {
|
||||
if (ret == -6) {
|
||||
/*
|
||||
@@ -563,6 +577,7 @@ MS5611::cycle()
|
||||
} else {
|
||||
//DEVICE_LOG("collection error %d", ret);
|
||||
}
|
||||
|
||||
/* issue a reset command to the sensor */
|
||||
_interface->dev_ioctl(IOCTL_RESET, dummy);
|
||||
/* reset the collection state machine and try again */
|
||||
@@ -594,6 +609,7 @@ MS5611::cycle()
|
||||
|
||||
/* measurement phase */
|
||||
ret = measure();
|
||||
|
||||
if (ret != OK) {
|
||||
//DEVICE_LOG("measure error %d", ret);
|
||||
/* issue a reset command to the sensor */
|
||||
@@ -630,8 +646,10 @@ MS5611::measure()
|
||||
* Send the command to begin measuring.
|
||||
*/
|
||||
ret = _interface->dev_ioctl(IOCTL_MEASURE, addr);
|
||||
if (OK != ret)
|
||||
|
||||
if (OK != ret) {
|
||||
perf_count(_comms_errors);
|
||||
}
|
||||
|
||||
perf_end(_measure_perf);
|
||||
|
||||
@@ -653,6 +671,7 @@ MS5611::collect()
|
||||
|
||||
/* read the most recent measurement - read offset/size are hardcoded in the interface */
|
||||
ret = _interface->dev_read(0, (void *)&raw, 0);
|
||||
|
||||
if (ret < 0) {
|
||||
perf_count(_comms_errors);
|
||||
perf_end(_sample_perf);
|
||||
@@ -745,8 +764,8 @@ MS5611::collect()
|
||||
if (_baro_topic != (orb_advert_t)(-1)) {
|
||||
/* publish it */
|
||||
orb_publish(ORB_ID(sensor_baro), _baro_topic, &report);
|
||||
}
|
||||
else {
|
||||
|
||||
} else {
|
||||
printf("MS5611::collect _baro_topic not initialized\n");
|
||||
}
|
||||
}
|
||||
@@ -897,6 +916,7 @@ start_bus(struct ms5611_bus_option &bus)
|
||||
|
||||
prom_u prom_buf;
|
||||
device::Device *interface = bus.interface_constructor(prom_buf, bus.busnum);
|
||||
|
||||
if (interface->init() != OK) {
|
||||
delete interface;
|
||||
warnx("no device on bus %u", (unsigned)bus.busid);
|
||||
@@ -904,6 +924,7 @@ start_bus(struct ms5611_bus_option &bus)
|
||||
}
|
||||
|
||||
bus.dev = new MS5611(interface, prom_buf, bus.devpath);
|
||||
|
||||
if (bus.dev != nullptr && OK != bus.dev->init()) {
|
||||
delete bus.dev;
|
||||
bus.dev = NULL;
|
||||
@@ -918,6 +939,7 @@ start_bus(struct ms5611_bus_option &bus)
|
||||
warnx("can't open baro device");
|
||||
return false;
|
||||
}
|
||||
|
||||
if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
|
||||
px4_close(fd);
|
||||
warnx("failed setting default poll rate");
|
||||
@@ -941,15 +963,17 @@ start(enum MS5611_BUS busid)
|
||||
uint8_t i;
|
||||
bool started = false;
|
||||
|
||||
for (i=0; i<NUM_BUS_OPTIONS; i++) {
|
||||
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 |= start_bus(bus_options[i]);
|
||||
}
|
||||
|
||||
@@ -968,7 +992,7 @@ start(enum MS5611_BUS busid)
|
||||
*/
|
||||
bool find_bus(enum MS5611_BUS busid, ms5611_bus_option &bus)
|
||||
{
|
||||
for (uint8_t i=0; i<NUM_BUS_OPTIONS; i++) {
|
||||
for (uint8_t i = 0; i < NUM_BUS_OPTIONS; i++) {
|
||||
if ((busid == MS5611_BUS_ALL ||
|
||||
busid == bus_options[i].busid) && bus_options[i].dev != NULL) {
|
||||
bus = bus_options[i];
|
||||
@@ -989,17 +1013,21 @@ int
|
||||
test(enum MS5611_BUS busid)
|
||||
{
|
||||
struct ms5611_bus_option bus;
|
||||
|
||||
if (!find_bus(busid, bus)) {
|
||||
return 1;
|
||||
}
|
||||
|
||||
struct baro_report report;
|
||||
|
||||
ssize_t sz;
|
||||
|
||||
int ret;
|
||||
|
||||
int fd;
|
||||
|
||||
fd = px4_open(bus.devpath, O_RDONLY);
|
||||
|
||||
if (fd < 0) {
|
||||
warn("open failed (try 'ms5611 start' if the driver is not running)");
|
||||
return 1;
|
||||
@@ -1071,6 +1099,7 @@ int
|
||||
reset(enum MS5611_BUS busid)
|
||||
{
|
||||
struct ms5611_bus_option bus;
|
||||
|
||||
if (!find_bus(busid, bus)) {
|
||||
return 1;
|
||||
}
|
||||
@@ -1078,6 +1107,7 @@ reset(enum MS5611_BUS busid)
|
||||
int fd;
|
||||
|
||||
fd = open(bus.devpath, O_RDONLY);
|
||||
|
||||
if (fd < 0) {
|
||||
warn("failed ");
|
||||
return 1;
|
||||
@@ -1092,6 +1122,7 @@ reset(enum MS5611_BUS busid)
|
||||
warn("driver poll restart failed");
|
||||
return 1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
@@ -1101,13 +1132,15 @@ reset(enum MS5611_BUS busid)
|
||||
int
|
||||
info()
|
||||
{
|
||||
for (uint8_t i=0; i<NUM_BUS_OPTIONS; i++) {
|
||||
for (uint8_t i = 0; i < NUM_BUS_OPTIONS; i++) {
|
||||
struct ms5611_bus_option &bus = bus_options[i];
|
||||
|
||||
if (bus.dev != nullptr) {
|
||||
warnx("%s", bus.devpath);
|
||||
bus.dev->print_info();
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
@@ -1118,12 +1151,15 @@ int
|
||||
calibrate(unsigned altitude, enum MS5611_BUS busid)
|
||||
{
|
||||
struct ms5611_bus_option bus;
|
||||
|
||||
if (!find_bus(busid, bus)) {
|
||||
return 1;
|
||||
}
|
||||
|
||||
struct baro_report report;
|
||||
|
||||
float pressure;
|
||||
|
||||
float p1;
|
||||
|
||||
int fd;
|
||||
@@ -1224,18 +1260,23 @@ ms5611_main(int argc, char *argv[])
|
||||
/* jump over start/off/etc and look at options first */
|
||||
int myoptind = 1;
|
||||
const char *myoptarg = NULL;
|
||||
|
||||
while ((ch = px4_getopt(argc, argv, "XIS", &myoptind, &myoptarg)) != EOF) {
|
||||
printf("ch = %d\n", ch);
|
||||
|
||||
switch (ch) {
|
||||
case 'X':
|
||||
busid = MS5611_BUS_I2C_EXTERNAL;
|
||||
break;
|
||||
|
||||
case 'I':
|
||||
busid = MS5611_BUS_I2C_INTERNAL;
|
||||
break;
|
||||
|
||||
case 'S':
|
||||
busid = MS5611_BUS_SIM_EXTERNAL;
|
||||
break;
|
||||
|
||||
default:
|
||||
ms5611::usage();
|
||||
return 1;
|
||||
@@ -1247,42 +1288,48 @@ ms5611_main(int argc, char *argv[])
|
||||
/*
|
||||
* Start/load the driver.
|
||||
*/
|
||||
if (!strcmp(verb, "start"))
|
||||
if (!strcmp(verb, "start")) {
|
||||
ret = ms5611::start(busid);
|
||||
}
|
||||
|
||||
/*
|
||||
* Test the driver/device.
|
||||
*/
|
||||
else if (!strcmp(verb, "test"))
|
||||
else if (!strcmp(verb, "test")) {
|
||||
ret = ms5611::test(busid);
|
||||
}
|
||||
|
||||
/*
|
||||
* Reset the driver.
|
||||
*/
|
||||
else if (!strcmp(verb, "reset"))
|
||||
else if (!strcmp(verb, "reset")) {
|
||||
ret = ms5611::reset(busid);
|
||||
}
|
||||
|
||||
/*
|
||||
* Print driver information.
|
||||
*/
|
||||
else if (!strcmp(verb, "info"))
|
||||
else if (!strcmp(verb, "info")) {
|
||||
ret = ms5611::info();
|
||||
}
|
||||
|
||||
/*
|
||||
* Perform MSL pressure calibration given an altitude in metres
|
||||
*/
|
||||
else if (!strcmp(verb, "calibrate")) {
|
||||
if (argc < 2)
|
||||
if (argc < 2) {
|
||||
PX4_WARN("missing altitude");
|
||||
}
|
||||
|
||||
long altitude = strtol(argv[optind+1], nullptr, 10);
|
||||
long altitude = strtol(argv[optind + 1], nullptr, 10);
|
||||
|
||||
ret = ms5611::calibrate(altitude, busid);
|
||||
}
|
||||
else {
|
||||
|
||||
} else {
|
||||
ms5611::usage();
|
||||
warnx("unrecognised command, try 'start', 'test', 'reset' or 'info'");
|
||||
return 1;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
@@ -31,7 +31,7 @@
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
/**
|
||||
* @file ms5611_i2c.cpp
|
||||
*
|
||||
* SIM interface for MS5611
|
||||
@@ -127,6 +127,7 @@ MS5611_SIM::dev_read(unsigned offset, void *data, unsigned count)
|
||||
/* read the most recent measurement */
|
||||
uint8_t cmd = 0;
|
||||
int ret = transfer(&cmd, 1, &buf[0], 3);
|
||||
|
||||
if (ret == PX4_OK) {
|
||||
/* fetch the raw value */
|
||||
cvt->b[0] = buf[2];
|
||||
|
||||
@@ -31,7 +31,7 @@
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
/**
|
||||
* @file ms5611_spi.cpp
|
||||
*
|
||||
* SPI interface for MS5611
|
||||
@@ -117,6 +117,7 @@ device::Device *
|
||||
MS5611_spi_interface(ms5611::prom_u &prom_buf, uint8_t busnum)
|
||||
{
|
||||
#ifdef PX4_SPI_BUS_EXT
|
||||
|
||||
if (busnum == PX4_SPI_BUS_EXT) {
|
||||
#ifdef PX4_SPIDEV_EXT_BARO
|
||||
return new MS5611_SPI(busnum, (spi_dev_e)PX4_SPIDEV_EXT_BARO, prom_buf);
|
||||
@@ -124,12 +125,13 @@ MS5611_spi_interface(ms5611::prom_u &prom_buf, uint8_t busnum)
|
||||
return nullptr;
|
||||
#endif
|
||||
}
|
||||
|
||||
#endif
|
||||
return new MS5611_SPI(busnum, (spi_dev_e)PX4_SPIDEV_BARO, prom_buf);
|
||||
}
|
||||
|
||||
MS5611_SPI::MS5611_SPI(uint8_t bus, spi_dev_e device, ms5611::prom_u &prom_buf) :
|
||||
SPI("MS5611_SPI", nullptr, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz */),
|
||||
SPI("MS5611_SPI", nullptr, bus, device, SPIDEV_MODE3, 11 * 1000 * 1000 /* will be rounded to 10.4 MHz */),
|
||||
_prom(prom_buf)
|
||||
{
|
||||
}
|
||||
@@ -144,6 +146,7 @@ MS5611_SPI::init()
|
||||
int ret;
|
||||
|
||||
ret = SPI::init();
|
||||
|
||||
if (ret != OK) {
|
||||
DEVICE_DEBUG("SPI init failed");
|
||||
goto out;
|
||||
@@ -151,6 +154,7 @@ MS5611_SPI::init()
|
||||
|
||||
/* send reset command */
|
||||
ret = _reset();
|
||||
|
||||
if (ret != OK) {
|
||||
DEVICE_DEBUG("reset failed");
|
||||
goto out;
|
||||
@@ -158,6 +162,7 @@ MS5611_SPI::init()
|
||||
|
||||
/* read PROM */
|
||||
ret = _read_prom();
|
||||
|
||||
if (ret != OK) {
|
||||
DEVICE_DEBUG("prom readout failed");
|
||||
goto out;
|
||||
@@ -214,6 +219,7 @@ MS5611_SPI::ioctl(unsigned operation, unsigned &arg)
|
||||
errno = ret;
|
||||
return -1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
@@ -245,23 +251,30 @@ MS5611_SPI::_read_prom()
|
||||
|
||||
/* read and convert PROM words */
|
||||
bool all_zero = true;
|
||||
|
||||
for (int i = 0; i < 8; i++) {
|
||||
uint8_t cmd = (ADDR_PROM_SETUP + (i * 2));
|
||||
_prom.c[i] = _reg16(cmd);
|
||||
if (_prom.c[i] != 0)
|
||||
|
||||
if (_prom.c[i] != 0) {
|
||||
all_zero = false;
|
||||
}
|
||||
|
||||
//DEVICE_DEBUG("prom[%u]=0x%x", (unsigned)i, (unsigned)_prom.c[i]);
|
||||
}
|
||||
|
||||
/* calculate CRC and return success/failure accordingly */
|
||||
int ret = ms5611::crc4(&_prom.c[0]) ? OK : -EIO;
|
||||
|
||||
if (ret != OK) {
|
||||
DEVICE_DEBUG("crc failed");
|
||||
}
|
||||
|
||||
if (all_zero) {
|
||||
DEVICE_DEBUG("prom all zero");
|
||||
ret = -EIO;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user