Some DMP troubles – part 2

Alright, it appears that I’m NOT colliding with reading the FIFO and new data coming in, as I thought earlier. Instead, in a previous loop, the resetting of the FIFO probably snaps off the “head” of some data packet being pushed in by the DMP at the same time. Makes a lot more sense, doesn’t it?

I duplicated the original problem with an Arduino UNO using basically jrowberg’s code out of the box, but by removing the Interrupt handler and replacing it by polling the FIFO_COUNT in the main program loop. After eventually reading the 42-byte data packet from the FIFO, I reset the FIFO. By adding a delay of 30 ms or so into the main loop (beyond the update rate of 10ms), I get the “jitter” effect on the little “teapot” airplane. It’s not severe, but it is detectable and annoying.

Then, I modified the program a little: By polling in a loop and letting the FIFO_COUNT go up to 400-something before reading the FIFO data and resetting it, I can see the FIFO_COUNT increase like this:

Multiples of 42 for reference: 42, 84, 126, 168, 210, 252, 294, 336, 378, 420, 462, 504, 546

...
FIFO while waiting: 126
FIFO while waiting: 294
FIFO while waiting: 420
FIFO before reading: 546
Resetting FIFO

FIFO while waiting: 154
FIFO while waiting: 294
FIFO while waiting: 420
FIFO before reading: 546
Resetting FIFO

FIFO while waiting: 136
FIFO while waiting: 262
FIFO while waiting: 388
FIFO before reading: 514 --> Glitch: Read quaternion magn. not 1
Resetting FIFO

FIFO while waiting: 126
FIFO while waiting: 252
FIFO while waiting: 378
FIFO before reading: 504
Resetting FIFO
...

This shows that the increasing FIFO_COUNT before the glitch keeps showing an offset of 10 bytes, but I can’t trust it to always be some constant value when polling.

I don’t really have a solution for this, as I want to read 20 totally unsynchronized sensors at the same time, but at least the problem got re-defined.

Some DMP troubles

Jeff Rowberg’s DMP code uses interrupts to notify the Arduino when a new set of data – a 42-byte packet – is available in the FIFO buffer. In the Teapot example the quaternion form this packet is sent to the PC. I modified the Arduino sketch so, that it does NOT use interrupts but instead polls the FIFO_COUNT register to see if a new packet is available i.e. the count is at least 42. If this is done fast enough it seems to work.

However, when delays are added, say 30us or more, and I read the packet from the FIFO every now and then it probably collides with the DMP writing new data into the FIFO. The result is corrupt data, I think. I found that in the case when the FIFO_COUNT read just prior to reading the FIFO buffer is not a multiple of 42 (the packet size), the quaternion extracted from that packet is not a unit quaternion as it should be.

This makes it a bit hard for my parallel TWI implementation (I don’t want to call it parallel I2C because of trademarks) to read many unsynchronized MPUs without having to discard glitchy data. Here is a related forum post, but I guess it’s only visible to registered users. I’ll quote: “DMP is meant to utilize the interrupt and FIFO system so that it can be synchronized with the host processor, and store the appropriate Quaternion data in the FIFO register. You can utilize the MPU-6050 without these features in raw data mode, but it is required for DMP operation.

(On the same note, the FIFO Count Register description first claims that it contains the “sample count” and later “the number of bytes stored”. I’ll go with “bytes stored”.)

EDIT: Haven’t yet checked if the MPU would do some I2C clock stretching (and I’m ignoring that). I’ll need to check that next.

EDIT #2: Didn’t look for clock stretching. Instead, I’ve got a new theory.

Arduino DUE’s SerialUSB and Processing

The Arduino DUE can have two serial ports over USB: the programming port and the native USB port acting as a CDC device. The programming port can be used through the Serial-object and the native port through the SerialUSB-object.

The trouble was this: I wanted to use the Arduino DUE’s SerialUSB-object (native port) to transmit application data to a Processing sketch. However Processing wouldn’t read anything from the port connected to the DUE’s native port. The Arduino IDE and PuTTY were able to read the native port just fine. The difference: When conneced to the programming port, both the Arduino IDE and PuTTY reset the Arduino (via DTR-signal) and Processing didn’t. So, I gather that it must be a handshake problem of some kind. (Apparently the Java RXTX library used by Processing doesn’t give you access to the control lines?)

Dug up the method size_t Serial_::write(const uint8_t *buffer, size_t size) in the CDC.cpp (arduino-1.5.2\hardware\arduino\sam\cores\arduino\USB) and commented out the check for if (_usbLineInfo.lineState > 0).

I think that did the trick for now… Seems like the Arduino sketch hangs when a certain (buffered?) amount of data is sent until it is actually read by the Processing sketch. Don’t know if there will be other side effects.

Here’s some code to illustrate the point for those who want to try this for themselves. First for the Arduino DUE:

uint8_t counter;

void setup() {

  Serial.begin(115200);
  SerialUSB.begin(115200); // speed is irrelevant for native port
  counter = 0;
}

void loop() {

  Serial.print("Programming port. Counter is at ");
  Serial.println( counter);

  SerialUSB.print("Native port. Counter is at ");
  SerialUSB.println( counter);

  counter++;
  delay(500);
}

And here is the Processing sketch (You’ll need to edit the line portName = Serial.list()[1]; to set the port you want to open on your machine):

import java.util.Arrays;
import processing.serial.*;

String portName;
Serial serialPort;
int speed = 115200;
int bytesReceived = 0;

