Diff: JY901.cpp

Revision:

5:a492cfb18242

Parent:

4:cd62e2d69f62

--- a/JY901.cpp	Thu Aug 23 05:33:30 2018 +0000
+++ b/JY901.cpp	Wed Nov 14 02:14:58 2018 +0000
@@ -4,6 +4,40 @@
 {
     _i2c = i2c;
 }
+JY901::JY901(I2C *i2c, Timer *timer)
+{
+    _i2c = i2c;
+    _timer = timer;
+    angleZ = 0;
+    for(int i = 0; i < 3; i++)  gyroZ[i] = 0;
+    _timer->start();
+    last_time = 0;
+    time = _timer->read();
+}
+
+float JY901::calculateAngleOnlyGyro()
+{
+    last_time = time;
+    time = _timer->read();
+    for(int i = 2; i > 0; i--)
+    {
+        gyroZ[i] = gyroZ[i-1];
+    }
+    gyroZ[0] = getZaxisAngularVelocity();
+    dt = time - last_time;
+    if( gyroZ[0] + gyroZ[2] != 0 )
+        angleZ += gyroZ[1] * dt;
+    return angleZ;
+}
+
+void JY901::reset()
+{
+    time = _timer->read();
+    for(int i = 0; i > 2; i++)
+    {
+        gyroZ[i] = 0;
+    }
+}
 
 void JY901::calibrateGyroAccel()
 {
@@ -129,7 +163,8 @@
 float JY901::getZaxisAngularVelocity()
 {
     char *data = getdata(GZ);
-    return (float)((*(data+1) << 8 ) | *data) / 32768 * 2000;
+    //return (float)((*(data+1) << 8 ) | *data) / 32768 * 2000;
+    return s16(*data, *(data+1)) / 32768 * 2000;
 }
 
 float JY901::getXaxisMagnetic()
@@ -253,3 +288,9 @@
     _i2c->stop();
     return data;
 }
+
+float JY901::s16(int dataL, int dataH)
+{
+    int value = (dataH << 8) | dataL;
+    return -(value & 0b1000000000000000) | (value & 0b0111111111111111);
+}
\ No newline at end of file