add QMC5883L driver for GPS compass module

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 <http://www.gnu.org/licenses/>.
+ *
+ * Driver by RadioLink LjWang, Jun 2017
+ */
+#include "AP_Compass_QMC5883L.h"
+
+#include <stdio.h>
+#include <utility>
+
+#include <AP_HAL/AP_HAL.h>
+#include <AP_HAL/utility/sparse-endian.h>
+#include <AP_Math/AP_Math.h>
+
+#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<AP_HAL::I2CDevice> 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<AP_HAL::Device> 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<int16_t>(le16toh(buffer.rx));
+    auto y = static_cast<int16_t>(le16toh(buffer.ry));
+    auto z = -static_cast<int16_t>(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();
+}

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 <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+
+#include <AP_Common/AP_Common.h>
+#include <AP_HAL/AP_HAL.h>
+#include <AP_HAL/I2CDevice.h>
+#include <AP_Math/AP_Math.h>
+
+#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<AP_HAL::I2CDevice> dev,
+                                     enum Rotation rotation = ROTATION_NONE);
+
+    void read() override;
+
+    static constexpr const char *name = "QMC5883L";
+
+private:
+    AP_Compass_QMC5883L(Compass &compass,
+                       AP_HAL::OwnPtr<AP_HAL::Device> dev,
+                       enum Rotation rotation);
+
+    void timer();
+    bool init();
+
+    AP_HAL::OwnPtr<AP_HAL::Device> _dev;
+
+    Vector3f _accum = Vector3f();
+    uint32_t _accum_count = 0;
+
+    enum Rotation _rotation;
+    uint8_t _instance;
+};