nucleo-l432kcのテンプレートです。
Dependencies: mbed ros_lib_kinetic
Diff: library/GY521.cpp
- Revision:
- 1:17051435cfc5
- Parent:
- 0:a8a56075e947
--- 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