Arduino I2Cdev & MPU-6050 code conversion to ChibiOS

[Jevermeister]

Hi,

for my master thesis i ported today the popular Arduino I2Cdev C++ library and corresponding MPU-6050 code to ChibiOS.
Available at https://github.com/jevermeister/MPU6050-ChibiOS - MIT/X11 license.
Please consider it highly experimental as i only tested a few functions.
I'll test as much as possible and try to implement the DMP functions within the next updates.

Feel free to use it and report bugs.
Regards,

Jan

[Jevermeister]

Just finished my first attempt to convert the corresponding 6-axis DMP MotionApps code (MPU6050_6Axis_MotionApps20.h).
Will upload it in the evening and do further tests tomorrow, the DMP initialisation seems to work.

Serial output example:

Code: Select all

STARTING...

Resetting MPU6050...
Disabling sleep mode...
Selecting user bank 16...
Selecting memory byte 6...
Checking hardware revision...Revision @ user[16][6] = 4D
Resetting memory bank selection to 0...
Reading OTP bank valid flag...OTP bank is valid!
Reading gyro offset values...X gyro offset = 62Y gyro offset = 58Z gyro offset = 63
Setting slave 0 address to 0x7F...
Disabling I2C Master mode...
Setting slave 0 address to 0x68 (self)...
Resetting I2C Master control...Writing DMP code to MPU memory banks (1929
 bytes)
Success! DMP code written and verified.Writing DMP configuration to MPU memory banks (192
 bytes in config def)
Success! DMP configuration written and verified.
Setting clock source to Z Gyro...
Setting DMP and FIFO_OFLOW interrupts enabled...
Setting sample rate to 200Hz...
Setting external frame sync to TEMP_OUT_L[0]...
Setting DLPF bandwidth to 42Hz...
Setting gyro sensitivity to +/- 2000 deg/sec...
Setting DMP configuration bytes (function unknown)...
Clearing OTP Bank flag...
Setting X/Y/Z gyro offsets to previous values...
Setting X/Y/Z gyro user offsets to zero...
Writing final memory update 1/7 (function unknown)...
Writing final memory update 2/7 (function unknown)...
Resetting FIFO...
Reading FIFO count...Current FIFO count=0
Setting motion detection threshold to 2...
Setting zero-motion detection threshold to 156...
Setting motion detection duration to 80...
Setting zero-motion detection duration to 0...
Resetting FIFO...
Enabling FIFO...
Enabling DMP...
Resetting DMP...
Writing final memory update 3/7 (function unknown)...
Writing final memory update 4/7 (function unknown)...
Writing final memory update 5/7 (function unknown)...
Waiting for FIFO count > 2...Current FIFO count=42
Reading FIFO data...
Reading interrupt status...Current interrupt status=7
Reading final memory update 6/7 (function unknown)...
Waiting for FIFO count > 2...Current FIFO count=126
Reading FIFO data...
Reading interrupt status...Current interrupt status=3
Writing final memory update 7/7 (function unknown)...
DMP is good to go! Finally.
Disabling DMP (you turn it on later)...
Setting up internal 42-byte (default) DMP packet buffer...
Resetting FIFO and clearing INT status one last time...
AX: 1254 AY: 16320 AZ: -3846 GX: -63 GY: -24 GZ: -6
AX: 1330 AY: 16300 AZ: -3856 GX: -63 GY: -25 GZ: -3
AX: 1306 AY: 16292 AZ: -3980 GX: -63 GY: -24 GZ: -4
...

[Jevermeister]

Code is up. Will convert the quaternion code and corresponding functions tomorrow.

[thomaskausm]

Hello Jevermeister,

I was able to build you example but wasn't able to get it running ... I think my problem is the I2C bus or better the configuration of it.

That's my I2C configuration code:

Code: Select all

#define I2C_MPU          I2CD1

static const I2CConfig i2cfg1 = { OPMODE_I2C, 400000,  FAST_DUTY_CYCLE_2 };
..
..

   palSetPadMode(GPIOB, 8, PAL_MODE_ALTERNATE(4) | PAL_STM32_OTYPE_OPENDRAIN );   /* SCL */
   palSetPadMode(GPIOB, 9, PAL_MODE_ALTERNATE(4) | PAL_STM32_OTYPE_OPENDRAIN );   /* SDA */

   i2cStart(&I2C_MPU, &i2cfg1);

   MPUdmpInitialize();

The result I got is:

Resetting MPU6050...I2C ERROR: 4

Disabling sleep mode...I2C ERROR: 4

