diff --git a/libraries/AP_Compass/AP_Compass_LIS3MDL.cpp b/libraries/AP_Compass/AP_Compass_LIS3MDL.cpp new file mode 100644 index 0000000000..99ba9578dd --- /dev/null +++ b/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 . + */ +/* + Driver by Andrew Tridgell, Nov 2016 + + thanks to Robert Dickenson and the PX4 team for register definitions + */ +#include "AP_Compass_LIS3MDL.h" + +#include +#include +#include +#include + +#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 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 _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(); +} diff --git a/libraries/AP_Compass/AP_Compass_LIS3MDL.h b/libraries/AP_Compass/AP_Compass_LIS3MDL.h new file mode 100644 index 0000000000..8c34de7804 --- /dev/null +++ b/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 . + */ +#pragma once + +#include +#include +#include +#include + +#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 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 dev, + bool force_external, + enum Rotation rotation); + + AP_HAL::OwnPtr 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; +};