From 8fff1ec4d4b42c2d752ee9e91ee5a441752014fa Mon Sep 17 00:00:00 2001 From: ljwang Date: Fri, 16 Jun 2017 13:52:02 +0800 Subject: [PATCH] add QMC5883L driver for GPS compass module --- libraries/AP_Compass/AP_Compass_QMC5883L.cpp | 219 +++++++++++++++++++ libraries/AP_Compass/AP_Compass_QMC5883L.h | 57 +++++ 2 files changed, 276 insertions(+) create mode 100644 libraries/AP_Compass/AP_Compass_QMC5883L.cpp create mode 100644 libraries/AP_Compass/AP_Compass_QMC5883L.h diff --git a/libraries/AP_Compass/AP_Compass_QMC5883L.cpp b/libraries/AP_Compass/AP_Compass_QMC5883L.cpp new file mode 100644 index 0000000000..3fd0b7a232 --- /dev/null +++ b/libraries/AP_Compass/AP_Compass_QMC5883L.cpp @@ -0,0 +1,219 @@ +/* + * Copyright (C) 2016 Emlid Ltd. All rights reserved. + * + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Driver by RadioLink LjWang, Jun 2017 + */ +#include "AP_Compass_QMC5883L.h" + +#include +#include + +#include +#include +#include + +#define QMC5883L_REG_CONF1 0x09 +#define QMC5883L_REG_CONF2 0x0A + +// data output rates for 5883L +#define QMC5883L_ODR_10HZ (0x00 << 2) +#define QMC5883L_ODR_50HZ (0x01 << 2) +#define QMC5883L_ODR_100HZ (0x02 << 2) +#define QMC5883L_ODR_200HZ (0x03 << 2) + +// Sensor operation modes +#define QMC5883L_MODE_STANDBY 0x00 +#define QMC5883L_MODE_CONTINUOUS 0x01 + +#define QMC5883L_RNG_2G (0x00 << 4) +#define QMC5883L_RNG_8G (0x01 << 4) + +#define QMC5883L_OSR_512 (0x00 << 6) +#define QMC5883L_OSR_256 (0x01 << 6) +#define QMC5883L_OSR_128 (0x10 << 6) +#define QMC5883L_OSR_64 (0x11 << 6) + +#define QMC5883L_RST 0x80 + +#define QMC5883L_REG_DATA_OUTPUT_X 0x00 +#define QMC5883L_REG_STATUS 0x06 + +#define QMC5883L_REG_ID 0x0D +#define QMC5883_ID_VAL 0xFF + +extern const AP_HAL::HAL &hal; + +AP_Compass_Backend *AP_Compass_QMC5883L::probe(Compass &compass, + AP_HAL::OwnPtr dev, + enum Rotation rotation) +{ + if (!dev) { + return nullptr; + } + + AP_Compass_QMC5883L *sensor = new AP_Compass_QMC5883L(compass, std::move(dev),rotation); + if (!sensor || !sensor->init()) { + delete sensor; + return nullptr; + } + + return sensor; +} + +AP_Compass_QMC5883L::AP_Compass_QMC5883L(Compass &compass, + AP_HAL::OwnPtr dev, + enum Rotation rotation) + : AP_Compass_Backend(compass) + , _dev(std::move(dev)) + , _rotation(rotation) +{ +} + +bool AP_Compass_QMC5883L::init() +{ + if (!_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { + return false; + } + //must reset first + _dev->write_register(QMC5883L_REG_CONF2,QMC5883L_RST); + + _dev->set_retries(10); + + uint8_t whoami; + if (!_dev->read_registers(QMC5883L_REG_ID, &whoami,1)|| + whoami != QMC5883_ID_VAL){ + // not an QMC5883L + goto fail; + } + + if (!_dev->write_register(0x0B, 0x01)|| + !_dev->write_register(0x20, 0x40)|| + !_dev->write_register(0x21, 0x01)|| + !_dev->write_register(QMC5883L_REG_CONF1, + QMC5883L_MODE_CONTINUOUS| + QMC5883L_ODR_100HZ| + QMC5883L_OSR_512| + QMC5883L_RNG_8G)) { + goto fail; + } + + // lower retries for run + _dev->set_retries(3); + + _dev->get_semaphore()->give(); + + _instance = register_compass(); + + printf("%s found on bus %u id %u address 0x%02x\n", name, + _dev->bus_num(), _dev->get_bus_id(), _dev->get_bus_address()); + + set_rotation(_instance, _rotation); + + _dev->set_device_type(DEVTYPE_QMC5883L); + set_dev_id(_instance, _dev->get_bus_id()); + + //Enable 100HZ + _dev->register_periodic_callback(10000, + FUNCTOR_BIND_MEMBER(&AP_Compass_QMC5883L::timer, void)); + + return true; + +fail: + _dev->get_semaphore()->give(); + return false; +} + +void AP_Compass_QMC5883L::timer() +{ + struct PACKED { + le16_t rx; + le16_t ry; + le16_t rz; + } buffer; + + const float range_scale = 1000.0f / 3000.0f; + + uint8_t status; + if(!_dev->read_registers(QMC5883L_REG_STATUS,&status,1)){ + return; + } + //new data is ready + if(!status & 0x04){ + return; + } + + if(!_dev->read_registers(QMC5883L_REG_DATA_OUTPUT_X, (uint8_t *) &buffer, sizeof(buffer))){ + return ; + } + + uint32_t now = AP_HAL::micros(); + + auto x = -static_cast(le16toh(buffer.rx)); + auto y = static_cast(le16toh(buffer.ry)); + auto z = -static_cast(le16toh(buffer.rz)); + +#if 0 + printf("mag.x:%d\n",x); + printf("mag.y:%d\n",y); + printf("mag.z:%d\n",z); +#endif + + Vector3f field = Vector3f{x * range_scale , y * range_scale, z * range_scale }; + + /* rotate raw_field from sensor frame to body frame */ + rotate_field(field, _instance); + + /* publish raw_field (uncorrected point sample) for calibration use */ + publish_raw_field(field, now, _instance); + + /* correct raw_field for known errors */ + correct_field(field, _instance); + + if (_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { + _accum += field; + _accum_count++; + if(_accum_count == 20){ + _accum.x /= 2; + _accum.y /= 2; + _accum.z /= 2; + _accum_count = 10; + } + + _sem->give(); + } +} + +void AP_Compass_QMC5883L::read() +{ + if (!_sem->take_nonblocking()) { + return; + } + + if (_accum_count == 0) { + _sem->give(); + return; + } + + Vector3f field(_accum); + field /= _accum_count; + + publish_filtered_field(field, _instance); + + _accum.zero(); + _accum_count = 0; + + _sem->give(); +} diff --git a/libraries/AP_Compass/AP_Compass_QMC5883L.h b/libraries/AP_Compass/AP_Compass_QMC5883L.h new file mode 100644 index 0000000000..ac70d8537e --- /dev/null +++ b/libraries/AP_Compass/AP_Compass_QMC5883L.h @@ -0,0 +1,57 @@ +/* + * Copyright (C) 2016 Emlid Ltd. All rights reserved. + * + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ +#pragma once + +#include +#include +#include +#include + +#include "AP_Compass.h" +#include "AP_Compass_Backend.h" + +#ifndef HAL_COMPASS_QMC5883L_I2C_ADDR +#define HAL_COMPASS_QMC5883L_I2C_ADDR 0x0D +#endif + +class AP_Compass_QMC5883L : public AP_Compass_Backend +{ +public: + static AP_Compass_Backend *probe(Compass &compass, + AP_HAL::OwnPtr dev, + enum Rotation rotation = ROTATION_NONE); + + void read() override; + + static constexpr const char *name = "QMC5883L"; + +private: + AP_Compass_QMC5883L(Compass &compass, + AP_HAL::OwnPtr dev, + enum Rotation rotation); + + void timer(); + bool init(); + + AP_HAL::OwnPtr _dev; + + Vector3f _accum = Vector3f(); + uint32_t _accum_count = 0; + + enum Rotation _rotation; + uint8_t _instance; +};