# main.ino

- Project: [esp32c3mini-qmc5883p-compass-heading](https://flashesp.com/explore/NzdB8NC5BFLTOs18nGmak.md) by Mikrobot.pl
- Board: ESP32C3 Dev Module
- Language: Arduino (C++)
- Web page: https://flashesp.com/explore/NzdB8NC5BFLTOs18nGmak/files/BZ6J8QP7yXtRJ79m1pJSK

```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-C3 SuperMini
constexpr int I2C_SDA = 8;
constexpr int I2C_SCL = 9;

// Conversion for +/-8G: 3750 LSB/G and 1G = 100 uT => uT 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 = 6.5f;

// How long to wait for a fresh sample before reporting a problem (ms)
constexpr unsigned long DRDY_TIMEOUT_MS = 2000;

// Is the sensor currently considered present and working?
bool sensorOK = false;

// ---------------------------------------------------------------------------
// Diagnostics
// ---------------------------------------------------------------------------

// Returns true if a device ACKs at the given address
bool devicePresent(uint8_t addr) {
  Wire.beginTransmission(addr);
  return Wire.endTransmission() == 0;
}

// Scans the bus and reports what it finds. Returns number of devices.
int scanBus() {
  int found = 0;
  Serial.println("Skanowanie magistrali I2C...");

  for (uint8_t addr = 1; addr < 127; addr++) {
    if (devicePresent(addr)) {
      Serial.printf("  -> urzadzenie pod adresem 0x%02X", addr);

      // Name the usual suspects so the user knows what they actually have
      if (addr == 0x2C) Serial.print("  (QMC5883P)");
      if (addr == 0x0D) Serial.print("  (QMC5883L - INNY UKLAD!)");
      if (addr == 0x1E) Serial.print("  (HMC5883L - INNY UKLAD!)");
      Serial.println();

      found++;
    }
  }

  if (found == 0) Serial.println("  (nic nie znaleziono)");
  return found;
}

void printNotDetectedHelp(int devicesFound) {
  Serial.println();
  Serial.println("***************************************************");
  Serial.println("*  BLAD: nie wykryto modulu QMC5883P (0x2C)       *");
  Serial.println("***************************************************");
  Serial.println();

  if (devicesFound == 0) {
    Serial.println("Na magistrali I2C nie odpowiada ZADNE urzadzenie.");
    Serial.println("Sprawdz kolejno:");
    Serial.println("  1. VCC modulu -> pin 3V3 plytki");
    Serial.println("  2. GND modulu -> GND plytki");
    Serial.printf ("  3. SDA modulu -> GPIO%d\n", I2C_SDA);
    Serial.printf ("  4. SCL modulu -> GPIO%d\n", I2C_SCL);
    Serial.println("  5. Czy SDA i SCL nie sa zamienione miejscami?");
    Serial.println("  6. Czy goldpiny sa wlutowane? Sam drut wlozony");
    Serial.println("     w otwor czesto nie daje kontaktu.");
    Serial.println("  7. Czy nie trzymasz wcisnietego przycisku BOOT?");
    Serial.println("     Na SuperMini zwiera on linie SCL (GPIO9).");
  } else {
    Serial.println("Na magistrali sa inne urzadzenia, ale nie ma 0x2C.");
    Serial.println("Najprawdopodobniej masz modul z INNYM ukladem:");
    Serial.println("  0x0D = QMC5883L   -> wymaga biblioteki QMC5883LCompass");
    Serial.println("  0x1E = HMC5883L   -> wymaga biblioteki Adafruit_HMC5883");
    Serial.println("Ten szkic dziala wylacznie z QMC5883P pod adresem 0x2C.");
  }

  Serial.println();
  Serial.println("Po poprawieniu polaczen nacisnij RESET na plytce.");
  Serial.println();
}

// ---------------------------------------------------------------------------
// Sensor
// ---------------------------------------------------------------------------

bool initQMC5883P() {
  // Continuous mode, ODR = 200 Hz
  Wire.beginTransmission(QMC5883P_ADDR);
  Wire.write(MODE_REG);
  Wire.write(0xCF);
  if (Wire.endTransmission() != 0) {
    Serial.println("BLAD: zapis do MODE_REG nie powiodl sie.");
    return false;
  }

  // Set/Reset ON, Range = +/-8G
  Wire.beginTransmission(QMC5883P_ADDR);
  Wire.write(CONFIG_REG);
  Wire.write(0x08);
  if (Wire.endTransmission() != 0) {
    Serial.println("BLAD: zapis do CONFIG_REG nie powiodl sie.");
    return false;
  }

  delay(50);
  return true;
}

bool dataReady() {
  Wire.beginTransmission(QMC5883P_ADDR);
  Wire.write(STATUS_REG);
  if (Wire.endTransmission(false) != 0) return 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) {
  Wire.beginTransmission(QMC5883P_ADDR);
  Wire.write(X_LSB_REG);
  if (Wire.endTransmission(false) != 0) return false;

  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) {
  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);
  delay(3000); // czas na enumeracje natywnego USB w ESP32-C3

  Serial.println();
  Serial.println("=== QMC5883P Compass ===");
  Serial.printf("Piny I2C: SDA=GPIO%d  SCL=GPIO%d\n", I2C_SDA, I2C_SCL);
  Serial.println();

  Wire.begin(I2C_SDA, I2C_SCL);
  Wire.setClock(100000);

  int devices = scanBus();
  Serial.println();

  if (!devicePresent(QMC5883P_ADDR)) {
    printNotDetectedHelp(devices);
    sensorOK = false;
    return;
  }

  Serial.println("Modul QMC5883P wykryty pod adresem 0x2C.");

  if (!initQMC5883P()) {
    Serial.println("Modul odpowiada, ale konfiguracja sie nie powiodla.");
    sensorOK = false;
    return;
  }

  Serial.println("Konfiguracja OK - start pomiarow.");
  Serial.println();
  sensorOK = true;
}

void loop() {
  // Sensor never came up - keep reminding instead of staying silent
  if (!sensorOK) {
    Serial.println("Czujnik niedostepny. Popraw polaczenia i nacisnij RESET.");
    delay(5000);
    return;
  }

  // Wait for fresh data, but give up after a timeout instead of hanging
  unsigned long t0 = millis();
  while (!dataReady()) {
    if (millis() - t0 > DRDY_TIMEOUT_MS) {
      if (!devicePresent(QMC5883P_ADDR)) {
        Serial.println("BLAD: modul przestal odpowiadac (0x2C zniknal).");
        Serial.println("Sprawdz polaczenia - luzny styk lub przerwa w zasilaniu.");
      } else {
        Serial.println("UWAGA: brak nowych danych (DRDY nie ustawia sie).");
        Serial.println("Modul odpowiada, ale nie konczy pomiaru.");
      }
      delay(1000);
      return;
    }
    delay(5);
  }

  int16_t x_raw, y_raw, z_raw;
  if (!readQMC5883PData(x_raw, y_raw, z_raw)) {
    Serial.println("BLAD: nie udalo sie odczytac danych z czujnika.");
    delay(1000);
    return;
  }

  float x_uT = x_raw * UT_PER_LSB;
  float y_uT = y_raw * UT_PER_LSB;
  float z_uT = z_raw * UT_PER_LSB;

  float heading = headingDegFromXY(x_uT, y_uT, DECLINATION_DEG);

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

  delay(250);
}
```
