Teensy Prop Shield Increasing IMU Data Rate

lopenguin

New member
Hello,

I'm working on a project with the Teensy Prop Shield (which is awesome by the way) and I'm running into trouble with increasing the IMU sensors output rate from 100 Hz to 200 Hz. I think the problem is related to the fact that there are two sensors on the I2C bus that I'm trying to read near-simultaneously. Let me explain:

First, the datasheets for the FXOS8700CQ and FXAS21002 have a table in their CTRL_REG_01 section that explains what binary values correspond to what data rates. I changed the gyro from 0x0E to 0x0D and the accel/mag from 0x15 to 0x0D.

Now, when I try to read each one individually, I get a data rate of just over 5 ms. However, when I read both together (the same way the library normally does it--literally the only change I've made is those hex values) the data rate collapses to every 10 ms. Clearly there is something about I2C I do not understand, but I am struggling to find resources online. Any help would be much appreciated!

For reference, I've attached the version of the NXPMotionSense library I made the changes in. Literally two lines of code are changed, but whatever.

PS: I've also tried an iterative version, where it reads one sensor repeatedly until it gets a value, then it reads the other sensor. This gets my data rate down to 5.4 ms, but it doesn't scale if, for example, I add another I2C device. This makes me thing I'm doing something wrong with I2C in general. :confused:

View attachment NXPMotionSense.zip
 
Hi Lopenguin,

Welcome to the forum. If you could post your top-level code
Code:
int wrappedInTagsLikeThis;

that would help. Look for the hash symbol at the top to get the tags. I presume you're using one of the examples in that library, but it would be easier if you posted so people don't have to download and figure out which one.
 
Makes sense. Here are the highlights:

First, the changes to the library:
Code:
bool NXPMotionSense::FXOS8700_begin()
{
	const uint8_t i2c_addr=FXOS8700_I2C_ADDR0;
	uint8_t b;

	//Serial.println("FXOS8700_begin");
	// detect if chip is present
	if (!read_regs(i2c_addr, FXOS8700_WHO_AM_I, &b, 1)) return false;
	//Serial.printf("FXOS8700 ID = %02X\n", b);
	if (b != 0xC7) return false;
	// place into standby mode
	if (!write_reg(i2c_addr, FXOS8700_CTRL_REG1, 0)) return false;
	// configure magnetometer
	if (!write_reg(i2c_addr, FXOS8700_M_CTRL_REG1, 0x1F)) return false;
	if (!write_reg(i2c_addr, FXOS8700_M_CTRL_REG2, 0x20)) return false;
	// configure accelerometer
	if (!write_reg(i2c_addr, FXOS8700_XYZ_DATA_CFG, 0x01)) return false; // 4G range
	if (!write_reg(i2c_addr, FXOS8700_CTRL_REG2, 0x02)) return false; // hires
	if (!write_reg(i2c_addr, FXOS8700_CTRL_REG1, 0x0D)) return false; // 100Hz A+M (changed from 0x15 to 0x0D to increase to 200  Hz)
	//Serial.println("FXOS8700 Configured");
	return true;
}

Very simple: I changed the CTRL_REG1 from 0x15 to 0x0D, which does in fact double the data rate of the sensor. I did something similar to the
Code:
NXPMotionSense::FXAS21002_begin()
function.

The top-level code is just the main example for this library: "Orientation.ino". Biggest issue is (I think) that I just don't understand how I2C timing works well enough. In leiu of an answer, links to good resources would be much appreciated (Google is of surprisingly little help).

Thanks!
 
Code:
filter.begin(100);
tells the filter function that you're update is 100Hz, so maybe you need to change this.

https://www.circuitbasics.com/basics-of-the-i2c-communication-protocol/
is a sensible intro to i2c.

inside the NXPMotionSense.cpp
Code:
Wire.setClock(400000);
sets the I2C clock to 400k, which is plenty fast enough to run the two devices simultaneously. If you're finding they work individually with 5ms update rates, but not together, I'd suspect it's a code issue. It's also worth noting the
Code:
Serial.begin(9600) ;
is actually a pretty slow serial, and not fast enough to print your data. If you consider that you're sending a bunch of data every 5ms, if your total message length is greater than 9600/200 = 48 characters or bytes, then the serial won't be able to keep up.
 
Code:
Serial.begin(9600) ;

is actually a pretty slow serial, and not fast enough to print your data. If you consider that you're sending a bunch of data every 5ms, if your total message length is greater than 9600/200 = 48 characters or bytes, then the serial won't be able to keep up.
The "9600" has NO EFFECT upon USB Serial speed.
 
Thanks for the reply. I agree that it is likely something more subtle than the Wire speed, and I've tried changing the filter rate (the filter class is separate from the IMU class, so it can even be completely omitted). I'll check out that resource you sent. and see what I can do.

Thanks!
 
You might what to look at this implementation of the propsheild IMU: https://github.com/kriswiner/Teensy_Prop_Shield to give you ideas.

Edit:
If you look in the file NXPMotionSense.h file you will see where the sample rate is set. If you want a different rate you can change it here. Of course make sure you configure your sensors for the correct sampleRates.

Code:
class NXPSensorFusion {
public:
	void begin(float sampleRate=100.0f);
 
Wow, thanks so much mjs513! That implementation really has all the details that go with setting up the data rate. I will sift through it today but I suspect it has all the answers I'm looking for.
 
I was noticing the same thing about the speed staying constant, despite doubling the rates of the sensors. I finally noticed that in lines 131 and 181 of the NXPMotionSense.cpp code, there is a timer function that counts counts down in usec. If you halve both these constants 5000 and 10000 to 2500 and 5000, you can double the speed.
 
I probably should have posted the code changes to make it easier, in case someone else runs into the same issue.

In the library NXPMotionSense.cpp:
Change the CTRL_REG1 values as shown below:

Set FXOS8700 (accelerometer and magnetometer) rate to 200Hz:

Code:
	//if (!write_reg(i2c_addr, FXOS8700_CTRL_REG1, 0x15)) return false; // 100Hz A+M
	if (!write_reg(i2c_addr, FXOS8700_CTRL_REG1, 0x0D)) return false; // 200Hz A+M

To set FXAS21002 (gyro) rate to 200Hz use a value of 0x0A (DON'T use 0x0D as mentioned in the first post of this thread, as that will keep it at 100Hz, but change it to a low power mode Ready state)

Code:
	//if (!write_reg(i2c_addr, FXAS21002_CTRL_REG1, 0x0E)) return false;
	if (!write_reg(i2c_addr, FXAS21002_CTRL_REG1, 0x0A)) return false;


Below is where it uses elapsedMicros as a timeout:
Halve the usec_history values in these two places:

Code:
bool NXPMotionSense::FXOS8700_read(int16_t *data)  // accel + mag
{
	static elapsedMicros usec_since;
	static int32_t usec_history= 2500; //was 5000
	const uint8_t i2c_addr=FXOS8700_I2C_ADDR0;
	uint8_t buf[13];

	int32_t usec = usec_since;
	if (usec + 100 < usec_history) return false;

	if (!read_regs(i2c_addr, FXOS8700_STATUS, buf, 1)) return false;
	if (buf[0] == 0) return false;

	usec_since -= usec;
	int diff = (usec - usec_history) >> 3;
	if (diff < -15) diff = -15;
	else if (diff > 15) diff = 15;
	usec_history += diff;

Code:
bool NXPMotionSense::FXAS21002_read(int16_t *data) // gyro
{
	static elapsedMicros usec_since;
	static int32_t usec_history= 5000;  //was 10000
	const uint8_t i2c_addr=FXAS21002_I2C_ADDR0;
	uint8_t buf[7];

	int32_t usec = usec_since;
	if (usec + 100 < usec_history) return false;

	if (!read_regs(i2c_addr, FXAS21002_STATUS, buf, 1)) return false;
	if (buf[0] == 0) return false;

	usec_since -= usec;
	int diff = (usec - usec_history) >> 3;
	if (diff < -15) diff = -15;
	else if (diff > 15) diff = 15;
	usec_history += diff;
	//Serial.println(usec);

and in your Orientation.ino code, change the filter rate to 200Hz:

Code:
  filter.begin(200);
 
Last edited:
Back
Top