ADIS16448.cpp

Go to the documentation of this file.

/****************************************************************************
 *
 *   Copyright (c) 2018-2019 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 ADIS16448.cpp
 */

#include "ADIS16448.h"

ADIS16448::ADIS16448(int bus, uint32_t device, enum Rotation rotation) :
    SPI("ADIS16448", nullptr, bus, device, SPIDEV_MODE3, 1000000),
    ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())),
    _px4_accel(get_device_id(), ORB_PRIO_MAX, rotation),
    _px4_baro(get_device_id(), ORB_PRIO_MAX),
    _px4_gyro(get_device_id(), ORB_PRIO_MAX, rotation),
    _px4_mag(get_device_id(), ORB_PRIO_MAX, rotation)
{
    _px4_accel.set_device_type(DRV_ACC_DEVTYPE_ADIS16448);
    _px4_baro.set_device_type(DRV_ACC_DEVTYPE_ADIS16448); // not a typo, baro uses accel device id
    _px4_gyro.set_device_type(DRV_GYR_DEVTYPE_ADIS16448);
    _px4_mag.set_device_type(DRV_MAG_DEVTYPE_ADIS16448);

    _px4_accel.set_scale(ADIS16448_ACCEL_SENSITIVITY);
    _px4_gyro.set_scale(ADIS16448_GYRO_INITIAL_SENSITIVITY);
    _px4_mag.set_scale(ADIS16448_MAG_SENSITIVITY);

    _px4_mag.set_external(external());
}

ADIS16448::~ADIS16448()
{
    // Ensure the driver is inactive.
    stop();

    // Delete the perf counter.
    perf_free(_perf_read);
    perf_free(_perf_transfer);
    perf_free(_perf_bad_transfer);
    perf_free(_perf_crc_bad);
}

int
ADIS16448::init()
{
    // Do SPI init (and probe) first.
    int ret = SPI::init();

    if (ret != PX4_OK) {
        DEVICE_DEBUG("SPI setup failed %d", ret);

        // If probe/setup failed, return result.
        return ret;
    }

    ret = measure();

    if (ret != PX4_OK) {
        PX4_ERR("measure failed");
        return PX4_ERROR;
    }

    start();

    return OK;
}

bool ADIS16448::reset()
{
    // Software reset
    write_reg16(ADIS16448_GLOB_CMD, 1 << 7); // GLOB_CMD bit 7 Software reset

    // Reset Recovery Time 90 ms
    usleep(90000);

    if (!self_test()) {
        return false;
    }

    // Factory calibration restore
    //write_reg16(ADIS16448_GLOB_CMD, 1 << 1); // GLOB_CMD bit 1 Factory calibration restore

    // include the CRC-16 code in burst read output sequence
    write_reg16(ADIS16448_MSC_CTRL, 1 << 4);

    // Set digital FIR filter tap.
    //if (!set_dlpf_filter(BITS_FIR_NO_TAP_CFG)) {
    //  return PX4_ERROR;
    //}

    // Set IMU sample rate.
    if (!set_sample_rate(_sample_rate)) {
        return false;
    }

    // Set gyroscope scale to default value.
    //if (!set_gyro_dyn_range(GYRO_INITIAL_SENSITIVITY)) {
    //  return false;
    //}

    // Settling time.
    usleep(100000);

    return true;
}

