forked from Archive/PX4-Autopilot
drivers/imu/analog_devices/adis16448: minor fixes and compatibility with older model
- increase SPI stall time slightly - tolerate mag self test failure (could be due to local magnetic field) - register configuration compatible with older ADIS16448AMLZ - don't publish duplicate accel/gyro - only allocate CRC perf counter if using CRC
This commit is contained in:
Daniel Agar
GitHub
parent
eee08601af
commit
820a442fe3
|
|
@ -110,9 +110,9 @@ ADIS16448::ADIS16448(I2CSPIBusOption bus_option, int bus, uint32_t device, enum
|
|||
ADIS16448::~ADIS16448()
|
||||
{
|
||||
perf_free(_reset_perf);
|
||||
perf_free(_perf_crc_bad);
|
||||
perf_free(_bad_register_perf);
|
||||
perf_free(_bad_transfer_perf);
|
||||
perf_free(_perf_crc_bad);
|
||||
}
|
||||
|
||||
int ADIS16448::init()
|
||||
|
|
@ -147,9 +147,9 @@ 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);
|
||||
perf_print_counter(_perf_crc_bad);
|
||||
}
|
||||
|
||||
int ADIS16448::probe()
|
||||
|
|
@ -159,25 +159,24 @@ int ADIS16448::probe()
|
|||
PX4_WARN("Power-On Start-Up Time is 205 ms");
|
||||
}
|
||||
|
||||
const uint16_t PROD_ID = RegisterRead(Register::PROD_ID);
|
||||
for (int attempt = 0; attempt < 3; attempt++) {
|
||||
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;
|
||||
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);
|
||||
|
||||
PX4_INFO("Serial Number: 0x%02x, Lot ID1: 0x%02x ID2: 0x%02x", SERIAL_NUM, LOT_ID1, LOT_ID2);
|
||||
|
||||
return PX4_OK;
|
||||
|
||||
} else {
|
||||
DEVICE_DEBUG("unexpected PROD_ID 0x%02x", PROD_ID);
|
||||
}
|
||||
}
|
||||
|
||||
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;
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
void ADIS16448::RunImpl()
|
||||
|
|
@ -211,20 +210,40 @@ void ADIS16448::RunImpl()
|
|||
ScheduleDelayed(100_ms);
|
||||
|
||||
} else {
|
||||
PX4_DEBUG("Reset not complete, check again in 10 ms");
|
||||
ScheduleDelayed(10_ms);
|
||||
PX4_DEBUG("Reset not complete, check again in 100 ms");
|
||||
ScheduleDelayed(100_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
|
||||
ScheduleDelayed(90_ms); // Automatic Self-Test Time > 45 ms
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case STATE::SELF_TEST_CHECK: {
|
||||
const uint16_t MSC_CTRL = RegisterRead(Register::MSC_CTRL);
|
||||
|
||||
if (MSC_CTRL & MSC_CTRL_BIT::Internal_self_test) {
|
||||
// self test not finished, check again
|
||||
if (hrt_elapsed_time(&_reset_timestamp) < 1000_ms) {
|
||||
ScheduleDelayed(45_ms);
|
||||
PX4_DEBUG("self test not complete, check again in 45 ms");
|
||||
return;
|
||||
|
||||
} else {
|
||||
// still not cleared, fail self test
|
||||
_self_test_passed = false;
|
||||
_state = STATE::RESET;
|
||||
ScheduleDelayed(1000_ms);
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
bool test_passed = true;
|
||||
|
||||
const uint16_t DIAG_STAT = RegisterRead(Register::DIAG_STAT);
|
||||
|
||||
if (DIAG_STAT & DIAG_STAT_BIT::Self_test_diagnostic_error_flag) {
|
||||
|
|
@ -248,6 +267,7 @@ void ADIS16448::RunImpl()
|
|||
|
||||
if (gyro_x_fail || gyro_y_fail || gyro_z_fail) {
|
||||
PX4_ERR("gyroscope self-test failure");
|
||||
test_passed = false;
|
||||
}
|
||||
|
||||
// Accelerometer
|
||||
|
|
@ -257,18 +277,20 @@ void ADIS16448::RunImpl()
|
|||
|
||||
if (accel_x_fail || accel_y_fail || accel_z_fail) {
|
||||
PX4_ERR("accelerometer self-test failure");
|
||||
test_passed = false;
|
||||
}
|
||||
}
|
||||
|
||||
_self_test_passed = false;
|
||||
_state = STATE::RESET;
|
||||
ScheduleDelayed(1000_ms);
|
||||
|
||||
} else {
|
||||
if (test_passed) {
|
||||
PX4_DEBUG("self test passed");
|
||||
_self_test_passed = true;
|
||||
_state = STATE::RESET;
|
||||
ScheduleNow();
|
||||
|
||||
} else {
|
||||
_self_test_passed = false;
|
||||
}
|
||||
|
||||
_state = STATE::RESET;
|
||||
ScheduleDelayed(10_ms);
|
||||
}
|
||||
|
||||
break;
|
||||
|
|
@ -364,18 +386,38 @@ void ADIS16448::RunImpl()
|
|||
_px4_accel.set_temperature(temperature);
|
||||
_px4_gyro.set_temperature(temperature);
|
||||
|
||||
bool imu_updated = false;
|
||||
|
||||
// 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;
|
||||
|
||||
if (accel_x != _accel_prev[0] || accel_y != _accel_prev[1] || accel_z != _accel_prev[2]) {
|
||||
imu_updated = true;
|
||||
|
||||
_accel_prev[0] = accel_x;
|
||||
_accel_prev[1] = accel_y;
|
||||
_accel_prev[2] = accel_z;
|
||||
}
|
||||
|
||||
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);
|
||||
if (gyro_x != _gyro_prev[0] || gyro_y != _gyro_prev[1] || gyro_z != _gyro_prev[2]) {
|
||||
imu_updated = true;
|
||||
|
||||
_gyro_prev[0] = gyro_x;
|
||||
_gyro_prev[1] = gyro_y;
|
||||
_gyro_prev[2] = gyro_z;
|
||||
}
|
||||
|
||||
if (imu_updated) {
|
||||
_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) {
|
||||
|
|
@ -435,6 +477,27 @@ void ADIS16448::RunImpl()
|
|||
|
||||
bool ADIS16448::Configure()
|
||||
{
|
||||
const uint16_t LOT_ID1 = RegisterRead(Register::LOT_ID1);
|
||||
|
||||
// Only enable CRC-16 for verified lots (HACK to support older ADIS16448AMLZ with no explicit detection)
|
||||
if (LOT_ID1 == 0x1824) {
|
||||
_check_crc = true;
|
||||
|
||||
if (_perf_crc_bad == nullptr) {
|
||||
_perf_crc_bad = perf_alloc(PC_COUNT, MODULE_NAME": CRC16 bad");
|
||||
}
|
||||
|
||||
} else {
|
||||
_check_crc = false;
|
||||
|
||||
for (auto ®_cfg : _register_cfg) {
|
||||
if (reg_cfg.reg == Register::MSC_CTRL) {
|
||||
reg_cfg.set_bits = reg_cfg.set_bits & ~MSC_CTRL_BIT::CRC16_for_burst;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 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);
|
||||
|
|
|
|||
|
|
@ -105,9 +105,9 @@ private:
|
|||
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")};
|
||||
perf_counter_t _perf_crc_bad{nullptr};
|
||||
|
||||
hrt_abstime _reset_timestamp{0};
|
||||
hrt_abstime _last_config_check_timestamp{0};
|
||||
|
|
@ -118,6 +118,9 @@ private:
|
|||
|
||||
bool _self_test_passed{false};
|
||||
|
||||
int16_t _accel_prev[3] {};
|
||||
int16_t _gyro_prev[3] {};
|
||||
|
||||
enum class STATE : uint8_t {
|
||||
RESET,
|
||||
WAIT_FOR_RESET,
|
||||
|
|
@ -127,12 +130,11 @@ private:
|
|||
} _state{STATE::RESET};
|
||||
|
||||
uint8_t _checked_register{0};
|
||||
static constexpr uint8_t size_register_cfg{4};
|
||||
static constexpr uint8_t size_register_cfg{3};
|
||||
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},
|
||||
{ Register::SENS_AVG, SENS_AVG_BIT::Measurement_range_1000_set | SENS_AVG_BIT::Filter_Size_Variable_B_set, SENS_AVG_BIT::Measurement_range_1000_clear | SENS_AVG_BIT::Filter_Size_Variable_B_clear },
|
||||
};
|
||||
};
|
||||
|
|
|
|||
|
|
@ -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 = 9; // 9 us Stall period between data
|
||||
static constexpr uint32_t SPI_STALL_PERIOD = 10; // 9 us Stall period between data
|
||||
|
||||
static constexpr uint16_t DIR_WRITE = 0x80;
|
||||
|
||||
|
|
@ -128,8 +128,8 @@ enum GLOB_CMD_BIT : uint16_t {
|
|||
|
||||
// SMPL_PRD
|
||||
enum SMPL_PRD_BIT : uint16_t {
|
||||
// [12:8] D, decimation rate setting, binomial,
|
||||
decimation_rate = Bit12 | Bit11 | Bit10 | Bit9, // disable
|
||||
// [12:8] D, decimation rate setting, binomial
|
||||
decimation_rate = Bit12 | Bit11 | Bit10 | Bit9 | Bit8, // disable
|
||||
|
||||
internal_sampling_clock = Bit0, // 1 = internal sampling clock, 819.2 SPS
|
||||
};
|
||||
|
|
@ -141,8 +141,8 @@ enum SENS_AVG_BIT : uint16_t {
|
|||
Measurement_range_1000_clear = Bit9 | Bit8,
|
||||
|
||||
// [2:0] Filter Size Variable B
|
||||
Filter_Size_Variable_B = Bit2 | Bit1 | Bit0, // disable
|
||||
|
||||
Filter_Size_Variable_B_set = Bit1,
|
||||
Filter_Size_Variable_B_clear = Bit2 | Bit0,
|
||||
};
|
||||
|
||||
// GPIO_CTRL
|
||||
|
|
|
|||
Loading…
Reference in New Issue