Selecting user bank 16...
Selecting memory byte 6...
Checking hardware revision...I2C ERROR: 4
Revision @ user[16][6] = 0
Resetting memory bank selection to 0...
Reading OTP bank valid flag...I2C ERROR: 4
OTP bank is invalid!
Reading gyro offset values...I2C ERROR: 4
I2C ERROR: 4
I2C ERROR: 4
X gyro offset = 0Y gyro offset = 0Z gyro offset = 0
Setting slave 0 address to 0x7F...
Disabling I2C Master mode...I2C ERROR: 4

Setting slave 0 address to 0x68 (self)...
Resetting I2C Master control...I2C ERROR: 4
Writing DMP code to MPU memory banks (1929
bytes)I2C ERROR: 4

ERROR! DMP code verification failed.2, 2, 49


I am not the expert nor in I2C either in ChibiOS (just begin to play a little bit around with my STM32F4 , Eclipse, MPU6050 and Chibios) but I would expect that I could see the 400 khz with my oszi at pin 8 on the GPIOB. But there is no signal.

It's absolutly not clear to me how the GPIO pins gets connected to the I2C subsystem. Also after digging into the sources it is not clear to me.
Could you please post your I2C configuration ?

Regards
Thomas

[Jevermeister]

Hi,

my I2C configuration struct is

/* I2C interface #1 */
static const I2CConfig i2cfg1 = {
OPMODE_I2C,
400000,
FAST_DUTY_CYCLE_2,
};

and

/* I2C interface #1 */
i2cStart(&I2CD1, &i2cfg1);
palSetPadMode(GPIOB, I2C1_SDA, PAL_MODE_ALTERNATE(4) | PAL_STM32_OTYPE_OPENDRAIN);
palSetPadMode(GPIOB, I2C1_SDL, PAL_MODE_ALTERNATE(4) | PAL_STM32_OTYPE_OPENDRAIN);

/* Disable I2C audio pins
* !!! IMPORTANT OTHERWISE I2C WON'T WORK!!!
*/
palSetPadMode(GPIOB, 6, PAL_MODE_ALTERNATE(0));
palSetPadMode(GPIOB, 9, PAL_MODE_ALTERNATE(0));

It is important that you disable the audio pins, otherwise I2C #1 didn't work for me. You can set pins to a specific mode which is listed in the STM32F4 manual in the section "alternate function mapping". An excellent overview can be found at http://kornakprotoblog.blogspot.de/2012 ... m32f4.html

Regards

[thomaskausm]

Hello Jan,

thanks for the information. Unfortunately I have no time to test it right now but I think I will have time at the weekend.
The information of the m32f4.html answered my questions regarding the pin configuration .

Again thanks and regards
Thomas

[plain]

Hi Jevermeister,
I am trying to using your code for my project. I use STM32F103RC. Now I can compile it successfully but when after I download it to the chip, when i try to init MPU6050 chip

Code: Select all

   MPU6050(0x68);
   chThdSleepMilliseconds(100);
   MPUinitialize();

It reports some error

Code: Select all

I2C ERROR: 4
I2C ERROR: 4
I2C ERROR: 4
I2C ERROR: 4

Both STM32F103 and ChibiOS are very new to me, so I don't know how to fix that error. Can you please give me some suggest?

[adrianalin]

I could compile your code as well, and i could get raw data from MPU6050 using getRawAccelGyro. Here is my code:

Code: Select all

bool_t init_i2c2(void)
{
   println("Init I2C");

   // I2C interface #2
   i2cStart(&I2CD2, &i2cfg2);
   palSetPadMode(GPIOB, 10, PAL_MODE_ALTERNATE(4) | PAL_STM32_OTYPE_OPENDRAIN);   // I2C2_SCL PB10; connect VIO to 3.3V !!!
   palSetPadMode(GPIOB, 11, PAL_MODE_ALTERNATE(4) | PAL_STM32_OTYPE_OPENDRAIN);   // I2C2_SDA PB11

   // init accelerometer
   MPU6050(0x69);
   chThdSleepMilliseconds(100);
   MPUinitialize();

   // verify connection
   if(MPUtestConnection() == true) {
      println("MPUtestConnection (i2c) ok");
      accelGyroOK = true;
   } else {
      println("MPUtestConnection (i2c) not ok");
      accelGyroOK = false;
   }

   MPUdmpInitialize();

   return accelGyroOK;
}

And as you see i tried to initialiez the DMP calling MPUdmpInitialize(); and it stops:

Code: Select all

..........
Waiting for FIFO count > 2...
Current FIFO count=72
Reading FIFO data... ERROR readBytes: length > I2CDEV BUFFERLENGTH
Reading interrupt status... Current interrupt status=17
Reading final memory update 6/7 (function unknown)...
Waiting for FIFO count > 2...

I also tried to set I2CDEV_BUFFERLENGTH to 128, but no success