bool ADIS16448::self_test()
{
    bool ret = true;

    // start internal self test routine
    write_reg16(ADIS16448_MSC_CTRL, 0x04);  // MSC_CTRL bit 10 Internal self test (cleared upon completion)

    // Automatic Self-Test Time 45 ms
    usleep(45000);

    // check test status (ADIS16448_DIAG_STAT)
    const uint16_t status = read_reg16(ADIS16448_DIAG_STAT);

    const bool self_test_error = (status & (1 << 5));   // 5: Self-test diagnostic error flag

    if (self_test_error) {
        //PX4_ERR("self test failed DIAG_STAT: 0x%04X", status);

        // Magnetometer
        const bool mag_fail = (status & (1 << 0));          // 0: Magnetometer functional test

        if (mag_fail) {
            // tolerate mag test failure (likely due to surrounding magnetic field)
            PX4_WARN("Magnetometer functional test fail");
        }

        // Barometer
        const bool baro_fail = (status & (1 << 1));         //  1: Barometer functional test

        if (baro_fail) {
            PX4_ERR("Barometer functional test test fail");
            ret = false;
        }

        // Gyroscope
        const bool gyro_x_fail = (status & (1 << 10));      // 10: X-axis gyroscope self-test failure
        const bool gyro_y_fail = (status & (1 << 11));      // 11: Y-axis gyroscope self-test failure
        const bool gyro_z_fail = (status & (1 << 12));      // 12: Z-axis gyroscope self-test failure

        if (gyro_x_fail || gyro_y_fail || gyro_z_fail) {
            PX4_ERR("gyroscope self-test failure");
            ret = false;
        }

        // Accelerometer
        const bool accel_x_fail = (status & (1 << 13));     // 13: X-axis accelerometer self-test failure
        const bool accel_y_fail = (status & (1 << 14));     // 14: Y-axis accelerometer self-test failure
        const bool accel_z_fail = (status & (1 << 15));     // 15: Z-axis accelerometer self-test failure

        if (accel_x_fail || accel_y_fail || accel_z_fail) {
            PX4_ERR("accelerometer self-test failure");
            ret = false;
        }
    }

    return ret;
}

int
ADIS16448::probe()
{
    bool reset_success = reset();

    // Retry 5 time to get the ADIS16448 PRODUCT ID number.
    for (size_t i = 0; i < 5; i++) {
        // Read product ID.
        _product_ID = read_reg16(ADIS16448_PRODUCT_ID);

        if (_product_ID == ADIS16448_Product) {
            break;
        }

        reset_success = reset();
    }

    if (!reset_success) {
        PX4_ERR("unable to successfully reset");
        return PX4_ERROR;
    }

    // Recognize product serial number.
    uint16_t serial_number = (read_reg16(ADIS16334_SERIAL_NUMBER) & 0xfff);

    // Verify product ID.
    switch (_product_ID) {
    case ADIS16448_Product:
        DEVICE_DEBUG("ADIS16448 is detected ID: 0x%02x, Serial: 0x%02x", _product_ID, serial_number);
        modify_reg16(ADIS16448_GPIO_CTRL, 0x0200, 0x0002);  // Turn on ADIS16448 adaptor board led.
        return OK;
    }

    DEVICE_DEBUG("unexpected ID 0x%02x", _product_ID);

    return -EIO;
}

bool
ADIS16448::set_sample_rate(uint16_t desired_sample_rate_hz)
{
    uint16_t smpl_prd = 0;

    if (desired_sample_rate_hz <= 51) {
        smpl_prd = BITS_SMPL_PRD_16_TAP_CFG;

    } else if (desired_sample_rate_hz <= 102) {
        smpl_prd = BITS_SMPL_PRD_8_TAP_CFG;

    } else if (desired_sample_rate_hz <= 204) {
        smpl_prd = BITS_SMPL_PRD_4_TAP_CFG;

    } else if (desired_sample_rate_hz <= 409) {
        smpl_prd = BITS_SMPL_PRD_2_TAP_CFG;

    } else {
        smpl_prd = BITS_SMPL_PRD_NO_TAP_CFG;
    }

    modify_reg16(ADIS16448_SMPL_PRD, 0x1F00, smpl_prd);

    if ((read_reg16(ADIS16448_SMPL_PRD) & 0x1F00) != smpl_prd) {
        PX4_ERR("failed to set IMU sample rate");

        return false;
    }

    _px4_accel.set_sample_rate(desired_sample_rate_hz);
    _px4_gyro.set_sample_rate(desired_sample_rate_hz);

    return true;
}

bool
ADIS16448::set_dlpf_filter(uint16_t desired_filter_tap)
{
    // Set the DLPF FIR filter tap. This affects both accelerometer and gyroscope.
    modify_reg16(ADIS16448_SENS_AVG, 0x0007, desired_filter_tap);

    // Verify data write on the IMU.
    if ((read_reg16(ADIS16448_SENS_AVG) & 0x0007) != desired_filter_tap) {
        PX4_ERR("failed to set IMU filter");

        return false;
    }

    return true;
}

