diff --git a/src/drivers/barometer/ms5837/CMakeLists.txt b/src/drivers/barometer/ms5837/CMakeLists.txt new file mode 100644 index 000000000000..532a31f6a5f3 --- /dev/null +++ b/src/drivers/barometer/ms5837/CMakeLists.txt @@ -0,0 +1,46 @@ +############################################################################ +# +# Copyright (c) 2022 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. +# +############################################################################ + +px4_add_module( + MODULE drivers__barometer__ms5837 + MAIN ms5837 + COMPILE_FLAGS + SRCS + ms5837_main.cpp + ms5837_registers.h + MS5837.cpp + MS5837.hpp + DEPENDS + drivers_barometer + px4_work_queue + ) diff --git a/src/drivers/barometer/ms5837/Kconfig b/src/drivers/barometer/ms5837/Kconfig new file mode 100644 index 000000000000..8d71727de622 --- /dev/null +++ b/src/drivers/barometer/ms5837/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_BAROMETER_MS5837 + bool "ms5837" + default n + ---help--- + Enable support for ms5837 diff --git a/src/drivers/barometer/ms5837/MS5837.cpp b/src/drivers/barometer/ms5837/MS5837.cpp new file mode 100644 index 000000000000..976988893acd --- /dev/null +++ b/src/drivers/barometer/ms5837/MS5837.cpp @@ -0,0 +1,441 @@ +/**************************************************************************** + * + * Copyright (c) 2022 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 ms5837.cpp + * Driver for the MS5837 barometric pressure sensor connected via I2C. + */ + +#include "MS5837.hpp" + +MS5837::MS5837(const I2CSPIDriverConfig &config) : + I2C(config), + I2CSPIDriver(config), + _px4_barometer(get_device_id()), + _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), + _measure_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": measure")), + _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": com_err")) +{ +} + +MS5837::~MS5837() +{ + // free perf counters + perf_free(_sample_perf); + perf_free(_measure_perf); + perf_free(_comms_errors); +} + +int MS5837::init() +{ + + int ret = I2C::init(); + + if (ret != PX4_OK) { + DEVICE_DEBUG("I2C::init failed (%i)", ret); + return ret; + } + + /* do a first measurement cycle to populate reports with valid data */ + _measure_phase = 0; + + while (true) { + /* do temperature first */ + if (OK != _measure()) { + ret = -EIO; + break; + } + + px4_usleep(MS5837_CONVERSION_INTERVAL); + + if (OK != _collect()) { + ret = -EIO; + break; + } + + /* now do a pressure measurement */ + if (OK != _measure()) { + ret = -EIO; + break; + } + + px4_usleep(MS5837_CONVERSION_INTERVAL); + + if (OK != _collect()) { + ret = -EIO; + break; + } + + _px4_barometer.set_device_type(DRV_BARO_DEVTYPE_MS5837); + + ret = OK; + + break; + + } + + if (ret == 0) { + _start(); + } + + return ret; +} + +int MS5837::_reset() +{ + unsigned old_retrycount = _retries; + uint8_t cmd = MS5837_RESET; + + /* bump the retry count */ + _retries = 3; + int result = transfer(&cmd, 1, nullptr, 0); + _retries = old_retrycount; + + return result; +} + +int MS5837::probe() +{ + if ((PX4_OK == _probe_address(MS5837_ADDRESS))) { + + return PX4_OK; + } + + _retries = 1; + + return -EIO; +} + +int MS5837::_probe_address(uint8_t address) +{ + /* select the address we are going to try */ + set_device_address(address); + + /* send reset command */ + if (PX4_OK != _reset()) { + return -EIO; + } + + /* read PROM */ + if (PX4_OK != _read_prom()) { + return -EIO; + } + + return PX4_OK; +} + +int MS5837::read(unsigned offset, void *data, unsigned count) +{ + union _cvt { + uint8_t b[4]; + uint32_t w; + } *cvt = (_cvt *)data; + uint8_t buf[3]; + + /* read the most recent measurement */ + uint8_t cmd = 0; + int ret = transfer(&cmd, 1, &buf[0], 3); + + if (ret == PX4_OK) { + /* fetch the raw value */ + cvt->b[0] = buf[2]; + cvt->b[1] = buf[1]; + cvt->b[2] = buf[0]; + cvt->b[3] = 0; + } + + return ret; +} + +void MS5837::RunImpl() +{ + int ret; + + /* collection phase? */ + if (_collect_phase) { + + /* perform collection */ + ret = _collect(); + + if (ret != OK) { + if (ret == -6) { + /* + * The ms5837 seems to regularly fail to respond to + * its address; this happens often enough that we'd rather not + * spam the console with a message for this. + */ + } else { + //DEVICE_LOG("collection error %d", ret); + } + + /* issue a reset command to the sensor */ + _reset(); + /* reset the collection state machine and try again - we need + * to wait 2.8 ms after issuing the sensor reset command + * according to the MS5837 datasheet + */ + ScheduleDelayed(2800); + return; + } + + /* next phase is measurement */ + _collect_phase = false; + } + + /* measurement phase */ + ret = _measure(); + + if (ret != OK) { + /* issue a reset command to the sensor */ + _reset(); + /* reset the collection state machine and try again */ + _start(); + return; + } + + /* next phase is collection */ + _collect_phase = true; + + /* schedule a fresh cycle call when the measurement is done */ + ScheduleDelayed(MS5837_CONVERSION_INTERVAL); +} + +void MS5837::_start() +{ + /* reset the report ring and state machine */ + _collect_phase = false; + _measure_phase = 0; + + /* schedule a cycle to start things */ + ScheduleDelayed(MS5837_CONVERSION_INTERVAL); +} + +int MS5837::_measure() +{ + perf_begin(_measure_perf); + + /* + * In phase zero, request temperature; in other phases, request pressure. + */ + unsigned addr = (_measure_phase == 0) ? ADDR_CMD_CONVERT_D2 : ADDR_CMD_CONVERT_D1; + + /* + * Send the command to begin measuring. + */ + uint8_t cmd = addr; + int ret = transfer(&cmd, 1, nullptr, 0); + + if (OK != ret) { + perf_count(_comms_errors); + } + + _px4_barometer.set_error_count(perf_event_count(_comms_errors)); + + perf_end(_measure_perf); + + return ret; +} + +int MS5837::_collect() +{ + uint32_t raw; + + perf_begin(_sample_perf); + + /* read the most recent measurement - read offset/size are hardcoded in the interface */ + const hrt_abstime timestamp_sample = hrt_absolute_time(); + int ret = read(0, (void *)&raw, 0); + + if (ret < 0) { + perf_count(_comms_errors); + perf_end(_sample_perf); + return ret; + } + + /* 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 */ + int32_t TEMP = 2000 + (int32_t)(((int64_t)dT * _prom.s.c6_temp_coeff_temp) >> 23); + + /* base sensor scale/offset values */ + + /* Perform MS5837 Caculation */ + _OFF = ((int64_t)_prom.s.c2_pressure_offset << 16) + (((int64_t)_prom.s.c4_temp_coeff_pres_offset * dT) >> 7); + _SENS = ((int64_t)_prom.s.c1_pressure_sens << 15) + (((int64_t)_prom.s.c3_temp_coeff_pres_sens * dT) >> 8); + + /* MS5837 temperature compensation */ + int64_t T2 = 0; + + int64_t f = 0; + int64_t OFF2 = 0; + int64_t SENS2 = 0; + + if (TEMP < 2000) { + + T2 = 3 * ((int64_t)POW2(dT) >> 33); + + f = POW2((int64_t)TEMP - 2000); + OFF2 = 3 * f >> 1; + SENS2 = 5 * f >> 3; + + if (TEMP < -1500) { + + int64_t f2 = POW2(TEMP + 1500); + OFF2 += 7 * f2; + SENS2 += f2 << 2; + } + + } else if (TEMP >= 2000) { + T2 = 2 * ((int64_t)POW2(dT) >> 37); + + f = POW2((int64_t)TEMP - 2000); + OFF2 = 1 * f >> 4; + SENS2 = 0; + } + + TEMP -= (int32_t)T2; + _OFF -= OFF2; + _SENS -= SENS2; + + + float temperature = TEMP / 100.0f; + _px4_barometer.set_temperature(temperature); + + } else { + /* pressure calculation, result in Pa */ + int32_t P = (((raw * _SENS) >> 21) - _OFF) >> 13; + + float pressure = P / 10.0f; /* convert to millibar */ + + _px4_barometer.update(timestamp_sample, pressure); + } + + /* update the measurement state machine */ + INCREMENT(_measure_phase, MS5837_MEASUREMENT_RATIO + 1); + + perf_end(_sample_perf); + + return OK; +} + +void MS5837::print_status() +{ + I2CSPIDriverBase::print_status(); + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + + printf("pressure: %f\n", (double)_px4_barometer.get().pressure); + printf("temperature: %f\n", (double)_px4_barometer.get().temperature); +} + +int MS5837::_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. + */ + px4_usleep(3000); + + uint8_t last_val = 0; + bool bits_stuck = true; + + /* read and convert PROM words */ + for (int i = 0; i < 7; i++) { + uint8_t cmd = MS5837_PROM_READ + (i * 2); + + if (PX4_OK != transfer(&cmd, 1, &prom_buf[0], 2)) { + break; + } + + /* check if all bytes are zero */ + if (i == 0) { + /* initialize to first byte read */ + last_val = prom_buf[0]; + } + + if ((prom_buf[0] != last_val) || (prom_buf[1] != last_val)) { + bits_stuck = false; + } + + /* 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]) && !bits_stuck) ? PX4_OK : -EIO; +} + +/** + * MS5837 crc4 cribbed from the datasheet + */ +bool MS5837::_crc4(uint16_t *n_prom) +{ + uint16_t n_rem = 0; + uint16_t crcRead = n_prom[0] >> 12; + n_prom[0] = ((n_prom[0]) & 0x0FFF); + n_prom[7] = 0; + + for (uint8_t i = 0 ; i < 16; i++) { + if (i % 2 == 1) { + n_rem ^= (uint16_t)((n_prom[i >> 1]) & 0x00FF); + + } else { + n_rem ^= (uint16_t)(n_prom[i >> 1] >> 8); + } + + for (uint8_t n_bit = 8 ; n_bit > 0 ; n_bit--) { + if (n_rem & 0x8000) { + n_rem = (n_rem << 1) ^ 0x3000; + + } else { + n_rem = (n_rem << 1); + } + } + } + + n_rem = ((n_rem >> 12) & 0x000F); + + return (n_rem ^ 0x00) == crcRead; +} diff --git a/src/drivers/barometer/ms5837/MS5837.hpp b/src/drivers/barometer/ms5837/MS5837.hpp new file mode 100644 index 000000000000..baad33a7081f --- /dev/null +++ b/src/drivers/barometer/ms5837/MS5837.hpp @@ -0,0 +1,134 @@ +/**************************************************************************** + * + * Copyright (c) 2022 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. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include "ms5837_registers.h" + +/* helper macro for handling report buffer indices */ +#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0) + +/* helper macro for arithmetic - returns the square of the argument */ +#define POW2(_x) ((_x) * (_x)) + +class MS5837 : public device::I2C, public I2CSPIDriver +{ +public: + MS5837(const I2CSPIDriverConfig &config); + ~MS5837() override; + + static void print_usage(); + + int init(); + + /** + * 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 RunImpl(); + void print_status() override; + int read(unsigned offset, void *data, unsigned count) override; + +private: + int probe() override; + + PX4Barometer _px4_barometer; + + ms5837::prom_u _prom{}; + + bool _collect_phase{false}; + unsigned _measure_phase{false}; + + /* intermediate temperature values per MS5611/MS5607 datasheet */ + int64_t _OFF{0}; + int64_t _SENS{0}; + + perf_counter_t _sample_perf; + perf_counter_t _measure_perf; + perf_counter_t _comms_errors; + + /** + * 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(); + + /** + * 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(); + + int _probe_address(uint8_t address); + + /** + * Send a reset command to the MS5837. + * + * This is required after any bus reset. + */ + int _reset(); + + /** + * Read the MS5837 PROM + * + * @return PX4_OK if the PROM reads successfully. + */ + int _read_prom(); + + bool _crc4(uint16_t *n_prom); +}; diff --git a/src/drivers/barometer/ms5837/ms5837_main.cpp b/src/drivers/barometer/ms5837/ms5837_main.cpp new file mode 100644 index 000000000000..ef6084c547e2 --- /dev/null +++ b/src/drivers/barometer/ms5837/ms5837_main.cpp @@ -0,0 +1,82 @@ +/**************************************************************************** + * + * Copyright (c) 2022 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. + * + ****************************************************************************/ + +#include +#include +#include +#include + +#include "MS5837.hpp" + +void MS5837::print_usage() +{ + PRINT_MODULE_USAGE_NAME("ms5837", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("baro"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +} + +extern "C" int ms5837_main(int argc, char *argv[]) +{ + using ThisDriver = MS5837; + BusCLIArguments cli{true, false}; + cli.default_i2c_frequency = 400000; + uint16_t dev_type_driver = DRV_BARO_DEVTYPE_MS5837; + + const char *verb = cli.parseDefaultArguments(argc, argv); + + if (!verb) { + ThisDriver::print_usage(); + return -1; + } + + cli.i2c_address = MS5837_ADDRESS; + + BusInstanceIterator iterator(MODULE_NAME, cli, dev_type_driver); + + if (!strcmp(verb, "start")) { + return ThisDriver::module_start(cli, iterator); + } + + if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); + } + + if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); + } + + ThisDriver::print_usage(); + return -1; +} diff --git a/src/drivers/barometer/ms5837/ms5837_registers.h b/src/drivers/barometer/ms5837/ms5837_registers.h new file mode 100644 index 000000000000..75ef7a6ddb12 --- /dev/null +++ b/src/drivers/barometer/ms5837/ms5837_registers.h @@ -0,0 +1,115 @@ +/**************************************************************************** + * + * Copyright (C) 2022 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 ms5837_registers.h + * + * Shared defines for the ms5837 driver. + */ + +#pragma once + +#include +#include "board_config.h" + +/* interface ioctls */ +#define IOCTL_RESET 2 +#define IOCTL_MEASURE 3 + +#define MS5837_ADDRESS 0x76 + +#define MS5837_RESET 0x1E +#define MS5837_ADC_READ 0x00 +#define MS5837_PROM_READ 0xA0 + +#define MS5837_30BA26 0x1A // Sensor version: From MS5837_30BA datasheet Version PROM Word 0 + +/* + * MS5837 internal constants and data structures. + */ +#define ADDR_CMD_CONVERT_D1_OSR256 0x40 /* write to this address to start pressure conversion */ +#define ADDR_CMD_CONVERT_D1_OSR512 0x42 /* write to this address to start pressure conversion */ +#define ADDR_CMD_CONVERT_D1_OSR1024 0x44 /* write to this address to start pressure conversion */ +#define ADDR_CMD_CONVERT_D1_OSR2048 0x46 /* write to this address to start pressure conversion */ +#define ADDR_CMD_CONVERT_D1_OSR4096 0x48 /* write to this address to start pressure conversion */ +#define ADDR_CMD_CONVERT_D2_OSR256 0x50 /* write to this address to start temperature conversion */ +#define ADDR_CMD_CONVERT_D2_OSR512 0x52 /* write to this address to start temperature conversion */ +#define ADDR_CMD_CONVERT_D2_OSR1024 0x54 /* write to this address to start temperature conversion */ +#define ADDR_CMD_CONVERT_D2_OSR2048 0x56 /* write to this address to start temperature conversion */ +#define ADDR_CMD_CONVERT_D2_OSR4096 0x58 /* write to this address to start temperature conversion */ + +/* + use an OSR of 1024 to reduce the self-heating effect of the + sensor. Information from MS tells us that some individual sensors + are quite sensitive to this effect and that reducing the OSR can + make a big difference + */ +#define ADDR_CMD_CONVERT_D1 ADDR_CMD_CONVERT_D1_OSR1024 +#define ADDR_CMD_CONVERT_D2 ADDR_CMD_CONVERT_D2_OSR1024 + +/* + * Maximum internal conversion time for OSR 1024 is 2.28 ms. We set an update + * rate of 100Hz which is be very safe not to read the ADC before the + * conversion finished + */ +#define MS5837_CONVERSION_INTERVAL 10000 /* microseconds */ +#define MS5837_MEASUREMENT_RATIO 3 /* pressure measurements per temperature measurement */ + +namespace ms5837 +{ + +/** + * Calibration PROM as reported by the device. + */ +#pragma pack(push,1) +struct prom_s { + uint16_t serial_and_crc; + 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 factory_setup; +}; + +/** + * Grody hack for crc4() + */ +union prom_u { + uint16_t c[8]; + prom_s s; +}; +#pragma pack(pop) + +} /* namespace */ diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index eafa5dc1f0a6..ee34b95a65e1 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -108,6 +108,8 @@ #define DRV_BARO_DEVTYPE_LPS33HW 0x4C #define DRV_BARO_DEVTYPE_TCBP001TA 0x4D +#define DRV_BARO_DEVTYPE_MS5837 0x4E + #define DRV_BARO_DEVTYPE_MPL3115A2 0x51 #define DRV_ACC_DEVTYPE_FXOS8701C 0x52