Diff: BNO055.cpp

Revision:

1:1c4507621cfb

Parent:

0:fe0b400ec8f5

diff -r fe0b400ec8f5 -r 1c4507621cfb BNO055.cpp
--- a/BNO055.cpp	Wed Oct 30 08:11:25 2019 +0000
+++ b/BNO055.cpp	Tue Feb 23 11:18:12 2021 +0000
@@ -11,6 +11,38 @@
     temp_scale = 1;
     }
     
+    
+BNO055::BNO055(PinName SDA, PinName SCL, double *p_yaw) : _i2c(SDA, SCL), yaw(p_yaw)
+{
+    // Set I2C fast and bring reset line high
+    _i2c.frequency(400000);
+    address = BNOAddress;
+    accel_scale = 0.001f;
+    rate_scale = 1.0f / 16.0f;
+    angle_scale = 1.0f / 16.0f;
+    temp_scale = 1;
+}
+
+void BNO055::get_yaw()
+{
+
+    static double rawyaw[2]= {0};
+
+    setmode(OPERATION_MODE_IMUPLUS);
+    get_angles();                   
+
+    rawyaw[1] = rawyaw[0];
+    rawyaw[0] = euler.yaw;
+
+    if (rawyaw[0] != 0) {  //不慮の再起動対策
+        if (rawyaw[1] < 90 && rawyaw[0] > 270)
+            rawyaw[1] += 360;
+        else if (rawyaw[1] > 270 && rawyaw[0] < 90)
+            rawyaw[1] -= 360;
+        *yaw -= rawyaw[0] - rawyaw[1];
+    }
+}
+    
 void BNO055::reset(){
 //Perform a power-on-reset
     readchar(BNO055_SYS_TRIGGER_ADDR);