EDIT: The previous version sucked and had bugs. This version should work perfectly.
I’ve been involved in a recent project that required the use of an Arduino Uno to obtain data from CH Robotics’ UM6-LT Inertial Measurement Unit (IMU); specifically, the gyroscope data. The task was fun but challenging since the Arduino environment does not have a debugger. Debugging was performed by inserting comments wherever I could, so that I could determine where the program was faulty. On the plus side, it did help me sharpen my rusty C skills.
I am assuming that the user is already familiar with a good portion of the UM6 datasheet, so I won’t be going over too many details. A few important notes:
• The checksum consists of the summation of all the bytes being sent out, and it is used to verify that the data coming in or going out is legitimate; it is, quite literally, a check-sum.
• These programs were developed for when the IMU’s Broadcast Mode is disabled. Enabling/disabling Broadcast Mode can be done using CH Robotics’ Serial Interface program.
• This program can be used to extract not only the gyro data, but also magnetometer, Euler, and accelerometer data. If you want all three axes, simply change the address of the initial register that you're requesting data from. The program is already set up to request a batch operation.
• For some reason I got lots of bogus data from the IMU when the Arduino's baud rate was set to 115200. Using the CHR Serial Interface, the data from the IMU looked fine. I guess the Arduino can't handle baud rates higher than 57600.
I feel it necessary to include a brief explanation of some of the code, so that there is little doubt as to what is going on:
The program stores the retrieved data to global variables. In the function read_UM6() the first set of IF statements check for the expected characters ‘s’ ‘n’ and ‘p’, the expected PT and the expected data register. This is because the sensor occasionally sends out erroneous data; if an error occurs, then usually the first incoming byte isn’t even ‘s’, in which case it’s safe to assume the rest are bogus. If an error is detected, the serial buffer is promptly flushed of remaining data and the IMU is polled again. If no errors are detected, the desired data are assigned to their respective variables. If the checksum received equals the sum of all the individual bytes received, we then return to the main loop where we print the good data. Otherwise, we re-enter the polling and reading process.
//AUTHOR: Jaz S
//Code for Arduino Uno to retrieve data from CH Robotics IMU
//Bytes sent from IMU is recieved in the following order (1st to last): B3, B2, B1, B0.
#include <SoftwareSerial.h>
#define Tx 3
#define Rx 2
#define BAUD 57600
#define REG_GYRO_PROC_XY 0x5C
#define REG_GYRO_PROC_Z 0x5D
#define GYRO_SCALE_FACTOR 0.0109863 // Convert register data to Degrees per second
/*PT byte we're sending out... */
#define PT_HAS_DATA 0x80 //10000000
#define PT_IS_BATCH 0x40 //01000000
#define PT_BATCH_LEN 0x08 //Batch length = 1
SoftwareSerial UM6Data(Rx, Tx); //pin2 is Rx; pin3 is Tx
struct IMU{
int in;
}phi, theta, psi; //Roll, pitch and yaw, respectively.
void setup(){
Serial.begin(BAUD); //Sets baud rate for communication with PC.
UM6Data.begin(BAUD);
}
void loop(){
POLL_AGAIN:
poll_UM6_gyro();
read_UM6_gyro(); //Needs to be called before being used in the 'if' statement.
if (read_UM6_gyro() == true) {
phi.in = phi.in*GYRO_SCALE_FACTOR;
theta.in = theta.in*GYRO_SCALE_FACTOR;
psi.in = psi.in*GYRO_SCALE_FACTOR;
}
else {
goto POLL_AGAIN;
}
}//end VOID
void poll_UM6_gyro(){ //This void function makes a request for a packet from the IMU
Serial.print("\nPolling the UM6...\n");
byte chksum0 = 0, chksum1 = 0;
unsigned int chksum = 0;
chksum = 's' + 'n' + 'p' + (PT_IS_BATCH | PT_BATCH_LEN) + REG_GYRO_PROC_XY;
chksum1 = chksum >> 8;
chksum0 = chksum & 0xFF;
UM6Data.write('s');
UM6Data.write('n');
UM6Data.write('p');
UM6Data.write(PT_IS_BATCH | PT_BATCH_LEN);
UM6Data.write(REG_GYRO_PROC_XY);
UM6Data.write(chksum1);
UM6Data.write(chksum0);
} //end poll_UM6_gyro
boolean read_UM6_gyro(){
Serial.print("Attempting to read IMU...\n");
phi.in = 0;
theta.in = 0;
psi.in = 0;
unsigned int c = 0;
unsigned int data[5] = {0};
unsigned long data_sum = 0;
byte blank = 0, temp = 0;
byte chksum1 = 0, chksum0 = 0;
unsigned int chksum = 0;
//If there's data in the serial temp register and we haven't finished retrieving data...
if ((UM6Data.available() > 0)){
c = UM6Data.read();
if ((c == 's')){
c = UM6Data.read();
if ((c == 'n')){
c = UM6Data.read();
if ((c == 'p')) {
c = UM6Data.read();
if (c == (PT_HAS_DATA | PT_IS_BATCH | PT_BATCH_LEN)){
c = UM6Data.read();
if (c == REG_GYRO_PROC_XY) {
for (byte i = 0; i < 6; i++) {
data[i] = UM6Data.read();
data_sum += data[i];
}
blank = UM6Data.read();
blank = UM6Data.read();
chksum1 = UM6Data.read();
chksum0 = UM6Data.read();
chksum = (chksum1 << 8) | chksum0;
if (chksum == ('s' + 'n' + 'p' + (PT_HAS_DATA | PT_IS_BATCH | PT_BATCH_LEN) + REG_GYRO_PROC_XY + phi.in + theta.in + psi.in)){
Serial.println("Checksum is good. Returning true!");
phi.in = (data[1] | (data[0] << 8));
theta.in = (data[3] | (data[2] << 8));
psi.in = (data[5] | (data[4] << 8));
return true;
}
else {
Serial.println("Flushing buffer...!");
FLUSH:
while ((UM6Data.available() > 0)) {
blank = UM6Data.read();
}
return false;
}
}
else {
goto FLUSH;
}
}
else {
goto FLUSH;
}
}
else {
goto FLUSH;
}
}
else {
goto FLUSH;
}
}
else {
goto FLUSH;
}
}//end if
else if (UM6Data.available() == 0) { //Requested data never arrived...
Serial.println("Data never arrived....");
return false;
} //end else
else {
Serial.println("Unexpected error.");
return false;
}
} //end read_UM6_gyro
I want to express sincere gratitude to Caleb Chamberlain from CH Robotics’ tech support for answering my pesky questions, and to kyler27 on the Arduino.cc forum whose code gave me an idea of how to start out.