From b1f3a5c92bd900ad15d4f13f43658563be5ed8de Mon Sep 17 00:00:00 2001 From: Sam Kelly Date: Tue, 25 Jun 2013 14:01:27 -0700 Subject: Enabled MS5611 by default on FMUv2. --- src/drivers/ms5611/ms5611.cpp | 1281 ++++++++++++++++++++++++++++++++++------- 1 file changed, 1069 insertions(+), 212 deletions(-) (limited to 'src/drivers/ms5611') diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 59ab5936e..76acf7ccd 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2012-2013 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 @@ -33,12 +33,13 @@ /** * @file ms5611.cpp - * Driver for the MS5611 barometric pressure sensor connected via I2C. + * Driver for the MS5611 barometric pressure sensor connected via I2C or SPI. */ #include #include +#include #include #include @@ -76,6 +77,11 @@ static const int ERROR = -1; # error This requires CONFIG_SCHED_WORKQUEUE. #endif +/* SPI protocol address bits */ +#define DIR_READ (1<<7) +#define DIR_WRITE (0<<7) +#define ADDR_INCREMENT (1<<6) + /** * Calibration PROM as reported by the device. */ @@ -100,11 +106,11 @@ union ms5611_prom_u { }; #pragma pack(pop) -class MS5611 : public device::I2C +class MS5611_I2C : public device::I2C { public: - MS5611(int bus); - virtual ~MS5611(); + MS5611_I2C(int bus); + virtual ~MS5611_I2C(); virtual int init(); @@ -118,8 +124,6 @@ public: protected: virtual int probe(); - -private: union ms5611_prom_u _prom; struct work_s _work; @@ -148,6 +152,85 @@ private: perf_counter_t _comms_errors; perf_counter_t _buffer_overflows; + /** + * 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_cycle(); + + /** + * Stop the automatic measurement state machine. + */ + void stop_cycle(); + + /** + * 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 cycle(); + + /** + * Static trampoline from the workq context; because we don't have a + * generic workq wrapper yet. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void cycle_trampoline(void *arg); + + /** + * 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(); + + /** + * Send a reset command to the MS5611. + * + * This is required after any bus reset. + */ + virtual int cmd_reset(); + + /** + * Read the MS5611 PROM + * + * @return OK if the PROM reads successfully. + */ + virtual int read_prom(); + + /** + * Execute the bus-specific ioctl (I2C or SPI) + * + * @return the bus-specific ioctl return value + */ + virtual int bus_ioctl(struct file *filp, int cmd, unsigned long arg); + + /** + * PROM CRC routine ported from MS5611 application note + * + * @param n_prom Pointer to words read from PROM. + * @return True if the CRC matches. + */ + bool crc4(uint16_t *n_prom); + +private: + /** * Test whether the device supported by the driver is present at a * specific address. @@ -157,8 +240,56 @@ private: */ int probe_address(uint8_t address); +}; + +class MS5611_SPI : public device::SPI +{ +public: + MS5611_SPI(int bus, const char* path, spi_dev_e device); + virtual ~MS5611_SPI(); + + virtual int init(); + + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + +protected: + virtual int probe(); + union ms5611_prom_u _prom; + + struct work_s _work; + unsigned _measure_ticks; + + unsigned _num_reports; + volatile unsigned _next_report; + volatile unsigned _oldest_report; + struct baro_report *_reports; + + bool _collect_phase; + unsigned _measure_phase; + + /* intermediate temperature values per MS5611 datasheet */ + int32_t _TEMP; + int64_t _OFF; + int64_t _SENS; + + /* altitude conversion calibration */ + unsigned _msl_pressure; /* in kPa */ + + orb_advert_t _baro_topic; + + perf_counter_t _sample_perf; + perf_counter_t _measure_perf; + perf_counter_t _comms_errors; + perf_counter_t _buffer_overflows; + /** - * Initialise the automatic measurement state machine and start it. + * 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. @@ -198,26 +329,33 @@ private: * * @return OK if the measurement command was successful. */ - int measure(); + virtual int measure(); /** * Collect the result of the most recent measurement. */ - int collect(); + virtual int collect(); /** * Send a reset command to the MS5611. * * This is required after any bus reset. */ - int cmd_reset(); + virtual int cmd_reset(); /** * Read the MS5611 PROM * * @return OK if the PROM reads successfully. */ - int read_prom(); + virtual int read_prom(); + + /** + * Execute the bus-specific ioctl (I2C or SPI) + * + * @return the bus-specific ioctl return value + */ + virtual int bus_ioctl(struct file *filp, int cmd, unsigned long arg); /** * PROM CRC routine ported from MS5611 application note @@ -227,6 +365,23 @@ private: */ bool crc4(uint16_t *n_prom); + // XXX this should really go into the SPI base class, as its common code + uint8_t read_reg(unsigned reg); + uint16_t read_reg16(unsigned reg); + void write_reg(unsigned reg, uint8_t value); + void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); + +private: + + /** + * Test whether the device supported by the driver is present at a + * specific address. + * + * @param address The I2C bus address to probe. + * @return True if the device is present. + */ + int probe_address(uint8_t address); + }; /* helper macro for handling report buffer indices */ @@ -243,8 +398,13 @@ private: #define MS5611_CONVERSION_INTERVAL 10000 /* microseconds */ #define MS5611_MEASUREMENT_RATIO 3 /* pressure measurements per temperature measurement */ -#define MS5611_BUS PX4_I2C_BUS_ONBOARD -#define MS5611_ADDRESS_1 PX4_I2C_OBDEV_MS5611 /* address select pins pulled high (PX4FMU series v1.6+) */ +#ifndef PX4_I2C_BUS_ONBOARD + #define MS5611_BUS 1 +#else + #define MS5611_BUS PX4_I2C_BUS_ONBOARD +#endif + +#define MS5611_ADDRESS_1 0x76 /* address select pins pulled high (PX4FMU series v1.6+) */ #define MS5611_ADDRESS_2 0x77 /* address select pins pulled low (PX4FMU prototypes) */ #define ADDR_RESET_CMD 0x1E /* write to this address to reset chip */ @@ -259,8 +419,7 @@ private: */ extern "C" __EXPORT int ms5611_main(int argc, char *argv[]); - -MS5611::MS5611(int bus) : +MS5611_I2C::MS5611_I2C(int bus) : I2C("MS5611", BARO_DEVICE_PATH, bus, 0, 400000), _measure_ticks(0), _num_reports(0), @@ -279,14 +438,46 @@ MS5611::MS5611(int bus) : _comms_errors(perf_alloc(PC_COUNT, "ms5611_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "ms5611_buffer_overflows")) { - // enable debug() calls - _debug_enabled = true; + // work_cancel in the dtor will explode if we don't do this... + memset(&_work, 0, sizeof(_work)); + warnx("MS5611 I2C constructor"); +} +MS5611_SPI::MS5611_SPI(int bus, const char* path, spi_dev_e device) : + SPI("MS5611", path, bus, device, SPIDEV_MODE3, 2000000), + _measure_ticks(0), + _num_reports(0), + _next_report(0), + _oldest_report(0), + _reports(nullptr), + _collect_phase(false), + _measure_phase(0), + _TEMP(0), + _OFF(0), + _SENS(0), + _msl_pressure(101325), + _baro_topic(-1), + _sample_perf(perf_alloc(PC_ELAPSED, "ms5611_read")), + _measure_perf(perf_alloc(PC_ELAPSED, "ms5611_measure")), + _comms_errors(perf_alloc(PC_COUNT, "ms5611_comms_errors")), + _buffer_overflows(perf_alloc(PC_COUNT, "ms5611_buffer_overflows")) +{ // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); + warnx("MS5611 SPI constructor"); +} + +MS5611_I2C::~MS5611_I2C() +{ + /* make sure we are truly inactive */ + stop_cycle(); + + /* free any existing reports */ + if (_reports != nullptr) + delete[] _reports; } -MS5611::~MS5611() +MS5611_SPI::~MS5611_SPI() { /* make sure we are truly inactive */ stop_cycle(); @@ -297,7 +488,7 @@ MS5611::~MS5611() } int -MS5611::init() +MS5611_I2C::init() { int ret = ERROR; @@ -327,8 +518,9 @@ out: } int -MS5611::probe() +MS5611_I2C::probe() { +#ifdef PX4_I2C_OBDEV_MS5611 _retries = 10; if ((OK == probe_address(MS5611_ADDRESS_1)) || @@ -340,12 +532,64 @@ MS5611::probe() _retries = 0; return OK; } +#endif return -EIO; } int -MS5611::probe_address(uint8_t address) +MS5611_SPI::init() +{ + int ret = ERROR; + + /* do SPI init (and probe) first */ + warnx("MS5611 SPI init function"); + if (SPI::init() != OK) { + warnx("SPI init failed"); + goto out; + } else { + warnx("SPI init ok"); + } + + /* allocate basic report buffers */ + _num_reports = 2; + _reports = new struct baro_report[_num_reports]; + + if (_reports == nullptr) + goto out; + + _oldest_report = _next_report = 0; + + /* 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"); + + ret = OK; +out: + return ret; +} + +int +MS5611_SPI::probe() +{ + + warnx("SPI probe"); + /* send reset command */ + if (OK != cmd_reset()) + return -EIO; + + /* read PROM */ + if (OK != read_prom()) + return -EIO; + + return OK; +} + +int +MS5611_I2C::probe_address(uint8_t address) { /* select the address we are going to try */ set_address(address); @@ -362,7 +606,7 @@ MS5611::probe_address(uint8_t address) } ssize_t -MS5611::read(struct file *filp, char *buffer, size_t buflen) +MS5611_I2C::read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct baro_report); int ret = 0; @@ -432,34 +676,238 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen) return ret; } -int -MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) +ssize_t +MS5611_SPI::read(struct file *filp, char *buffer, size_t buflen) { - switch (cmd) { - - case SENSORIOCSPOLLRATE: { - switch (arg) { + unsigned count = buflen / sizeof(struct baro_report); + int ret = 0; - /* switching to manual polling */ - case SENSOR_POLLRATE_MANUAL: - stop_cycle(); - _measure_ticks = 0; - return OK; + /* buffer must be large enough */ + if (count < 1) + return -ENOSPC; - /* external signalling not supported */ - case SENSOR_POLLRATE_EXTERNAL: + /* if automatic measurement is enabled */ + if (_measure_ticks > 0) { - /* zero would be bad */ - case 0: - return -EINVAL; + /* + * While there is space in the caller's buffer, and reports, copy them. + * Note that we may be pre-empted by the workq thread while we are doing this; + * we are careful to avoid racing with them. + */ + while (count--) { + if (_oldest_report != _next_report) { + memcpy(buffer, _reports + _oldest_report, sizeof(*_reports)); + ret += sizeof(_reports[0]); + INCREMENT(_oldest_report, _num_reports); + } + } - /* set default/max polling rate */ - case SENSOR_POLLRATE_MAX: - case SENSOR_POLLRATE_DEFAULT: { - /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } - /* set interval for next measurement to minimum legal value */ + /* manual measurement - run one conversion */ + /* XXX really it'd be nice to lock against other readers here */ + do { + _measure_phase = 0; + _oldest_report = _next_report = 0; + + /* do temperature first */ + if (OK != measure()) { + ret = -EIO; + break; + } + + usleep(MS5611_CONVERSION_INTERVAL); + + if (OK != collect()) { + ret = -EIO; + break; + } + + /* now do a pressure measurement */ + if (OK != measure()) { + ret = -EIO; + break; + } + + usleep(MS5611_CONVERSION_INTERVAL); + + if (OK != collect()) { + ret = -EIO; + break; + } + + /* state machine will have generated a report, copy it out */ + memcpy(buffer, _reports, sizeof(*_reports)); + ret = sizeof(*_reports); + + } while (0); + + return ret; +} + +int +MS5611_SPI::bus_ioctl(struct file *filp, int cmd, unsigned long arg) +{ + /* give it to the superclass */ + return SPI::ioctl(filp, cmd, arg); +} + +int +MS5611_I2C::bus_ioctl(struct file *filp, int cmd, unsigned long arg) +{ + /* give it to the superclass */ + return I2C::ioctl(filp, cmd, arg); +} + +int +MS5611_I2C::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop_cycle(); + _measure_ticks = 0; + return OK; + + /* external signalling not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* set interval for next measurement to minimum legal value */ + _measure_ticks = USEC2TICK(MS5611_CONVERSION_INTERVAL); + + /* if we need to start the poll state machine, do it */ + if (want_start) + start_cycle(); + + return OK; + } + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* convert hz to tick interval via microseconds */ + unsigned ticks = USEC2TICK(1000000 / arg); + + /* check against maximum rate */ + 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) + start_cycle(); + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_measure_ticks == 0) + return SENSOR_POLLRATE_MANUAL; + + return (1000 / _measure_ticks); + + case SENSORIOCSQUEUEDEPTH: { + /* add one to account for the sentinel in the ring */ + arg++; + + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 2) || (arg > 100)) + return -EINVAL; + + /* allocate new buffer */ + struct baro_report *buf = new struct baro_report[arg]; + + if (nullptr == buf) + return -ENOMEM; + + /* reset the measurement state machine with the new buffer, free the old */ + stop_cycle(); + delete[] _reports; + _num_reports = arg; + _reports = buf; + start_cycle(); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _num_reports - 1; + + case SENSORIOCRESET: + /* XXX implement this */ + return -EINVAL; + + case BAROIOCSMSLPRESSURE: + + /* range-check for sanity */ + if ((arg < 80000) || (arg > 120000)) + return -EINVAL; + + _msl_pressure = arg; + return OK; + + case BAROIOCGMSLPRESSURE: + return _msl_pressure; + + default: + break; + } + + /* give it to the bus-specific superclass */ + // return bus_ioctl(filp, cmd, arg); + return I2C::ioctl(filp, cmd, arg); +} + +int +MS5611_SPI::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop_cycle(); + _measure_ticks = 0; + return OK; + + /* external signalling not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* set interval for next measurement to minimum legal value */ _measure_ticks = USEC2TICK(MS5611_CONVERSION_INTERVAL); /* if we need to start the poll state machine, do it */ @@ -507,145 +955,498 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) if ((arg < 2) || (arg > 100)) return -EINVAL; - /* allocate new buffer */ - struct baro_report *buf = new struct baro_report[arg]; + /* allocate new buffer */ + struct baro_report *buf = new struct baro_report[arg]; + + if (nullptr == buf) + return -ENOMEM; + + /* reset the measurement state machine with the new buffer, free the old */ + stop_cycle(); + delete[] _reports; + _num_reports = arg; + _reports = buf; + start_cycle(); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _num_reports - 1; + + case SENSORIOCRESET: + /* XXX implement this */ + return -EINVAL; + + case BAROIOCSMSLPRESSURE: + + /* range-check for sanity */ + if ((arg < 80000) || (arg > 120000)) + return -EINVAL; + + _msl_pressure = arg; + return OK; + + case BAROIOCGMSLPRESSURE: + return _msl_pressure; + + default: + break; + } + + /* give it to the bus-specific superclass */ + // return bus_ioctl(filp, cmd, arg); + return SPI::ioctl(filp, cmd, arg); +} + +void +MS5611_I2C::start_cycle() +{ + + /* reset the report ring and state machine */ + _collect_phase = false; + _measure_phase = 0; + _oldest_report = _next_report = 0; + + /* schedule a cycle to start things */ + work_queue(HPWORK, &_work, (worker_t)&MS5611_I2C::cycle_trampoline, this, 1); +} + +void +MS5611_I2C::stop_cycle() +{ + work_cancel(HPWORK, &_work); +} + +void +MS5611_I2C::cycle_trampoline(void *arg) +{ + MS5611_I2C *dev = (MS5611_I2C *)arg; + + dev->cycle(); +} + +void +MS5611_I2C::cycle() +{ + int ret; + + /* collection phase? */ + if (_collect_phase) { + + /* perform collection */ + ret = collect(); + if (ret != OK) { + if (ret == -6) { + /* + * The ms5611 seems to regularly fail to respond to + * its address; this happens often enough that we'd rather not + * spam the console with the message. + */ + } else { + //log("collection error %d", ret); + } + /* reset the collection state machine and try again */ + start_cycle(); + return; + } + + /* next phase is measurement */ + _collect_phase = false; + + /* + * Is there a collect->measure gap? + * Don't inject one after temperature measurements, so we can keep + * doing pressure measurements at something close to the desired rate. + */ + if ((_measure_phase != 0) && + (_measure_ticks > USEC2TICK(MS5611_CONVERSION_INTERVAL))) { + + /* schedule a fresh cycle call when we are ready to measure again */ + work_queue(HPWORK, + &_work, + (worker_t)&MS5611_I2C::cycle_trampoline, + this, + _measure_ticks - USEC2TICK(MS5611_CONVERSION_INTERVAL)); + + return; + } + } + + /* measurement phase */ + ret = measure(); + if (ret != OK) { + //log("measure error %d", ret); + /* reset the collection state machine and try again */ + start_cycle(); + return; + } + + /* next phase is collection */ + _collect_phase = true; + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, + &_work, + (worker_t)&MS5611_I2C::cycle_trampoline, + this, + USEC2TICK(MS5611_CONVERSION_INTERVAL)); +} + +void +MS5611_SPI::start_cycle() +{ + + /* reset the report ring and state machine */ + _collect_phase = false; + _measure_phase = 0; + _oldest_report = _next_report = 0; + + /* schedule a cycle to start things */ + work_queue(HPWORK, &_work, (worker_t)&MS5611_SPI::cycle_trampoline, this, 1); +} + +void +MS5611_SPI::stop_cycle() +{ + work_cancel(HPWORK, &_work); +} + +void +MS5611_SPI::cycle_trampoline(void *arg) +{ + MS5611_SPI *dev = (MS5611_SPI *)arg; + + dev->cycle(); +} + +void +MS5611_SPI::cycle() +{ + int ret; + + /* collection phase? */ + if (_collect_phase) { + + /* perform collection */ + ret = collect(); + if (ret != OK) { + if (ret == -6) { + /* + * The ms5611 seems to regularly fail to respond to + * its address; this happens often enough that we'd rather not + * spam the console with the message. + */ + } else { + //log("collection error %d", ret); + } + /* reset the collection state machine and try again */ + start_cycle(); + return; + } + + /* next phase is measurement */ + _collect_phase = false; + + /* + * Is there a collect->measure gap? + * Don't inject one after temperature measurements, so we can keep + * doing pressure measurements at something close to the desired rate. + */ + if ((_measure_phase != 0) && + (_measure_ticks > USEC2TICK(MS5611_CONVERSION_INTERVAL))) { + + /* schedule a fresh cycle call when we are ready to measure again */ + work_queue(HPWORK, + &_work, + (worker_t)&MS5611_SPI::cycle_trampoline, + this, + _measure_ticks - USEC2TICK(MS5611_CONVERSION_INTERVAL)); + + return; + } + } + + /* measurement phase */ + ret = measure(); + if (ret != OK) { + //log("measure error %d", ret); + /* reset the collection state machine and try again */ + start_cycle(); + return; + } + + /* next phase is collection */ + _collect_phase = true; + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, + &_work, + (worker_t)&MS5611_SPI::cycle_trampoline, + this, + USEC2TICK(MS5611_CONVERSION_INTERVAL)); +} + +int +MS5611_I2C::measure() +{ + int ret; + + perf_begin(_measure_perf); + + /* + * In phase zero, request temperature; in other phases, request pressure. + */ + uint8_t cmd_data = (_measure_phase == 0) ? ADDR_CMD_CONVERT_D2 : ADDR_CMD_CONVERT_D1; + + /* + * Send the command to begin measuring. + * + * Disable retries on this command; we can't know whether failure + * means the device did or did not see the write. + */ + _retries = 0; + ret = transfer(&cmd_data, 1, nullptr, 0); + + if (OK != ret) + perf_count(_comms_errors); + + perf_end(_measure_perf); + + return ret; +} + +int +MS5611_I2C::collect() +{ + int ret; + uint8_t cmd; + uint8_t data[3]; + union { + uint8_t b[4]; + uint32_t w; + } cvt; + + /* read the most recent measurement */ + cmd = 0; + + perf_begin(_sample_perf); + + /* this should be fairly close to the end of the conversion, so the best approximation of the time */ + _reports[_next_report].timestamp = hrt_absolute_time(); + + ret = transfer(&cmd, 1, &data[0], 3); + if (ret != OK) { + perf_count(_comms_errors); + perf_end(_sample_perf); + return ret; + } + + /* fetch the raw value */ + cvt.b[0] = data[2]; + cvt.b[1] = data[1]; + cvt.b[2] = data[0]; + cvt.b[3] = 0; + uint32_t raw = cvt.w; + + /* handle a measurement */ + if (_measure_phase == 0) { + + /* temperature offset (in ADC units) */ + int32_t dT = (int32_t)raw - ((int32_t)_prom.s.c5_reference_temp << 8); + + /* absolute temperature in centidegrees - note intermediate value is outside 32-bit range */ + _TEMP = 2000 + (int32_t)(((int64_t)dT * _prom.s.c6_temp_coeff_temp) >> 23); + + /* base sensor scale/offset values */ + _SENS = ((int64_t)_prom.s.c1_pressure_sens << 15) + (((int64_t)_prom.s.c3_temp_coeff_pres_sens * dT) >> 8); + _OFF = ((int64_t)_prom.s.c2_pressure_offset << 16) + (((int64_t)_prom.s.c4_temp_coeff_pres_offset * dT) >> 7); + + /* temperature compensation */ + if (_TEMP < 2000) { + + int32_t T2 = POW2(dT) >> 31; + + int64_t f = POW2((int64_t)_TEMP - 2000); + int64_t OFF2 = 5 * f >> 1; + int64_t SENS2 = 5 * f >> 2; + + if (_TEMP < -1500) { + int64_t f2 = POW2(_TEMP + 1500); + OFF2 += 7 * f2; + SENS2 += 11 * f2 >> 1; + } + + _TEMP -= T2; + _OFF -= OFF2; + _SENS -= SENS2; + } + + } else { + + /* pressure calculation, result in Pa */ + int32_t P = (((raw * _SENS) >> 21) - _OFF) >> 15; + + /* generate a new report */ + _reports[_next_report].temperature = _TEMP / 100.0f; + _reports[_next_report].pressure = P / 100.0f; /* convert to millibar */ + + /* altitude calculations based on http://www.kansasflyer.org/index.asp?nav=Avi&sec=Alti&tab=Theory&pg=1 */ + + /* + * PERFORMANCE HINT: + * + * The single precision calculation is 50 microseconds faster than the double + * precision variant. It is however not obvious if double precision is required. + * Pending more inspection and tests, we'll leave the double precision variant active. + * + * Measurements: + * double precision: ms5611_read: 992 events, 258641us elapsed, min 202us max 305us + * single precision: ms5611_read: 963 events, 208066us elapsed, min 202us max 241us + */ + + /* tropospheric properties (0-11km) for standard atmosphere */ + const double T1 = 15.0 + 273.15; /* temperature at base height in Kelvin */ + const double a = -6.5 / 1000; /* temperature gradient in degrees per metre */ + const double g = 9.80665; /* gravity constant in m/s/s */ + const double R = 287.05; /* ideal gas constant in J/kg/K */ + + /* current pressure at MSL in kPa */ + double p1 = _msl_pressure / 1000.0; + + /* measured pressure in kPa */ + double p = P / 1000.0; + + /* + * Solve: + * + * / -(aR / g) \ + * | (p / p1) . T1 | - T1 + * \ / + * h = ------------------------------- + h1 + * a + */ + _reports[_next_report].altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; + + /* publish it */ + orb_publish(ORB_ID(sensor_baro), _baro_topic, &_reports[_next_report]); + + /* post a report to the ring - note, not locked */ + INCREMENT(_next_report, _num_reports); + + /* if we are running up against the oldest report, toss it */ + if (_next_report == _oldest_report) { + perf_count(_buffer_overflows); + INCREMENT(_oldest_report, _num_reports); + } + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + } + + /* update the measurement state machine */ + INCREMENT(_measure_phase, MS5611_MEASUREMENT_RATIO + 1); - if (nullptr == buf) - return -ENOMEM; + perf_end(_sample_perf); - /* reset the measurement state machine with the new buffer, free the old */ - stop_cycle(); - delete[] _reports; - _num_reports = arg; - _reports = buf; - start_cycle(); + return OK; +} - return OK; - } +int +MS5611_I2C::cmd_reset() +{ + unsigned old_retrycount = _retries; + uint8_t cmd = ADDR_RESET_CMD; + int result; - case SENSORIOCGQUEUEDEPTH: - return _num_reports - 1; + /* bump the retry count */ + _retries = 10; + result = transfer(&cmd, 1, nullptr, 0); + _retries = old_retrycount; - case SENSORIOCRESET: - /* XXX implement this */ - return -EINVAL; + return result; +} - case BAROIOCSMSLPRESSURE: +int +MS5611_I2C::read_prom() +{ + uint8_t prom_buf[2]; + union { + uint8_t b[2]; + uint16_t w; + } cvt; - /* range-check for sanity */ - if ((arg < 80000) || (arg > 120000)) - return -EINVAL; + /* + * Wait for PROM contents to be in the device (2.8 ms) in the case we are + * called immediately after reset. + */ + usleep(3000); - _msl_pressure = arg; - return OK; + /* read and convert PROM words */ + for (int i = 0; i < 8; i++) { + uint8_t cmd = ADDR_PROM_SETUP + (i * 2); - case BAROIOCGMSLPRESSURE: - return _msl_pressure; + if (OK != transfer(&cmd, 1, &prom_buf[0], 2)) + break; - default: - break; + /* assemble 16 bit value and convert from big endian (sensor) to little endian (MCU) */ + cvt.b[0] = prom_buf[1]; + cvt.b[1] = prom_buf[0]; + _prom.c[i] = cvt.w; } - /* give it to the superclass */ - return I2C::ioctl(filp, cmd, arg); + /* calculate CRC and return success/failure accordingly */ + return crc4(&_prom.c[0]) ? OK : -EIO; } -void -MS5611::start_cycle() +uint8_t +MS5611_SPI::read_reg(unsigned reg) { + uint8_t cmd[2]; - /* reset the report ring and state machine */ - _collect_phase = false; - _measure_phase = 0; - _oldest_report = _next_report = 0; + cmd[0] = reg | DIR_READ; - /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&MS5611::cycle_trampoline, this, 1); -} + transfer(cmd, cmd, sizeof(cmd)); -void -MS5611::stop_cycle() -{ - work_cancel(HPWORK, &_work); + return cmd[1]; } -void -MS5611::cycle_trampoline(void *arg) +uint16_t +MS5611_SPI::read_reg16(unsigned reg) { - MS5611 *dev = (MS5611 *)arg; + uint8_t cmd[3]; - dev->cycle(); + cmd[0] = reg | DIR_READ; + + transfer(cmd, cmd, sizeof(cmd)); + + return (uint16_t)(cmd[1] << 8) | cmd[2]; } void -MS5611::cycle() +MS5611_SPI::write_reg(unsigned reg, uint8_t value) { - int ret; - - /* collection phase? */ - if (_collect_phase) { - - /* perform collection */ - ret = collect(); - if (ret != OK) { - if (ret == -6) { - /* - * The ms5611 seems to regularly fail to respond to - * its address; this happens often enough that we'd rather not - * spam the console with the message. - */ - } else { - //log("collection error %d", ret); - } - /* reset the collection state machine and try again */ - start_cycle(); - return; - } - - /* next phase is measurement */ - _collect_phase = false; - - /* - * Is there a collect->measure gap? - * Don't inject one after temperature measurements, so we can keep - * doing pressure measurements at something close to the desired rate. - */ - if ((_measure_phase != 0) && - (_measure_ticks > USEC2TICK(MS5611_CONVERSION_INTERVAL))) { - - /* schedule a fresh cycle call when we are ready to measure again */ - work_queue(HPWORK, - &_work, - (worker_t)&MS5611::cycle_trampoline, - this, - _measure_ticks - USEC2TICK(MS5611_CONVERSION_INTERVAL)); + uint8_t cmd[2]; - return; - } - } + cmd[0] = reg | DIR_WRITE; + cmd[1] = value; - /* measurement phase */ - ret = measure(); - if (ret != OK) { - //log("measure error %d", ret); - /* reset the collection state machine and try again */ - start_cycle(); - return; - } + transfer(cmd, nullptr, sizeof(cmd)); +} - /* next phase is collection */ - _collect_phase = true; +void +MS5611_SPI::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) +{ + uint8_t val; - /* schedule a fresh cycle call when the measurement is done */ - work_queue(HPWORK, - &_work, - (worker_t)&MS5611::cycle_trampoline, - this, - USEC2TICK(MS5611_CONVERSION_INTERVAL)); + val = read_reg(reg); + val &= ~clearbits; + val |= setbits; + write_reg(reg, val); } int -MS5611::measure() +MS5611_SPI::measure() { int ret; @@ -655,6 +1456,7 @@ MS5611::measure() * In phase zero, request temperature; in other phases, request pressure. */ uint8_t cmd_data = (_measure_phase == 0) ? ADDR_CMD_CONVERT_D2 : ADDR_CMD_CONVERT_D1; + cmd_data |= DIR_WRITE; /* * Send the command to begin measuring. @@ -662,8 +1464,7 @@ MS5611::measure() * Disable retries on this command; we can't know whether failure * means the device did or did not see the write. */ - _retries = 0; - ret = transfer(&cmd_data, 1, nullptr, 0); + ret = transfer(&cmd_data, nullptr, 1); if (OK != ret) perf_count(_comms_errors); @@ -674,25 +1475,25 @@ MS5611::measure() } int -MS5611::collect() +MS5611_SPI::collect() { int ret; - uint8_t cmd; - uint8_t data[3]; + + uint8_t data[4]; union { uint8_t b[4]; uint32_t w; } cvt; /* read the most recent measurement */ - cmd = 0; + data[0] = 0 | DIR_WRITE; perf_begin(_sample_perf); /* this should be fairly close to the end of the conversion, so the best approximation of the time */ _reports[_next_report].timestamp = hrt_absolute_time(); - ret = transfer(&cmd, 1, &data[0], 3); + ret = transfer(&data[0], &data[0], sizeof(data)); if (ret != OK) { perf_count(_comms_errors); perf_end(_sample_perf); @@ -700,9 +1501,9 @@ MS5611::collect() } /* fetch the raw value */ - cvt.b[0] = data[2]; - cvt.b[1] = data[1]; - cvt.b[2] = data[0]; + cvt.b[0] = data[3]; + cvt.b[1] = data[2]; + cvt.b[2] = data[1]; cvt.b[3] = 0; uint32_t raw = cvt.w; @@ -761,30 +1562,7 @@ MS5611::collect() * double precision: ms5611_read: 992 events, 258641us elapsed, min 202us max 305us * single precision: ms5611_read: 963 events, 208066us elapsed, min 202us max 241us */ -#if 0/* USE_FLOAT */ - /* tropospheric properties (0-11km) for standard atmosphere */ - const float T1 = 15.0f + 273.15f; /* temperature at base height in Kelvin */ - const float a = -6.5f / 1000f; /* temperature gradient in degrees per metre */ - const float g = 9.80665f; /* gravity constant in m/s/s */ - const float R = 287.05f; /* ideal gas constant in J/kg/K */ - - /* current pressure at MSL in kPa */ - float p1 = _msl_pressure / 1000.0f; - - /* measured pressure in kPa */ - float p = P / 1000.0f; - /* - * Solve: - * - * / -(aR / g) \ - * | (p / p1) . T1 | - T1 - * \ / - * h = ------------------------------- + h1 - * a - */ - _reports[_next_report].altitude = (((powf((p / p1), (-(a * R) / g))) * T1) - T1) / a; -#else /* tropospheric properties (0-11km) for standard atmosphere */ const double T1 = 15.0 + 273.15; /* temperature at base height in Kelvin */ const double a = -6.5 / 1000; /* temperature gradient in degrees per metre */ @@ -807,7 +1585,7 @@ MS5611::collect() * a */ _reports[_next_report].altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; -#endif + /* publish it */ orb_publish(ORB_ID(sensor_baro), _baro_topic, &_reports[_next_report]); @@ -833,24 +1611,21 @@ MS5611::collect() } int -MS5611::cmd_reset() +MS5611_SPI::cmd_reset() { - unsigned old_retrycount = _retries; - uint8_t cmd = ADDR_RESET_CMD; + uint8_t cmd = ADDR_RESET_CMD | DIR_WRITE; int result; - /* bump the retry count */ - _retries = 10; - result = transfer(&cmd, 1, nullptr, 0); - _retries = old_retrycount; + result = transfer(&cmd, nullptr, 1); + warnx("transferred reset, result: %d", result); return result; } int -MS5611::read_prom() +MS5611_SPI::read_prom() { - uint8_t prom_buf[2]; + uint8_t prom_buf[3]; union { uint8_t b[2]; uint16_t w; @@ -864,23 +1639,91 @@ MS5611::read_prom() /* read and convert PROM words */ 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] = read_reg16(cmd); + } - if (OK != transfer(&cmd, 1, &prom_buf[0], 2)) - break; + warnx("going for CRC"); - /* assemble 16 bit value and convert from big endian (sensor) to little endian (MCU) */ - cvt.b[0] = prom_buf[1]; - cvt.b[1] = prom_buf[0]; - _prom.c[i] = cvt.w; + /* calculate CRC and return success/failure accordingly */ + int ret = crc4(&_prom.c[0]) ? OK : -EIO; + if (ret == OK) { + warnx("CRC OK"); + } else { + warnx("CRC FAIL"); } + return ret; +} - /* calculate CRC and return success/failure accordingly */ - return crc4(&_prom.c[0]) ? OK : -EIO; +bool +MS5611_I2C::crc4(uint16_t *n_prom) +{ + int16_t cnt; + uint16_t n_rem; + uint16_t crc_read; + uint8_t n_bit; + + n_rem = 0x00; + + /* save the read crc */ + crc_read = n_prom[7]; + + /* remove CRC byte */ + n_prom[7] = (0xFF00 & (n_prom[7])); + + for (cnt = 0; cnt < 16; cnt++) { + /* uneven bytes */ + if (cnt & 1) { + n_rem ^= (uint8_t)((n_prom[cnt >> 1]) & 0x00FF); + + } else { + n_rem ^= (uint8_t)(n_prom[cnt >> 1] >> 8); + } + + for (n_bit = 8; n_bit > 0; n_bit--) { + if (n_rem & 0x8000) { + n_rem = (n_rem << 1) ^ 0x3000; + + } else { + n_rem = (n_rem << 1); + } + } + } + + /* final 4 bit remainder is CRC value */ + n_rem = (0x000F & (n_rem >> 12)); + n_prom[7] = crc_read; + + /* return true if CRCs match */ + return (0x000F & crc_read) == (n_rem ^ 0x00); +} + +void +MS5611_I2C::print_info() +{ + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + perf_print_counter(_buffer_overflows); + printf("poll interval: %u ticks\n", _measure_ticks); + printf("report queue: %u (%u/%u @ %p)\n", + _num_reports, _oldest_report, _next_report, _reports); + printf("TEMP: %d\n", _TEMP); + printf("SENS: %lld\n", _SENS); + printf("OFF: %lld\n", _OFF); + printf("MSL pressure: %10.4f\n", (double)(_msl_pressure / 100.f)); + + printf("factory_setup %u\n", _prom.s.factory_setup); + printf("c1_pressure_sens %u\n", _prom.s.c1_pressure_sens); + printf("c2_pressure_offset %u\n", _prom.s.c2_pressure_offset); + printf("c3_temp_coeff_pres_sens %u\n", _prom.s.c3_temp_coeff_pres_sens); + printf("c4_temp_coeff_pres_offset %u\n", _prom.s.c4_temp_coeff_pres_offset); + printf("c5_reference_temp %u\n", _prom.s.c5_reference_temp); + printf("c6_temp_coeff_temp %u\n", _prom.s.c6_temp_coeff_temp); + printf("serial_and_crc %u\n", _prom.s.serial_and_crc); } bool -MS5611::crc4(uint16_t *n_prom) +MS5611_SPI::crc4(uint16_t *n_prom) { int16_t cnt; uint16_t n_rem; @@ -923,7 +1766,7 @@ MS5611::crc4(uint16_t *n_prom) } void -MS5611::print_info() +MS5611_SPI::print_info() { perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); @@ -952,7 +1795,7 @@ MS5611::print_info() namespace ms5611 { -MS5611 *g_dev; +device::CDev *g_dev; void start(); void test(); @@ -971,8 +1814,21 @@ start() if (g_dev != nullptr) errx(1, "already started"); - /* create the driver */ - g_dev = new MS5611(MS5611_BUS); + /* create the driver, try SPI first, fall back to I2C if required */ + #ifdef PX4_SPIDEV_BARO + { + warnx("Attempting SPI start"); + g_dev = new MS5611_SPI(1 /* XXX magic number, SPI1 */, BARO_DEVICE_PATH,(spi_dev_e)PX4_SPIDEV_BARO); + } + #endif + /* try I2C if configuration exists and SPI failed*/ + #ifdef MS5611_BUS + if (g_dev == nullptr) { + warnx("Attempting I2C start"); + g_dev = new MS5611_I2C(MS5611_BUS); + } + + #endif if (g_dev == nullptr) goto fail; @@ -1096,7 +1952,8 @@ info() errx(1, "driver not running"); printf("state @ %p\n", g_dev); - g_dev->print_info(); + MS5611_SPI* spi = (MS5611_SPI*)g_dev; + spi->print_info(); exit(0); } @@ -1154,11 +2011,11 @@ calibrate(unsigned altitude) const float g = 9.80665f; /* gravity constant in m/s/s */ const float R = 287.05f; /* ideal gas constant in J/kg/K */ - warnx("averaged pressure %10.4fkPa at %um", pressure, altitude); + warnx("averaged pressure %10.4fkPa at %um", (double)pressure, altitude); p1 = pressure * (powf(((T1 + (a * (float)altitude)) / T1), (g / (a * R)))); - warnx("calculated MSL pressure %10.4fkPa", p1); + warnx("calculated MSL pressure %10.4fkPa", (double)p1); /* save as integer Pa */ p1 *= 1000.0f; @@ -1211,4 +2068,4 @@ ms5611_main(int argc, char *argv[]) } errx(1, "unrecognised command, try 'start', 'test', 'reset' or 'info'"); -} +} \ No newline at end of file -- cgit v1.2.3