From 77ab9b617e5e4374f4dfe67c69a5e312620ecb40 Mon Sep 17 00:00:00 2001 From: dlwalter Date: Fri, 8 Mar 2019 19:04:43 -0800 Subject: [PATCH] QMC5883 Magnetometer Driver (#11140) --- src/drivers/drv_sensor.h | 1 + src/drivers/magnetometer/CMakeLists.txt | 1 + .../magnetometer/qmc5883/CMakeLists.txt | 44 + src/drivers/magnetometer/qmc5883/qmc5883.cpp | 1180 +++++++++++++++++ src/drivers/magnetometer/qmc5883/qmc5883.h | 52 + .../magnetometer/qmc5883/qmc5883_i2c.cpp | 180 +++ .../magnetometer/qmc5883/qmc5883_spi.cpp | 185 +++ 7 files changed, 1643 insertions(+) create mode 100644 src/drivers/magnetometer/qmc5883/CMakeLists.txt create mode 100644 src/drivers/magnetometer/qmc5883/qmc5883.cpp create mode 100644 src/drivers/magnetometer/qmc5883/qmc5883.h create mode 100644 src/drivers/magnetometer/qmc5883/qmc5883_i2c.cpp create mode 100644 src/drivers/magnetometer/qmc5883/qmc5883_spi.cpp diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 759f8044d4..a2e9dcb3d7 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -59,6 +59,7 @@ #define DRV_MAG_DEVTYPE_LIS3MDL 0x05 #define DRV_MAG_DEVTYPE_IST8310 0x06 #define DRV_MAG_DEVTYPE_RM3100 0x07 +#define DRV_MAG_DEVTYPE_QMC5883 0x08 #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 541c9e0f14..ccbbe267b6 100644 --- a/src/drivers/magnetometer/CMakeLists.txt +++ b/src/drivers/magnetometer/CMakeLists.txt @@ -33,6 +33,7 @@ add_subdirectory(bmm150) add_subdirectory(hmc5883) +add_subdirectory(qmc5883) add_subdirectory(ist8310) add_subdirectory(lis3mdl) add_subdirectory(lsm303agr) diff --git a/src/drivers/magnetometer/qmc5883/CMakeLists.txt b/src/drivers/magnetometer/qmc5883/CMakeLists.txt new file mode 100644 index 0000000000..8b9a78a614 --- /dev/null +++ b/src/drivers/magnetometer/qmc5883/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2019 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__qmc5883 + MAIN qmc5883 + STACK_MAIN 1500 + COMPILE_FLAGS + SRCS + qmc5883_i2c.cpp + qmc5883_spi.cpp + qmc5883.cpp + DEPENDS + ) + diff --git a/src/drivers/magnetometer/qmc5883/qmc5883.cpp b/src/drivers/magnetometer/qmc5883/qmc5883.cpp new file mode 100644 index 0000000000..8f230de33d --- /dev/null +++ b/src/drivers/magnetometer/qmc5883/qmc5883.cpp @@ -0,0 +1,1180 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2019 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 qmc5883.cpp + * + * Driver for the QMC5883 magnetometer connected via I2C or SPI. + */ + +#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 + +#include "qmc5883.h" + +/* + * QMC5883 internal constants and data structures. + */ + +/* Max measurement rate is 200Hz */ +#define QMC5883_CONVERSION_INTERVAL (1000000 / 150) /* microseconds */ +#define QMC5883_MAX_COUNT 32767 + +#define QMC5883_ADDR_DATA_OUT_X_LSB 0x00 +#define QMC5883_ADDR_DATA_OUT_X_MSB 0x01 +#define QMC5883_ADDR_DATA_OUT_Y_LSB 0x02 +#define QMC5883_ADDR_DATA_OUT_Y_MSB 0x03 +#define QMC5883_ADDR_DATA_OUT_Z_LSB 0x04 +#define QMC5883_ADDR_DATA_OUT_Z_MSB 0x05 +#define QMC5883_ADDR_STATUS 0x06 +#define QMC5883_ADDR_TEMP_OUT_LSB 0x07 +#define QMC5883_ADDR_TEMP_OUT_MSB 0x08 +#define QMC5883_ADDR_CONTROL_1 0x09 +#define QMC5883_ADDR_CONTROL_2 0x0A +#define QMC5883_ADDR_SET_RESET 0x0B + + +#define QMC5883_STATUS_REG_DRDY (1 << 0) /* Data Ready: "0": no new data, "1": new data is ready */ +#define QMC5883_STATUS_REG_OVL (1 << 1) /* Overflow Flag: "0": normal, "1": data overflow */ +#define QMC5883_STATUS_REG_DOR (1 << 2) /* Data Skip: "0": normal, "1": data skipped for reading */ + +/* Control Register 1 */ +#define QMC5883_MODE_REG_STANDBY (0 << 0) +#define QMC5883_MODE_REG_CONTINOUS_MODE (1 << 0) +#define QMC5883_OUTPUT_DATA_RATE_10 (0 << 2) /* Hz */ +#define QMC5883_OUTPUT_DATA_RATE_50 (1 << 2) +#define QMC5883_OUTPUT_DATA_RATE_100 (2 << 2) +#define QMC5883_OUTPUT_DATA_RATE_200 (3 << 2) +#define QMC5883_OUTPUT_RANGE_2G (0 << 4) /* +/- 2 gauss */ +#define QMC5883_OUTPUT_RANGE_8G (1 << 4) /* +/- 8 gauss */ +#define QMC5883_OVERSAMPLE_512 (0 << 6) /* controls digital filter bw - larger OSR -> smaller bw */ +#define QMC5883_OVERSAMPLE_256 (1 << 6) +#define QMC5883_OVERSAMPLE_128 (2 << 6) +#define QMC5883_OVERSAMPLE_64 (3 << 6) + +/* Control Register 2 */ +#define QMC5883_INT_ENB (1 << 0) +#define QMC5883_ROL_PNT (1 << 6) +#define QMC5883_SOFT_RESET (1 << 7) + +/* Set Register */ +#define QMC5883_SET_DEFAULT (1 << 0) + +#define QMC5883_TEMP_OFFSET 50 /* deg celsius */ + +enum QMC5883_BUS { + QMC5883_BUS_ALL = 0, + QMC5883_BUS_I2C_INTERNAL, + QMC5883_BUS_I2C_EXTERNAL, + QMC5883_BUS_SPI +}; + +#ifndef CONFIG_SCHED_WORKQUEUE +# error This requires CONFIG_SCHED_WORKQUEUE. +#endif + +class QMC5883 : public device::CDev +{ +public: + QMC5883(device::Device *interface, const char *path, enum Rotation rotation); + virtual ~QMC5883(); + + 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); + + /** + * Stop the automatic measurement state machine. + */ + void stop(); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + +protected: + Device *_interface; + +private: + work_s _work{}; + unsigned _measure_ticks; + + ringbuffer::RingBuffer *_reports; + struct mag_calibration_s _scale; + float _range_scale; + float _range_ga; + bool _collect_phase; + int _class_instance; + int _orb_class_instance; + + orb_advert_t _mag_topic; + + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; + perf_counter_t _range_errors; + perf_counter_t _conf_errors; + + /* status reporting */ + bool _sensor_ok; /**< sensor was found and reports ok */ + + enum Rotation _rotation; + + struct mag_report _last_report {}; /**< used for info() */ + + uint8_t _range_bits; + uint8_t _conf_reg; + uint8_t _temperature_counter; + uint8_t _temperature_error_count; + + /** + * Initialise the automatic measurement state machine and start it. + * + * @note This function is called at open and error time. It might make sense + * to make it more aggressive about resetting the bus in case of errors. + */ + void start(); + + /** + * Reset the device + */ + int reset(); + + /** + * check the sensor configuration. + * + * checks that the config of the sensor is correctly set, to + * cope with communication errors causing the configuration to + * change + */ + void check_conf(void); + + /** + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + * + * This is the heart of the measurement state machine. This function + * alternately starts a measurement, or collects the data from the + * previous measurement. + * + * When the interval between measurements is greater than the minimum + * measurement interval, a gap is inserted between collection + * and measurement to provide the most recent measurement possible + * at the next interval. + */ + void cycle(); + + /** + * Static trampoline from the workq context; because we don't have a + * generic workq wrapper yet. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void cycle_trampoline(void *arg); + + /** + * Write a register. + * + * @param reg The register to write. + * @param val The value to write. + * @return OK on write success. + */ + int write_reg(uint8_t reg, uint8_t val); + + /** + * Read a register. + * + * @param reg The register to read. + * @param val The value read. + * @return OK on read success. + */ + int read_reg(uint8_t reg, uint8_t &val); + + /** + * Collect the result of the most recent measurement. + */ + int collect(); + + /* this class has pointer data members, do not allow copying it */ + QMC5883(const QMC5883 &); + QMC5883 operator=(const QMC5883 &); +}; + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int qmc5883_main(int argc, char *argv[]); + + +QMC5883::QMC5883(device::Device *interface, const char *path, enum Rotation rotation) : + CDev("QMC5883", path), + _interface(interface), + _measure_ticks(0), + _reports(nullptr), + _scale{}, + _range_scale(1.0f / 12000.0f), + _range_ga(2.0f), + _collect_phase(false), + _class_instance(-1), + _orb_class_instance(-1), + _mag_topic(nullptr), + _sample_perf(perf_alloc(PC_ELAPSED, "qmc5883_read")), + _comms_errors(perf_alloc(PC_COUNT, "qmc5883_com_err")), + _range_errors(perf_alloc(PC_COUNT, "qmc5883_rng_err")), + _conf_errors(perf_alloc(PC_COUNT, "qmc5883_conf_err")), + _sensor_ok(false), + _rotation(rotation), + _range_bits(0), + _conf_reg(0), + _temperature_counter(0), + _temperature_error_count(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_QMC5883; + + // 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)); +} + +QMC5883::~QMC5883() +{ + /* 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 +QMC5883::init() +{ + int ret = PX4_ERROR; + + ret = CDev::init(); + + if (ret != OK) { + DEVICE_DEBUG("CDev init failed"); + goto out; + } + + /* allocate basic report buffers */ + _reports = new ringbuffer::RingBuffer(2, sizeof(mag_report)); + + if (_reports == nullptr) { + goto out; + } + + /* reset the device configuration */ + reset(); + + _class_instance = register_class_devname(MAG_BASE_DEVICE_PATH); + + ret = OK; + /* sensor is ok */ + _sensor_ok = true; +out: + return ret; +} + +/** + check that the configuration register has the right value. This is + done periodically to cope with I2C bus noise causing the + configuration of the compass to change. + */ +void QMC5883::check_conf(void) +{ + int ret; + + uint8_t conf_reg_in = 0; + ret = read_reg(QMC5883_ADDR_CONTROL_1, conf_reg_in); + + if (OK != ret) { + perf_count(_comms_errors); + return; + } + + if (conf_reg_in != _conf_reg) { + perf_count(_conf_errors); + ret = write_reg(QMC5883_ADDR_CONTROL_1, _conf_reg); + + if (OK != ret) { + perf_count(_comms_errors); + } + } +} + +ssize_t +QMC5883::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / 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(); + + /* wait for it to complete */ + usleep(QMC5883_CONVERSION_INTERVAL); + + /* run the collection phase */ + if (OK != collect()) { + ret = -EIO; + break; + } + + if (_reports->get(mag_buf)) { + ret = sizeof(struct mag_report); + } + } while (0); + + return ret; +} + +int +QMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + unsigned dummy = arg; + + switch (cmd) { + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default polling rate */ + case SENSOR_POLLRATE_DEFAULT: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* set interval for next measurement to minimum legal value */ + _measure_ticks = USEC2TICK(QMC5883_CONVERSION_INTERVAL); + + /* if we need to start the poll state machine, do it */ + if (want_start) { + start(); + } + + return OK; + } + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* convert hz to tick interval via microseconds */ + unsigned ticks = USEC2TICK(1000000 / arg); + + /* check against maximum rate */ + if (ticks < USEC2TICK(QMC5883_CONVERSION_INTERVAL)) { + return -EINVAL; + } + + /* update interval for next measurement */ + _measure_ticks = ticks; + + /* if we need to start the poll state machine, do it */ + if (want_start) { + start(); + } + + return OK; + } + } + } + + case SENSORIOCRESET: + return reset(); + + case MAGIOCSRANGE: + return OK; + + 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 MAGIOCGEXTERNAL: + DEVICE_DEBUG("MAGIOCGEXTERNAL in main driver"); + return _interface->ioctl(cmd, dummy); + + default: + /* give it to the superclass */ + return CDev::ioctl(filp, cmd, arg); + } +} + +void +QMC5883::start() +{ + /* reset the report ring and state machine */ + _collect_phase = false; + _reports->flush(); + + /* schedule a cycle to start things */ + work_queue(HPWORK, &_work, (worker_t)&QMC5883::cycle_trampoline, this, 1); +} + +void +QMC5883::stop() +{ + if (_measure_ticks > 0) { + /* ensure no new items are queued while we cancel this one */ + _measure_ticks = 0; + work_cancel(HPWORK, &_work); + } +} + +int +QMC5883::reset() +{ + /* read 0x00 once */ + uint8_t data_bits_in = 0; + read_reg(QMC5883_ADDR_DATA_OUT_X_LSB, data_bits_in); + + /* software reset */ + write_reg(QMC5883_ADDR_CONTROL_2, QMC5883_SOFT_RESET); + + + /* set reset period to 0x01 */ + write_reg(QMC5883_ADDR_SET_RESET, QMC5883_SET_DEFAULT); + + //use fixed range of 2G + _range_scale = 1.0f / 12000.0f; // 12000 LSB/Gauss at +/- 2G range + _range_ga = 2.00f; + _range_bits = 0x00; + + /* set control register */ + _conf_reg = QMC5883_MODE_REG_CONTINOUS_MODE | + QMC5883_OUTPUT_DATA_RATE_200 | + QMC5883_OVERSAMPLE_512 | + QMC5883_OUTPUT_RANGE_2G; + write_reg(QMC5883_ADDR_CONTROL_1, _conf_reg); + + return OK; +} + +void +QMC5883::cycle_trampoline(void *arg) +{ + QMC5883 *dev = (QMC5883 *)arg; + + dev->cycle(); +} + +void +QMC5883::cycle() +{ + if (_measure_ticks == 0) { + return; + } + + /* collection phase? */ + if (_collect_phase) { + + /* perform collection */ + if (OK != collect()) { + DEVICE_DEBUG("collection error"); + /* restart the measurement state machine */ + start(); + return; + } + + /* next phase is measurement */ + _collect_phase = false; + + /* + * Is there a collect->measure gap? + */ + if (_measure_ticks > USEC2TICK(QMC5883_CONVERSION_INTERVAL)) { + + /* schedule a fresh cycle call when we are ready to measure again */ + work_queue(HPWORK, + &_work, + (worker_t)&QMC5883::cycle_trampoline, + this, + _measure_ticks - USEC2TICK(QMC5883_CONVERSION_INTERVAL)); + + return; + } + } + + /* next phase is collection */ + _collect_phase = true; + + if (_measure_ticks > 0) { + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, + &_work, + (worker_t)&QMC5883::cycle_trampoline, + this, + USEC2TICK(QMC5883_CONVERSION_INTERVAL)); + } +} + + +int +QMC5883::collect() +{ +#pragma pack(push, 1) + struct { /* status register and data as read back from the device */ + uint8_t x[2]; + uint8_t y[2]; + uint8_t z[2]; + } qmc_report; +#pragma pack(pop) + struct { + int16_t x, y, z; + } report; + + int ret; + uint8_t check_counter; + + perf_begin(_sample_perf); + struct mag_report new_report; + bool sensor_is_onboard = false; + + float xraw_f; + float yraw_f; + float zraw_f; + + /* this should be fairly close to the end of the measurement, so the best approximation of the time */ + new_report.timestamp = hrt_absolute_time(); + new_report.error_count = perf_event_count(_comms_errors); + new_report.scaling = _range_scale; + new_report.device_id = _device_id.devid; + + /* + * @note We could read the status register here, which could tell us that + * we were too early and that the output registers are still being + * written. In the common case that would just slow us down, and + * we're better off just never being early. + */ + + /* get measurements from the device */ + ret = _interface->read(QMC5883_ADDR_DATA_OUT_X_LSB, (uint8_t *)&qmc_report, sizeof(qmc_report)); + + if (ret != OK) { + perf_count(_comms_errors); + DEVICE_DEBUG("data/status read error"); + goto out; + } + + /* map data we just received LSB, MSB */ + report.x = (((int16_t)qmc_report.x[1]) << 8) + qmc_report.x[0]; + report.y = (((int16_t)qmc_report.y[1]) << 8) + qmc_report.y[0]; + report.z = (((int16_t)qmc_report.z[1]) << 8) + qmc_report.z[0]; + + /* + * If any of the values are -4096, there was an internal math error in the sensor. + * Generalise this to a simple range check that will also catch some bit errors. + */ + if ((abs(report.x) > QMC5883_MAX_COUNT) || + (abs(report.y) > QMC5883_MAX_COUNT) || + (abs(report.z) > QMC5883_MAX_COUNT)) { + perf_count(_comms_errors); + goto out; + } + + /* get temperature measurements from the device */ + new_report.temperature = 0; + + if (_temperature_counter++ == 100) { + uint8_t raw_temperature[2]; + + _temperature_counter = 0; + + ret = _interface->read(QMC5883_ADDR_TEMP_OUT_LSB, + raw_temperature, sizeof(raw_temperature)); + + if (ret == OK) { + int16_t temp16 = (((int16_t)raw_temperature[1]) << 8) + + raw_temperature[0]; + new_report.temperature = QMC5883_TEMP_OFFSET + temp16 * 1.0f / 100.0f; + } + + } else { + new_report.temperature = _last_report.temperature; + } + + + /* + * RAW outputs + * + * to align the sensor axes with the board, x and y need to be flipped + * and y needs to be negated + */ + new_report.x_raw = -report.y; + new_report.y_raw = report.x; + /* z remains z */ + new_report.z_raw = report.z; + + /* scale values for output */ + + // XXX revisit for SPI part, might require a bus type IOCTL + unsigned dummy; + sensor_is_onboard = !_interface->ioctl(MAGIOCGEXTERNAL, dummy); + new_report.is_external = !sensor_is_onboard; + + if (sensor_is_onboard) { + // convert onboard so it matches offboard for the + // scaling below + report.y = -report.y; + report.x = -report.x; + } + + /* the standard external mag by 3DR has x pointing to the + * right, y pointing backwards, and z down, therefore switch x + * and y and invert y */ + //TODO: sort out axes mapping + xraw_f = -report.y; + yraw_f = report.x; + zraw_f = report.z; + + // apply user specified rotation + rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); + + new_report.x = ((xraw_f * _range_scale) - _scale.x_offset) * _scale.x_scale; + /* flip axes and negate value for y */ + new_report.y = ((yraw_f * _range_scale) - _scale.y_offset) * _scale.y_scale; + /* z remains z */ + new_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_report); + + } else { + _mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &new_report, + &_orb_class_instance, (sensor_is_onboard) ? ORB_PRIO_HIGH : ORB_PRIO_MAX); + + if (_mag_topic == nullptr) { + DEVICE_DEBUG("ADVERT FAIL"); + } + } + } + + _last_report = new_report; + + /* post a report to the ring */ + _reports->force(&new_report); + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + + /* + periodically check the configuration + registers. With a bad I2C cable it is possible for the + registers to become corrupt, leading to bad readings. It + doesn't happen often, but given the poor cables some + vehicles have it is worth checking for. + */ + check_counter = perf_event_count(_sample_perf) % 256; + + + if (check_counter == 0) { + check_conf(); + } + + ret = OK; + +out: + perf_end(_sample_perf); + return ret; +} + +int +QMC5883::write_reg(uint8_t reg, uint8_t val) +{ + uint8_t buf = val; + return _interface->write(reg, &buf, 1); +} + +int +QMC5883::read_reg(uint8_t reg, uint8_t &val) +{ + uint8_t buf = val; + int ret = _interface->read(reg, &buf, 1); + val = buf; + return ret; +} + +void +QMC5883::print_info() +{ + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + printf("poll interval: %u ticks\n", _measure_ticks); + print_message(_last_report); + _reports->print_info("report queue"); +} + +/** + * Local functions in support of the shell command. + */ +namespace qmc5883 +{ + +/* + list of supported bus configurations + */ +struct qmc5883_bus_option { + enum QMC5883_BUS busid; + const char *devpath; + QMC5883_constructor interface_constructor; + uint8_t busnum; + QMC5883 *dev; +} bus_options[] = { + { QMC5883_BUS_I2C_EXTERNAL, "/dev/qmc5883_ext", &QMC5883_I2C_interface, PX4_I2C_BUS_EXPANSION, NULL }, +#ifdef PX4_I2C_BUS_EXPANSION1 + { QMC5883_BUS_I2C_EXTERNAL, "/dev/qmc5883_ext1", &QMC5883_I2C_interface, PX4_I2C_BUS_EXPANSION1, NULL }, +#endif +#ifdef PX4_I2C_BUS_EXPANSION2 + { QMC5883_BUS_I2C_EXTERNAL, "/dev/qmc5883_ext2", &QMC5883_I2C_interface, PX4_I2C_BUS_EXPANSION2, NULL }, +#endif +#ifdef PX4_I2C_BUS_ONBOARD + { QMC5883_BUS_I2C_INTERNAL, "/dev/qmc5883_int", &QMC5883_I2C_interface, PX4_I2C_BUS_ONBOARD, NULL }, +#endif +#ifdef PX4_SPIDEV_HMC + { QMC5883_BUS_SPI, "/dev/qmc5883_spi", &QMC5883_SPI_interface, PX4_SPI_BUS_SENSORS, NULL }, +#endif +}; +#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0])) + +void start(enum QMC5883_BUS busid, enum Rotation rotation); +int stop(); +bool start_bus(struct qmc5883_bus_option &bus, enum Rotation rotation); +struct qmc5883_bus_option &find_bus(enum QMC5883_BUS busid); +void test(enum QMC5883_BUS busid); +void reset(enum QMC5883_BUS busid); +int info(enum QMC5883_BUS busid); +int temp_enable(QMC5883_BUS busid, bool enable); +void usage(); + +/** + * start driver for a specific bus option + */ +bool +start_bus(struct qmc5883_bus_option &bus, enum Rotation rotation) +{ + if (bus.dev != nullptr) { + errx(1, "bus option already started"); + } + + device::Device *interface = bus.interface_constructor(bus.busnum); + + if (interface->init() != OK) { + delete interface; + warnx("no device on bus %u (type: %u)", (unsigned)bus.busnum, (unsigned)bus.busid); + return false; + } + + bus.dev = new QMC5883(interface, bus.devpath, rotation); + + if (bus.dev != nullptr && OK != bus.dev->init()) { + delete bus.dev; + bus.dev = NULL; + return false; + } + + int fd = open(bus.devpath, O_RDONLY); + + if (fd < 0) { + return false; + } + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + close(fd); + errx(1, "Failed to setup poll rate"); + } + + close(fd); + + return true; +} + +/** + * Start the driver. + * + * This function call only returns once the driver + * is either successfully up and running or failed to start. + */ +void +start(enum QMC5883_BUS busid, enum Rotation rotation) +{ + bool started = false; + + for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) { + if (busid == QMC5883_BUS_ALL && bus_options[i].dev != NULL) { + // this device is already started + continue; + } + + if (busid != QMC5883_BUS_ALL && bus_options[i].busid != busid) { + // not the one that is asked for + continue; + } + + started |= start_bus(bus_options[i], rotation); + } + + if (!started) { + exit(1); + } +} + +int +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; +} + +/** + * find a bus structure for a busid + */ +struct qmc5883_bus_option &find_bus(enum QMC5883_BUS busid) +{ + for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) { + if ((busid == QMC5883_BUS_ALL || + busid == bus_options[i].busid) && bus_options[i].dev != NULL) { + return bus_options[i]; + } + } + + errx(1, "bus %u not started", (unsigned)busid); +} + + +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + */ +void +test(enum QMC5883_BUS busid) +{ + struct qmc5883_bus_option &bus = find_bus(busid); + struct mag_report report; + ssize_t sz; + int ret; + const char *path = bus.devpath; + + int fd = open(path, O_RDONLY); + + if (fd < 0) { + err(1, "%s open failed (try 'qmc5883 start')", path); + } + + /* do a simple demand read */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + err(1, "immediate read failed"); + } + + print_message(report); + + /* check if mag is onboard or external */ + if ((ret = ioctl(fd, MAGIOCGEXTERNAL, 0)) < 0) { + errx(1, "failed to get if mag is onboard or external"); + } + + warnx("device active: %s", ret ? "external" : "onboard"); + + /* read the sensor 5x and report each value */ + for (unsigned i = 0; i < 5; i++) { + struct pollfd fds; + + /* wait for data to be ready */ + fds.fd = fd; + fds.events = POLLIN; + ret = poll(&fds, 1, 2000); + + if (ret != 1) { + errx(1, "timed out waiting for sensor data"); + } + + /* now go get it */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + err(1, "periodic read failed"); + } + + print_message(report); + } + + PX4_INFO("PASS"); + exit(0); +} + +/** + * Reset the driver. + */ +void +reset(enum QMC5883_BUS busid) +{ + struct qmc5883_bus_option &bus = find_bus(busid); + const char *path = bus.devpath; + + 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); +} + + +/** + * Print a little info about the driver. + */ +int +info(enum QMC5883_BUS busid) +{ + struct qmc5883_bus_option &bus = find_bus(busid); + + warnx("running on bus: %u (%s)\n", (unsigned)bus.busid, bus.devpath); + bus.dev->print_info(); + exit(0); +} + +void +usage() +{ + warnx("missing command: try 'start', 'info', 'test', 'reset', 'info'"); + warnx("options:"); + warnx(" -R rotation"); + warnx(" -X only external bus"); +#if (PX4_I2C_BUS_ONBOARD || PX4_SPIDEV_HMC) + warnx(" -I only internal bus"); +#endif +} + +} // namespace + +int +qmc5883_main(int argc, char *argv[]) +{ + int ch; + enum QMC5883_BUS busid = QMC5883_BUS_ALL; + enum Rotation rotation = ROTATION_NONE; + + if (argc < 2) { + qmc5883::usage(); + exit(0); + } + + while ((ch = getopt(argc, argv, "XISR:CT")) != EOF) { + switch (ch) { + case 'R': + rotation = (enum Rotation)atoi(optarg); + break; +#if (PX4_I2C_BUS_ONBOARD || PX4_SPIDEV_HMC) + + case 'I': + busid = QMC5883_BUS_I2C_INTERNAL; + break; +#endif + + case 'X': + busid = QMC5883_BUS_I2C_EXTERNAL; + break; + + case 'S': + busid = QMC5883_BUS_SPI; + break; + + default: + qmc5883::usage(); + exit(0); + } + } + + const char *verb = argv[optind]; + + /* + * Start/load the driver. + */ + if (!strcmp(verb, "start")) { + qmc5883::start(busid, rotation); + + exit(0); + } + + /* + * Stop the driver + */ + if (!strcmp(verb, "stop")) { + return qmc5883::stop(); + } + + /* + * Test the driver/device. + */ + if (!strcmp(verb, "test")) { + qmc5883::test(busid); + } + + /* + * Reset the driver. + */ + if (!strcmp(verb, "reset")) { + qmc5883::reset(busid); + } + + + /* + * Print driver information. + */ + if (!strcmp(verb, "info") || !strcmp(verb, "status")) { + qmc5883::info(busid); + } + + errx(1, "unrecognized command, try 'start', 'test', 'reset', or 'info'"); +} diff --git a/src/drivers/magnetometer/qmc5883/qmc5883.h b/src/drivers/magnetometer/qmc5883/qmc5883.h new file mode 100644 index 0000000000..400d128b12 --- /dev/null +++ b/src/drivers/magnetometer/qmc5883/qmc5883.h @@ -0,0 +1,52 @@ +/**************************************************************************** + * + * Copyright (c) 2019 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 qmc5883.h + * + * Shared defines for the qmc5883 driver. + */ + +#pragma once + +#define ADDR_ID_A 0x0C +#define ADDR_ID_B 0x0D + +#define ID_A_WHO_AM_I 0x01 +#define ID_B_WHO_AM_I 0xFF + + +/* interface factories */ +extern device::Device *QMC5883_SPI_interface(int bus); +extern device::Device *QMC5883_I2C_interface(int bus); +typedef device::Device *(*QMC5883_constructor)(int); diff --git a/src/drivers/magnetometer/qmc5883/qmc5883_i2c.cpp b/src/drivers/magnetometer/qmc5883/qmc5883_i2c.cpp new file mode 100644 index 0000000000..a8378ef013 --- /dev/null +++ b/src/drivers/magnetometer/qmc5883/qmc5883_i2c.cpp @@ -0,0 +1,180 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2019 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 QMC5883_I2C.cpp + * + * I2C interface for QMC5883 + */ + +/* XXX trim includes */ +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include "qmc5883.h" + +#include "board_config.h" + +#define QMC5883L_ADDRESS 0x0D + +device::Device *QMC5883_I2C_interface(int bus); + +class QMC5883_I2C : public device::I2C +{ +public: + QMC5883_I2C(int bus); + virtual ~QMC5883_I2C() = default; + + virtual int read(unsigned address, void *data, unsigned count); + virtual int write(unsigned address, void *data, unsigned count); + + virtual int ioctl(unsigned operation, unsigned &arg); + +protected: + virtual int probe(); + +}; + +device::Device * +QMC5883_I2C_interface(int bus) +{ + return new QMC5883_I2C(bus); +} + +QMC5883_I2C::QMC5883_I2C(int bus) : + I2C("QMC5883_I2C", nullptr, bus, QMC5883L_ADDRESS, 400000) +{ + _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_QMC5883; +} + +int +QMC5883_I2C::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 +QMC5883_I2C::probe() +{ + uint8_t data[2] = {0, 0}; + + // must read registers 0x00 once or reset to read ID registers reliably + read(0x00, &data[0], 1); + read(0x00, &data[0], 1); + read(0x00, &data[0], 1); + + _retries = 10; + + bool read_valid = false; + bool id_valid = false; + + for (unsigned i = 0; i < _retries; i++) { + //attempt read + if (!read(ADDR_ID_A, &data[0], 1) && + !read(ADDR_ID_B, &data[1], 1)) { + read_valid = true; + } + + if (read_valid && data[0] == ID_A_WHO_AM_I && + data[1] == ID_B_WHO_AM_I) { + id_valid = true; + } + + if (read_valid && id_valid) { + return OK; + } + + // wait 100 usec + usleep(100); + } + + if (!read_valid) { + DEVICE_DEBUG("read_reg fail"); + } + + if (!id_valid) { + DEVICE_DEBUG("ID byte mismatch (%02x,%02x)", data[0], data[1]); + } + + return -EIO; +} + +int +QMC5883_I2C::write(unsigned address, void *data, unsigned count) +{ + uint8_t buf[32]; + + if (sizeof(buf) < (count + 1)) { + return -EIO; + } + + buf[0] = address; + memcpy(&buf[1], data, count); + + return transfer(&buf[0], count + 1, nullptr, 0); +} + +int +QMC5883_I2C::read(unsigned address, void *data, unsigned count) +{ + uint8_t cmd = address; + return transfer(&cmd, 1, (uint8_t *)data, count); +} diff --git a/src/drivers/magnetometer/qmc5883/qmc5883_spi.cpp b/src/drivers/magnetometer/qmc5883/qmc5883_spi.cpp new file mode 100644 index 0000000000..103a1a97ac --- /dev/null +++ b/src/drivers/magnetometer/qmc5883/qmc5883_spi.cpp @@ -0,0 +1,185 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2019 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 QMC5883_SPI.cpp + * + * SPI interface for HMC5983 + */ + +/* XXX trim includes */ +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include "qmc5883.h" +#include + +#ifdef PX4_SPIDEV_HMC + +/* SPI protocol address bits */ +#define DIR_READ (1<<7) +#define DIR_WRITE (0<<7) +#define ADDR_INCREMENT (1<<6) + +#define HMC_MAX_SEND_LEN 4 +#define HMC_MAX_RCV_LEN 8 + +device::Device *QMC5883_SPI_interface(int bus); + +class QMC5883_SPI : public device::SPI +{ +public: + QMC5883_SPI(int bus, uint32_t device); + virtual ~QMC5883_SPI() = default; + + virtual int init(); + virtual int read(unsigned address, void *data, unsigned count); + virtual int write(unsigned address, void *data, unsigned count); + + virtual int ioctl(unsigned operation, unsigned &arg); + +}; + +device::Device * +QMC5883_SPI_interface(int bus) +{ + return new QMC5883_SPI(bus, PX4_SPIDEV_HMC); +} + +QMC5883_SPI::QMC5883_SPI(int bus, uint32_t device) : + SPI("QMC5883_SPI", nullptr, bus, device, SPIDEV_MODE3, 11 * 1000 * 1000 /* will be rounded to 10.4 MHz */) +{ + _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_QMC5883; +} + +int +QMC5883_SPI::init() +{ + int ret; + + ret = SPI::init(); + + if (ret != OK) { + DEVICE_DEBUG("SPI init failed"); + return -EIO; + } + + // read WHO_AM_I value + uint8_t data[2] = {0, 0}; + + if (read(ADDR_ID_A, &data[0], 1) || + read(ADDR_ID_B, &data[1], 1)) { + DEVICE_DEBUG("read_reg fail"); + } + + if ((data[0] != ID_A_WHO_AM_I) || + (data[1] != ID_B_WHO_AM_I)) { + DEVICE_DEBUG("ID byte mismatch (%02x,%02x)", data[0], data[1]); + return -EIO; + } + + return OK; +} + +int +QMC5883_SPI::ioctl(unsigned operation, unsigned &arg) +{ + int ret; + + switch (operation) { + + case MAGIOCGEXTERNAL: + /* + * Even if this sensor is on the external SPI + * bus it is still internal to the autopilot + * assembly, so always return 0 for internal. + */ + return 0; + + case DEVIOCGDEVICEID: + return CDev::ioctl(nullptr, operation, arg); + + default: { + ret = -EINVAL; + } + } + + return ret; +} + +int +QMC5883_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); +} + +int +QMC5883_SPI::read(unsigned address, void *data, unsigned count) +{ + uint8_t buf[32]; + + if (sizeof(buf) < (count + 1)) { + return -EIO; + } + + buf[0] = address | DIR_READ | ADDR_INCREMENT; + + int ret = transfer(&buf[0], &buf[0], count + 1); + memcpy(data, &buf[1], count); + return ret; +} + +#endif /* PX4_SPIDEV_HMC */