Add functions about magnetometer

README.md

@@ -44,6 +44,28 @@ void loop() {
 }
 ```
 
+## Gyrometer
+```c
+```c
+#include <MPU9250_asukiaaa.h>
+MPU9250 mySensor;
+float gX, gY, gZ;
+
+void setup() {
+  Wire.begin();
+  mySensor.setWire(&Wire);
+  mySensor.beginGyro();
+}
+
+void loop() {
+  mySensor.gyroUpdate();
+  gX = mySensor.gyroX();
+  gY = mySensor.gyroY();
+  gZ = mySensor.gyroZ();
+  // Do what you want
+}
+```
+
 ## Magnetometer
 ```c
 #include <MPU9250_asukiaaa.h>
@@ -118,3 +140,5 @@ MIT
 - [jrowberg/i2cdevlib/Arduino/AK8963](https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/AK8963)
 - [SparkFun_MPU-9250](https://github.com/sparkfun/SparkFun_MPU-9250_Breakout_Arduino_Library/blob/master/src/MPU9250.cpp)
 - [ArduinoでMPU9250（加速度センサ、磁気センサ）を使う方法](http://asukiaaa.blogspot.jp/2017/07/arduinompu9250.html)
+- [「MPU-9250 ９軸センサモジュール (メーカー品番：MPU-9250)」を使う](https://qiita.com/boyaki_machine/items/915f7730c737f2a5cc79)
+- [【PSoC】MPU-9250のデータを取得（加速度、ジャイロ編）](http://amamitokachi.com/2017/04/12/post-71/)

examples/GetData/GetData.ino

@@ -7,7 +7,7 @@
 
 MPU9250 mySensor;
 
-float aX, aY, aZ, aSqrt, mDirection;
+float aX, aY, aZ, aSqrt, gX, gY, gZ, mDirection;
 int16_t mX, mY, mZ;
 
 void setup() {
@@ -26,6 +26,7 @@ void setup() {
   mySensor.setWire(&Wire);
 
   mySensor.beginAccel();
+  mySensor.beginGyro();
   mySensor.beginMag();
 
   // you can set your own offset for mag values
@@ -40,23 +41,30 @@ void loop() {
   aY = mySensor.accelY();
   aZ = mySensor.accelZ();
   aSqrt = mySensor.accelSqrt();
-  Serial.println("print accel values");
   Serial.println("accelX: " + String(aX));
   Serial.println("accelY: " + String(aY));
   Serial.println("accelZ: " + String(aZ));
   Serial.println("accelSqrt: " + String(aSqrt));
 
+  mySensor.gyroUpdate();
+  gX = mySensor.gyroX();
+  gY = mySensor.gyroY();
+  gZ = mySensor.gyroZ();
+  Serial.println("gyroX: " + String(gX));
+  Serial.println("gyroY: " + String(gY));
+  Serial.println("gyroZ: " + String(gZ));
+
   mySensor.magUpdate();
   mX = mySensor.magX();
   mY = mySensor.magY();
   mZ = mySensor.magZ();
   mDirection = mySensor.magHorizDirection();
-  Serial.println("print mag values");
   Serial.println("magX: " + String(mX));
   Serial.println("maxY: " + String(mY));
   Serial.println("magZ: " + String(mZ));
   Serial.println("horizontal direction: " + String(mDirection));
 
   Serial.println("at " + String(millis()) + "ms");
+  Serial.println(""); // Add an empty line
   delay(500);
 }

library.properties

@@ -2,8 +2,8 @@ name=MPU9250_asukiaaa
 version=1.2.3
 author=Asuki Kono
 maintainer=Asuki Kono
-sentence=A library to control MPU9250
-paragraph=It can get sensor value of accelerometer and magnetometer.
+sentence=It controls MPU9250
+paragraph=It can get sensor value of accelerometer, gyrometer and magnetometer.
 category=Sensors
 url=https://github.com/asukiaaa/MPU9250_asukiaaa
 architectures=*

src/MPU9250_asukiaaa.cpp

@@ -23,9 +23,6 @@ void MPU9250::setWire(TwoWire* wire) {
 }
 
 void MPU9250::beginAccel() {
-  delay(40);
-
-  i2cWriteByte(address, 27, GYRO_FULL_SCALE_2000_DPS);
   i2cWriteByte(address, 28, ACC_FULL_SCALE_16_G);
   delay(10);
 }
@@ -41,15 +38,14 @@ void MPU9250::magReadAdjustValues() {
 }
 
 void MPU9250::beginMag(uint8_t mode) {
-  delay(10);
-
   // trun on magnetometor
   i2cWriteByte(address, 0x37, 0x02);
   delay(10);
 
   magReadAdjustValues();
   magSetMode(AK8963_MODE_POWERDOWN);
   magSetMode(mode);
+  delay(10);
 }
 
 void MPU9250::magSetMode(uint8_t mode) {
@@ -93,7 +89,7 @@ void MPU9250::accelUpdate() {
 
 float MPU9250::accelGet(uint8_t highIndex, uint8_t lowIndex) {
   int16_t v = -(accelBuf[highIndex]<<8 | accelBuf[lowIndex]);
-  return ((float)v) * 16.0/32768.0;
+  return ((float)v) * 16.0 / (float) 0x8000; // (float)0x8000 == 32768.0
 }
 
 float MPU9250::accelX() {
@@ -113,3 +109,47 @@ float MPU9250::accelSqrt() {
               pow(accelGet(2, 3), 2) +
               pow(accelGet(4, 5), 2));
 }
+
+
+void MPU9250::beginGyro(uint8_t mode) {
+  i2cWriteByte(address, 27, mode);
+  switch (mode) {
+  case GYRO_FULL_SCALE_250_DPS:
+    gyroRange = 250.0;
+    break;
+  case GYRO_FULL_SCALE_500_DPS:
+    gyroRange = 500.0;
+    break;
+  case GYRO_FULL_SCALE_1000_DPS:
+    gyroRange = 1000.0;
+    break;
+  case GYRO_FULL_SCALE_2000_DPS:
+    gyroRange = 2000.0;
+  default:
+    gyroRange = 0;
+  }
+  delay(10);
+}
+
+void MPU9250::gyroUpdate() {
+  Serial.print("gyroRange");
+  Serial.println(gyroRange);
+  i2cRead(address, 0x43, 6, gyroBuf);
+}
+
+float MPU9250::gyroGet(uint8_t highIndex, uint8_t lowIndex) {
+  int16_t v = -(gyroBuf[highIndex]<<8 | gyroBuf[lowIndex]);
+  return ((float)v) * gyroRange / (float)0x8000;
+}
+
+float MPU9250::gyroX() {
+  return gyroGet(0, 1);
+}
+
+float MPU9250::gyroY() {
+  return gyroGet(2, 3);
+}
+
+float MPU9250::gyroZ() {
+  return gyroGet(4, 5);
+}

src/MPU9250_asukiaaa.h

@@ -48,6 +48,12 @@ class MPU9250 {
   float accelZ();
   float accelSqrt();
 
+  void beginGyro(uint8_t mode = GYRO_FULL_SCALE_2000_DPS);
+  void gyroUpdate();
+  float gyroX();
+  float gyroY();
+  float gyroZ();
+
   void beginMag(uint8_t mode = AK8963_MODE_CONTINUOUS_8HZ);
   void magUpdate();
   int16_t magX();
@@ -60,9 +66,12 @@ class MPU9250 {
   TwoWire* myWire;
   uint8_t address;
   uint8_t accelBuf[14];
+  uint8_t gyroBuf[6];
+  float gyroRange;
   uint8_t magBuf[7];
   uint8_t magXAdjust, magYAdjust, magZAdjust;
   float accelGet(uint8_t highIndex, uint8_t lowIndex);
+  float gyroGet(uint8_t highIndex, uint8_t lowIndex);
   int16_t magGet(uint8_t highIndex, uint8_t lowIndex);
   void magReadAdjustValues();
   void i2cRead(uint8_t Address, uint8_t Register, uint8_t Nbytes, uint8_t* Data);