diff --git a/src/drivers/magnetometer/isentek/ist8310/IST8310.cpp b/src/drivers/magnetometer/isentek/ist8310/IST8310.cpp index e5baf14ac5..e6c78d77f9 100644 --- a/src/drivers/magnetometer/isentek/ist8310/IST8310.cpp +++ b/src/drivers/magnetometer/isentek/ist8310/IST8310.cpp @@ -85,31 +85,31 @@ void IST8310::print_status() int IST8310::probe() { - uint8_t id = RegisterRead(Register::WAI); + // Reading the WAI register is not always reliable, it can return 0xff or + // other values if the sensor has been powered up in a certain way. In + // addition, the I2C address is not always correct, sometimes it boots with + // 0x0C rather than 0x0E. + const auto start_time = hrt_absolute_time(); - if (id != Device_ID) { - DEVICE_DEBUG("unexpected WAI 0x%02x", id); + while (hrt_elapsed_time(&start_time) < 50_ms) { + set_device_address(I2C_ADDRESS_DEFAULT); + const int 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 has the right I2C address and register content + return PX4_OK; } - return PX4_ERROR; + // send reset command to all four possible addresses + for (uint8_t addr = 0x0C; addr <= 0x0F; addr++) { + set_device_address(addr); + RegisterWrite(Register::CNTL2, CNTL2_BIT::SRST); + } + + px4_usleep(10'000); } - return PX4_OK; + return PX4_ERROR; } void IST8310::RunImpl() @@ -290,11 +290,14 @@ bool IST8310::RegisterCheck(const register_config_t ®_cfg) return success; } -uint8_t IST8310::RegisterRead(Register reg) +int IST8310::RegisterRead(Register reg) { const uint8_t cmd = static_cast(reg); uint8_t buffer{}; - transfer(&cmd, 1, &buffer, 1); + const int ret = transfer(&cmd, 1, &buffer, 1); + + if (ret != OK) { return -1; } + return buffer; } diff --git a/src/drivers/magnetometer/isentek/ist8310/IST8310.hpp b/src/drivers/magnetometer/isentek/ist8310/IST8310.hpp index a251f49cf0..5762377cb6 100644 --- a/src/drivers/magnetometer/isentek/ist8310/IST8310.hpp +++ b/src/drivers/magnetometer/isentek/ist8310/IST8310.hpp @@ -79,7 +79,7 @@ private: bool RegisterCheck(const register_config_t ®_cfg); - uint8_t RegisterRead(Register reg); + int RegisterRead(Register reg); void RegisterWrite(Register reg, uint8_t value); void RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits);