﻿#include <Wire.h>

// MPU6050 I2C címe
#define MPU6050_ADDR 0x68

// Regiszterek
#define INT_PIN_CFG 0x37  // Interrupt pin config regiszter
#define PWR_MGMT_1  0x6B  // Power management regiszter

// QMC5883P I2C címe
const int QMC5883P_ADDR = 0x2C;


const int MODE_REG = 0x0A;
const int CONFIG_REG = 0x0B;

// Adatkimeneti regiszterek
const int X_LSB_REG = 0x01;
const int X_MSB_REG = 0x02;
const int Y_LSB_REG = 0x03;
const int Y_MSB_REG = 0x04;
const int Z_LSB_REG = 0x05;
const int Z_MSB_REG = 0x06;

// Állapotregiszter
const int STATUS_REG = 0x09;

// QMC5883P inicializálás
void initQMC5883P() {
  Wire.begin();
  // Folyamatos mérési módra állítva，Adatkimeneti frekvencia 200Hz，tartomány ±8G
  Wire.beginTransmission(QMC5883P_ADDR);
  Wire.write(MODE_REG);
  Wire.write(0xCF); // 0xCF: folyamatos üzemmód, 200Hz frekvencia
  Wire.endTransmission();

  Wire.beginTransmission(QMC5883P_ADDR);
  Wire.write(CONFIG_REG);
  Wire.write(0x08); // 0x08: Set/Reset mód bekapcsolása，tartomány beállítása ±8G
  Wire.endTransmission();
}

// QMC5883P adatok beolvasása
void readQMC5883PData(int16_t& x, int16_t& y, int16_t& z) {
  Wire.beginTransmission(QMC5883P_ADDR);
  Wire.write(X_LSB_REG);
  Wire.endTransmission(false);
  Wire.requestFrom(QMC5883P_ADDR, 6);

  if (Wire.available() == 6) {
    byte x_lsb = Wire.read();
    byte x_msb = Wire.read();
    byte y_lsb = Wire.read();
    byte y_msb = Wire.read();
    byte z_lsb = Wire.read();
    byte z_msb = Wire.read();

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

void setup() {
  Serial.begin(9600);
  Wire.begin();

  // Wake up MPU6050
  writeMPU6050(PWR_MGMT_1, 0x00);
  delay(100);

  // Enable I2C bypass mode (set bit 1 in 0x37 regiszter)
  enableBypass();

  Serial.println("I2C bypass bekapcsolva.");
  delay(200);
  initQMC5883P();
}

void loop() {
  int16_t x, y, z;
  readQMC5883PData(x, y, z);

  Serial.print("X: ");
  Serial.print(x);
  Serial.print("\tY: ");
  Serial.print(y);
  Serial.print("\tZ: ");
  Serial.println(z);

  delay(1000);
  delay(1000);
}

void enableBypass() {
  // INT_PIN_CFG regiszter 1. bitjét állítjuk be
  // Bit 1 = I2C_BYPASS_EN (0x02)
  writeMPU6050(INT_PIN_CFG, 0x02);
}

void writeMPU6050(uint8_t reg, uint8_t data) {
  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(reg);
  Wire.write(data);
  Wire.endTransmission();
}
