明石高専ロボ研 mbedライブラリ
Dependents: MDD_L432KC USB2RS485 pathtracking odometry ... more
gy521.cpp@3:28c77df7c0b6, 2020-01-06 (annotated)
- Committer:
- TanakaRobo
- Date:
- Mon Jan 06 11:06:26 2020 +0000
- Revision:
- 3:28c77df7c0b6
- Parent:
- 0:ca84ed7518f5
- Child:
- 4:39ef4d91dc34
send not available
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
TanakaRobo | 0:ca84ed7518f5 | 1 | #include "gy521.hpp" |
TanakaRobo | 0:ca84ed7518f5 | 2 | |
TanakaRobo | 0:ca84ed7518f5 | 3 | const double GY521_LSB_MAP[4] = {131, 65.5, 32.8, 16.4}; |
TanakaRobo | 0:ca84ed7518f5 | 4 | const unsigned int dev_id = 0x68 << 1; |
TanakaRobo | 0:ca84ed7518f5 | 5 | enum GY521RegisterMap { |
TanakaRobo | 0:ca84ed7518f5 | 6 | WHO_AM_I = 0x75, |
TanakaRobo | 0:ca84ed7518f5 | 7 | PWR_MGMT_1 = 0x6B, |
TanakaRobo | 0:ca84ed7518f5 | 8 | LPF = 0x1A, |
TanakaRobo | 0:ca84ed7518f5 | 9 | FS_SEL = 0x1B, |
TanakaRobo | 0:ca84ed7518f5 | 10 | AFS_SEL = 0x1C, |
TanakaRobo | 0:ca84ed7518f5 | 11 | ACCEL_XOUT_H = 0x3B, |
TanakaRobo | 0:ca84ed7518f5 | 12 | ACCEL_YOUT_H = 0x3D, |
TanakaRobo | 0:ca84ed7518f5 | 13 | ACCEL_ZOUT_H = 0x3F, |
TanakaRobo | 0:ca84ed7518f5 | 14 | //TEMPERATURE = 0x41, |
TanakaRobo | 0:ca84ed7518f5 | 15 | //GYRO_XOUT_H = 0x43, |
TanakaRobo | 0:ca84ed7518f5 | 16 | //GYRO_YOUT_H = 0x45, |
TanakaRobo | 0:ca84ed7518f5 | 17 | GYRO_ZOUT_H = 0x47 |
TanakaRobo | 0:ca84ed7518f5 | 18 | }; |
TanakaRobo | 0:ca84ed7518f5 | 19 | |
TanakaRobo | 0:ca84ed7518f5 | 20 | GY521::GY521(I2C &i2c,int bit,int calibration,double user_reg):i2c_(i2c),bit_(bit),calibration_(calibration){ |
TanakaRobo | 0:ca84ed7518f5 | 21 | char check; |
TanakaRobo | 0:ca84ed7518f5 | 22 | char data[2] = {WHO_AM_I,0x00}; |
TanakaRobo | 0:ca84ed7518f5 | 23 | i2c_.write(dev_id,data,1,true); |
TanakaRobo | 0:ca84ed7518f5 | 24 | i2c_.read(dev_id,&check,1); |
TanakaRobo | 0:ca84ed7518f5 | 25 | if(check == dev_id >> 1){ |
TanakaRobo | 0:ca84ed7518f5 | 26 | data[0] = PWR_MGMT_1; |
TanakaRobo | 0:ca84ed7518f5 | 27 | i2c_.write(dev_id,data,1,true); |
TanakaRobo | 0:ca84ed7518f5 | 28 | i2c_.read(dev_id,&check,1); |
TanakaRobo | 0:ca84ed7518f5 | 29 | if(check == 0x40){ |
TanakaRobo | 0:ca84ed7518f5 | 30 | i2c_.write(dev_id,data,2); |
TanakaRobo | 0:ca84ed7518f5 | 31 | } |
TanakaRobo | 0:ca84ed7518f5 | 32 | } |
TanakaRobo | 0:ca84ed7518f5 | 33 | data[0] = LPF; |
TanakaRobo | 0:ca84ed7518f5 | 34 | data[1] = 0x00; |
TanakaRobo | 0:ca84ed7518f5 | 35 | i2c_.write(dev_id,data,2); |
TanakaRobo | 0:ca84ed7518f5 | 36 | data[0] = AFS_SEL; |
TanakaRobo | 0:ca84ed7518f5 | 37 | data[1] = 0x00; |
TanakaRobo | 0:ca84ed7518f5 | 38 | i2c_.write(dev_id,data,2); |
TanakaRobo | 0:ca84ed7518f5 | 39 | short accel_x_now = 0,accel_y_now = 0,accel_z_now = 0; |
TanakaRobo | 0:ca84ed7518f5 | 40 | double accel_x_aver = 0, accel_y_aver = 0, accel_z_aver = 0; |
TanakaRobo | 0:ca84ed7518f5 | 41 | for (int i = 0; i < calibration_; ++i) { |
TanakaRobo | 0:ca84ed7518f5 | 42 | accel_x_now = gyroRead2(ACCEL_XOUT_H); |
TanakaRobo | 0:ca84ed7518f5 | 43 | accel_y_now = gyroRead2(ACCEL_YOUT_H); |
TanakaRobo | 0:ca84ed7518f5 | 44 | accel_z_now = gyroRead2(ACCEL_ZOUT_H); |
TanakaRobo | 0:ca84ed7518f5 | 45 | accel_x_aver += accel_x_now; |
TanakaRobo | 0:ca84ed7518f5 | 46 | accel_y_aver += accel_y_now; |
TanakaRobo | 0:ca84ed7518f5 | 47 | accel_z_aver += accel_z_now; |
TanakaRobo | 0:ca84ed7518f5 | 48 | } |
TanakaRobo | 0:ca84ed7518f5 | 49 | accel_x_aver /= calibration_; |
TanakaRobo | 0:ca84ed7518f5 | 50 | accel_y_aver /= calibration_; |
TanakaRobo | 0:ca84ed7518f5 | 51 | accel_z_aver /= calibration_; |
TanakaRobo | 0:ca84ed7518f5 | 52 | double gyro_reg = hypot(hypot(accel_x_aver, accel_y_aver), accel_z_aver) / accel_z_aver; |
TanakaRobo | 0:ca84ed7518f5 | 53 | data[0] = FS_SEL; |
TanakaRobo | 0:ca84ed7518f5 | 54 | data[1] = bit << 3; |
TanakaRobo | 0:ca84ed7518f5 | 55 | i2c_.write(dev_id,data,2); |
TanakaRobo | 0:ca84ed7518f5 | 56 | gyro_LSB_ = GY521_LSB_MAP[bit_] / gyro_reg / user_reg; |
TanakaRobo | 0:ca84ed7518f5 | 57 | |
TanakaRobo | 0:ca84ed7518f5 | 58 | short gyro_z_now_; |
TanakaRobo | 0:ca84ed7518f5 | 59 | gyro_z_aver_ = 0; |
TanakaRobo | 0:ca84ed7518f5 | 60 | for (int i = 0; i < calibration_; ++i) { |
TanakaRobo | 0:ca84ed7518f5 | 61 | gyro_z_now_ = gyroRead2(GYRO_ZOUT_H); |
TanakaRobo | 0:ca84ed7518f5 | 62 | gyro_z_aver_ += gyro_z_now_; |
TanakaRobo | 0:ca84ed7518f5 | 63 | } |
TanakaRobo | 0:ca84ed7518f5 | 64 | gyro_z_aver_ /= calibration_; |
TanakaRobo | 0:ca84ed7518f5 | 65 | |
TanakaRobo | 0:ca84ed7518f5 | 66 | yaw = diff_yaw_ = 0; |
TanakaRobo | 0:ca84ed7518f5 | 67 | timer_.start(); |
TanakaRobo | 0:ca84ed7518f5 | 68 | flag_ = false; |
TanakaRobo | 0:ca84ed7518f5 | 69 | } |
TanakaRobo | 0:ca84ed7518f5 | 70 | |
TanakaRobo | 3:28c77df7c0b6 | 71 | void GY521::update(){ |
TanakaRobo | 0:ca84ed7518f5 | 72 | if(flag_){ |
TanakaRobo | 0:ca84ed7518f5 | 73 | return; |
TanakaRobo | 0:ca84ed7518f5 | 74 | } |
TanakaRobo | 0:ca84ed7518f5 | 75 | short gyro_z_now_ = gyroRead2(GYRO_ZOUT_H); |
TanakaRobo | 0:ca84ed7518f5 | 76 | gyro_z_now_ = ((double)gyro_z_now_ - gyro_z_aver_) / gyro_LSB_; |
TanakaRobo | 0:ca84ed7518f5 | 77 | diff_yaw_ = (gyro_z_now_ + gyro_z_prev_) / 2 * timer_.read(); |
TanakaRobo | 0:ca84ed7518f5 | 78 | timer_.reset(); |
TanakaRobo | 0:ca84ed7518f5 | 79 | yaw += diff_yaw_; |
TanakaRobo | 0:ca84ed7518f5 | 80 | gyro_z_prev_ = gyro_z_now_; |
TanakaRobo | 0:ca84ed7518f5 | 81 | //temp = (double)gyroRead2(TEMPERATURE)/340+36.53; |
TanakaRobo | 0:ca84ed7518f5 | 82 | if (yaw > 180) { |
TanakaRobo | 0:ca84ed7518f5 | 83 | yaw -= 360; |
TanakaRobo | 0:ca84ed7518f5 | 84 | } else if (yaw <= -180) { |
TanakaRobo | 0:ca84ed7518f5 | 85 | yaw += 360; |
TanakaRobo | 0:ca84ed7518f5 | 86 | } |
TanakaRobo | 0:ca84ed7518f5 | 87 | } |
TanakaRobo | 0:ca84ed7518f5 | 88 | |
TanakaRobo | 0:ca84ed7518f5 | 89 | void GY521::reset(int user){ |
TanakaRobo | 0:ca84ed7518f5 | 90 | yaw = user; |
TanakaRobo | 0:ca84ed7518f5 | 91 | short gyro_z_now_; |
TanakaRobo | 0:ca84ed7518f5 | 92 | flag_ = true; |
TanakaRobo | 0:ca84ed7518f5 | 93 | gyro_z_aver_ = 0; |
TanakaRobo | 0:ca84ed7518f5 | 94 | for (int i = 0; i < calibration_; i++) { |
TanakaRobo | 0:ca84ed7518f5 | 95 | gyro_z_now_ = gyroRead2(GYRO_ZOUT_H); |
TanakaRobo | 0:ca84ed7518f5 | 96 | gyro_z_aver_ += gyro_z_now_; |
TanakaRobo | 0:ca84ed7518f5 | 97 | } |
TanakaRobo | 0:ca84ed7518f5 | 98 | gyro_z_aver_ /= calibration_; |
TanakaRobo | 0:ca84ed7518f5 | 99 | flag_ = false; |
TanakaRobo | 0:ca84ed7518f5 | 100 | } |
TanakaRobo | 0:ca84ed7518f5 | 101 | |
TanakaRobo | 0:ca84ed7518f5 | 102 | double GY521::checkStatus(int mode) { |
TanakaRobo | 0:ca84ed7518f5 | 103 | if (mode == 0) { |
TanakaRobo | 0:ca84ed7518f5 | 104 | return gyro_LSB_; |
TanakaRobo | 0:ca84ed7518f5 | 105 | } else if (mode == 1) { |
TanakaRobo | 0:ca84ed7518f5 | 106 | return gyro_z_aver_; |
TanakaRobo | 0:ca84ed7518f5 | 107 | } else if (mode == 2) { |
TanakaRobo | 0:ca84ed7518f5 | 108 | return acos(gyro_LSB_ / GY521_LSB_MAP[bit_]); |
TanakaRobo | 0:ca84ed7518f5 | 109 | } |
TanakaRobo | 0:ca84ed7518f5 | 110 | return 0; |
TanakaRobo | 0:ca84ed7518f5 | 111 | } |
TanakaRobo | 0:ca84ed7518f5 | 112 | |
TanakaRobo | 0:ca84ed7518f5 | 113 | int16_t GY521::gyroRead2(enum GY521RegisterMap reg) { |
TanakaRobo | 0:ca84ed7518f5 | 114 | char data[2]; |
TanakaRobo | 0:ca84ed7518f5 | 115 | char addr = reg; |
TanakaRobo | 0:ca84ed7518f5 | 116 | i2c_.write(dev_id,&addr,1,true); |
TanakaRobo | 0:ca84ed7518f5 | 117 | i2c_.read(dev_id,data,2); |
TanakaRobo | 0:ca84ed7518f5 | 118 | return (data[0] << 8) + data[1]; |
TanakaRobo | 0:ca84ed7518f5 | 119 | } |