bool
ADIS16448::set_gyro_dyn_range(uint16_t desired_gyro_dyn_range)
{
    uint16_t gyro_range_selection = 0;

    if (desired_gyro_dyn_range <= 250) {
        gyro_range_selection = BITS_GYRO_DYN_RANGE_250_CFG;

    } else if (desired_gyro_dyn_range <= 500) {
        gyro_range_selection = BITS_GYRO_DYN_RANGE_500_CFG;

    } else {
        gyro_range_selection = BITS_GYRO_DYN_RANGE_1000_CFG;
    }

    modify_reg16(ADIS16448_SENS_AVG, 0x0700, gyro_range_selection);

    // Verify data write on the IMU.
    if ((read_reg16(ADIS16448_SENS_AVG) & 0x0700) != gyro_range_selection) {
        PX4_ERR("failed to set gyro range");
        return false;

    } else {
        _px4_gyro.set_scale(((gyro_range_selection >> 8) / 100.0f) * M_PI_F / 180.0f);
    }

    return true;
}

void
ADIS16448::print_info()
{
    perf_print_counter(_perf_read);
    perf_print_counter(_perf_transfer);
    perf_print_counter(_perf_bad_transfer);
    perf_print_counter(_perf_crc_bad);

    _px4_accel.print_status();
    _px4_baro.print_status();
    _px4_gyro.print_status();
    _px4_mag.print_status();
}

void
ADIS16448::modify_reg16(unsigned reg, uint16_t clearbits, uint16_t setbits)
{
    uint16_t val = read_reg16(reg);
    val &= ~clearbits;
    val |= setbits;
    write_reg16(reg, val);
}

uint16_t
ADIS16448::read_reg16(unsigned reg)
{
    uint16_t cmd[1];

    cmd[0] = ((reg | DIR_READ) << 8) & 0xff00;

    transferhword(cmd, nullptr, 1);
    usleep(T_STALL);
    transferhword(nullptr, cmd, 1);
    usleep(T_STALL);

    return cmd[0];
}

void
ADIS16448::write_reg16(unsigned reg, uint16_t value)
{
    uint16_t cmd[2];

    cmd[0] = ((reg | DIR_WRITE) << 8) | (0x00ff & value);
    cmd[1] = (((reg + 0x1) | DIR_WRITE) << 8) | ((0xff00 & value) >> 8);

    transferhword(cmd, nullptr, 1);
    usleep(T_STALL);
    transferhword(cmd + 1, nullptr, 1);
    usleep(T_STALL);
}

void
ADIS16448::start()
{
    // Make sure we are stopped first.
    stop();

    // Start polling at the specified interval
    ScheduleOnInterval((1_s / _sample_rate), 10000);
}

void
ADIS16448::stop()
{
    ScheduleClear();
}

// computes the CCITT CRC16 on the data received from a burst read
static uint16_t ComputeCRC16(uint16_t burstData[13])
{
    uint16_t crc = 0xFFFF; // Holds the CRC value

    unsigned int data; // Holds the lower/Upper byte for CRC computation
    static constexpr unsigned int POLY = 0x1021; // Divisor used during CRC computation

    // Compute CRC on burst data starting from XGYRO_OUT and ending with TEMP_OUT.
    // Start with the lower byte and then the upper byte of each word.
    // i.e. Compute XGYRO_OUT_LSB CRC first and then compute XGYRO_OUT_MSB CRC.
    for (int i = 1; i < 12; i++) {
        unsigned int upperByte = (burstData[i] >> 8) & 0xFF;
        unsigned int lowerByte = (burstData[i] & 0xFF);
        data = lowerByte; // Compute lower byte CRC first

        for (int ii = 0; ii < 8; ii++, data >>= 1) {
            if ((crc & 0x0001) ^ (data & 0x0001)) {
                crc = (crc >> 1) ^ POLY;

            } else {
                crc >>= 1;
            }
        }

        data = upperByte; // Compute upper byte of CRC

        for (int ii = 0; ii < 8; ii++, data >>= 1) {
            if ((crc & 0x0001) ^ (data & 0x0001)) {
                crc = (crc >> 1) ^ POLY;

            } else {
                crc >>= 1;
            }
        }
    }

    crc = ~crc; // Compute complement of CRC
    data = crc;
    crc = (crc << 8) | (data >> 8 & 0xFF); // Perform byte swap prior to returning CRC

    return crc;
}

