ADIS16477.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.
 *
 ****************************************************************************/

#include "ADIS16477.hpp"

#define DIR_READ                0x00
#define DIR_WRITE               0x80

//  ADIS16477 registers
static constexpr uint8_t DIAG_STAT  = 0x02; // Output, system error flags
static constexpr uint8_t FILT_CTRL  = 0x5C;
static constexpr uint8_t MSC_CTRL   = 0x60;
static constexpr uint8_t DEC_RATE   = 0x64;
static constexpr uint8_t GLOB_CMD   = 0x68;
static constexpr uint8_t PROD_ID    = 0x72;

static constexpr uint16_t PROD_ID_ADIS16477 = 0x405D;   // ADIS16477 Identification, device number

// Stall time between SPI transfers
static constexpr uint8_t T_STALL = 16;

static constexpr uint32_t ADIS16477_DEFAULT_RATE = 1000;

using namespace time_literals;

ADIS16477::ADIS16477(int bus, uint32_t device, enum Rotation rotation) :
    SPI("ADIS16477", 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_gyro(get_device_id(), ORB_PRIO_MAX, rotation),
    _sample_perf(perf_alloc(PC_ELAPSED, "adis16477: read")),
    _bad_transfers(perf_alloc(PC_COUNT, "adis16477: bad transfers"))
{
#ifdef GPIO_SPI1_RESET_ADIS16477
    // Configure hardware reset line
    px4_arch_configgpio(GPIO_SPI1_RESET_ADIS16477);
#endif // GPIO_SPI1_RESET_ADIS16477

    _px4_accel.set_device_type(DRV_ACC_DEVTYPE_ADIS16477);
    _px4_accel.set_sample_rate(ADIS16477_DEFAULT_RATE);
    _px4_accel.set_scale(1.25f * CONSTANTS_ONE_G / 1000.0f); // accel 1.25 mg/LSB

    _px4_gyro.set_device_type(DRV_GYR_DEVTYPE_ADIS16477);
    _px4_gyro.set_sample_rate(ADIS16477_DEFAULT_RATE);
    _px4_gyro.set_scale(math::radians(0.025f)); // gyro 0.025 °/sec/LSB
}

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

    // delete the perf counters
    perf_free(_sample_perf);
    perf_free(_bad_transfers);
}

int
ADIS16477::init()
{
    // do SPI init (and probe) first
    if (SPI::init() != OK) {
        // if probe/setup failed, bail now
        PX4_DEBUG("SPI setup failed");
        return PX4_ERROR;
    }

    start();

    return PX4_OK;
}

int ADIS16477::reset()
{
#ifdef GPIO_SPI1_RESET_ADIS16477
    // Hardware reset
    px4_arch_gpiowrite(GPIO_SPI1_RESET_ADIS16477, 0);

    // The RST line must be in a low state for at least 10 μs to ensure a proper reset initiation and recovery.
    usleep(10_us);

    px4_arch_gpiowrite(GPIO_SPI1_RESET_ADIS16477, 1);
#else
    // Software reset (global command bit 7)
    uint8_t value[2] {};
    value[0] = (1 << 7);
    write_reg16(GLOB_CMD, (uint16_t)value[0]);
#endif // GPIO_SPI1_RESET_ADIS16477

    // Reset recovery time
    usleep(193_ms);


    // Miscellaneous Control Register (MSC_CTRL)
    static constexpr uint16_t MSC_CTRL_DEFAULT = 0x00C1;
    usleep(100);
    // verify
    const uint16_t msc_ctrl = read_reg16(MSC_CTRL);

    if (msc_ctrl != MSC_CTRL_DEFAULT) {
        PX4_ERR("invalid setup, MSC_CTRL=%#X", msc_ctrl);
        return PX4_ERROR;
    }


    // Bartlett Window FIR Filter
    static constexpr uint16_t FILT_CTRL_SETUP = 0x0004; // (disabled: 0x0000, 2 taps: 0x0001, 16 taps: 0x0004)
    write_reg16(FILT_CTRL, FILT_CTRL_SETUP);
    usleep(100);
    // verify
    const uint16_t filt_ctrl = read_reg16(FILT_CTRL);

    if (filt_ctrl != FILT_CTRL_SETUP) {
        PX4_ERR("invalid setup, FILT_CTRL=%#X", filt_ctrl);
        return PX4_ERROR;
    }


    // Decimation Filter
    //  set for 1000 samples per second
    static constexpr uint16_t DEC_RATE_SETUP = 0x0001;
    write_reg16(DEC_RATE, DEC_RATE_SETUP);
    usleep(100);
    // verify
    const uint16_t dec_rate = read_reg16(DEC_RATE);

    if (dec_rate != DEC_RATE_SETUP) {
        PX4_ERR("invalid setup, DEC_RATE=%#X", dec_rate);
        return PX4_ERROR;
    }

    return OK;
}

