Skip to content

Commit 54cc96f

Browse files
dagarJaeyoung-Lim
dagar
authored and
Jaeyoung-Lim
committed
drivers ADIS16448 misc fixes
 - 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
1 parent ceb8c02 commit 54cc96f

File tree

3 files changed

+102
-37
lines changed

3 files changed

+102
-37
lines changed

‎src/drivers/imu/analog_devices/adis16448/ADIS16448.cpp‎

Lines changed: 91 additions & 28 deletions
Original file line numberDiff line numberDiff line change
@@ -110,9 +110,9 @@ ADIS16448::ADIS16448(I2CSPIBusOption bus_option, int bus, uint32_t device, enum
110110
ADIS16448::~ADIS16448()
111111
{
112112
perf_free(_reset_perf);
113-
perf_free(_perf_crc_bad);
114113
perf_free(_bad_register_perf);
115114
perf_free(_bad_transfer_perf);
115+
perf_free(_perf_crc_bad);
116116
}
117117

118118
int ADIS16448::init()
@@ -147,9 +147,9 @@ void ADIS16448::print_status()
147147
I2CSPIDriverBase::print_status();
148148

149149
perf_print_counter(_reset_perf);
150-
perf_print_counter(_perf_crc_bad);
151150
perf_print_counter(_bad_register_perf);
152151
perf_print_counter(_bad_transfer_perf);
152+
perf_print_counter(_perf_crc_bad);
153153
}
154154

155155
int ADIS16448::probe()
@@ -159,25 +159,24 @@ int ADIS16448::probe()
159159
PX4_WARN("Power-On Start-Up Time is 205 ms");
160160
}
161161

162-
const uint16_t PROD_ID = RegisterRead(Register::PROD_ID);
162+
for (int attempt = 0; attempt < 3; attempt++) {
163+
const uint16_t PROD_ID = RegisterRead(Register::PROD_ID);
163164

164-
if (PROD_ID != Product_identification) {
165-
DEVICE_DEBUG("unexpected PROD_ID 0x%02x", PROD_ID);
166-
return PX4_ERROR;
167-
}
165+
if (PROD_ID == Product_identification) {
166+
const uint16_t SERIAL_NUM = RegisterRead(Register::SERIAL_NUM);
167+
const uint16_t LOT_ID1 = RegisterRead(Register::LOT_ID1);
168+
const uint16_t LOT_ID2 = RegisterRead(Register::LOT_ID2);
168169

169-
const uint16_t SERIAL_NUM = RegisterRead(Register::SERIAL_NUM);
170-
const uint16_t LOT_ID1 = RegisterRead(Register::LOT_ID1);
171-
const uint16_t LOT_ID2 = RegisterRead(Register::LOT_ID2);
170+
PX4_INFO("Serial Number: 0x%02x, Lot ID1: 0x%02x ID2: 0x%02x", SERIAL_NUM, LOT_ID1, LOT_ID2);
172171

173-
PX4_INFO("Serial Number: 0x%02x, Lot ID1: 0x%02x ID2: 0x%02x", SERIAL_NUM, LOT_ID1, LOT_ID2);
172+
return PX4_OK;
174173

175-
// Only enable CRC-16 for verified lots (HACK to support older ADIS16448AMLZ with no explicit detection)
176-
if (LOT_ID1 == 0x1824) {
177-
_check_crc = true;
174+
} else {
175+
DEVICE_DEBUG("unexpected PROD_ID 0x%02x", PROD_ID);
176+
}
178177
}
179178

180-
return PX4_OK;
179+
return PX4_ERROR;
181180
}
182181

183182
void ADIS16448::RunImpl()
@@ -208,23 +207,43 @@ void ADIS16448::RunImpl()
208207
if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
209208
PX4_DEBUG("Reset failed, retrying");
210209
_state = STATE::RESET;
211-
ScheduleDelayed(100_ms);
210+
ScheduleDelayed(1000_ms);
212211

213212
} else {
214-
PX4_DEBUG("Reset not complete, check again in 10 ms");
215-
ScheduleDelayed(10_ms);
213+
PX4_DEBUG("Reset not complete, check again in 100 ms");
214+
ScheduleDelayed(100_ms);
216215
}
217216
}
218217

219218
} else {
220219
RegisterWrite(Register::MSC_CTRL, MSC_CTRL_BIT::Internal_self_test);
221220
_state = STATE::SELF_TEST_CHECK;
222-
ScheduleDelayed(45_ms); // Automatic Self-Test Time 45 ms
221+
ScheduleDelayed(90_ms); // Automatic Self-Test Time > 45 ms
223222
}
224223

225224
break;
226225

