166 lines
6.2 KiB
C++
166 lines
6.2 KiB
C++
#include <Wire.h>
|
|
// Similar to https://github.com/Levi--G/IMU-WhoAmIVerifier
|
|
|
|
//Do be aware that MPU9150's will report as 6050s, this is because a 9150 is a 6050 with a magnetometer
|
|
//if you have a 9250 board and it reports as a 6050, it's most likely a 9150.
|
|
|
|
//This code will check for an IMU when reset and, if one is found, it will report what it is.
|
|
//To re run the check without resetting the Arduino, pull pin 4 to GND.
|
|
|
|
#define NUM_IMUS 37
|
|
|
|
bool errorflag;
|
|
|
|
typedef struct IMU {
|
|
uint8_t Address1;
|
|
uint8_t Address2;
|
|
uint8_t Register;
|
|
uint8_t ExpectedID;
|
|
const char* IMUName PROGMEM;
|
|
const char* IMUCapabilities PROGMEM;
|
|
bool LibSupported;
|
|
};
|
|
|
|
const IMU IMUList[NUM_IMUS] =
|
|
{
|
|
{0x68, 0x69, 0x75, 0x68, "MPU6050", "3A,3G", true},
|
|
{0x68, 0x69, 0x75, 0x70, "MPU6500", "3A,3G", true},
|
|
{0x68, 0x69, 0x75, 0x74, "MPU6515", "3A,3G", true},
|
|
{0x68, 0x69, 0x75, 0x19, "MPU6886", "3A,3G", true},
|
|
{0x68, 0x69, 0x75, 0x71, "MPU9250", "3A,3G,3M", true},
|
|
{0x68, 0x69, 0x75, 0x73, "MPU9255", "3A,3G,3M", true},
|
|
{0x69, 0x68, 0x00, 0xD1, "BMI160", "3A,3G", true},
|
|
{0x6B, 0x6A, 0x0F, 0x69, "LSM6DS3", "3A,3G", true},
|
|
{0x6B, 0x6A, 0x0F, 0x6A, "LSM6DSL", "3A,3G", true},
|
|
{0x68, 0x69, 0x75, 0x98, "ICM20689", "3A,3G", true},
|
|
{0x68, 0x69, 0x75, 0x20, "ICM20690", "3A,3G", true},
|
|
{0x18, 0x19, 0x00, 0xFA, "BMI055 or BMX055", "3A,3G or 3A,3G,3M", true},
|
|
{0x0D, 0x0D, 0x0D, 0xFF, "QMC5883L", "3M", true},
|
|
{0x68, 0x69, 0x75, 0x75, "Unknown or fake IMU, proceed with caution, use 'IMU_Generic' as IMU type", "3A,3G, possibly 3M?", true},
|
|
{0x6B, 0x6A, 0x0F, 0x6B, "LSM6DSR", "3A,3G", false},
|
|
{0x68, 0x69, 0x75, 0x92, "ICG20330", "3G", false},
|
|
{0x68, 0x69, 0x75, 0xB5, "IAM20380", "3A", false},
|
|
{0x68, 0x69, 0x75, 0xB6, "IAM20381", "3G", false},
|
|
{0x68, 0x69, 0x75, 0x11, "ICM20600", "3A,3G", false},
|
|
{0x68, 0x69, 0x75, 0xAC, "ICM20601", "3A,3G", false},
|
|
{0x68, 0x69, 0x75, 0x12, "ICM20602", "3A,3G", false},
|
|
{0x68, 0x69, 0x75, 0xAF, "ICM20608-G", "3A,3G", false},
|
|
{0x68, 0x69, 0x75, 0xA6, "ICM20609", "3A,3G", false},
|
|
{0x68, 0x69, 0x00, 0xE0, "ICM20648", "3A,3G", false},
|
|
{0x68, 0x69, 0x00, 0xE1, "ICM20649", "3A,3G", false},
|
|
{0x68, 0x69, 0x75, 0xA9, "ICG20660", "3A,3G", false},
|
|
{0x68, 0x69, 0x75, 0x91, "IAM20680", "3A,3G", false},
|
|
{0x68, 0x69, 0x00, 0xEA, "ICM20948", "3A,3G,3M", false},
|
|
{0x68, 0x69, 0x75, 0x6C, "IIM42351", "3A", false},
|
|
{0x68, 0x69, 0x75, 0x6D, "IIM42352", "3A", false},
|
|
{0x68, 0x69, 0x75, 0x4E, "ICM40627", "3A,3G", false},
|
|
{0x68, 0x69, 0x75, 0x42, "ICM42605", "3A,3G", false},
|
|
{0x68, 0x69, 0x75, 0x6F, "IIM42652", "3A,3G", false},
|
|
{0x68, 0x69, 0x75, 0x67, "ICM42670-P", "3A,3G", false},
|
|
{0x68, 0x69, 0x75, 0xDB, "ICM42688-V", "3A,3G", false},
|
|
{0x68, 0x69, 0x00, 0x68, "MPU3050", "3G", false},
|
|
{0x1E, 0x1E, 0x0C, 0x33, "HMC5883L", "3M", false},
|
|
};
|
|
|
|
void setup() {
|
|
Serial.begin(9600);
|
|
while (!Serial) ;
|
|
errorflag = false;
|
|
pinMode(4, INPUT_PULLUP);
|
|
Wire.begin();
|
|
#ifdef WIRE_HAS_TIMEOUT
|
|
Wire.setWireTimeout(3000);
|
|
#endif
|
|
Serial.println(F("\n=========== IMU Identifier ==========="));
|
|
}
|
|
|
|
void loop() {
|
|
static int a = 0;
|
|
while (digitalRead(4) && a != 0) ; //do once
|
|
a = 1;
|
|
bool detected = false;
|
|
for (int i = 0; i < NUM_IMUS; i++)
|
|
{
|
|
#ifdef WIRE_HAS_TIMEOUT
|
|
if (errorflag || Wire.getWireTimeoutFlag()) {
|
|
Serial.print(F("Error while reading address 0x"));
|
|
Serial.print(IMUList[i].Address1, HEX);
|
|
Serial.print(F(": "));
|
|
if (Wire.getWireTimeoutFlag()) {
|
|
Serial.println(F("I2C bus timed out. (Bad IMU? check wiring.)"));
|
|
}
|
|
else {
|
|
Serial.println(F("Unknown error while reading/writing"));
|
|
}
|
|
Serial.println(F("======================================"));
|
|
Wire.clearWireTimeoutFlag();
|
|
errorflag = false;
|
|
delay(2000);
|
|
return;
|
|
}
|
|
#endif
|
|
if (readByte(IMUList[i].Address1, IMUList[i].Register) == IMUList[i].ExpectedID)
|
|
{
|
|
detected = true;
|
|
Serial.print(F("IMU Found: "));
|
|
Serial.print(IMUList[i].IMUName);
|
|
Serial.print(F(" On address: 0x"));
|
|
Serial.println(IMUList[i].Address1, HEX);
|
|
Serial.print(F("This IMU is capable of the following axis: "));
|
|
Serial.println(IMUList[i].IMUCapabilities);
|
|
if (IMUList[i].LibSupported) {
|
|
Serial.println(F("This IMU is supported by the FastIMU library."));
|
|
}
|
|
else
|
|
{
|
|
Serial.println(F("This IMU is not supported by the FastIMU library."));
|
|
}
|
|
Serial.println(F("======================================"));
|
|
}
|
|
else if (readByte(IMUList[i].Address2, IMUList[i].Register) == IMUList[i].ExpectedID)
|
|
{
|
|
detected = true;
|
|
Serial.print(F("IMU Found: "));
|
|
Serial.print(IMUList[i].IMUName);
|
|
Serial.print(F(" On address: 0x"));
|
|
Serial.println(IMUList[i].Address2, HEX);
|
|
Serial.print(F("This IMU is capable of the following axis: "));
|
|
Serial.println(IMUList[i].IMUCapabilities);
|
|
if (IMUList[i].LibSupported) {
|
|
Serial.println(F("This IMU is supported by the FastIMU library."));
|
|
}
|
|
else
|
|
{
|
|
Serial.println(F(" This IMU is not supported by the FastIMU library."));
|
|
}
|
|
Serial.println(F("======================================"));
|
|
}
|
|
}
|
|
if (!detected) {
|
|
Serial.println(F("No IMU detected"));
|
|
Serial.println(F("======================================"));
|
|
}
|
|
delay(1000);
|
|
}
|
|
|
|
uint8_t readByte(uint8_t address, uint8_t subAddress)
|
|
{
|
|
uint8_t data; // `data` will store the register data
|
|
Wire.beginTransmission(address); // Initialize the Tx buffer
|
|
Wire.write(subAddress); // Put slave register address in Tx buffer
|
|
int i = Wire.endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive
|
|
if (i == 5) {
|
|
return 0;
|
|
errorflag = true;
|
|
}
|
|
i = Wire.requestFrom(address, (uint8_t) 1, true); // Read one byte from slave register address
|
|
if (i == 0) {
|
|
return 0;
|
|
errorflag = true;
|
|
}
|
|
if (Wire.available()) {
|
|
data = Wire.read(); // Fill Rx buffer with result
|
|
}
|
|
return data; // Return data read from slave register
|
|
}
|