MS5611 code style fix

This commit is contained in:
Lorenz Meier
2015-09-05 22:06:15 +02:00
parent b1213a64f5
commit ebfae73108
6 changed files with 246 additions and 131 deletions

View File

@@ -150,6 +150,7 @@ MS5611_I2C::read(device::file_t *handlep, char *data, size_t buflen)
/* read the most recent measurement */ /* read the most recent measurement */
uint8_t cmd = 0; uint8_t cmd = 0;
int ret = transfer(&cmd, 1, &buf[0], 3); int ret = transfer(&cmd, 1, &buf[0], 3);
if (ret == PX4_OK) { if (ret == PX4_OK) {
/* fetch the raw value */ /* fetch the raw value */
cvt->b[0] = buf[2]; cvt->b[0] = buf[2];
@@ -208,12 +209,14 @@ MS5611_I2C::_probe_address(uint8_t address)
set_address(address); set_address(address);
/* send reset command */ /* send reset command */
if (PX4_OK != _reset()) if (PX4_OK != _reset()) {
return -EIO; return -EIO;
}
/* read PROM */ /* read PROM */
if (PX4_OK != _read_prom()) if (PX4_OK != _read_prom()) {
return -EIO; return -EIO;
}
return PX4_OK; return PX4_OK;
} }
@@ -267,8 +270,9 @@ MS5611_I2C::_read_prom()
for (int i = 0; i < 8; i++) { for (int i = 0; i < 8; i++) {
uint8_t cmd = ADDR_PROM_SETUP + (i * 2); 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; break;
}
/* assemble 16 bit value and convert from big endian (sensor) to little endian (MCU) */ /* assemble 16 bit value and convert from big endian (sensor) to little endian (MCU) */
cvt.b[0] = prom_buf[1]; cvt.b[0] = prom_buf[1];

View File

@@ -243,12 +243,14 @@ MS5611::~MS5611()
/* make sure we are truly inactive */ /* make sure we are truly inactive */
stop_cycle(); stop_cycle();
if (_class_instance != -1) if (_class_instance != -1) {
unregister_class_devname(get_devname(), _class_instance); unregister_class_devname(get_devname(), _class_instance);
}
/* free any existing reports */ /* free any existing reports */
if (_reports != nullptr) if (_reports != nullptr) {
delete _reports; delete _reports;
}
// free perf counters // free perf counters
perf_free(_sample_perf); perf_free(_sample_perf);
@@ -265,6 +267,7 @@ MS5611::init()
int ret; int ret;
ret = CDev::init(); ret = CDev::init();
if (ret != OK) { if (ret != OK) {
DEVICE_DEBUG("CDev init failed"); DEVICE_DEBUG("CDev init failed");
goto out; goto out;
@@ -342,8 +345,9 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen)
int ret = 0; int ret = 0;
/* buffer must be large enough */ /* buffer must be large enough */
if (count < 1) if (count < 1) {
return -ENOSPC; return -ENOSPC;
}
/* if automatic measurement is enabled */ /* if automatic measurement is enabled */
if (_measure_ticks > 0) { 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 */ /* state machine will have generated a report, copy it out */
if (_reports->get(brp)) if (_reports->get(brp)) {
ret = sizeof(*brp); ret = sizeof(*brp);
}
} while (0); } while (0);
@@ -435,8 +440,9 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
_measure_ticks = USEC2TICK(MS5611_CONVERSION_INTERVAL); _measure_ticks = USEC2TICK(MS5611_CONVERSION_INTERVAL);
/* if we need to start the poll state machine, do it */ /* if we need to start the poll state machine, do it */
if (want_start) if (want_start) {
start_cycle(); start_cycle();
}
return OK; return OK;
} }
@@ -450,15 +456,17 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
unsigned ticks = USEC2TICK(1000000 / arg); unsigned ticks = USEC2TICK(1000000 / arg);
/* check against maximum rate */ /* check against maximum rate */
if (ticks < USEC2TICK(MS5611_CONVERSION_INTERVAL)) if (ticks < USEC2TICK(MS5611_CONVERSION_INTERVAL)) {
return -EINVAL; return -EINVAL;
}
/* update interval for next measurement */ /* update interval for next measurement */
_measure_ticks = ticks; _measure_ticks = ticks;
/* if we need to start the poll state machine, do it */ /* if we need to start the poll state machine, do it */
if (want_start) if (want_start) {
start_cycle(); start_cycle();
}
return OK; return OK;
} }
@@ -466,21 +474,25 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
} }
case SENSORIOCGPOLLRATE: case SENSORIOCGPOLLRATE:
if (_measure_ticks == 0) if (_measure_ticks == 0) {
return SENSOR_POLLRATE_MANUAL; return SENSOR_POLLRATE_MANUAL;
}
return (1000 / _measure_ticks); return (1000 / _measure_ticks);
case SENSORIOCSQUEUEDEPTH: { case SENSORIOCSQUEUEDEPTH: {
/* lower bound is mandatory, upper bound is a sanity check */ /* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100)) if ((arg < 1) || (arg > 100)) {
return -EINVAL; return -EINVAL;
}
irqstate_t flags = irqsave(); irqstate_t flags = irqsave();
if (!_reports->resize(arg)) { if (!_reports->resize(arg)) {
irqrestore(flags); irqrestore(flags);
return -ENOMEM; return -ENOMEM;
} }
irqrestore(flags); irqrestore(flags);
return OK; return OK;
} }
@@ -498,8 +510,9 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
case BAROIOCSMSLPRESSURE: case BAROIOCSMSLPRESSURE:
/* range-check for sanity */ /* range-check for sanity */
if ((arg < 80000) || (arg > 120000)) if ((arg < 80000) || (arg > 120000)) {
return -EINVAL; return -EINVAL;
}
_msl_pressure = arg; _msl_pressure = arg;
return OK; return OK;
@@ -554,6 +567,7 @@ MS5611::cycle()
/* perform collection */ /* perform collection */
ret = collect(); ret = collect();
if (ret != OK) { if (ret != OK) {
if (ret == -6) { if (ret == -6) {
/* /*
@@ -564,6 +578,7 @@ MS5611::cycle()
} else { } else {
//DEVICE_LOG("collection error %d", ret); //DEVICE_LOG("collection error %d", ret);
} }
/* issue a reset command to the sensor */ /* issue a reset command to the sensor */
_interface->ioctl(IOCTL_RESET, dummy); _interface->ioctl(IOCTL_RESET, dummy);
/* reset the collection state machine and try again - we need /* reset the collection state machine and try again - we need
@@ -598,6 +613,7 @@ MS5611::cycle()
/* measurement phase */ /* measurement phase */
ret = measure(); ret = measure();
if (ret != OK) { if (ret != OK) {
/* issue a reset command to the sensor */ /* issue a reset command to the sensor */
_interface->ioctl(IOCTL_RESET, dummy); _interface->ioctl(IOCTL_RESET, dummy);
@@ -633,8 +649,10 @@ MS5611::measure()
* Send the command to begin measuring. * Send the command to begin measuring.
*/ */
ret = _interface->ioctl(IOCTL_MEASURE, addr); ret = _interface->ioctl(IOCTL_MEASURE, addr);
if (OK != ret)
if (OK != ret) {
perf_count(_comms_errors); perf_count(_comms_errors);
}
perf_end(_measure_perf); perf_end(_measure_perf);
@@ -656,6 +674,7 @@ MS5611::collect()
/* read the most recent measurement - read offset/size are hardcoded in the interface */ /* read the most recent measurement - read offset/size are hardcoded in the interface */
ret = _interface->read(0, (void *)&raw, 0); ret = _interface->read(0, (void *)&raw, 0);
if (ret < 0) { if (ret < 0) {
perf_count(_comms_errors); perf_count(_comms_errors);
perf_end(_sample_perf); perf_end(_sample_perf);
@@ -883,11 +902,13 @@ crc4(uint16_t *n_prom)
bool bool
start_bus(struct ms5611_bus_option &bus) start_bus(struct ms5611_bus_option &bus)
{ {
if (bus.dev != nullptr) if (bus.dev != nullptr) {
errx(1, "bus option already started"); errx(1, "bus option already started");
}
prom_u prom_buf; prom_u prom_buf;
device::Device *interface = bus.interface_constructor(prom_buf, bus.busnum); device::Device *interface = bus.interface_constructor(prom_buf, bus.busnum);
if (interface->init() != OK) { if (interface->init() != OK) {
delete interface; delete interface;
warnx("no device on bus %u", (unsigned)bus.busid); 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); bus.dev = new MS5611(interface, prom_buf, bus.devpath);
if (bus.dev != nullptr && OK != bus.dev->init()) { if (bus.dev != nullptr && OK != bus.dev->init()) {
delete bus.dev; delete bus.dev;
bus.dev = NULL; bus.dev = NULL;
@@ -907,6 +929,7 @@ start_bus(struct ms5611_bus_option &bus)
if (fd == -1) { if (fd == -1) {
errx(1, "can't open baro device"); errx(1, "can't open baro device");
} }
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
close(fd); close(fd);
errx(1, "failed setting default poll rate"); errx(1, "failed setting default poll rate");
@@ -934,15 +957,18 @@ start(enum MS5611_BUS busid)
// this device is already started // this device is already started
continue; continue;
} }
if (busid != MS5611_BUS_ALL && bus_options[i].busid != busid) { if (busid != MS5611_BUS_ALL && bus_options[i].busid != busid) {
// not the one that is asked for // not the one that is asked for
continue; continue;
} }
started |= start_bus(bus_options[i]); started |= start_bus(bus_options[i]);
} }
if (!started) if (!started) {
errx(1, "driver start failed"); errx(1, "driver start failed");
}
// one or more drivers started OK // one or more drivers started OK
exit(0); exit(0);
@@ -960,6 +986,7 @@ struct ms5611_bus_option &find_bus(enum MS5611_BUS busid)
return bus_options[i]; 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; int fd;
fd = open(bus.devpath, O_RDONLY); 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)"); err(1, "open failed (try 'ms5611 start' if the driver is not running)");
}
/* do a simple demand read */ /* do a simple demand read */
sz = read(fd, &report, sizeof(report)); sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report)) if (sz != sizeof(report)) {
err(1, "immediate read failed"); err(1, "immediate read failed");
}
warnx("single read"); warnx("single read");
warnx("pressure: %10.4f", (double)report.pressure); warnx("pressure: %10.4f", (double)report.pressure);
@@ -995,12 +1025,14 @@ test(enum MS5611_BUS busid)
warnx("time: %lld", report.timestamp); warnx("time: %lld", report.timestamp);
/* set the queue depth to 10 */ /* 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"); errx(1, "failed to set queue depth");
}
/* start the sensor polling at 2Hz */ /* 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"); errx(1, "failed to set 2Hz poll rate");
}
/* read the sensor 5x and report each value */ /* read the sensor 5x and report each value */
for (unsigned i = 0; i < 5; i++) { for (unsigned i = 0; i < 5; i++) {
@@ -1011,14 +1043,16 @@ test(enum MS5611_BUS busid)
fds.events = POLLIN; fds.events = POLLIN;
ret = poll(&fds, 1, 2000); ret = poll(&fds, 1, 2000);
if (ret != 1) if (ret != 1) {
errx(1, "timed out waiting for sensor data"); errx(1, "timed out waiting for sensor data");
}
/* now go get it */ /* now go get it */
sz = read(fd, &report, sizeof(report)); sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report)) if (sz != sizeof(report)) {
err(1, "periodic read failed"); err(1, "periodic read failed");
}
warnx("periodic read %u", i); warnx("periodic read %u", i);
warnx("pressure: %10.4f", (double)report.pressure); warnx("pressure: %10.4f", (double)report.pressure);
@@ -1041,14 +1075,18 @@ reset(enum MS5611_BUS busid)
int fd; int fd;
fd = open(bus.devpath, O_RDONLY); fd = open(bus.devpath, O_RDONLY);
if (fd < 0)
if (fd < 0) {
err(1, "failed "); err(1, "failed ");
}
if (ioctl(fd, SENSORIOCRESET, 0) < 0) if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
err(1, "driver reset failed"); 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"); err(1, "driver poll restart failed");
}
exit(0); exit(0);
} }
@@ -1061,11 +1099,13 @@ 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]; struct ms5611_bus_option &bus = bus_options[i];
if (bus.dev != nullptr) { if (bus.dev != nullptr) {
warnx("%s", bus.devpath); warnx("%s", bus.devpath);
bus.dev->print_info(); bus.dev->print_info();
} }
} }
exit(0); exit(0);
} }
@@ -1084,12 +1124,14 @@ calibrate(unsigned altitude, enum MS5611_BUS busid)
fd = open(bus.devpath, O_RDONLY); 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)"); err(1, "open failed (try 'ms5611 start' if the driver is not running)");
}
/* start the sensor polling at max */ /* 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"); errx(1, "failed to set poll rate");
}
/* average a few measurements */ /* average a few measurements */
pressure = 0.0f; pressure = 0.0f;
@@ -1104,14 +1146,16 @@ calibrate(unsigned altitude, enum MS5611_BUS busid)
fds.events = POLLIN; fds.events = POLLIN;
ret = poll(&fds, 1, 1000); ret = poll(&fds, 1, 1000);
if (ret != 1) if (ret != 1) {
errx(1, "timed out waiting for sensor data"); errx(1, "timed out waiting for sensor data");
}
/* now go get it */ /* now go get it */
sz = read(fd, &report, sizeof(report)); sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report)) if (sz != sizeof(report)) {
err(1, "sensor read failed"); err(1, "sensor read failed");
}
pressure += report.pressure; pressure += report.pressure;
} }
@@ -1134,8 +1178,9 @@ calibrate(unsigned altitude, enum MS5611_BUS busid)
/* save as integer Pa */ /* save as integer Pa */
p1 *= 1000.0f; p1 *= 1000.0f;
if (ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)p1) != OK) if (ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)p1) != OK) {
err(1, "BAROIOCSMSLPRESSURE"); err(1, "BAROIOCSMSLPRESSURE");
}
close(fd); close(fd);
exit(0); exit(0);
@@ -1166,15 +1211,19 @@ ms5611_main(int argc, char *argv[])
case 'X': case 'X':
busid = MS5611_BUS_I2C_EXTERNAL; busid = MS5611_BUS_I2C_EXTERNAL;
break; break;
case 'I': case 'I':
busid = MS5611_BUS_I2C_INTERNAL; busid = MS5611_BUS_I2C_INTERNAL;
break; break;
case 'S': case 'S':
busid = MS5611_BUS_SPI_EXTERNAL; busid = MS5611_BUS_SPI_EXTERNAL;
break; break;
case 's': case 's':
busid = MS5611_BUS_SPI_INTERNAL; busid = MS5611_BUS_SPI_INTERNAL;
break; break;
default: default:
ms5611::usage(); ms5611::usage();
exit(0); exit(0);
@@ -1215,8 +1264,9 @@ ms5611_main(int argc, char *argv[])
* Perform MSL pressure calibration given an altitude in metres * Perform MSL pressure calibration given an altitude in metres
*/ */
if (!strcmp(verb, "calibrate")) { if (!strcmp(verb, "calibrate")) {
if (argc < 2) if (argc < 2) {
errx(1, "missing altitude"); errx(1, "missing altitude");
}
long altitude = strtol(argv[optind + 1], nullptr, 10); long altitude = strtol(argv[optind + 1], nullptr, 10);

View File

@@ -240,12 +240,14 @@ MS5611::~MS5611()
/* make sure we are truly inactive */ /* make sure we are truly inactive */
stop_cycle(); stop_cycle();
if (_class_instance != -1) if (_class_instance != -1) {
unregister_class_devname(get_devname(), _class_instance); unregister_class_devname(get_devname(), _class_instance);
}
/* free any existing reports */ /* free any existing reports */
if (_reports != nullptr) if (_reports != nullptr) {
delete _reports; delete _reports;
}
// free perf counters // free perf counters
perf_free(_sample_perf); perf_free(_sample_perf);
@@ -263,6 +265,7 @@ MS5611::init()
warnx("MS5611::init"); warnx("MS5611::init");
ret = VDev::init(); ret = VDev::init();
if (ret != OK) { if (ret != OK) {
DEVICE_DEBUG("VDev init failed"); DEVICE_DEBUG("VDev init failed");
goto out; goto out;
@@ -328,6 +331,7 @@ MS5611::init()
if (_baro_topic == nullptr) { if (_baro_topic == nullptr) {
warnx("failed to create sensor_baro publication"); warnx("failed to create sensor_baro publication");
} }
//warnx("sensor_baro publication %ld", _baro_topic); //warnx("sensor_baro publication %ld", _baro_topic);
} while (0); } while (0);
@@ -344,8 +348,9 @@ MS5611::read(device::file_t *handlep, char *buffer, size_t buflen)
int ret = 0; int ret = 0;
/* buffer must be large enough */ /* buffer must be large enough */
if (count < 1) if (count < 1) {
return -ENOSPC; return -ENOSPC;
}
/* if automatic measurement is enabled */ /* if automatic measurement is enabled */
if (_measure_ticks > 0) { 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 */ /* state machine will have generated a report, copy it out */
if (_reports->get(brp)) if (_reports->get(brp)) {
ret = sizeof(*brp); ret = sizeof(*brp);
}
} while (0); } while (0);
@@ -437,8 +443,9 @@ MS5611::ioctl(device::file_t *handlep, int cmd, unsigned long arg)
_measure_ticks = USEC2TICK(MS5611_CONVERSION_INTERVAL); _measure_ticks = USEC2TICK(MS5611_CONVERSION_INTERVAL);
/* if we need to start the poll state machine, do it */ /* if we need to start the poll state machine, do it */
if (want_start) if (want_start) {
start_cycle(); start_cycle();
}
return OK; return OK;
} }
@@ -452,15 +459,17 @@ MS5611::ioctl(device::file_t *handlep, int cmd, unsigned long arg)
unsigned ticks = USEC2TICK(1000000 / arg); unsigned ticks = USEC2TICK(1000000 / arg);
/* check against maximum rate */ /* check against maximum rate */
if ((unsigned long)ticks < USEC2TICK(MS5611_CONVERSION_INTERVAL)) if ((unsigned long)ticks < USEC2TICK(MS5611_CONVERSION_INTERVAL)) {
return -EINVAL; return -EINVAL;
}
/* update interval for next measurement */ /* update interval for next measurement */
_measure_ticks = ticks; _measure_ticks = ticks;
/* if we need to start the poll state machine, do it */ /* if we need to start the poll state machine, do it */
if (want_start) if (want_start) {
start_cycle(); start_cycle();
}
return OK; return OK;
} }
@@ -468,19 +477,22 @@ MS5611::ioctl(device::file_t *handlep, int cmd, unsigned long arg)
} }
case SENSORIOCGPOLLRATE: case SENSORIOCGPOLLRATE:
if (_measure_ticks == 0) if (_measure_ticks == 0) {
return SENSOR_POLLRATE_MANUAL; return SENSOR_POLLRATE_MANUAL;
}
return (1000 / _measure_ticks); return (1000 / _measure_ticks);
case SENSORIOCSQUEUEDEPTH: { case SENSORIOCSQUEUEDEPTH: {
/* lower bound is mandatory, upper bound is a sanity check */ /* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100)) if ((arg < 1) || (arg > 100)) {
return -EINVAL; return -EINVAL;
}
if (!_reports->resize(arg)) { if (!_reports->resize(arg)) {
return -ENOMEM; return -ENOMEM;
} }
return OK; return OK;
} }
@@ -497,8 +509,9 @@ MS5611::ioctl(device::file_t *handlep, int cmd, unsigned long arg)
case BAROIOCSMSLPRESSURE: case BAROIOCSMSLPRESSURE:
/* range-check for sanity */ /* range-check for sanity */
if ((arg < 80000) || (arg > 120000)) if ((arg < 80000) || (arg > 120000)) {
return -EINVAL; return -EINVAL;
}
_msl_pressure = arg; _msl_pressure = arg;
return OK; return OK;
@@ -553,6 +566,7 @@ MS5611::cycle()
/* perform collection */ /* perform collection */
ret = collect(); ret = collect();
if (ret != OK) { if (ret != OK) {
if (ret == -6) { if (ret == -6) {
/* /*
@@ -563,6 +577,7 @@ MS5611::cycle()
} else { } else {
//DEVICE_LOG("collection error %d", ret); //DEVICE_LOG("collection error %d", ret);
} }
/* issue a reset command to the sensor */ /* issue a reset command to the sensor */
_interface->dev_ioctl(IOCTL_RESET, dummy); _interface->dev_ioctl(IOCTL_RESET, dummy);
/* reset the collection state machine and try again */ /* reset the collection state machine and try again */
@@ -594,6 +609,7 @@ MS5611::cycle()
/* measurement phase */ /* measurement phase */
ret = measure(); ret = measure();
if (ret != OK) { if (ret != OK) {
//DEVICE_LOG("measure error %d", ret); //DEVICE_LOG("measure error %d", ret);
/* issue a reset command to the sensor */ /* issue a reset command to the sensor */
@@ -630,8 +646,10 @@ MS5611::measure()
* Send the command to begin measuring. * Send the command to begin measuring.
*/ */
ret = _interface->dev_ioctl(IOCTL_MEASURE, addr); ret = _interface->dev_ioctl(IOCTL_MEASURE, addr);
if (OK != ret)
if (OK != ret) {
perf_count(_comms_errors); perf_count(_comms_errors);
}
perf_end(_measure_perf); perf_end(_measure_perf);
@@ -653,6 +671,7 @@ MS5611::collect()
/* read the most recent measurement - read offset/size are hardcoded in the interface */ /* read the most recent measurement - read offset/size are hardcoded in the interface */
ret = _interface->dev_read(0, (void *)&raw, 0); ret = _interface->dev_read(0, (void *)&raw, 0);
if (ret < 0) { if (ret < 0) {
perf_count(_comms_errors); perf_count(_comms_errors);
perf_end(_sample_perf); perf_end(_sample_perf);
@@ -745,8 +764,8 @@ MS5611::collect()
if (_baro_topic != (orb_advert_t)(-1)) { if (_baro_topic != (orb_advert_t)(-1)) {
/* publish it */ /* publish it */
orb_publish(ORB_ID(sensor_baro), _baro_topic, &report); orb_publish(ORB_ID(sensor_baro), _baro_topic, &report);
}
else { } else {
printf("MS5611::collect _baro_topic not initialized\n"); printf("MS5611::collect _baro_topic not initialized\n");
} }
} }
@@ -897,6 +916,7 @@ start_bus(struct ms5611_bus_option &bus)
prom_u prom_buf; prom_u prom_buf;
device::Device *interface = bus.interface_constructor(prom_buf, bus.busnum); device::Device *interface = bus.interface_constructor(prom_buf, bus.busnum);
if (interface->init() != OK) { if (interface->init() != OK) {
delete interface; delete interface;
warnx("no device on bus %u", (unsigned)bus.busid); 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); bus.dev = new MS5611(interface, prom_buf, bus.devpath);
if (bus.dev != nullptr && OK != bus.dev->init()) { if (bus.dev != nullptr && OK != bus.dev->init()) {
delete bus.dev; delete bus.dev;
bus.dev = NULL; bus.dev = NULL;
@@ -918,6 +939,7 @@ start_bus(struct ms5611_bus_option &bus)
warnx("can't open baro device"); warnx("can't open baro device");
return false; return false;
} }
if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
px4_close(fd); px4_close(fd);
warnx("failed setting default poll rate"); warnx("failed setting default poll rate");
@@ -946,10 +968,12 @@ start(enum MS5611_BUS busid)
// this device is already started // this device is already started
continue; continue;
} }
if (busid != MS5611_BUS_ALL && bus_options[i].busid != busid) { if (busid != MS5611_BUS_ALL && bus_options[i].busid != busid) {
// not the one that is asked for // not the one that is asked for
continue; continue;
} }
started |= start_bus(bus_options[i]); started |= start_bus(bus_options[i]);
} }
@@ -989,17 +1013,21 @@ int
test(enum MS5611_BUS busid) test(enum MS5611_BUS busid)
{ {
struct ms5611_bus_option bus; struct ms5611_bus_option bus;
if (!find_bus(busid, bus)) { if (!find_bus(busid, bus)) {
return 1; return 1;
} }
struct baro_report report; struct baro_report report;
ssize_t sz; ssize_t sz;
int ret; int ret;
int fd; int fd;
fd = px4_open(bus.devpath, O_RDONLY); fd = px4_open(bus.devpath, O_RDONLY);
if (fd < 0) { if (fd < 0) {
warn("open failed (try 'ms5611 start' if the driver is not running)"); warn("open failed (try 'ms5611 start' if the driver is not running)");
return 1; return 1;
@@ -1071,6 +1099,7 @@ int
reset(enum MS5611_BUS busid) reset(enum MS5611_BUS busid)
{ {
struct ms5611_bus_option bus; struct ms5611_bus_option bus;
if (!find_bus(busid, bus)) { if (!find_bus(busid, bus)) {
return 1; return 1;
} }
@@ -1078,6 +1107,7 @@ reset(enum MS5611_BUS busid)
int fd; int fd;
fd = open(bus.devpath, O_RDONLY); fd = open(bus.devpath, O_RDONLY);
if (fd < 0) { if (fd < 0) {
warn("failed "); warn("failed ");
return 1; return 1;
@@ -1092,6 +1122,7 @@ reset(enum MS5611_BUS busid)
warn("driver poll restart failed"); warn("driver poll restart failed");
return 1; return 1;
} }
return 0; return 0;
} }
@@ -1103,11 +1134,13 @@ 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]; struct ms5611_bus_option &bus = bus_options[i];
if (bus.dev != nullptr) { if (bus.dev != nullptr) {
warnx("%s", bus.devpath); warnx("%s", bus.devpath);
bus.dev->print_info(); bus.dev->print_info();
} }
} }
return 0; return 0;
} }
@@ -1118,12 +1151,15 @@ int
calibrate(unsigned altitude, enum MS5611_BUS busid) calibrate(unsigned altitude, enum MS5611_BUS busid)
{ {
struct ms5611_bus_option bus; struct ms5611_bus_option bus;
if (!find_bus(busid, bus)) { if (!find_bus(busid, bus)) {
return 1; return 1;
} }
struct baro_report report; struct baro_report report;
float pressure; float pressure;
float p1; float p1;
int fd; int fd;
@@ -1224,18 +1260,23 @@ ms5611_main(int argc, char *argv[])
/* jump over start/off/etc and look at options first */ /* jump over start/off/etc and look at options first */
int myoptind = 1; int myoptind = 1;
const char *myoptarg = NULL; const char *myoptarg = NULL;
while ((ch = px4_getopt(argc, argv, "XIS", &myoptind, &myoptarg)) != EOF) { while ((ch = px4_getopt(argc, argv, "XIS", &myoptind, &myoptarg)) != EOF) {
printf("ch = %d\n", ch); printf("ch = %d\n", ch);
switch (ch) { switch (ch) {
case 'X': case 'X':
busid = MS5611_BUS_I2C_EXTERNAL; busid = MS5611_BUS_I2C_EXTERNAL;
break; break;
case 'I': case 'I':
busid = MS5611_BUS_I2C_INTERNAL; busid = MS5611_BUS_I2C_INTERNAL;
break; break;
case 'S': case 'S':
busid = MS5611_BUS_SIM_EXTERNAL; busid = MS5611_BUS_SIM_EXTERNAL;
break; break;
default: default:
ms5611::usage(); ms5611::usage();
return 1; return 1;
@@ -1247,42 +1288,48 @@ ms5611_main(int argc, char *argv[])
/* /*
* Start/load the driver. * Start/load the driver.
*/ */
if (!strcmp(verb, "start")) if (!strcmp(verb, "start")) {
ret = ms5611::start(busid); ret = ms5611::start(busid);
}
/* /*
* Test the driver/device. * Test the driver/device.
*/ */
else if (!strcmp(verb, "test")) else if (!strcmp(verb, "test")) {
ret = ms5611::test(busid); ret = ms5611::test(busid);
}
/* /*
* Reset the driver. * Reset the driver.
*/ */
else if (!strcmp(verb, "reset")) else if (!strcmp(verb, "reset")) {
ret = ms5611::reset(busid); ret = ms5611::reset(busid);
}
/* /*
* Print driver information. * Print driver information.
*/ */
else if (!strcmp(verb, "info")) else if (!strcmp(verb, "info")) {
ret = ms5611::info(); ret = ms5611::info();
}
/* /*
* Perform MSL pressure calibration given an altitude in metres * Perform MSL pressure calibration given an altitude in metres
*/ */
else if (!strcmp(verb, "calibrate")) { else if (!strcmp(verb, "calibrate")) {
if (argc < 2) if (argc < 2) {
PX4_WARN("missing altitude"); 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); ret = ms5611::calibrate(altitude, busid);
}
else { } else {
ms5611::usage(); ms5611::usage();
warnx("unrecognised command, try 'start', 'test', 'reset' or 'info'"); warnx("unrecognised command, try 'start', 'test', 'reset' or 'info'");
return 1; return 1;
} }
return ret; return ret;
} }

View File

@@ -127,6 +127,7 @@ MS5611_SIM::dev_read(unsigned offset, void *data, unsigned count)
/* read the most recent measurement */ /* read the most recent measurement */
uint8_t cmd = 0; uint8_t cmd = 0;
int ret = transfer(&cmd, 1, &buf[0], 3); int ret = transfer(&cmd, 1, &buf[0], 3);
if (ret == PX4_OK) { if (ret == PX4_OK) {
/* fetch the raw value */ /* fetch the raw value */
cvt->b[0] = buf[2]; cvt->b[0] = buf[2];

View File

@@ -117,6 +117,7 @@ device::Device *
MS5611_spi_interface(ms5611::prom_u &prom_buf, uint8_t busnum) MS5611_spi_interface(ms5611::prom_u &prom_buf, uint8_t busnum)
{ {
#ifdef PX4_SPI_BUS_EXT #ifdef PX4_SPI_BUS_EXT
if (busnum == PX4_SPI_BUS_EXT) { if (busnum == PX4_SPI_BUS_EXT) {
#ifdef PX4_SPIDEV_EXT_BARO #ifdef PX4_SPIDEV_EXT_BARO
return new MS5611_SPI(busnum, (spi_dev_e)PX4_SPIDEV_EXT_BARO, prom_buf); return new MS5611_SPI(busnum, (spi_dev_e)PX4_SPIDEV_EXT_BARO, prom_buf);
@@ -124,6 +125,7 @@ MS5611_spi_interface(ms5611::prom_u &prom_buf, uint8_t busnum)
return nullptr; return nullptr;
#endif #endif
} }
#endif #endif
return new MS5611_SPI(busnum, (spi_dev_e)PX4_SPIDEV_BARO, prom_buf); return new MS5611_SPI(busnum, (spi_dev_e)PX4_SPIDEV_BARO, prom_buf);
} }
@@ -144,6 +146,7 @@ MS5611_SPI::init()
int ret; int ret;
ret = SPI::init(); ret = SPI::init();
if (ret != OK) { if (ret != OK) {
DEVICE_DEBUG("SPI init failed"); DEVICE_DEBUG("SPI init failed");
goto out; goto out;
@@ -151,6 +154,7 @@ MS5611_SPI::init()
/* send reset command */ /* send reset command */
ret = _reset(); ret = _reset();
if (ret != OK) { if (ret != OK) {
DEVICE_DEBUG("reset failed"); DEVICE_DEBUG("reset failed");
goto out; goto out;
@@ -158,6 +162,7 @@ MS5611_SPI::init()
/* read PROM */ /* read PROM */
ret = _read_prom(); ret = _read_prom();
if (ret != OK) { if (ret != OK) {
DEVICE_DEBUG("prom readout failed"); DEVICE_DEBUG("prom readout failed");
goto out; goto out;
@@ -214,6 +219,7 @@ MS5611_SPI::ioctl(unsigned operation, unsigned &arg)
errno = ret; errno = ret;
return -1; return -1;
} }
return 0; return 0;
} }
@@ -245,23 +251,30 @@ MS5611_SPI::_read_prom()
/* read and convert PROM words */ /* read and convert PROM words */
bool all_zero = true; bool all_zero = true;
for (int i = 0; i < 8; i++) { for (int i = 0; i < 8; i++) {
uint8_t cmd = (ADDR_PROM_SETUP + (i * 2)); uint8_t cmd = (ADDR_PROM_SETUP + (i * 2));
_prom.c[i] = _reg16(cmd); _prom.c[i] = _reg16(cmd);
if (_prom.c[i] != 0)
if (_prom.c[i] != 0) {
all_zero = false; all_zero = false;
}
//DEVICE_DEBUG("prom[%u]=0x%x", (unsigned)i, (unsigned)_prom.c[i]); //DEVICE_DEBUG("prom[%u]=0x%x", (unsigned)i, (unsigned)_prom.c[i]);
} }
/* calculate CRC and return success/failure accordingly */ /* calculate CRC and return success/failure accordingly */
int ret = ms5611::crc4(&_prom.c[0]) ? OK : -EIO; int ret = ms5611::crc4(&_prom.c[0]) ? OK : -EIO;
if (ret != OK) { if (ret != OK) {
DEVICE_DEBUG("crc failed"); DEVICE_DEBUG("crc failed");
} }
if (all_zero) { if (all_zero) {
DEVICE_DEBUG("prom all zero"); DEVICE_DEBUG("prom all zero");
ret = -EIO; ret = -EIO;
} }
return ret; return ret;
} }