mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-18 14:03:34 +08:00
Analog Device ADIS16448 cleanup
- move to PX4Accelerometer, PX4Gyroscope, PX4Magnetometer helpers - add barometer support (using PX4Barometer)
This commit is contained in:
Daniel Agar
@@ -0,0 +1,541 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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(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_read_interval);
|
||||
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_read_interval);
|
||||
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()
|
||||
{
|
||||
perf_count(_perf_read_interval);
|
||||
|
||||
// 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();
|
||||
}
|
||||
@@ -0,0 +1,219 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 <ecl/geo/geo.h>
|
||||
#include <perf/perf_counter.h>
|
||||
#include <drivers/device/spi.h>
|
||||
#include <px4_work_queue/ScheduledWorkItem.hpp>
|
||||
#include <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>
|
||||
|
||||
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 px4::ScheduledWorkItem
|
||||
{
|
||||
public:
|
||||
ADIS16448(int bus, uint32_t device, enum Rotation rotation);
|
||||
virtual ~ADIS16448();
|
||||
|
||||
virtual int init();
|
||||
|
||||
/**
|
||||
* Diagnostics - print some basic information about the driver and sensor.
|
||||
*/
|
||||
void print_info();
|
||||
|
||||
protected:
|
||||
virtual int probe();
|
||||
|
||||
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();
|
||||
|
||||
/**
|
||||
* Static trampoline from the hrt_call context; because we don't have a
|
||||
* generic hrt wrapper yet.
|
||||
* Called by the HRT in interrupt context at the specified rate if
|
||||
* automatic polling is enabled.
|
||||
*
|
||||
* @param arg Instance pointer for the driver that is polling.
|
||||
*/
|
||||
void Run();
|
||||
|
||||
/**
|
||||
* 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();
|
||||
|
||||
/**
|
||||
* Stop automatic measurement.
|
||||
*/
|
||||
void stop();
|
||||
|
||||
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_read_interval{perf_alloc(PC_INTERVAL, "ADIS16448: read interval")};
|
||||
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"))};
|
||||
|
||||
};
|
||||
@@ -1,6 +1,6 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2015 PX4 Development Team. All rights reserved.
|
||||
# Copyright (c) 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
|
||||
@@ -31,11 +31,18 @@
|
||||
#
|
||||
############################################################################
|
||||
px4_add_module(
|
||||
MODULE drivers__adis16448
|
||||
MODULE drivers__imu__adis16448
|
||||
MAIN adis16448
|
||||
STACK_MAIN 1200
|
||||
STACK_MAIN 1400
|
||||
COMPILE_FLAGS
|
||||
-Wno-cast-align # TODO: fix and enable
|
||||
SRCS
|
||||
adis16448.cpp
|
||||
ADIS16448.cpp
|
||||
adis16448_main.cpp
|
||||
DEPENDS
|
||||
px4_work_queue
|
||||
drivers_accelerometer
|
||||
drivers_barometer
|
||||
drivers_gyroscope
|
||||
drivers_magnetometer
|
||||
)
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,166 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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"
|
||||
|
||||
/**
|
||||
* Local functions in support of the shell command.
|
||||
*/
|
||||
namespace adis16448
|
||||
{
|
||||
|
||||
ADIS16448 *g_dev;
|
||||
|
||||
int info();
|
||||
int start(enum Rotation rotation);
|
||||
int stop();
|
||||
void usage();
|
||||
|
||||
/**
|
||||
* Start the driver.
|
||||
*/
|
||||
int
|
||||
start(enum Rotation rotation)
|
||||
{
|
||||
if (g_dev != nullptr) {
|
||||
// If already started, the still command succeeded.
|
||||
PX4_INFO("already started");
|
||||
}
|
||||
|
||||
// Create the driver.
|
||||
#if defined(PX4_SPI_BUS_EXT)
|
||||
g_dev = new ADIS16448(PX4_SPI_BUS_EXT, PX4_SPIDEV_EXT_MPU, rotation);
|
||||
#else
|
||||
PX4_ERR("External SPI not available");
|
||||
#endif
|
||||
|
||||
if (g_dev != nullptr) {
|
||||
if (g_dev->init() == OK) {
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
delete g_dev;
|
||||
g_dev = nullptr;
|
||||
}
|
||||
|
||||
PX4_ERR("driver start failed");
|
||||
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
int stop()
|
||||
{
|
||||
if (g_dev == nullptr) {
|
||||
PX4_INFO("driver not running");
|
||||
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
delete g_dev;
|
||||
g_dev = nullptr;
|
||||
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
/**
|
||||
* Print a little info about the driver.
|
||||
*/
|
||||
int
|
||||
info()
|
||||
{
|
||||
if (g_dev == nullptr) {
|
||||
PX4_INFO("driver not running");
|
||||
}
|
||||
|
||||
g_dev->print_info();
|
||||
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
void
|
||||
usage()
|
||||
{
|
||||
PX4_INFO("missing command: try 'start', 'info', 'stop'");
|
||||
PX4_INFO("options:");
|
||||
PX4_INFO(" -R rotation");
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
|
||||
/**
|
||||
* Driver 'main' command.
|
||||
*/
|
||||
extern "C" int adis16448_main(int argc, char *argv[])
|
||||
{
|
||||
enum Rotation rotation = ROTATION_NONE;
|
||||
int ch;
|
||||
|
||||
/* start options */
|
||||
while ((ch = getopt(argc, argv, "R:")) != EOF) {
|
||||
switch (ch) {
|
||||
case 'R':
|
||||
rotation = (enum Rotation)atoi(optarg);
|
||||
break;
|
||||
|
||||
default:
|
||||
adis16448::usage();
|
||||
exit(0);
|
||||
}
|
||||
}
|
||||
|
||||
const char *verb = argv[optind];
|
||||
|
||||
// Start/load the driver.
|
||||
if (!strcmp(verb, "start")) {
|
||||
return adis16448::start(rotation);
|
||||
}
|
||||
|
||||
// Print driver information.
|
||||
if (!strcmp(verb, "info")) {
|
||||
return adis16448::info();
|
||||
}
|
||||
|
||||
// Stop
|
||||
if (!strcmp(verb, "stop")) {
|
||||
return adis16448::stop();
|
||||
}
|
||||
|
||||
adis16448::usage();
|
||||
|
||||
return PX4_OK;
|
||||
}
|
||||
Reference in New Issue
Block a user