/****************************************************************
*                  Arduino CMPS09 example code                  *
*                    CMPS09 running I2C mode                    *
*                    by James Henderson 2010                    *
*                                                               *
*  This example makes use of the LCDi2c library by Dale Wentz   *
*       http://www.arduino.cc/playground/Code/LCDi2c            *
*****************************************************************/
#include <Wire.h>
#include <LCDi2cR.h>

#define ADDRESS 0x60                          // Defines address of CMPS09

LCDi2cR lcd = LCDi2cR(4,40,0x63,0);           // Defines lcd screen

void setup(){
  Wire.begin();                               // Conects I2C
  lcd.init();                                 // Sets up the LCD display
}

void loop(){
   byte highByte, lowByte, fine;              // highByte and lowByte store high and low bytes of the bearing and fine stores decimal place of bearing
   char pitch, roll;                          // Stores pitch and roll values of CMPS09, chars are used because they support signed value
   int bearing;                               // Stores full bearing
   
   Wire.beginTransmission(ADDRESS);           //starts communication with CMPS09
   Wire.send(2);                              //Sends the register we wish to start reading from
   Wire.endTransmission();

   Wire.requestFrom(ADDRESS, 4);              // Request 4 bytes from CMPS09
   while(Wire.available() < 4);               // Wait for bytes to become available
   highByte = Wire.receive();           
   lowByte = Wire.receive();            
   pitch = Wire.receive();              
   roll = Wire.receive();               
   
   bearing = ((highByte<<8)+lowByte)/10;      // Calculate full bearing
   fine = ((highByte<<8)+lowByte)%10;         // Calculate decimal place of bearing
   
   display_data(bearing, fine, pitch, roll);  // Display data to the LCD03
   
   delay(100);
}

void display_data(int b, int f, int p, int r){ // pitch and roll (p, r) are recieved as ints instead oif bytes so that they will display corectly as signed values.
  
  lcd.setCursor(0,0);                          
  lcd.print("CMPS09 Example V:");
  lcd.print(soft_ver());                        // Display software version of the CMPS09
  
  lcd.setCursor(1,0);                          
  lcd.print("Bearing = ");                    // Display the full bearing and fine bearing seperated by a decimal poin on the LCD03
  lcd.print(b);                               
  lcd.print(".");
  lcd.print(f);
  lcd.print("  ");

  lcd.setCursor(2,0);                         // Display the Pitch value to the LCD03
  lcd.print("Pitch = ");
  lcd.print(p);
  lcd.print(" ");

  lcd.setCursor(3,0);                         // Display the roll value to the LCD03
  lcd.print("Roll = ");
  lcd.print(r);
  lcd.print(" ");
}

int soft_ver(){
   int data;                             // Software version of  CMPS09 is read into data and then returned
   
   Wire.beginTransmission(ADDRESS);
   Wire.send(0);                         // Sends the register we wish to start reading from
   Wire.endTransmission();

   Wire.requestFrom(ADDRESS, 1);        // Request byte from CMPS09
   while(Wire.available() < 1);
   data = Wire.receive();           
   
   return(data);
}