Add magHorizlDirection

asukiaaa · Aug 10, 2017 · 084b512 · 084b512
1 parent 4a20065
commit 084b512
Show file tree

Hide file tree

Showing 3 changed files with 9 additions and 0 deletions.
diff --git a/examples/GetData/GetData.ino b/examples/GetData/GetData.ino
@@ -44,6 +44,7 @@ void loop() {
   Serial.println("magX: " + String(mySensor.magX()));
   Serial.println("maxY: " + String(mySensor.magY()));
   Serial.println("magZ: " + String(mySensor.magZ()));
+  Serial.println("horizontal direction: " + String(mySensor.magHorizDirection()));
 
   Serial.println("at " + String(millis()) + "ms");
   delay(500);

diff --git a/src/MPU9250_asukiaaa.cpp b/src/MPU9250_asukiaaa.cpp
@@ -57,6 +57,12 @@ void MPU9250::magSetMode(uint8_t mode) {
   delay(10);
 }
 
+const float Pi = 3.14159;
+
+float MPU9250::magHorizDirection() {
+  return atan2((float) magX(), (float) magY()) * 180 / Pi;
+}
+
 void MPU9250::magUpdate() {
   I2Cread(MAG_ADDRESS, AK8963_RA_HXL, 7, magBuf);
 }

diff --git a/src/MPU9250_asukiaaa.h b/src/MPU9250_asukiaaa.h
@@ -2,6 +2,7 @@
 #define MPU9250_ASUKIAAA
 #include <Arduino.h>
 #include <Wire.h>
+#include <math.h>
 
 #define MPU9250_ADDRESS_AD0_LOW  0x68
 #define MPU9250_ADDRESS_AD0_HIGH 0x69
@@ -53,6 +54,7 @@ class MPU9250 {
   int16_t magY();
   int16_t magZ();
   void magSetMode(uint8_t mode);
+  float magHorizDirection();
 
   private:
   TwoWire* myWire;