227226
case STATE::SELF_TEST_CHECK: {
227+
const uint16_t MSC_CTRL = RegisterRead(Register::MSC_CTRL);
228+
229+
if (MSC_CTRL & MSC_CTRL_BIT::Internal_self_test) {
230+
// self test not finished, check again
231+
if (hrt_elapsed_time(&_reset_timestamp) < 1000_ms) {
232+
ScheduleDelayed(45_ms);
233+
PX4_DEBUG("self test not complete, check again in 45 ms");
234+
return;
235+
236+
} else {
237+
// still not cleared, fail self test
238+
_self_test_passed = false;
239+
_state = STATE::RESET;
240+
ScheduleDelayed(1000_ms);
241+
return;
242+
}
243+
}
244+
245+
bool test_passed = true;
246+
228247
const uint16_t DIAG_STAT = RegisterRead(Register::DIAG_STAT);
229248

230249
if (DIAG_STAT & DIAG_STAT_BIT::Self_test_diagnostic_error_flag) {
@@ -248,6 +267,7 @@ void ADIS16448::RunImpl()
248267

249268
if (gyro_x_fail || gyro_y_fail || gyro_z_fail) {
250269
PX4_ERR("gyroscope self-test failure");
270+
test_passed = false;
251271
}
252272

253273
// Accelerometer
@@ -257,18 +277,20 @@ void ADIS16448::RunImpl()
257277

258278
if (accel_x_fail || accel_y_fail || accel_z_fail) {
259279
PX4_ERR("accelerometer self-test failure");
280+
test_passed = false;
260281
}
282+
}
261283

262-
_self_test_passed = false;
263-
_state = STATE::RESET;
264-
ScheduleDelayed(1000_ms);
265-
266-
} else {
284+
if (test_passed) {
267285
PX4_DEBUG("self test passed");
268286
_self_test_passed = true;
269-
_state = STATE::RESET;
270-
ScheduleNow();
287+
288+
} else {
289+
_self_test_passed = false;
271290
}
291+
292+
_state = STATE::RESET;
293+
ScheduleDelayed(10_ms);
272294
}
273295

274296
break;
@@ -364,18 +386,38 @@ void ADIS16448::RunImpl()
364386
_px4_accel.set_temperature(temperature);
365387
_px4_gyro.set_temperature(temperature);
366388

389+
bool imu_updated = false;
390+
367391
// sensor's frame is +x forward, +y left, +z up
368392
// flip y & z to publish right handed with z down (x forward, y right, z down)
369393
const int16_t accel_x = buffer.XACCL_OUT;
370394
const int16_t accel_y = (buffer.YACCL_OUT == INT16_MIN) ? INT16_MAX : -buffer.YACCL_OUT;
371395
const int16_t accel_z = (buffer.ZACCL_OUT == INT16_MIN) ? INT16_MAX : -buffer.ZACCL_OUT;
372396

397+
if (accel_x != _accel_prev[0] || accel_y != _accel_prev[1] || accel_z != _accel_prev[2]) {
398+
imu_updated = true;
399+
400+
_accel_prev[0] = accel_x;
401+
_accel_prev[1] = accel_y;
402+
_accel_prev[2] = accel_z;
403+
}
404+
373405
const int16_t gyro_x = buffer.XGYRO_OUT;
374406
const int16_t gyro_y = (buffer.YGYRO_OUT == INT16_MIN) ? INT16_MAX : -buffer.YGYRO_OUT;
375407
const int16_t gyro_z = (buffer.ZGYRO_OUT == INT16_MIN) ? INT16_MAX : -buffer.ZGYRO_OUT;
376408

377-
_px4_accel.update(now, accel_x, accel_y, accel_z);
378-
_px4_gyro.update(now, gyro_x, gyro_y, gyro_z);
409+
if (gyro_x != _gyro_prev[0] || gyro_y != _gyro_prev[1] || gyro_z != _gyro_prev[2]) {
410+
imu_updated = true;
411+
412+
_gyro_prev[0] = gyro_x;
413+
_gyro_prev[1] = gyro_y;
414+
_gyro_prev[2] = gyro_z;
415+
}
416+
417+
if (imu_updated) {
418+
_px4_accel.update(now, accel_x, accel_y, accel_z);
419+
_px4_gyro.update(now, gyro_x, gyro_y, gyro_z);
420+
}
379421

