diff options
Diffstat (limited to 'apps/drivers/ms5611/ms5611.cpp')
-rw-r--r-- | apps/drivers/ms5611/ms5611.cpp | 1214 |
1 files changed, 0 insertions, 1214 deletions
diff --git a/apps/drivers/ms5611/ms5611.cpp b/apps/drivers/ms5611/ms5611.cpp deleted file mode 100644 index 59ab5936e..000000000 --- a/apps/drivers/ms5611/ms5611.cpp +++ /dev/null @@ -1,1214 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file ms5611.cpp - * Driver for the MS5611 barometric pressure sensor connected via I2C. - */ - -#include <nuttx/config.h> - -#include <drivers/device/i2c.h> - -#include <sys/types.h> -#include <stdint.h> -#include <stdbool.h> -#include <stdlib.h> -#include <semaphore.h> -#include <string.h> -#include <fcntl.h> -#include <poll.h> -#include <errno.h> -#include <stdio.h> -#include <math.h> -#include <unistd.h> - -#include <nuttx/arch.h> -#include <nuttx/wqueue.h> -#include <nuttx/clock.h> - -#include <arch/board/board.h> - -#include <drivers/drv_hrt.h> - -#include <systemlib/perf_counter.h> -#include <systemlib/err.h> - -#include <drivers/drv_baro.h> - -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; - -#ifndef CONFIG_SCHED_WORKQUEUE -# error This requires CONFIG_SCHED_WORKQUEUE. -#endif - -/** - * Calibration PROM as reported by the device. - */ -#pragma pack(push,1) -struct ms5611_prom_s { - uint16_t factory_setup; - uint16_t c1_pressure_sens; - uint16_t c2_pressure_offset; - uint16_t c3_temp_coeff_pres_sens; - uint16_t c4_temp_coeff_pres_offset; - uint16_t c5_reference_temp; - uint16_t c6_temp_coeff_temp; - uint16_t serial_and_crc; -}; - -/** - * Grody hack for crc4() - */ -union ms5611_prom_u { - uint16_t c[8]; - struct ms5611_prom_s s; -}; -#pragma pack(pop) - -class MS5611 : public device::I2C -{ -public: - MS5611(int bus); - virtual ~MS5611(); - - 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(); - -private: - 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; - - /** - * 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); - - /** - * Initialise 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. - */ - int measure(); - - /** - * Collect the result of the most recent measurement. - */ - int collect(); - - /** - * Send a reset command to the MS5611. - * - * This is required after any bus reset. - */ - int cmd_reset(); - - /** - * Read the MS5611 PROM - * - * @return OK if the PROM reads successfully. - */ - int read_prom(); - - /** - * 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); - -}; - -/* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) - -/* helper macro for arithmetic - returns the square of the argument */ -#define POW2(_x) ((_x) * (_x)) - -/* - * MS5611 internal constants and data structures. - */ - -/* internal conversion time: 9.17 ms, so should not be read at rates higher than 100 Hz */ -#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+) */ -#define MS5611_ADDRESS_2 0x77 /* address select pins pulled low (PX4FMU prototypes) */ - -#define ADDR_RESET_CMD 0x1E /* write to this address to reset chip */ -#define ADDR_CMD_CONVERT_D1 0x48 /* write to this address to start temperature conversion */ -#define ADDR_CMD_CONVERT_D2 0x58 /* write to this address to start pressure conversion */ -#define ADDR_DATA 0x00 /* address of 3 bytes / 32bit pressure data */ -#define ADDR_PROM_SETUP 0xA0 /* address of 8x 2 bytes factory and calibration data */ -#define ADDR_PROM_C1 0xA2 /* address of 6x 2 bytes calibration data */ - -/* - * Driver 'main' command. - */ -extern "C" __EXPORT int ms5611_main(int argc, char *argv[]); - - -MS5611::MS5611(int bus) : - I2C("MS5611", BARO_DEVICE_PATH, bus, 0, 400000), - _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")) -{ - // enable debug() calls - _debug_enabled = true; - - // work_cancel in the dtor will explode if we don't do this... - memset(&_work, 0, sizeof(_work)); -} - -MS5611::~MS5611() -{ - /* make sure we are truly inactive */ - stop_cycle(); - - /* free any existing reports */ - if (_reports != nullptr) - delete[] _reports; -} - -int -MS5611::init() -{ - int ret = ERROR; - - /* do I2C init (and probe) first */ - if (I2C::init() != OK) - goto out; - - /* 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::probe() -{ - _retries = 10; - - if ((OK == probe_address(MS5611_ADDRESS_1)) || - (OK == probe_address(MS5611_ADDRESS_2))) { - /* - * Disable retries; we may enable them selectively in some cases, - * but the device gets confused if we retry some of the commands. - */ - _retries = 0; - return OK; - } - - return -EIO; -} - -int -MS5611::probe_address(uint8_t address) -{ - /* select the address we are going to try */ - set_address(address); - - /* send reset command */ - if (OK != cmd_reset()) - return -EIO; - - /* read PROM */ - if (OK != read_prom()) - return -EIO; - - return OK; -} - -ssize_t -MS5611::read(struct file *filp, char *buffer, size_t buflen) -{ - unsigned count = buflen / sizeof(struct baro_report); - int ret = 0; - - /* buffer must be large enough */ - if (count < 1) - return -ENOSPC; - - /* if automatic measurement is enabled */ - if (_measure_ticks > 0) { - - /* - * 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); - } - } - - /* if there was no data, warn the caller */ - return ret ? ret : -EAGAIN; - } - - /* 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::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 superclass */ - return I2C::ioctl(filp, cmd, arg); -} - -void -MS5611::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::cycle_trampoline, this, 1); -} - -void -MS5611::stop_cycle() -{ - work_cancel(HPWORK, &_work); -} - -void -MS5611::cycle_trampoline(void *arg) -{ - MS5611 *dev = (MS5611 *)arg; - - dev->cycle(); -} - -void -MS5611::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::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::cycle_trampoline, - this, - USEC2TICK(MS5611_CONVERSION_INTERVAL)); -} - -int -MS5611::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::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 - */ -#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 */ - 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; -#endif - /* 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); - - perf_end(_sample_perf); - - return OK; -} - -int -MS5611::cmd_reset() -{ - unsigned old_retrycount = _retries; - uint8_t cmd = ADDR_RESET_CMD; - int result; - - /* bump the retry count */ - _retries = 10; - result = transfer(&cmd, 1, nullptr, 0); - _retries = old_retrycount; - - return result; -} - -int -MS5611::read_prom() -{ - uint8_t prom_buf[2]; - union { - uint8_t b[2]; - uint16_t w; - } cvt; - - /* - * Wait for PROM contents to be in the device (2.8 ms) in the case we are - * called immediately after reset. - */ - usleep(3000); - - /* read and convert PROM words */ - for (int i = 0; i < 8; i++) { - uint8_t cmd = ADDR_PROM_SETUP + (i * 2); - - if (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]; - cvt.b[1] = prom_buf[0]; - _prom.c[i] = cvt.w; - } - - /* calculate CRC and return success/failure accordingly */ - return crc4(&_prom.c[0]) ? OK : -EIO; -} - -bool -MS5611::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::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); -} - -/** - * Local functions in support of the shell command. - */ -namespace ms5611 -{ - -MS5611 *g_dev; - -void start(); -void test(); -void reset(); -void info(); -void calibrate(unsigned altitude); - -/** - * Start the driver. - */ -void -start() -{ - int fd; - - if (g_dev != nullptr) - errx(1, "already started"); - - /* create the driver */ - g_dev = new MS5611(MS5611_BUS); - - if (g_dev == nullptr) - goto fail; - - if (OK != g_dev->init()) - goto fail; - - /* 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"); -} - -/** - * Perform some basic functional tests on the driver; - * make sure we can collect data from the sensor in polled - * and automatic modes. - */ -void -test() -{ - struct baro_report report; - ssize_t sz; - 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"); - - warnx("single read"); - warnx("pressure: %10.4f", (double)report.pressure); - warnx("altitude: %11.4f", (double)report.altitude); - warnx("temperature: %8.4f", (double)report.temperature); - warnx("time: %lld", report.timestamp); - - /* set the queue depth to 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)) - errx(1, "failed to set 2Hz poll rate"); - - /* read the sensor 5x and report each value */ - for (unsigned i = 0; i < 5; i++) { - struct pollfd fds; - - /* wait for data to be ready */ - fds.fd = fd; - fds.events = POLLIN; - ret = poll(&fds, 1, 2000); - - 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)) - err(1, "periodic read failed"); - - warnx("periodic read %u", i); - warnx("pressure: %10.4f", (double)report.pressure); - warnx("altitude: %11.4f", (double)report.altitude); - warnx("temperature: %8.4f", (double)report.temperature); - warnx("time: %lld", report.timestamp); - } - - errx(0, "PASS"); -} - -/** - * Reset the driver. - */ -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"); - - exit(0); -} - -/** - * Print a little info about the driver. - */ -void -info() -{ - if (g_dev == nullptr) - errx(1, "driver not running"); - - printf("state @ %p\n", g_dev); - g_dev->print_info(); - - exit(0); -} - -/** - * Calculate actual MSL pressure given current altitude - */ -void -calibrate(unsigned altitude) -{ - struct baro_report report; - float pressure; - 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); - - /* start the sensor polling at max */ - if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX)) - errx(1, "failed to set poll rate"); - - /* average a few measurements */ - pressure = 0.0f; - - for (unsigned i = 0; i < 20; i++) { - struct pollfd fds; - int ret; - ssize_t sz; - - /* wait for data to be ready */ - fds.fd = fd; - fds.events = POLLIN; - ret = poll(&fds, 1, 1000); - - 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)) - err(1, "sensor read failed"); - - pressure += report.pressure; - } - - pressure /= 20; /* average */ - pressure /= 10; /* scale from millibar to kPa */ - - /* tropospheric properties (0-11km) for standard atmosphere */ - const float T1 = 15.0 + 273.15; /* temperature at base height in Kelvin */ - const float a = -6.5 / 1000; /* 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 */ - - warnx("averaged pressure %10.4fkPa at %um", pressure, altitude); - - p1 = pressure * (powf(((T1 + (a * (float)altitude)) / T1), (g / (a * R)))); - - warnx("calculated MSL pressure %10.4fkPa", p1); - - /* save as integer Pa */ - p1 *= 1000.0f; - - if (ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)p1) != OK) - err(1, "BAROIOCSMSLPRESSURE"); - - exit(0); -} - -} // namespace - -int -ms5611_main(int argc, char *argv[]) -{ - /* - * Start/load the driver. - */ - if (!strcmp(argv[1], "start")) - ms5611::start(); - - /* - * Test the driver/device. - */ - if (!strcmp(argv[1], "test")) - ms5611::test(); - - /* - * Reset the driver. - */ - if (!strcmp(argv[1], "reset")) - ms5611::reset(); - - /* - * Print driver information. - */ - if (!strcmp(argv[1], "info")) - ms5611::info(); - - /* - * Perform MSL pressure calibration given an altitude in metres - */ - if (!strcmp(argv[1], "calibrate")) { - if (argc < 2) - errx(1, "missing altitude"); - - long altitude = strtol(argv[2], nullptr, 10); - - ms5611::calibrate(altitude); - } - - errx(1, "unrecognised command, try 'start', 'test', 'reset' or 'info'"); -} |