From 1b0f41e36ea8c36f4169cf01d1e29fe4768c489e Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Mon, 18 Jul 2016 13:49:12 -1000 Subject: [PATCH] Added support to MS5611 Driver for MS5607 --- src/drivers/ms5611/ms5611.cpp | 112 +++++++++++++++++++++++++--------- 1 file changed, 83 insertions(+), 29 deletions(-) diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 5b50b1ff91..3858a306b5 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -33,7 +33,7 @@ /** * @file ms5611.cpp - * Driver for the MS5611 barometric pressure sensor connected via I2C or SPI. + * Driver for the MS5611 and MS6507 barometric pressure sensor connected via I2C or SPI. */ #include @@ -69,6 +69,11 @@ #include "ms5611.h" +enum MS56XX_DEVICE_TYPES { + MS5611_DEVICE = 0, + MS5607_DEVICE = 1 +}; + enum MS5611_BUS { MS5611_BUS_ALL = 0, MS5611_BUS_I2C_INTERNAL, @@ -94,7 +99,7 @@ static const int ERROR = -1; #define POW2(_x) ((_x) * (_x)) /* - * MS5611 internal constants and data structures. + * MS5611/MS5607 internal constants and data structures. */ /* internal conversion time: 9.17 ms, so should not be read at rates higher than 100 Hz */ @@ -106,7 +111,7 @@ static const int ERROR = -1; class MS5611 : public device::CDev { public: - MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char *path); + MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char *path, enum MS56XX_DEVICE_TYPES device_type); ~MS5611(); virtual int init(); @@ -128,11 +133,11 @@ protected: unsigned _measure_ticks; ringbuffer::RingBuffer *_reports; - + enum MS56XX_DEVICE_TYPES _device_type; bool _collect_phase; unsigned _measure_phase; - /* intermediate temperature values per MS5611 datasheet */ + /* intermediate temperature values per MS5611/MS5607 datasheet */ int32_t _TEMP; int64_t _OFF; int64_t _SENS; @@ -214,12 +219,14 @@ protected: */ extern "C" __EXPORT int ms5611_main(int argc, char *argv[]); -MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char *path) : +MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char *path, + enum MS56XX_DEVICE_TYPES device_type) : CDev("MS5611", path), _interface(interface), _prom(prom_buf.s), _measure_ticks(0), _reports(nullptr), + _device_type(device_type), _collect_phase(false), _measure_phase(0), _TEMP(0), @@ -691,27 +698,62 @@ MS5611::collect() _TEMP = 2000 + (int32_t)(((int64_t)dT * _prom.c6_temp_coeff_temp) >> 23); /* base sensor scale/offset values */ - _SENS = ((int64_t)_prom.c1_pressure_sens << 15) + (((int64_t)_prom.c3_temp_coeff_pres_sens * dT) >> 8); - _OFF = ((int64_t)_prom.c2_pressure_offset << 16) + (((int64_t)_prom.c4_temp_coeff_pres_offset * dT) >> 7); + if (_device_type == MS5611_DEVICE) { + + /* Perform MS5611 Caculation */ + + _OFF = ((int64_t)_prom.c2_pressure_offset << 16) + (((int64_t)_prom.c4_temp_coeff_pres_offset * dT) >> 7); + _SENS = ((int64_t)_prom.c1_pressure_sens << 15) + (((int64_t)_prom.c3_temp_coeff_pres_sens * dT) >> 8); + + /* MS5611 temperature compensation */ - /* temperature compensation */ - if (_TEMP < 2000) { + if (_TEMP < 2000) { - int32_t T2 = POW2(dT) >> 31; + 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; + 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; + if (_TEMP < -1500) { + + int64_t f2 = POW2(_TEMP + 1500); + OFF2 += 7 * f2; + SENS2 += 11 * f2 >> 1; + } + + _TEMP -= T2; + _OFF -= OFF2; + _SENS -= SENS2; } - _TEMP -= T2; - _OFF -= OFF2; - _SENS -= SENS2; + } else if (_device_type == MS5607_DEVICE) { + + /* Perform MS5607 Caculation */ + + _OFF = ((int64_t)_prom.c2_pressure_offset << 17) + (((int64_t)_prom.c4_temp_coeff_pres_offset * dT) >> 6); + _SENS = ((int64_t)_prom.c1_pressure_sens << 16) + (((int64_t)_prom.c3_temp_coeff_pres_sens * dT) >> 7); + + /* MS5607 temperature compensation */ + + if (_TEMP < 2000) { + + int32_t T2 = POW2(dT) >> 31; + + int64_t f = POW2((int64_t)_TEMP - 2000); + int64_t OFF2 = 61 * f >> 4; + int64_t SENS2 = 2 * f; + + if (_TEMP < -1500) { + int64_t f2 = POW2(_TEMP + 1500); + OFF2 += 15 * f2; + SENS2 += 8 * f2; + } + + _TEMP -= T2; + _OFF -= OFF2; + _SENS -= SENS2; + } } } else { @@ -840,9 +882,9 @@ struct ms5611_bus_option { }; #define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0])) -bool start_bus(struct ms5611_bus_option &bus); +bool start_bus(struct ms5611_bus_option &bus, enum MS56XX_DEVICE_TYPES device_type); struct ms5611_bus_option &find_bus(enum MS5611_BUS busid); -void start(enum MS5611_BUS busid); +void start(enum MS5611_BUS busid, enum MS56XX_DEVICE_TYPES device_type); void test(enum MS5611_BUS busid); void reset(enum MS5611_BUS busid); void info(); @@ -900,7 +942,7 @@ crc4(uint16_t *n_prom) * Start the driver. */ bool -start_bus(struct ms5611_bus_option &bus) +start_bus(struct ms5611_bus_option &bus, enum MS56XX_DEVICE_TYPES device_type) { if (bus.dev != nullptr) { errx(1, "bus option already started"); @@ -915,7 +957,7 @@ start_bus(struct ms5611_bus_option &bus) return false; } - bus.dev = new MS5611(interface, prom_buf, bus.devpath); + bus.dev = new MS5611(interface, prom_buf, bus.devpath, device_type); if (bus.dev != nullptr && OK != bus.dev->init()) { delete bus.dev; @@ -947,7 +989,7 @@ start_bus(struct ms5611_bus_option &bus) * is either successfully up and running or failed to start. */ void -start(enum MS5611_BUS busid) +start(enum MS5611_BUS busid, enum MS56XX_DEVICE_TYPES device_type) { uint8_t i; bool started = false; @@ -963,7 +1005,7 @@ start(enum MS5611_BUS busid) continue; } - started = started | start_bus(bus_options[i]); + started = started | start_bus(bus_options[i], device_type); } if (!started) { @@ -1195,6 +1237,8 @@ usage() warnx(" -I (intternal I2C bus)"); warnx(" -S (external SPI bus)"); warnx(" -s (internal SPI bus)"); + warnx(" -T 5611|5607 (default 5611)"); + } } // namespace @@ -1203,10 +1247,11 @@ int ms5611_main(int argc, char *argv[]) { enum MS5611_BUS busid = MS5611_BUS_ALL; + int device_type = 5611; // Default to MS5611 int ch; /* jump over start/off/etc and look at options first */ - while ((ch = getopt(argc, argv, "XISs")) != EOF) { + while ((ch = getopt(argc, argv, "T:XISs")) != EOF) { switch (ch) { case 'X': busid = MS5611_BUS_I2C_EXTERNAL; @@ -1224,19 +1269,28 @@ ms5611_main(int argc, char *argv[]) busid = MS5611_BUS_SPI_INTERNAL; break; + case 'T': + device_type = atoi(optarg); + + if (device_type == 5611 || device_type == 5607) { + break; + } + + //no break default: ms5611::usage(); exit(0); } } + const char *verb = argv[optind]; /* * Start/load the driver. */ if (!strcmp(verb, "start")) { - ms5611::start(busid); + ms5611::start(busid, device_type == 5607 ? MS5607_DEVICE : MS5611_DEVICE); } /*