380422
// DIAG_STAT bit 7: New data, xMAGN_OUT/BARO_OUT
381423
if (buffer.DIAG_STAT & DIAG_STAT_BIT::New_data_xMAGN_OUT_BARO_OUT) {
@@ -435,6 +477,27 @@ void ADIS16448::RunImpl()
435477

436478
bool ADIS16448::Configure()
437479
{
480+
const uint16_t LOT_ID1 = RegisterRead(Register::LOT_ID1);
481+
482+
// Only enable CRC-16 for verified lots (HACK to support older ADIS16448AMLZ with no explicit detection)
483+
if (LOT_ID1 == 0x1824) {
484+
_check_crc = true;
485+
486+
if (_perf_crc_bad == nullptr) {
487+
_perf_crc_bad = perf_alloc(PC_COUNT, MODULE_NAME": CRC16 bad");
488+
}
489+
490+
} else {
491+
_check_crc = false;
492+
493+
for (auto &reg_cfg : _register_cfg) {
494+
if (reg_cfg.reg == Register::MSC_CTRL) {
495+
reg_cfg.set_bits = reg_cfg.set_bits & ~MSC_CTRL_BIT::CRC16_for_burst;
496+
break;
497+
}
498+
}
499+
}
500+
438501
// first set and clear all configured register bits
439502
for (const auto &reg_cfg : _register_cfg) {
440503
RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);

‎src/drivers/imu/analog_devices/adis16448/ADIS16448.hpp‎

Lines changed: 6 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -105,9 +105,9 @@ class ADIS16448 : public device::SPI, public I2CSPIDriver<ADIS16448>
105105
PX4Magnetometer _px4_mag;
106106

107107
perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": reset")};
108-
perf_counter_t _perf_crc_bad{perf_counter_t(perf_alloc(PC_COUNT, MODULE_NAME": CRC16 bad"))};
109108
perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")};
110109
perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")};
110+
perf_counter_t _perf_crc_bad{nullptr};
111111

112112
hrt_abstime _reset_timestamp{0};
113113
hrt_abstime _last_config_check_timestamp{0};
@@ -118,6 +118,9 @@ class ADIS16448 : public device::SPI, public I2CSPIDriver<ADIS16448>
118118

119119
bool _self_test_passed{false};
120120

121+
int16_t _accel_prev[3] {};
122+
int16_t _gyro_prev[3] {};
123+
121124
enum class STATE : uint8_t {
122125
RESET,
123126
WAIT_FOR_RESET,
@@ -127,12 +130,11 @@ class ADIS16448 : public device::SPI, public I2CSPIDriver<ADIS16448>
127130
} _state{STATE::RESET};
128131

129132
uint8_t _checked_register{0};
130-
static constexpr uint8_t size_register_cfg{4};
133+
static constexpr uint8_t size_register_cfg{3};
131134
register_config_t _register_cfg[size_register_cfg] {
132135
// Register | Set bits, Clear bits
133136
{ Register::MSC_CTRL, MSC_CTRL_BIT::CRC16_for_burst, 0 },
134137
{ Register::SMPL_PRD, SMPL_PRD_BIT::internal_sampling_clock, SMPL_PRD_BIT::decimation_rate },
135-
{ Register::SENS_AVG, SENS_AVG_BIT::Measurement_range_1000_set, SENS_AVG_BIT::Measurement_range_1000_clear | SENS_AVG_BIT::Filter_Size_Variable_B },
136-
{ Register::GPIO_CTRL, GPIO_CTRL_BIT::GPIO2_DIRECTION | GPIO_CTRL_BIT::GPIO1_DIRECTION, 0},
138+
{ 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 },
137139
};
138140
};

‎src/drivers/imu/analog_devices/adis16448/Analog_Devices_ADIS16448_registers.hpp‎

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -65,7 +65,7 @@ namespace Analog_Devices_ADIS16448
6565
static constexpr uint32_t SPI_SPEED = 2 * 1000 * 1000; // 2 MHz SPI serial interface
6666
static constexpr uint32_t SPI_SPEED_BURST = 1 * 1000 * 1000; // 1 MHz SPI serial interface for burst read
6767

68-
static constexpr uint32_t SPI_STALL_PERIOD = 9; // 9 us Stall period between data
68+
static constexpr uint32_t SPI_STALL_PERIOD = 10; // 9 us Stall period between data
6969

7070
static constexpr uint16_t DIR_WRITE = 0x80;
7171

@@ -128,8 +128,8 @@ enum GLOB_CMD_BIT : uint16_t {
128128

129129
// SMPL_PRD
130130
enum SMPL_PRD_BIT : uint16_t {
131-
// [12:8] D, decimation rate setting, binomial,
132-
decimation_rate = Bit12 | Bit11 | Bit10 | Bit9, // disable
131+
// [12:8] D, decimation rate setting, binomial
132+
decimation_rate = Bit12 | Bit11 | Bit10 | Bit9 | Bit8, // disable
133133

134134
internal_sampling_clock = Bit0, // 1 = internal sampling clock, 819.2 SPS
135135
};
@@ -141,8 +141,8 @@ enum SENS_AVG_BIT : uint16_t {
141141
Measurement_range_1000_clear = Bit9 | Bit8,
142142

143143
// [2:0] Filter Size Variable B
144-
Filter_Size_Variable_B = Bit2 | Bit1 | Bit0, // disable
145-
144+
Filter_Size_Variable_B_set = Bit1,
145+
Filter_Size_Variable_B_clear = Bit2 | Bit0,
146146
};
147147

148148
// GPIO_CTRL

0 commit comments

Comments
 (0)