Digital Compass & Inclinometer

#include <Wire.h>
#include <Preferences.h>
#include <Adafruit_LEDBackpack.h>

const uint8_t QMC = 0x0D, MPU = 0x69;
const int B1_PIN = 4, B2_PIN = 5, B3_PIN = 6;
const float DECLINATION = 2.0;           // degrees, UAE ≈ +2

Adafruit_7segment disp;
Preferences prefs;
float offX, offY, offZ, zeroPitch = 0, zeroRoll = 0;
int mode = 0;                            // 0 heading, 1 pitch, 2 roll

void w8(uint8_t a, uint8_t r, uint8_t v) { Wire.beginTransmission(a); Wire.write(r); Wire.write(v); Wire.endTransmission(); }
void rd(uint8_t a, uint8_t r, uint8_t* d, int n) {
  Wire.beginTransmission(a); Wire.write(r); Wire.endTransmission(false);
  Wire.requestFrom(a, (uint8_t)n); for (int i = 0; i < n; i++) d[i] = Wire.read();
}
void readMag(float& x, float& y, float& z) {
  uint8_t d[6]; rd(QMC, 0x00, d, 6);    // little-endian X, Y, Z
  x = (int16_t)(d[0] | d[1] << 8); y = (int16_t)(d[2] | d[3] << 8); z = (int16_t)(d[4] | d[5] << 8);
}
void readAcc(float& x, float& y, float& z) {
  uint8_t d[6]; rd(MPU, 0x3B, d, 6);    // big-endian X, Y, Z
  x = (int16_t)(d[0] << 8 | d[1]); y = (int16_t)(d[2] << 8 | d[3]); z = (int16_t)(d[4] << 8 | d[5]);
}

void calibrate() {
  disp.println("CAL"); disp.writeDisplay();
  float mn[3] = {1e6, 1e6, 1e6}, mx[3] = {-1e6, -1e6, -1e6}, v[3];
  unsigned long t0 = millis();
  while (millis() - t0 < 20000) {
    readMag(v[0], v[1], v[2]);
    for (int i = 0; i < 3; i++) { mn[i] = min(mn[i], v[i]); mx[i] = max(mx[i], v[i]); }
    delay(20);
  }
  offX = (mn[0] + mx[0]) / 2; offY = (mn[1] + mx[1]) / 2; offZ = (mn[2] + mx[2]) / 2;
  prefs.putFloat("x", offX); prefs.putFloat("y", offY); prefs.putFloat("z", offZ);
}

void setup() {
  Serial.begin(115200);
  Wire.begin(8, 9);
  pinMode(B1_PIN, INPUT_PULLUP); pinMode(B2_PIN, INPUT_PULLUP); pinMode(B3_PIN, INPUT_PULLUP);

  w8(QMC, 0x0B, 0x01);                   // set/reset period
  w8(QMC, 0x09, 0x1D);                   // continuous, 200 Hz, 8 G, OSR 512
  w8(MPU, 0x6B, 0x00);                   // wake MPU-6500
  disp.begin(0x70);

  prefs.begin("compass");
  offX = prefs.getFloat("x", 0); offY = prefs.getFloat("y", 0); offZ = prefs.getFloat("z", 0);
}

void loop() {
  // buttons
  static bool last1 = HIGH; bool b1 = digitalRead(B1_PIN);
  if (b1 == LOW && last1 == HIGH) mode = (mode + 1) % 3;
  last1 = b1;
  static unsigned long b2Down = 0;
  if (digitalRead(B2_PIN) == LOW) { if (!b2Down) b2Down = millis(); else if (millis() - b2Down > 2000) { calibrate(); b2Down = 0; } }
  else b2Down = 0;

  float ax, ay, az, mx, my, mz;
  readAcc(ax, ay, az);
  readMag(mx, my, mz);
  mx -= offX; my -= offY; mz -= offZ;

  float roll  = atan2(ay, az);
  float pitch = atan2(-ax, sqrt(ay * ay + az * az));
  if (digitalRead(B3_PIN) == LOW) { zeroPitch = pitch; zeroRoll = roll; }

  // tilt-compensated heading
  float xh = mx * cos(pitch) + mz * sin(pitch);
  float yh = mx * sin(roll) * sin(pitch) + my * cos(roll) - mz * sin(roll) * cos(pitch);
  float heading = atan2(-yh, xh) * RAD_TO_DEG + DECLINATION;
  if (heading < 0) heading += 360;
  if (heading >= 360) heading -= 360;

  float value = mode == 0 ? heading
              : mode == 1 ? (pitch - zeroPitch) * RAD_TO_DEG
                          : (roll - zeroRoll) * RAD_TO_DEG;
  if (mode == 0) disp.print((int)heading, DEC);
  else disp.printFloat(value, 1);
  disp.writeDisplay();

  Serial.printf("heading %.0f  pitch %.1f  roll %.1f\n", heading,
                (pitch - zeroPitch) * RAD_TO_DEG, (roll - zeroRoll) * RAD_TO_DEG);
  delay(100);
}