lis3mdl.cpp

Go to the documentation of this file.

/****************************************************************************
 *
 *   Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/**
 * @file lis3mdl.cpp
 *
 * Driver for the LIS3MDL magnetometer connected via I2C or SPI.
 *
 * Based on the hmc5883 driver.
 */

#include <px4_platform_common/time.h>
#include "lis3mdl.h"

LIS3MDL::LIS3MDL(device::Device *interface, const char *path, enum Rotation rotation) :
    CDev("LIS3MDL", path),
    ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id())),
    _interface(interface),
    _reports(nullptr),
    _scale{},
    _last_report{},
    _mag_topic(nullptr),
    _comms_errors(perf_alloc(PC_COUNT, "lis3mdl_comms_errors")),
    _conf_errors(perf_alloc(PC_COUNT, "lis3mdl_conf_errors")),
    _range_errors(perf_alloc(PC_COUNT, "lis3mdl_range_errors")),
    _sample_perf(perf_alloc(PC_ELAPSED, "lis3mdl_read")),
    _calibrated(false),
    _continuous_mode_set(false),
    _mode(CONTINUOUS),
    _rotation(rotation),
    _measure_interval(0),
    _class_instance(-1),
    _orb_class_instance(-1),
    _range_ga(4.0f),
    _range_scale(0),                // default range scale from counts to gauss */
    _check_state_cnt(0),
    _cntl_reg1(
        CNTL_REG1_DEFAULT),     // 1 11 111 0 0 | temp-en, ultra high performance (XY), fast_odr disabled, self test disabled
    _cntl_reg2(CNTL_REG2_DEFAULT),  // 4 gauss FS range, reboot settings default
    _cntl_reg3(CNTL_REG3_DEFAULT),  // operating mode CONTINUOUS!
    _cntl_reg4(CNTL_REG4_DEFAULT),  // Z-axis ultra high performance mode
    _cntl_reg5(CNTL_REG5_DEFAULT),  // fast read disabled, continious update disabled (block data update)
    _range_bits(0),
    _temperature_counter(0),
    _temperature_error_count(0)
{
    // set the device type from the interface
    _device_id.devid_s.bus_type = _interface->get_device_bus_type();
    _device_id.devid_s.bus = _interface->get_device_bus();
    _device_id.devid_s.address = _interface->get_device_address();
    _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_LIS3MDL;

    // default scaling
    _scale.x_offset = 0;
    _scale.x_scale = 1.0f;
    _scale.y_offset = 0;
    _scale.y_scale = 1.0f;
    _scale.z_offset = 0;
    _scale.z_scale = 1.0f;
}

LIS3MDL::~LIS3MDL()
{
    /* make sure we are truly inactive */
    stop();

    if (_mag_topic != nullptr) {
        orb_unadvertise(_mag_topic);
    }

    if (_reports != nullptr) {
        delete _reports;
    }

    if (_class_instance != -1) {
        unregister_class_devname(MAG_BASE_DEVICE_PATH, _class_instance);
    }

    // free perf counters
    perf_free(_sample_perf);
    perf_free(_comms_errors);
    perf_free(_range_errors);
    perf_free(_conf_errors);
}

