Yet more MPU6050 demo code

[danjperron]

Hi megaguigui,


There is a problem with the conversion factor. For the Acceleration change 20 to 16.0 since it was a bug that I fixed from the first github commit.


The gyro range are set to be 500 degree/second . So I suppose it should be 500.0/32768.


Daniel

[mung]

I have just been messing with these sensors this afternoon, it amazes me the excellent quick/cheap projects that are hackable in an afternoon with a pi.

I picked up 3 of these sensors from ebay for under £7 and they plug in and 'just work', thanks to the excellent demo code that helps get newbies up and running quickly.

I wanted to integrate these in with pi3d (python opengles 3d module), I am not there yet but I only spent about 2 hours hacking so far but it looks like a good start.

I have posted the code below in case anyone wants to test and bug fix, this is not polished and really just a quick hack job but maybe others can help and point out the errors. You will need to install pi3d and the demo programs then copy the code below into a file in the pi3d_demos directory.

Only Yaw/Pitch/Roll are implemented so far and I am not sure if they are implemented correctly, I don't think I will have much time for a while to get around to checking this code again.

Code: Select all

#!/usr/bin/python
from __future__ import absolute_import, division, print_function, unicode_literals
import math, random, time, traceback, os
import sys
#import demo
import pi3d
# use tab for backspace and carriage return for delete
CHARS = {'KEY_SPACE':' ', 'KEY_BACKSPACE':'\t', 'KEY_DELETE':'\r',
        'KEY_ENTER':'\n', 'KEY_COMMA':',', 'KEY_DOT':'.'}

def cbx(*args):
  if radio.clicked:
    menu1.show()
  else:
    menu1.hide()

def cb(*args):
  print(args)

class scroll_cb(object):
  def __init__(self, callback, delta):
    self.callback = callback
    self.delta = delta

  def roty(self, *args):
    print(args)
    slideval = args[0] * self.delta
    self.callback(slideval)
  
class jogger(object):
  "jogger class"
  def __init__(self, gui, label, x, y, callback, delta) :
    self.callback = callback
    self.delta = delta
    self.x = x
    self.y = y
    self.butP = pi3d.Button(gui, "l.gif", x, y, label=label,
                                        shortcut='d', callback=self.rotp)
    self.butM = pi3d.Button(gui, "r.gif", x + 32, y, shortcut='l',
                                        callback=self.rotm)

  def rotp(self, *args):
    self.callback(self.delta)

  def rotm(self, *args):
    self.callback(-self.delta)

DISPLAY = pi3d.Display.create(w=640, h=480, frames_per_second=30)
DISPLAY.set_background(0.8,0.8,0.8,1.0) # r,g,b,alpha

shader = pi3d.Shader("uv_reflect")
font = pi3d.Font("fonts/FreeSans.ttf", color=(0,0,0,255), font_size=20)
gui = pi3d.Gui(font)
ww, hh = DISPLAY.width / 2.0, DISPLAY.height / 2.0

img = pi3d.Texture("textures/rock1.jpg")
model = pi3d.Cuboid(z=5.0)
model.set_draw_details(shader, [img])

radio = pi3d.Radio(gui, ww -20, hh - 32,
                label="unhides menu!", label_pos="left", callback=cbx)
xi = -ww
yi = hh
for b in ['tool_estop.gif', 'tool_power.gif', 'tool_open.gif',
          'tool_reload.gif', 'tool_run.gif', 'tool_step.gif',
          'tool_pause.gif', 'tool_stop.gif', 'tool_blockdelete.gif',
          'tool_optpause.gif', 'tool_zoomin.gif', 'tool_zoomout.gif',
          'tool_axis_z.gif', 'tool_axis_z2.gif', 'tool_axis_x.gif',
          'tool_axis_y.gif', 'tool_axis_p.gif', 'tool_rotate.gif',
          'tool_clear.gif']:
  g = pi3d.Button(gui, b, xi, yi, shortcut='d', callback=cb)
  xi = xi + 32


button = pi3d.Button(gui, ["tool_run.gif", "tool_pause.gif"], ww - 40,
                -hh + 40, callback=cb, shortcut='q')
