Diff: library/GY521.cpp

Revision:

1:17051435cfc5

Parent:

0:a8a56075e947

diff -r a8a56075e947 -r 17051435cfc5 library/GY521.cpp
--- a/library/GY521.cpp	Sat May 18 12:09:43 2019 +0000
+++ b/library/GY521.cpp	Tue Jul 09 07:56:07 2019 +0000
@@ -1,28 +1,44 @@
 #include "GY521.hpp"
 
-GY521::GY521(I2C &i2c,int bit,int calibration,double user_reg):i2c(i2c),calibration(calibration){
-    dev_id = 0x68 << 1;
+const double GY521_LSB_MAP[4] = {131, 65.5, 32.8, 16.4};
+const unsigned int dev_id = 0x68 <<  1;
+enum GY521RegisterMap {
+  WHO_AM_I = 0x75,
+  PWR_MGMT_1 = 0x6B,
+  LPF = 0x1A,
+  FS_SEL = 0x1B,
+  AFS_SEL = 0x1C,
+  ACCEL_XOUT_H = 0x3B,
+  ACCEL_YOUT_H = 0x3D,
+  ACCEL_ZOUT_H = 0x3F,
+  //TEMPERATURE  = 0x41,
+  //GYRO_XOUT_H = 0x43,
+  //GYRO_YOUT_H = 0x45,
+  GYRO_ZOUT_H = 0x47
+};
+
+GY521::GY521(I2C &i2c,int bit,int calibration,double user_reg):i2c_(i2c),bit_(bit),calibration_(calibration){
     char check;
     char data[2] = {WHO_AM_I,0x00};
-    i2c.write(dev_id,data,1,true);
-    i2c.read(dev_id,&check,1);
+    i2c_.write(dev_id,data,1,true);
+    i2c_.read(dev_id,&check,1);
     if(check == dev_id >> 1){
         data[0] = PWR_MGMT_1;
-        i2c.write(dev_id,data,1,true);
-        i2c.read(dev_id,&check,1);
+        i2c_.write(dev_id,data,1,true);
+        i2c_.read(dev_id,&check,1);
         if(check == 0x40){
-            i2c.write(dev_id,data,2);
+            i2c_.write(dev_id,data,2);
         }
     }
     data[0] = LPF;
     data[1] = 0x00;
-    i2c.write(dev_id,data,2);
+    i2c_.write(dev_id,data,2);
     data[0] = AFS_SEL;
     data[1] = 0x00;
-    i2c.write(dev_id,data,2);
+    i2c_.write(dev_id,data,2);
     short accel_x_now = 0,accel_y_now = 0,accel_z_now = 0;
     double accel_x_aver = 0, accel_y_aver = 0, accel_z_aver = 0;
-    for (int i = 0; i < calibration; ++i) {
+    for (int i = 0; i < calibration_; ++i) {
         accel_x_now = gyroRead2(ACCEL_XOUT_H);
         accel_y_now = gyroRead2(ACCEL_YOUT_H);
         accel_z_now = gyroRead2(ACCEL_ZOUT_H);
@@ -30,37 +46,38 @@
         accel_y_aver += accel_y_now;
         accel_z_aver += accel_z_now;
     }
-    accel_x_aver /= calibration;
-    accel_y_aver /= calibration;
-    accel_z_aver /= calibration;
+    accel_x_aver /= calibration_;
+    accel_y_aver /= calibration_;
+    accel_z_aver /= calibration_;
     double gyro_reg = hypot(hypot(accel_x_aver, accel_y_aver), accel_z_aver) / accel_z_aver;
     data[0] = FS_SEL;
     data[1] = bit << 3;
-    i2c.write(dev_id,data,2);
-    bit_ = bit;
-    gyro_LSB = GY521_LSB_MAP[bit_] / gyro_reg / user_reg;
+    i2c_.write(dev_id,data,2);
+    gyro_LSB_ = GY521_LSB_MAP[bit_] / gyro_reg / user_reg;
     
-    short gyro_z_now;
-    gyro_z_aver = 0;
-    for (int i = 0; i < calibration; ++i) {
-        gyro_z_now = gyroRead2(GYRO_ZOUT_H);
-        gyro_z_aver += gyro_z_now;
+    short gyro_z_now_;
+    gyro_z_aver_ = 0;
+    for (int i = 0; i < calibration_; ++i) {
+        gyro_z_now_ = gyroRead2(GYRO_ZOUT_H);
+        gyro_z_aver_ += gyro_z_now_;
     }
-    gyro_z_aver /= calibration;
+    gyro_z_aver_ /= calibration_;
 
-    yaw = diffyaw = 0;
-    time.start();
-    flag = false;
+    yaw = diff_yaw_ = 0;
+    timer_.start();
+    flag_ = false;
 }
 
 void GY521::updata(){
-    if(flag)return;
+    if(flag_){
+        return;
+    }
     short gyro_z_now_ = gyroRead2(GYRO_ZOUT_H);
-    gyro_z_now = ((double)gyro_z_now_ - gyro_z_aver) / gyro_LSB;
-    diffyaw = (gyro_z_now + gyro_z_prev) / 2 * time.read();
-    time.reset();
-    yaw += diffyaw;
-    gyro_z_prev = gyro_z_now;
+    gyro_z_now_ = ((double)gyro_z_now_ - gyro_z_aver_) / gyro_LSB_;
+    diff_yaw_ = (gyro_z_now_ + gyro_z_prev_) / 2 * timer_.read();
+    timer_.reset();
+    yaw += diff_yaw_;
+    gyro_z_prev_ = gyro_z_now_;
     //temp = (double)gyroRead2(TEMPERATURE)/340+36.53;
     if (yaw > 180) {
         yaw -= 360;
@@ -71,24 +88,24 @@
 
 void GY521::reset(int user){
     yaw = user;
-    short gyro_z_now;
-    flag = true;
-    gyro_z_aver = 0;
-    for (int i = 0; i < calibration; i++) {
-        gyro_z_now = gyroRead2(GYRO_ZOUT_H);
-        gyro_z_aver += gyro_z_now;
+    short gyro_z_now_;
+    flag_ = true;
+    gyro_z_aver_ = 0;
+    for (int i = 0; i < calibration_; i++) {
+        gyro_z_now_ = gyroRead2(GYRO_ZOUT_H);
+        gyro_z_aver_ += gyro_z_now_;
     }
-    gyro_z_aver /= calibration;
-    flag = false;
+    gyro_z_aver_ /= calibration_;
+    flag_ = false;
 }
     
 double GY521::checkStatus(int mode) {
   if (mode == 0) {
-    return gyro_LSB;
+    return gyro_LSB_;
   } else if (mode == 1) {
-    return gyro_z_aver;
+    return gyro_z_aver_;
   } else if (mode == 2) {
-    return acos(gyro_LSB / GY521_LSB_MAP[bit_]);
+    return acos(gyro_LSB_ / GY521_LSB_MAP[bit_]);
   }
   return 0;
 }
@@ -96,7 +113,7 @@
 int16_t GY521::gyroRead2(enum GY521RegisterMap reg) {
     char data[2];
     char addr = reg;
-    i2c.write(dev_id,&addr,1,true);
-    i2c.read(dev_id,data,2);
+    i2c_.write(dev_id,&addr,1,true);
+    i2c_.read(dev_id,data,2);
     return (data[0] << 8) + data[1];
 }
\ No newline at end of file