OK, I believe I see a few problems. Let me start with explaining what you probably already know. In binary output mode the data comes in “bursts” at the sampling frequency. Each burst consists of :
-
a leading ASCII “A”, one byte, value = 65d, 41h, 0100 0001b (that’s decimal, hex and binary representations)
-
2 bytes of unsigned integer for the count, value = from 0 to 32767d (7F,FF h in MSB then LSB order)
-
2 bytes of unsigned integer for the X accel, value = from 0 to 1023d (03, FF h in MSB then LSB order)
-
2 bytes of unsigned integer for the Y accel, value = from 0 to 1023d (03, FF h in MSB then LSB order)
-
2 bytes of unsigned integer for the Z accel, value = from 0 to 1023d (03, FF h in MSB then LSB order)
-
2 bytes of unsigned integer for the pitch gyro, value = from 0 to 1023d (03, FF h in MSB then LSB order)
-
2 bytes of unsigned integer for the yaw gyro, value = from 0 to 1023d (03, FF h in MSB then LSB order)
-
2 bytes of unsigned integer for the roll gyro, value = from 0 to 1023d (03, FF h in MSB then LSB order)
-
a trailing ASCII “Z”, one byte, value = 90d, 5Ah, 0101 1010b
… for a total of 16 bytes per burst.
First problem is that you do this …
int imuData[9];
void setup() {
Serial.begin(115200);
Serial.println("#");
Serial.println("-");
}
void loop() {
for(int i=0; i<9; i++) {
imuData[i]=Serial.read();
Serial.print(imuData[i]);
Serial.print("__");
}
… which treats each incoming byte as a signed 2 byte integer. I think (because I don’t speak Arduino sketch language) you need to declare imuData() as an unsigned integer and then you need to combine the bytes just read in before storing them in imuData(). This gets the binary data, which was split into bytes to be transmitted to the Arduino, back into it’s proper form. Obviously the single byte characters “A” and “Z” don’t get this treatment.
Problem #2 : that should be serial1.read
Problem #3 : you need to setup the serial port (0) to/from the PC at the Arduino end
? Problem #4 ? : why are you sending a space to the IMU after starting it ? Is that required ? I don’t think so. I read that it would stop the IMU (so I got rid of it below).
So what to do ? Read the IMU data in as bytes because that’s all you can do (I think), store them that way. Combine them (where required) and store them as imuData() because later in “life” you’ll want those 2 byte values to run your autopilot. Then just for debug purposes, print everything out as ASCII characters via the serial port to the PC where you can look at them and marvel at their goodness.
Take the following as a hint, a starting point as to what to do, not as proper code to do everything.
byte rawData[16]; // this will store the bytes as received from the IMU
unsigned int imuData[6]; // this will store only the imu data needed for the autopilot (accels and gyro)
unsigned int count[1]; // this will store the "recombined" value for count
unsigned int temp[1]; // create a temporary 16 bit variable for use in converting bytes to ints
void setup()
{
Serial.begin(115200); // setup the serial port to the PC to be 115k baud
Serial1.begin(115200); // setup the serial port to the IMU to be 115k baud
Serial1.flush(); // empty the serial port input buffer of any garbage due to power up
Serial1.print("41"); // set the IMU to 50 Hz sample rate just until this all gets working
delay(10); // wait for a 10 msec just in case
Serial1.print("#"); // command the IMU to start sending data, I changed it to a simple print, no CR needed
}
void loop()
{
for(int i=0; i<16; i++)
{
if (Serial1.available()) // make sure data is available from serial port, seems everyone does this
{
rawData[i]=Serial1.read(); // read the raw data from the IMU, byte by byte
}
}
// convert the raw data into 2 byte unsigned integers
for(int i=0; i<6; i++)
{
// high byte of imuData is MSB = 1'st of 2 bytes received from IMU, index into rawData to skip "A" and count
temp = rawData(2*i+3); // copy 1'st byte into unsigned int
temp = temp*256; // do it this way to get around Arduino byte to int problem
imuData(i) = temp + rawData(2*i+4); // 2 bytes of binary are now 1 word of binary
{
temp = rawData(1);
temp = temp*256;
count[1] = temp + rawData(2); // convert count as well, store separately from imuData
// Now send data out over serial0 port to PC, delete this later when everything is working as it takes a lot of CPU cycles
Serial.print(rawData(0),BYTE); // print leading "A" knowing it's already ASCII
Serial.print(","); // print a comma, will make it easy to import into plotting program (as CSV file)
Serial.print(count); // print count of samples
Serial.print(",");
for(int i=0; i<6; i++)
{
Serial.print(imuData[i]); // print IMU data
Serial.print(",");
}
Serial.println(rawData(15),BYTE); // print trailing "Z" followed by carriage return
}
Now sending all this data out over the serial port as ASCII characters takes some time. If I have 4 (the max it could be) characters (bytes) per accel & gyro parameter plus 5 (max) for ‘count’ plus the commas and an “A” and a “Z” and a CR at the end … that will take < 4 msec and if you’re running sampling at 150 Hz*, it should leave you with ~ 2.66 msec to do the reading and converting. Reading in the data from the IMU will take ~ 1.66 msec so you have 1 msec to do all the converting I’ve coded above. I think that’s enough time but if you can do it more efficiently (and I’m sure it can be done !) then do it.
*Just to keep problems to a minimum while we’re debugging this, I changed the sampling rate to something slower (sending an ASCII 41d to the IMU before starting it sets the sample rate to 50 Hz). Feel free to keep or remove it. When everything is working properly, you may want to set it back to 100+ Hz (and remove the print commands).
I did not add in any debug statements for timeouts on the IMU-Arduino serial port, nor check for overruns or framing errors (I don’t know how). Nor did I verify the 1’st byte received from the IMU was indeed an ASCII “A”. You might want to consider doing these if the data still comes out weird.