#include <Wire.h>
#define I2C_Address 2
#define UartBaudRate 19200
int Position;
int NextData;
boolean SentenceState = false;
char OS4000Data[128];
char command;
int Heading;
int Heading100;
int Heading10;
int Heading1;
int Heading01;
byte HeadingMSB;
byte HeadingLSB;
int Pitch;
int Pitch100;
int Pitch10;
int Pitch1;
int Pitch01;
byte PitchMSB;
byte PitchLSB;
int Roll;
int Roll100;
int Roll10;
int Roll1;
int Roll01;
byte RollMSB;
byte RollLSB;
int Temp;
byte TempMSB;
byte TempLSB;
int Depth;
byte DepthMSB;
byte DepthLSB;
int MagneticVector;
byte MagneticVectorMSB;
byte MagneticVectorLSB;
int MagneticX;
byte MagneticXMSB;
byte MagneticXLSB;
int MagneticY;
byte MagneticYMSB;
byte MagneticYLSB;
int MagneticZ;
byte MagneticZMSB;
byte MagneticZLSB;
int AccelarationVector;
byte AccelarationVectorMSB;
byte AccelarationVectorLSB;
int AccelarationX;
byte AccelarationXMSB;
byte AccelarationXLSB;
int AccelarationY;
byte AccelarationYMSB;
byte AccelarationYLSB;
int AccelarationZ;
byte AccelarationZMSB;
byte AccelarationZLSB;
int GyroX;
byte GyroXMSB;
byte GyroXLSB;
int GyroY;
byte GyroYMSB;
byte GyroYLSB;
void setup()
{
Serial.begin(UartBaudRate);
Wire.begin(I2C_Address);
Wire.onRequest(requestEvent);
Wire.onReceive(ReceiveCommand);
}
void loop()
{
Capture_OS4000();
Check_Sentence();
if ( SentenceState == true ) Extract_Info();
}
void ReceiveCommand(int howMany)
{
while(!Wire.available());
command = Wire.receive();
}
void requestEvent()
{
if ( command == 'A' ) // Send All
{
Wire.send(HeadingMSB);
Wire.send(HeadingLSB);
Wire.send(PitchMSB);
Wire.send(PitchLSB);
Wire.send(RollMSB);
Wire.send(RollLSB);
Wire.send(TempMSB);
Wire.send(TempLSB);
Wire.send(DepthMSB);
Wire.send(DepthLSB);
Wire.send(MagneticVectorMSB);
Wire.send(MagneticVectorLSB);
Wire.send(MagneticXMSB);
Wire.send(MagneticXLSB);
Wire.send(MagneticYMSB);
Wire.send(MagneticYLSB);
Wire.send(MagneticZMSB);
Wire.send(MagneticZLSB);
Wire.send(AccelarationVectorMSB);
Wire.send(AccelarationVectorLSB);
Wire.send(AccelarationXMSB);
Wire.send(AccelarationXLSB);
Wire.send(AccelarationYMSB);
Wire.send(AccelarationYLSB);
Wire.send(AccelarationZMSB);
Wire.send(AccelarationZLSB);
Wire.send(GyroXMSB);
Wire.send(GyroXLSB);
Wire.send(GyroYMSB);
Wire.send(GyroYLSB);
}
else if ( command == 'H' ) // Heading
{
Wire.send(Heading);
}
else if ( command == 'P' ) // Pitch
{
Wire.send(PitchMSB);
Wire.send(PitchLSB);
}
else if ( command == 'R' ) // Roll
{
Wire.send(RollMSB);
Wire.send(RollLSB);
}
else if ( command == 'T' ) // Temp
{
Wire.send(TempMSB);
Wire.send(TempLSB);
}
else if ( command == 'D' ) // Depth
{
Wire.send(DepthMSB);
Wire.send(DepthLSB);
}
else if ( command == 'M' ) // Magnetic Vector
{
Wire.send(MagneticVectorMSB);
Wire.send(MagneticVectorLSB);
}
else if ( command == 'X' ) // Magnetic X
{
Wire.send(MagneticXMSB);
Wire.send(MagneticXLSB);
}
else if ( command == 'Y' ) // Magnetic Y
{
Wire.send(MagneticYMSB);
Wire.send(MagneticYLSB);
}
else if ( command == 'Z' ) // Magnetic Z
{
Wire.send(MagneticZMSB);
Wire.send(MagneticZLSB);
}
else if ( command == 'a' ) // Accelaration Vector
{
Wire.send(AccelarationVectorMSB);
Wire.send(AccelarationVectorLSB);
}
else if ( command == 'x' ) // Accelaration X
{
Wire.send(AccelarationXMSB);
Wire.send(AccelarationXLSB);
}
else if ( command == 'y' ) // Accelaration Y
{
Wire.send(AccelarationYMSB);
Wire.send(AccelarationYLSB);
}
else if ( command == 'z' ) // Accelaration Z
{
Wire.send(AccelarationZMSB);
Wire.send(AccelarationZLSB);
}
else if ( command == 'G' ) // Gyro X
{
Wire.send(GyroXMSB);
Wire.send(GyroXLSB);
}
else if ( command == 'g' ) // Gyro Y
{
Wire.send(GyroYMSB);
Wire.send(GyroYLSB);
}
else
{
//Error
}
command = 'N';
}
void Capture_OS4000()
{
Serial.flush();
while ( Serial.read () != '$' );
Serial.flush();
while( Serial.available() < 127 );
for ( Position = 0; Position < 126; Position++ ){
OS4000Data[Position] = char( Serial.read());
}
}
void Check_Sentence()
{
if ( ( OS4000Data[0] == 'O' ) && ( OS4000Data[1] == 'H' ) && ( OS4000Data[2] == 'P' ) && ( OS4000Data[3] == 'R' ) && ( OS4000Data[4] == ',' ) )SentenceState = true;
else SentenceState = false;
}
void Extract_Info()
{
Heading100 = 0;
Heading10 = 0;
Heading1 = 0;
Heading01 = 0;
if ( OS4000Data[8] == '.' ){
Heading100 = OS4000Data[5] - '0';
Heading10 = OS4000Data[6] - '0';
Heading1 = OS4000Data[7] - '0';
Heading01 = OS4000Data[9] - '0';
NextData = 10;
}
else if ( OS4000Data[7] == '.' ){
Heading10 = OS4000Data[5] - '0';
Heading1 = OS4000Data[6] - '0';
Heading01 = OS4000Data[8] - '0';
NextData = 9;
}
else if ( OS4000Data[6] == '.' ){
Heading1 = OS4000Data[5] - '0';
Heading01 = OS4000Data[7] - '0';
NextData = 8;
}
Heading = 1000Heading100 + 100Heading10 + 10*Heading1 + Heading01;
Pitch100 = 0;
Pitch10 = 0;
Pitch1 = 0;
Pitch01 = 0;
if ( OS4000Data[NextData+1] == '-' ){
if ( OS4000Data[NextData+5] == '.' ){
Pitch100 = OS4000Data[NextData+2] - '0';
Pitch10 = OS4000Data[NextData+3] - '0';
Pitch1 = OS4000Data[NextData+4] - '0';
Pitch01 = OS4000Data[NextData+6] - '0';
NextData += 7;
}
else if ( OS4000Data[NextData+4] == '.' ){
Pitch10 = OS4000Data[NextData+2] - '0';
Pitch1 = OS4000Data[NextData+3] - '0';
Pitch01 = OS4000Data[NextData+5] - '0';
NextData += 6;
}
else if ( OS4000Data[NextData+3] == '.' ){
Pitch1 = OS4000Data[NextData+2] - '0';
Pitch01 = OS4000Data[NextData+4] - '0';
NextData += 5;
}
Pitch = - ( 1000Pitch100 + 100Pitch10 + 10Pitch1 + Pitch01 );
}
else
{
if ( OS4000Data[NextData+4] == '.' ){
Pitch100 = OS4000Data[NextData+1] - '0';
Pitch10 = OS4000Data[NextData+2] - '0';
Pitch1 = OS4000Data[NextData+3] - '0';
Pitch01 = OS4000Data[NextData+5] - '0';
NextData += 6;
}
else if ( OS4000Data[NextData+3] == '.' ){
Pitch10 = OS4000Data[NextData+1] - '0';
Pitch1 = OS4000Data[NextData+2] - '0';
Pitch01 = OS4000Data[NextData+4] - '0';
NextData += 5;
}
else if ( OS4000Data[NextData+2] == '.' ){
Pitch1 = OS4000Data[NextData+1] - '0';
Pitch01 = OS4000Data[NextData+3] - '0';
NextData += 4;
}
Pitch = ( 1000Pitch100 + 100Pitch10 + 10Pitch1 + Pitch01 );
}
Roll100 = 0;
Roll10 = 0;
Roll1 = 0;
Roll01 = 0;
if ( OS4000Data[NextData+1] == '-' ){
if ( OS4000Data[NextData+5] == '.' ){
Roll100 = OS4000Data[NextData+2] - '0';
Roll10 = OS4000Data[NextData+3] - '0';
Roll1 = OS4000Data[NextData+4] - '0';
Roll01 = OS4000Data[NextData+6] - '0';
NextData += 7;
}
else if ( OS4000Data[NextData+4] == '.' ){
Roll10 = OS4000Data[NextData+2] - '0';
Roll1 = OS4000Data[NextData+3] - '0';
Roll01 = OS4000Data[NextData+5] - '0';
NextData += 6;
}
else if ( OS4000Data[NextData+3] == '.' ){
Roll1 = OS4000Data[NextData+2] - '0';
Roll01 = OS4000Data[NextData+4] - '0';
NextData += 5;
}
Roll = - ( 1000Roll100 + 100Roll10 + 10Roll1 + Roll01 );
}
else
{
if ( OS4000Data[NextData+4] == '.' ){
Roll100 = OS4000Data[NextData+1] - '0';
Roll10 = OS4000Data[NextData+2] - '0';
Roll1 = OS4000Data[NextData+3] - '0';
Roll01 = OS4000Data[NextData+5] - '0';
NextData += 6;
}
else if ( OS4000Data[NextData+3] == '.' ){
Roll10 = OS4000Data[NextData+1] - '0';
Roll1 = OS4000Data[NextData+2] - '0';
Roll01 = OS4000Data[NextData+4] - '0';
NextData += 5;
}
else if ( OS4000Data[NextData+2] == '.' ){
Roll1 = OS4000Data[NextData+1] - '0';
Roll01 = OS4000Data[NextData+3] - '0';
NextData += 4;
}
Roll = ( 1000Roll100 + 100Roll10 + 10Roll1 + Roll01 );
}
}