From 8155c8b17890aa4ab1e1518e2e5e055df81491fc Mon Sep 17 00:00:00 2001 From: thomass Date: Fri, 1 Feb 2019 16:13:28 +0100 Subject: [PATCH] AP_Compass: added backend implementation for RM3100 compass --- libraries/AP_Compass/AP_Compass.cpp | 1 + libraries/AP_Compass/AP_Compass.h | 1 + libraries/AP_Compass/AP_Compass_Backend.h | 1 + libraries/AP_Compass/AP_Compass_RM3100.cpp | 253 +++++++++++++++++++++ libraries/AP_Compass/AP_Compass_RM3100.h | 60 +++++ 5 files changed, 316 insertions(+) create mode 100644 libraries/AP_Compass/AP_Compass_RM3100.cpp create mode 100644 libraries/AP_Compass/AP_Compass_RM3100.h diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp index a39e713d98..bf9a720c29 100644 --- a/libraries/AP_Compass/AP_Compass.cpp +++ b/libraries/AP_Compass/AP_Compass.cpp @@ -23,6 +23,7 @@ #endif #include "AP_Compass_MMC3416.h" #include "AP_Compass_MAG3110.h" +#include "AP_Compass_RM3100.h" #include "AP_Compass.h" #include "Compass_learn.h" diff --git a/libraries/AP_Compass/AP_Compass.h b/libraries/AP_Compass/AP_Compass.h index 3c60cb7602..63e1c505d2 100644 --- a/libraries/AP_Compass/AP_Compass.h +++ b/libraries/AP_Compass/AP_Compass.h @@ -379,6 +379,7 @@ private: DRIVER_SITL =13, DRIVER_MAG3110 =14, DRIVER_IST8308 = 15, + DRIVER_RM3100 =16, }; bool _driver_enabled(enum DriverType driver_type); diff --git a/libraries/AP_Compass/AP_Compass_Backend.h b/libraries/AP_Compass/AP_Compass_Backend.h index d3a3203c47..14ca416353 100644 --- a/libraries/AP_Compass/AP_Compass_Backend.h +++ b/libraries/AP_Compass/AP_Compass_Backend.h @@ -58,6 +58,7 @@ public: DEVTYPE_MAG3110 = 0x0E, DEVTYPE_SITL = 0x0F, DEVTYPE_IST8308 = 0x10, + DEVTYPE_RM3100 = 0x11, }; diff --git a/libraries/AP_Compass/AP_Compass_RM3100.cpp b/libraries/AP_Compass/AP_Compass_RM3100.cpp new file mode 100644 index 0000000000..bfb2891cdd --- /dev/null +++ b/libraries/AP_Compass/AP_Compass_RM3100.cpp @@ -0,0 +1,253 @@ +/* + * 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 Thomas Schumacher, Jan 2019 + Structure based on LIS3MDL driver + + */ +#include "AP_Compass_RM3100.h" + +#include +#include +#include +#include + +#define RM3100_POLL_REG 0x00 + +#define RM3100_CMM_REG 0x01 + +#define RM3100_CCX1_REG 0x04 +#define RM3100_CCX0_REG 0x05 +#define RM3100_CCY1_REG 0x06 +#define RM3100_CCY0_REG 0x07 +#define RM3100_CCZ1_REG 0x08 +#define RM3100_CCZ0_REG 0x09 + +#define RM3100_TMRC_REG 0x0B + +#define RM3100_MX2_REG 0x24 +#define RM3100_MX1_REG 0x25 +#define RM3100_MX0_REG 0x26 +#define RM3100_MY2_REG 0x27 +#define RM3100_MY1_REG 0x28 +#define RM3100_MY0_REG 0x29 +#define RM3100_MZ2_REG 0x2A +#define RM3100_MZ1_REG 0x2B +#define RM3100_MZ0_REG 0x2C + +#define RM3100_BIST_REG 0x33 +#define RM3100_STATUS_REG 0x34 +#define RM3100_HSHAKE_REG 0x34 +#define RM3100_REVID_REG 0x36 + +#define CCP0 0xC8 // Default Cycle Count +#define CCP1 0x00 +#define GAIN_CC50 20.0f // LSB/uT +#define GAIN_CC100 38.0f +#define GAIN_CC200 75.0f +#define UTESLA_TO_MGAUSS 10.0f // uT to mGauss conversion + +#define TMRC 0x94 // Update rate 150Hz +#define CMM 0x71 // read 3 axes and set data ready if 3 axes are ready + +extern const AP_HAL::HAL &hal; + +AP_Compass_Backend *AP_Compass_RM3100::probe(Compass &compass, + AP_HAL::OwnPtr dev, + bool force_external, + enum Rotation rotation) +{ + if (!dev) { + return nullptr; + } + AP_Compass_RM3100 *sensor = new AP_Compass_RM3100(compass, std::move(dev), force_external, rotation); + if (!sensor || !sensor->init()) { + delete sensor; + return nullptr; + } + + return sensor; +} + +AP_Compass_RM3100::AP_Compass_RM3100(Compass &compass, + AP_HAL::OwnPtr _dev, + bool _force_external, + enum Rotation _rotation) + : AP_Compass_Backend(compass) + , dev(std::move(_dev)) + , force_external(_force_external) + , rotation(_rotation) +{ +} + +bool AP_Compass_RM3100::init() +{ + if (!dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { + return false; + } + + // high retries for init + dev->set_retries(10); + + // maybe add a test for revision id value here and goto fail if not correct + // it is not clear if revision id is the same for all compasses, in the same manner as a whoami + + dev->setup_checked_registers(8); + + dev->write_register(RM3100_TMRC_REG, TMRC, true); // cycle count z + dev->write_register(RM3100_CMM_REG, CMM, false); // CMM configuration + dev->write_register(RM3100_CCX1_REG, CCP1, true); // cycle count x + dev->write_register(RM3100_CCX0_REG, CCP0, true); // cycle count x + dev->write_register(RM3100_CCY1_REG, CCP1, true); // cycle count y + dev->write_register(RM3100_CCY0_REG, CCP0, true); // cycle count y + dev->write_register(RM3100_CCZ1_REG, CCP1, true); // cycle count z + dev->write_register(RM3100_CCZ0_REG, CCP0, true); // cycle count z + + _scaler = (1 / GAIN_CC200) * UTESLA_TO_MGAUSS; // has to be changed if using a different cycle count + + // lower retries for run + dev->set_retries(3); + + dev->get_semaphore()->give(); + + /* register the compass instance in the frontend */ + compass_instance = register_compass(); + + printf("Found a RM3100 at address 0x%x as compass %u\n", dev->get_bus_address(), compass_instance); + + set_rotation(compass_instance, rotation); + + if (force_external) { + set_external(compass_instance, true); + } + + dev->set_device_type(DEVTYPE_RM3100); + set_dev_id(compass_instance, dev->get_bus_id()); + + // call timer() at 80Hz + dev->register_periodic_callback(1000000U/80U, + FUNCTOR_BIND_MEMBER(&AP_Compass_RM3100::timer, void)); + + return true; + +//fail: +// dev->get_semaphore()->give(); +// return false; +} + +void AP_Compass_RM3100::timer() +{ + struct PACKED { + uint8_t magx_2; + uint8_t magx_1; + uint8_t magx_0; + uint8_t magy_2; + uint8_t magy_1; + uint8_t magy_0; + uint8_t magz_2; + uint8_t magz_1; + uint8_t magz_0; + } data; + Vector3f field; + + uint32_t magx = 0; + uint32_t magy = 0; + uint32_t magz = 0; + + // check data ready on 3 axis + uint8_t status; + if (!dev->read_registers(RM3100_STATUS_REG, (uint8_t *)&status, 1)) { + goto check_registers; + } + + if (!(status & 0x80)) { + // data not available yet + goto check_registers; + } + + if (!dev->read_registers(RM3100_MX2_REG, (uint8_t *)&data, sizeof(data))) { + goto check_registers; + } + + + + magx = ((uint32_t)data.magx_2 << 16) | ((uint32_t)data.magx_1 << 8) | (uint32_t)data.magx_0; + magy = ((uint32_t)data.magy_2 << 16) | ((uint32_t)data.magy_1 << 8) | (uint32_t)data.magy_0; + magz = ((uint32_t)data.magz_2 << 16) | ((uint32_t)data.magz_1 << 8) | (uint32_t)data.magz_0; + + if (magx & 0x800000) { + magx |= 0xFF000000; + } + if (magy & 0x800000) { + magy |= 0xFF000000; + } + if (magz & 0x800000) { + magz |= 0xFF000000; + } + + field((int32_t)magx * _scaler, (int32_t)magy * _scaler, (int32_t)magz * _scaler); + + // 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, compass_instance); + + // correct raw_field for known errors + correct_field(field, compass_instance); + + if (_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { + accum += field; + accum_count++; + _sem->give(); + } + +check_registers: + dev->check_next_register(); +} + +void AP_Compass_RM3100::read() +{ + if (!_sem->take_nonblocking()) { + return; + } + if (accum_count == 0) { + _sem->give(); + return; + } + +#if 0 + // debugging code for sample rate + static uint32_t lastt; + static uint32_t total; + total += accum_count; + uint32_t now = AP_HAL::micros(); + float dt = (now - lastt) * 1.0e-6; + if (dt > 1) { + printf("%u %f samples RM3100 \n", total, dt); + lastt = now; + total = 0; + } +#endif + + accum /= accum_count; + + publish_filtered_field(accum, compass_instance); + + accum.zero(); + accum_count = 0; + + _sem->give(); +} diff --git a/libraries/AP_Compass/AP_Compass_RM3100.h b/libraries/AP_Compass/AP_Compass_RM3100.h new file mode 100644 index 0000000000..23541fb1b6 --- /dev/null +++ b/libraries/AP_Compass/AP_Compass_RM3100.h @@ -0,0 +1,60 @@ +/* + * 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_RM3100_I2C_ADDR +# define HAL_COMPASS_RM3100_I2C_ADDR 0x20 +#endif + +class AP_Compass_RM3100 : public AP_Compass_Backend +{ +public: + static AP_Compass_Backend *probe(Compass &compass, + AP_HAL::OwnPtr dev, + bool force_external = false, + enum Rotation rotation = ROTATION_NONE); + + void read() override; + + static constexpr const char *name = "RM3100"; + +private: + AP_Compass_RM3100(Compass &compass, AP_HAL::OwnPtr dev, + bool force_external, + enum Rotation rotation); + + AP_HAL::OwnPtr dev; + + /** + * Device periodic callback to read data from the sensor. + */ + bool init(); + void timer(); + + uint8_t compass_instance; + Vector3f accum; + uint16_t accum_count; + bool force_external; + enum Rotation rotation; + float _scaler = 1.0; +};