diff --git a/libraries/AP_Compass/AP_Compass_Backend.h b/libraries/AP_Compass/AP_Compass_Backend.h index 00df86ead7..c2c0c91aec 100644 --- a/libraries/AP_Compass/AP_Compass_Backend.h +++ b/libraries/AP_Compass/AP_Compass_Backend.h @@ -55,6 +55,7 @@ public: DEVTYPE_LSM9DS1 = 0x06, DEVTYPE_LIS3MDL = 0x08, DEVTYPE_AK09916 = 0x09, + DEVTYPE_IST8310 = 0x0A, }; diff --git a/libraries/AP_Compass/AP_Compass_IST8310.cpp b/libraries/AP_Compass/AP_Compass_IST8310.cpp new file mode 100644 index 0000000000..3889aafcb1 --- /dev/null +++ b/libraries/AP_Compass/AP_Compass_IST8310.cpp @@ -0,0 +1,218 @@ +/* + * Copyright (C) 2016 Emlid Ltd. All rights reserved. + * + * 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 Georgii Staroselskii, Sep 2016 + */ + +#include "AP_Compass_IST8310.h" + +#include + +#include +#include + +#include +#include + +#define WAI_REG 0x0 +#define DEVICE_ID 0x10 + +#define CNTL1_REG 0xA +#define SINGLE_MEASUREMENT_MODE 0x1 +#define ODR_100HZ 0x6 + +#define STAT1_REG 0x2 +#define DATA_RDY 0x1 + +#define AVGCNTL_REG 0x41 +#define AVERAGING_Y_BY_2 0x20 +#define AVERAGING_XZ_BY_4 0x04 + +#define PDCNTL_REG 0x42 +#define NORMAL_PULSE_DURATION 0xC0 + +extern const AP_HAL::HAL &hal; + +AP_Compass_Backend *AP_Compass_IST8310::probe(Compass &compass, + AP_HAL::OwnPtr dev, + enum Rotation rotation + ) +{ + AP_Compass_IST8310 *sensor = new AP_Compass_IST8310(compass, std::move(dev), rotation); + if (!sensor || !sensor->init()) { + delete sensor; + return nullptr; + } + + return sensor; +} + +AP_Compass_IST8310::AP_Compass_IST8310(Compass &compass, + AP_HAL::OwnPtr dev, + enum Rotation _rotation) + : AP_Compass_Backend(compass) + , _dev(std::move(dev)) + , rotation(_rotation) +{ +} + +bool AP_Compass_IST8310::init() +{ + if (!_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { + hal.console->printf("IST8310: Unable to get bus semaphore\n"); + goto fail_sem; + } + + if (!check_id()) { + hal.console->printf("IST8310: Not a IST device\n"); + goto fail; + } + + if (!setup_sampling()) { + goto fail; + } + + start_conversion(); + + _dev->get_semaphore()->give(); + + printf("%s found on bus %u address 0x%02x\n", name, _dev->bus_num(), _dev->get_bus_address()); + + compass_instance = register_compass(); + + hal.console->printf("Found a IST8310 on 0x%x as compass %u\n", _dev->get_bus_id(), compass_instance); + + set_rotation(compass_instance, rotation); + + /* register the compass instance in the frontend */ + _dev->set_device_type(DEVTYPE_IST8310); + set_dev_id(compass_instance, _dev->get_bus_id()); + + _dev->register_periodic_callback(10 * USEC_PER_MSEC, FUNCTOR_BIND_MEMBER(&AP_Compass_IST8310::timer, bool)); + + return true; + +fail: + _dev->get_semaphore()->give(); +fail_sem: + return false; +} + +bool AP_Compass_IST8310::check_id() +{ + uint8_t val = 0; + + if (!_dev->read_registers(WAI_REG, &val, 1)) { + return false; + } + + if (val != DEVICE_ID) { + return false; + } + + return true; +} + +bool AP_Compass_IST8310::setup_sampling() +{ + bool ret = _dev->write_register(AVGCNTL_REG, AVERAGING_Y_BY_2 | AVERAGING_XZ_BY_4); + + return ret && _dev->write_register(PDCNTL_REG, NORMAL_PULSE_DURATION); +} + +void AP_Compass_IST8310::start_conversion() +{ + _dev->write_register(CNTL1_REG, + SINGLE_MEASUREMENT_MODE); +} + +bool AP_Compass_IST8310::timer() +{ + + bool ret = false; + + struct PACKED { + uint8_t status; + le16_t rx; + le16_t ry; + le16_t rz; + } buffer; + + + ret = _dev->read_registers(STAT1_REG, (uint8_t *) &buffer, sizeof(buffer)); + + if (!ret) { + return true; /* We're going to be back on the next iteration either way */ + } + + auto status = buffer.status; + + if (!(status & 0x01)) { + /* We're not ready yet */ + return true; + } + + auto x = static_cast(le16toh(buffer.rx)); + auto y = static_cast(le16toh(buffer.ry)); + auto z = static_cast(le16toh(buffer.rz)); + + /* convert uT to milligauss */ + Vector3f field = Vector3f{x * 3.0f, y * 3.0f, z * 3.0f}; + + /* rotate raw_field from sensor frame to body frame */ + rotate_field(field, compass_instance); + + const auto now = AP_HAL::micros(); + + /* publish raw_field (uncorrected point sample) for calibration use */ + publish_raw_field(field, now, compass_instance); + + /* correct raw_field for known errors */ + correct_field(field, compass_instance); + + if (_sem->take(0)) { + mag_accum += field; + accum_count++; + _sem->give(); + } + + + start_conversion(); + + return true; +} + +void AP_Compass_IST8310::read() +{ + if (!_sem->take_nonblocking()) { + return; + } + + if (accum_count == 0) { + _sem->give(); + return; + } + + Vector3f field(mag_accum); + field /= accum_count; + mag_accum.zero(); + accum_count = 0; + + _sem->give(); + + publish_filtered_field(field, compass_instance); + +} diff --git a/libraries/AP_Compass/AP_Compass_IST8310.h b/libraries/AP_Compass/AP_Compass_IST8310.h new file mode 100644 index 0000000000..93959b0f45 --- /dev/null +++ b/libraries/AP_Compass/AP_Compass_IST8310.h @@ -0,0 +1,57 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* + * Copyright (C) 2016 Emlid Ltd. All rights reserved. + * + * 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_IST8310 : public AP_Compass_Backend +{ +public: + static AP_Compass_Backend *probe(Compass &compass, + AP_HAL::OwnPtr dev, + enum Rotation rotation = ROTATION_NONE); + + void read() override; + + static constexpr const char *name = "IST8310"; + +private: + AP_Compass_IST8310(Compass &compass, + AP_HAL::OwnPtr dev, + enum Rotation rotation); + + bool timer(); + bool init(); + bool check_id(); + bool setup_sampling(); + void start_conversion(); + + AP_HAL::OwnPtr _dev; + + Vector3f mag_accum = Vector3f(); + uint32_t accum_count = 0; + uint8_t compass_instance; + enum Rotation rotation; + +};