mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-14 20:33:34 +08:00
adis16448: add additional delay after transfer in case of back to back transcations
- add verified register read method
This commit is contained in:
Daniel Agar
committed by
Florian Achermann
Florian Achermann
parent
01307a43fe
commit
a65a591638
@@ -105,6 +105,11 @@ ADIS16448::ADIS16448(I2CSPIBusOption bus_option, int bus, uint32_t device, enum
|
||||
_px4_gyro(get_device_id(), rotation),
|
||||
_px4_mag(get_device_id(), rotation)
|
||||
{
|
||||
_debug_enabled = true;
|
||||
|
||||
if (_drdy_gpio != 0) {
|
||||
_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed");
|
||||
}
|
||||
}
|
||||
|
||||
ADIS16448::~ADIS16448()
|
||||
@@ -160,12 +165,12 @@ int ADIS16448::probe()
|
||||
}
|
||||
|
||||
for (int attempt = 0; attempt < 3; attempt++) {
|
||||
const uint16_t PROD_ID = RegisterRead(Register::PROD_ID);
|
||||
const uint16_t PROD_ID = RegisterReadVerified(Register::PROD_ID);
|
||||
|
||||
if (PROD_ID == Product_identification) {
|
||||
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);
|
||||
const uint16_t SERIAL_NUM = RegisterReadVerified(Register::SERIAL_NUM);
|
||||
const uint16_t LOT_ID1 = RegisterReadVerified(Register::LOT_ID1);
|
||||
const uint16_t LOT_ID2 = RegisterReadVerified(Register::LOT_ID2);
|
||||
|
||||
PX4_INFO("Serial Number: 0x%02x, Lot ID1: 0x%02x ID2: 0x%02x", SERIAL_NUM, LOT_ID1, LOT_ID2);
|
||||
|
||||
@@ -197,7 +202,7 @@ void ADIS16448::RunImpl()
|
||||
case STATE::WAIT_FOR_RESET:
|
||||
|
||||
if (_self_test_passed) {
|
||||
if ((RegisterRead(Register::PROD_ID) == Product_identification)) {
|
||||
if ((RegisterReadVerified(Register::PROD_ID) == Product_identification)) {
|
||||
// if reset succeeded then configure
|
||||
_state = STATE::CONFIGURE;
|
||||
ScheduleNow();
|
||||
@@ -224,7 +229,7 @@ void ADIS16448::RunImpl()
|
||||
break;
|
||||
|
||||
case STATE::SELF_TEST_CHECK: {
|
||||
const uint16_t MSC_CTRL = RegisterRead(Register::MSC_CTRL);
|
||||
const uint16_t MSC_CTRL = RegisterReadVerified(Register::MSC_CTRL);
|
||||
|
||||
if (MSC_CTRL & MSC_CTRL_BIT::Internal_self_test) {
|
||||
// self test not finished, check again
|
||||
@@ -244,7 +249,7 @@ void ADIS16448::RunImpl()
|
||||
|
||||
bool test_passed = true;
|
||||
|
||||
const uint16_t DIAG_STAT = RegisterRead(Register::DIAG_STAT);
|
||||
const uint16_t DIAG_STAT = RegisterReadVerified(Register::DIAG_STAT);
|
||||
|
||||
if (DIAG_STAT & DIAG_STAT_BIT::Self_test_diagnostic_error_flag) {
|
||||
PX4_ERR("self test failed");
|
||||
@@ -477,7 +482,7 @@ void ADIS16448::RunImpl()
|
||||
|
||||
bool ADIS16448::Configure()
|
||||
{
|
||||
const uint16_t LOT_ID1 = RegisterRead(Register::LOT_ID1);
|
||||
const uint16_t LOT_ID1 = RegisterReadVerified(Register::LOT_ID1);
|
||||
|
||||
// Only enable CRC-16 for verified lots (HACK to support older ADIS16448AMLZ with no explicit detection)
|
||||
if (LOT_ID1 == 0x1824) {
|
||||
@@ -603,7 +608,7 @@ bool ADIS16448::RegisterCheck(const register_config_t ®_cfg)
|
||||
{
|
||||
bool success = true;
|
||||
|
||||
const uint16_t reg_value = RegisterRead(reg_cfg.reg);
|
||||
const uint16_t reg_value = RegisterReadVerified(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);
|
||||
@@ -628,10 +633,27 @@ uint16_t ADIS16448::RegisterRead(Register reg)
|
||||
transferhword(cmd, nullptr, 1);
|
||||
px4_udelay(SPI_STALL_PERIOD);
|
||||
transferhword(nullptr, cmd, 1);
|
||||
px4_udelay(SPI_STALL_PERIOD);
|
||||
|
||||
return cmd[0];
|
||||
}
|
||||
|
||||
uint16_t ADIS16448::RegisterReadVerified(Register reg)
|
||||
{
|
||||
// 3 attempts
|
||||
for (int i = 0; i < 3; i++) {
|
||||
uint16_t read1 = RegisterRead(reg);
|
||||
uint16_t read2 = RegisterRead(reg);
|
||||
|
||||
if (read1 == read2) {
|
||||
return read1;
|
||||
}
|
||||
}
|
||||
|
||||
// failed
|
||||
return 0;
|
||||
}
|
||||
|
||||
void ADIS16448::RegisterWrite(Register reg, uint16_t value)
|
||||
{
|
||||
set_frequency(SPI_SPEED);
|
||||
@@ -643,11 +665,12 @@ void ADIS16448::RegisterWrite(Register reg, uint16_t value)
|
||||
transferhword(cmd, nullptr, 1);
|
||||
px4_udelay(SPI_STALL_PERIOD);
|
||||
transferhword(cmd + 1, nullptr, 1);
|
||||
px4_udelay(SPI_STALL_PERIOD);
|
||||
}
|
||||
|
||||
void ADIS16448::RegisterSetAndClearBits(Register reg, uint16_t setbits, uint16_t clearbits)
|
||||
{
|
||||
const uint16_t orig_val = RegisterRead(reg);
|
||||
const uint16_t orig_val = RegisterReadVerified(reg);
|
||||
|
||||
uint16_t val = (orig_val & ~clearbits) | setbits;
|
||||
|
||||
|
||||
@@ -94,6 +94,7 @@ private:
|
||||
bool RegisterCheck(const register_config_t ®_cfg);
|
||||
|
||||
uint16_t RegisterRead(Register reg);
|
||||
uint16_t RegisterReadVerified(Register reg);
|
||||
void RegisterWrite(Register reg, uint16_t value);
|
||||
void RegisterSetAndClearBits(Register reg, uint16_t setbits, uint16_t clearbits);
|
||||
|
||||
|
||||
@@ -65,7 +65,7 @@ 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 = 10; // 9 us Stall period between data
|
||||
static constexpr uint32_t SPI_STALL_PERIOD = 11; // 9 us Stall period between data
|
||||
|
||||
static constexpr uint16_t DIR_WRITE = 0x80;
|
||||
|
||||
|
||||
@@ -36,7 +36,7 @@ px4_add_module(
|
||||
MAIN adis16448
|
||||
COMPILE_FLAGS
|
||||
${MAX_CUSTOM_OPT_LEVEL}
|
||||
#-DDEBUG_BUILD
|
||||
-DDEBUG_BUILD
|
||||
SRCS
|
||||
ADIS16448.cpp
|
||||
ADIS16448.hpp
|
||||
|
||||
Reference in New Issue
Block a user