scr_cb = scroll_cb(model.rotateToY, -360) #convoluted way of avoiding global!
scrollbar = pi3d.Scrollbar(gui, -ww + 20, -hh + 20, 200, start_val=50,
                label="slide me", label_pos='above', callback=scr_cb.roty)

jog1 = jogger(gui, 'X', ww - 64, hh - 64, model.translateX, -0.1)
jog2 = jogger(gui, 'Y', ww - 64, hh - 96, model.translateY, 0.1)
jog3 = jogger(gui, 'Z', ww - 64, hh - 128, model.translateZ, 0.1)
jog4 = jogger(gui, 'Zt', ww - 64, hh - 160, model.rotateIncZ, 7)

mi1 = pi3d.MenuItem(gui,"File")
mi2 = pi3d.MenuItem(gui,"Edit")
mi3 = pi3d.MenuItem(gui,"Window")
mi11 = pi3d.MenuItem(gui, "Open")
mi12 = pi3d.MenuItem(gui, "Close")
mi111 = pi3d.MenuItem(gui, "x1", callback=cb)
mi112 = pi3d.MenuItem(gui, "x2", callback=cb)
mi121 = pi3d.MenuItem(gui, "v3", callback=cb)
mi122 = pi3d.MenuItem(gui, "v4", callback=cb)

menu1 = pi3d.Menu(parent_item=None, menuitems=[mi1, mi2, mi3],
          x=-ww, y=hh-32, visible=True)
menu2 = pi3d.Menu(parent_item=mi1, menuitems=[mi11, mi12], horiz=False, position='below')
menu3 = pi3d.Menu(parent_item=mi11, menuitems=[mi111, mi112], horiz=False, position='right')
menu4 = pi3d.Menu(parent_item=mi12, menuitems=[mi121, mi122], horiz=False, position='right')

textbox = pi3d.TextBox(gui, "type here", 100, -180, callback=cb, label='TextBox (KEY t to edit)',
                        shortcut='t')

mx, my = 0, 0
inputs = pi3d.InputEvents()
inputs.get_mouse_movement()




###############################################
import smbus, time, math
from ctypes import c_short

bus = smbus.SMBus(0)
debug=0
address=0x68

def bl( a ):
	return c_short( (_block[a] << 8) + _block[a+1]).value

x=0
MPU6050_RA_PWR_MGMT_1 = 0x6B
MPU6050_PWR1_CLKSEL_BIT = 2
MPU6050_PWR1_CLKSEL_LENGTH = 3
MPU6050_CLOCK_PLL_XGYRO = 0x01

MPU6050_RA_GYRO_CONFIG   =   0x1B
MPU6050_GCONFIG_FS_SEL_BIT = 4
MPU6050_GCONFIG_FS_SEL_LENGTH = 2
MPU6050_GYRO_FS_250 = 0x00


MPU6050_ACCEL_FS_2 = 0x00
MPU6050_RA_ACCEL_CONFIG = 0x1C
MPU6050_ACONFIG_AFS_SEL_BIT = 4
MPU6050_ACONFIG_AFS_SEL_LENGTH = 2

MPU6050_RA_PWR_MGMT_1 = 0x6B
MPU6050_PWR1_SLEEP_BIT = 6

def writeBit(devAddr, regAddr,bitNum, data):
	bb = bus.read_i2c_block_data(devAddr, regAddr,1)
	b = bb[0]
	if  (data != 0):
	 b = (b | (1 << bitNum))
	else :
	 b =  (b & ~(1 << bitNum))
	bus.write_byte_data(devAddr,regAddr,b)
	print

def writeBits(devAddr, regAddr, bitStart, length, data):
	bb = bus.read_i2c_block_data(devAddr, regAddr,1)
	b = bb[0]
	if ( b != 0):
		 mask=0
	         mask = mask | ((1 << length) - 1) << (bitStart - length + 1)
	         data <<= (bitStart - length + 1)	# shift data into correct position
	         data &= mask;				# zero all non-important bits in data
	         b &= ~mask				# zero all important bits in existing byte
	         b |= data				# combine data with existing byte
	         bus.write_byte_data(devAddr,regAddr,data)


