Deteksi tipe IMU yang dipakai menggunakan mikrokontroler ESP32 Library yang dipakai adalah FastIMU https://github.com/LiquidCGS/FastIMU
Kode #
// modifikasi dari https://github.com/LiquidCGS/FastIMU/blob/main/examples/IMUIdentifier/IMUIdentifier.ino
#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 47
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, 0x71, "MPU9250", "3A,3G,3M", true },
{ 0x68, 0x69, 0x75, 0x72, "Counterfeit IMU, use 'IMU_Generic'", "3A,3G, possibly 3M?", true }, //MPU9350? https://android.googlesource.com/kernel/msm/+/android-msm-dory-3.10-kitkat-wear/drivers/iio/imu/inv_mpu6515/inv_mpu_iio.h
{ 0x68, 0x69, 0x75, 0x73, "MPU9255", "3A,3G,3M", true },
{ 0x68, 0x69, 0x75, 0x74, "MPU6515", "3A,3G", true },
{ 0x68, 0x69, 0x75, 0x75, "Counterfeit IMU, use 'IMU_Generic'", "3A,3G, possibly 3M?", true },
{ 0x68, 0x69, 0x75, 0x19, "MPU6886", "3A,3G", true },
{ 0x69, 0x68, 0x00, 0xD1, "BMI160", "3A,3G", true },
{ 0x6B, 0x6A, 0x0F, 0x69, "LSM6DS3", "3A,3G", true },
{ 0x6B, 0x6A, 0x0F, 0x6A, "LSM6DSL or LSM6DS3TR-C", "3A,3G", true },
{ 0x68, 0x69, 0x75, 0x98, "ICM20689", "3A,3G", true },
{ 0x68, 0x69, 0x75, 0x20, "ICM20690", "3A,3G", true },
{ 0x6B, 0x6A, 0x00, 0x05, "QMI8658", "3A,3G", true },
{ 0x18, 0x19, 0x00, 0xFA, "BMI055 or BMX055", "3A,3G or 3A,3G,3M", true },
{ 0x1E, 0x1E, 0x0C, 0x33, "HMC5883L", "3M", true },
{ 0x0D, 0x0D, 0x0D, 0xFF, "QMC5883L", "3M", true },
{ 0x0C, 0x0D, 0x01, 0x09, "AK8975", "3M", false },
{ 0x0E, 0x0F, 0x01, 0x09, "AK8975", "3M", false },
{ 0x0C, 0x0D, 0x01, 0x9A, "AK8963", "3M", true },
{ 0x0E, 0x0F, 0x01, 0x9A, "AK8963", "3M", true },
{ 0x13, 0x10, 0x40, 0x32, "BMM150", "3M", false },
{ 0x12, 0x11, 0x40, 0x32, "BMM150", "3M", false },
{ 0x6B, 0x6A, 0x0F, 0x6B, "LSM6DSR", "3A,3G", false },
{ 0x6B, 0x6A, 0x0F, 0x6C, "LSM6DSO", "3A,3G", false },
{ 0x6B, 0x6A, 0x00, 0xFC, "QMI8610", "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 }
};
#define SDA 15 // sesuaikan dengan port yang dipakai untuk SDA dan SCL
#define SCL 13
void setup() {
Serial.begin(115200);
while (!Serial)
;
errorflag = false;
pinMode(4, INPUT_PULLUP);
Wire.begin(SDA, SCL);
#ifdef WIRE_HAS_TIMEOUT
Wire.setWireTimeout(3000);
#endif
//wake sensors
//BMM150
writeByte(0x10, 0x4B, 0x01);
writeByte(0x11, 0x4B, 0x01);
writeByte(0x12, 0x4B, 0x01);
writeByte(0x13, 0x4B, 0x01);
//enable Magnetometer bypass on invsense IMUs
writeByte(0x68, 0x37, 0x22);
writeByte(0x69, 0x37, 0x22);
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
}
void writeByte(uint8_t address, uint8_t subAddress, uint8_t data) {
Wire.beginTransmission(address); // Initialize the Tx buffer
Wire.write(subAddress); // Put slave register address in Tx buffer
Wire.write(data); // Put data in Tx buffer
Wire.endTransmission(); // Send the Tx buffer
}
Output #
Berikut ini contoh output ketika dipakai mendeteksi MPU-6050 Ternyata tipenya sebenarnya adalah MPU6500
rst:0x1 (POWERON_RESET),boot:0x13 (SPI_FAST_FLASH_BOOT)
configsip: 0, SPIWP:0xee
clk_drv:0x00,q_drv:0x00,d_drv:0x00,cs0_drv:0x00,hd_drv:0x00,wp_drv:0x00
mode:DIO, clock div:1
load:0x3fff0030,len:4744
load:0x40078000,len:15672
load:0x40080400,len:3164
entry 0x4008059c
=========== IMU Identifier ===========
IMU Found: MPU6500 On address: 0x68
Contoh output ketika mendeteksi MPU9250
ets Jun 8 2016 00:22:57
rst:0x1 (POWERON_RESET),boot:0x13 (SPI_FAST_FLASH_BOOT)
configsip: 0, SPIWP:0xee
clk_drv:0x00,q_drv:0x00,d_drv:0x00,cs0_drv:0x00,hd_drv:0x00,wp_drv:0x00
mode:DIO, clock div:1
load:0x3fff0030,len:4744
load:0x40078000,len:15672
load:0x40080400,len:3164
entry 0x4008059c
=========== IMU Identifier ===========
IMU Found: MPU9250 On address: 0x68