/*
 * MPU6050 : lecture brute sans bibliothèque — Kaihatsu Lab
 * Lit accélération, température et rotation directement dans les registres, et affiche le résultat converti.
 * Adresse I2C : 0x68 (0x69 si la broche AD0 est reliée au 3,3 V).
 */
#include <Wire.h>

const uint8_t MPU_ADDR = 0x68;
const int PIN_SDA = 21;
const int PIN_SCL = 22;

// Sensibilités pour les plages choisies plus bas : ±2 g -> 16384 LSB/g, ±250 °/s -> 131 LSB/(°/s)
const float LSB_PER_G   = 16384.0;
const float LSB_PER_DPS = 131.0;

void writeReg(uint8_t reg, uint8_t value) {
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(reg);
  Wire.write(value);
  Wire.endTransmission();
}

int16_t read16() {                        // deux octets, poids fort en premier
  uint8_t hi = Wire.read();
  uint8_t lo = Wire.read();
  return (int16_t)((hi << 8) | lo);
}

bool readAll(int16_t &ax, int16_t &ay, int16_t &az, int16_t &tmp, int16_t &gx, int16_t &gy, int16_t &gz) {
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x3B);                       // ACCEL_XOUT_H : début des 14 octets de mesure
  if (Wire.endTransmission(false) != 0) return false;
  if (Wire.requestFrom(MPU_ADDR, (uint8_t)14) != 14) return false;
  ax = read16(); ay = read16(); az = read16();
  tmp = read16();
  gx = read16(); gy = read16(); gz = read16();
  return true;
}

void setup() {
  Serial.begin(115200);
  Wire.begin(PIN_SDA, PIN_SCL);
  Wire.setClock(400000);

  writeReg(0x6B, 0x00);                   // PWR_MGMT_1 : sort du mode veille (le capteur démarre endormi)
  writeReg(0x1C, 0x00);                   // ACCEL_CONFIG : ±2 g
  writeReg(0x1B, 0x00);                   // GYRO_CONFIG  : ±250 °/s
  writeReg(0x1A, 0x03);                   // CONFIG : filtre passe-bas interne à environ 44 Hz

  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x75);                       // WHO_AM_I : 0x68 attendu
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_ADDR, (uint8_t)1);
  Serial.print(F("WHO_AM_I = 0x"));
  Serial.println(Wire.read(), HEX);
}

void loop() {
  int16_t ax, ay, az, tmp, gx, gy, gz;
  if (!readAll(ax, ay, az, tmp, gx, gy, gz)) {
    Serial.println(F("Lecture impossible : verifie SDA, SCL et l'adresse"));
    delay(500);
    return;
  }
  Serial.print(F("acc (g) "));
  Serial.print(ax / LSB_PER_G, 2); Serial.print(' ');
  Serial.print(ay / LSB_PER_G, 2); Serial.print(' ');
  Serial.print(az / LSB_PER_G, 2);
  Serial.print(F(" | gyro (deg/s) "));
  Serial.print(gx / LSB_PER_DPS, 1); Serial.print(' ');
  Serial.print(gy / LSB_PER_DPS, 1); Serial.print(' ');
  Serial.print(gz / LSB_PER_DPS, 1);
  Serial.print(F(" | temp "));
  Serial.print(tmp / 340.0 + 36.53, 1);
  Serial.println(F(" C"));
  delay(100);
}