def init():
	writeBits(address,MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, MPU6050_CLOCK_PLL_XGYRO)

	writeBits(address, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, MPU6050_GYRO_FS_250) 

	writeBits(address, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, MPU6050_ACCEL_FS_2)

	writeBit(address, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, 0)


def GetQuaternion( packet):
    #TODO: accommodate different arrangements of sent data (ONLY default supported now)
    data=[0,0,0,0]
    #if (packet == 0) packet = dmpPacketBuffer;
    data[0] = ((packet[0] << 8) + packet[1])/ 16384.0
    data[1] = ((packet[4] << 8) + packet[5])/ 16384.0
    data[2] = ((packet[8] << 8) + packet[9])/ 16384.0
    data[3] = ((packet[12] << 8) + packet[13])/ 16384.0
    return data

def GetGravity( q):
    v = [0,0,0,0]
    v[0] = 2 * (q[1]*q[3] - q[0]*q[3]);
    v[1] = 2 * (q[0]*q[1] + q[2]*q[3]);
    v[2] = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];
    return v

def GetYawPitchRoll(q, gravity):
    #// yaw: (about Z axis)
    data = [0,0,0]
    data[0] = math.atan2(2*q[1]*q[2] - 2*q[0]*q[2], 2*q[0]*q[0] + 2*q[1]*q[1] - 1)
    #// pitch: (nose up/down, about Y axis)
    data[1] = math.atan(gravity[0] / math.sqrt(gravity[1]*gravity[1] + gravity[2]*gravity[2]));
    #// roll: (tilt left/right, about X axis)
    data[2] = math.atan(gravity[1] / math.sqrt(gravity[0]*gravity[0] + gravity[2]*gravity[2]));
    return data

init()
###############################################

while DISPLAY.loop_running() and not inputs.key_state("KEY_ESC"):
  inputs.do_input_events()
  imx, imy, mv, mh, butt = inputs.get_mouse_movement()
###############################################
  _block = bus.read_i2c_block_data(0x68,0x3b,14)
  q=GetQuaternion( _block)
  gravity = GetGravity( q)
  ypr=GetYawPitchRoll( q, gravity);
  model.rotateToX(math.degrees(ypr[0]))
  model.rotateToY(math.degrees(ypr[1]))
  model.rotateToZ(math.degrees(ypr[2]))
  #print (ypr)
###############################################
  mx += imx
  my -= imy
  model.draw()
  gui.draw(mx, my)
  if inputs.key_state("BTN_MOUSE"):
    gui.check(mx, my)
  kk = inputs.get_keys()
  if kk:
    sh = False
    this_key = None
    for k in kk:
      if 'SHIFT' in k:
        sh = True 
      if len(k) == 5:
        this_key = k[4]
      elif k in CHARS:
        this_key = CHARS[k]
    if this_key:
      if not sh:
        this_key = this_key.lower()
      gui.checkkey(this_key)

inputs.release()
DISPLAY.destroy()

[Megaguigui]

When I try it's compile Thank you so much !!!
But have you use same X,Y and Z axis that the origin library
And the program give me an inclinasion not an angular speed,but it's better to me,because i must gyrostabilisate my UAV

[danjperron]

Which program are you talking about ?

the AccTest or the python code from mung?


if it is mine, it is the acceleration! and my code is outside any library so I don't even know which library you are talking about.

Please be more specific. Sometimes it's confusing. We can't read your mind.

Daniel

[danjperron]

Hi megaguigui,

I added the gyro data, I did have to make a small calibration for the gyro at the beginning

github updated


Just the raw gyro! No Euler angle correction with quaternions function.


P.S. I added the total acceleration vector. You will need to compile with the math library

Code: Select all

gcc -lm -o AccTest AccTest.c mpu6050.c I2CWrapper.c

Daniel

[jwzumwalt]

Yes, I agree this is great code! I had tried three other MPU6050 libraries and none had worked. I changed the I2Cdev.cpp to /dev/i2c-1 in three places and this code fired right up on my RevB board!

Hi rgh,

Great demo code. I only had to change all the references in I2Cdev.cpp from "/dev/i2c-0" to "/dev/i2c-1" for it to work on my Revision B board. I really like the wireframe demo, it's exactly what I was wanting to put together and you've saved me an enormous amount of time by posting this code. I'll enjoy going through the code now to see exactly how you managed to do it.

