mirror of
https://gitee.com/xiaohuolufeihua/bizhang_-obav.git
synced 2026-05-21 01:12:11 +00:00
Analog Devices ADIS16448 rewrite
- 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 lots
This commit is contained in:
Daniel Agar
committed by
Lorenz Meier
Lorenz Meier
parent
59b31e3c5b
commit
44df0fb7a2
@@ -30,7 +30,7 @@ px4_add_board(
|
||||
gps
|
||||
heater
|
||||
#imu # all available imu drivers
|
||||
imu/adis16448
|
||||
imu/analog_devices/adis16448
|
||||
imu/adis16477
|
||||
imu/adis16497
|
||||
imu/bosch/bmi088
|
||||
|
||||
@@ -30,7 +30,7 @@ px4_add_board(
|
||||
gps
|
||||
#heater
|
||||
#imu # all available imu drivers
|
||||
imu/adis16448
|
||||
imu/analog_devices/adis16448
|
||||
imu/adis16477
|
||||
imu/adis16497
|
||||
imu/invensense/icm20602
|
||||
|
||||
@@ -30,7 +30,7 @@ px4_add_board(
|
||||
gps
|
||||
#heater
|
||||
#imu # all available imu drivers
|
||||
imu/adis16448
|
||||
imu/analog_devices/adis16448
|
||||
imu/adis16477
|
||||
imu/adis16497
|
||||
imu/invensense/icm20602
|
||||
|
||||
@@ -29,7 +29,7 @@ px4_add_board(
|
||||
gps
|
||||
#heater
|
||||
#imu # all available imu drivers
|
||||
imu/adis16448
|
||||
imu/analog_devices/adis16448
|
||||
imu/adis16477
|
||||
imu/adis16497
|
||||
imu/invensense/icm20602
|
||||
|
||||
@@ -29,7 +29,7 @@ px4_add_board(
|
||||
gps
|
||||
#heater
|
||||
#imu # all available imu drivers
|
||||
imu/adis16448
|
||||
imu/analog_devices/adis16448
|
||||
imu/adis16477
|
||||
imu/adis16497
|
||||
imu/invensense/icm20602
|
||||
|
||||
@@ -29,7 +29,7 @@ px4_add_board(
|
||||
gps
|
||||
heater
|
||||
#imu # all available imu drivers
|
||||
imu/adis16448
|
||||
imu/analog_devices/adis16448
|
||||
imu/adis16477
|
||||
imu/adis16497
|
||||
imu/bosch/bmi088
|
||||
|
||||
@@ -28,7 +28,7 @@ px4_add_board(
|
||||
gps
|
||||
#heater
|
||||
#imu # all available imu drivers
|
||||
imu/adis16448
|
||||
imu/analog_devices/adis16448
|
||||
imu/adis16477
|
||||
imu/adis16497
|
||||
imu/bosch/bmi055
|
||||
|
||||
@@ -24,7 +24,7 @@ px4_add_board(
|
||||
distance_sensor # all available distance sensor drivers
|
||||
# dshot not ported
|
||||
gps
|
||||
#imu/adis16448
|
||||
#imu/analog_devices/adis16448
|
||||
#imu/adis16477
|
||||
#imu/adis16497
|
||||
#imu # all available imu drivers
|
||||
|
||||
@@ -34,7 +34,7 @@ px4_add_board(
|
||||
gps
|
||||
#heater
|
||||
#imu # all available imu drivers
|
||||
#imu/adis16448
|
||||
#imu/analog_devices/adis16448
|
||||
#imu/adis16477
|
||||
#imu/adis16497
|
||||
imu/l3gd20
|
||||
|
||||
@@ -32,7 +32,7 @@ px4_add_board(
|
||||
distance_sensor # all available distance sensor drivers
|
||||
gps
|
||||
#heater
|
||||
#imu/adis16448
|
||||
#imu/analog_devices/adis16448
|
||||
#imu # all available imu drivers
|
||||
imu/l3gd20
|
||||
imu/lsm303d
|
||||
|
||||
@@ -33,7 +33,7 @@ px4_add_board(
|
||||
gps
|
||||
#heater
|
||||
#imu # all available imu drivers
|
||||
#imu/adis16448
|
||||
#imu/analog_devices/adis16448
|
||||
#imu/adis16477
|
||||
#imu/adis16497
|
||||
imu/l3gd20
|
||||
|
||||
@@ -31,7 +31,7 @@ px4_add_board(
|
||||
gps
|
||||
#heater
|
||||
#imu # all available imu drivers
|
||||
imu/adis16448
|
||||
imu/analog_devices/adis16448
|
||||
imu/adis16477
|
||||
imu/adis16497
|
||||
imu/l3gd20
|
||||
|
||||
@@ -31,7 +31,7 @@ px4_add_board(
|
||||
gps
|
||||
#heater
|
||||
#imu # all available imu drivers
|
||||
imu/adis16448
|
||||
imu/analog_devices/adis16448
|
||||
imu/adis16477
|
||||
imu/adis16497
|
||||
imu/l3gd20
|
||||
|
||||
@@ -30,7 +30,7 @@ px4_add_board(
|
||||
gps
|
||||
#heater
|
||||
#imu # all available imu drivers
|
||||
imu/adis16448
|
||||
imu/analog_devices/adis16448
|
||||
imu/adis16477
|
||||
imu/adis16497
|
||||
imu/l3gd20
|
||||
|
||||
@@ -30,7 +30,7 @@ px4_add_board(
|
||||
distance_sensor # all available distance sensor drivers
|
||||
gps
|
||||
#heater
|
||||
imu/adis16448
|
||||
imu/analog_devices/adis16448
|
||||
#imu # all available imu drivers
|
||||
imu/l3gd20
|
||||
imu/lsm303d
|
||||
|
||||
@@ -42,7 +42,7 @@ px4_add_board(
|
||||
#dshot
|
||||
gps
|
||||
#imu # all available imu drivers
|
||||
#imu/adis16448
|
||||
#imu/analog_devices/adis16448
|
||||
#imu/adis16477
|
||||
#imu/adis16497
|
||||
imu/invensense/icm20602
|
||||
|
||||
@@ -29,7 +29,7 @@ px4_add_board(
|
||||
gps
|
||||
heater
|
||||
#imu # all available imu drivers
|
||||
imu/adis16448
|
||||
imu/analog_devices/adis16448
|
||||
imu/adis16477
|
||||
imu/adis16497
|
||||
imu/invensense/icm20602
|
||||
|
||||
@@ -29,7 +29,7 @@ px4_add_board(
|
||||
gps
|
||||
heater
|
||||
#imu # all available imu drivers
|
||||
imu/adis16448
|
||||
imu/analog_devices/adis16448
|
||||
imu/adis16477
|
||||
imu/adis16497
|
||||
imu/invensense/icm20602
|
||||
|
||||
@@ -30,7 +30,7 @@ px4_add_board(
|
||||
gps
|
||||
#heater
|
||||
#imu # all available imu drivers
|
||||
#imu/adis16448
|
||||
#imu/analog_devices/adis16448
|
||||
#imu/adis16477
|
||||
#imu/adis16497
|
||||
imu/invensense/icm20602
|
||||
|
||||
@@ -26,7 +26,7 @@ px4_add_board(
|
||||
gps
|
||||
heater
|
||||
#imu # all available imu drivers
|
||||
imu/adis16448
|
||||
imu/analog_devices/adis16448
|
||||
imu/adis16477
|
||||
imu/adis16497
|
||||
imu/invensense/icm20602
|
||||
|
||||
@@ -26,7 +26,7 @@ px4_add_board(
|
||||
gps
|
||||
heater
|
||||
#imu # all available imu drivers
|
||||
#imu/adis16448
|
||||
#imu/analog_devices/adis16448
|
||||
#imu/adis16477
|
||||
#imu/adis16497
|
||||
imu/invensense/icm20602
|
||||
|
||||
@@ -28,7 +28,7 @@ px4_add_board(
|
||||
gps
|
||||
heater
|
||||
#imu # all available imu drivers
|
||||
imu/adis16448
|
||||
imu/analog_devices/adis16448
|
||||
imu/adis16477
|
||||
imu/adis16497
|
||||
imu/invensense/icm20602
|
||||
|
||||
@@ -27,7 +27,7 @@ px4_add_board(
|
||||
gps
|
||||
#heater
|
||||
#imu # all available imu drivers
|
||||
imu/adis16448
|
||||
imu/analog_devices/adis16448
|
||||
imu/adis16477
|
||||
imu/adis16497
|
||||
imu/bosch/bmi055
|
||||
|
||||
@@ -28,7 +28,7 @@ px4_add_board(
|
||||
gps
|
||||
#heater
|
||||
#imu # all available imu drivers
|
||||
imu/adis16448
|
||||
imu/analog_devices/adis16448
|
||||
imu/adis16477
|
||||
imu/adis16497
|
||||
imu/bosch/bmi055
|
||||
|
||||
@@ -29,7 +29,7 @@ px4_add_board(
|
||||
gps
|
||||
#heater
|
||||
#imu # all available imu drivers
|
||||
#imu/adis16448
|
||||
#imu/analog_devices/adis16448
|
||||
#imu/adis16477
|
||||
#imu/adis16497
|
||||
#imu/bosch/bmi055
|
||||
|
||||
@@ -28,7 +28,7 @@ px4_add_board(
|
||||
gps
|
||||
#heater
|
||||
#imu # all available imu drivers
|
||||
imu/adis16448
|
||||
imu/analog_devices/adis16448
|
||||
imu/adis16477
|
||||
imu/adis16497
|
||||
imu/bosch/bmi055
|
||||
|
||||
@@ -23,7 +23,7 @@ px4_add_board(
|
||||
differential_pressure # all available differential pressure drivers
|
||||
distance_sensor # all available distance sensor drivers
|
||||
gps
|
||||
imu/adis16448
|
||||
imu/analog_devices/adis16448
|
||||
imu/adis16477
|
||||
imu/adis16497
|
||||
#imu # all available imu drivers
|
||||
|
||||
@@ -27,7 +27,7 @@ px4_add_board(
|
||||
gps
|
||||
#heater
|
||||
#imu # all available imu drivers
|
||||
imu/adis16448
|
||||
imu/analog_devices/adis16448
|
||||
imu/adis16477
|
||||
imu/adis16497
|
||||
imu/bosch/bmi055
|
||||
|
||||
@@ -24,7 +24,7 @@ px4_add_board(
|
||||
distance_sensor # all available distance sensor drivers
|
||||
dshot
|
||||
gps
|
||||
imu/adis16448
|
||||
imu/analog_devices/adis16448
|
||||
imu/adis16477
|
||||
imu/adis16497
|
||||
#imu # all available imu drivers
|
||||
|
||||
@@ -22,7 +22,7 @@ px4_add_board(
|
||||
camera_trigger
|
||||
distance_sensor # all available distance sensor drivers
|
||||
gps
|
||||
imu/adis16448
|
||||
imu/analog_devices/adis16448
|
||||
imu/adis16477
|
||||
imu/adis16497
|
||||
imu/bosch/bmi055
|
||||
|
||||
@@ -27,7 +27,7 @@ px4_add_board(
|
||||
gps
|
||||
#heater
|
||||
#imu # all available imu drivers
|
||||
imu/adis16448
|
||||
imu/analog_devices/adis16448
|
||||
imu/adis16477
|
||||
imu/adis16497
|
||||
imu/bosch/bmi055
|
||||
|
||||
@@ -29,7 +29,7 @@ px4_add_board(
|
||||
gps
|
||||
#heater
|
||||
#imu # all available imu drivers
|
||||
#imu/adis16448
|
||||
#imu/analog_devices/adis16448
|
||||
#imu/adis16477
|
||||
#imu/adis16497
|
||||
#imu/bosch/bmi055
|
||||
|
||||
@@ -28,7 +28,7 @@ px4_add_board(
|
||||
gps
|
||||
#heater
|
||||
#imu # all available imu drivers
|
||||
imu/adis16448
|
||||
imu/analog_devices/adis16448
|
||||
imu/adis16477
|
||||
imu/adis16497
|
||||
imu/bosch/bmi055
|
||||
|
||||
@@ -28,7 +28,7 @@ px4_add_board(
|
||||
gps
|
||||
#heater
|
||||
#imu # all available imu drivers
|
||||
imu/adis16448
|
||||
imu/analog_devices/adis16448
|
||||
imu/adis16477
|
||||
imu/adis16497
|
||||
imu/bosch/bmi088
|
||||
|
||||
@@ -29,7 +29,7 @@ px4_add_board(
|
||||
gps
|
||||
#heater
|
||||
#imu # all available imu drivers
|
||||
imu/adis16448
|
||||
imu/analog_devices/adis16448
|
||||
imu/adis16477
|
||||
imu/adis16497
|
||||
imu/bosch/bmi088
|
||||
|
||||
@@ -28,7 +28,7 @@ px4_add_board(
|
||||
gps
|
||||
heater
|
||||
#imu # all available imu drivers
|
||||
imu/adis16448
|
||||
imu/analog_devices/adis16448
|
||||
imu/adis16477
|
||||
imu/adis16497
|
||||
imu/bosch/bmi088
|
||||
|
||||
@@ -28,7 +28,7 @@ px4_add_board(
|
||||
gps
|
||||
heater
|
||||
#imu # all available imu drivers
|
||||
imu/adis16448
|
||||
imu/analog_devices/adis16448
|
||||
imu/adis16477
|
||||
imu/adis16497
|
||||
imu/bosch/bmi088
|
||||
|
||||
@@ -29,7 +29,7 @@ px4_add_board(
|
||||
gps
|
||||
heater
|
||||
#imu # all available imu drivers
|
||||
imu/adis16448
|
||||
imu/analog_devices/adis16448
|
||||
imu/adis16477
|
||||
imu/adis16497
|
||||
imu/bosch/bmi088
|
||||
|
||||
@@ -29,7 +29,7 @@ px4_add_board(
|
||||
gps
|
||||
heater
|
||||
#imu # all available imu drivers
|
||||
imu/adis16448
|
||||
imu/analog_devices/adis16448
|
||||
imu/adis16477
|
||||
imu/adis16497
|
||||
imu/bosch/bmi088
|
||||
|
||||
@@ -24,7 +24,7 @@ px4_add_board(
|
||||
gps
|
||||
#heater
|
||||
#imu # all available imu drivers
|
||||
#imu/adis16448
|
||||
#imu/analog_devices/adis16448
|
||||
#imu/adis16477
|
||||
#imu/adis16497
|
||||
#imu/bmi088
|
||||
|
||||
@@ -31,9 +31,9 @@
|
||||
#
|
||||
############################################################################
|
||||
|
||||
add_subdirectory(adis16448)
|
||||
add_subdirectory(adis16477)
|
||||
add_subdirectory(adis16497)
|
||||
add_subdirectory(analog_devices)
|
||||
add_subdirectory(bma180)
|
||||
add_subdirectory(bmi055)
|
||||
add_subdirectory(bmi088)
|
||||
|
||||
@@ -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 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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"))};
|
||||
|
||||
};
|
||||
34
src/drivers/imu/analog_devices/CMakeLists.txt
Normal file
34
src/drivers/imu/analog_devices/CMakeLists.txt
Normal file
@@ -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)
|
||||
594
src/drivers/imu/analog_devices/adis16448/ADIS16448.cpp
Normal file
594
src/drivers/imu/analog_devices/adis16448/ADIS16448.cpp
Normal file
@@ -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);
|
||||
}
|
||||
}
|
||||
138
src/drivers/imu/analog_devices/adis16448/ADIS16448.hpp
Normal file
138
src/drivers/imu/analog_devices/adis16448/ADIS16448.hpp
Normal file
@@ -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 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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
|
||||
@@ -1,6 +1,6 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2019 PX4 Development Team. All rights reserved.
|
||||
# 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
|
||||
@@ -30,18 +30,22 @@
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_module(
|
||||
MODULE drivers__imu__adis16448
|
||||
MODULE drivers__imu__analog_devices__adis16448
|
||||
MAIN adis16448
|
||||
COMPILE_FLAGS
|
||||
-Wno-cast-align # TODO: fix and enable
|
||||
${MAX_CUSTOM_OPT_LEVEL}
|
||||
#-DDEBUG_BUILD
|
||||
SRCS
|
||||
ADIS16448.cpp
|
||||
ADIS16448.hpp
|
||||
adis16448_main.cpp
|
||||
Analog_Devices_ADIS16448_registers.hpp
|
||||
DEPENDS
|
||||
px4_work_queue
|
||||
drivers_accelerometer
|
||||
drivers_barometer
|
||||
drivers_gyroscope
|
||||
drivers_magnetometer
|
||||
px4_work_queue
|
||||
)
|
||||
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2018-2019 PX4 Development Team. All rights reserved.
|
||||
* 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
|
||||
@@ -31,13 +31,12 @@
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "ADIS16448.h"
|
||||
#include "ADIS16448.hpp"
|
||||
|
||||
#include <px4_platform_common/getopt.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
|
||||
void
|
||||
ADIS16448::print_usage()
|
||||
void ADIS16448::print_usage()
|
||||
{
|
||||
PRINT_MODULE_USAGE_NAME("adis16448", "driver");
|
||||
PRINT_MODULE_USAGE_SUBCATEGORY("imu");
|
||||
@@ -51,7 +50,7 @@ I2CSPIDriverBase *ADIS16448::instantiate(const BusCLIArguments &cli, const BusIn
|
||||
int runtime_instance)
|
||||
{
|
||||
ADIS16448 *instance = new ADIS16448(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation,
|
||||
cli.bus_frequency, cli.spi_mode);
|
||||
cli.bus_frequency, iterator.DRDYGPIO());
|
||||
|
||||
if (!instance) {
|
||||
PX4_ERR("alloc failed");
|
||||
@@ -71,7 +70,7 @@ extern "C" int adis16448_main(int argc, char *argv[])
|
||||
int ch;
|
||||
using ThisDriver = ADIS16448;
|
||||
BusCLIArguments cli{false, true};
|
||||
cli.default_spi_frequency = 1000000;
|
||||
cli.default_spi_frequency = SPI_SPEED;
|
||||
|
||||
while ((ch = cli.getopt(argc, argv, "R:")) != EOF) {
|
||||
switch (ch) {
|
||||
@@ -92,13 +91,11 @@ extern "C" int adis16448_main(int argc, char *argv[])
|
||||
|
||||
if (!strcmp(verb, "start")) {
|
||||
return ThisDriver::module_start(cli, iterator);
|
||||
}
|
||||
|
||||
if (!strcmp(verb, "stop")) {
|
||||
} else if (!strcmp(verb, "stop")) {
|
||||
return ThisDriver::module_stop(iterator);
|
||||
}
|
||||
|
||||
if (!strcmp(verb, "status")) {
|
||||
} else if (!strcmp(verb, "status")) {
|
||||
return ThisDriver::module_status(iterator);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user