From c10ea132b420072b5a0dfcb03940ddef2618dc87 Mon Sep 17 00:00:00 2001 From: Kevin Lopez Alvarez Date: Tue, 28 Aug 2018 17:44:34 +0200 Subject: [PATCH] PNI RM3100 magnetometer driver (#10302) * tested on SPI (px4fmu-v4pro) * WIP I2C support --- ROMFS/px4fmu_common/init.d/rc.sensors | 3 + .../boards/px4fmu-v4pro/board_config.h | 4 + src/drivers/drv_sensor.h | 1 + src/drivers/magnetometer/CMakeLists.txt | 1 + .../magnetometer/rm3100/CMakeLists.txt | 44 ++ src/drivers/magnetometer/rm3100/rm3100.cpp | 662 ++++++++++++++++++ src/drivers/magnetometer/rm3100/rm3100.h | 252 +++++++ .../magnetometer/rm3100/rm3100_i2c.cpp | 165 +++++ .../magnetometer/rm3100/rm3100_main.cpp | 360 ++++++++++ src/drivers/magnetometer/rm3100/rm3100_main.h | 122 ++++ .../magnetometer/rm3100/rm3100_spi.cpp | 176 +++++ 11 files changed, 1790 insertions(+) create mode 100644 src/drivers/magnetometer/rm3100/CMakeLists.txt create mode 100644 src/drivers/magnetometer/rm3100/rm3100.cpp create mode 100644 src/drivers/magnetometer/rm3100/rm3100.h create mode 100644 src/drivers/magnetometer/rm3100/rm3100_i2c.cpp create mode 100644 src/drivers/magnetometer/rm3100/rm3100_main.cpp create mode 100644 src/drivers/magnetometer/rm3100/rm3100_main.h create mode 100644 src/drivers/magnetometer/rm3100/rm3100_spi.cpp diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 08375f0186..71018dc073 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -322,6 +322,9 @@ then # Possible external compasses hmc5883 -C -T -X start + + #RM3100 + rm3100 start fi if ver hwcmp PX4FMU_V5 diff --git a/src/drivers/boards/px4fmu-v4pro/board_config.h b/src/drivers/boards/px4fmu-v4pro/board_config.h index fd279db638..9f824ee382 100644 --- a/src/drivers/boards/px4fmu-v4pro/board_config.h +++ b/src/drivers/boards/px4fmu-v4pro/board_config.h @@ -153,6 +153,8 @@ #define PX4_SPI_BUS_EXT0 5 #define PX4_SPI_BUS_EXT1 6 +#define PX4_SPI_BUS_EXT PX4_SPI_BUS_EXT0 + /* Use these in place of the uint32_t enumeration to select a specific SPI device on SPI1 */ #define PX4_SPIDEV_GYRO PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 1) @@ -171,6 +173,8 @@ #define PX4_SPIDEV_EXT0 PX4_MK_SPI_SEL(PX4_SPI_BUS_EXT0, 1) #define PX4_SPIDEV_EXT1 PX4_MK_SPI_SEL(PX4_SPI_BUS_EXT1, 1) +#define PX4_SPIDEV_RM_EXT PX4_SPIDEV_EXT0 + /* I2C busses */ #define PX4_I2C_BUS_ONBOARD 1 #define PX4_I2C_BUS_EXPANSION 2 diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 40b11ef8f5..cc84ba5bfb 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -58,6 +58,7 @@ #define DRV_MAG_DEVTYPE_MPU9250 0x04 #define DRV_MAG_DEVTYPE_LIS3MDL 0x05 #define DRV_MAG_DEVTYPE_IST8310 0x06 +#define DRV_MAG_DEVTYPE_RM3100 0x07 #define DRV_ACC_DEVTYPE_LSM303D 0x11 #define DRV_ACC_DEVTYPE_BMA180 0x12 #define DRV_ACC_DEVTYPE_MPU6000 0x13 diff --git a/src/drivers/magnetometer/CMakeLists.txt b/src/drivers/magnetometer/CMakeLists.txt index 98dbefa8a8..541c9e0f14 100644 --- a/src/drivers/magnetometer/CMakeLists.txt +++ b/src/drivers/magnetometer/CMakeLists.txt @@ -36,3 +36,4 @@ add_subdirectory(hmc5883) add_subdirectory(ist8310) add_subdirectory(lis3mdl) add_subdirectory(lsm303agr) +add_subdirectory(rm3100) diff --git a/src/drivers/magnetometer/rm3100/CMakeLists.txt b/src/drivers/magnetometer/rm3100/CMakeLists.txt new file mode 100644 index 0000000000..af791a83b5 --- /dev/null +++ b/src/drivers/magnetometer/rm3100/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2018 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__rm3100 + MAIN rm3100 + STACK_MAIN 1200 + COMPILE_FLAGS + SRCS + rm3100_i2c.cpp + rm3100_spi.cpp + rm3100_main.cpp + rm3100.cpp + DEPENDS + ) diff --git a/src/drivers/magnetometer/rm3100/rm3100.cpp b/src/drivers/magnetometer/rm3100/rm3100.cpp new file mode 100644 index 0000000000..fb37f0730a --- /dev/null +++ b/src/drivers/magnetometer/rm3100/rm3100.cpp @@ -0,0 +1,662 @@ +/**************************************************************************** + * + * Copyright (c) 2018 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 rm3100.cpp + * + * Driver for the RM3100 magnetometer connected via I2C or SPI. + * + * Based on the lis3mdl driver. + */ + +#include "rm3100.h" + +RM3100::RM3100(device::Device *interface, const char *path, enum Rotation rotation) : + CDev("RM3100", path), + _interface(interface), + _work{}, + _reports(nullptr), + _scale{}, + _last_report{}, + _mag_topic(nullptr), + _comms_errors(perf_alloc(PC_COUNT, "rm3100_comms_errors")), + _conf_errors(perf_alloc(PC_COUNT, "rm3100_conf_errors")), + _range_errors(perf_alloc(PC_COUNT, "rm3100_range_errors")), + _sample_perf(perf_alloc(PC_ELAPSED, "rm3100_read")), + _calibrated(false), + _continuous_mode_set(false), + _mode(SINGLE), + _rotation(rotation), + _measure_ticks(0), + _class_instance(-1), + _orb_class_instance(-1), + _range_scale(1.0f / (RM3100_SENSITIVITY * UTESLA_TO_GAUSS)), + _check_state_cnt(0) +{ + // set the device type from the interface + _device_id.devid_s.bus_type = _interface->get_device_bus_type(); + _device_id.devid_s.bus = _interface->get_device_bus(); + _device_id.devid_s.address = _interface->get_device_address(); + _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_RM3100; + + // enable debug() calls + _debug_enabled = false; + + // 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; + + // work_cancel in the dtor will explode if we don't do this... + memset(&_work, 0, sizeof(_work)); +} + +RM3100::~RM3100() +{ + /* make sure we are truly inactive */ + stop(); + + if (_reports != nullptr) { + delete _reports; + } + + if (_class_instance != -1) { + unregister_class_devname(MAG_BASE_DEVICE_PATH, _class_instance); + } + + // free perf counters + perf_free(_sample_perf); + perf_free(_comms_errors); + perf_free(_range_errors); + perf_free(_conf_errors); +} + +int +RM3100::self_test() +{ + /* Stop current measurements */ + stop(); + + /* Chances are that a poll event was triggered, so wait for conversion and read registers in order to clear DRDY bit */ + usleep(RM3100_CONVERSION_INTERVAL); + collect(); + + /* Fail if calibration is not good */ + int ret = 0; + uint8_t cmd = 0; + + /* Configure mag into self test mode */ + cmd = BIST_SELFTEST; + ret = _interface->write(ADDR_BIST, &cmd, 1); + + if (ret != PX4_OK) { + return ret; + } + + /* Now we need to write to POLL to launch self test */ + cmd = POLL_XYZ; + ret = _interface->write(ADDR_POLL, &cmd, 1); + + if (ret != PX4_OK) { + return ret; + } + + /* Now wait for status register */ + usleep(RM3100_CONVERSION_INTERVAL); + + if (check_measurement() != PX4_OK) { + return -1;; + } + + /* Now check BIST register to see whether self test is ok or not*/ + ret = _interface->read(ADDR_BIST, &cmd, 1); + + if (ret != PX4_OK) { + return ret; + } + + ret = !((cmd & BIST_XYZ_OK) == BIST_XYZ_OK); + + /* Restart measurement state machine */ + start(); + + return ret; +} + +int +RM3100::check_measurement() +{ + uint8_t status = 0; + int ret = -1; + + ret = _interface->read(ADDR_STATUS, &status, 1); + + if (ret != 0) { + return ret; + } + + return !((status & STATUS_DRDY) == STATUS_DRDY) ; +} + +int +RM3100::collect() +{ + /* Check whether a measurement is available or not, otherwise return immediately */ + if (check_measurement() != 0) { + DEVICE_DEBUG("No measurement available"); + return 0; + } + +#pragma pack(push, 1) + struct { + uint8_t x[3]; + uint8_t y[3]; + uint8_t z[3]; + } rm_report; +#pragma pack(pop) + + int ret = 0; + + int32_t xraw; + int32_t yraw; + int32_t zraw; + + float xraw_f; + float yraw_f; + float zraw_f; + + struct mag_report new_mag_report; + bool sensor_is_onboard = false; + + perf_begin(_sample_perf); + + new_mag_report.timestamp = hrt_absolute_time(); + new_mag_report.error_count = perf_event_count(_comms_errors); + new_mag_report.scaling = _range_scale; + new_mag_report.device_id = _device_id.devid; + + ret = _interface->read(ADDR_MX, (uint8_t *)&rm_report, sizeof(rm_report)); + + if (ret != OK) { + perf_count(_comms_errors); + PX4_WARN("Register read error."); + return ret; + } + + /* Rearrange mag data */ + xraw = ((rm_report.x[0] << 16) | (rm_report.x[1] << 8) | rm_report.x[2]); + yraw = ((rm_report.y[0] << 16) | (rm_report.y[1] << 8) | rm_report.y[2]); + zraw = ((rm_report.z[0] << 16) | (rm_report.z[1] << 8) | rm_report.z[2]); + + /* Convert 24 bit signed values to 32 bit signed values */ + convert_signed(&xraw); + convert_signed(&yraw); + convert_signed(&zraw); + + /* There is no temperature sensor */ + new_mag_report.temperature = 0.0f; + + // XXX revisit for SPI part, might require a bus type IOCTL + unsigned dummy = 0; + sensor_is_onboard = !_interface->ioctl(MAGIOCGEXTERNAL, dummy); + new_mag_report.is_external = !sensor_is_onboard; + + /** + * RAW outputs + * As we only have 16 bits to store raw data, the following values are not correct + */ + new_mag_report.x_raw = (int16_t)(xraw >> 8); + new_mag_report.y_raw = (int16_t)(yraw >> 8); + new_mag_report.z_raw = (int16_t)(zraw >> 8); + + xraw_f = xraw; + yraw_f = yraw; + zraw_f = zraw; + + /* apply user specified rotation */ + rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); + + new_mag_report.x = ((xraw_f * _range_scale) - _scale.x_offset) * _scale.x_scale; + new_mag_report.y = ((yraw_f * _range_scale) - _scale.y_offset) * _scale.y_scale; + new_mag_report.z = ((zraw_f * _range_scale) - _scale.z_offset) * _scale.z_scale; + + if (!(_pub_blocked)) { + + if (_mag_topic != nullptr) { + /* publish it */ + orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_mag_report); + + } else { + _mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &new_mag_report, + &_orb_class_instance, (sensor_is_onboard) ? ORB_PRIO_HIGH : ORB_PRIO_MAX); + + if (_mag_topic == nullptr) { + DEVICE_DEBUG("ADVERT FAIL"); + } + } + } + + _last_report = new_mag_report; + + /* post a report to the ring */ + _reports->force(&new_mag_report); + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + + ret = OK; + + perf_end(_sample_perf); + return ret; +} + +void +RM3100::convert_signed(int32_t *n) +{ + /* Sensor returns values as 24 bit signed values, so we need to manually convert to 32 bit signed values */ + if ((*n & (1 << 23)) == (1 << 23)) { + *n |= 0xFF000000; + } +} + +void +RM3100::cycle() +{ + /* _measure_ticks == 0 is used as _task_should_exit */ + if (_measure_ticks == 0) { + return; + } + + /* Collect last measurement at the start of every cycle */ + if (collect() != OK) { + DEVICE_DEBUG("collection error"); + /* restart the measurement state machine */ + start(); + return; + } + + + if (measure() != OK) { + DEVICE_DEBUG("measure error"); + } + + if (_measure_ticks > 0) { + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, + &_work, + (worker_t)&RM3100::cycle_trampoline, + this, + _measure_ticks); + } +} + +void +RM3100::cycle_trampoline(void *arg) +{ + RM3100 *dev = (RM3100 *)arg; + + dev->cycle(); +} + +int +RM3100::init() +{ + int ret = PX4_ERROR; + + ret = CDev::init(); + + if (ret != OK) { + DEVICE_DEBUG("CDev init failed"); + return ret; + } + + /* allocate basic report buffers */ + _reports = new ringbuffer::RingBuffer(2, sizeof(mag_report)); + + if (_reports == nullptr) { + return PX4_ERROR; + } + + /* reset the device configuration */ + reset(); + + _class_instance = register_class_devname(MAG_BASE_DEVICE_PATH); + + ret = self_test(); + + if (ret != PX4_OK) { + PX4_ERR("self test failed"); + } + + return ret; +} + +int +RM3100::ioctl(struct file *file_pointer, int cmd, unsigned long arg) +{ + unsigned dummy = 0; + + switch (cmd) { + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _measure_ticks = 0; + return PX4_OK; + + /* zero would be bad */ + case 0: + return -EINVAL; + + case SENSOR_POLLRATE_DEFAULT: { + /* do we need to start internal polling? */ + bool not_started = (_measure_ticks == 0); + + /* set interval for next measurement to minimum legal value */ + _measure_ticks = USEC2TICK(RM3100_CONVERSION_INTERVAL); + + /* if we need to start the poll state machine, do it */ + if (not_started) { + start(); + } + + return PX4_OK; + } + + /* Uses arg (hz) for a custom poll rate */ + default: { + /* do we need to start internal polling? */ + bool not_started = (_measure_ticks == 0); + + /* convert hz to tick interval via microseconds */ + unsigned ticks = USEC2TICK(1000000 / arg); + + /* check against maximum rate */ + if (ticks < USEC2TICK(RM3100_CONVERSION_INTERVAL)) { + return -EINVAL; + } + + /* update interval for next measurement */ + _measure_ticks = ticks; + + /* if we need to start the poll state machine, do it */ + if (not_started) { + start(); + } + + return PX4_OK; + } + } + } + + 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 PX4_OK; + } + + case SENSORIOCRESET: + return reset(); + + case MAGIOCSSAMPLERATE: + /* same as pollrate because device is in single measurement mode*/ + return ioctl(file_pointer, SENSORIOCSPOLLRATE, arg); + + case MAGIOCGSAMPLERATE: + /* same as pollrate because device is in single measurement mode*/ + return 1000000 / TICK2USEC(_measure_ticks); + + case MAGIOCSRANGE: + /* field measurement range cannot be configured for this sensor (8 Gauss) */ + return OK; + + case MAGIOCGRANGE: + /* field measurement range cannot be configured for this sensor (8 Gauss) */ + return 8; + + case MAGIOCSSCALE: + /* set new scale factors */ + memcpy(&_scale, (struct mag_calibration_s *)arg, sizeof(_scale)); + return 0; + + case MAGIOCGSCALE: + /* copy out scale factors */ + memcpy((struct mag_calibration_s *)arg, &_scale, sizeof(_scale)); + return 0; + + + case MAGIOCCALIBRATE: + /* This is left for compatibility with the IOCTL call in mag calibration */ + return OK; + + case MAGIOCGEXTERNAL: + DEVICE_DEBUG("MAGIOCGEXTERNAL in main driver"); + return _interface->ioctl(cmd, dummy); + + case DEVIOCGDEVICEID: + return _interface->ioctl(cmd, dummy); + + default: + /* give it to the superclass */ + return CDev::ioctl(file_pointer, cmd, arg); + } +} + +int +RM3100::measure() +{ + int ret = 0; + uint8_t cmd = 0; + + /* Send the command to begin a measurement. */ + if ((_mode == CONTINUOUS) && !_continuous_mode_set) { + cmd = (CMM_DEFAULT | CONTINUOUS_MODE); + ret = _interface->write(ADDR_CMM, &cmd, 1); + _continuous_mode_set = true; + + } else if (_mode == SINGLE) { + if (_continuous_mode_set) { + /* This is needed for polling mode */ + cmd = (CMM_DEFAULT | POLLING_MODE); + ret = _interface->write(ADDR_CMM, &cmd, 1); + + if (ret != OK) { + perf_count(_comms_errors); + return ret; + } + + _continuous_mode_set = false; + } + + cmd = POLL_XYZ; + ret = _interface->write(ADDR_POLL, &cmd, 1); + } + + + if (ret != OK) { + perf_count(_comms_errors); + } + + return ret; +} + +void +RM3100::print_info() +{ + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + PX4_INFO("poll interval: %u ticks", _measure_ticks); + print_message(_last_report); + _reports->print_info("report queue"); +} + +int +RM3100::reset() +{ + int ret = 0; + + ret = set_default_register_values(); + + if (ret != OK) { + return PX4_ERROR; + } + + return PX4_OK; +} + +int +RM3100::read(struct file *file_pointer, char *buffer, size_t buffer_len) +{ + unsigned count = buffer_len / sizeof(struct mag_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 (_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 (_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(); + + /* trigger a measurement */ + if (measure() != OK) { + ret = -EIO; + break; + } + + /* wait for it to complete */ + usleep(RM3100_CONVERSION_INTERVAL); + + /* run the collection phase */ + if (collect() != OK) { + ret = -EIO; + break; + } + + if (_reports->get(mag_buf)) { + ret = sizeof(struct mag_report); + } + } while (0); + + return ret; +} + +int +RM3100::set_default_register_values() +{ + uint8_t cmd[2] = {0, 0}; + + cmd[0] = CCX_DEFAULT_MSB; + cmd[1] = CCX_DEFAULT_LSB; + _interface->write(ADDR_CCX, cmd, 2); + + cmd[0] = CCY_DEFAULT_MSB; + cmd[1] = CCY_DEFAULT_LSB; + _interface->write(ADDR_CCY, cmd, 2); + + cmd[0] = CCZ_DEFAULT_MSB; + cmd[1] = CCZ_DEFAULT_LSB; + _interface->write(ADDR_CCZ, cmd, 2); + + cmd[0] = CMM_DEFAULT; + _interface->write(ADDR_CMM, cmd, 1); + + cmd[0] = TMRC_DEFAULT; + _interface->write(ADDR_TMRC, cmd, 1); + + cmd[0] = BIST_DEFAULT; + _interface->write(ADDR_BIST, cmd, 1); + + return PX4_OK; +} + +void +RM3100::start() +{ + /* reset the report ring and state machine */ + _reports->flush(); + + set_default_register_values(); + _measure_ticks = USEC2TICK(RM3100_CONVERSION_INTERVAL); + + /* schedule a cycle to start things */ + work_queue(HPWORK, &_work, (worker_t)&RM3100::cycle_trampoline, this, 1); +} + +void +RM3100::stop() +{ + if (_measure_ticks > 0) { + /* ensure no new items are queued while we cancel this one */ + _measure_ticks = 0; + work_cancel(HPWORK, &_work); + } +} diff --git a/src/drivers/magnetometer/rm3100/rm3100.h b/src/drivers/magnetometer/rm3100/rm3100.h new file mode 100644 index 0000000000..d31239c217 --- /dev/null +++ b/src/drivers/magnetometer/rm3100/rm3100.h @@ -0,0 +1,252 @@ +/**************************************************************************** + * + * Copyright (c) 2018 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 rm3100.h + * + * Shared defines for the RM3100 driver. + */ + +#pragma once + +#include + +#include +#include +#include +#include + +#include + +#include +#include + +#ifndef CONFIG_SCHED_WORKQUEUE +# error This requires CONFIG_SCHED_WORKQUEUE. +#endif + +/** + * RM3100 internal constants and data structures. + */ + +#define RM3100_CONVERSION_INTERVAL 6850 // Microseconds, corresponds to 146 Hz (cycle count 200 on 3 axis) +#define UTESLA_TO_GAUSS 100.0f +#define RM3100_SENSITIVITY 75.0f + +#define ADDR_POLL 0x00 +#define ADDR_CMM 0x01 +#define ADDR_CCX 0x04 +#define ADDR_CCY 0x06 +#define ADDR_CCZ 0x08 +#define ADDR_TMRC 0x0B +#define ADDR_MX 0x24 +#define ADDR_MY 0x27 +#define ADDR_MZ 0x2A +#define ADDR_BIST 0x33 +#define ADDR_STATUS 0x34 +#define ADDR_HSHAKE 0x35 +#define ADDR_REVID 0x36 + +#define CCX_DEFAULT_MSB 0x00 +#define CCX_DEFAULT_LSB 0xC8 +#define CCY_DEFAULT_MSB CCX_DEFAULT_MSB +#define CCY_DEFAULT_LSB CCX_DEFAULT_LSB +#define CCZ_DEFAULT_MSB CCX_DEFAULT_MSB +#define CCZ_DEFAULT_LSB CCX_DEFAULT_LSB +#define CMM_DEFAULT 0x70 // No continuous mode +#define CONTINUOUS_MODE (1 << 0) +#define POLLING_MODE (0 << 0) +#define TMRC_DEFAULT 0x94 +#define BIST_SELFTEST 0x8F +#define BIST_DEFAULT 0x00 +#define BIST_XYZ_OK ((1 << 4) | (1 << 5) | (1 << 6)) +#define STATUS_DRDY (1 << 7) +#define POLL_XYZ 0x70 +#define RM3100_REVID 0x22 + +#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0])) + +/* interface factories */ +extern device::Device *RM3100_SPI_interface(int bus); +extern device::Device *RM3100_I2C_interface(int bus); +typedef device::Device *(*RM3100_constructor)(int); + +enum RM3100_BUS { + RM3100_BUS_ALL = 0, + RM3100_BUS_I2C_INTERNAL, + RM3100_BUS_I2C_EXTERNAL, + RM3100_BUS_SPI_INTERNAL, + RM3100_BUS_SPI_EXTERNAL +}; + +enum OPERATING_MODE { + CONTINUOUS = 0, + SINGLE +}; + + +class RM3100 : public device::CDev +{ +public: + RM3100(device::Device *interface, const char *path, enum Rotation rotation); + + virtual ~RM3100(); + + virtual int init(); + + virtual int ioctl(struct file *file_pointer, int cmd, unsigned long arg); + + virtual int read(struct file *file_pointer, char *buffer, size_t buffer_len); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + + /** + * Configures the device with default register values. + */ + int set_default_register_values(); + + /** + * Stop the automatic measurement state machine. + */ + void stop(); + +protected: + Device *_interface; + +private: + work_s _work; + + ringbuffer::RingBuffer *_reports; + + struct mag_calibration_s _scale; + + struct mag_report _last_report {}; /**< used for info() */ + + orb_advert_t _mag_topic; + + perf_counter_t _comms_errors; + perf_counter_t _conf_errors; + perf_counter_t _range_errors; + perf_counter_t _sample_perf; + + /* status reporting */ + bool _calibrated; /**< the calibration is valid */ + bool _continuous_mode_set; + + enum OPERATING_MODE _mode; + enum Rotation _rotation; + + unsigned int _measure_ticks; + + int _class_instance; + int _orb_class_instance; + + float _range_scale; + + uint8_t _check_state_cnt; + + /** + * Collect the result of the most recent measurement. + */ + int collect(); + + /** + * Run sensor self-test + * + * @return 0 if self-test is ok, 1 else + */ + int self_test(); + + /** + * Check whether new data is available or not + * + * @return 0 if new data is available, 1 else + */ + int check_measurement(); + + /** + * Converts int24_t stored in 32-bit container to int32_t + */ + void convert_signed(int32_t *n); + + /** + * @brief Performs 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(); + + /** + * @brief 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. + * + * @return OK if the measurement command was successful. + */ + int measure(); + + /** + * @brief Resets the device + */ + int reset(); + + /** + * @brief Initialises 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(); + + /* this class has pointer data members, do not allow copying it */ + RM3100(const RM3100 &); + + RM3100 operator=(const RM3100 &); +}; // class RM3100 diff --git a/src/drivers/magnetometer/rm3100/rm3100_i2c.cpp b/src/drivers/magnetometer/rm3100/rm3100_i2c.cpp new file mode 100644 index 0000000000..900010db2a --- /dev/null +++ b/src/drivers/magnetometer/rm3100/rm3100_i2c.cpp @@ -0,0 +1,165 @@ +/**************************************************************************** + * + * Copyright (c) 2018 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 rm3100_i2c.cpp + * + * I2C interface for RM3100 + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include "board_config.h" +#include "rm3100.h" + +#if defined(PX4_I2C_BUS_ONBOARD) || defined(PX4_I2C_BUS_EXPANSION) + +#define RM3100_ADDRESS 0x20 + +#define DIR_READ (1<<7) +#define DIR_WRITE (0<<7) + +class RM3100_I2C : public device::I2C +{ +public: + RM3100_I2C(int bus); + virtual ~RM3100_I2C() = default; + + virtual int init(); + virtual int ioctl(unsigned operation, unsigned &arg); + virtual int read(unsigned address, void *data, unsigned count); + virtual int write(unsigned address, void *data, unsigned count); + +protected: + virtual int probe(); + +}; + +device::Device * +RM3100_I2C_interface(int bus); + +device::Device * +RM3100_I2C_interface(int bus) +{ + return new RM3100_I2C(bus); +} + +RM3100_I2C::RM3100_I2C(int bus) : + I2C("RM300_I2C", nullptr, bus, RM3100_ADDRESS, 400000) +{ + _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_RM3100; +} + +int +RM3100_I2C::init() +{ + /* this will call probe() */ + return I2C::init(); +} + +int +RM3100_I2C::ioctl(unsigned operation, unsigned &arg) +{ + switch (operation) { + + case MAGIOCGEXTERNAL: + return external(); + + case DEVIOCGDEVICEID: + return CDev::ioctl(nullptr, operation, arg); + + default: + return -EINVAL; + } +} + +int +RM3100_I2C::probe() +{ + uint8_t data = 0; + + _retries = 10; + + if (read(ADDR_REVID, &data, 1)) { + DEVICE_DEBUG("RM3100 read_reg fail"); + return -EIO; + } + + _retries = 2; + + if (data != RM3100_REVID) { + DEVICE_DEBUG("RM3100 bad ID: %02x", data); + return -EIO; + } + + return OK; +} + +int +RM3100_I2C::read(unsigned address, void *data, unsigned count) +{ + uint8_t cmd = address | DIR_READ; + return transfer(&cmd, 1, (uint8_t *)data, count); +} + +int +RM3100_I2C::write(unsigned address, void *data, unsigned count) +{ + uint8_t buf[32]; + + if (sizeof(buf) < (count + 1)) { + return -EIO; + } + + buf[0] = address | DIR_WRITE; + memcpy(&buf[1], data, count); + + return transfer(&buf[0], count + 1, nullptr, 0); +} + +#endif /* PX4_I2C_OBDEV_RM3100 */ diff --git a/src/drivers/magnetometer/rm3100/rm3100_main.cpp b/src/drivers/magnetometer/rm3100/rm3100_main.cpp new file mode 100644 index 0000000000..90916d548b --- /dev/null +++ b/src/drivers/magnetometer/rm3100/rm3100_main.cpp @@ -0,0 +1,360 @@ +/**************************************************************************** + * + * Copyright (c) 2018 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 rm3100_main.cpp + * + * Driver for the RM3100 magnetometer connected via I2C or SPI. + */ + +#include "rm3100_main.h" +#include + +/** + * Driver 'main' command. + */ +extern "C" __EXPORT int rm3100_main(int argc, char *argv[]); + +int +rm3100::info(RM3100_BUS bus_id) +{ + struct rm3100_bus_option &bus = find_bus(bus_id); + + PX4_WARN("running on bus: %u (%s)\n", (unsigned)bus.bus_id, bus.devpath); + bus.dev->print_info(); + return 1; +} + +bool +rm3100::init(RM3100_BUS bus_id) +{ + struct rm3100_bus_option &bus = find_bus(bus_id); + const char *path = bus.devpath; + + int fd = open(path, O_RDONLY); + + if (fd < 0) { + return false; + } + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + close(fd); + errx(1, "Failed to setup poll rate"); + return false; + + } else { + PX4_INFO("Poll rate set to max (146 Hz)"); + } + + close(fd); + + return true; +} + +bool +rm3100::start_bus(struct rm3100_bus_option &bus, Rotation rotation) +{ + if (bus.dev != nullptr) { + errx(1, "bus option already started"); + return false; + } + + device::Device *interface = bus.interface_constructor(bus.busnum); + + if (interface->init() != OK) { + delete interface; + warnx("no device on bus %u", (unsigned)bus.bus_id); + + return false; + } + + bus.dev = new RM3100(interface, bus.devpath, rotation); + + if (bus.dev != nullptr && + bus.dev->init() != OK) { + delete bus.dev; + bus.dev = NULL; + return false; + } + + return true; +} + +int +rm3100::start(RM3100_BUS bus_id, Rotation rotation) +{ + bool started = false; + + for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) { + if (bus_id == RM3100_BUS_ALL && bus_options[i].dev != NULL) { + // this device is already started + continue; + } + + if (bus_id != RM3100_BUS_ALL && bus_options[i].bus_id != bus_id) { + // not the one that is asked for + continue; + } + + started |= start_bus(bus_options[i], rotation); + //init(bus_id); + } + + return started; +} + +int +rm3100::stop() +{ + bool stopped = false; + + for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) { + if (bus_options[i].dev != nullptr) { + bus_options[i].dev->stop(); + delete bus_options[i].dev; + bus_options[i].dev = nullptr; + stopped = true; + } + } + + return !stopped; +} + +bool +rm3100::test(RM3100_BUS bus_id) +{ + struct rm3100_bus_option &bus = find_bus(bus_id); + struct mag_report report; + ssize_t sz; + int ret; + const char *path = bus.devpath; + + int fd = open(path, O_RDONLY); + + if (fd < 0) { + PX4_WARN("%s open failed (try 'rm3100 start')", path); + return 1; + } + + /* do a simple demand read */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + PX4_WARN("immediate read failed"); + return 1; + } + + print_message(report); + + /* check if mag is onboard or external */ + if (ioctl(fd, MAGIOCGEXTERNAL, 0) < 0) { + PX4_WARN("failed to get if mag is onboard or external"); + return 1; + } + + /* set the queue depth to 5 */ + if (ioctl(fd, SENSORIOCSQUEUEDEPTH, 10) != OK) { + PX4_WARN("failed to set queue depth"); + return 1; + } + + /* start the sensor polling at 2Hz */ + if (ioctl(fd, SENSORIOCSPOLLRATE, 2) != OK) { + PX4_WARN("failed to set 2Hz poll rate"); + return 1; + } + + struct pollfd fds; + + /* read the sensor 5x and report each value */ + for (unsigned i = 0; i < 5; i++) { + + /* wait for data to be ready */ + fds.fd = fd; + fds.events = POLLIN; + ret = poll(&fds, 1, 2000); + + if (ret != 1) { + PX4_WARN("timed out waiting for sensor data"); + return 1; + } + + /* now go get it */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + PX4_WARN("periodic read failed"); + return 1; + } + + print_message(report); + } + + PX4_INFO("PASS"); + return 1; +} + +bool +rm3100::reset(RM3100_BUS bus_id) +{ + struct rm3100_bus_option &bus = find_bus(bus_id); + const char *path = bus.devpath; + + int fd = open(path, O_RDONLY); + + if (fd < 0) { + PX4_WARN("open failed "); + return 1; + } + + if (ioctl(fd, SENSORIOCRESET, 0) < 0) { + PX4_WARN("driver reset failed"); + return 1; + } + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + PX4_WARN("driver poll restart failed"); + return 1; + } + + return 0; +} + +void +rm3100::usage() +{ + PX4_WARN("missing command: try 'start', 'info', 'test', 'reset', 'info'"); + PX4_WARN("options:"); + PX4_WARN(" -R rotation"); + PX4_WARN(" -X external I2C bus"); + PX4_WARN(" -I internal I2C bus"); + PX4_WARN(" -S external SPI bus"); + PX4_WARN(" -s internal SPI bus"); +} + +int +rm3100_main(int argc, char *argv[]) +{ + int myoptind = 1; + int ch; + const char *myoptarg = nullptr; + + enum RM3100_BUS bus_id = RM3100_BUS_ALL; + enum Rotation rotation = ROTATION_NONE; + + while ((ch = px4_getopt(argc, argv, "XISR:T", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'R': + rotation = (enum Rotation)atoi(myoptarg); + break; + + case 'I': + bus_id = RM3100_BUS_I2C_INTERNAL; + break; + + case 'X': + bus_id = RM3100_BUS_I2C_EXTERNAL; + break; + + case 'S': + bus_id = RM3100_BUS_SPI_EXTERNAL; + break; + + case 's': + bus_id = RM3100_BUS_SPI_INTERNAL; + break; + + default: + rm3100::usage(); + return 0; + } + } + + if (myoptind >= argc) { + rm3100::usage(); + return -1; + } + + const char *verb = argv[myoptind]; + + // Start/load the driver + if (!strcmp(verb, "start")) { + + if (rm3100::start(bus_id, rotation)) { + + rm3100::init(bus_id); + + return 0; + + } else { + return 1; + } + } + + // Stop the driver + if (!strcmp(verb, "stop")) { + return rm3100::stop(); + } + + // Test the driver/device + if (!strcmp(verb, "test")) { + return rm3100::test(bus_id); + } + + // Reset the driver + if (!strcmp(verb, "reset")) { + return rm3100::reset(bus_id); + } + + // Print driver information + if (!strcmp(verb, "info") || + !strcmp(verb, "status")) { + return rm3100::info(bus_id); + } + + PX4_INFO("unrecognized command, try 'start', 'test', 'reset' or 'info'"); + return 1; +} + +struct +rm3100::rm3100_bus_option &rm3100::find_bus(RM3100_BUS bus_id) +{ + for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) { + if ((bus_id == RM3100_BUS_ALL || + bus_id == bus_options[i].bus_id) && bus_options[i].dev != NULL) { + return bus_options[i]; + } + } + + errx(1, "bus %u not started", (unsigned)bus_id); +} diff --git a/src/drivers/magnetometer/rm3100/rm3100_main.h b/src/drivers/magnetometer/rm3100/rm3100_main.h new file mode 100644 index 0000000000..7eb6958178 --- /dev/null +++ b/src/drivers/magnetometer/rm3100/rm3100_main.h @@ -0,0 +1,122 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file rm3100_main.h + */ + +#pragma once + +#include "rm3100.h" + +namespace rm3100 +{ +/** + * @struct List of supported bus configurations + */ +struct rm3100_bus_option { + RM3100_BUS bus_id; + const char *devpath; + RM3100_constructor interface_constructor; + uint8_t busnum; + RM3100 *dev; +} bus_options[] = { +#ifdef PX4_I2C_BUS_EXPANSION + { RM3100_BUS_I2C_EXTERNAL, "/dev/rm3100_ext", &RM3100_I2C_interface, PX4_I2C_BUS_EXPANSION, NULL }, +#endif /* PX4_I2C_BUS_EXPANSION */ +#ifdef PX4_I2C_BUS_EXPANSION1 + { RM3100_BUS_I2C_EXTERNAL, "/dev/rm3100_ext1", &RM3100_I2C_interface, PX4_I2C_BUS_EXPANSION1, NULL }, +#endif /* PX4_I2C_BUS_EXPANSION1 */ +#ifdef PX4_I2C_BUS_EXPANSION2 + { RM3100_BUS_I2C_EXTERNAL, "/dev/rm3100_ext2", &RM3100_I2C_interface, PX4_I2C_BUS_EXPANSION2, NULL }, +#endif /* PX4_I2C_BUS_EXPANSION2 */ +#ifdef PX4_I2C_BUS_ONBOARD + { RM3100_BUS_I2C_INTERNAL, "/dev/rm3100_int", &RM3100_I2C_interface, PX4_I2C_BUS_ONBOARD, NULL }, +#endif /* PX4_I2C_BUS_ONBOARD */ +#ifdef PX4_SPIDEV_RM + { RM3100_BUS_SPI_INTERNAL, "/dev/rm3100_spi_int", &RM3100_SPI_interface, PX4_SPI_BUS_SENSORS, NULL }, +#endif /* PX4_SPIDEV_RM */ +#ifdef PX4_SPIDEV_RM_EXT + { RM3100_BUS_SPI_EXTERNAL, "/dev/rm3100_spi_ext", &RM3100_SPI_interface, PX4_SPI_BUS_EXT, NULL }, +#endif /* PX4_SPIDEV_RM_EXT */ +}; + +/** + * @brief Finds a bus structure for a bus_id + */ +rm3100_bus_option &find_bus(RM3100_BUS bus_id); + +/** + * @brief Prints info about the driver. + */ +int info(RM3100_BUS bus_id); + +/** + * @brief Initializes the driver -- sets defaults and starts a cycle + */ +bool init(RM3100_BUS bus_id); + +/** + * @brief Resets the driver. + */ +bool reset(RM3100_BUS bus_id); + +/** + * @brief Starts the driver for a specific bus option + */ +bool start_bus(struct rm3100_bus_option &bus, Rotation rotation); + +/** + * @brief Starts the driver. This function call only returns once the driver + * is either successfully up and running or failed to start. + */ +int start(RM3100_BUS bus_id, Rotation rotation); + +/** + * @brief Stop the driver. + */ +int stop(); + +/** + * @brief Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + */ +bool test(RM3100_BUS bus_id); + +/** + * @brief Prints info about the driver argument usage. + */ +void usage(); + +} // namespace RM3100 diff --git a/src/drivers/magnetometer/rm3100/rm3100_spi.cpp b/src/drivers/magnetometer/rm3100/rm3100_spi.cpp new file mode 100644 index 0000000000..26d9ee219c --- /dev/null +++ b/src/drivers/magnetometer/rm3100/rm3100_spi.cpp @@ -0,0 +1,176 @@ +/**************************************************************************** + * + * Copyright (c) 2018 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 rm3100_spi.cpp + * + * SPI interface for RM3100 + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include "board_config.h" +#include "rm3100.h" + +#if defined(PX4_SPIDEV_RM) || defined (PX4_SPIDEV_RM_EXT) + +/* SPI protocol address bits */ +#define DIR_READ (1<<7) +#define DIR_WRITE (0<<7) + +class RM3100_SPI : public device::SPI +{ +public: + RM3100_SPI(int bus, uint32_t device); + virtual ~RM3100_SPI() = default; + + virtual int init(); + virtual int ioctl(unsigned operation, unsigned &arg); + virtual int read(unsigned address, void *data, unsigned count); + virtual int write(unsigned address, void *data, unsigned count); +}; + +device::Device * +RM3100_SPI_interface(int bus); + +device::Device * +RM3100_SPI_interface(int bus) +{ +#ifdef PX4_SPIDEV_RM_EXT + return new RM3100_SPI(bus, PX4_SPIDEV_RM_EXT); +#else + return new RM3100_SPI(bus, PX4_SPIDEV_RM); +#endif +} + +RM3100_SPI::RM3100_SPI(int bus, uint32_t device) : + SPI("RM3100_SPI", nullptr, bus, device, SPIDEV_MODE3, 1 * 1000 * 1000 /* */) +{ + _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_RM3100; +} + +int +RM3100_SPI::init() +{ + int ret; + + ret = SPI::init(); + + if (ret != OK) { + DEVICE_DEBUG("SPI init failed"); + return -EIO; + } + + // Read REV_ID value + uint8_t data = 0; + + if (read(ADDR_REVID, &data, 1)) { + DEVICE_DEBUG("RM3100 read_reg fail"); + } + + if (data != RM3100_REVID) { + DEVICE_DEBUG("RM3100 ID: %02x", data); + return -EIO; + } + + return OK; +} + +int +RM3100_SPI::ioctl(unsigned operation, unsigned &arg) +{ + int ret; + + switch (operation) { + + case MAGIOCGEXTERNAL: + return external(); + + case DEVIOCGDEVICEID: + return CDev::ioctl(nullptr, operation, arg); + + default: { + ret = -EINVAL; + } + } + + return ret; +} + +int +RM3100_SPI::read(unsigned address, void *data, unsigned count) +{ + uint8_t buf[32]; + + if (sizeof(buf) < (count + 1)) { + return -EIO; + } + + buf[0] = address | DIR_READ; + + int ret = transfer(&buf[0], &buf[0], count + 1); + memcpy(data, &buf[1], count); + return ret; +} + +int +RM3100_SPI::write(unsigned address, void *data, unsigned count) +{ + uint8_t buf[32]; + + if (sizeof(buf) < (count + 1)) { + return -EIO; + } + + buf[0] = address | DIR_WRITE; + memcpy(&buf[1], data, count); + + return transfer(&buf[0], &buf[0], count + 1); +} + +#endif /* PX4_SPIDEV_RM || PX4_SPIDEV_RM_EXT */