From c3711efd27ea36adf25359926dd9da6da6fdce9a Mon Sep 17 00:00:00 2001 From: Sergej Scheiermann Date: Tue, 28 Feb 2017 18:31:06 +0100 Subject: [PATCH] initial commit of bmm150 --- ROMFS/px4fmu_common/init.d/rc.sensors | 47 +- cmake/configs/nuttx_px4fmu-v4_default.cmake | 1 + src/drivers/bmm150/CMakeLists.txt | 44 + src/drivers/bmm150/bmm150.cpp | 1122 +++++++++++++++++++ src/drivers/bmm150/bmm150.hpp | 364 ++++++ src/drivers/boards/px4fmu-v4/board_config.h | 2 + src/drivers/drv_sensor.h | 1 + 7 files changed, 1560 insertions(+), 21 deletions(-) create mode 100644 src/drivers/bmm150/CMakeLists.txt create mode 100644 src/drivers/bmm150/bmm150.cpp create mode 100644 src/drivers/bmm150/bmm150.hpp diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 82f6c7a889..c2388efe7a 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -180,42 +180,42 @@ fi if ver hwcmp PX4FMU_V4 then # External I2C bus - if hmc5883 -C -T -X start - then - fi + #if hmc5883 -C -T -X start + #then + #fi - if lis3mdl -R 2 start - then - fi + #if lis3mdl -R 2 start + #then + #fi # Internal SPI bus is rotated 90 deg yaw - if hmc5883 -C -T -S -R 2 start - then - fi + #if hmc5883 -C -T -S -R 2 start + #then + #fi # Internal SPI bus ICM-20608-G is rotated 90 deg yaw - if mpu6000 -R 2 -T 20608 start - then - fi + #if mpu6000 -R 2 -T 20608 start + #then + #fi # Internal SPI bus ICM-20602-G is rotated 90 deg yaw - if mpu6000 -R 2 -T 20602 start - then - fi + #if mpu6000 -R 2 -T 20602 start + #then + #fi # Start either MPU9250 or BMI160. They are both connected to the same SPI bus and use the same # chip select pin. There are different boards with either one of them and the WHO_AM_I register # will prevent the incorrect driver from a successful initialization. # Internal SPI bus mpu9250 is rotated 90 deg yaw - if mpu9250 -R 2 start - then - fi + #if mpu9250 -R 2 start + #then + #fi # Internal SPI bus BMI160 - if bmi160 start - then - fi + #if bmi160 start + #then + #fi # Start either ICM2060X or BMI055. They are both connected to the same SPI bus and use the same # chip select pin. There are different boards with either one of them and the WHO_AM_I register @@ -230,6 +230,11 @@ then if bmi055 -G start then fi + + # expansion i2c used for BMM150 + if bmm150 start + then + fi fi diff --git a/cmake/configs/nuttx_px4fmu-v4_default.cmake b/cmake/configs/nuttx_px4fmu-v4_default.cmake index f268af3848..6965a6026a 100644 --- a/cmake/configs/nuttx_px4fmu-v4_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v4_default.cmake @@ -52,6 +52,7 @@ set(config_module_list drivers/bma180 drivers/bmi160 drivers/bmi055 + drivers/bmm150 drivers/tap_esc drivers/iridiumsbd diff --git a/src/drivers/bmm150/CMakeLists.txt b/src/drivers/bmm150/CMakeLists.txt new file mode 100644 index 0000000000..aee7284746 --- /dev/null +++ b/src/drivers/bmm150/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2015 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__bmm150 + MAIN bmm150 + STACK_MAIN 1200 + COMPILE_FLAGS + -Weffc++ + SRCS + bmm150.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/bmm150/bmm150.cpp b/src/drivers/bmm150/bmm150.cpp new file mode 100644 index 0000000000..07760f6498 --- /dev/null +++ b/src/drivers/bmm150/bmm150.cpp @@ -0,0 +1,1122 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2014 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 bmm150.cpp + * Driver for the Bosch BMM 150 MEMS magnetometer connected via I2C. + */ + +#include "bmm150.hpp" + +/** driver 'main' command */ +extern "C" { __EXPORT int bmm150_main(int argc, char *argv[]); } + + +namespace bmm150 +{ + +BMM150 *g_dev_ext; +BMM150 *g_dev_int; + +void start(bool, enum Rotation); +void test(bool); +void reset(bool); +void info(bool); +void regdump(bool external_bus); +void usage(); + + +/** + * Start the driver. + * + * This function only returns if the driver is up and running + * or failed to detect the sensor. + */ +void start(bool external_bus, enum Rotation rotation) +{ + int fd; + BMM150 **g_dev_ptr = (external_bus ? &g_dev_ext : &g_dev_int); + const char *path = (external_bus ? BMM150_DEVICE_PATH_MAG_EXT : BMM150_DEVICE_PATH_MAG); + + if (*g_dev_ptr != nullptr) + /* if already started, the still command succeeded */ + { + errx(0, "already started"); + } + + /* create the driver */ + if (external_bus) { +#if defined(PX4_SPI_BUS_EXT) && defined(PX4_SPIDEV_EXT_BMI) + *g_dev_ptr = new BMM150(PX4_I2C_BUS_BMM150, path, external_bus, rotation); +#else + errx(0, "External I2C not available"); +#endif + + } else { + *g_dev_ptr = new BMM150(PX4_I2C_BUS_BMM150, path, external_bus, rotation); + } + + if (*g_dev_ptr == nullptr) { + goto fail; + } + + + if (OK != (*g_dev_ptr)->init()) { + goto fail; + } + + /* set the poll rate to default, starts automatic data collection */ + fd = open(path, O_RDONLY); + + + if (fd < 0) { + goto fail; + } + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + goto fail; + } + + close(fd); + + exit(0); +fail: + + if (*g_dev_ptr != nullptr) { + delete(*g_dev_ptr); + *g_dev_ptr = nullptr; + } + + errx(1, "driver start failed"); +} + + +void test(bool external_bus) +{ + int fd = -1; + const char *path = (external_bus ? BMM150_DEVICE_PATH_MAG_EXT : BMM150_DEVICE_PATH_MAG); + struct mag_report m_report; + ssize_t sz; + + + /* get the driver */ + fd = open(path, O_RDONLY); + + if (fd < 0) + err(1, "%s open failed (try 'bmm150 start' if the driver is not running)", + path); + + /* reset to Max polling rate*/ + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX) < 0) { + err(1, "reset to Max polling rate"); + } + + /* do a simple demand read */ + sz = read(fd, &m_report, sizeof(m_report)); + + if (sz != sizeof(m_report)) { + err(1, "immediate mag read failed"); + } + + warnx("single read"); + warnx("time: %lld", m_report.timestamp); + warnx("mag x: \t%8.4f\t", (double)m_report.x); + warnx("mag y: \t%8.4f\t", (double)m_report.y); + warnx("mag z: \t%8.4f\t", (double)m_report.z); + warnx("mag x: \t%d\traw 0x%0x", (short)m_report.x_raw, (unsigned short)m_report.x_raw); + warnx("mag y: \t%d\traw 0x%0x", (short)m_report.y_raw, (unsigned short)m_report.y_raw); + warnx("mag z: \t%d\traw 0x%0x", (short)m_report.z_raw, (unsigned short)m_report.z_raw); + + errx(0, "PASS"); + +} + + + +void +reset(bool external_bus) +{ + const char *path = external_bus ? BMM150_DEVICE_PATH_MAG_EXT : BMM150_DEVICE_PATH_MAG; + int fd = open(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); + +} + +void +info(bool external_bus) +{ + BMM150 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int; + + if (*g_dev_ptr == nullptr) { + errx(1, "driver not running"); + } + + printf("state @ %p\n", *g_dev_ptr); + (*g_dev_ptr)->print_info(); + + exit(0); + +} + +/** + * Dump the register information + */ +void +regdump(bool external_bus) +{ + BMM150 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int; + + if (*g_dev_ptr == nullptr) { + errx(1, "driver not running"); + } + + printf("regdump @ %p\n", *g_dev_ptr); + (*g_dev_ptr)->print_registers(); + + exit(0); +} + +void +usage() +{ + warnx("missing command: try 'start', 'info', 'test', 'stop',\n'reset', 'regdump'"); + warnx("options:"); + warnx(" -X (external bus)"); + +} + + +} + + +BMM150 :: BMM150(int bus, const char *path, bool external, enum Rotation rotation) : + I2C("BMM150", path, bus, BMM150_SLAVE_ADDRESS, BMM150_BUS_SPEED), + _work{}, + _external(false), + _call_interval(0), + _report{0}, + _reports(nullptr), + _scale{}, + _range_scale(0.01), /* default range scale from from uT to gauss */ + _topic(nullptr), + _orb_class_instance(-1), + _class_instance(-1), + _power(BMM150_DEFAULT_POWER_MODE), + _output_data_rate(BMM150_DATA_RATE_30HZ), + _calibrated(false), + dig_x1(0), + dig_y1(0), + dig_x2(0), + dig_y2(0), + dig_z1(0), + dig_z2(0), + dig_z3(0), + dig_z4(0), + dig_xy1(0), + dig_xy2(0), + dig_xyz1(0), + _sample_perf(perf_alloc(PC_ELAPSED, "bmm150_read")), + _bad_transfers(perf_alloc(PC_COUNT, "bmm150_bad_transfers")), + _good_transfers(perf_alloc(PC_COUNT, "bmm150_good_transfers")), + _duplicates(perf_alloc(PC_COUNT, "bmm150_duplicates")), + _rotation(rotation), + _got_duplicate(false), + _last_report{0} +{ + _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_BMM150; + + _external = external; + // work_cancel in the dtor will explode if we don't do this... + memset(&_work, 0, sizeof(_work)); + + // default scaling + _scale.x_offset = 0; + _scale.x_scale = 1.0f; + _scale.y_offset = 0; + _scale.y_scale = 1.0f; + _scale.z_offset = 0; + _scale.z_scale = 1.0f; + +} + + +BMM150 :: ~BMM150() +{ + /* make sure we are truly inactive */ + stop(); + + /* free any existing reports */ + if (_reports != nullptr) { + delete _reports; + } + + + if (_class_instance != -1) { + unregister_class_devname(MAG_BASE_DEVICE_PATH, _class_instance); + } + + /* delete the perf counter */ + perf_free(_sample_perf); + perf_free(_bad_transfers); + perf_free(_good_transfers); + perf_free(_duplicates); + +} + +int BMM150::init() +{ + int ret = OK; + + /* do I2C init (and probe) first */ + ret = I2C::init(); + + /* if probe/setup failed, bail now */ + if (ret != OK) { + DEVICE_DEBUG("I2C setup failed"); + return ret; + } + + /* allocate basic report buffers */ + _reports = new ringbuffer::RingBuffer(2, sizeof(_report)); + + if (_reports == nullptr) { + goto out; + } + + /* Bring the device to sleep mode */ + modify_reg(BMM150_POWER_CTRL_REG, 1, 1); + up_udelay(10000); + + + /* check id*/ + if (read_reg(BMM150_CHIP_ID_REG) != BMM150_CHIP_ID) { + warnx("id of magnetometer is not: 0x%02x", BMM150_CHIP_ID); + return -EIO; + } + + if (reset() != OK) { + goto out; + } + + init_trim_registers(); + + _class_instance = register_class_devname(MAG_BASE_DEVICE_PATH); + + collect(); + + /* advertise sensor topic, measure manually to initialize valid report */ + struct mag_report mrb; + _reports->get(&mrb); + + /* measurement will have generated a report, publish */ + _topic = orb_advertise_multi(ORB_ID(sensor_mag), &mrb, + &_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_MAX); + + if (_topic == nullptr) { + warnx("ADVERT FAIL"); + } + +out: + return ret; +} + +int +BMM150::probe() +{ + /* During I2C Initialization, sensor is in suspend mode + * and chip Id will return 0x00, hence returning OK. After + * I2C initialization, sensor is brought to sleep mode + * and Chip Id is verified. In sleep mode, we can read + * chip id. */ + + /* @Note: Please read BMM150 Datasheet for more details */ + return OK; +} + + +void +BMM150::start() +{ + /* reset the report ring and state machine */ + _reports->flush(); + + /* schedule a cycle to start things */ + work_queue(HPWORK, &_work, (worker_t)&BMM150::cycle_trampoline, this, 1); + + +} + +void +BMM150::stop() +{ + work_cancel(HPWORK, &_work); +} + +ssize_t +BMM150::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(_report); + struct mag_report *mag_buf = reinterpret_cast(buffer); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) { + return -ENOSPC; + } + + /* if automatic measurement is enabled */ + if (_call_interval > 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 (_reports->get(mag_buf)) { + ret += sizeof(struct mag_report); + mag_buf++; + } + } + + /* 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 { + _reports->flush(); + + /* run the collection phase */ + if (OK != collect()) { + ret = -EIO; + break; + } + + + if (_reports->get(mag_buf)) { + ret = sizeof(struct mag_report); + } + } while (0); + + /* return the number of bytes transferred */ + return ret; + +} + + +void +BMM150::cycle_trampoline(void *arg) +{ + BMM150 *dev = reinterpret_cast(arg); + + /* make measurement */ + dev->cycle(); + +} + +void +BMM150::cycle() +{ + collect(); + work_queue(HPWORK, &_work, (worker_t)&BMM150::cycle_trampoline, this, USEC2TICK(BMM150_CONVERSION_INTERVAL)); + +} + + +int +BMM150::collect() +{ + bool mag_notify = true; + uint8_t mag_data[8], status; + uint16_t resistance, lsb, msb, msblsb; + mag_report mrb; + + + /* start collecting data */ + perf_begin(_sample_perf); + + status = read_reg(BMM150_R_LSB); + + /* Read Magnetometer data*/ + if (OK != get_data(BMM150_DATA_X_LSB_REG, mag_data, sizeof(mag_data))) { + return -EIO; + } + + /* Array holding the mag XYZ and R data + mag_data[0] - X LSB + mag_data[1] - X MSB + mag_data[2] - Y LSB + mag_data[3] - Y MSB + mag_data[4] - Z LSB + mag_data[5] - Z MSB + mag_data[6] - R LSB + mag_data[7] - R MSB + */ + + /* Extract X axis data */ + lsb = ((mag_data[0] & 0xF8) >> 3); + msb = (((int8_t)mag_data[1]) << 5); + msblsb = (msb | lsb); + mrb.x_raw = (int16_t)msblsb; + + + /* Extract Y axis data */ + lsb = ((mag_data[2] & 0xF8) >> 3); + msb = (((int8_t)mag_data[3]) << 5); + msblsb = (msb | lsb); + mrb.y_raw = (int16_t)msblsb; + + /* Extract Z axis data */ + lsb = ((mag_data[4] & 0xFE) >> 1); + msb = (((int8_t)mag_data[5]) << 7); + msblsb = (msb | lsb); + mrb.z_raw = (int16_t)msblsb; + + /* Extract Resistance data */ + lsb = ((mag_data[6] & 0xFC) >> 2); + msb = (mag_data[7] << 6); + msblsb = (msb | lsb); + resistance = (uint16_t)msblsb; + + /* Check whether data is new or old */ + if (!(status & 0x01)) { + perf_end(_sample_perf); + perf_count(_duplicates); + _got_duplicate = true; + return -EIO; + } + + _got_duplicate = false; + + if (mrb.x_raw == 0 && + mrb.y_raw == 0 && + mrb.z_raw == 0 && + resistance == 0) { + // all zero data - probably a I2C bus error + perf_count(_bad_transfers); + perf_end(_sample_perf); + return -EIO; + } + + perf_count(_good_transfers); + + /* Compensation for X axis */ + if (mrb.x_raw != BMM150_FLIP_OVERFLOW_ADCVAL) { + /* no overflow */ + if ((resistance != 0) && (dig_xyz1 != 0)) { + mrb.x = ((dig_xyz1 * 16384.0 / resistance) - 16384.0); + mrb.x = (((mrb.x_raw * ((((dig_xy2 * ((float)(mrb.x * mrb.x / (float)268435456.0)) + mrb.x * dig_xy1 / + (float)16384.0)) + (float)256.0) * (dig_x2 + (float)160.0))) / (float)8192.0) + (dig_x1 * (float)8.0)) / (float)16.0; + + } else { + mrb.x = BMM150_OVERFLOW_OUTPUT_FLOAT; + } + + } else { + mrb.x = BMM150_OVERFLOW_OUTPUT_FLOAT; + } + + /* Compensation for Y axis */ + if (mrb.y_raw != BMM150_FLIP_OVERFLOW_ADCVAL) { + /* no overflow */ + if ((resistance != 0) && (dig_xyz1 != 0)) { + + mrb.y = ((((float)dig_xyz1) * (float)16384.0 / resistance) - (float)16384.0); + mrb.y = (((mrb.y_raw * ((((dig_xy2 * (mrb.y * mrb.y / (float)268435456.0) + mrb.y * dig_xy1 / (float)16384.0)) + + (float)256.0) * (dig_y2 + (float)160.0))) / (float)8192.0) + (dig_y1 * (float)8.0)) / (float)16.0; + + + } else { + mrb.y = BMM150_OVERFLOW_OUTPUT_FLOAT; + } + + } else { + /* overflow, set output to 0.0f */ + mrb.y = BMM150_OVERFLOW_OUTPUT_FLOAT; + } + + + /* Compensation for Z axis */ + if (mrb.z_raw != BMM150_HALL_OVERFLOW_ADCVAL) { + /* no overflow */ + if ((dig_z2 != 0) && (dig_z1 != 0) && (dig_xyz1 != 0) && (resistance != 0)) { + mrb.z = ((((mrb.z_raw - dig_z4) * (float)131072.0) - (dig_z3 * (resistance - dig_xyz1))) / (( + dig_z2 + dig_z1 * resistance / (float)32768.0) * (float)4.0)) / (float)16.0; + } + + } else { + /* overflow, set output to 0.0f */ + mrb.z = BMM150_OVERFLOW_OUTPUT_FLOAT; + } + + + mrb.timestamp = hrt_absolute_time(); + + // report the error count as the number of bad transfers. + // This allows the higher level code to decide if it + // should use this sensor based on whether it has had failures + mrb.error_count = perf_event_count(_bad_transfers); + + + /* Scaling the data */ + mrb.x = ((mrb.x * _range_scale) - _scale.x_offset) * _scale.x_scale; + mrb.y = ((mrb.y * _range_scale) - _scale.y_offset) * _scale.y_scale; + mrb.z = ((mrb.z * _range_scale) - _scale.z_offset) * _scale.z_scale; + + mrb.scaling = _range_scale; + mrb.device_id = _device_id.devid; + + _last_report.x = mrb.x; + _last_report.y = mrb.y; + _last_report.z = mrb.z; + + _reports->force(&mrb); + + /* notify anyone waiting for data */ + if (mag_notify) { + poll_notify(POLLIN); + } + + if (mag_notify && !(_pub_blocked)) { + /* publish it */ + orb_publish(ORB_ID(sensor_mag), _topic, &mrb); + } + + perf_end(_sample_perf); + return OK; + +} + +int +BMM150::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + + switch (cmd) { + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _call_interval = 0; + return OK; + + /* external signalling (DRDY) 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: + return ioctl(filp, SENSORIOCSPOLLRATE, BMM150_MAX_DATA_RATE); + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_call_interval == 0); + + /* convert hz to tick interval via microseconds */ + unsigned ticks = USEC2TICK(1000000 / arg); + + /* check against maximum rate */ + if (ticks < USEC2TICK(BMM150_CONVERSION_INTERVAL)) { + return -EINVAL; + } + + /* update interval for next measurement */ + _call_interval = ticks; + + /* if we need to start the poll state machine, do it */ + if (want_start) { + start(); + } + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_call_interval == 0) { + return SENSOR_POLLRATE_MANUAL; + } + + return 1000000 / _call_interval; + + case SENSORIOCSQUEUEDEPTH: { + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } + + irqstate_t flags = px4_enter_critical_section(); + + if (!_reports->resize(arg)) { + px4_leave_critical_section(flags); + return -ENOMEM; + } + + px4_leave_critical_section(flags); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _reports->size(); + + case SENSORIOCRESET: + return reset(); + + case MAGIOCSSCALE: + return OK; + + case MAGIOCGSCALE: + return OK; + + case MAGIOCCALIBRATE: + return OK; + + case MAGIOCEXSTRAP: + return OK; + + case MAGIOCSELFTEST: + return OK; + + case MAGIOCSSAMPLERATE: + return ioctl(filp, SENSORIOCSPOLLRATE, arg); + + case MAGIOCGSAMPLERATE: + return 1000000 / _call_interval; + + case MAGIOCSRANGE: + return OK; + + case MAGIOCGRANGE: + return OK; + + case MAGIOCSLOWPASS: + case MAGIOCGLOWPASS: + /* not supported, no internal filtering */ + return -EINVAL; + + default: + /* give it to the superclass */ + return I2C::ioctl(filp, cmd, arg); + } +} + + +int BMM150::reset() +{ + int ret = OK; + + /* Soft-reset */ + modify_reg(BMM150_POWER_CTRL_REG, BMM150_SOFT_RESET_MASK, BMM150_SOFT_RESET_VALUE); + up_udelay(5000); + + /* Enable Magnetometer in normal mode */ + ret += set_power_mode(BMM150_DEFAULT_POWER_MODE); + up_udelay(1000); + + /* Set the data rate to default */ + ret += set_data_rate(BMM150_DEFAULT_ODR); + + /* set the preset mode as regular*/ + ret += set_presetmode(BMM150_PRESETMODE_REGULAR); + + return OK; +} + + +int +BMM150::self_test() +{ + if (perf_event_count(_sample_perf) == 0) { + collect(); + } + + /* return 0 on success, 1 else */ + return (perf_event_count(_sample_perf) > 0) ? 0 : 1; +} + +bool BMM150::is_external() +{ + return _external; +} + + +uint8_t +BMM150::read_reg(uint8_t reg) +{ + const uint8_t cmd = reg; + uint8_t ret; + + transfer(&cmd, 1, &ret, 1); + + return ret; +} + +int +BMM150::write_reg(uint8_t reg, uint8_t value) +{ + const uint8_t cmd[2] = { reg, value}; + + return transfer(cmd, 2, nullptr, 0); +} + +int +BMM150::get_data(uint8_t reg, uint8_t *data, unsigned len) +{ + const uint8_t cmd = reg; + + return transfer(&cmd, 1, data, len); + +} + +void +BMM150::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) +{ + uint8_t val; + + val = read_reg(reg); + val &= ~clearbits; + val |= setbits; + write_reg(reg, val); +} + + + +int BMM150 :: set_power_mode(uint8_t power_mode) +{ + uint8_t setbits = 0; + uint8_t clearbits = BMM150_POWER_MASK; + + switch (power_mode) { + case BMM150_NORMAL_MODE: + _power = 0; + break; + + case BMM150_FORCED_MODE: + _power = 1; + break; + + case BMM150_SLEEP_MODE: + _power = 3; + break; + + default: + return -EINVAL; + } + + setbits |= power_mode; + modify_reg(BMM150_CTRL_REG, clearbits, setbits); + + return OK; + +} + +int BMM150 :: set_data_rate(uint8_t data_rate) +{ + uint8_t setbits = 0; + uint8_t clearbits = BMM150_OUTPUT_DATA_RATE_MASK; + + switch (data_rate) { + case BMM150_DATA_RATE_10HZ: + _output_data_rate = 10; + break; + + case BMM150_DATA_RATE_02HZ: + _output_data_rate = 2; + break; + + case BMM150_DATA_RATE_06HZ: + _output_data_rate = 6; + break; + + case BMM150_DATA_RATE_08HZ: + _output_data_rate = 8; + break; + + case BMM150_DATA_RATE_15HZ: + _output_data_rate = 15; + break; + + case BMM150_DATA_RATE_20HZ: + _output_data_rate = 20; + break; + + case BMM150_DATA_RATE_25HZ: + _output_data_rate = 25; + break; + + case BMM150_DATA_RATE_30HZ: + _output_data_rate = 30; + break; + + default: + return -EINVAL; + } + + setbits |= data_rate; + modify_reg(BMM150_CTRL_REG, clearbits, setbits); + + return OK; + +} + + +int BMM150 :: init_trim_registers(void) +{ + int ret = OK; + uint8_t data[2] = {0}; + uint16_t msb, lsb, msblsb; + + dig_x1 = read_reg(BMM150_DIG_X1); + dig_y1 = read_reg(BMM150_DIG_Y1); + dig_x2 = read_reg(BMM150_DIG_X2); + dig_y2 = read_reg(BMM150_DIG_Y2); + dig_xy1 = read_reg(BMM150_DIG_XY1); + dig_xy2 = read_reg(BMM150_DIG_XY2); + + ret += get_data(BMM150_DIG_Z1_LSB, data, 2); + lsb = data[0]; + msb = (data[1] << 8); + msblsb = (msb | lsb); + dig_z1 = (uint16_t)msblsb; + + ret += get_data(BMM150_DIG_Z2_LSB, data, 2); + lsb = data[0]; + msb = ((int8_t)data[1] << 8); + msblsb = (msb | lsb); + dig_z2 = (int16_t)msblsb; + + ret += get_data(BMM150_DIG_Z3_LSB, data, 2); + lsb = data[0]; + msb = ((int8_t)data[1] << 8); + msblsb = (msb | lsb); + dig_z3 = (int16_t)msblsb; + + ret += get_data(BMM150_DIG_Z4_LSB, data, 2); + lsb = data[0]; + msb = ((int8_t)data[1] << 8); + msblsb = (msb | lsb); + dig_z4 = (int16_t)msblsb; + + ret += get_data(BMM150_DIG_XYZ1_LSB, data, 2); + lsb = data[0]; + msb = ((data[1] & 0x7F) << 8); + msblsb = (msb | lsb); + dig_xyz1 = (uint16_t)msblsb; + + return ret; + +} + +int BMM150::set_rep_xy(uint8_t rep_xy) +{ + uint8_t cmd[2] = {BMM150_XY_REP_CTRL_REG, rep_xy}; + + return transfer((const uint8_t *)cmd, sizeof(cmd), nullptr, 0); + +} + +int BMM150::set_rep_z(uint8_t rep_z) +{ + uint8_t cmd[2] = {BMM150_Z_REP_CTRL_REG, rep_z}; + + return transfer((const uint8_t *)cmd, sizeof(cmd), nullptr, 0); + +} + + +int BMM150::set_presetmode(uint8_t presetmode) +{ + int ret = OK; + uint8_t data_rate, rep_xy, rep_z; + + if (presetmode == 1) { + data_rate = BMM150_LOWPOWER_DR; + rep_xy = BMM150_LOWPOWER_REPXY; + rep_z = BMM150_LOWPOWER_REPZ; + + } else if (presetmode == 2) { + data_rate = BMM150_REGULAR_DR; + rep_xy = BMM150_REGULAR_REPXY; + rep_z = BMM150_REGULAR_REPZ; + + } else if (presetmode == 3) { + data_rate = BMM150_HIGHACCURACY_DR; + rep_xy = BMM150_HIGHACCURACY_REPXY; + rep_z = BMM150_HIGHACCURACY_REPZ; + + } else if (presetmode == 4) { + data_rate = BMM150_ENHANCED_DR; + rep_xy = BMM150_ENHANCED_REPXY; + rep_z = BMM150_ENHANCED_REPZ; + + } else { + return -EINVAL; + } + + ret = set_data_rate(data_rate); + + ret += set_rep_xy(rep_xy); + ret += set_rep_z(rep_z); + + return ret; + +} + +void +BMM150::print_info() +{ + perf_print_counter(_sample_perf); + perf_print_counter(_bad_transfers); + perf_print_counter(_good_transfers); + _reports->print_info("mag queue"); + + printf("output (%.2f %.2f %.2f)\n", (double)_last_report.x, (double)_last_report.y, (double)_last_report.z); + printf("offsets (%.2f %.2f %.2f)\n", (double)_scale.x_offset, (double)_scale.y_offset, (double)_scale.z_offset); + printf("scaling (%.2f %.2f %.2f) 1/range_scale %.2f ", + (double)_scale.x_scale, (double)_scale.y_scale, (double)_scale.z_scale, + (double)(1.0f / _range_scale)); + printf("\n"); + + +} + +void +BMM150::print_registers() +{ + printf("BMM150 registers\n"); + + uint8_t reg = BMM150_CHIP_ID_REG; + uint8_t v = read_reg(reg); + printf("Chip Id: %02x:%02x ", (unsigned)reg, (unsigned)v); + printf("\n"); + + reg = BMM150_INT_SETT_CTRL_REG; + v = read_reg(reg); + printf("Int sett Ctrl reg: %02x:%02x ", (unsigned)reg, (unsigned)v); + printf("\n"); + + reg = BMM150_AXES_EN_CTRL_REG; + v = read_reg(reg); + printf("Axes En Ctrl reg: %02x:%02x ", (unsigned)reg, (unsigned)v); + printf("\n"); + +} + +int +bmm150_main(int argc, char *argv[]) +{ + bool external_bus = false; + int ch; + enum Rotation rotation = ROTATION_NONE; + + /* jump over start/off/etc and look at options first */ + while ((ch = getopt(argc, argv, "XR:")) != EOF) { + switch (ch) { + case 'X': + external_bus = true; + break; + + case 'R': + rotation = (enum Rotation)atoi(optarg); + break; + + default: + bmm150::usage(); + exit(0); + } + } + + const char *verb = argv[optind]; + + /* + * Start/load the driver. + */ + if (!strcmp(verb, "start")) { + bmm150::start(external_bus, rotation); + } + + + /* + * Test the driver/device. + */ + if (!strcmp(verb, "test")) { + bmm150::test(external_bus); + } + + /* + * Reset the driver. + */ + if (!strcmp(verb, "reset")) { + bmm150::reset(external_bus); + } + + /* + * Print driver information. + */ + if (!strcmp(verb, "info")) { + bmm150::info(external_bus); + } + + /* + * Print register information. + */ + if (!strcmp(verb, "regdump")) { + bmm150::regdump(external_bus); + } + + + bmm150::usage(); + exit(1); +} diff --git a/src/drivers/bmm150/bmm150.hpp b/src/drivers/bmm150/bmm150.hpp new file mode 100644 index 0000000000..dc12df0b3d --- /dev/null +++ b/src/drivers/bmm150/bmm150.hpp @@ -0,0 +1,364 @@ +#ifndef BMM150_HPP_ +#define BMM150_HPP_ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + + +#define BMM150_DEVICE_PATH_MAG "/dev/bmm150_i2c_int" + +#define BMM150_DEVICE_PATH_MAG_EXT "/dev/bmm150_i2c_ext" + +#define BMM150_SLAVE_ADDRESS PX4_I2C_OBDEV_BMM150 + +#define BMM150_BUS_SPEED 1000*100 + +/* Chip Identification number */ +#define BMM150_CHIP_ID 0x32 + +/* Chip Id Register */ +#define BMM150_CHIP_ID_REG 0x40 + +/* Data Registers */ +#define BMM150_DATA_X_LSB_REG 0x42 +#define BMM150_DATA_X_MSB_REG 0x43 +#define BMM150_DATA_Y_LSB_REG 0x44 +#define BMM150_DATA_Y_MSB_REG 0x45 +#define BMM150_DATA_Z_LSB_REG 0x46 +#define BMM150_DATA_Z_MSB_REG 0x47 +#define BMM150_R_LSB 0x48 +#define BMM150_R_MSB 0x49 + +/* Interrupt status registers */ +#define BMM150_INT_STATUS_REG 0x4A + +/* Control Registers */ +#define BMM150_POWER_CTRL_REG 0x4B +#define BMM150_CTRL_REG 0x4C +#define BMM150_INT_SETT_CTRL_REG 0x4D +#define BMM150_AXES_EN_CTRL_REG 0x4E +#define BMM150_LOW_THRES_SETT_REG 0x4F +#define BMM150_HIGH_THERS_SETT_REG 0x50 + +/* Repetitions control registers */ +#define BMM150_XY_REP_CTRL_REG 0x51 +#define BMM150_Z_REP_CTRL_REG 0x52 + +/* Preset mode definitions */ +#define BMM150_PRESETMODE_LOWPOWER 1 +#define BMM150_PRESETMODE_REGULAR 2 +#define BMM150_PRESETMODE_HIGHACCURACY 3 +#define BMM150_PRESETMODE_ENHANCED 4 + + +/* Data rate value definitions */ +#define BMM150_DATA_RATE_10HZ 0x00 +#define BMM150_DATA_RATE_02HZ 0x08 +#define BMM150_DATA_RATE_06HZ 0x10 +#define BMM150_DATA_RATE_08HZ 0x18 +#define BMM150_DATA_RATE_15HZ 0x20 +#define BMM150_DATA_RATE_20HZ 0x28 +#define BMM150_DATA_RATE_25HZ 0x30 +#define BMM150_DATA_RATE_30HZ 0x38 + +/* Advance self-test settings Definitions */ +#define BMM150_ADV_ST_OFF 0x00 +#define BMM150_ADV_ST_NEG 0x80 +#define BMM150_ADV_ST_POS 0xC0 + + +/* Interrupt settings and axes enable bits definitions */ +#define BMM150_CHANNEL_X_ENABLE 0x08 +#define BMM150_CHANNEL_Y_ENABLE 0x10 + + +/*Overflow Definitions */ +/* compensated output value returned if sensor had overflow */ +#define BMM150_OVERFLOW_OUTPUT -32768 +#define BMM150_OVERFLOW_OUTPUT_S32 ((int32_t)(-2147483647-1)) +#define BMM150_OVERFLOW_OUTPUT_FLOAT 0.0f +#define BMM150_FLIP_OVERFLOW_ADCVAL -4096 +#define BMM150_HALL_OVERFLOW_ADCVAL -16384 + + +/* Preset modes - Repetitions-XY Rates */ +#define BMM150_LOWPOWER_REPXY 1 +#define BMM150_REGULAR_REPXY 4 +#define BMM150_HIGHACCURACY_REPXY 23 +#define BMM150_ENHANCED_REPXY 7 + +/* Preset modes - Repetitions-Z Rates */ +#define BMM150_LOWPOWER_REPZ 2 +#define BMM150_REGULAR_REPZ 14 +#define BMM150_HIGHACCURACY_REPZ 82 +#define BMM150_ENHANCED_REPZ 26 + +/* Preset modes - Data rates */ +#define BMM150_LOWPOWER_DR BMM150_DATA_RATE_30HZ +#define BMM150_REGULAR_DR BMM150_DATA_RATE_30HZ +#define BMM150_HIGHACCURACY_DR BMM150_DATA_RATE_25HZ +#define BMM150_ENHANCED_DR BMM150_DATA_RATE_10HZ + +/* Power modes value definitions */ +#define BMM150_NORMAL_MODE 0x00 +#define BMM150_FORCED_MODE 0x02 +#define BMM150_SLEEP_MODE 0x06 + +/* Default power mode */ +#define BMM150_DEFAULT_POWER_MODE BMM150_NORMAL_MODE + +/* Default output data rate */ +#define BMM150_DEFAULT_ODR BMM150_DATA_RATE_30HZ + +/* Maximum output data rate */ +#define BMM150_MAX_DATA_RATE 30 + +/* Default BMM150_INT_SETT_CTRL_REG Value */ +#define BMM150_DEFAULT_INT_SETT 0x3F + +/* Trim Extended Registers */ +#define BMM150_DIG_X1 0x5D +#define BMM150_DIG_Y1 0x5E +#define BMM150_DIG_Z4_LSB 0x62 +#define BMM150_DIG_Z4_MSB 0x63 +#define BMM150_DIG_X2 0x64 +#define BMM150_DIG_Y2 0x65 +#define BMM150_DIG_Z2_LSB 0x68 +#define BMM150_DIG_Z2_MSB 0x69 +#define BMM150_DIG_Z1_LSB 0x6A +#define BMM150_DIG_Z1_MSB 0x6B +#define BMM150_DIG_XYZ1_LSB 0x6C +#define BMM150_DIG_XYZ1_MSB 0x6D +#define BMM150_DIG_Z3_LSB 0x6E +#define BMM150_DIG_Z3_MSB 0x6F +#define BMM150_DIG_XY2 0x70 +#define BMM150_DIG_XY1 0x71 + + +/* Mask definitions for power mode */ +#define BMM150_POWER_MASK 0x06 + +/* Mask definitions for data rate */ +#define BMM150_OUTPUT_DATA_RATE_MASK 0x38 + +#define BMM150_SOFT_RESET_VALUE 0x82 + +/* Mask definitions for Soft-Reset */ +#define BMM150_SOFT_RESET_MASK 0x82 + +/* This value is set based on Max output data rate value */ +#define BMM150_CONVERSION_INTERVAL (1000000 / 30) /* microseconds */ + + + +struct bmm150_data { + int16_t x; + int16_t y; + int16_t z; +}; + + +class BMM150 : public device::I2C +{ +public: + BMM150(int bus, const char *path, bool external, enum Rotation rotation); + virtual ~BMM150(); + + bool is_external(); + 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(); + + void print_registers(); + +protected: + virtual int probe(); + +private: + work_s _work; + bool _external; + + /* altitude conversion calibration */ + unsigned _call_interval; + + + struct mag_report _report; + ringbuffer::RingBuffer *_reports; + + struct mag_calibration_s _scale; + float _range_scale; + + orb_advert_t _topic; + int _orb_class_instance; + int _class_instance; + uint8_t _power; + uint8_t _output_data_rate; + bool _calibrated; /**< the calibration is valid */ + + int8_t dig_x1;/**< trim x1 data */ + int8_t dig_y1;/**< trim y1 data */ + + int8_t dig_x2;/**< trim x2 data */ + int8_t dig_y2;/**< trim y2 data */ + + uint16_t dig_z1;/**< trim z1 data */ + int16_t dig_z2;/**< trim z2 data */ + int16_t dig_z3;/**< trim z3 data */ + int16_t dig_z4;/**< trim z4 data */ + + uint8_t dig_xy1;/**< trim xy1 data */ + int8_t dig_xy2;/**< trim xy2 data */ + + uint16_t dig_xyz1;/**< trim xyz1 data */ + + perf_counter_t _sample_perf; + perf_counter_t _bad_transfers; + perf_counter_t _good_transfers; + perf_counter_t _duplicates; + + enum Rotation _rotation; + bool _got_duplicate; + + struct mag_report _last_report; /**< used for info() */ + + int init_trim_registers(void); + + /** + * Start automatic measurement. + */ + void start(); + + /** + * Stop automatic measurement. + */ + void stop(); + + + int collect(); //get results and publish + + static void cycle_trampoline(void *arg); + void cycle(); //main execution + + /** + * Read the specified number of bytes from BMM150. + * + * @param reg The register to read. + * @param data Pointer to buffer for bytes read. + * @param len Number of bytes to read + * @return OK if the transfer was successful, -errno otherwise. + */ + int get_data(uint8_t reg, uint8_t *data, unsigned len); + + /** + * Resets the chip. + */ + int reset(); + + /** + * Measurement self test + * + * @return 0 on success, 1 on failure + */ + int self_test(); + + /** + * Read a register from the BMM150 + * + * @param reg The register to read. + * @return The value that was read. + */ + uint8_t read_reg(uint8_t reg); + + /** + * Write a register in the BMM150 + * + * @param reg The register to write. + * @param value The new value to write. + * @return OK if the transfer was successful, -errno otherwise. + */ + int write_reg(uint8_t reg, uint8_t value); + + /** + * Modify a register in the BMM150 + * + * Bits are cleared before bits are set. + * + * @param reg The register to modify. + * @param clearbits Bits in the register to clear. + * @param setbits Bits in the register to set. + */ + void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); + + /* + set the power mode of BMM150. + */ + int set_power_mode(uint8_t power); + + /* + Set the data rate of BMM150. + */ + int set_data_rate(uint8_t data_rate); + + /* + Set the XY-repetitions + */ + int set_rep_xy(uint8_t rep_xy); + + /* + Set the Z- repetitions number + */ + int set_rep_z(uint8_t rep_z); + + /* + Set the preset modes for BMM150 sensor.The preset mode setting is + depend on Data Rate, XY and Z repetitions + */ + int set_presetmode(uint8_t presetmode); + + + /* do not allow to copy this class due to pointer data members */ + BMM150(const BMM150 &); + BMM150 operator=(const BMM150 &); + +}; + + + +#endif /* BMM150_HPP_ */ diff --git a/src/drivers/boards/px4fmu-v4/board_config.h b/src/drivers/boards/px4fmu-v4/board_config.h index 8fedff12c3..5426983820 100644 --- a/src/drivers/boards/px4fmu-v4/board_config.h +++ b/src/drivers/boards/px4fmu-v4/board_config.h @@ -155,6 +155,7 @@ /* I2C busses */ #define PX4_I2C_BUS_EXPANSION 1 #define PX4_I2C_BUS_LED PX4_I2C_BUS_EXPANSION +#define PX4_I2C_BUS_BMM150 PX4_I2C_BUS_EXPANSION /* Devices on the external bus. * @@ -163,6 +164,7 @@ #define PX4_I2C_OBDEV_LED 0x55 #define PX4_I2C_OBDEV_HMC5883 0x1e #define PX4_I2C_OBDEV_LIS3MDL 0x1e +#define PX4_I2C_OBDEV_BMM150 0x10 /* * ADC channels diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 77ae6d3e7c..bcde5728ec 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -88,6 +88,7 @@ #define DRV_BARO_DEVTYPE_LPS25H 0x40 #define DRV_ACC_DEVTYPE_BMI055 0x41 #define DRV_GYR_DEVTYPE_BMI055 0x42 +#define DRV_MAG_DEVTYPE_BMM150 0x43 /* * ioctl() definitions