AP_Compass: added a LIS3MDL compass driver

libraries/AP_Compass/AP_Compass_LIS3MDL.cpp

@@ -0,0 +1,197 @@
+/*
+ * 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 Andrew Tridgell, Nov 2016
+
+  thanks to Robert Dickenson and the PX4 team for register definitions
+ */
+#include "AP_Compass_LIS3MDL.h"
+
+#include <AP_HAL/AP_HAL.h>
+#include <utility>
+#include <AP_Math/AP_Math.h>
+#include <stdio.h>
+
+#define ADDR_CTRL_REG1      0x20
+#define ADDR_CTRL_REG2      0x21
+#define ADDR_CTRL_REG3      0x22
+#define ADDR_CTRL_REG4      0x23
+#define ADDR_CTRL_REG5      0x24
+
+#define ADDR_STATUS_REG     0x27
+#define ADDR_OUT_X_L        0x28
+#define ADDR_OUT_X_H        0x29
+#define ADDR_OUT_Y_L        0x2a
+#define ADDR_OUT_Y_H        0x2b
+#define ADDR_OUT_Z_L        0x2c
+#define ADDR_OUT_Z_H        0x2d
+#define ADDR_OUT_T_L        0x2e
+#define ADDR_OUT_T_H        0x2f
+
+#define MODE_REG_CONTINOUS_MODE     (0 << 0)
+#define MODE_REG_SINGLE_MODE        (1 << 0)
+
+#define ADDR_WHO_AM_I       0x0f
+#define ID_WHO_AM_I         0x3d
+
+extern const AP_HAL::HAL &hal;
+
+AP_Compass_Backend *AP_Compass_LIS3MDL::probe(Compass &compass,
+                                              AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
+                                              bool force_external,
+                                              enum Rotation rotation)
+{
+    if (!dev) {
+        return nullptr;
+    }
+    AP_Compass_LIS3MDL *sensor = new AP_Compass_LIS3MDL(compass, std::move(dev), force_external, rotation);
+    if (!sensor || !sensor->init()) {
+        delete sensor;
+        return nullptr;
+    }
+
+    return sensor;
+}
+
+AP_Compass_LIS3MDL::AP_Compass_LIS3MDL(Compass &compass,
+                                       AP_HAL::OwnPtr<AP_HAL::Device> _dev,
+                                       bool _force_external,
+                                       enum Rotation _rotation)
+    : AP_Compass_Backend(compass)
+    , dev(std::move(_dev))
+    , force_external(_force_external)
+    , rotation(_rotation)
+{
+}
+
+bool AP_Compass_LIS3MDL::init()
+{
+    if (!dev->get_semaphore()->take(0)) {
+        return false;
+    }
+
+    if (dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI) {
+        dev->set_read_flag(0xC0);
+    }
+
+    uint8_t whoami;
+    if (!dev->read_registers(ADDR_WHO_AM_I, &whoami, 1) ||
+        whoami != ID_WHO_AM_I) {
+        // not a 3MDL
+        goto fail;
+    }
+
+    dev->setup_checked_registers(4);
+
+    dev->write_register(ADDR_CTRL_REG1, 0xFC, true);
+    dev->write_register(ADDR_CTRL_REG2, 0, true); // 4Ga range
+    dev->write_register(ADDR_CTRL_REG4, 0x0C, true); // z-axis ultra high perf
+    dev->write_register(ADDR_CTRL_REG5, 0x40, true);
+    
+    dev->get_semaphore()->give();
+
+    /* register the compass instance in the frontend */
+    compass_instance = register_compass();
+
+    printf("Found a LIS3MDL on 0x%x as compass %u\n", dev->get_bus_id(), compass_instance);
+    
+    set_rotation(compass_instance, rotation);
+
+    if (force_external) {
+        set_external(compass_instance, true);
+    }
+    
+    dev->set_device_type(DEVTYPE_LIS3MDL);
+    set_dev_id(compass_instance, dev->get_bus_id());
+
+    phase = PHASE_MEASURE;
+    
+    // call timer() at 80Hz
+    dev->register_periodic_callback(12500,
+                                     FUNCTOR_BIND_MEMBER(&AP_Compass_LIS3MDL::timer, bool));
+
+    return true;
+
+fail:
+    dev->get_semaphore()->give();
+    return false;
+}
+
+bool AP_Compass_LIS3MDL::timer()
+{
+    if (phase == PHASE_MEASURE) {
+        // start single measurement
+        if (dev->write_register(ADDR_CTRL_REG3, 1)) {
+            phase = PHASE_COLLECT;
+        }
+        return true;
+    }
+
+    struct PACKED {
+        uint8_t status;
+        int16_t magx;
+        int16_t magy;
+        int16_t magz;
+    } data;
+    const float range_scale = 1000.0f / 6842.0f;
+    Vector3f field;
+
+    if (!dev->read_registers(ADDR_STATUS_REG, (uint8_t *)&data, sizeof(data))) {
+        goto check_registers;
+    }
+
+    field(data.magx * range_scale, data.magy * range_scale, data.magz * range_scale);
+
+    /* rotate raw_field from sensor frame to body frame */
+    rotate_field(field, compass_instance);
+
+    /* publish raw_field (uncorrected point sample) for calibration use */
+    publish_raw_field(field, AP_HAL::micros(), compass_instance);
+
+    /* correct raw_field for known errors */
+    correct_field(field, compass_instance);
+
+    if (_sem->take(0)) {
+        accum += field;
+        accum_count++;
+        _sem->give();
+    }
+
+    phase = PHASE_MEASURE;
+    
+check_registers:
+    dev->check_next_register();
+    return true;
+}
+
+void AP_Compass_LIS3MDL::read()
+{
+    if (!_sem->take_nonblocking()) {
+        return;
+    }
+    if (accum_count == 0) {
+        _sem->give();
+        return;
+    }
+
+    accum /= accum_count;
+
+    publish_filtered_field(accum, compass_instance);
+
+    accum.zero();
+    accum_count = 0;
+    
+    _sem->give();
+}

libraries/AP_Compass/AP_Compass_LIS3MDL.h

@@ -0,0 +1,59 @@
+/*
+ * 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"
+
+class AP_Compass_LIS3MDL : public AP_Compass_Backend
+{
+public:
+    static AP_Compass_Backend *probe(Compass &compass,
+                                     AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
+                                     bool force_external = false,
+                                     enum Rotation rotation = ROTATION_NONE);
+
+    void read() override;
+
+    static constexpr const char *name = "LIS3MDL";
+
+private:
+    AP_Compass_LIS3MDL(Compass &compass, AP_HAL::OwnPtr<AP_HAL::Device> dev,
+                       bool force_external,
+                       enum Rotation rotation);
+
+    AP_HAL::OwnPtr<AP_HAL::Device> dev;
+    
+    /**
+     * Device periodic callback to read data from the sensor.
+     */
+    bool init();
+    bool timer();
+
+    enum {
+        PHASE_MEASURE, PHASE_COLLECT
+    } phase;
+    
+    uint8_t compass_instance;
+    Vector3f accum;
+    uint16_t accum_count;
+    enum Rotation rotation;
+    bool force_external;
+};