Add methods for identifying the IMU model

okalachev · Oct 31, 2024 · 225b370 · 225b370
1 parent 0ff6929
commit 225b370
Show file tree

Hide file tree

Showing 2 changed files with 14 additions and 5 deletions.
diff --git a/MPU9250.cpp b/MPU9250.cpp
@@ -103,12 +103,12 @@ bool MPU9250::begin() {
     return false;
   }
   /* Check the AK8963 WHOAMI */
-  if (!ReadAk8963Registers(AK8963_WHOAMI_, sizeof(who_am_i_), &who_am_i_)) {
+  if (!ReadAk8963Registers(AK8963_WHOAMI_, sizeof(who_am_i_ak8963_), &who_am_i_ak8963_)) {
     log(errorFmt, 8);
     return false;
   }
-  if (!is_mpu6500_ && who_am_i_ != WHOAMI_AK8963_) {
-    log("Wrong AK8963 WHO_AM_I: 0x%02X", who_am_i_);
+  if (!is_mpu6500_ && who_am_i_ak8963_ != WHOAMI_AK8963_) {
+    log("Wrong AK8963 WHO_AM_I: 0x%02X", who_am_i_ak8963_);
     return false;
   }
   /* Get the magnetometer calibration */
@@ -471,6 +471,14 @@ void MPU9250::getMag(float& x, float& y, float& z) const {
   y = mag_[1];
   z = mag_[2];
 }
+const char* MPU9250::getType() const {
+  switch (who_am_i_) {
+    case WHOAMI_MPU6500_: return "MPU-6500";
+    case WHOAMI_MPU9250_: return "MPU-9250";
+    case WHOAMI_MPU9255_: return "MPU-9255";
+    default: return "UNKNOWN";
+  }
+}
 bool MPU9250::WriteRegister(const uint8_t reg, const uint8_t data) {
   return imu_.WriteRegister(reg, data, spi_clock_);
 }

diff --git a/MPU9250.h b/MPU9250.h
@@ -106,6 +106,7 @@ class MPU9250 : public Logger {
   void getAccel(float& x, float& y, float& z) const;
   void getGyro(float& x, float& y, float& z) const;
   void getMag(float& x, float& y, float& z) const;
+  const char* getType() const;
   inline bool new_imu_data() const {return new_imu_data_;}
   inline float accel_x_mps2() const {return accel_[0];}
   inline float accel_y_mps2() const {return accel_[1];}
@@ -118,7 +119,7 @@ class MPU9250 : public Logger {
   inline float mag_y_ut() const {return mag_[1];}
   inline float mag_z_ut() const {return mag_[2];}
   inline float die_temp_c() const {return temp_;}
-  inline bool isMPU6500() const {return is_mpu6500_;}
+  inline uint8_t whoAmI() const {return who_am_i_;}
 
  private:
   InvensenseImu imu_;
@@ -140,7 +141,7 @@ class MPU9250 : public Logger {
   static constexpr float TEMP_SCALE_ = 333.87f;
   uint8_t asa_buff_[3];
   float mag_scale_[3];
-  uint8_t who_am_i_;
+  uint8_t who_am_i_, who_am_i_ak8963_;
   static constexpr uint8_t WHOAMI_MPU6500_ = 0x70;
   static constexpr uint8_t WHOAMI_MPU9250_ = 0x71;
   static constexpr uint8_t WHOAMI_MPU9255_ = 0x73;