void setup() {
  size(640, 480);
  System.out.println("Available serial ports are: " + Arrays.toString( Serial.list())); //debug

  // Open 2nd visible serial port
  portName = Serial.list()[1];

  // Serial.list() array's indexes on my machine (this is machine specific):
  // 0 = integrated serial on motherboard
  // 1 = Arduino DUE programming port (when plugged in) - gets received by IDE, PuTTY, Processing
  // 2 = Arduino DUE native port (when plugged in also) - gets received by IDE, PuTTY, but NOT by Processing

  serialPort = new Serial(this, portName, speed);
  System.out.println( "Opened port " + portName); //debug
}

void draw() {
  background(100);
  text("Reading: " + portName, 50, 50);
  text("Received " + bytesReceived + " bytes.", 50, 80);
}

void serialEvent( Serial port) {
  while( port.available() > 0) {
    char c = (char)port.read();
    bytesReceived++;
    System.out.print( c); //debug
  }
}

About the jerky data transfer on the Duemilanove

When trying out serial transfer speeds a while back, I noticed the data flow on the Arduino Duemilanove to be “jerky” compared tho the Arduino Uno. This held true to other boards using the FTDI chip, too. Seems, like there is an explanation for it. I came across Jan Axelson’s book Serial Port Complete (2nd ed), where it says (on page 332) that there are two situations for slow data transfer:

The first occurs when sending only very little data as the FTDI chip likes to fill it’s buffer of 62 bytes before passing them on the USB-bus. (This USB-endpoint sends packets of 62 data and 2 status bytes.) The chip has a latency timer of 16 ms, after which it sends the data regardless of the buffer’s lower byte count. [Edit: the 64 byte packet size is for full-speed USB, but high-speed USB requires 512 byte packets. (p. 345)]

The second case probably explains my problem: When receiving data at 38.75 kbps or higher, the driver holds the data until it has received 4096 bytes (or it’s latency timer has timed out) before passing it to the application.

I assume that the Arduino UNO’s serial driver just moves smaller chunks of data at a time. (Judging by source code that is provided with the Arduino environment, the USB-serial is in UNO’s case programmed on the Atmega8U2 or Atmega16U2 using LUFA.)

The book also suggests reading this app. note. It describes a few methods to adjust some latency times and buffer sizes – in windows. It also says that modem status lines can be used to flush the (FTDI’s, I assume) buffer.

Serial speeds on the Arduino

In the case of having to send lots of data back to a PC, I tested an Arduino UNO (rev 1) for different serial speed setups. I used this simple program on the Arduino to send running counter values and a Processing sketch to read the values and report an error it would receive a value that was out of order. After reading a forum post, I was actually a bit amazed how fast the thing could go using the Atmega8U2 as the USB-serial port. So, I also did the same measurement for an older Arduino Duemilanove which uses the FTDI-chip as the USB-serial port. This was a like the previously mentioned forum post predicted as 230400 was the maximum speed. Also, the Duemilanove’s data rate seemed “jerky” at lower speeds (although the data on the 328’s TX pin seemed to flow evenly).

Arduino Duemilanove

  • 115200 OK
  • 230400 OK
  • 250000 Failed
  • 500000 Errors
  • 1000000 Errors

Arduino UNO (rev 1)

  • 115200 OK
  • 230400 OK
  • 250000 Failed
  • 500000 OK
  • 1000000 OK

It would be interesting to know what limits the Duemilanove’s performance. Wheter it’s in the software or the FTDI interface? The FTDI-chip itself should be capable of even faster speeds than 1Mbit/sec. Go figure.

Distance measurement

Fixed the Processing sketch that attempts to measure distances based on acceleration. (It was off by a factor of 10. Guess who didn’t convert from g to m/s^2.) Anyhow, instead of compensating for tilting in code, I built a rail:

sled_1

Of course, this doesn’t mean I might not have to take the orientation into account at some point. Still, with the rail I can roughly measure the moved distance as the change in acceleration happens almost exclusively on the x axis.

sled_2

Drifting is quite bad, though. I’ve got a good 15cm of travel on the rail and measuring movement that’s done right after the measurement starts (within a couple of seconds) seems accurate within +/- 1 cm. But when measuring a completely stationary sensor over a time of 30 seconds, the measurement may have wandered off even beyond 40cm. Currently, I’m fresh out of ideas on how to counter this.

I figured that building a data glove that can track the hands position in 3D space may be quite difficult measuring the accelration of the glove alone. Because the MPU-6050 is able to produce orientation data (as a quaternion), one could of course track the hands position from the shoulder on onward. The shoulder would be a “fixed point” with a ball joint. Having sensors on the upper and fore arm measuring the direction where the arm is pointing to (and knowing one’s arm’s length), one could calculate the location of the hand in relation to the shoulder. It’s not ideal for motion capturing either since the shoulder is capeable of subtle movements, but I think it might be the way to go.

MPU-6050 /w DMP accelerometer demo

After connecting the MPU-6050 again to an Arduino, initializing the DMP and realizing that the accelrometer data is in *signed* 16-bit integers, I’ve got the following:

  • Looks like the default setup is for the 4 g range. That’s very nice.
  • The x- and y-axis are fairly precise, but in my case, the z-axis shows slightly less than 0.9 g, when it should be 1.0 g. I wonder if it varies from chip to chip..

I converted the previous demo for the analog accelerometer for the MPU-6050. You can download the thing here. (There are 2 Processing sketches: one showing the 16-bit measurements and one showing those measurements converted to g forces.)

Also: I wrote a Processing sketch that tries to measure a traveled distance based on the accelerometer. It just uses the s = 0.5 a * t^2 + v*t formula. Currently the results are way off. But this is partly, because rotation isn’t taken into account. Tilting the sensor one way causes it to “accelerate” into two directions (because I compare the acceleration to a pre-measured offset). So, there is plenty room for improvement. I uploaded it here. (Use the same sketch for Arduino as in the aforementioned “mpuAccelerometerDemo”.)