# main.ino

- Project: [esp32-qmc5883p-compass-heading](https://flashesp.com/explore/UYUmF7ygOj8LwH4BxZO1N.md) by Mikrobot.pl
- Board: ESP32 Dev Module
- Language: Arduino (C++)
- Web page: https://flashesp.com/explore/UYUmF7ygOj8LwH4BxZO1N/files/cP6wDgnK2CcBrVVMuQOcc

```cpp
#include <Wire.h>
#include <math.h>

// QMC5883P I2C address (datasheet default)
const uint8_t QMC5883P_ADDR = 0x2C;

// Registers
const uint8_t MODE_REG   = 0x0A;
const uint8_t CONFIG_REG = 0x0B;

const uint8_t X_LSB_REG  = 0x01;
const uint8_t STATUS_REG = 0x09;

// I2C pins for ESP32
constexpr int I2C_SDA = 21;
constexpr int I2C_SCL = 22;

// Conversion for ±8G: 3750 LSB/G and 1G = 100 µT => µT per LSB = 100/3750
constexpr float UT_PER_LSB = 100.0f / 3750.0f;

// Magnetic declination (degrees). Set to your local value for true-north heading.
constexpr float DECLINATION_DEG = 0.0f;

void initQMC5883P() {
  // Continuous mode, ODR = 200 Hz (matches your original settings)
  Wire.beginTransmission(QMC5883P_ADDR);
  Wire.write(MODE_REG);
  Wire.write(0xCF);
  Wire.endTransmission();

  // Set/Reset ON, Range = ±8G (matches your original settings)
  Wire.beginTransmission(QMC5883P_ADDR);
  Wire.write(CONFIG_REG);
  Wire.write(0x08);
  Wire.endTransmission();
}

bool dataReady() {
  Wire.beginTransmission(QMC5883P_ADDR);
  Wire.write(STATUS_REG);
  Wire.endTransmission(false);
  Wire.requestFrom(QMC5883P_ADDR, (uint8_t)1);

  if (Wire.available() != 1) return false;
  uint8_t st = Wire.read();
  return (st & 0x01) != 0; // DRDY bit
}

bool readQMC5883PData(int16_t& x, int16_t& y, int16_t& z) {
  // Start reading from X_LSB_REG
  Wire.beginTransmission(QMC5883P_ADDR);
  Wire.write(X_LSB_REG);
  if (Wire.endTransmission(false) != 0) return false;

  // Read 6 bytes: X (LSB/MSB), Y (LSB/MSB), Z (LSB/MSB)
  uint8_t n = Wire.requestFrom(QMC5883P_ADDR, (uint8_t)6);
  if (n != 6) return false;

  uint8_t x_lsb = Wire.read();
  uint8_t x_msb = Wire.read();
  uint8_t y_lsb = Wire.read();
  uint8_t y_msb = Wire.read();
  uint8_t z_lsb = Wire.read();
  uint8_t z_msb = Wire.read();

  x = (int16_t)((x_msb << 8) | x_lsb);
  y = (int16_t)((y_msb << 8) | y_lsb);
  z = (int16_t)((z_msb << 8) | z_lsb);

  return true;
}

float headingDegFromXY(float x_uT, float y_uT, float decl_deg) {
  // atan2 gives -180..180 degrees; convert to 0..360 and apply declination
  float hdg = atan2f(y_uT, x_uT) * 180.0f / (float)M_PI;
  hdg += decl_deg;

  if (hdg < 0) hdg += 360.0f;
  if (hdg >= 360.0f) hdg -= 360.0f;
  return hdg;
}

void setup() {
  Serial.begin(115200);

  // Initialize I2C on ESP32 custom pins
  Wire.begin(I2C_SDA, I2C_SCL);
  Wire.setClock(100000);

  initQMC5883P();
  Serial.println("QMC5883P ready.");
}

void loop() {
  // Wait until new data is ready
  if (!dataReady()) {
    delay(5);
    return;
  }

  int16_t x_raw, y_raw, z_raw;
  if (!readQMC5883PData(x_raw, y_raw, z_raw)) {
    Serial.println("Read error");
    delay(100);
    return;
  }

  // Convert raw readings to microtesla (µT)
  float x_uT = x_raw * UT_PER_LSB;
  float y_uT = y_raw * UT_PER_LSB;
  float z_uT = z_raw * UT_PER_LSB;

  // Compute heading (no tilt compensation)
  float heading = headingDegFromXY(x_uT, y_uT, DECLINATION_DEG);

  Serial.printf("X:%.2f  Y:%.2f  Z:%.2f uT  |  Heading: %3.0f°\n",
                x_uT, y_uT, z_uT, heading);

  delay(250);
}
```