int
ADIS16477::probe()
{
    reset();

    // read product id (5 attempts)
    for (int i = 0; i < 5; i++) {
        uint16_t product_id = read_reg16(PROD_ID);

        if (product_id == PROD_ID_ADIS16477) {
            PX4_DEBUG("PRODUCT: %X", product_id);

            if (self_test_memory() && self_test_sensor()) {
                return PX4_OK;

            } else {
                PX4_ERR("probe attempt %d: self test failed, resetting", i);
                reset();
            }

        } else {
            PX4_ERR("probe attempt %d: read product id failed, resetting", i);
            reset();
        }
    }

    return -EIO;
}

bool
ADIS16477::self_test_memory()
{
    DEVICE_DEBUG("self test memory");

    // self test (global command bit 4)
    uint8_t value[2] {};
    value[0] = (1 << 4);
    write_reg16(GLOB_CMD, (uint16_t)value[0]);
    usleep(32_ms); // Flash Memory Test Time

    // read DIAG_STAT to check result
    uint16_t diag_stat = read_reg16(DIAG_STAT);

    if (diag_stat != 0) {
        PX4_ERR("DIAG_STAT: %#X", diag_stat);
        return false;
    }

    return true;
}

bool
ADIS16477::self_test_sensor()
{
    PX4_DEBUG("self test sensor");

    // self test (global command bit 2)
    uint8_t value[2] {};
    value[0] = (1 << 2);
    write_reg16(GLOB_CMD, (uint16_t)value[0]);
    usleep(14_ms); // Self Test Time

    // read DIAG_STAT to check result
    uint16_t diag_stat = read_reg16(DIAG_STAT);

    if (diag_stat != 0) {
        PX4_ERR("DIAG_STAT: %#X", diag_stat);
        return false;
    }

    return true;
}

uint16_t
ADIS16477::read_reg16(uint8_t reg)
{
    uint16_t cmd[1] {};

    cmd[0] = ((reg | DIR_READ) << 8) & 0xff00;
    transferhword(cmd, nullptr, 1);
    up_udelay(T_STALL);
    transferhword(nullptr, cmd, 1);
    up_udelay(T_STALL);

    return cmd[0];
}

void
ADIS16477::write_reg(uint8_t reg, uint8_t val)
{
    uint8_t cmd[2] {};
    cmd[0] = reg | 0x8;
    cmd[1] = val;
    transfer(cmd, cmd, sizeof(cmd));
}

void
ADIS16477::write_reg16(uint8_t 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);
    up_udelay(T_STALL);
    transferhword(cmd + 1, nullptr, 1);
    up_udelay(T_STALL);
}

void
ADIS16477::start()
{
#ifdef GPIO_SPI1_DRDY1_ADIS16477
    // Setup data ready on rising edge
    px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ADIS16477, true, false, true, &ADIS16477::data_ready_interrupt, this);
