From c285f4c748215762555e95f1e1a7bb3b434bbd70 Mon Sep 17 00:00:00 2001 From: Willian Galvani Date: Wed, 5 Jan 2022 15:56:33 -0300 Subject: [PATCH] AP_Compass: Retry reading MMC5983 ID up to 10 times --- libraries/AP_Compass/AP_Compass_MMC5xx3.cpp | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass_MMC5xx3.cpp b/libraries/AP_Compass/AP_Compass_MMC5xx3.cpp index 34be971819..0c164ca444 100644 --- a/libraries/AP_Compass/AP_Compass_MMC5xx3.cpp +++ b/libraries/AP_Compass/AP_Compass_MMC5xx3.cpp @@ -78,9 +78,17 @@ bool AP_Compass_MMC5XX3::init() dev->set_read_flag(0x80); } - uint8_t whoami; - if (!dev->read_registers(REG_PRODUCT_ID, &whoami, 1) || - whoami != MMC5983_ID) { + // Reading REG_PRODUCT_ID fails sometimes on SPI, so we retry up to 10 times + uint8_t whoami = 0; + uint8_t tries = 10; + while (whoami == 0 && tries > 0) { + tries--; + dev->read_registers(REG_PRODUCT_ID, &whoami, 1); + hal.scheduler->delay(5); + } + + if (whoami != MMC5983_ID) { + printf("MMC5983 got unexpected product id: %d, expected: %d\n", whoami, MMC5983_ID); // not a MMC5983 return false; }