diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp index 5814fce96b..653b434216 100644 --- a/libraries/AP_Compass/AP_Compass.cpp +++ b/libraries/AP_Compass/AP_Compass.cpp @@ -20,6 +20,7 @@ #if HAL_WITH_UAVCAN #include "AP_Compass_UAVCAN.h" #endif +#include "AP_Compass_MMC3416.h" #include "AP_Compass.h" extern AP_HAL::HAL& hal; diff --git a/libraries/AP_Compass/AP_Compass_Backend.h b/libraries/AP_Compass/AP_Compass_Backend.h index f7c7ba0818..60c01b7434 100644 --- a/libraries/AP_Compass/AP_Compass_Backend.h +++ b/libraries/AP_Compass/AP_Compass_Backend.h @@ -60,6 +60,7 @@ public: DEVTYPE_AK09916 = 0x09, DEVTYPE_IST8310 = 0x0A, DEVTYPE_ICM20948 = 0x0B, + DEVTYPE_MMC3416 = 0x0C, }; diff --git a/libraries/AP_Compass/AP_Compass_MMC3416.cpp b/libraries/AP_Compass/AP_Compass_MMC3416.cpp new file mode 100644 index 0000000000..ee98f5aa64 --- /dev/null +++ b/libraries/AP_Compass/AP_Compass_MMC3416.cpp @@ -0,0 +1,172 @@ +/* + * 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 Andrew Tridgell, Nov 2016 + */ +#include "AP_Compass_MMC3416.h" + +#include +#include +#include +#include + +extern const AP_HAL::HAL &hal; + +#define REG_PRODUCT_ID 0x20 +#define REG_XOUT_L 0x00 +#define REG_STATUS 0x06 +#define REG_CONTROL0 0x07 +#define REG_CONTROL1 0x08 + +AP_Compass_Backend *AP_Compass_MMC3416::probe(Compass &compass, + AP_HAL::OwnPtr dev, + bool force_external, + enum Rotation rotation) +{ + if (!dev) { + return nullptr; + } + AP_Compass_MMC3416 *sensor = new AP_Compass_MMC3416(compass, std::move(dev), force_external, rotation); + if (!sensor || !sensor->init()) { + delete sensor; + return nullptr; + } + + return sensor; +} + +AP_Compass_MMC3416::AP_Compass_MMC3416(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_MMC3416::init() +{ + if (!dev->get_semaphore()->take(0)) { + return false; + } + + dev->set_retries(10); + + uint8_t whoami; + if (!dev->read_registers(REG_PRODUCT_ID, &whoami, 1) || + whoami != 0x06) { + // not a MMC3416 + goto fail; + } + + dev->setup_checked_registers(2); + + // reset sensor + dev->write_register(REG_CONTROL1, 0x80); + hal.scheduler->delay(10); + + dev->write_register(REG_CONTROL0, 0x0E, true); // continuous 50Hz + dev->write_register(REG_CONTROL1, 0x00, true); // 16 bit, 7.92ms + + dev->get_semaphore()->give(); + + /* register the compass instance in the frontend */ + compass_instance = register_compass(); + + printf("Found a MMC3416 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_MMC3416); + set_dev_id(compass_instance, dev->get_bus_id()); + + dev->set_retries(1); + + // call timer() at 50Hz + dev->register_periodic_callback(20000, + FUNCTOR_BIND_MEMBER(&AP_Compass_MMC3416::timer, bool)); + + return true; + +fail: + dev->get_semaphore()->give(); + return false; +} + +bool AP_Compass_MMC3416::timer() +{ + struct PACKED { + uint16_t magx; + uint16_t magy; + uint16_t magz; + } data; + const uint16_t zero_offset = 32768; // 16 bit mode + const uint16_t sensitivity = 2048; // counts per Gauss, 16 bit mode + const float counts_to_milliGauss = 1.0e3f / sensitivity; + Vector3f field; + + if (!dev->read_registers(REG_XOUT_L, (uint8_t *)&data, sizeof(data))) { + goto check_registers; + } + + // this assumes 16 bit output, which gives zero of 32768 + field(float(data.magx) - zero_offset, float(data.magy) - zero_offset, float(data.magz) - zero_offset); + field *= counts_to_milliGauss; + + /* 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(); + } + +check_registers: + dev->check_next_register(); + return true; +} + +void AP_Compass_MMC3416::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(); +} diff --git a/libraries/AP_Compass/AP_Compass_MMC3416.h b/libraries/AP_Compass/AP_Compass_MMC3416.h new file mode 100644 index 0000000000..e334e21d32 --- /dev/null +++ b/libraries/AP_Compass/AP_Compass_MMC3416.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 . + */ +#pragma once + +#include +#include +#include +#include + +#include "AP_Compass.h" +#include "AP_Compass_Backend.h" + +#ifndef HAL_COMPASS_MMC3416_I2C_ADDR +# define HAL_COMPASS_MMC3416_I2C_ADDR 0x30 +#endif + +class AP_Compass_MMC3416 : 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 = "MMC3416"; + +private: + AP_Compass_MMC3416(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(); + bool timer(); + + uint8_t compass_instance; + Vector3f accum; + uint16_t accum_count; + bool force_external; + enum Rotation rotation; +};