diff options
Diffstat (limited to 'apps/drivers/ms5611/ms5611.cpp')
-rw-r--r-- | apps/drivers/ms5611/ms5611.cpp | 36 |
1 files changed, 29 insertions, 7 deletions
diff --git a/apps/drivers/ms5611/ms5611.cpp b/apps/drivers/ms5611/ms5611.cpp index 893ef6c37..699cd36d2 100644 --- a/apps/drivers/ms5611/ms5611.cpp +++ b/apps/drivers/ms5611/ms5611.cpp @@ -303,6 +303,7 @@ MS5611::init() /* allocate basic report buffers */ _num_reports = 2; _reports = new struct baro_report[_num_reports]; + if (_reports == nullptr) goto out; @@ -311,6 +312,7 @@ MS5611::init() /* get a publish handle on the baro topic */ memset(&_reports[0], 0, sizeof(_reports[0])); _baro_topic = orb_advertise(ORB_ID(sensor_baro), &_reports[0]); + if (_baro_topic < 0) debug("failed to create sensor_baro object"); @@ -323,9 +325,10 @@ int MS5611::probe() { _retries = 10; - if((OK == probe_address(MS5611_ADDRESS_1)) || - (OK == probe_address(MS5611_ADDRESS_2))) { - _retries = 1; + + if ((OK == probe_address(MS5611_ADDRESS_1)) || + (OK == probe_address(MS5611_ADDRESS_2))) { + _retries = 1; return OK; } @@ -484,6 +487,7 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) case SENSORIOCGPOLLRATE: if (_measure_ticks == 0) return SENSOR_POLLRATE_MANUAL; + return (1000 / _measure_ticks); case SENSORIOCSQUEUEDEPTH: { @@ -518,9 +522,11 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) return -EINVAL; case BAROIOCSMSLPRESSURE: + /* range-check for sanity */ if ((arg < 80000) || (arg > 120000)) return -EINVAL; + _msl_pressure = arg; return OK; @@ -688,7 +694,7 @@ MS5611::collect() int64_t SENS2 = 5 * f >> 2; if (_TEMP < -1500) { - int64_t f2 = POW2(_TEMP + 1500); + int64_t f2 = POW2(_TEMP + 1500); OFF2 += 7 * f2; SENS2 += 11 * f2 >> 1; } @@ -697,6 +703,7 @@ MS5611::collect() _OFF -= OFF2; _SENS -= SENS2; } + } else { /* pressure calculation, result in Pa */ @@ -814,8 +821,8 @@ MS5611::read_prom() uint16_t w; } cvt; - /* - * Wait for PROM contents to be in the device (2.8 ms) in the case we are + /* + * Wait for PROM contents to be in the device (2.8 ms) in the case we are * called immediately after reset. */ usleep(3000); @@ -941,17 +948,22 @@ start() /* set the poll rate to default, starts automatic data collection */ fd = open(BARO_DEVICE_PATH, O_RDONLY); + if (fd < 0) goto fail; + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) goto fail; + exit(0); fail: + if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; } + errx(1, "driver start failed"); } @@ -968,11 +980,13 @@ test() int ret; int fd = open(BARO_DEVICE_PATH, O_RDONLY); + if (fd < 0) err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", BARO_DEVICE_PATH); /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); + if (sz != sizeof(report)) err(1, "immediate read failed"); @@ -1025,10 +1039,13 @@ void reset() { int fd = open(BARO_DEVICE_PATH, O_RDONLY); + if (fd < 0) err(1, "failed "); + if (ioctl(fd, SENSORIOCRESET, 0) < 0) err(1, "driver reset failed"); + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) err(1, "driver poll restart failed"); @@ -1061,6 +1078,7 @@ calibrate(unsigned altitude) float p1; int fd = open(BARO_DEVICE_PATH, O_RDONLY); + if (fd < 0) err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", BARO_DEVICE_PATH); @@ -1070,6 +1088,7 @@ calibrate(unsigned altitude) /* average a few measurements */ pressure = 0.0f; + for (unsigned i = 0; i < 20; i++) { struct pollfd fds; int ret; @@ -1091,6 +1110,7 @@ calibrate(unsigned altitude) pressure += report.pressure; } + pressure /= 20; /* average */ pressure /= 10; /* scale from millibar to kPa */ @@ -1108,8 +1128,10 @@ calibrate(unsigned altitude) /* save as integer Pa */ p1 *= 1000.0f; + if (ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)p1) != OK) err(1, "BAROIOCSMSLPRESSURE"); + exit(0); } @@ -1150,7 +1172,7 @@ ms5611_main(int argc, char *argv[]) errx(1, "missing altitude"); long altitude = strtol(argv[2], nullptr, 10); - + ms5611::calibrate(altitude); } |