This is a library of MPU-9250 9 axis sensor.
Hi,I'm Wataru Nakata. I am a student of Nagaoka National College of Technology ,JAPAN . I am a member of robotics club. So I'm using mbeds for robot controlling.
MPU-9250.cpp@2:2bc5dd0ab0f0, 2015-02-27 (annotated)
- Committer:
- WAT34
- Date:
- Fri Feb 27 00:15:00 2015 +0000
- Revision:
- 2:2bc5dd0ab0f0
- Parent:
- 1:c7a984ee3409
ver1.3;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
WAT34 | 0:f29582d9e1fd | 1 | #include "MPU-9250.h" |
WAT34 | 0:f29582d9e1fd | 2 | |
WAT34 | 2:2bc5dd0ab0f0 | 3 | MPU9250::MPU9250 (PinName sda, PinName scl) : i2c(sda, scl) |
WAT34 | 0:f29582d9e1fd | 4 | { |
WAT34 | 0:f29582d9e1fd | 5 | } |
WAT34 | 0:f29582d9e1fd | 6 | |
WAT34 | 2:2bc5dd0ab0f0 | 7 | int MPU9250::start() |
WAT34 | 0:f29582d9e1fd | 8 | { |
WAT34 | 1:c7a984ee3409 | 9 | i2c.frequency(400000); |
WAT34 | 0:f29582d9e1fd | 10 | bdf[0] = 0x75; |
WAT34 | 0:f29582d9e1fd | 11 | i2c.write(addr,bdf,1); |
WAT34 | 0:f29582d9e1fd | 12 | i2c.read(raddr,bdf,1); |
WAT34 | 0:f29582d9e1fd | 13 | if (bdf[0]== 0x71) |
WAT34 | 0:f29582d9e1fd | 14 | { |
WAT34 | 0:f29582d9e1fd | 15 | bdf[0] = 0x6b; |
WAT34 | 0:f29582d9e1fd | 16 | bdf[1] = 0x00; |
WAT34 | 0:f29582d9e1fd | 17 | i2c.write(addr,bdf,2); |
WAT34 | 0:f29582d9e1fd | 18 | bdf[0] = 0x37; |
WAT34 | 0:f29582d9e1fd | 19 | bdf[1] = 0x02; |
WAT34 | 0:f29582d9e1fd | 20 | i2c.write(addr,bdf,2); |
WAT34 | 0:f29582d9e1fd | 21 | return 0; |
WAT34 | 0:f29582d9e1fd | 22 | }else{ |
WAT34 | 0:f29582d9e1fd | 23 | return 1; |
WAT34 | 0:f29582d9e1fd | 24 | } |
WAT34 | 0:f29582d9e1fd | 25 | } |
WAT34 | 0:f29582d9e1fd | 26 | |
WAT34 | 2:2bc5dd0ab0f0 | 27 | void MPU9250::read_data(int a,double *x,double *y,double *z) |
WAT34 | 0:f29582d9e1fd | 28 | { |
WAT34 | 0:f29582d9e1fd | 29 | char data[13] = {0}; |
WAT34 | 0:f29582d9e1fd | 30 | if (a == 0) //引数aが一のときx,y,zの地磁気の強さを返すよ。(単位はuT) If a = 1 This program will return each axis's magnetometer value.(Unit is uT) |
WAT34 | 0:f29582d9e1fd | 31 | { |
WAT34 | 0:f29582d9e1fd | 32 | bdf[0] = 0x0A; |
WAT34 | 0:f29582d9e1fd | 33 | bdf[1] = 0x12; |
WAT34 | 0:f29582d9e1fd | 34 | i2c.write(maddr,bdf,2); |
WAT34 | 0:f29582d9e1fd | 35 | bdf[0] = 0x03; |
WAT34 | 0:f29582d9e1fd | 36 | i2c.write(maddr,bdf,1); |
WAT34 | 0:f29582d9e1fd | 37 | i2c.read(mraddr,data,6); |
WAT34 | 0:f29582d9e1fd | 38 | *x = (short(data[0]+data[1]<<8)*0.15); |
WAT34 | 0:f29582d9e1fd | 39 | *y = (short(data[2]+data[3]<<8)*0.15); |
WAT34 | 0:f29582d9e1fd | 40 | *z = (short(data[4]+data[5]<<8)*0.15); |
WAT34 | 0:f29582d9e1fd | 41 | }else if(a == 1){ |
WAT34 | 0:f29582d9e1fd | 42 | bdf[0] = 0x0A; |
WAT34 | 0:f29582d9e1fd | 43 | bdf[1] = 0x12; |
WAT34 | 0:f29582d9e1fd | 44 | i2c.write(maddr,bdf,2); |
WAT34 | 0:f29582d9e1fd | 45 | bdf[0] = 0x03; |
WAT34 | 0:f29582d9e1fd | 46 | i2c.write(maddr,bdf,1); |
WAT34 | 0:f29582d9e1fd | 47 | i2c.read(mraddr,data,6); |
WAT34 | 0:f29582d9e1fd | 48 | *x = (short(data[0]+data[1]<<8)*0.15); |
WAT34 | 0:f29582d9e1fd | 49 | *y = (short(data[2]+data[3]<<8)*0.15); |
WAT34 | 0:f29582d9e1fd | 50 | *z = (short(data[4]+data[5]<<8)*0.15); |
WAT34 | 0:f29582d9e1fd | 51 | *x = atan2(*x,*y); |
WAT34 | 0:f29582d9e1fd | 52 | if(*x>=0) |
WAT34 | 0:f29582d9e1fd | 53 | { |
WAT34 | 0:f29582d9e1fd | 54 | *x = *x/pi*180; |
WAT34 | 0:f29582d9e1fd | 55 | }else{ |
WAT34 | 0:f29582d9e1fd | 56 | *x = (pi+*x)/pi*180+180; |
WAT34 | 0:f29582d9e1fd | 57 | } |
WAT34 | 0:f29582d9e1fd | 58 | }else{ |
WAT34 | 0:f29582d9e1fd | 59 | bdf[0] = 0x3b; |
WAT34 | 0:f29582d9e1fd | 60 | i2c.write(addr,bdf,1); |
WAT34 | 0:f29582d9e1fd | 61 | i2c.read(raddr,data,14); |
WAT34 | 0:f29582d9e1fd | 62 | if (a == 2) |
WAT34 | 0:f29582d9e1fd | 63 | { |
WAT34 | 0:f29582d9e1fd | 64 | *x = (short(data[0]<<8+data[1])*0.061); |
WAT34 | 0:f29582d9e1fd | 65 | *y = (short(data[2]<<8+data[3])*0.061); |
WAT34 | 0:f29582d9e1fd | 66 | *z = (short(data[4]<<8+data[5])*0.061); |
WAT34 | 0:f29582d9e1fd | 67 | }else if(a == 3){ |
WAT34 | 0:f29582d9e1fd | 68 | *x = (short(data[8]<<8+data[9])*0.00763); |
WAT34 | 0:f29582d9e1fd | 69 | *y = (short(data[10]<<8+data[11])*0.00763); |
WAT34 | 0:f29582d9e1fd | 70 | *z = (short(data[12]<<8+data[13])*0.00763); |
WAT34 | 0:f29582d9e1fd | 71 | } |
WAT34 | 0:f29582d9e1fd | 72 | } |
WAT34 | 0:f29582d9e1fd | 73 | } |
WAT34 | 0:f29582d9e1fd | 74 |