int
LIS3MDL::calibrate(struct file *file_pointer, unsigned enable)
{
    struct mag_report report;
    ssize_t sz;
    int ret = 1;
    uint8_t num_samples = 10;

    // XXX do something smarter here
    int fd = (int)enable;

    float sum_excited[3] = {0.0f, 0.0f, 0.0f};
    float sum_non_excited[3] = {0.0f, 0.0f, 0.0f};

    /* start the sensor polling at 50 Hz */
    if (ioctl(file_pointer, SENSORIOCSPOLLRATE, 50) != OK) {
        warn("FAILED: SENSORIOCSPOLLRATE 50Hz");
        ret = 1;
        goto out;
    }

    /* Set to 12 Gauss */
    if (ioctl(file_pointer, MAGIOCSRANGE, 12) != OK) {
        PX4_WARN("FAILED: MAGIOCSRANGE 12 Ga");
        ret = 1;
        goto out;
    }

    px4_usleep(20000);

    /* discard 10 samples to let the sensor settle */
    for (uint8_t i = 0; i < num_samples; i++) {
        struct pollfd fds;

        /* wait for data to be ready */
        fds.fd = fd;
        fds.events = POLLIN;
        ret = ::poll(&fds, 1, 2000);

        if (ret != 1) {
            warn("ERROR: TIMEOUT 1");
            goto out;
        }

        /* now go get it */
        sz = ::read(fd, &report, sizeof(report));

        if (sz != sizeof(report)) {
            warn("ERROR: READ 1");
            ret = -EIO;
            goto out;
        }
    }

    /* read the sensor up to 10x */
    for (uint8_t i = 0; i < num_samples; i++) {
        struct pollfd fds;

        /* wait for data to be ready */
        fds.fd = fd;
        fds.events = POLLIN;
        ret = ::poll(&fds, 1, 2000);

        if (ret != 1) {
            warn("ERROR: TIMEOUT 2");
            goto out;
        }

        /* now go get it */
        sz = ::read(fd, &report, sizeof(report));

        if (sz != sizeof(report)) {
            warn("ERROR: READ 2");
            ret = -EIO;
            goto out;
        }

        sum_non_excited[0] += report.x;
        sum_non_excited[1] += report.y;
        sum_non_excited[2] += report.z;
    }

    sum_non_excited[0] /= num_samples;
    sum_non_excited[1] /= num_samples;
    sum_non_excited[2] /= num_samples;

    /* excite strap and take measurements */
    if (ioctl(file_pointer, MAGIOCEXSTRAP, 1) != OK) {
        PX4_WARN("FAILED: MAGIOCEXSTRAP 1");
        ret = 1;
        goto out;
    }

    px4_usleep(60000);

    /* discard 10 samples to let the sensor settle */
    for (uint8_t i = 0; i < num_samples; i++) {
        struct pollfd fds;

        /* wait for data to be ready */
        fds.fd = fd;
        fds.events = POLLIN;
        ret = ::poll(&fds, 1, 2000);

        if (ret != 1) {
            warn("ERROR: TIMEOUT 1");
            goto out;
        }

        /* now go get it */
        sz = ::read(fd, &report, sizeof(report));

        if (sz != sizeof(report)) {
            warn("ERROR: READ 1");
            ret = -EIO;
            goto out;
        }
    }

    /* read the sensor up to 10x */
    for (uint8_t i = 0; i < 10; i++) {
        struct pollfd fds;

        /* wait for data to be ready */
        fds.fd = fd;
        fds.events = POLLIN;
        ret = ::poll(&fds, 1, 2000);

        if (ret != 1) {
            warn("ERROR: TIMEOUT 2");
            goto out;
        }

        /* now go get it */
        sz = ::read(fd, &report, sizeof(report));

        if (sz != sizeof(report)) {
            warn("ERROR: READ 2");
            ret = -EIO;
            goto out;
        }

        sum_excited[0] += report.x;
        sum_excited[1] += report.y;
        sum_excited[2] += report.z;
    }

    sum_excited[0] /= num_samples;
    sum_excited[1] /= num_samples;
    sum_excited[2] /= num_samples;

    if (1.0f < fabsf(sum_excited[0] - sum_non_excited[0]) && fabsf(sum_excited[0] - sum_non_excited[0]) < 3.0f &&
        1.0f < fabsf(sum_excited[1] - sum_non_excited[1]) && fabsf(sum_excited[1] - sum_non_excited[1]) < 3.0f &&
        0.1f < fabsf(sum_excited[2] - sum_non_excited[2]) && fabsf(sum_excited[2] - sum_non_excited[2]) < 1.0f) {
        ret = OK;

    } else {
        ret = -EIO;
        goto out;
    }

out:

    /* set back to normal mode */
    set_range(4);
    set_default_register_values();

    px4_usleep(20000);

    return ret;
}

int
LIS3MDL::check_offset()
{
    bool offset_valid;

    if ((-2.0f * FLT_EPSILON < _scale.x_offset && _scale.x_offset < 2.0f * FLT_EPSILON) &&
        (-2.0f * FLT_EPSILON < _scale.y_offset && _scale.y_offset < 2.0f * FLT_EPSILON) &&
        (-2.0f * FLT_EPSILON < _scale.z_offset && _scale.z_offset < 2.0f * FLT_EPSILON)) {
        /* offset is zero */
        offset_valid = false;

    } else {
        offset_valid = true;
    }

    /* return 0 if calibrated, 1 else */
    return !offset_valid;
}

