diff --git a/lib/i2cscan/i2cscan.cpp b/lib/i2cscan/i2cscan.cpp index 2eda286..55a7d0f 100644 --- a/lib/i2cscan/i2cscan.cpp +++ b/lib/i2cscan/i2cscan.cpp @@ -1,116 +1,156 @@ #include "i2cscan.h" + +#include +#include +#include + #include "../../src/globals.h" +#include "../../src/consts.h" -#ifdef ESP8266 -uint8_t portArray[] = {16, 5, 4, 2, 14, 12, 13}; -uint8_t portExclude[] = {LED_PIN}; -String portMap[] = {"D0", "D1", "D2", "D4", "D5", "D6", "D7"}; -#elif defined(ESP32C3) -uint8_t portArray[] = {2, 3, 4, 5, 6, 7, 8, 9, 10}; -uint8_t portExclude[] = {18, 19, 20, 21, LED_PIN}; -String portMap[] = {"2", "3", "4", "5", "6", "7", "8", "9", "10"}; -#elif defined(ESP32C6) -uint8_t portArray[] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 14, 15, 18, 19, 20, 21, 22, 23}; -String portMap[] = {"0", "1", "2", "3", "4", "5", "6", "7", "8", "9", "10", "11", "14", "15", "18", "19", "20", "21", "22", "23"}; -uint8_t portExclude[] = {12, 13, 16, 17, LED_PIN}; -#elif defined(ESP32) -uint8_t portArray[] = {4, 13, 14, 15, 16, 17, 18, 19, 21, 22, 23, 25, 26, 27, 32, 33}; -String portMap[] = {"4", "13", "14", "15", "16", "17", "18", "19", "21", "22", "23", "25", "26", "27", "32", "33"}; -uint8_t portExclude[] = {LED_PIN}; -#endif - -namespace I2CSCAN -{ - enum class ScanState { +namespace I2CSCAN { + enum class ScanState : uint8_t { IDLE, SCANNING, DONE }; - ScanState scanState = ScanState::IDLE; - uint8_t currentSDA = 0; - uint8_t currentSCL = 0; - uint8_t currentAddress = 1; - bool found = false; - std::vector validPorts; + namespace { + ScanState scanState = ScanState::IDLE; + uint8_t currentSDA = 0; + uint8_t currentSCL = 0; + uint8_t currentAddress = 1; + bool found = false; + uint8_t txFails = 0; + std::vector validPorts; - void scani2cports() - { +#ifdef ESP8266 + std::array portArray = {16, 5, 4, 2, 14, 12, 13}; + std::array portMap = {"D0", "D1", "D2", "D4", "D5", "D6", "D7"}; + std::array portExclude = {LED_PIN}; +#elif defined(ESP32C3) + std::array portArray = {2, 3, 4, 5, 6, 7, 8, 9, 10}; + std::array portMap = {"2", "3", "4", "5", "6", "7", "8", "9", "10"}; + std::array portExclude = {18, 19, 20, 21, LED_PIN}; +#elif defined(ESP32C6) + std::array portArray = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 14, 15, 18, 19, 20, 21, 22, 23}; + std::array portMap = {"0", "1", "2", "3", "4", "5", "6", "7", "8", "9", "10", "11", "14", "15", "18", "19", "20", "21", "22", "23"}; + std::array portExclude = {12, 13, 16, 17, LED_PIN}; +#elif defined(ESP32) + std::array portArray = {4, 13, 14, 15, 16, 17, 18, 19, 21, 22, 23, 25, 26, 27, 32, 33}; + std::array portMap = {"4", "13", "14", "15", "16", "17", "18", "19", "21", "22", "23", "25", "26", "27", "32", "33"}; + std::array portExclude = {LED_PIN}; +#endif + + bool selectNextPort() { + currentSCL++; + + if(validPorts[currentSCL] == validPorts[currentSDA]) currentSCL++; + + if (currentSCL < validPorts.size()) { + Wire.begin((int)validPorts[currentSDA], (int)validPorts[currentSCL]); //NOLINT + return true; + } + + currentSCL = 0; + currentSDA++; + + if (currentSDA >= validPorts.size()) { + if (!found) { + Serial.println("[ERROR] I2C: No I2C devices found"); //NOLINT + } + #ifdef ESP32 + Wire.end(); + #endif + Wire.begin(static_cast(PIN_IMU_SDA), static_cast(PIN_IMU_SCL)); + scanState = ScanState::DONE; + return false; + } + + Wire.begin((int)validPorts[currentSDA], (int)validPorts[currentSCL]); + return true; + } + template + uint8_t countCommonElements( + const std::array& array1, + const std::array& array2) { + + uint8_t count = 0; + for (const auto& elem1 : array1) { + for (const auto& elem2 : array2) { + if (elem1 == elem2) { + count++; + } + } + } + + return count; + } + } // anonymous namespace + + void scani2cports() { if (scanState != ScanState::IDLE) { - return; + if (scanState == ScanState::DONE) { + Serial.println("[DEBUG] I2C scan finished previously, resetting and scanning again..."); //NOLINT + } else { + return; // Already scanning, do not start again + } } // Filter out excluded ports - for (size_t i = 0; i < sizeof(portArray); i++) { - if (!inArray(portArray[i], portExclude, sizeof(portExclude))) { - validPorts.push_back(portArray[i]); - } - } + validPorts.clear(); + uint8_t excludes = countCommonElements(portArray, portExclude); + validPorts.reserve(portArray.size() - excludes); // Reserve space to avoid reallocations + for (const auto& port : portArray) { + if (std::find(portExclude.begin(), portExclude.end(), port) == portExclude.end()) { + validPorts.push_back(port); // Port is valid, add it to the list + } + } + + // Reset scan variables and start scanning found = false; currentSDA = 0; currentSCL = 1; currentAddress = 1; + txFails = 0; scanState = ScanState::SCANNING; - } + } - bool selectNextPort() { - currentSCL++; - if(validPorts[currentSCL] == validPorts[currentSDA]) - currentSCL++; - if (currentSCL < validPorts.size()) { - Wire.begin((int)validPorts[currentSDA], (int)validPorts[currentSCL]); - return true; - } - - currentSCL = 0; - currentSDA++; - - if (currentSDA >= validPorts.size()) { - if (!found) { - Serial.println("[ERR] I2C: No I2C devices found"); - } -#ifdef ESP32 - Wire.end(); -#endif - Wire.begin(static_cast(PIN_IMU_SDA), static_cast(PIN_IMU_SCL)); - scanState = ScanState::DONE; - return false; - } - - Wire.begin((int)validPorts[currentSDA], (int)validPorts[currentSCL]); - return true; - } - - void update() - { + void update() { if (scanState != ScanState::SCANNING) { return; } - if (currentAddress == 1) { #ifdef ESP32 + if (currentAddress == 1) { Wire.end(); + } #endif - } Wire.beginTransmission(currentAddress); - byte error = Wire.endTransmission(); + const uint8_t error = Wire.endTransmission(); - if (error == 0) - { - Serial.printf("[DBG] I2C (@ %s(%d) : %s(%d)): I2C device found at address 0x%02x !\n", + if (error == 0) { + Serial.printf("[INFO ] I2C (@ %s(%d) : %s(%d)): I2C device found at address 0x%02x!\n", portMap[currentSDA].c_str(), validPorts[currentSDA], portMap[currentSCL].c_str(), validPorts[currentSCL], currentAddress); found = true; - } - else if (error == 4) - { - Serial.printf("[ERR] I2C (@ %s(%d) : %s(%d)): Unknown error at address 0x%02x\n", + } else if (error == 4) { // Unable to start transaction, log and warn + Serial.printf("[WARN ] I2C (@ %s(%d) : %s(%d)): Unable to start transaction at address 0x%02x!\n", portMap[currentSDA].c_str(), validPorts[currentSDA], portMap[currentSCL].c_str(), validPorts[currentSCL], currentAddress); + txFails++; } currentAddress++; + if (currentAddress <= 127) { + if (txFails > 5) { +#if BOARD == BOARD_SLIMEVR_LEGACY || BOARD == BOARD_SLIMEVR_DEV || BOARD == BOARD_SLIMEVR || BOARD == BOARD_SLIMEVR_V1_2 + Serial.printf("[ERROR] I2C: Too many transaction errors (%d), please power off the tracker and contact SlimeVR support!\n", txFails); +#else + Serial.printf("[ERROR] I2C: Too many transaction errors (%d), please power off the tracker and check the IMU connections!\n", txFails); +#endif + } + return; } @@ -118,19 +158,6 @@ namespace I2CSCAN selectNextPort(); } - bool inArray(uint8_t value, uint8_t* array, size_t arraySize) - { - for (size_t i = 0; i < arraySize; i++) - { - if (value == array[i]) - { - return true; - } - } - - return false; - } - bool hasDevOnBus(uint8_t addr) { byte error; #if ESP32C3 @@ -165,9 +192,9 @@ namespace I2CSCAN */ int clearBus(uint8_t SDA, uint8_t SCL) { - #if defined(TWCR) && defined(TWEN) +#if defined(TWCR) && defined(TWEN) TWCR &= ~(_BV(TWEN)); // Disable the Atmel 2-Wire interface so we can control the SDA and SCL pins directly - #endif +#endif pinMode(SDA, INPUT_PULLUP); pinMode(SCL, INPUT_PULLUP); diff --git a/lib/i2cscan/i2cscan.h b/lib/i2cscan/i2cscan.h index 3fa2eaf..d658e55 100644 --- a/lib/i2cscan/i2cscan.h +++ b/lib/i2cscan/i2cscan.h @@ -11,7 +11,7 @@ namespace I2CSCAN { bool hasDevOnBus(uint8_t addr); uint8_t pickDevice(uint8_t addr1, uint8_t addr2, bool scanIfNotFound); int clearBus(uint8_t SDA, uint8_t SCL); - boolean inArray(uint8_t value, uint8_t* arr, size_t arrSize); + bool inArray(uint8_t value, const uint8_t *array, size_t arraySize); } -#endif // _I2CSCAN_H_ \ No newline at end of file +#endif // _I2CSCAN_H_ diff --git a/src/debug.h b/src/debug.h index 307eea7..0e85a5d 100644 --- a/src/debug.h +++ b/src/debug.h @@ -39,6 +39,8 @@ // disable if problems. Server does nothing with value so disabled atm #define SEND_ACCELERATION true // send linear acceleration to the server +#define EXT_SERIAL_COMMANDS false // Set to true to enable extra serial debug commands + // Debug information #define LOG_LEVEL LOG_LEVEL_DEBUG diff --git a/src/serial/serialcommands.cpp b/src/serial/serialcommands.cpp index 72e4e65..124c7df 100644 --- a/src/serial/serialcommands.cpp +++ b/src/serial/serialcommands.cpp @@ -35,10 +35,19 @@ #include "nvs_flash.h" #endif +#ifdef EXT_SERIAL_COMMANDS +#define CALLBACK_SIZE 7 // Increase callback size to allow for debug commands +#include "i2cscan.h" +#endif + +#ifndef CALLBACK_SIZE +#define CALLBACK_SIZE 6 // Default callback size +#endif + namespace SerialCommands { SlimeVR::Logging::Logger logger("SerialCommands"); -CmdCallback<6> cmdCallbacks; +CmdCallback cmdCallbacks; CmdParser cmdParser; CmdBuffer<256> cmdBuffer; @@ -457,6 +466,13 @@ void cmdDeleteCalibration(CmdParser* parser) { configuration.eraseSensors(); } +#if EXT_SERIAL_COMMANDS +void cmdScanI2C(CmdParser* parser) { + logger.info("Forcing I2C scan..."); + I2CSCAN::scani2cports(); +} +#endif + void setUp() { cmdCallbacks.addCmd("SET", &cmdSet); cmdCallbacks.addCmd("GET", &cmdGet); @@ -464,6 +480,9 @@ void setUp() { cmdCallbacks.addCmd("REBOOT", &cmdReboot); cmdCallbacks.addCmd("DELCAL", &cmdDeleteCalibration); cmdCallbacks.addCmd("TCAL", &cmdTemperatureCalibration); +#if EXT_SERIAL_COMMANDS + cmdCallbacks.addCmd("SCANI2C", &cmdScanI2C); +#endif } void update() { cmdCallbacks.updateCmdProcessing(&cmdParser, &cmdBuffer, &Serial); }