AP_Compass: mAG3110 driver

libraries/AP_Compass/AP_Compass.cpp

@@ -24,6 +24,7 @@
 #include "AP_Compass_UAVCAN.h"
 #endif
 #include "AP_Compass_MMC3416.h"
+#include "AP_Compass_MAG3110.h"
 #include "AP_Compass.h"
 
 extern AP_HAL::HAL& hal;
@@ -807,6 +808,9 @@ void Compass::_detect_backends(void)
 #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_LIS3MDL
     ADD_BACKEND(DRIVER_LIS3MDL, AP_Compass_LIS3MDL::probe(*this, hal.spi->get_device(HAL_COMPASS_LIS3MDL_NAME), false, ROTATION_ROLL_180_YAW_90),
                 AP_Compass_LIS3MDL::name, false);
+#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_MAG3110                
+    ADD_BACKEND(DRIVER_MAG3110, AP_Compass_MAG3110::probe(*this, hal.i2c_mgr->get_device(HAL_MAG3110_I2C_BUS, HAL_MAG3110_I2C_ADDR), ROTATION_NONE),
+                AP_Compass_MAG3110::name, true);
 #elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX && CONFIG_HAL_BOARD_SUBTYPE != HAL_BOARD_SUBTYPE_LINUX_NONE
     ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR)),
                 AP_Compass_HMC5843::name, false);

libraries/AP_Compass/AP_Compass.h

@@ -339,6 +339,7 @@ private:
         DRIVER_UAVCAN   =11,
         DRIVER_QMC5883  =12,
         DRIVER_SITL     =13,
+        DRIVER_MAG3110  =14,
     };
 
     bool _driver_enabled(enum DriverType driver_type);

libraries/AP_Compass/AP_Compass_Backend.h

@@ -61,7 +61,8 @@ public:
         DEVTYPE_IST8310 = 0x0A,
         DEVTYPE_ICM20948 = 0x0B,
         DEVTYPE_MMC3416 = 0x0C,
-		DEVTYPE_QMC5883L = 0x0D,
+	DEVTYPE_QMC5883L = 0x0D,
+	DEVTYPE_MAG3110  = 0x0E,
     };
 
 

libraries/AP_Compass/AP_Compass_MAG3110.cpp

