diff --git a/src/drivers/magnetometer/isentek/ist8310/IST8310.cpp b/src/drivers/magnetometer/isentek/ist8310/IST8310.cpp index d48321d148..e5baf14ac5 100644 --- a/src/drivers/magnetometer/isentek/ist8310/IST8310.cpp +++ b/src/drivers/magnetometer/isentek/ist8310/IST8310.cpp @@ -85,19 +85,27 @@ void IST8310::print_status() int IST8310::probe() { - // Apparently, the IST8310's WHOAMI register is writeable. Presumably, - // this can get corrupted by bus noise. It is only reset if powered off - // for 30s or by a reset. - // - // Therefore, we do a reset before doing the WHOAMI. - RegisterWrite(Register::CNTL2, CNTL2_BIT::SRST); + uint8_t id = RegisterRead(Register::WAI); - px4_usleep(10000); + if (id != Device_ID) { + DEVICE_DEBUG("unexpected WAI 0x%02x", id); - const uint8_t WAI = RegisterRead(Register::WAI); + // Apparently, the IST8310's WHOAMI register is writeable. Presumably, + // this can get corrupted by bus noise. It is only reset if powered off + // for 30s or by a reset. + RegisterWrite(Register::CNTL2, CNTL2_BIT::SRST); + + auto start_time = hrt_absolute_time(); + + while (hrt_elapsed_time(&start_time) < 50_ms) { + px4_usleep(10'000); + id = RegisterRead(Register::WAI); + + if (id == Device_ID) { + return PX4_OK; + } + } - if (WAI != Device_ID) { - DEVICE_DEBUG("unexpected WAI 0x%02x", WAI); return PX4_ERROR; }