Verifying Integrity Of 9DOF Data Sent To Arduino Mega
In this video I show you how I verify the integrity of data being sent from the 9DOF (9 degrees of freedom) to an Arduino Mega. The only reason for doing this is to ensure that a poor cable connection does not result in the Arduino using corrupted or incorrect X,Y,Z data (which could be catastrophic for a flying craft).
Here's my code for receiving the 9DOF data and checking it, there may be bugs, use at your own risk!
float processByteToVal(int positionInArray, byte theData, float currentTotal)
{
float newData=(float)(theData-48);//turn a byte of 49 into a number of 1, byte of 54 into int 6, etc.
float outputValue=0;
if(positionInArray==0)
{
outputValue=currentTotal+(newData/100);
}
else if(positionInArray==1)
{
outputValue=currentTotal+(newData/10);
}
else if(positionInArray==2)
{
outputValue=currentTotal;
}
else if(positionInArray==3)
{
outputValue=currentTotal+newData;
}
else if(positionInArray==4)
{
if(theData==45)//the original byte passed was a minus sign;
{
outputValue=0-currentTotal;
}
else
{
outputValue=currentTotal+(newData*10);
}
}
else if(positionInArray==5)
{
outputValue=currentTotal+(newData/100);
if(theData==45)//the original byte passed was a minus sign;
{
outputValue=0-currentTotal;
}
else
{
outputValue=currentTotal+(newData*100);
}
}
else if(positionInArray==6)//positionInArray 6 can only ever be a negative sign
{
if(theData==45)//the original byte passed was a minus sign;
{
outputValue=0-currentTotal;
}
}
return outputValue;
}
int getData()
{
int inByte;
int dataFound=0;
while(dataFound<1)
{
if(Serial3.available())
{
inByte = Serial3.read();
return inByte;
dataFound=1;
}
else
{
//delay(1);
}
}
}
void getAttitude() {
int inByte=0;
Serial3.flush();
while(inByte!=58) // 58 is :
{
inByte = getData();
}
//XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
//XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
//XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
inByte = getData();
int xPos=0;
float xVal=0;
byte xData[7];
while(inByte!=44) // 44 is ,
{
if(inByte!=-1) // -1 is not available
{
xData[xPos]=inByte;
xPos++;
}
inByte = getData();
}
int newPos=0;
xPos--;
while(xPos>=0)
{
xVal=processByteToVal(newPos, xData[xPos], xVal);
xPos--;
newPos++;
}
//YYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYY
//YYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYY
//YYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYY
inByte = getData();
int yPos=0;
float yVal=0;
byte yData[7];
while(inByte!=44) // 44 is ,
{
if(inByte!=-1) // -1 is not available
{
yData[yPos]=inByte;
yPos++;
}
inByte = getData();
}
newPos=0;
yPos--;
while(yPos>=0)
{
yVal=processByteToVal(newPos, yData[yPos], yVal);
yPos--;
newPos++;
}
//ZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZ
//ZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZ
//ZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZ
inByte = getData();
int zPos=0;
float zVal=0;
byte zData[7];
while(inByte!=44) // 44 is ,
{
if(inByte!=-1) // -1 is not available
{
zData[zPos]=inByte;
zPos++;
}
inByte = getData();
}
newPos=0;
zPos--;
while(zPos>=0)
{
zVal=processByteToVal(newPos, zData[zPos], zVal);
zPos--;
newPos++;
}
//TOTAL - TOTAL - TOTAL - TOTAL -TOTAL
//TOTAL - TOTAL - TOTAL - TOTAL -TOTAL
//TOTAL - TOTAL - TOTAL - TOTAL -TOTAL
inByte = getData();
//Serial.println ("");
//Serial.print("This is total: ");
int totalPos=0;
float totalVal=0;
byte totalData[7];
while(inByte!=13) // 44 is ,
{
if(inByte!=-1) // -1 is not available
{
totalData[totalPos]=inByte;
//Serial.print(inByte, BYTE);
totalPos++;
}
inByte = getData();
}
newPos=0;
totalPos--;
while(totalPos>=0)
{
totalVal=processByteToVal(newPos, totalData[totalPos], totalVal);
totalPos--;
newPos++;
}
// Serial.print("totalVal is: ");
// Serial.println(totalVal);
float localTotal=xVal+yVal+zVal;
if((abs(totalVal)-abs(localTotal)<=0.03) && (abs(totalVal)-abs(localTotal)>=-0.03))
{
currentXAngle=0-xVal;//currentXAngle and currentYAngle are the gloabl variables that store the current x,y,z that we use throughout the code.
currentYAngle=yVal;
currentZAngle=0-zVal;
//FOR DEBUG PURPOSES ONLY, MUST REMOVE BEFORE FLIGHT!!!!!!!!!!!!!!!!!!!
//currentXAngle=0;
//currentYAngle=0;
imuSuccesses++;
}
else
{
Serial.println("-------------------------The data is NOT NOT NOT NOT NOT NOT valid!!!!!!------------------");
Serial.println("");
imuFailures++;
}
}