#include <Wire.h>

//#define WITHRC
#ifdef WITHRC
  TwoWire WIRE2 (2,I2C_FAST_MODE);
#else
  TwoWire WIRE2 (PB11,PB10);
#endif

#define MyWire WIRE2

uint8_t MPU_address = 0x68;  
uint8_t error;


void setup() {
  Serial.begin(57600);
     
  #ifdef WITHRC
    MyWire.begin();
  #else
    MyWire.begin();
    MyWire.setClock(400000);  //Fast Mode 
  #endif

  delay(1000);

  show_I2C2_Registers();
   
  gyro_setup();
  Serial.println("\nGyro Setup Done");

  delay(6000); //time to prepare Oscillo

  Serial.println("\nMPU 1 Call");
  if (read_sensor(MPU_address, 0x3B, 2) <0 )   Serial.println("MPU 1 error");
  else  Serial.println("MPU 1 Ok");
  show_I2C2_Registers();
  Serial.println("\nMPU 2 Call");
  if (read_sensor(MPU_address, 0x3B, 2) <0 )   Serial.println("MPU 2 error");
  else  Serial.println("MPU 2 Ok");
  show_I2C2_Registers();
     
}


void loop() {}


///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//In this part the various registers of the MPU-6050 are set.
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void gyro_setup(void) {
  MyWire.beginTransmission(MPU_address);                        //Start communication with the MPU-6050.
  MyWire.write(0x6B);                                            //We want to write to the PWR_MGMT_1 register (6B hex).
  MyWire.write(0x00);                                            //Set the register bits as 00000000 to activate the gyro.
  error = MyWire.endTransmission();
  if (error != 0) {
    Serial.println("0x6B Error: "+String(error));
  }

  MyWire.beginTransmission(MPU_address);                        //Start communication with the MPU-6050.
  MyWire.write(0x1B);                                            //We want to write to the GYRO_CONFIG register (1B hex).
  MyWire.write(0x08);                                            //Set the register bits as 00001000 (500dps full scale).
  error = MyWire.endTransmission();
  if (error != 0) {
    Serial.println("0x1B Error: "+String(error));
  }

  MyWire.beginTransmission(MPU_address);                        //Start communication with the MPU-6050.
  MyWire.write(0x1C);                                            //We want to write to the ACCEL_CONFIG register (1A hex).
  MyWire.write(0x10);                                            //Set the register bits as 00010000 (+/- 8g full scale range).
  error = MyWire.endTransmission();
  if (error != 0) {
    Serial.println("0x1C Error: "+String(error));
  }

  MyWire.beginTransmission(MPU_address);                        //Start communication with the MPU-6050.
  MyWire.write(0x1A);                                            //We want to write to the CONFIG register (1A hex).
  MyWire.write(0x03);                                            //Set the register bits as 00000011 (Set Digital Low Pass Filter to ~43Hz).
  error = MyWire.endTransmission();
  if (error != 0) {
    Serial.println("0x1A Error: "+String(error));
  }

 }

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//This part reads the raw gyro and accelerometer data from the MPU-6050
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
int8_t read_sensor(uint8_t sensor_address, uint8_t from_address, uint8_t nbvaluestoread) {
  int8_t sensorData[nbvaluestoread];
  int8_t nbvaluesreturned;
  int watchcount = 0;
  MyWire.beginTransmission(sensor_address);                       //Start communication with the sensor.
  MyWire.write(from_address); 
  error = MyWire.endTransmission();
  while (error != 0) {                                          //Stay in this loop because the MPU-6050 did not responde.
    Serial.println("endTransmission Error :"+String(error));  
    MyWire.beginTransmission(sensor_address);                       
    MyWire.write(from_address);                                     
    error = MyWire.endTransmission();
    watchcount ++;
    if (watchcount >1 ) {Serial.println("Transmission Error after "+String(watchcount)+" try :"+String(error)); return -1 ;}
  }                                           

  
  nbvaluesreturned = MyWire.requestFrom(sensor_address, nbvaluestoread);  
  if( nbvaluesreturned != nbvaluestoread ) Serial.println("nbvaluesreturned Ko: "+String(nbvaluesreturned));
  int i=0;
  while(MyWire.available()) {
      sensorData[i] = MyWire.read();  
      Serial.println("Valeur ["+String(i)+"]= "+String(sensorData[i]));
      i++;
  }
  return 0;
 }


void show_I2C2_Registers() {
  #ifdef WITHRC
    Serial.print("RCC->APB1ENR: "); Serial.println(RCC_BASE->APB1ENR, HEX);
    Serial.print("GPIOB->CRH: "); Serial.println(GPIOB_BASE->CRH, HEX);
    Serial.print("I2C2->CR1: "); Serial.println(I2C2_BASE->CR1, HEX);
    Serial.print("I2C2->CR2: "); Serial.println(I2C2_BASE->CR2, HEX);
    Serial.print("I2C2->SR1: "); Serial.println(I2C2_BASE->SR1, HEX);
    Serial.print("I2C2->SR2: "); Serial.println(I2C2_BASE->SR2, HEX);
    Serial.print("I2C2->CCR: "); Serial.println(I2C2_BASE->CCR, HEX);
    Serial.print("I2C2->TRISE: "); Serial.println(I2C2_BASE->TRISE, HEX);

  #else
    Serial.print("RCC->APB1ENR: "); Serial.println(RCC->APB1ENR, HEX);
    Serial.print("GPIOB->CRH: "); Serial.println(GPIOB->CRH, HEX);
    Serial.print("I2C2->CR1: "); Serial.println(I2C2->CR1, HEX);
    Serial.print("I2C2->CR2: "); Serial.println(I2C2->CR2, HEX);
    Serial.print("I2C2->SR1: "); Serial.println(I2C2->SR1, HEX);
    Serial.print("I2C2->SR2: "); Serial.println(I2C2->SR2, HEX);
    Serial.print("I2C2->CCR: "); Serial.println(I2C2->CCR, HEX);
    Serial.print("I2C2->TRISE: "); Serial.println(I2C2->TRISE, HEX);
  #endif

}