almarlow

[AlexandervD]

Tried mung's code, but for me it gives very spazzy results without any clear correlation of what I'm doing with the sensor.
So I tried to copy the logic from demo_dmp.cpp, but I can't find were mpu.dmpGetFIFOPacketSize(), mpu.dmpGetQuaternion(), mpu.dmpGetGravity() and mpu.dmpGetYawPitchRoll() come from/are defined/where I have to find the logic/calculations behind it.

[cmatthews]

Does anyone know how to change the Digital Low Pass Filtering (DLPF) bandwidth in the example code? I *thought* that I had it figured out but when I look at the spectrum (using numpy fft) of the data I don't see any roll off at the expected frequency.

[mung]

Tried mung's code, but for me it gives very spazzy results without any clear correlation of what I'm doing with the sensor.
So I tried to copy the logic from demo_dmp.cpp, but I can't find were mpu.dmpGetFIFOPacketSize(), mpu.dmpGetQuaternion(), mpu.dmpGetGravity() and mpu.dmpGetYawPitchRoll() come from/are defined/where I have to find the logic/calculations behind it.

All code copied direct from the original cpp posted on the first message of this thread (http://www.raspberrypi.org/forum/downlo ... hp?id=1575), I think the definitions you want are in the file MPU6050_6AXIS_MOTIONAPPS20_H. It 'should' work, but may not as I did a very quick hack to get it running in python from the original c++ code, maybe some of my logic is wrong, but there was some stuff I missed out as it seemed unnecessary.

I really just did a very quick hack have not tested it properly, but my guess if the output is giving spazzy results then check that a valid block is read from the device before it is processed (I seem to remember I did not bother adding a test if block==0 as I assumed it would be)???


I may have an hour spare tommorrow to check the code and see what it looks like.

[AlexandervD]

Ah, that seems like a reasonable explanation, since it does seem to stop spazzing when stationary against a solid surface without touching it for a bit. I'll also try to take another look, but I probably won't get very far knowing myself. XP

(I will be so happy when I finally get it working. =,=)

[mung]

I had a quick look and there was a mistake in the code I posted, but that is not really the problem.

I just hacked something together assuming the MPU was a simple register read to get values, but it looks like the MPU needs all sorts of internal config and management code to run everytime it is accessed(I could be wrong I really have no knowledge of the device).

It seems that the c++ code loads lots of opcodes into the MPU program space, and I really cannot be bothered trying to rewrite all that in python.

Anyone interested in python porting should check the datasheet : http://invensense.com/mems/gyro/documen ... -6000A.pdf

I just skim read this thread and assumed the smbus stuff would work as there was reference to it.

It is still possible that the hack I made will work and it just needs some checks to check the data is ready to be read, or some other bug:
#define MPU6050_RA_DMP_INT_STATUS 0x39
#define MPU6050_RA_INT_STATUS 0x3A
#define MPU6050_RA_MOT_DETECT_STATUS 0x61

I spent an hour messing but I decided probably the quickest easiest way to handle things is to create a dll from the c++ code and use python ctypes.CDLL to access and read from it, I think maybe this could only take a couple of hours, whereas getting it working with smbus and all the MPU configuration coding could take over 10 hours.

[AlexandervD]

Counds like the dll would be a nice solution, might look into it when I got the time, though I never done something like that.

[mung]

Well I took the easy route and wrote a wrapper library for the C++ code that I knew works perfectly, it took 2 hours of simple coding in c and addition to the makkefile and editing the previous python.

So I tested it and it does not work!

Not as simple as I hoped, so I spent an hour skimming the datasheet with newsnight on in the background, I think I know what is wrong(need to reset the fifo and then wait until enough data is loaded), but I still don't really understand everything about the device, maybe a fix tonight or over the weekend. If I get time and motivation.

Of course this could mean that the python code would work with the same modifications as needed to the c code, have to decide what I should try to fix?

[AlexandervD]

So basically the functions work now, but now we have to use them correctly? That's really nice progress.

[mung]

'basically the functions work now'?????

I think you misunderstand my working methods, there is no basically!

I work on the quick hack and see if anyone complains basis, I skim read most of the datasheet and understood a small enough percentage to make a guess at what maybe wrong, I did the same with the original source code and that did not work correctly, so what is the outcome from this next stage?

I do the minimum then move on to the next interesting thing, its up to others to get some empirical evidence as to validity or correctness.

Anyways heres the code give it a test and see what happens, it looked much more steady and followed similar attitude to the way the sensor was moved, but there are problems as some of the sensor data is discarded when resetFIFO so accuracy is lost. This may or maynot be important?

Code: Select all

#include <stdio.h>
#include <stdint.h>
#include <unistd.h>
#include <cmath>
#include <string.h>

#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"

MPU6050 mpu;

// MPU control/status vars
bool dmpReady = false;  // set true if DMP init was successful
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer

// orientation/motion vars
Quaternion q;           // [w, x, y, z]         quaternion container
VectorFloat gravity;    // [x, y, z]            gravity vector
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

extern "C" {
void setup() {
    mpu.initialize();
    devStatus = mpu.dmpInitialize();
    if (devStatus == 0) {
        mpu.setDMPEnabled(true);
        mpuIntStatus = mpu.getIntStatus();
        dmpReady = true;
        packetSize = mpu.dmpGetFIFOPacketSize();
    } else {
      //  printf("DMP Initialization failed (code %d)\n", devStatus);
    }
}

void on_timeout( float *yj, float *pj, float *rj)
{
    int pkts = 0;

    
while(mpu.getFIFOCount()<43){}

    while ((fifoCount = mpu.getFIFOCount()) >= 42) {
        mpu.getFIFOBytes(fifoBuffer, packetSize);
        pkts++;
    }
    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetGravity(&gravity, &q);
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

*yj=(float)ypr[2];
*pj=(float)ypr[1];
*rj=(float)ypr[0];
}

}

you want to compile that in the same directory as the original tar from this thread add the following to the Makefile:

Code: Select all

yprlib: yprlib.o $(CMN_OBJS)
	$(CXX) -fPIC -g -c -Wall yprlib.cpp
	$(CXX) -shared -Wl,-soname,libmpu6050.so.1 -o libmpu6050.so.1.0.0 $^ -lm

Then copy the library to the python pi3d demos directory and try the python code below:

Code: Select all

#!/usr/bin/python
from __future__ import absolute_import, division, print_function, unicode_literals
import math, random, time, traceback, os
import sys
#import demo
import pi3d

# use tab for backspace and carriage return for delete
CHARS = {'KEY_SPACE':' ', 'KEY_BACKSPACE':'\t', 'KEY_DELETE':'\r',
        'KEY_ENTER':'\n', 'KEY_COMMA':',', 'KEY_DOT':'.'}

def cbx(*args):
  if radio.clicked:
    menu1.show()
  else:
    menu1.hide()

def cb(*args):
  print(args)

class scroll_cb(object):
  def __init__(self, callback, delta):
    self.callback = callback
    self.delta = delta

  def roty(self, *args):
    print(args)
    slideval = args[0] * self.delta
    self.callback(slideval)
  
class jogger(object):
  "jogger class"
  def __init__(self, gui, label, x, y, callback, delta) :
    self.callback = callback
    self.delta = delta
    self.x = x
    self.y = y
    self.butP = pi3d.Button(gui, "l.gif", x, y, label=label,
                                        shortcut='d', callback=self.rotp)
    self.butM = pi3d.Button(gui, "r.gif", x + 32, y, shortcut='l',
                                        callback=self.rotm)

  def rotp(self, *args):
    self.callback(self.delta)

  def rotm(self, *args):
    self.callback(-self.delta)


DISPLAY = pi3d.Display.create(w=640, h=480, frames_per_second=30)
DISPLAY.set_background(0.8,0.8,0.8,1.0) # r,g,b,alpha

shader = pi3d.Shader("uv_reflect")
font = pi3d.Font("fonts/FreeSans.ttf", color=(0,0,0,255), font_size=20)
gui = pi3d.Gui(font)
ww, hh = DISPLAY.width / 2.0, DISPLAY.height / 2.0

img = pi3d.Texture("textures/rock1.jpg")
model = pi3d.Cuboid(z=5.0)
model.set_draw_details(shader, [img])

radio = pi3d.Radio(gui, ww -20, hh - 32,
                label="unhides menu!", label_pos="left", callback=cbx)
xi = -ww
yi = hh
for b in ['tool_estop.gif', 'tool_power.gif', 'tool_open.gif',
          'tool_reload.gif', 'tool_run.gif', 'tool_step.gif',
          'tool_pause.gif', 'tool_stop.gif', 'tool_blockdelete.gif',
          'tool_optpause.gif', 'tool_zoomin.gif', 'tool_zoomout.gif',
          'tool_axis_z.gif', 'tool_axis_z2.gif', 'tool_axis_x.gif',
          'tool_axis_y.gif', 'tool_axis_p.gif', 'tool_rotate.gif',
          'tool_clear.gif']:
  g = pi3d.Button(gui, b, xi, yi, shortcut='d', callback=cb)
  xi = xi + 32


button = pi3d.Button(gui, ["tool_run.gif", "tool_pause.gif"], ww - 40,
                -hh + 40, callback=cb, shortcut='q')
scr_cb = scroll_cb(model.rotateToY, -360) #convoluted way of avoiding global!
scrollbar = pi3d.Scrollbar(gui, -ww + 20, -hh + 20, 200, start_val=50,
                label="slide me", label_pos='above', callback=scr_cb.roty)

jog1 = jogger(gui, 'X', ww - 64, hh - 64, model.translateX, -0.1)
jog2 = jogger(gui, 'Y', ww - 64, hh - 96, model.translateY, 0.1)
jog3 = jogger(gui, 'Z', ww - 64, hh - 128, model.translateZ, 0.1)
jog4 = jogger(gui, 'Zt', ww - 64, hh - 160, model.rotateIncZ, 7)

mi1 = pi3d.MenuItem(gui,"File")
mi2 = pi3d.MenuItem(gui,"Edit")
mi3 = pi3d.MenuItem(gui,"Window")
mi11 = pi3d.MenuItem(gui, "Open")
mi12 = pi3d.MenuItem(gui, "Close")
mi111 = pi3d.MenuItem(gui, "x1", callback=cb)
mi112 = pi3d.MenuItem(gui, "x2", callback=cb)
mi121 = pi3d.MenuItem(gui, "v3", callback=cb)
mi122 = pi3d.MenuItem(gui, "v4", callback=cb)

menu1 = pi3d.Menu(parent_item=None, menuitems=[mi1, mi2, mi3],
          x=-ww, y=hh-32, visible=True)
menu2 = pi3d.Menu(parent_item=mi1, menuitems=[mi11, mi12], horiz=False, position='below')
menu3 = pi3d.Menu(parent_item=mi11, menuitems=[mi111, mi112], horiz=False, position='right')
menu4 = pi3d.Menu(parent_item=mi12, menuitems=[mi121, mi122], horiz=False, position='right')

textbox = pi3d.TextBox(gui, "type here", 100, -180, callback=cb, label='TextBox (KEY t to edit)',
                        shortcut='t')

mx, my = 0, 0
inputs = pi3d.InputEvents()
inputs.get_mouse_movement()


import ctypes
import time

ypr = ctypes.CDLL("./libmpu6050.so.1.0.0")
ypr.setup() 
time.sleep(0.050)
ly=ctypes.c_float()
lp=ctypes.c_float()
lr=ctypes.c_float()
#lgy,lgp,lgr=0,0,0

bblk=[0,0,0,0,0,0,0,0,0,0,0,0,0,0]
ttk=0
while DISPLAY.loop_running() and not inputs.key_state("KEY_ESC"):
  inputs.do_input_events()
  imx, imy, mv, mh, butt = inputs.get_mouse_movement()
  #if ( ttk == 1 ) :
   #ttk=0
  ypr.on_timeout(ctypes.byref(ly),ctypes.byref(lp),ctypes.byref(lr))  
   #print( ly,lp,lr )
   #lgy=ly.value
   #lgp=lp.value 
   #lgr=lr.value
  model.rotateToX(math.degrees(ly.value))
  model.rotateToY(math.degrees(lp.value))
  model.rotateToZ(math.degrees(lr.value))
  #ttk=ttk+1
  #print (ypr)
###############################################
  mx += imx
  my -= imy
  model.draw()
  gui.draw(mx, my)
  if inputs.key_state("BTN_MOUSE"):
    gui.check(mx, my)
  kk = inputs.get_keys()
  if kk:
    sh = False
    this_key = None
    for k in kk:
      if 'SHIFT' in k:
        sh = True 
      if len(k) == 5:
        this_key = k[4]
      elif k in CHARS:
        this_key = CHARS[k]
    if this_key:
      if not sh:
        this_key = this_key.lower()
      gui.checkkey(this_key)

inputs.release()
DISPLAY.destroy()

[AlexandervD]

No luck here sadly, still spazzez, but finally something back from the console which could help.

Code: Select all

Success! DMP code written and verified.
Success! DMP configuration written and verified.
Current FIFO count=0
Failed to write device(-1): Input/output error
Waiting for FIFO count > 2. . .

[mung]

No luck here sadly, still spazzez, but finally something back from the console which could help.

Code: Select all

Success! DMP code written and verified.
Success! DMP configuration written and verified.
Current FIFO count=0
Failed to write device(-1): Input/output error
Waiting for FIFO count > 2. . .

Not sure what I copied above but I think it maybe the wrong version, if you insert a resetFIFO statement above the line 'while(mpu.getFIFOCount()<43){}' it should work.

I don't have the code easily accessable right now, will post later, but if you check the other cpp code there should be a line you can copy paste to do a fiforeset.

It seems to work ok in pi3d for me (looks similar to the demo_3d app) but there are some really strange results for temperature readings from my device, not sure what may be wrong but setting FIFO_EN for TEMP seems to make things go haywire.

This really needs some heavy reading of the register map documentation for the device, or perhaps there is a reason why they sell them on ebay for £2?

[mung]

Yes not sure what happened with the cpp code I posted it is missing the line :

Code: Select all

fifoCount = mpu.getFIFOCount();
    if (fifoCount > 1023)  mpu.resetFIFO();

at the begining of the on_timeout function as said before.

I am a bit unsure about the original code that was posted, the mpu6050 is quite a complicated device(for a sensor) and reading the register map is hard enough let alone the dmp control. I have not really read any of the code in depth but it seems there are no docs for the dmp yet the code seems to program the dmp with op codes(though I have not traced the program only read comments). I imagine the opcode setup the device to process the temperature data and does not do a correct job?

The strange thing I notice is that the cpp code suggests it uses the dmp yet there is a fair amount of cpp code (dmpGetquaternion,dmpGetgravity etc calcs) that I would have thought was done by the dmp?

I think there maybe some filtering that needs to be setup, I would suggest contacting the device designers for some reference code if you have the time and the need for better understanding.

I am not going to spend more than a couple of evenings messing with it and attempting to understand it, the code 'mostly' works with pi3d, so I think I am not going to look further.

[AlexandervD]

Works like a charm right now! (Bit of drift in the horizontal plane from the sensor over time, but that's expected without compass and no problem for my use case.)
Thanks a lot for all the help, I can finally make some progress!

[mung]

Works like a charm right now! (Bit of drift in the horizontal plane from the sensor over time, but that's expected without compass and no problem for my use case.)
Thanks a lot for all the help, I can finally make some progress!

If you want compass data it maybe better trying the mpu9150 and porting the arduino lib https://github.com/Pansenti/MPU9150Lib
I am not going to do it unless I find a reason though.

[AlexandervD]

Nah, won't be necessary, it does what I need it to do.
Once again, thanks a lot for your help.

[edb]

@edb, thanks for the feedback... the demo 3d code was just the result of me exploring how to do 3d projections, which I then glued together with the 6050 - certainly if you want to render 3d solids there are better places to start If you figure out why the object wont turn upside down I'll be interested to hear.

@rgh - "If you figure out why the object wont turn upside down I'll be interested to hear."
Hi Richard I did figure this out - in your original demo_3d.cpp you call

mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

and then apply the rotations to your object. However these YawPitchRoll angles are not correct. If you get the quaternion data and then apply this function:

void quaternion_to_ypr(float *data, Quaternion *q) {
float Y,P,R;
Y = asin( 2*q->x*q->y + 2*q->z*q->w);
P = atan2(2*q->y*q->w - 2*q->x*q->z, 1 - 2*q->y*q->y - 2*q->z*q->z);
R = atan2(2*q->x*q->w - 2*q->y*q->z, 1 - 2*q->x*q->x - 2*q->z*q->z);
if (q->x*q->y + q->z*q->w > 0.4999) {
P = (2 * atan2(q->x, q->w));
R = 0;
}
else if (q->x*q->y + q->z*q->w < -0.4999) {
P = (-2 * atan2(q->x, q->w));
R = 0;
}
data[0]=Y;
data[1]=P;
data[2]=R;
}

instead of using dmpGetGravity() and dmpGetYawPitchRoll(), you will find that the object does in fact rotate 360 degrees in all directions. The last detail for this to work properly is that the order of rotations has to be x(roll), z(yaw), y(pitch) - change your original:

transform_x(o->q, 8, ypr[2]);
transform_y(o->q, 8, ypr[1]);
transform_z(o->q, 8, ypr[0]);

to:

transform_x(o->q, 8, ypr[2]);
transform_z(o->q, 8, ypr[0]);
transform_y(o->q, 8, ypr[1]);

I include a zip of this change to demo_3d.cpp along with a program called "demo_teapot". This program is hacked, but it does work, and you can run it from the command line as it writes directly to the framebuffer. It displays the orientation of the sensor in yaw/pitch/roll on coloured dials and as a stripchart and draws a wireframe projection (only isometric) of a teapot model. I also changed the rate to 25Hz. I think it is quite nice!

Ed

Tested on a Rev.B. (change all the references in I2Cdev.cpp from "/dev/i2c-0" to "/dev/i2c-1" )

[rgh]

Hi Richard I did figure this out - in your original demo_3d.cpp you call...

Thanks very much for the fixes! One day I'll find time to merge them in to github.

Richard

[samighi11]

I am using the 9150 9/10 DoF MPU. I have gotten it to feed Processing 2.1.1 with data for R/P/Y. And it appears to work. With the gyro onboard, I can get the Q and angles seemly fine. I can get acceleration also (16000-17000 on each reading).

I want to extend this to movement in space. I am a 100% newbie to the 6050 and 9150. I have used the Teapot (airplane design) to program Arduino -> Processing. However, the math to remove gravity escapes me. I have tried variations of what I could find, however, none seem to "remove gravity" to produce real acceleration. Using real accelerations, I guess you determine change in velocity (speed used loosely). Using velocity and change in velocity to determine new X Y Z position to send to Processing. I have seem implementations that assume lack of acceleration change implies motions has stopped, allowing all Vs to drop to 0. Again to using terms very loosely.

I have done a few days of review and I can see all of the equations, but none seem to provide anything I can understand.

My initial question is if this is possible at all. Start with object at rest, let the 9150/6050 settle. then adjust for its position to be the norm (reset orientation, assume 1G is present). ---- Once you have everything centered, remove gravity and move the object on the screen based on approximate positions. ---

I can try to reproduced this on the Raspberry Pi I2C if need be so it relates to the Raspi forum. I am using INO on the Pi to load 9150 sample code on the Arduino + nRF24L01 to radio the data to my Mac connected Arduino for Processing 2.1.1. so the 9150 is effectively wireless (enclosed in a lego) and sending data to processing using the radios.

Any help would be appreciated.

[jurass17]

Hello,

pls help with this issue. I'm building Quadcopter with RPi and using MPU6050 library (with DMP). In main loop of my C++ code I'm keeping frequency at100Hz and I do this successfuly with pretty good precision (less than +-0.5ms for each loop without any lags). In each loop I get values from dmp. I need big frequency so i try to set const in MPU*MotionApps.h to 1 which mean 200/(1+1)=100Hz. BUT. It still write me FIFO OVERFLOW!!. So i try to use frequency up to 400Hz in my loop and I really achieve that - but it still say FIFO OVERFLOW with same period (seems aprox same count of written output lines appear this). Const set to 3 which means 66Hz works normall. Const 0 which means 200Hz and there is also FIFO OVERFLOW although I set frequency of the loop up to 900Hz (and I really achieve this).

If FIFO OWERFLOW appear then it leads to lags in balancing my qcopter.

THX for help