🤖Have you ever tried Chat.M5Stack.com before asking??😎
    M5Stack Community
    • Categories
    • Recent
    • Tags
    • Popular
    • Users
    • Groups
    • Search
    • Register
    • Login

    Problems using MPU6886 (M5 Atom Matrix) with HMC5883L

    General
    1
    1
    2.0k
    Loading More Posts
    • Oldest to Newest
    • Newest to Oldest
    • Most Votes
    Reply
    • Reply as topic
    Log in to reply
    This topic has been deleted. Only users with topic management privileges can see it.
    • M
      Mikicherry
      last edited by Mikicherry

      Hello people!

      I am trying to use the IMU of M5 Atom Matrix with a HMC5883L (magnetometer), nevertheless, when i join the code of HMC5883L and MPU6886, the sensors don't work well (they give wrong informations about roll, pitch and yaw). The accelerometer of MPU6886 keeps the acceleration values freezed (this must be the reason roll and pitch show wrong values). Yaw values given by HMC5883L are strange too. When I test the two codes separated (HMC5883L is tested connected to M5 Atom Matrix), the sensors work perfectly fine. I use I2C ports 25 (SDA) and 21 (SCL). I've already used 21 (SDA) and 22 (SCL) too, but the same problems appear.

      My code is shown below:

      #include <Wire.h>
      #include <HMC5883L.h>
      #include "M5Atom.h"

      #define CALIBRATION__MAGN_USE_EXTENDED true

      const float magn_ellipsoid_center[3] = {-112.932, -417.847, -153.557};
      const float magn_ellipsoid_transform[3][3] = {{0.922957, 0.0179191, -0.0157219}, {0.0179191, 0.984359, 0.0385355}, {-0.0157219, 0.0385355, 0.890763}};
      float pitch, pitch1, roll, roll1, yaw, yaw1;
      float accX, accY, accZ, gyroX, gyroY, gyroZ, heading;
      float magnetom[3];
      float magnetom_tmp[3];

      HMC5883L compass;

      void setup()
      {
      // Enable Serial, enable I2C, enable Display
      M5.begin(true, true, true);
      //Wire.begin(25, 21);

      Serial.begin(115200);

      // Initialize HMC5883L
      Serial.println("Initialize HMC5883L");
      while (!compass.begin())
      {
      Serial.println("Could not find a valid HMC5883L sensor, check wiring!");
      delay(300);
      }

      M5.IMU.Init();

      // Set measurement range
      // +/- 0.88 Ga: HMC5883L_RANGE_0_88GA
      // +/- 1.30 Ga: HMC5883L_RANGE_1_3GA (default)
      // +/- 1.90 Ga: HMC5883L_RANGE_1_9GA
      // +/- 2.50 Ga: HMC5883L_RANGE_2_5GA
      // +/- 4.00 Ga: HMC5883L_RANGE_4GA
      // +/- 4.70 Ga: HMC5883L_RANGE_4_7GA
      // +/- 5.60 Ga: HMC5883L_RANGE_5_6GA
      // +/- 8.10 Ga: HMC5883L_RANGE_8_1GA
      compass.setRange(HMC5883L_RANGE_1_9GA);

      // Set measurement mode
      // Idle mode: HMC5883L_IDLE
      // Single-Measurement: HMC5883L_SINGLE
      // Continuous-Measurement: HMC5883L_CONTINOUS (default)
      compass.setMeasurementMode(HMC5883L_CONTINOUS);

      // Set data rate
      // 0.75Hz: HMC5883L_DATARATE_0_75HZ
      // 1.50Hz: HMC5883L_DATARATE_1_5HZ
      // 3.00Hz: HMC5883L_DATARATE_3HZ
      // 7.50Hz: HMC5883L_DATARATE_7_50HZ
      // 15.00Hz: HMC5883L_DATARATE_15HZ (default)
      // 30.00Hz: HMC5883L_DATARATE_30HZ
      // 75.00Hz: HMC5883L_DATARATE_75HZ
      compass.setDataRate(HMC5883L_DATARATE_15HZ);

      // Set number of samples averaged
      // 1 sample: HMC5883L_SAMPLES_1 (default)
      // 2 samples: HMC5883L_SAMPLES_2
      // 4 samples: HMC5883L_SAMPLES_4
      // 8 samples: HMC5883L_SAMPLES_8
      compass.setSamples(HMC5883L_SAMPLES_4);

      }

      void Matrix_Vector_Multiply(const float a[3][3], const float b[3], float out[3])
      {
      for(int x = 0; x < 3; x++)
      {

      = a[x][0] * b[0] + a[x][1] * b[1] + a[x][2] * b[2];

      }
      }

      float tiltCompensate(Vector mag, float roll, float pitch)
      {

      // Some of these are used twice, so rather than computing them twice in the algorithem we precompute them before hand.
      

      float cosRoll = cos(roll);
      float sinRoll = sin(roll);
      float cosPitch = cos(pitch);
      float sinPitch = sin(pitch);

      // Tilt compensation
      float Xh = mag.XAxis * cosPitch + mag.ZAxis * sinPitch;
      float Yh = mag.XAxis * sinRoll * sinPitch + mag.YAxis * cosRoll - mag.ZAxis * sinRoll * cosPitch;

      float heading = atan2(Yh, Xh);

      return heading;
      }

      float correctAngle(float heading)
      {
      if (heading < 0) { heading += 2 * PI; }
      if (heading > 2 * PI) { heading -= 2 * PI; }

      return heading;
      }

      void loop()
      {
      Vector raw = compass.readRaw();
      Vector norm = compass.readNormalize();

      M5.IMU.getAccelData(&accX, &accY, &accZ);
      M5.IMU.getGyroData(&gyroX, &gyroY, &gyroZ);
      M5.IMU.getMadData(&pitch, &roll, &yaw);

      heading = atan2(magnetom[1], magnetom[0]);
      //heading=tiltCompensate(norm, roll, pitch);

      float declinationAngle = -(43 + (23.0 / 60.0)) / (180 / M_PI);
      heading += declinationAngle;
      heading = correctAngle(heading);
      heading = heading * 180/M_PI;

      magnetom[0] = raw.XAxis;
      magnetom[1] = raw.YAxis;
      magnetom[2] = raw.ZAxis;

      //soft iron calibration
      #if CALIBRATION__MAGN_USE_EXTENDED == true
      for (int i = 0; i < 3; i++)
      magnetom_tmp[i] = magnetom[i] - magn_ellipsoid_center[i];
      Matrix_Vector_Multiply(magn_ellipsoid_transform, magnetom_tmp, magnetom);
      //hard iron calibration in the HMC5883L.cpp code
      #else
      magnetom[0] = norm.XAxis;
      magnetom[1] = norm.YAxis;
      magnetom[2] = norm.ZAxis;
      #endif

      //NWU
      //MadgwickAHRSupdate(gyroX * DEG_TO_RAD, gyroY * DEG_TO_RAD, gyroZ * DEG_TO_RAD, accX, accY, accZ, magnetom[1], -magnetom[0], -magnetom[2], &pitch1, &roll1, &yaw1);

      roll = abs(roll-90);

      Serial.printf("roll: %5.2f, pitch: %5.2f, heading: %5.2f", roll, pitch, heading);
      Serial.printf("\n");
      Serial.printf("ax: %5.2f, ay: %5.2f, az: %5.2f", accX, accY, accZ);
      Serial.printf(" g");
      Serial.printf("\n");
      // Serial.printf("magX: %5.2f, magY: %5.2f, magZ: %5.2f", norm.XAxis, norm.YAxis, norm.ZAxis);
      // Serial.printf("\n");

      delay(50);
      }

      1 Reply Last reply Reply Quote 0
      • First post
        Last post