@@ -0,0 +1,274 @@
+/*
+   This program 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 program 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/>.
+ */
+#include <utility>
+
+#include <AP_HAL/AP_HAL.h>
+#include <AP_Math/AP_Math.h>
+#include <stdio.h>
+
+#include "AP_Compass_MAG3110.h"
+
+extern const AP_HAL::HAL &hal;
+
+
+/*
+EN:
+    at first glance, the magnetometer MAG3110 consists only of flaws:
+    * noisy, with a bad characteristic, with very large difference on axes
+    * it can't be calibrated in any way, you just have to believe what he has been measured
+    * There are no adjustments and settings, it just sends data with some unknown sensitivity. Or does not sends :)
+    * One and a half setup registers, in which only the frequency of operation and the number of averagings are specified
+    
+    This is a device, wooden to the waist. But of all these shortcomings, its sole and basic virtue arises:
+    * He will never comes buggy or clevers.
+    
+    And since we do not need much from a magnetometer and it is required to calibrate the Ardupilot itself, the device
+    appears in a completely new light - as a reliable info "north is there." What we really need.
+
+RUS:
+    на первый взгляд, магнитометр MAG3110 состоит из одних лишь недостатков:
+    * шумный, с кривой характеристикой, 
+    * никак не калибруется, приходится просто верить тому что он намерял
+    * нет никаких регулировок и настроек, он просто выдает данные с некой неизвестной чувствительностью. Или не выдает :)
+    * полтора настроечных регистра, в которых задается только частота работы и количество усреднений
+    
+    Такой вот девайс, по пояс деревянный. Но из всех этих недостатков проистекает его единственное и основное достоинство:
+    * он никогда не глючит и не умничает.
+    
+    А так как нам от магнитометра особо много и не требуется, а калибровать Ардупилот и сам умеет, то девайс
+    предстает в совсем новом свете - как надежный указатель "север там". Что нам собственно и надо.
+
+*/
+
+
+
+// Registers
+#define MAG3110_MAG_REG_STATUS       0x00
+#define MAG3110_MAG_REG_HXL          0x01
+#define MAG3110_MAG_REG_HXH          0x02
+#define MAG3110_MAG_REG_HYL          0x03
+#define MAG3110_MAG_REG_HYH          0x04
+#define MAG3110_MAG_REG_HZL          0x05
+#define MAG3110_MAG_REG_HZH          0x06
+#define MAG3110_MAG_REG_WHO_AM_I     0x07
+#define MAG3110_MAG_REG_SYSMODE      0x08
+#define MAG3110_MAG_REG_CTRL_REG1    0x10
+#define MAG3110_MAG_REG_CTRL_REG2    0x11
+
+#define BIT_STATUS_REG_DATA_READY    (1 << 3)
+
+
+
+
+AP_Compass_MAG3110::AP_Compass_MAG3110(Compass &compass, AP_HAL::OwnPtr<AP_HAL::Device> dev)
+    : AP_Compass_Backend(compass)
+    , _dev(std::move(dev))
+{
+}
+
+AP_Compass_Backend *AP_Compass_MAG3110::probe(Compass &compass,
+                                              AP_HAL::OwnPtr<AP_HAL::Device> dev,
+                                              enum Rotation rotation)
+{
+    if (!dev) {
+        return nullptr;
+    }
+    AP_Compass_MAG3110 *sensor = new AP_Compass_MAG3110(compass, std::move(dev));
+    if (!sensor || !sensor->init(rotation)) {
+        delete sensor;
+        return nullptr;
+    }
+
+    return sensor;
+}
+
+
+bool AP_Compass_MAG3110::init(enum Rotation rotation)
+{
+
+    bool success = _hardware_init();
+
+    if (!success) {
+        return false;
+    }
+
+    _initialised = true;
+
+    /* register the compass instance in the frontend */
+    _compass_instance = register_compass();
+
+    set_rotation(_compass_instance, rotation);
+
+    _dev->set_device_type(DEVTYPE_MAG3110);
+    set_dev_id(_compass_instance, _dev->get_bus_id());
+
+    // read at 75Hz
+    _dev->register_periodic_callback(13333, FUNCTOR_BIND_MEMBER(&AP_Compass_MAG3110::_update, void)); 
+
+    return true;
+}
+
+
+bool AP_Compass_MAG3110::_hardware_init()
+{
+
+    AP_HAL::Semaphore *bus_sem = _dev->get_semaphore();
+    if (!bus_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
+        AP_HAL::panic("MAG3110: Unable to get semaphore");
+    }
+
+    // initially run the bus at low speed
+    _dev->set_speed(AP_HAL::Device::SPEED_LOW);
+
+    bool ret=false;
+    
+    _dev->set_retries(5);
+    
+    uint8_t sig = 0;
+    bool ack = _dev->read_registers(MAG3110_MAG_REG_WHO_AM_I, &sig, 1);    
+    if (!ack || sig != 0xC4) goto exit;
+
+    ack = _dev->write_register(MAG3110_MAG_REG_CTRL_REG1, 0x01); //  active mode 80 Hz ODR with OSR = 1
+    if (!ack) goto exit;
+
+    hal.scheduler->delay(20);
+    
+    ack = _dev->write_register(MAG3110_MAG_REG_CTRL_REG2, 0xA0); // AUTO_MRST_EN + RAW
+    if (!ack) goto exit;
+
+    ret = true;
+
+    _dev->set_retries(3);
+    
+    printf("MAG3110 found on bus 0x%x\n", (uint16_t)_dev->get_bus_id());
+
+exit:
+    _dev->set_speed(AP_HAL::Device::SPEED_HIGH);
+    bus_sem->give();
+    return ret;
+}
+
+
+// Read Sensor data
+bool AP_Compass_MAG3110::_read_sample()
+{
+    {
+        uint8_t status;
+        bool ack = _dev->read_registers(MAG3110_MAG_REG_STATUS, &status, 1);
+    
+        if (!ack || (status & BIT_STATUS_REG_DATA_READY) == 0) {
+            return false;
+        }
+    }
+
+    uint8_t buf[6];
+    bool ack = _dev->read_registers(MAG3110_MAG_REG_HXL, buf, 6);
+    if (!ack) {
+        return false;
+    }
+
+    _mag_x = (int16_t)(buf[0] << 8 | buf[1]);
+    _mag_y = (int16_t)(buf[2] << 8 | buf[3]);
+    _mag_z = (int16_t)(buf[4] << 8 | buf[5]);
+
+    return true;
+}
+
+
+#define MAG_SCALE 1.0/10000  // 1 Tesla full scale of +-10000
+
+void AP_Compass_MAG3110::_update()
+{
+    if (!_read_sample()) {
+        return;
+    }
+
+    Vector3f raw_field = Vector3f(_mag_x, _mag_y, _mag_z) * MAG_SCALE;
+
+
+    bool ret=true;
+    
+    float len = raw_field.length();
+    if(is_zero(compass_len)) {
+        compass_len=len;
+    } else {
+#define FILTER_KOEF 0.1
+
+        float d = abs(compass_len-len)/(compass_len+len);
+        if(d*100 > 25) { // difference more than 50% from mean value
+            printf("\ncompass len error: mean %f got %f\n", compass_len, len );
+            ret= false;
+            float k = FILTER_KOEF / (d*10); // 2.5 and more, so one bad sample never change mean more than 4%
+            compass_len = compass_len * (1-k) + len*k; // complimentary filter 1/k on bad samples
+        } else {
+            compass_len = compass_len * (1-FILTER_KOEF) + len*FILTER_KOEF; // complimentary filter 1/10 on good samples
+        }
+    }
+    
+    if(ret) {
+
+        // rotate raw_field from sensor frame to body frame
+        rotate_field(raw_field, _compass_instance);
+
+        // publish raw_field (uncorrected point sample) for calibration use
+        publish_raw_field(raw_field, _compass_instance);
+
+        // correct raw_field for known errors
+        correct_field(raw_field, _compass_instance);
+
+        if (_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
+            _mag_x_accum += raw_field.x;
+            _mag_y_accum += raw_field.y;
+            _mag_z_accum += raw_field.z;
+            _accum_count++;
+            if (_accum_count == 10) {
+                _mag_x_accum /= 2;
+                _mag_y_accum /= 2;
+                _mag_z_accum /= 2;
+                _accum_count /= 2;
+            }
+            _sem->give();
+        }
+    }
+}
+
+
+// Read Sensor data
+void AP_Compass_MAG3110::read()
+{
+    if (!_initialised) {
+        return;
+    }
+
+    if (!_sem->take_nonblocking()) {
+        return;
+    }
+    
+    if (_accum_count == 0) {
+        /* We're not ready to publish*/
+        _sem->give();
+        return;
+    }
+
+    Vector3f field(_mag_x_accum, _mag_y_accum, _mag_z_accum);
+    field /= _accum_count;
+
+    _accum_count = 0;
+    _mag_x_accum = _mag_y_accum = _mag_z_accum = 0;
+
+    _sem->give();    
+
+    publish_filtered_field(field, _compass_instance);
+}