int
LIS3MDL::check_scale()
{
    bool scale_valid;

    if ((-FLT_EPSILON + 1.0f < _scale.x_scale && _scale.x_scale < FLT_EPSILON + 1.0f) &&
        (-FLT_EPSILON + 1.0f < _scale.y_scale && _scale.y_scale < FLT_EPSILON + 1.0f) &&
        (-FLT_EPSILON + 1.0f < _scale.z_scale && _scale.z_scale < FLT_EPSILON + 1.0f)) {
        /* scale is one */
        scale_valid = false;

    } else {
        scale_valid = true;
    }

    /* return 0 if calibrated, 1 else */
    return !scale_valid;
}

int
LIS3MDL::collect()
{
#pragma pack(push, 1)
    struct {
        uint8_t x[2];
        uint8_t y[2];
        uint8_t z[2];
    }       lis_report;

    struct {
        int16_t x;
        int16_t y;
        int16_t z;
        int16_t t;
    } report;
#pragma pack(pop)

    int     ret = 0;
    uint8_t buf_rx[2] = {0};

    float xraw_f;
    float yraw_f;
    float zraw_f;

    struct mag_report new_mag_report;
    bool sensor_is_onboard = false;

    perf_begin(_sample_perf);

    new_mag_report.timestamp = hrt_absolute_time();
    new_mag_report.error_count = perf_event_count(_comms_errors);
    new_mag_report.scaling = _range_scale;
    new_mag_report.device_id = _device_id.devid;

    ret = _interface->read(ADDR_OUT_X_L, (uint8_t *)&lis_report, sizeof(lis_report));

    /**
     * Silicon Bug: the X axis will be read instead of the temperature registers if you do a sequential read through XYZ.
     * The temperature registers must be addressed directly.
     */
    ret = _interface->read(ADDR_OUT_T_L, (uint8_t *)&buf_rx, sizeof(buf_rx));

    if (ret != OK) {
        perf_count(_comms_errors);
        PX4_WARN("Register read error.");
        return ret;
    }

    report.x = (int16_t)((lis_report.x[1] << 8) | lis_report.x[0]);
    report.y = (int16_t)((lis_report.y[1] << 8) | lis_report.y[0]);
    report.z = (int16_t)((lis_report.z[1] << 8) | lis_report.z[0]);

    report.t = (int16_t)((buf_rx[1] << 8) | buf_rx[0]);

    float temperature = report.t;
    new_mag_report.temperature = 25.0f + (temperature / 8.0f);

    // XXX revisit for SPI part, might require a bus type IOCTL

    unsigned dummy = 0;
    sensor_is_onboard = !_interface->ioctl(MAGIOCGEXTERNAL, dummy);
    new_mag_report.is_external = !sensor_is_onboard;

    /**
     * RAW outputs
     */
    new_mag_report.x_raw = report.x;
    new_mag_report.y_raw = report.y;
    new_mag_report.z_raw = report.z;

    xraw_f = report.x;
    yraw_f = report.y;
    zraw_f = report.z;

    // apply user specified rotation
    rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);

    new_mag_report.x = ((xraw_f * _range_scale) - _scale.x_offset) * _scale.x_scale;
    /* flip axes and negate value for y */
    new_mag_report.y = ((yraw_f * _range_scale) - _scale.y_offset) * _scale.y_scale;
    /* z remains z */
    new_mag_report.z = ((zraw_f * _range_scale) - _scale.z_offset) * _scale.z_scale;

    if (!(_pub_blocked)) {

        if (_mag_topic != nullptr) {
            /* publish it */
            orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_mag_report);

        } else {
            _mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &new_mag_report,
                             &_orb_class_instance, (sensor_is_onboard) ? ORB_PRIO_HIGH : ORB_PRIO_MAX);

            if (_mag_topic == nullptr) {
                DEVICE_DEBUG("ADVERT FAIL");
            }
        }
    }

    _last_report = new_mag_report;

    /* post a report to the ring */
    _reports->force(&new_mag_report);

    /* notify anyone waiting for data */
    poll_notify(POLLIN);

    ret = OK;

    perf_end(_sample_perf);
    return ret;
}

void
LIS3MDL::Run()
{
    /* _measure_interval == 0  is used as _task_should_exit */
    if (_measure_interval == 0) {
        return;
    }

    /* Collect last measurement at the start of every cycle */
    if (collect() != OK) {
        DEVICE_DEBUG("collection error");
        /* restart the measurement state machine */
        start();
        return;
    }


    if (measure() != OK) {
        DEVICE_DEBUG("measure error");
    }

    if (_measure_interval > 0) {
        /* schedule a fresh cycle call when the measurement is done */
        ScheduleDelayed(LIS3MDL_CONVERSION_INTERVAL);
    }
}