/**
 * convert 12 bit integer format to int16.
 */
static int16_t
convert12BitToINT16(uint16_t word)
{
    int16_t outputbuffer = 0;

    if ((word >> 11) & 0x1) {
        outputbuffer = (word & 0xfff) | 0xf000;

    } else {
        outputbuffer = (word & 0x0fff);
    }

    return (outputbuffer);
}

int
ADIS16448::measure()
{
    // Start measuring.
    perf_begin(_perf_read);

    // Fetch the full set of measurements from the ADIS16448 in one pass (burst read).
#pragma pack(push, 1) // Ensure proper memory alignment.
    struct Report {
        uint16_t cmd;

        uint16_t DIAG_STAT;

        int16_t XGYRO_OUT;
        int16_t YGYRO_OUT;
        int16_t ZGYRO_OUT;

        int16_t XACCL_OUT;
        int16_t YACCL_OUT;
        int16_t ZACCL_OUT;

        int16_t XMAGN_OUT;
        int16_t YMAGN_OUT;
        int16_t ZMAGN_OUT;

        uint16_t BARO_OUT;

        uint16_t TEMP_OUT;

        uint16_t CRC16;
    } report{};
#pragma pack(pop)

    report.cmd = ((ADIS16448_GLOB_CMD | DIR_READ) << 8) & 0xff00;

    const hrt_abstime timestamp_sample = hrt_absolute_time();

    perf_begin(_perf_transfer);

    if (OK != transferhword((uint16_t *)&report, ((uint16_t *)&report), sizeof(report) / sizeof(int16_t))) {
        perf_end(_perf_transfer);
        perf_end(_perf_read);

        perf_count(_perf_bad_transfer);

        return -EIO;
    }

    perf_end(_perf_transfer);

    // checksum
    if (report.CRC16 != ComputeCRC16((uint16_t *)&report.DIAG_STAT)) {
        perf_count(_perf_crc_bad);
        perf_end(_perf_read);
        return -EIO;
    }

    // error count
    const uint64_t error_count = perf_event_count(_perf_bad_transfer) + perf_event_count(_perf_crc_bad);

    // temperature
    const float temperature = (convert12BitToINT16(report.TEMP_OUT) * 0.07386f) + 31.0f; // 0.07386°C/LSB, 31°C = 0x000

    _px4_accel.set_error_count(error_count);
    _px4_accel.set_temperature(temperature);
    _px4_accel.update(timestamp_sample, report.XACCL_OUT, report.YACCL_OUT, report.ZACCL_OUT);

    _px4_gyro.set_error_count(error_count);
    _px4_gyro.set_temperature(temperature);
    _px4_gyro.update(timestamp_sample, report.XGYRO_OUT, report.YGYRO_OUT, report.ZGYRO_OUT);

    // DIAG_STAT bit 7: New data, xMAGN_OUT/BARO_OUT
    const bool baro_mag_update = (report.DIAG_STAT & (1 << 7));

    if (baro_mag_update) {
        _px4_mag.set_error_count(error_count);
        _px4_mag.set_temperature(temperature);
        _px4_mag.update(timestamp_sample, report.XMAGN_OUT, report.YMAGN_OUT, report.ZMAGN_OUT);

        _px4_baro.set_error_count(error_count);
        _px4_baro.set_temperature(temperature);
        _px4_baro.update(timestamp_sample, report.BARO_OUT * ADIS16448_BARO_SENSITIVITY);
    }

    // Stop measuring.
    perf_end(_perf_read);

    return OK;
}

void
ADIS16448::Run()
{
    // Make another measurement.
    measure();
}