#else
    // Make sure we are stopped first
    stop();

    // start polling at the specified rate
    ScheduleOnInterval((1_s / ADIS16477_DEFAULT_RATE), 10000);
#endif
}

void
ADIS16477::stop()
{
#ifdef GPIO_SPI1_DRDY1_ADIS16477
    // Disable data ready callback
    px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ADIS16477, false, false, false, nullptr, nullptr);
#else
    ScheduleClear();
#endif
}

int
ADIS16477::data_ready_interrupt(int irq, void *context, void *arg)
{
    ADIS16477 *dev = static_cast<ADIS16477 *>(arg);

    // make another measurement
    dev->ScheduleNow();

    return PX4_OK;
}

void
ADIS16477::Run()
{
    // make another measurement
    measure();
}

int
ADIS16477::measure()
{
    perf_begin(_sample_perf);

    // Fetch the full set of measurements from the ADIS16477 in one pass (burst read).
    ADISReport adis_report{};
    adis_report.cmd = ((GLOB_CMD | DIR_READ) << 8) & 0xff00;

    // ADIS16477 burst report should be 176 bits
    static_assert(sizeof(adis_report) == (176 / 8), "ADIS16477 report not 176 bits");

    const hrt_abstime timestamp_sample = hrt_absolute_time();

    if (OK != transferhword((uint16_t *)&adis_report, ((uint16_t *)&adis_report), sizeof(adis_report) / sizeof(uint16_t))) {
        perf_count(_bad_transfers);
        perf_end(_sample_perf);
        return -EIO;
    }

    // Calculate checksum and compare

    // Checksum = DIAG_STAT, Bits[15:8] + DIAG_STAT, Bits[7:0] +
    //  X_GYRO_OUT, Bits[15:8] + X_GYRO_OUT, Bits[7:0] +
    //  Y_GYRO_OUT, Bits[15:8] + Y_GYRO_OUT, Bits[7:0] +
    //  Z_GYRO_OUT, Bits[15:8] + Z_GYRO_OUT, Bits[7:0] +
    //  X_ACCL_OUT, Bits[15:8] + X_ACCL_OUT, Bits[7:0] +
    //  Y_ACCL_OUT, Bits[15:8] + Y_ACCL_OUT, Bits[7:0] +
    //  Z_ACCL_OUT, Bits[15:8] + Z_ACCL_OUT, Bits[7:0] +
    //  TEMP_OUT, Bits[15:8] + TEMP_OUT, Bits[7:0] +
    //  DATA_CNTR, Bits[15:8] + DATA_CNTR, Bits[7:0]
    uint8_t *checksum_helper = (uint8_t *)&adis_report.diag_stat;

    uint8_t checksum = 0;

    for (int i = 0; i < 18; i++) {
        checksum += checksum_helper[i];
    }

    if (adis_report.checksum != checksum) {
        PX4_DEBUG("adis_report.checksum: %X vs calculated: %X", adis_report.checksum, checksum);

        perf_count(_bad_transfers);
        perf_end(_sample_perf);

        return -EIO;
    }

    // Check all Status/Error Flag Indicators (DIAG_STAT)
    if (adis_report.diag_stat != 0) {
        perf_count(_bad_transfers);
        perf_end(_sample_perf);

        return -EIO;
    }

    // temperature 1 LSB = 0.1°C
    const float temperature = adis_report.temp * 0.1f;
    _px4_accel.set_temperature(temperature);
    _px4_gyro.set_temperature(temperature);

    _px4_accel.update(timestamp_sample, adis_report.accel_x, adis_report.accel_y, adis_report.accel_z);
    _px4_gyro.update(timestamp_sample, adis_report.gyro_x, adis_report.gyro_y, adis_report.gyro_z);

    perf_end(_sample_perf);

    return OK;
}

void
ADIS16477::print_info()
{
    perf_print_counter(_sample_perf);
    perf_print_counter(_bad_transfers);

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