int
LIS3MDL::init()
{
    int ret = PX4_ERROR;

    ret = CDev::init();

    if (ret != OK) {
        DEVICE_DEBUG("CDev init failed");
        return ret;
    }

    /* allocate basic report buffers */
    _reports = new ringbuffer::RingBuffer(2, sizeof(mag_report));

    if (_reports == nullptr) {
        return PX4_ERROR;
    }

    /* reset the device configuration */
    reset();

    _class_instance = register_class_devname(MAG_BASE_DEVICE_PATH);

    return PX4_OK;
}

int
LIS3MDL::ioctl(struct file *file_pointer, int cmd, unsigned long arg)
{
    unsigned dummy = 0;

    switch (cmd) {
    case SENSORIOCSPOLLRATE: {
            switch (arg) {

            /* zero would be bad */
            case 0:
                return -EINVAL;

            case SENSOR_POLLRATE_DEFAULT: {
                    /* do we need to start internal polling? */
                    bool not_started = (_measure_interval == 0);

                    /* set interval for next measurement to minimum legal value */
                    _measure_interval = (LIS3MDL_CONVERSION_INTERVAL);

                    /* if we need to start the poll state machine, do it */
                    if (not_started) {
                        start();
                    }

                    return PX4_OK;
                }

            /* Uses arg (hz) for a custom poll rate */
            default: {
                    /* do we need to start internal polling? */
                    bool not_started = (_measure_interval == 0);

                    /* convert hz to tick interval via microseconds */
                    unsigned interval = (1000000 / arg);

                    /* update interval for next measurement */
                    _measure_interval = interval;

                    /* if we need to start the poll state machine, do it */
                    if (not_started) {
                        start();
                    }

                    return PX4_OK;
                }
            }
        }

    case SENSORIOCRESET:
        return reset();

    case MAGIOCSRANGE:
        return set_range(arg);

    case MAGIOCSSCALE:
        /* set new scale factors */
        memcpy(&_scale, (struct mag_calibration_s *)arg, sizeof(_scale));
        return 0;

    case MAGIOCGSCALE:
        /* copy out scale factors */
        memcpy((struct mag_calibration_s *)arg, &_scale, sizeof(_scale));
        return 0;

    case MAGIOCCALIBRATE:
        return calibrate(file_pointer, arg);

    case MAGIOCEXSTRAP:
        return set_excitement(arg);

    case MAGIOCGEXTERNAL:
        DEVICE_DEBUG("MAGIOCGEXTERNAL in main driver");
        return _interface->ioctl(cmd, dummy);

    case DEVIOCGDEVICEID:
        return _interface->ioctl(cmd, dummy);

    default:
        /* give it to the superclass */
        return CDev::ioctl(file_pointer, cmd, arg);
    }
}

int
LIS3MDL::measure()
{
    int ret = 0;

    /* Send the command to begin a measurement. */
    if ((_mode == CONTINUOUS) && !_continuous_mode_set) {
        ret = write_reg(ADDR_CTRL_REG3, MODE_REG_CONTINOUS_MODE);
        _continuous_mode_set = true;

    } else if (_mode == SINGLE) {
        ret = write_reg(ADDR_CTRL_REG3, MODE_REG_SINGLE_MODE);
        _continuous_mode_set = false;
    }


    if (ret != OK) {
        perf_count(_comms_errors);
    }

    return ret;
}

void
LIS3MDL::print_info()
{
    perf_print_counter(_sample_perf);
    perf_print_counter(_comms_errors);
    PX4_INFO("poll interval:  %u", _measure_interval);
    print_message(_last_report);
    _reports->print_info("report queue");
}

int
LIS3MDL::reset()
{
    int ret = 0;

    ret = set_default_register_values();

    if (ret != OK) {
        return PX4_ERROR;
    }

    ret = set_range(_range_ga);

    if (ret != OK) {
        return PX4_ERROR;
    }

    return PX4_OK;
}

