Hi,
I am using an Arduino Mega ADK and the above Inertia Measurement Unit to get information about pitch, roll and yaw (Euler angles). I used their software to adjust the output setting so it only outputs Euler angles as orientation data. I am interfacing it with full duplex UART using a code which I found in an old thread : Serial Comm with XSens Motion Tracker - Libraries - Arduino Forum.
In case you can't open the link:
//0xFA preamble
byte gotoConfig[] = {0xFA,0xFF,0x30,0x00,0xD1};
byte gotoMeasurement[] = {0xFA,0xFF,0x10,0x00,0xF1};
byte setOutputSkipFactor[] = {0xFA,0x01,0xD4,0x02,0xFF,0xFF,0x2B};
byte reqData[] = {0xFA,0x01,0x34,0x00,0xCB};
byte inByte = 0x00;
byte xsensBID,xsensMID,xsensLEN,xsensCHECKSUM,xsensSTATUS;
union {
byte asByte[4];
float asFloat;
} xsensLatitude;
union {
byte asByte[4];
float asFloat;
} xsensLongitude;
union {
byte asByte[4];
float asFloat;
} xsensAltitude;
union {
byte asByte[2];
unsigned int asInt;
} xsensSample;
bool pin1State;
void setup(){
Serial.begin(115200);
Serial1.begin(115200);
//xsensConfigure();
}
void loop() {
/*digitalWrite(1, HIGH);
pin1State = digitalRead(1);
Serial.println(pin1State);/*
/*poll xsens for a message*/
xsensGetData();
delay(1000);
}
/*********************************************************
This function is to take the MTData message from xsens and
place it into global variables which can be recorded by the
microcontroller
**********************************************************/
void xsensGetData(){
byte xsensBuffer[100];
int i = 0;
Serial1.write(reqData,5);
//Serial.print("\nRequest Data");
while(Serial1.available() ==0){}
while(Serial1.available()){
xsensBuffer[i] = Serial1.read();
i++;
}
xsensBID=xsensBuffer[1];
xsensMID=xsensBuffer[2];
xsensLEN=xsensBuffer[3];
if((xsensBID==0xFF&& xsensMID == 0x32)||true){
//Latitude, longitude, altitude
xsensLatitude.asByte[3] = xsensBuffer[4];
xsensLatitude.asByte[2] = xsensBuffer[5];
xsensLatitude.asByte[1] = xsensBuffer[6];
xsensLatitude.asByte[0] = xsensBuffer[7];
xsensLongitude.asByte[3] = xsensBuffer[8];
xsensLongitude.asByte[2] = xsensBuffer[9];
xsensLongitude.asByte[1] = xsensBuffer[10];
xsensLongitude.asByte[0] = xsensBuffer[11];
xsensAltitude.asByte[3] = xsensBuffer[12];
xsensAltitude.asByte[2] = xsensBuffer[13];
xsensAltitude.asByte[1] = xsensBuffer[14];
xsensAltitude.asByte[0] = xsensBuffer[15];
xsensSTATUS = xsensBuffer[16];
xsensSample.asByte[1] = xsensBuffer[17];
xsensSample.asByte[0] = xsensBuffer[18];
xsensCHECKSUM = xsensBuffer[19];
byte byteSum = 0;
for(i = 1;i<=19;i++){
byteSum += xsensBuffer[i];
}
/*The following will print GPS Active - 1, Kalman filter active -1, latitude, longitude, altidude, sample number*/
//Serial.print("MSG-0:");Serial.print(byteSum,HEX);
//Serial.print("\tGPS:");Serial.print(bitRead(xsensSTATUS,2));
//Serial.print("\tKXF:");Serial.print(bitRead(xsensSTATUS,1));
Serial.print("\t");Serial.print(xsensLatitude.asFloat,DEC);
Serial.print("\t");Serial.print(xsensLongitude.asFloat,DEC);
Serial.print("\t");Serial.print(xsensAltitude.asFloat,DEC);
//Serial.print("\t");Serial.print(xsensSample.asInt,DEC);
Serial.println();
xsensLatitude.asFloat = 0;
xsensLongitude.asFloat = 0;
xsensAltitude.asFloat = 0;
xsensSample.asInt = 0;
}//end if
Serial.print("\tBID:");
Serial.print(xsensBID,HEX);
Serial.print("\tMID:");
Serial.print(xsensMID,HEX);
Serial.print("\tLEN:");
Serial.print(xsensLEN,DEC);
Serial.print("\n");
}
/********************************************************
the purpose of this function is to configure the
xsens to only send data when it is polled
*********************************************************/
void xsensConfigure(){
byte xsensByte = 0x00;
int i = 0; //i is a generic counter
Serial.println("\nConfiguration Mode\n");
/*set xsens to configuration mode*/
Serial1.write(gotoConfig,5);
while(Serial1.available() ==0){}
while(Serial1.available()){
xsensByte = Serial1.read();
Serial.print(xsensByte);
Serial.print("\t");
delay(5);
}
/*Tells xsens to only send messagewhen polled*/
Serial.println("\nOnly send data when polled\n");
Serial1.write(setOutputSkipFactor,7);
while(Serial1.available() ==0){}
while(Serial1.available()){
xsensByte = Serial1.read();
Serial.print(xsensByte);
Serial.print("\t");
delay(5);
}
/*set xsens to configuration mode*/
Serial.println("\nMeasurement Mode\n");
Serial1.write(gotoMeasurement,5);
while(Serial1.available() ==0){}
while(Serial1.available()){
xsensByte = Serial1.read();
Serial.print(xsensByte);
Serial.print("\t");
delay(5);
}
Serial.print("\n");
}
/*Message Order
1. GPSPVT Data
2.Temp
3. Calibrated Data
4. Orientation Data
5. Auxiliary Data
6. Position (float - 4 BYTE)
7. Velocity (float - 4 BYTE)
8. Status
9. Sample (unsigned int 2 BYTES)
*/
I uncommented the function xsens configure Function since I already did that with the MT Manager and it would be an endless loop otherwise I guess.
I understand that Latitude and Longitude is in this case the same as yaw and pitch?
This is the documentation (https://www.xsens.com/download/pdf/documentation/mti-1/mti-1-series_datasheet.pdf and the manual which differs in some aspects such as pinouts for some reason: https://www.xsens.com/download/pdf/documentation/mti-1/mti-1-series_dk_user_manual.pdf) .
It uses the Xbus Low level communication protocol: http://amtechs.co.jp/2_gps/pdf/MT%20Low-Level%20Communication%20Protocol%20Documentation.pdf According to page 28 of this the Euler angles are output as a 12 byte value, four bytes for each angle.
My wiring is as follows:
IMU Arduino
VDDIO ---- IORef
VDD ---- 5v
GND ---- GND
nRST ---- RESET
TX ---- RX1
RX ---- TX1
CTS ---- GND
I learned that I need to shift the logic levels from the IMU to the Arduinos TTL by connecting VDDIO and VDD as I did. The if statement in xsensgetData() is never true since MID (Message identifier returns something like 36 (Hex) rather than 32 which is the Request Data command. I attached a screenshot of my Serial monitor. When I change this if statement to always be true I get some data but it is very confusing (2nd screenshot). Maybe because Arduino does not know where the start of the message is? When I move the IMU nothing happens and occasionally I get random high values etc.
I hope someone can understand my problem I know it's a lot of information at once...
