/*****************************************
*     CMPS12 I2C example for Arduino     *
*        By James Henderson, 2014        * 
*****************************************/

#include <Wire.h>

#define CMPS12_ADDRESS 0x60  // Addresse du CMPS12 sur 7 bits
#define ANGLE_8  1           // Registre contenant la valeur de l'angle sur 8bits
                             // Sert de référence pour la lecture dans la table
                             
unsigned char high_byte, low_byte, angle8;
char pitch, roll;
unsigned int angle16;

void setup()
{
  Serial.begin(9600);  // Port série
  Wire.begin();
}

void loop()
{

  Wire.beginTransmission(CMPS12_ADDRESS);  // Démarrage de la communication avec le CMPS12
  Wire.write(ANGLE_8);                     // Envoi du numéro du premier registre de la 
                                           // table à partir duquel on souhaite lire
  Wire.endTransmission();
 
  // On demande la lecture de 5 octet à partir du registre 0x01
  // afin d'obtenir l'angle sur 8bits, 
  // l'angle sur 16 bits, le pitch et le roll
  Wire.requestFrom(CMPS12_ADDRESS, 5);       
  
  while(Wire.available() < 5);        // On attend la fin de la transmission des 5 octets
  
  angle8 = Wire.read();               // Lecture des octets
  high_byte = Wire.read();
  low_byte = Wire.read();
  pitch = Wire.read();
  roll = Wire.read();
  
  angle16 = high_byte;                 // Calcul de l'angle sur 16bits
  angle16 <<= 8;
  angle16 += low_byte;
    
  Serial.print("roll: ");               // Affichage de roll
  Serial.print(roll, DEC);
  
  Serial.print("    pitch: ");          // Affichage de pitch
  Serial.print(pitch, DEC);
  
  Serial.print("    angle full: ");     // Affichage de l'angle codé sur 16 bits avec décimale
  Serial.print(angle16 / 10, DEC);
  Serial.print(".");
  Serial.print(angle16 % 10, DEC);
  
  Serial.print("    angle 8: ");        // Affichage de l'angle codé sur 8 bits
  Serial.println(angle8, DEC);
  
  delay(1000);                           // Attente entre deux mesures
}