Browse Source
- new IMU driver structure with state machine (no sleeps in bus thread) - verify all configured registers and trigger reset on failure - detect if DIO1 or DIO2 are actually connected for data ready interrupt usage - don't use CRC-16 on burst transfers except for verified lotsrelease/1.12
Daniel Agar
4 years ago
committed by
Lorenz Meier
49 changed files with 983 additions and 775 deletions
@ -1,514 +0,0 @@
@@ -1,514 +0,0 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* 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(I2CSPIBusOption bus_option, int bus, int32_t device, enum Rotation rotation, int bus_frequency, |
||||
spi_mode_e spi_mode) : |
||||
SPI(DRV_IMU_DEVTYPE_ADIS16448, MODULE_NAME, bus, device, spi_mode, bus_frequency), |
||||
I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), |
||||
_px4_accel(get_device_id(), rotation), |
||||
_px4_baro(get_device_id()), |
||||
_px4_gyro(get_device_id(), rotation), |
||||
_px4_mag(get_device_id(), rotation) |
||||
{ |
||||
_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() |
||||
{ |
||||
// 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) { |
||||
DEVICE_DEBUG("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; |
||||
} |
||||
|
||||
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_status() |
||||
{ |
||||
I2CSPIDriverBase::print_status(); |
||||
perf_print_counter(_perf_read); |
||||
perf_print_counter(_perf_transfer); |
||||
perf_print_counter(_perf_bad_transfer); |
||||
perf_print_counter(_perf_crc_bad); |
||||
} |
||||
|
||||
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() |
||||
{ |
||||
// Start polling at the specified interval
|
||||
ScheduleOnInterval((1_s / _sample_rate), 10000); |
||||
} |
||||
|
||||
// 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::RunImpl() |
||||
{ |
||||
// Make another measurement.
|
||||
measure(); |
||||
} |
||||
@ -1,206 +0,0 @@
@@ -1,206 +0,0 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* 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 |
||||
* |
||||
* Driver for the Analog device ADIS16448 connected via SPI. |
||||
* |
||||
* @author Amir Melzer |
||||
* @author Andrew Tridgell |
||||
* @author Pat Hickey |
||||
* @author Lorenz Meier <lm@inf.ethz.ch> |
||||
*/ |
||||
|
||||
#include <drivers/device/spi.h> |
||||
#include <ecl/geo/geo.h> |
||||
#include <lib/conversion/rotation.h> |
||||
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp> |
||||
#include <lib/drivers/barometer/PX4Barometer.hpp> |
||||
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp> |
||||
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp> |
||||
#include <perf/perf_counter.h> |
||||
#include <px4_platform_common/i2c_spi_buses.h> |
||||
|
||||
using namespace time_literals; |
||||
|
||||
#define DIR_READ 0x00 |
||||
#define DIR_WRITE 0x80 |
||||
|
||||
#define ADIS16448_GPIO_CTRL 0x32 /* Auxiliary digital input/output control */ |
||||
#define ADIS16448_MSC_CTRL 0x34 /* Miscellaneous control */ |
||||
#define ADIS16448_SMPL_PRD 0x36 /* Internal sample period (rate) control */ |
||||
#define ADIS16448_SENS_AVG 0x38 /* Dynamic range and digital filter control */ |
||||
#define ADIS16448_DIAG_STAT 0x3C /* System status */ |
||||
#define ADIS16448_GLOB_CMD 0x3E /* System command */ |
||||
#define ADIS16448_PRODUCT_ID 0x56 /* Product identifier */ |
||||
#define ADIS16334_SERIAL_NUMBER 0x58 /* Serial number, lot specific */ |
||||
|
||||
#define ADIS16448_Product 0x4040/* Product ID Description for ADIS16448 */ |
||||
|
||||
#define BITS_SMPL_PRD_NO_TAP_CFG (0<<8) |
||||
#define BITS_SMPL_PRD_2_TAP_CFG (1<<8) |
||||
#define BITS_SMPL_PRD_4_TAP_CFG (2<<8) |
||||
#define BITS_SMPL_PRD_8_TAP_CFG (3<<8) |
||||
#define BITS_SMPL_PRD_16_TAP_CFG (4<<8) |
||||
|
||||
#define BITS_GYRO_DYN_RANGE_1000_CFG (4<<8) |
||||
#define BITS_GYRO_DYN_RANGE_500_CFG (2<<8) |
||||
#define BITS_GYRO_DYN_RANGE_250_CFG (1<<8) |
||||
|
||||
#define BITS_FIR_NO_TAP_CFG (0<<0) |
||||
#define BITS_FIR_2_TAP_CFG (1<<0) |
||||
#define BITS_FIR_4_TAP_CFG (2<<0) |
||||
#define BITS_FIR_8_TAP_CFG (3<<0) |
||||
#define BITS_FIR_16_TAP_CFG (4<<0) |
||||
#define BITS_FIR_32_TAP_CFG (5<<0) |
||||
#define BITS_FIR_64_TAP_CFG (6<<0) |
||||
#define BITS_FIR_128_TAP_CFG (7<<0) |
||||
|
||||
#define T_STALL 9 |
||||
|
||||
static constexpr float ADIS16448_ACCEL_SENSITIVITY{1.0f / 1200.0f * CONSTANTS_ONE_G}; // 1200 LSB/g
|
||||
static constexpr float ADIS16448_GYRO_INITIAL_SENSITIVITY{math::radians(1.0 / 25.0)}; // 25 LSB/°/sec
|
||||
static constexpr float ADIS16448_BARO_SENSITIVITY{0.02f}; // 20 microbar per LSB,
|
||||
static constexpr float ADIS16448_MAG_SENSITIVITY{1.0 / 7.0 / 1000.0}; // 7 LSB/mgauss
|
||||
|
||||
|
||||
static constexpr float ADIS16448_ACCEL_GYRO_UPDATE_RATE{819.2}; // accel and gryo max update 819.2 samples per second
|
||||
static constexpr float ADIS16448_MAG_BARO_UPDATE_RATE{51.2}; // xMAGN_OUT and BARO_OUT registers update at 51.2 samples per second
|
||||
|
||||
class ADIS16448 : public device::SPI, public I2CSPIDriver<ADIS16448> |
||||
{ |
||||
public: |
||||
ADIS16448(I2CSPIBusOption bus_option, int bus, int32_t device, enum Rotation rotation, int bus_frequency, |
||||
spi_mode_e spi_mode); |
||||
virtual ~ADIS16448(); |
||||
|
||||
static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, |
||||
int runtime_instance); |
||||
static void print_usage(); |
||||
|
||||
int init() override; |
||||
|
||||
void print_status() override; |
||||
|
||||
void RunImpl(); |
||||
protected: |
||||
int probe() override; |
||||
|
||||
private: |
||||
|
||||
enum class |
||||
Register : uint8_t { |
||||
GPIO_CTRL = 0x32, // Auxiliary digital input/output control
|
||||
MSC_CTRL = 0x34, // Miscellaneous control
|
||||
SMPL_PRD = 0x36, // Internal sample period (rate) control
|
||||
SENS_AVG = 0x38, // Dynamic range and digital filter control
|
||||
|
||||
DIAG_STAT = 0x3C, // System status
|
||||
GLOB_CMD = 0x3E, // System command
|
||||
|
||||
PRODUCT_ID = 0x56, // Product identifier
|
||||
SERIAL_NUMBER = 0x58, // Serial number, lot specific
|
||||
}; |
||||
|
||||
/**
|
||||
* Fetch measurements from the sensor and update the report buffers. |
||||
*/ |
||||
int measure(); |
||||
|
||||
/**
|
||||
* Modify a register in the ADIS16448 |
||||
* Bits are cleared before bits are set. |
||||
* |
||||
* @param reg The register to modify. |
||||
* @param clearbits Bits in the register to clear. |
||||
* @param setbits Bits in the register to set. |
||||
*/ |
||||
void modify_reg16(unsigned reg, uint16_t clearbits, uint16_t setbits); |
||||
|
||||
/**
|
||||
* Resets the chip and measurements ranges |
||||
*/ |
||||
bool reset(); |
||||
|
||||
bool self_test(); |
||||
|
||||
/**
|
||||
* Read a register from the ADIS16448 |
||||
* @arg reg The register to read. |
||||
* @return Returns the register value. |
||||
*/ |
||||
uint16_t read_reg16(unsigned reg); |
||||
|
||||
/**
|
||||
* Write a register in the ADIS16448 |
||||
* @param reg The register to write. |
||||
* @param value The new value to write. |
||||
*/ |
||||
void write_reg16(unsigned reg, uint16_t value); |
||||
|
||||
/**
|
||||
* Set low pass filter frequency. |
||||
*/ |
||||
bool set_dlpf_filter(uint16_t frequency_hz); |
||||
|
||||
/**
|
||||
* Set the gyroscope dynamic range. |
||||
*/ |
||||
bool set_gyro_dyn_range(uint16_t desired_gyro_dyn_range); |
||||
|
||||
/**
|
||||
* Set sample rate (approximate) - 1kHz to 5Hz. |
||||
*/ |
||||
bool set_sample_rate(uint16_t desired_sample_rate_hz); |
||||
|
||||
/**
|
||||
* Start automatic measurement. |
||||
*/ |
||||
void start(); |
||||
|
||||
PX4Accelerometer _px4_accel; |
||||
PX4Barometer _px4_baro; |
||||
PX4Gyroscope _px4_gyro; |
||||
PX4Magnetometer _px4_mag; |
||||
|
||||
uint16_t _product_ID{0}; // Product ID code.
|
||||
|
||||
static constexpr float _sample_rate{ADIS16448_ACCEL_GYRO_UPDATE_RATE}; |
||||
|
||||
perf_counter_t _perf_read{perf_counter_t(perf_alloc(PC_ELAPSED, "ADIS16448: read"))}; |
||||
perf_counter_t _perf_transfer{perf_counter_t(perf_alloc(PC_ELAPSED, "ADIS16448: transfer"))}; |
||||
perf_counter_t _perf_bad_transfer{perf_counter_t(perf_alloc(PC_COUNT, "ADIS16448: bad transfers"))}; |
||||
perf_counter_t _perf_crc_bad{perf_counter_t(perf_alloc(PC_COUNT, "ADIS16448: CRC16 bad"))}; |
||||
|
||||
}; |
||||
@ -0,0 +1,34 @@
@@ -0,0 +1,34 @@
|
||||
############################################################################ |
||||
# |
||||
# Copyright (c) 2021 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. |
||||
# |
||||
############################################################################ |
||||
|
||||
add_subdirectory(adis16448) |
||||
@ -0,0 +1,594 @@
@@ -0,0 +1,594 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* Copyright (c) 2021 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 "ADIS16448.hpp" |
||||
|
||||
using namespace time_literals; |
||||
|
||||
// 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 output = 0; |
||||
|
||||
if ((word >> 11) & 0x1) { |
||||
// sign extend
|
||||
output = (word & 0xFFF) | 0xF000; |
||||
|
||||
} else { |
||||
output = (word & 0x0FFF); |
||||
} |
||||
|
||||
return output; |
||||
} |
||||
|
||||
ADIS16448::ADIS16448(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, |
||||
spi_drdy_gpio_t drdy_gpio) : |
||||
SPI(DRV_IMU_DEVTYPE_ADIS16448, MODULE_NAME, bus, device, SPIDEV_MODE3, bus_frequency), |
||||
I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), |
||||
_drdy_gpio(drdy_gpio), // TODO: DRDY disabled
|
||||
_px4_accel(get_device_id(), rotation), |
||||
_px4_baro(get_device_id()), |
||||
_px4_gyro(get_device_id(), rotation), |
||||
_px4_mag(get_device_id(), rotation) |
||||
{ |
||||
} |
||||
|
||||
ADIS16448::~ADIS16448() |
||||
{ |
||||
perf_free(_reset_perf); |
||||
perf_free(_perf_crc_bad); |
||||
perf_free(_bad_register_perf); |
||||
perf_free(_bad_transfer_perf); |
||||
} |
||||
|
||||
int ADIS16448::init() |
||||
{ |
||||
int ret = SPI::init(); |
||||
|
||||
if (ret != PX4_OK) { |
||||
DEVICE_DEBUG("SPI::init failed (%i)", ret); |
||||
return ret; |
||||
} |
||||
|
||||
return Reset() ? 0 : -1; |
||||
} |
||||
|
||||
bool ADIS16448::Reset() |
||||
{ |
||||
_state = STATE::RESET; |
||||
DataReadyInterruptDisable(); |
||||
ScheduleClear(); |
||||
ScheduleNow(); |
||||
return true; |
||||
} |
||||
|
||||
void ADIS16448::exit_and_cleanup() |
||||
{ |
||||
DataReadyInterruptDisable(); |
||||
I2CSPIDriverBase::exit_and_cleanup(); |
||||
} |
||||
|
||||
void ADIS16448::print_status() |
||||
{ |
||||
I2CSPIDriverBase::print_status(); |
||||
|
||||
perf_print_counter(_reset_perf); |
||||
perf_print_counter(_perf_crc_bad); |
||||
perf_print_counter(_bad_register_perf); |
||||
perf_print_counter(_bad_transfer_perf); |
||||
} |
||||
|
||||
int ADIS16448::probe() |
||||
{ |
||||
// Power-On Start-Up Time 205 ms
|
||||
if (hrt_absolute_time() < 205_ms) { |
||||
PX4_WARN("Power-On Start-Up Time is 205 ms"); |
||||
} |
||||
|
||||
const uint16_t PROD_ID = RegisterRead(Register::PROD_ID); |
||||
|
||||
if (PROD_ID != Product_identification) { |
||||
DEVICE_DEBUG("unexpected PROD_ID 0x%02x", PROD_ID); |
||||
return PX4_ERROR; |
||||
} |
||||
|
||||
const uint16_t SERIAL_NUM = RegisterRead(Register::SERIAL_NUM); |
||||
const uint16_t LOT_ID1 = RegisterRead(Register::LOT_ID1); |
||||
const uint16_t LOT_ID2 = RegisterRead(Register::LOT_ID2); |
||||
|
||||
PX4_INFO("Serial Number: 0x%02x, Lot ID1: 0x%02x ID2: 0x%02x", SERIAL_NUM, LOT_ID1, LOT_ID2); |
||||
|
||||
// Only enable CRC-16 for verified lots (HACK to support older ADIS16448AMLZ with no explicit detection)
|
||||
if (LOT_ID1 == 0x1824) { |
||||
_check_crc = true; |
||||
} |
||||
|
||||
return PX4_OK; |
||||
} |
||||
|
||||
void ADIS16448::RunImpl() |
||||
{ |
||||
const hrt_abstime now = hrt_absolute_time(); |
||||
|
||||
switch (_state) { |
||||
case STATE::RESET: |
||||
perf_count(_reset_perf); |
||||
// GLOB_CMD: software reset
|
||||
RegisterWrite(Register::GLOB_CMD, GLOB_CMD_BIT::Software_reset); |
||||
_reset_timestamp = now; |
||||
_failure_count = 0; |
||||
_state = STATE::WAIT_FOR_RESET; |
||||
ScheduleDelayed(90_ms); // Reset Recovery Time 90 ms
|
||||
break; |
||||
|
||||
case STATE::WAIT_FOR_RESET: |
||||
|
||||
if (_self_test_passed) { |
||||
if ((RegisterRead(Register::PROD_ID) == Product_identification)) { |
||||
// if reset succeeded then configure
|
||||
_state = STATE::CONFIGURE; |
||||
ScheduleNow(); |
||||
|
||||
} else { |
||||
// RESET not complete
|
||||
if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { |
||||
PX4_DEBUG("Reset failed, retrying"); |
||||
_state = STATE::RESET; |
||||
ScheduleDelayed(100_ms); |
||||
|
||||
} else { |
||||
PX4_DEBUG("Reset not complete, check again in 10 ms"); |
||||
ScheduleDelayed(10_ms); |
||||
} |
||||
} |
||||
|
||||
} else { |
||||
RegisterWrite(Register::MSC_CTRL, MSC_CTRL_BIT::Internal_self_test); |
||||
_state = STATE::SELF_TEST_CHECK; |
||||
ScheduleDelayed(45_ms); // Automatic Self-Test Time 45 ms
|
||||
} |
||||
|
||||
break; |
||||
|
||||
case STATE::SELF_TEST_CHECK: { |
||||
const uint16_t DIAG_STAT = RegisterRead(Register::DIAG_STAT); |
||||
|
||||
if (DIAG_STAT & DIAG_STAT_BIT::Self_test_diagnostic_error_flag) { |
||||
PX4_ERR("self test failed"); |
||||
|
||||
// Magnetometer
|
||||
if (DIAG_STAT & DIAG_STAT_BIT::Magnetometer_functional_test) { |
||||
// tolerate mag test failure (likely due to surrounding magnetic field)
|
||||
PX4_ERR("Magnetometer functional test fail"); |
||||
} |
||||
|
||||
// Barometer
|
||||
if (DIAG_STAT & DIAG_STAT_BIT::Barometer_functional_test) { |
||||
PX4_ERR("Barometer functional test test fail"); |
||||
} |
||||
|
||||
// Gyroscope
|
||||
const bool gyro_x_fail = DIAG_STAT & DIAG_STAT_BIT::X_axis_gyroscope_self_test_failure; |
||||
const bool gyro_y_fail = DIAG_STAT & DIAG_STAT_BIT::Y_axis_gyroscope_self_test_failure; |
||||
const bool gyro_z_fail = DIAG_STAT & DIAG_STAT_BIT::Z_axis_gyroscope_self_test_failure; |
||||
|
||||
if (gyro_x_fail || gyro_y_fail || gyro_z_fail) { |
||||
PX4_ERR("gyroscope self-test failure"); |
||||
} |
||||
|
||||
// Accelerometer
|
||||
const bool accel_x_fail = DIAG_STAT & DIAG_STAT_BIT::X_axis_accelerometer_self_test_failure; |
||||
const bool accel_y_fail = DIAG_STAT & DIAG_STAT_BIT::Y_axis_accelerometer_self_test_failure; |
||||
const bool accel_z_fail = DIAG_STAT & DIAG_STAT_BIT::Z_axis_accelerometer_self_test_failure; |
||||
|
||||
if (accel_x_fail || accel_y_fail || accel_z_fail) { |
||||
PX4_ERR("accelerometer self-test failure"); |
||||
} |
||||
|
||||
_self_test_passed = false; |
||||
_state = STATE::RESET; |
||||
ScheduleDelayed(1000_ms); |
||||
|
||||
} else { |
||||
PX4_DEBUG("self test passed"); |
||||
_self_test_passed = true; |
||||
_state = STATE::RESET; |
||||
ScheduleNow(); |
||||
} |
||||
} |
||||
|
||||
break; |
||||
|
||||
case STATE::CONFIGURE: |
||||
if (Configure()) { |
||||
// if configure succeeded then start reading
|
||||
_state = STATE::READ; |
||||
|
||||
if (DataReadyInterruptConfigure()) { |
||||
_data_ready_interrupt_enabled = true; |
||||
|
||||
// backup schedule as a watchdog timeout
|
||||
ScheduleDelayed(100_ms); |
||||
|
||||
} else { |
||||
_data_ready_interrupt_enabled = false; |
||||
ScheduleOnInterval(SAMPLE_INTERVAL_US, SAMPLE_INTERVAL_US); |
||||
} |
||||
|
||||
} else { |
||||
// CONFIGURE not complete
|
||||
if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { |
||||
PX4_DEBUG("Configure failed, resetting"); |
||||
_state = STATE::RESET; |
||||
|
||||
} else { |
||||
PX4_DEBUG("Configure failed, retrying"); |
||||
} |
||||
|
||||
ScheduleDelayed(100_ms); |
||||
} |
||||
|
||||
break; |
||||
|
||||
case STATE::READ: { |
||||
if (_data_ready_interrupt_enabled) { |
||||
// push backup schedule back
|
||||
ScheduleDelayed(SAMPLE_INTERVAL_US * 2); |
||||
} |
||||
|
||||
bool success = false; |
||||
|
||||
struct BurstRead { |
||||
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; |
||||
} buffer{}; |
||||
|
||||
// ADIS16448 burst report should be 224 bits
|
||||
static_assert(sizeof(BurstRead) == (224 / 8), "ADIS16448 report not 224 bits"); |
||||
|
||||
buffer.cmd = static_cast<uint16_t>(Register::GLOB_CMD) << 8; |
||||
set_frequency(SPI_SPEED_BURST); |
||||
|
||||
if (transferhword((uint16_t *)&buffer, (uint16_t *)&buffer, sizeof(buffer) / sizeof(uint16_t)) == PX4_OK) { |
||||
|
||||
bool publish_data = true; |
||||
|
||||
// checksum
|
||||
if (_check_crc) { |
||||
if (buffer.CRC16 != ComputeCRC16((uint16_t *)&buffer.DIAG_STAT)) { |
||||
perf_count(_perf_crc_bad); |
||||
publish_data = false; |
||||
} |
||||
} |
||||
|
||||
if (buffer.DIAG_STAT == DIAG_STAT_BIT::SPI_communication_failure) { |
||||
perf_count(_bad_transfer_perf); |
||||
publish_data = false; |
||||
} |
||||
|
||||
if (publish_data) { |
||||
|
||||
const uint32_t error_count = perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf); |
||||
_px4_accel.set_error_count(error_count); |
||||
_px4_gyro.set_error_count(error_count); |
||||
|
||||
// temperature 0.07386°C/LSB, 31°C = 0x000
|
||||
const float temperature = (convert12BitToINT16(buffer.TEMP_OUT) * 0.07386f) + 31.f; |
||||
|
||||
_px4_accel.set_temperature(temperature); |
||||
_px4_gyro.set_temperature(temperature); |
||||
|
||||
// sensor's frame is +x forward, +y left, +z up
|
||||
// flip y & z to publish right handed with z down (x forward, y right, z down)
|
||||
const int16_t accel_x = buffer.XACCL_OUT; |
||||
const int16_t accel_y = (buffer.YACCL_OUT == INT16_MIN) ? INT16_MAX : -buffer.YACCL_OUT; |
||||
const int16_t accel_z = (buffer.ZACCL_OUT == INT16_MIN) ? INT16_MAX : -buffer.ZACCL_OUT; |
||||
|
||||
const int16_t gyro_x = buffer.XGYRO_OUT; |
||||
const int16_t gyro_y = (buffer.YGYRO_OUT == INT16_MIN) ? INT16_MAX : -buffer.YGYRO_OUT; |
||||
const int16_t gyro_z = (buffer.ZGYRO_OUT == INT16_MIN) ? INT16_MAX : -buffer.ZGYRO_OUT; |
||||
|
||||
_px4_accel.update(now, accel_x, accel_y, accel_z); |
||||
_px4_gyro.update(now, gyro_x, gyro_y, gyro_z); |
||||
|
||||
// DIAG_STAT bit 7: New data, xMAGN_OUT/BARO_OUT
|
||||
if (buffer.DIAG_STAT & DIAG_STAT_BIT::New_data_xMAGN_OUT_BARO_OUT) { |
||||
_px4_mag.set_error_count(error_count); |
||||
_px4_mag.set_temperature(temperature); |
||||
|
||||
const int16_t mag_x = buffer.XMAGN_OUT; |
||||
const int16_t mag_y = (buffer.YMAGN_OUT == INT16_MIN) ? INT16_MAX : -buffer.YMAGN_OUT; |
||||
const int16_t mag_z = (buffer.ZMAGN_OUT == INT16_MIN) ? INT16_MAX : -buffer.ZMAGN_OUT; |
||||
_px4_mag.update(now, mag_x, mag_y, mag_z); |
||||
|
||||
_px4_baro.set_error_count(error_count); |
||||
_px4_baro.set_temperature(temperature); |
||||
|
||||
float pressure_pa = buffer.BARO_OUT * 0.02f; // 20 μbar per LSB
|
||||
_px4_baro.update(now, pressure_pa); |
||||
} |
||||
|
||||
success = true; |
||||
|
||||
if (_failure_count > 0) { |
||||
_failure_count--; |
||||
} |
||||
} |
||||
|
||||
} else { |
||||
perf_count(_bad_transfer_perf); |
||||
} |
||||
|
||||
if (!success) { |
||||
_failure_count++; |
||||
|
||||
// full reset if things are failing consistently
|
||||
if (_failure_count > 10) { |
||||
Reset(); |
||||
return; |
||||
} |
||||
} |
||||
|
||||
if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) { |
||||
// check configuration registers periodically or immediately following any failure
|
||||
if (RegisterCheck(_register_cfg[_checked_register])) { |
||||
_last_config_check_timestamp = now; |
||||
_checked_register = (_checked_register + 1) % size_register_cfg; |
||||
|
||||
} else { |
||||
// register check failed, force reset
|
||||
perf_count(_bad_register_perf); |
||||
Reset(); |
||||
} |
||||
} |
||||
} |
||||
|
||||
break; |
||||
} |
||||
} |
||||
|
||||
bool ADIS16448::Configure() |
||||
{ |
||||
// first set and clear all configured register bits
|
||||
for (const auto ®_cfg : _register_cfg) { |
||||
RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits); |
||||
} |
||||
|
||||
// now check that all are configured
|
||||
bool success = true; |
||||
|
||||
for (const auto ®_cfg : _register_cfg) { |
||||
if (!RegisterCheck(reg_cfg)) { |
||||
success = false; |
||||
} |
||||
} |
||||
|
||||
_px4_accel.set_scale(0.833f * 1e-3f * CONSTANTS_ONE_G); // 0.833 mg/LSB
|
||||
_px4_gyro.set_scale(math::radians(0.04f)); // 0.04 °/sec/LSB
|
||||
_px4_mag.set_scale(142.9f * 1e-6f); // μgauss/LSB
|
||||
|
||||
_px4_accel.set_range(18.f * CONSTANTS_ONE_G); |
||||
_px4_gyro.set_range(math::radians(1000.f)); |
||||
|
||||
_px4_mag.set_external(external()); |
||||
|
||||
return success; |
||||
} |
||||
|
||||
int ADIS16448::DataReadyInterruptCallback(int irq, void *context, void *arg) |
||||
{ |
||||
static_cast<ADIS16448 *>(arg)->DataReady(); |
||||
return 0; |
||||
} |
||||
|
||||
void ADIS16448::DataReady() |
||||
{ |
||||
ScheduleNow(); |
||||
} |
||||
|
||||
bool ADIS16448::DataReadyInterruptConfigure() |
||||
{ |
||||
if (_drdy_gpio == 0) { |
||||
return false; |
||||
} |
||||
|
||||
// check if DIO1 is connected to data ready
|
||||
{ |
||||
// DIO1 output set low
|
||||
RegisterWrite(Register::GPIO_CTRL, GPIO_CTRL_BIT::GPIO1_DIRECTION); |
||||
bool write0_valid = (px4_arch_gpioread(_drdy_gpio) == 1); |
||||
|
||||
// DIO1 output set high
|
||||
RegisterWrite(Register::GPIO_CTRL, GPIO_CTRL_BIT::GPIO1_DATA_LEVEL | GPIO_CTRL_BIT::GPIO1_DIRECTION); |
||||
bool write1_valid = (px4_arch_gpioread(_drdy_gpio) == 0); |
||||
|
||||
// DIO1 output set low again
|
||||
RegisterWrite(Register::GPIO_CTRL, GPIO_CTRL_BIT::GPIO1_DIRECTION); |
||||
bool write2_valid = (px4_arch_gpioread(_drdy_gpio) == 1); |
||||
|
||||
if (write0_valid && write1_valid && write2_valid) { |
||||
PX4_INFO("DIO1 DRDY valid"); |
||||
// Setup data ready on falling edge
|
||||
return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0; |
||||
|
||||
} else { |
||||
PX4_DEBUG("DIO1 DRDY invalid"); |
||||
} |
||||
} |
||||
|
||||
// check if DIO2 is connected to data ready
|
||||
{ |
||||
// DIO2 output set low
|
||||
RegisterWrite(Register::GPIO_CTRL, GPIO_CTRL_BIT::GPIO2_DIRECTION); |
||||
bool write0_valid = (px4_arch_gpioread(_drdy_gpio) == 1); |
||||
|
||||
// DIO2 output set high
|
||||
RegisterWrite(Register::GPIO_CTRL, GPIO_CTRL_BIT::GPIO2_DATA_LEVEL | GPIO_CTRL_BIT::GPIO2_DIRECTION); |
||||
bool write1_valid = (px4_arch_gpioread(_drdy_gpio) == 0); |
||||
|
||||
// DIO2 output set low again
|
||||
RegisterWrite(Register::GPIO_CTRL, GPIO_CTRL_BIT::GPIO2_DIRECTION); |
||||
bool write2_valid = (px4_arch_gpioread(_drdy_gpio) == 1); |
||||
|
||||
if (write0_valid && write1_valid && write2_valid) { |
||||
PX4_INFO("DIO2 DRDY valid"); |
||||
|
||||
} else { |
||||
PX4_DEBUG("DIO2 DRDY invalid"); |
||||
} |
||||
} |
||||
|
||||
return false; |
||||
} |
||||
|
||||
bool ADIS16448::DataReadyInterruptDisable() |
||||
{ |
||||
if (_drdy_gpio == 0) { |
||||
return false; |
||||
} |
||||
|
||||
return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0; |
||||
} |
||||
|
||||
bool ADIS16448::RegisterCheck(const register_config_t ®_cfg) |
||||
{ |
||||
bool success = true; |
||||
|
||||
const uint16_t reg_value = RegisterRead(reg_cfg.reg); |
||||
|
||||
if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) { |
||||
PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits); |
||||
success = false; |
||||
} |
||||
|
||||
if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) { |
||||
PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits); |
||||
success = false; |
||||
} |
||||
|
||||
return success; |
||||
} |
||||
|
||||
uint16_t ADIS16448::RegisterRead(Register reg) |
||||
{ |
||||
set_frequency(SPI_SPEED); |
||||
|
||||
uint16_t cmd[1]; |
||||
cmd[0] = (static_cast<uint16_t>(reg) << 8); |
||||
|
||||
transferhword(cmd, nullptr, 1); |
||||
usleep(SPI_STALL_PERIOD); |
||||
transferhword(nullptr, cmd, 1); |
||||
|
||||
return cmd[0]; |
||||
} |
||||
|
||||
void ADIS16448::RegisterWrite(Register reg, uint16_t value) |
||||
{ |
||||
set_frequency(SPI_SPEED); |
||||
|
||||
uint16_t cmd[2]; |
||||
cmd[0] = (((static_cast<uint16_t>(reg)) | DIR_WRITE) << 8) | ((0x00FF & value)); |
||||
cmd[1] = (((static_cast<uint16_t>(reg) + 1) | DIR_WRITE) << 8) | ((0xFF00 & value) >> 8); |
||||
|
||||
transferhword(cmd, nullptr, 1); |
||||
usleep(SPI_STALL_PERIOD); |
||||
transferhword(cmd + 1, nullptr, 1); |
||||
} |
||||
|
||||
void ADIS16448::RegisterSetAndClearBits(Register reg, uint16_t setbits, uint16_t clearbits) |
||||
{ |
||||
const uint16_t orig_val = RegisterRead(reg); |
||||
|
||||
uint16_t val = (orig_val & ~clearbits) | setbits; |
||||
|
||||
if (orig_val != val) { |
||||
RegisterWrite(reg, val); |
||||
} |
||||
} |
||||
@ -0,0 +1,138 @@
@@ -0,0 +1,138 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* Copyright (c) 2021 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.hpp |
||||
* |
||||
* Driver for the Analog Devices ADIS16448 connected via SPI. |
||||
* |
||||
*/ |
||||
|
||||
#pragma once |
||||
|
||||
#include "Analog_Devices_ADIS16448_registers.hpp" |
||||
|
||||
#include <drivers/drv_hrt.h> |
||||
#include <lib/drivers/device/spi.h> |
||||
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp> |
||||
#include <lib/drivers/barometer/PX4Barometer.hpp> |
||||
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp> |
||||
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp> |
||||
#include <lib/ecl/geo/geo.h> |
||||
#include <lib/perf/perf_counter.h> |
||||
#include <px4_platform_common/atomic.h> |
||||
#include <px4_platform_common/i2c_spi_buses.h> |
||||
|
||||
using namespace Analog_Devices_ADIS16448; |
||||
|
||||
class ADIS16448 : public device::SPI, public I2CSPIDriver<ADIS16448> |
||||
{ |
||||
public: |
||||
ADIS16448(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, |
||||
spi_drdy_gpio_t drdy_gpio); |
||||
~ADIS16448() override; |
||||
|
||||
static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, |
||||
int runtime_instance); |
||||
static void print_usage(); |
||||
|
||||
void RunImpl(); |
||||
|
||||
int init() override; |
||||
void print_status() override; |
||||
|
||||
private: |
||||
void exit_and_cleanup() override; |
||||
|
||||
struct register_config_t { |
||||
Register reg; |
||||
uint16_t set_bits{0}; |
||||
uint16_t clear_bits{0}; |
||||
}; |
||||
|
||||
int probe() override; |
||||
|
||||
bool Reset(); |
||||
|
||||
bool Configure(); |
||||
|
||||
static int DataReadyInterruptCallback(int irq, void *context, void *arg); |
||||
void DataReady(); |
||||
bool DataReadyInterruptConfigure(); |
||||
bool DataReadyInterruptDisable(); |
||||
|
||||
bool RegisterCheck(const register_config_t ®_cfg); |
||||
|
||||
uint16_t RegisterRead(Register reg); |
||||
void RegisterWrite(Register reg, uint16_t value); |
||||
void RegisterSetAndClearBits(Register reg, uint16_t setbits, uint16_t clearbits); |
||||
|
||||
const spi_drdy_gpio_t _drdy_gpio; |
||||
|
||||
PX4Accelerometer _px4_accel; |
||||
PX4Barometer _px4_baro; |
||||
PX4Gyroscope _px4_gyro; |
||||
PX4Magnetometer _px4_mag; |
||||
|
||||
perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": reset")}; |
||||
perf_counter_t _perf_crc_bad{perf_counter_t(perf_alloc(PC_COUNT, MODULE_NAME": CRC16 bad"))}; |
||||
perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")}; |
||||
perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")}; |
||||
|
||||
hrt_abstime _reset_timestamp{0}; |
||||
hrt_abstime _last_config_check_timestamp{0}; |
||||
int _failure_count{0}; |
||||
|
||||
bool _check_crc{false}; // CRC-16 not supported on earlier models (eg ADIS16448AMLZ)
|
||||
bool _data_ready_interrupt_enabled{false}; |
||||
|
||||
bool _self_test_passed{false}; |
||||
|
||||
enum class STATE : uint8_t { |
||||
RESET, |
||||
WAIT_FOR_RESET, |
||||
SELF_TEST_CHECK, |
||||
CONFIGURE, |
||||
READ, |
||||
} _state{STATE::RESET}; |
||||
|
||||
uint8_t _checked_register{0}; |
||||
static constexpr uint8_t size_register_cfg{4}; |
||||
register_config_t _register_cfg[size_register_cfg] { |
||||
// Register | Set bits, Clear bits
|
||||
{ Register::MSC_CTRL, MSC_CTRL_BIT::CRC16_for_burst, 0 }, |
||||
{ Register::SMPL_PRD, SMPL_PRD_BIT::internal_sampling_clock, SMPL_PRD_BIT::decimation_rate }, |
||||
{ Register::SENS_AVG, SENS_AVG_BIT::Measurement_range_1000_set, SENS_AVG_BIT::Measurement_range_1000_clear | SENS_AVG_BIT::Filter_Size_Variable_B }, |
||||
{ Register::GPIO_CTRL, GPIO_CTRL_BIT::GPIO2_DIRECTION | GPIO_CTRL_BIT::GPIO1_DIRECTION, 0}, |
||||
}; |
||||
}; |
||||
@ -0,0 +1,161 @@
@@ -0,0 +1,161 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* Copyright (c) 2021 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 Analog_Devices_ADIS16448_registers.hpp |
||||
* |
||||
* Analog Devices ADIS16448 registers. |
||||
* |
||||
*/ |
||||
|
||||
#pragma once |
||||
|
||||
#include <cstdint> |
||||
|
||||
// TODO: move to a central header
|
||||
static constexpr uint16_t Bit0 = (1 << 0); |
||||
static constexpr uint16_t Bit1 = (1 << 1); |
||||
static constexpr uint16_t Bit2 = (1 << 2); |
||||
static constexpr uint16_t Bit3 = (1 << 3); |
||||
static constexpr uint16_t Bit4 = (1 << 4); |
||||
static constexpr uint16_t Bit5 = (1 << 5); |
||||
static constexpr uint16_t Bit6 = (1 << 6); |
||||
static constexpr uint16_t Bit7 = (1 << 7); |
||||
static constexpr uint16_t Bit8 = (1 << 8); |
||||
static constexpr uint16_t Bit9 = (1 << 9); |
||||
static constexpr uint16_t Bit10 = (1 << 10); |
||||
static constexpr uint16_t Bit11 = (1 << 11); |
||||
static constexpr uint16_t Bit12 = (1 << 12); |
||||
static constexpr uint16_t Bit13 = (1 << 13); |
||||
static constexpr uint16_t Bit14 = (1 << 14); |
||||
static constexpr uint16_t Bit15 = (1 << 15); |
||||
|
||||
namespace Analog_Devices_ADIS16448 |
||||
{ |
||||
static constexpr uint32_t SPI_SPEED = 2 * 1000 * 1000; // 2 MHz SPI serial interface
|
||||
static constexpr uint32_t SPI_SPEED_BURST = 1 * 1000 * 1000; // 1 MHz SPI serial interface for burst read
|
||||
|
||||
static constexpr uint32_t SPI_STALL_PERIOD = 9; // 9 us Stall period between data
|
||||
|
||||
static constexpr uint16_t DIR_WRITE = 0x80; |
||||
|
||||
static constexpr uint16_t Product_identification = 0x4040; |
||||
|
||||
static constexpr uint32_t SAMPLE_INTERVAL_US = (1e6f / 819.2f); // ~819.2 Hz
|
||||
|
||||
|
||||
enum class Register : uint16_t { |
||||
GPIO_CTRL = 0x32, // Auxiliary digital input/output control
|
||||
|
||||
MSC_CTRL = 0x34, // Miscellaneous control
|
||||
SMPL_PRD = 0x36, // Internal sample period (rate) control
|
||||
SENS_AVG = 0x38, // Dynamic range and digital filter control
|
||||
|
||||
DIAG_STAT = 0x3C, // System status
|
||||
GLOB_CMD = 0x3E, // System command
|
||||
|
||||
LOT_ID1 = 0x52, // Lot identification number
|
||||
LOT_ID2 = 0x54, // Lot identification number
|
||||
PROD_ID = 0x56, // Product identifier
|
||||
SERIAL_NUM = 0x58, // Lot-specific serial number
|
||||
}; |
||||
|
||||
// MSC_CTRL
|
||||
enum MSC_CTRL_BIT : uint16_t { |
||||
Checksum_memory_test = Bit11, // Checksum memory test (cleared upon completion)
|
||||
Internal_self_test = Bit10, // Internal self test (cleared upon completion)
|
||||
// Not used = Bit5
|
||||
CRC16_for_burst = Bit4, // include the CRC-16 code in burst read output sequence
|
||||
// Not used = Bit3
|
||||
Data_ready_enable = Bit2, |
||||
Data_ready_polarity = Bit1, // 1 = active high when data is valid
|
||||
Data_ready_line_select = Bit0, // Data ready line select 1 = DIO2, 0 = DIO1
|
||||
}; |
||||
|
||||
// DIAG_STAT
|
||||
enum DIAG_STAT_BIT : uint16_t { |
||||
Z_axis_accelerometer_self_test_failure = Bit15, // 1 = fail, 0 = pass
|
||||
Y_axis_accelerometer_self_test_failure = Bit14, // 1 = fail, 0 = pass
|
||||
X_axis_accelerometer_self_test_failure = Bit13, // 1 = fail, 0 = pass
|
||||
Z_axis_gyroscope_self_test_failure = Bit12, // 1 = fail, 0 = pass
|
||||
Y_axis_gyroscope_self_test_failure = Bit11, // 1 = fail, 0 = pass
|
||||
X_axis_gyroscope_self_test_failure = Bit10, // 1 = fail, 0 = pass
|
||||
|
||||
New_data_xMAGN_OUT_BARO_OUT = Bit7, // New data, xMAGN_OUT/BARO_OUT
|
||||
Flash_test_checksum_flag = Bit6, // 1 = fail, 0 = pass
|
||||
Self_test_diagnostic_error_flag = Bit5, // 1 = fail, 0 = pass
|
||||
|
||||
SPI_communication_failure = Bit3, // 1 = fail, 0 = pass
|
||||
|
||||
Barometer_functional_test = Bit1, // 1 = fail, 0 = pass
|
||||
Magnetometer_functional_test = Bit0, // 1 = fail, 0 = pass
|
||||
}; |
||||
|
||||
// GLOB_CMD
|
||||
enum GLOB_CMD_BIT : uint16_t { |
||||
Software_reset = Bit7, |
||||
}; |
||||
|
||||
// SMPL_PRD
|
||||
enum SMPL_PRD_BIT : uint16_t { |
||||
// [12:8] D, decimation rate setting, binomial,
|
||||
decimation_rate = Bit12 | Bit11 | Bit10 | Bit9, // disable
|
||||
|
||||
internal_sampling_clock = Bit0, // 1 = internal sampling clock, 819.2 SPS
|
||||
}; |
||||
|
||||
// SENS_AVG
|
||||
enum SENS_AVG_BIT : uint16_t { |
||||
// [10:8] Measurement range (sensitivity) selection
|
||||
Measurement_range_1000_set = Bit10, // 100 = ±1000°/sec (default condition)
|
||||
Measurement_range_1000_clear = Bit9 | Bit8, |
||||
|
||||
// [2:0] Filter Size Variable B
|
||||
Filter_Size_Variable_B = Bit2 | Bit1 | Bit0, // disable
|
||||
|
||||
}; |
||||
|
||||
// GPIO_CTRL
|
||||
enum GPIO_CTRL_BIT : uint16_t { |
||||
GPIO4_DATA_LEVEL = Bit11, // General-Purpose I/O Line 1 (DIO1) data level
|
||||
GPIO3_DATA_LEVEL = Bit10, // General-Purpose I/O Line 1 (DIO1) data level
|
||||
GPIO2_DATA_LEVEL = Bit9, // General-Purpose I/O Line 1 (DIO1) data level
|
||||
GPIO1_DATA_LEVEL = Bit8, // General-Purpose I/O Line 1 (DIO1) data level
|
||||
|
||||
GPIO4_DIRECTION = Bit3, // General-Purpose I/O Line 4 (DIO4) direction control 1 = output, 0 = input
|
||||
GPIO3_DIRECTION = Bit2, // General-Purpose I/O Line 3 (DIO3) direction control 1 = output, 0 = input
|
||||
GPIO2_DIRECTION = Bit1, // General-Purpose I/O Line 2 (DIO2) direction control 1 = output, 0 = input
|
||||
GPIO1_DIRECTION = Bit0, // General-Purpose I/O Line 1 (DIO1) direction control 1 = output, 0 = input
|
||||
}; |
||||
|
||||
} // namespace Analog_Devices_ADIS16448
|
||||
Loading…
Reference in new issue