int
LIS3MDL::read(struct file *file_pointer, char *buffer, size_t buffer_len)
{
    unsigned count = buffer_len / sizeof(struct mag_report);
    struct mag_report *mag_buf = reinterpret_cast<struct mag_report *>(buffer);
    int ret = 0;

    /* buffer must be large enough */
    if (count < 1) {
        return -ENOSPC;
    }

    /* if automatic measurement is enabled */
    if (_measure_interval > 0) {
        /*
         * While there is space in the caller's buffer, and reports, copy them.
         * Note that we may be pre-empted by the workq thread while we are doing this;
         * we are careful to avoid racing with them.
         */
        while (count--) {
            if (_reports->get(mag_buf)) {
                ret += sizeof(struct mag_report);
                mag_buf++;
            }
        }

        /* if there was no data, warn the caller */
        return ret ? ret : -EAGAIN;
    }

    /* manual measurement - run one conversion */
    /* XXX really it'd be nice to lock against other readers here */
    do {
        _reports->flush();

        /* trigger a measurement */
        if (measure() != OK) {
            ret = -EIO;
            break;
        }

        /* wait for it to complete */
        px4_usleep(LIS3MDL_CONVERSION_INTERVAL);

        /* run the collection phase */
        if (collect() != OK) {
            ret = -EIO;
            break;
        }

        if (_reports->get(mag_buf)) {
            ret = sizeof(struct mag_report);
        }
    } while (0);

    return ret;
}

int
LIS3MDL::set_default_register_values()
{
    write_reg(ADDR_CTRL_REG1, CNTL_REG1_DEFAULT);
    write_reg(ADDR_CTRL_REG2, CNTL_REG2_DEFAULT);
    write_reg(ADDR_CTRL_REG3, CNTL_REG3_DEFAULT);
    write_reg(ADDR_CTRL_REG4, CNTL_REG4_DEFAULT);
    write_reg(ADDR_CTRL_REG5, CNTL_REG5_DEFAULT);

    return PX4_OK;
}

int
LIS3MDL::set_excitement(unsigned enable)
{
    int ret;
    /* arm the excitement strap */
    ret = read_reg(ADDR_CTRL_REG1, _cntl_reg1);

    if (ret != OK) {
        perf_count(_comms_errors);
    }

    _cntl_reg1 &= ~0x01; // reset previous excitement mode

    if (enable > 0) {
        _cntl_reg1 |= 0x01;
    }

    ::printf("set_excitement enable=%d cntl1=0x%x\n", (int)enable, (unsigned)_cntl_reg1);

    ret = write_reg(ADDR_CTRL_REG1, _cntl_reg1);

    if (ret != OK) {
        perf_count(_comms_errors);
    }

    uint8_t conf_reg_ret = 0;
    read_reg(ADDR_CTRL_REG1, conf_reg_ret);

    //print_info();

    return !(_cntl_reg1 == conf_reg_ret);
}

int
LIS3MDL::set_range(unsigned range)
{
    if (range <= 4) {
        _range_bits = 0x00;
        _range_scale = 1.0f / 6842.0f;
        _range_ga = 4.0f;

    } else if (range <= 8) {
        _range_bits = 0x01;
        _range_scale = 1.0f / 3421.0f;
        _range_ga = 8.0f;

    } else if (range <= 12) {
        _range_bits = 0x02;
        _range_scale = 1.0f / 2281.0f;
        _range_ga = 12.0f;

    } else {
        _range_bits = 0x03;
        _range_scale = 1.0f / 1711.0f;
        _range_ga = 16.0f;
    }

    int ret = 0;

    /*
     * Send the command to set the range
     */
    ret = write_reg(ADDR_CTRL_REG2, (_range_bits << 5));

    if (ret != OK) {
        perf_count(_comms_errors);
    }

    uint8_t range_bits_in = 0;
    ret = read_reg(ADDR_CTRL_REG2, range_bits_in);

    if (ret != OK) {
        perf_count(_comms_errors);
    }

    if (range_bits_in == (_range_bits << 5)) {
        return PX4_OK;

    } else {
        return PX4_ERROR;
    }
}

void
LIS3MDL::start()
{
    /* reset the report ring and state machine */
    _reports->flush();

    set_default_register_values();

    /* schedule a cycle to start things */
    ScheduleNow();
}

void
LIS3MDL::stop()
{
    if (_measure_interval > 0) {
        /* ensure no new items are queued while we cancel this one */
        _measure_interval = 0;
        ScheduleClear();
    }
}

int
LIS3MDL::read_reg(uint8_t reg, uint8_t &val)
{
    uint8_t buf = val;
    int ret = _interface->read(reg, &buf, 1);
    val = buf;
    return ret;
}

int
LIS3MDL::write_reg(uint8_t reg, uint8_t val)
{
    uint8_t buf = val;
    return _interface->write(reg, &buf, 1);
}