main.ino
Arduino (C++) · 120 lines
#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);
}