From 24125b3da48908e8ff5cb59cb57c21a7e12749b6 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 2 Sep 2020 13:16:27 -0400 Subject: [PATCH] magnetometer/qmc5883l: cleanup/rewrite driver (#14384) --- ROMFS/px4fmu_common/init.d/rc.sensors | 2 +- .../airmind/mindpx-v2/init/rc.board_sensors | 2 +- src/drivers/drv_sensor.h | 2 +- src/drivers/magnetometer/CMakeLists.txt | 2 +- src/drivers/magnetometer/qmc5883/QMC5883.cpp | 311 ------------------ src/drivers/magnetometer/qmc5883/QMC5883.hpp | 181 ---------- .../magnetometer/qmc5883/QMC5883_I2C.cpp | 135 -------- src/drivers/magnetometer/qmc5883/qmc5883.h | 52 --- .../{qmc5883 => qmc5883l}/CMakeLists.txt | 15 +- .../magnetometer/qmc5883l/QMC5883L.cpp | 306 +++++++++++++++++ .../magnetometer/qmc5883l/QMC5883L.hpp | 115 +++++++ .../qmc5883l/QST_QMC5883L_registers.hpp | 113 +++++++ .../qmc5883l_main.cpp} | 57 +--- 13 files changed, 561 insertions(+), 732 deletions(-) delete mode 100644 src/drivers/magnetometer/qmc5883/QMC5883.cpp delete mode 100644 src/drivers/magnetometer/qmc5883/QMC5883.hpp delete mode 100644 src/drivers/magnetometer/qmc5883/QMC5883_I2C.cpp delete mode 100644 src/drivers/magnetometer/qmc5883/qmc5883.h rename src/drivers/magnetometer/{qmc5883 => qmc5883l}/CMakeLists.txt (89%) create mode 100644 src/drivers/magnetometer/qmc5883l/QMC5883L.cpp create mode 100644 src/drivers/magnetometer/qmc5883l/QMC5883L.hpp create mode 100644 src/drivers/magnetometer/qmc5883l/QST_QMC5883L_registers.hpp rename src/drivers/magnetometer/{qmc5883/qmc5883_main.cpp => qmc5883l/qmc5883l_main.cpp} (70%) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 77a0e1b990..84ecaa7795 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -105,7 +105,7 @@ then ist8310 -X -q start lis2mdl -X -q start lis3mdl -X -q start - qmc5883 -X -q start + qmc5883l -X -q start rm3100 -X -q start # differential pressure sensors diff --git a/boards/airmind/mindpx-v2/init/rc.board_sensors b/boards/airmind/mindpx-v2/init/rc.board_sensors index eef88143cc..c0aec4c19d 100644 --- a/boards/airmind/mindpx-v2/init/rc.board_sensors +++ b/boards/airmind/mindpx-v2/init/rc.board_sensors @@ -7,7 +7,7 @@ adc start # Internal I2C bus hmc5883 -T -I -R 12 start -qmc5883 -I -R 12 start +qmc5883l -I -R 2 start mpu6000 -s -R 2 start mpu6500 -s -R 2 start diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index eaaf6892fa..ffec8257af 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -59,7 +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_MAG_DEVTYPE_QMC5883L 0x08 #define DRV_MAG_DEVTYPE_AK09916 0x09 #define DRV_MAG_DEVTYPE_IST8308 0x0B diff --git a/src/drivers/magnetometer/CMakeLists.txt b/src/drivers/magnetometer/CMakeLists.txt index adffb80b36..8fd6620994 100644 --- a/src/drivers/magnetometer/CMakeLists.txt +++ b/src/drivers/magnetometer/CMakeLists.txt @@ -34,7 +34,7 @@ add_subdirectory(akm) add_subdirectory(bmm150) add_subdirectory(hmc5883) -add_subdirectory(qmc5883) +add_subdirectory(qmc5883l) add_subdirectory(isentek) add_subdirectory(lis2mdl) add_subdirectory(lis3mdl) diff --git a/src/drivers/magnetometer/qmc5883/QMC5883.cpp b/src/drivers/magnetometer/qmc5883/QMC5883.cpp deleted file mode 100644 index 9c0bed8b4d..0000000000 --- a/src/drivers/magnetometer/qmc5883/QMC5883.cpp +++ /dev/null @@ -1,311 +0,0 @@ -/**************************************************************************** - * - * 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. - * - ****************************************************************************/ - -#include "QMC5883.hpp" - -QMC5883::QMC5883(device::Device *interface, enum Rotation rotation, I2CSPIBusOption bus_option, int bus, - int i2c_address) : - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id()), bus_option, bus, i2c_address), - _px4_mag(interface->get_device_id(), rotation), - _interface(interface), - _collect_phase(false), - _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), - _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": com_err")), - _range_errors(perf_alloc(PC_COUNT, MODULE_NAME": rng_err")), - _conf_errors(perf_alloc(PC_COUNT, MODULE_NAME": conf_err")), - _conf_reg(0), - _temperature_counter(0), - _temperature_error_count(0) -{ - _px4_mag.set_external(_interface->external()); -} - -QMC5883::~QMC5883() -{ - // free perf counters - perf_free(_sample_perf); - perf_free(_comms_errors); - perf_free(_range_errors); - perf_free(_conf_errors); - - delete _interface; -} - -int QMC5883::init() -{ - /* reset the device configuration */ - reset(); - - _measure_interval = QMC5883_CONVERSION_INTERVAL; - start(); - - return PX4_OK; -} - -/** - 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() -{ - uint8_t conf_reg_in = 0; - int 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); - } - } -} - -void QMC5883::start() -{ - /* reset the report ring and state machine */ - _collect_phase = false; - - /* schedule a cycle to start things */ - ScheduleNow(); -} - -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 - _px4_mag.set_scale(1.f / 12000.f); // 12000 LSB/Gauss at +/- 2G range - - /* 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::RunImpl() -{ - if (_measure_interval == 0) { - return; - } - - /* collection phase? */ - if (_collect_phase) { - - /* perform collection */ - if (OK != collect()) { - PX4_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_interval > QMC5883_CONVERSION_INTERVAL) { - /* schedule a fresh cycle call when we are ready to measure again */ - ScheduleDelayed(_measure_interval - QMC5883_CONVERSION_INTERVAL); - - return; - } - } - - /* next phase is collection */ - _collect_phase = true; - - if (_measure_interval > 0) { - /* schedule a fresh cycle call when the measurement is done */ - ScheduleDelayed(QMC5883_CONVERSION_INTERVAL); - } -} - -int QMC5883::collect() -{ - 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{}; - - struct { - int16_t x, y, z; - } report{}; - - int ret; - uint8_t check_counter; - - perf_begin(_sample_perf); - - float xraw_f; - float yraw_f; - float zraw_f; - - _px4_mag.set_error_count(perf_event_count(_comms_errors)); - - /* - * @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 */ - const hrt_abstime timestamp_sample = hrt_absolute_time(); - ret = _interface->read(QMC5883_ADDR_DATA_OUT_X_LSB, (uint8_t *)&qmc_report, sizeof(qmc_report)); - - if (ret != OK) { - perf_count(_comms_errors); - PX4_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 */ - 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]; - float temperature = QMC5883_TEMP_OFFSET + temp16 * 1.0f / 100.0f; - _px4_mag.set_temperature(temperature); - } - } - - /* - * RAW outputs - * - * to align the sensor axes with the board, x and y need to be flipped - * and y needs to be negated - */ - - if (!_px4_mag.external()) { - // 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; - - _px4_mag.update(timestamp_sample, xraw_f, yraw_f, zraw_f); - - /* - 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_status() -{ - I2CSPIDriverBase::print_status(); - perf_print_counter(_sample_perf); - perf_print_counter(_comms_errors); - printf("interval: %u us\n", _measure_interval); -} diff --git a/src/drivers/magnetometer/qmc5883/QMC5883.hpp b/src/drivers/magnetometer/qmc5883/QMC5883.hpp deleted file mode 100644 index 24ad93fdac..0000000000 --- a/src/drivers/magnetometer/qmc5883/QMC5883.hpp +++ /dev/null @@ -1,181 +0,0 @@ -/**************************************************************************** - * - * 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. - */ - -#pragma once - -#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 */ - -class QMC5883 : public I2CSPIDriver -{ -public: - QMC5883(device::Device *interface, enum Rotation rotation, I2CSPIBusOption bus_option, int bus, int i2c_address); - virtual ~QMC5883(); - - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); - static void print_usage(); - - void RunImpl(); - - int init(); - -protected: - void print_status() override; - -private: - PX4Magnetometer _px4_mag; - device::Device *_interface; - unsigned _measure_interval{0}; - - bool _collect_phase; - - perf_counter_t _sample_perf; - perf_counter_t _comms_errors; - perf_counter_t _range_errors; - perf_counter_t _conf_errors; - - 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(); - - /** - * 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(); -}; diff --git a/src/drivers/magnetometer/qmc5883/QMC5883_I2C.cpp b/src/drivers/magnetometer/qmc5883/QMC5883_I2C.cpp deleted file mode 100644 index c8a5562d52..0000000000 --- a/src/drivers/magnetometer/qmc5883/QMC5883_I2C.cpp +++ /dev/null @@ -1,135 +0,0 @@ -/**************************************************************************** - * - * 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 - */ - -#include -#include -#include - -#include "qmc5883.h" - -device::Device *QMC5883_I2C_interface(int bus, int bus_frequency, int i2c_address); - -class QMC5883_I2C : public device::I2C -{ -public: - QMC5883_I2C(int bus, int bus_frequency, int i2c_address); - virtual ~QMC5883_I2C() = default; - - virtual int read(unsigned address, void *data, unsigned count); - virtual int write(unsigned address, void *data, unsigned count); - -protected: - virtual int probe(); -}; - -device::Device * -QMC5883_I2C_interface(int bus, int bus_frequency, int i2c_address) -{ - return new QMC5883_I2C(bus, bus_frequency, i2c_address); -} - -QMC5883_I2C::QMC5883_I2C(int bus, int bus_frequency, int i2c_address) : - I2C(DRV_MAG_DEVTYPE_QMC5883, MODULE_NAME, bus, i2c_address, bus_frequency) -{ -} - -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.h b/src/drivers/magnetometer/qmc5883/qmc5883.h deleted file mode 100644 index d6b9df26f7..0000000000 --- a/src/drivers/magnetometer/qmc5883/qmc5883.h +++ /dev/null @@ -1,52 +0,0 @@ -/**************************************************************************** - * - * 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 - -#include - -#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_I2C_interface(int bus, int bus_frequency, int i2c_address); diff --git a/src/drivers/magnetometer/qmc5883/CMakeLists.txt b/src/drivers/magnetometer/qmc5883l/CMakeLists.txt similarity index 89% rename from src/drivers/magnetometer/qmc5883/CMakeLists.txt rename to src/drivers/magnetometer/qmc5883l/CMakeLists.txt index fdab77bb23..a48c25e38a 100644 --- a/src/drivers/magnetometer/qmc5883/CMakeLists.txt +++ b/src/drivers/magnetometer/qmc5883l/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2019 PX4 Development Team. All rights reserved. +# Copyright (c) 2020 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 @@ -30,15 +30,16 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ + px4_add_module( - MODULE drivers__magnetometer__qmc5883 - MAIN qmc5883 + MODULE drivers__magnetometer__qmc5883l + MAIN qmc5883l COMPILE_FLAGS SRCS - QMC5883_I2C.cpp - QMC5883.cpp - QMC5883.hpp - qmc5883_main.cpp + QMC5883L.cpp + QMC5883L.hpp + qmc5883l_main.cpp + QST_QMC5883L_registers.hpp DEPENDS drivers_magnetometer px4_work_queue diff --git a/src/drivers/magnetometer/qmc5883l/QMC5883L.cpp b/src/drivers/magnetometer/qmc5883l/QMC5883L.cpp new file mode 100644 index 0000000000..3efc443711 --- /dev/null +++ b/src/drivers/magnetometer/qmc5883l/QMC5883L.cpp @@ -0,0 +1,306 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "QMC5883L.hpp" + +using namespace time_literals; + +static constexpr int16_t combine(uint8_t msb, uint8_t lsb) +{ + return (msb << 8u) | lsb; +} + +QMC5883L::QMC5883L(I2CSPIBusOption bus_option, int bus, int bus_frequency, enum Rotation rotation) : + I2C(DRV_MAG_DEVTYPE_QMC5883L, MODULE_NAME, bus, I2C_ADDRESS_DEFAULT, bus_frequency), + I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), + _px4_mag(get_device_id(), rotation) +{ + _px4_mag.set_external(external()); +} + +QMC5883L::~QMC5883L() +{ + perf_free(_reset_perf); + perf_free(_bad_register_perf); + perf_free(_bad_transfer_perf); +} + +int QMC5883L::init() +{ + int ret = I2C::init(); + + if (ret != PX4_OK) { + DEVICE_DEBUG("I2C::init failed (%i)", ret); + return ret; + } + + return Reset() ? 0 : -1; +} + +bool QMC5883L::Reset() +{ + _state = STATE::RESET; + ScheduleClear(); + ScheduleNow(); + return true; +} + +void QMC5883L::print_status() +{ + I2CSPIDriverBase::print_status(); + + perf_print_counter(_reset_perf); + perf_print_counter(_bad_register_perf); + perf_print_counter(_bad_transfer_perf); +} + +int QMC5883L::probe() +{ + _retries = 1; + + for (int i = 0; i < 3; i++) { + // first read 0x0 once + const uint8_t cmd = 0; + uint8_t buffer{}; + + if (transfer(&cmd, 1, &buffer, 1) == PX4_OK) { + const uint8_t CHIP_ID = RegisterRead(Register::CHIP_ID); + + if (CHIP_ID == Chip_ID) { + return PX4_OK; + } + } + } + + return PX4_ERROR; +} + +void QMC5883L::RunImpl() +{ + const hrt_abstime now = hrt_absolute_time(); + + switch (_state) { + case STATE::RESET: + // CNTL2: Software Reset + RegisterWrite(Register::CNTL2, CNTL2_BIT::SOFT_RST); + _reset_timestamp = now; + _failure_count = 0; + _state = STATE::WAIT_FOR_RESET; + perf_count(_reset_perf); + ScheduleDelayed(100_ms); // POR Completion Time + break; + + case STATE::WAIT_FOR_RESET: + + // SOFT_RST: This bit is automatically reset to zero after POR routine + if ((RegisterRead(Register::CHIP_ID) == Chip_ID) + && ((RegisterRead(Register::CNTL2) & CNTL2_BIT::SOFT_RST) == 0)) { + + // if reset succeeded then configure + _state = STATE::CONFIGURE; + ScheduleDelayed(10_ms); + + } else { + // RESET not complete + if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + PX4_DEBUG("Reset failed, retrying"); + _state = STATE::RESET; + ScheduleDelayed(100_ms); + + } else { + PX4_DEBUG("Reset not complete, check again in 10 ms"); + ScheduleDelayed(10_ms); + } + } + + break; + + case STATE::CONFIGURE: + if (Configure()) { + // if configure succeeded then start reading every 20 ms (50 Hz) + _state = STATE::READ; + ScheduleOnInterval(20_ms, 20_ms); + + } else { + // CONFIGURE not complete + if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + PX4_DEBUG("Configure failed, resetting"); + _state = STATE::RESET; + + } else { + PX4_DEBUG("Configure failed, retrying"); + } + + ScheduleDelayed(100_ms); + } + + break; + + case STATE::READ: { + struct TransferBuffer { + uint8_t X_LSB; + uint8_t X_MSB; + uint8_t Y_LSB; + uint8_t Y_MSB; + uint8_t Z_LSB; + uint8_t Z_MSB; + uint8_t STATUS; + } buffer{}; + + bool success = false; + uint8_t cmd = static_cast(Register::X_LSB); + + if (transfer(&cmd, 1, (uint8_t *)&buffer, sizeof(buffer)) == PX4_OK) { + // process data if successful transfer, no overflow + if ((buffer.STATUS & STATUS_BIT::OVL) == 0) { + int16_t x = combine(buffer.X_MSB, buffer.X_LSB); + int16_t y = combine(buffer.Y_MSB, buffer.Y_LSB); + int16_t z = combine(buffer.Z_MSB, buffer.Z_LSB); + + if (x != _prev_data[0] || y != _prev_data[1] || z != _prev_data[2]) { + _prev_data[0] = x; + _prev_data[1] = y; + _prev_data[2] = z; + + // Sensor orientation + // Forward X := +X + // Right Y := -Y + // Down Z := -Z + y = (y == INT16_MIN) ? INT16_MAX : -y; // -y + z = (z == INT16_MIN) ? INT16_MAX : -z; // -z + + _px4_mag.update(now, x, y, z); + + success = true; + + if (_failure_count > 0) { + _failure_count--; + } + } + } + + } else { + perf_count(_bad_transfer_perf); + } + + if (!success) { + _failure_count++; + + // full reset if things are failing consistently + if (_failure_count > 10) { + Reset(); + return; + } + } + + if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) { + // check configuration registers periodically or immediately following any failure + if (RegisterCheck(_register_cfg[_checked_register])) { + _last_config_check_timestamp = now; + _checked_register = (_checked_register + 1) % size_register_cfg; + + } else { + // register check failed, force reset + perf_count(_bad_register_perf); + Reset(); + } + } + } + + break; + } +} + +bool QMC5883L::Configure() +{ + // first set and clear all configured register bits + for (const auto ®_cfg : _register_cfg) { + RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits); + } + + // now check that all are configured + bool success = true; + + for (const auto ®_cfg : _register_cfg) { + if (!RegisterCheck(reg_cfg)) { + success = false; + } + } + + _px4_mag.set_scale(1.f / 12000.f); // 12000 LSB/Gauss (Field Range = ±2G) + + return success; +} + +bool QMC5883L::RegisterCheck(const register_config_t ®_cfg) +{ + bool success = true; + + const uint8_t reg_value = RegisterRead(reg_cfg.reg); + + if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) { + PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits); + success = false; + } + + if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) { + PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits); + success = false; + } + + return success; +} + +uint8_t QMC5883L::RegisterRead(Register reg) +{ + const uint8_t cmd = static_cast(reg); + uint8_t buffer{}; + transfer(&cmd, 1, &buffer, 1); + return buffer; +} + +void QMC5883L::RegisterWrite(Register reg, uint8_t value) +{ + uint8_t buffer[2] { (uint8_t)reg, value }; + transfer(buffer, sizeof(buffer), nullptr, 0); +} + +void QMC5883L::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits) +{ + const uint8_t orig_val = RegisterRead(reg); + uint8_t val = (orig_val & ~clearbits) | setbits; + + if (orig_val != val) { + RegisterWrite(reg, val); + } +} diff --git a/src/drivers/magnetometer/qmc5883l/QMC5883L.hpp b/src/drivers/magnetometer/qmc5883l/QMC5883L.hpp new file mode 100644 index 0000000000..a0e23e3daa --- /dev/null +++ b/src/drivers/magnetometer/qmc5883l/QMC5883L.hpp @@ -0,0 +1,115 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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 QMC5883L.hpp + * + * Driver for the QMC5883L connected via I2C. + * + */ + +#pragma once + +#include "QST_QMC5883L_registers.hpp" + +#include +#include +#include +#include +#include + +using namespace QST_QMC5883L; + +class QMC5883L : public device::I2C, public I2CSPIDriver +{ +public: + QMC5883L(I2CSPIBusOption bus_option, int bus, int bus_frequency, enum Rotation rotation = ROTATION_NONE); + ~QMC5883L() override; + + static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance); + static void print_usage(); + + void RunImpl(); + + int init() override; + void print_status() override; + +private: + // Sensor Configuration + struct register_config_t { + Register reg; + uint8_t set_bits{0}; + uint8_t clear_bits{0}; + }; + + int probe() override; + + bool Reset(); + + bool Configure(); + + bool RegisterCheck(const register_config_t ®_cfg); + + uint8_t RegisterRead(Register reg); + void RegisterWrite(Register reg, uint8_t value); + void RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits); + + PX4Magnetometer _px4_mag; + + perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")}; + perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")}; + perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": reset")}; + + hrt_abstime _reset_timestamp{0}; + hrt_abstime _last_config_check_timestamp{0}; + int _failure_count{0}; + + int16_t _prev_data[3] {}; + + enum class STATE : uint8_t { + RESET, + WAIT_FOR_RESET, + CONFIGURE, + READ, + } _state{STATE::RESET}; + + uint8_t _checked_register{0}; + static constexpr uint8_t size_register_cfg{3}; + register_config_t _register_cfg[size_register_cfg] { + // Register | Set bits, Clear bits + { Register::CNTL1, CNTL1_BIT::ODR_50HZ | CNTL1_BIT::Mode_Continuous, CNTL1_BIT::OSR_512 | CNTL1_BIT::RNG_2G}, + { Register::CNTL2, CNTL2_BIT::ROL_PNT, CNTL2_BIT::SOFT_RST}, + { Register::SET_RESET_PERIOD, SET_RESET_PERIOD_BIT::FBR, 0 }, + }; +}; diff --git a/src/drivers/magnetometer/qmc5883l/QST_QMC5883L_registers.hpp b/src/drivers/magnetometer/qmc5883l/QST_QMC5883L_registers.hpp new file mode 100644 index 0000000000..094b1f1f8f --- /dev/null +++ b/src/drivers/magnetometer/qmc5883l/QST_QMC5883L_registers.hpp @@ -0,0 +1,113 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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 QST_QMC5883L_registers.hpp + * + * QST QMC5883L registers. + * + */ + +#pragma once + +#include + +namespace QST_QMC5883L +{ + +// TODO: move to a central header +static constexpr uint8_t Bit0 = (1 << 0); +static constexpr uint8_t Bit1 = (1 << 1); +static constexpr uint8_t Bit2 = (1 << 2); +static constexpr uint8_t Bit3 = (1 << 3); +static constexpr uint8_t Bit4 = (1 << 4); +static constexpr uint8_t Bit5 = (1 << 5); +static constexpr uint8_t Bit6 = (1 << 6); +static constexpr uint8_t Bit7 = (1 << 7); + +static constexpr uint32_t I2C_SPEED = 400 * 1000; // 400 kHz I2C serial interface +static constexpr uint8_t I2C_ADDRESS_DEFAULT = 0b0001101; // default I2C address + +static constexpr uint8_t Chip_ID = 0xFF; + +enum class Register : uint8_t { + X_LSB = 0x00, // Data Output X LSB Register XOUT[7:0] + X_MSB = 0x01, // Data Output X MSB Register XOUT[15:8] + Y_LSB = 0x02, // Data Output Y LSB Register YOUT[7:0] + Y_MSB = 0x03, // Data Output Y MSB Register YOUT[15:8] + Z_LSB = 0x04, // Data Output Z LSB Register ZOUT[7:0] + Z_MSB = 0x05, // Data Output Z MSB Register ZOUT[15:8] + STATUS = 0x06, // Status Register 1 + + CNTL1 = 0x09, // Control Register 1 + CNTL2 = 0x0A, // Control Register 2 + + SET_RESET_PERIOD = 0x0B, // SET/RESET Period is controlled by FBR [7:0], it is recommended that the register 0BH is written by 0x01. + + CHIP_ID = 0x0D, +}; + +// STATUS +enum STATUS_BIT : uint8_t { + DOR = Bit2, // Data Skip + OVL = Bit1, // Overflow flag + DRDY = Bit0, // Data Ready +}; + +// CNTL1 +enum CNTL1_BIT : uint8_t { + // OSR[7:6] + OSR_512 = Bit7 | Bit6, // 00 + + // RNG[5:4] + RNG_2G = Bit5 | Bit4, // 00 + + // ODR[3:2] + ODR_50HZ = Bit2, // 01 + + // MODE[1:0] + Mode_Continuous = Bit0, // 01 +}; + +// CNTL2 +enum CNTL2_BIT : uint8_t { + SOFT_RST = Bit7, // Soft reset, restore default value of all registers. + ROL_PNT = Bit6, // Enable pointer roll-over function. +}; + +// SET_RESET_PERIOD +enum SET_RESET_PERIOD_BIT : uint8_t { + FBR = Bit0, // it is recommended that the register is written by 0x01. +}; + +} // namespace QMC5883L diff --git a/src/drivers/magnetometer/qmc5883/qmc5883_main.cpp b/src/drivers/magnetometer/qmc5883l/qmc5883l_main.cpp similarity index 70% rename from src/drivers/magnetometer/qmc5883/qmc5883_main.cpp rename to src/drivers/magnetometer/qmc5883l/qmc5883l_main.cpp index 23eb3cefb8..183c07c7ff 100644 --- a/src/drivers/magnetometer/qmc5883/qmc5883_main.cpp +++ b/src/drivers/magnetometer/qmc5883l/qmc5883l_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2020 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 @@ -31,73 +31,46 @@ * ****************************************************************************/ -/** - * @file qmc5883.cpp - * - * Driver for the QMC5883 magnetometer connected via I2C. - */ +#include "QMC5883L.hpp" #include #include -#include "QMC5883.hpp" -#include "qmc5883.h" - -extern "C" __EXPORT int qmc5883_main(int argc, char *argv[]); - -I2CSPIDriverBase *QMC5883::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) +I2CSPIDriverBase * +QMC5883L::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, int runtime_instance) { - device::Device *interface = nullptr; + QMC5883L *instance = new QMC5883L(iterator.configuredBusOption(), iterator.bus(), cli.bus_frequency, cli.rotation); - if (iterator.busType() == BOARD_I2C_BUS) { - interface = QMC5883_I2C_interface(iterator.bus(), cli.bus_frequency, cli.i2c_address); - } - - if (interface == nullptr) { + if (!instance) { PX4_ERR("alloc failed"); return nullptr; } - if (interface->init() != OK) { - delete interface; + if (instance->init() != PX4_OK) { + delete instance; PX4_DEBUG("no device on bus %i (devid 0x%x)", iterator.bus(), iterator.devid()); return nullptr; } - QMC5883 *dev = new QMC5883(interface, cli.rotation, iterator.configuredBusOption(), iterator.bus(), cli.i2c_address); - - if (dev == nullptr) { - delete interface; - return nullptr; - } - - if (OK != dev->init()) { - delete dev; - return nullptr; - } - - return dev; + return instance; } -void QMC5883::print_usage() +void QMC5883L::print_usage() { - PRINT_MODULE_USAGE_NAME("qmc5883", "driver"); + PRINT_MODULE_USAGE_NAME("qmc5883l", "driver"); PRINT_MODULE_USAGE_SUBCATEGORY("magnetometer"); PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); - PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x0D); PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -extern "C" int qmc5883_main(int argc, char *argv[]) +extern "C" int qmc5883l_main(int argc, char *argv[]) { - using ThisDriver = QMC5883; int ch; + using ThisDriver = QMC5883L; BusCLIArguments cli{true, false}; - cli.i2c_address = ADDR_ID_B; - cli.default_i2c_frequency = 400000; + cli.default_i2c_frequency = I2C_SPEED; while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { switch (ch) { @@ -114,7 +87,7 @@ extern "C" int qmc5883_main(int argc, char *argv[]) return -1; } - BusInstanceIterator iterator(MODULE_NAME, cli, DRV_MAG_DEVTYPE_QMC5883); + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_MAG_DEVTYPE_QMC5883L); if (!strcmp(verb, "start")) { return ThisDriver::module_start(cli, iterator);