/****************************************************************
*                  Arduino CMPS10 example code                  *
*                  CMPS10 running serial mode                   *
*                    by James Henderson 2011                    *
*                                                               *
*  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 GETDATA 0x23                          // Used to get angle high and low bytes and pitch and roll values.
#define GETSOFT 0x11                          // Used to get the software version from the CMPS10

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

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

void loop(){
   byte highByte, lowByte, fine;              // Used to store angle values
   char pitch, roll;                          // Used to store pitch and roll values
   int bearing;
   Serial.print(GETDATA, BYTE);
   delay(10);                                 // 10mS delay to allow data to be sent
   if(Serial.available() > 0);
   {
     highByte = Serial.read();
     lowByte = Serial.read();
     pitch = Serial.read();
     roll = Serial.read();
   }
   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 of bytes so that they will display corectly as signed values.
  
  lcd.setCursor(0,0);                          
  lcd.print("CMPS10 Example V:");
  lcd.print(soft_ver());                      // Display software version of the CMPS10
  
  lcd.setCursor(1,0);                          
  lcd.print("Bearing = ");                    // Display the full bearing and fine bearing seperated by a decimal point 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(){                               // Function returns software version of CMPS10
   int data;
   Serial.print(GETSOFT, BYTE); 
   delay(10);   
   if(Serial.available() > 0){
     data = Serial.read();
   }    
   return(data);
}