libraries/AP_Compass/AP_Compass_MAG3110.h

@@ -0,0 +1,53 @@
+#pragma once
+
+#include <AP_Common/AP_Common.h>
+#include <AP_HAL/AP_HAL.h>
+#include <AP_HAL/Device.h>
+#include <AP_Math/AP_Math.h>
+
+#include "AP_Compass.h"
+#include "AP_Compass_Backend.h"
+
+
+#ifndef HAL_MAG3110_I2C_ADDR 
+ #define HAL_MAG3110_I2C_ADDR     0x0E
+#endif
+
+class AP_Compass_MAG3110 : public AP_Compass_Backend
+{
+public:
+    static AP_Compass_Backend *probe(Compass &compass,
+                                     AP_HAL::OwnPtr<AP_HAL::Device> dev,
+                                     enum Rotation = ROTATION_NONE);
+
+    static constexpr const char *name = "MAG3110";
+
+    void read() override;
+
+    ~AP_Compass_MAG3110() { }
+
+private:
+    AP_Compass_MAG3110(Compass &compass, AP_HAL::OwnPtr<AP_HAL::Device> dev);
+
+    bool init(enum Rotation rotation);
+
+    bool _read_sample();
+
+    bool _hardware_init();
+    void _update();
+
+    AP_HAL::OwnPtr<AP_HAL::Device> _dev;
+
+    int32_t _mag_x;
+    int32_t _mag_y;
+    int32_t _mag_z;
+
+    float _mag_x_accum;
+    float _mag_y_accum;
+    float _mag_z_accum;
+    uint8_t _accum_count;
+
+    uint8_t _compass_instance;
+    bool _initialised;
+    float compass_len;
+};