yotaro morizumi / Mbed 2 deprecated zoomy_customLibrary

my new gear...

Dependencies:   mbed

sensor/gy521.cpp@3:a9b4b2565a23, 2022-03-27 (annotated)

Committer:
yootee
Date:
Sun Mar 27 04:51:16 2022 +0000
Revision:
3:a9b4b2565a23
my new gear...

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yootee 3:a9b4b2565a23 1 #include "gy521.hpp"
yootee 3:a9b4b2565a23 2
yootee 3:a9b4b2565a23 3 const double GY521_LSB_MAP[4] = {131, 65.5, 32.8, 16.4};
yootee 3:a9b4b2565a23 4 const unsigned int dev_id = 0x68 << 1;
yootee 3:a9b4b2565a23 5
yootee 3:a9b4b2565a23 6 GY521::GY521(I2C &i2c,int bit,int calibration,double user_reg):i2c_(i2c),bit_(bit),calibration_(calibration){
yootee 3:a9b4b2565a23 7 char check;
yootee 3:a9b4b2565a23 8 char data[2] = {WHO_AM_I,0x00};
yootee 3:a9b4b2565a23 9 i2c_.write(dev_id,data,1,true);
yootee 3:a9b4b2565a23 10 i2c_.read(dev_id,&check,1);
yootee 3:a9b4b2565a23 11 if(check == dev_id >> 1){
yootee 3:a9b4b2565a23 12 data[0] = PWR_MGMT_1;
yootee 3:a9b4b2565a23 13 i2c_.write(dev_id,data,1,true);
yootee 3:a9b4b2565a23 14 i2c_.read(dev_id,&check,1);
yootee 3:a9b4b2565a23 15 if(check == 0x40){
yootee 3:a9b4b2565a23 16 i2c_.write(dev_id,data,2);
yootee 3:a9b4b2565a23 17 }
yootee 3:a9b4b2565a23 18 }
yootee 3:a9b4b2565a23 19 data[0] = LPF;
yootee 3:a9b4b2565a23 20 data[1] = 0x00;
yootee 3:a9b4b2565a23 21 i2c_.write(dev_id,data,2);
yootee 3:a9b4b2565a23 22 data[0] = AFS_SEL;
yootee 3:a9b4b2565a23 23 data[1] = 0x00;
yootee 3:a9b4b2565a23 24 i2c_.write(dev_id,data,2);
yootee 3:a9b4b2565a23 25 short accel_x_now = 0,accel_y_now = 0,accel_z_now = 0;
yootee 3:a9b4b2565a23 26 double accel_x_aver = 0, accel_y_aver = 0, accel_z_aver = 0;
yootee 3:a9b4b2565a23 27 for (int i = 0; i < calibration_; ++i) {
yootee 3:a9b4b2565a23 28 accel_x_now = gyroRead2(ACCEL_XOUT_H);
yootee 3:a9b4b2565a23 29 accel_y_now = gyroRead2(ACCEL_YOUT_H);
yootee 3:a9b4b2565a23 30 accel_z_now = gyroRead2(ACCEL_ZOUT_H);
yootee 3:a9b4b2565a23 31 accel_x_aver += accel_x_now;
yootee 3:a9b4b2565a23 32 accel_y_aver += accel_y_now;
yootee 3:a9b4b2565a23 33 accel_z_aver += accel_z_now;
yootee 3:a9b4b2565a23 34 }
yootee 3:a9b4b2565a23 35 accel_x_aver /= calibration_;
yootee 3:a9b4b2565a23 36 accel_y_aver /= calibration_;
yootee 3:a9b4b2565a23 37 accel_z_aver /= calibration_;
yootee 3:a9b4b2565a23 38 double gyro_reg = hypot(hypot(accel_x_aver, accel_y_aver), accel_z_aver) / accel_z_aver;
yootee 3:a9b4b2565a23 39 data[0] = FS_SEL;
yootee 3:a9b4b2565a23 40 data[1] = bit << 3;
yootee 3:a9b4b2565a23 41 i2c_.write(dev_id,data,2);
yootee 3:a9b4b2565a23 42 gyro_LSB_ = GY521_LSB_MAP[bit_] / gyro_reg / user_reg;
yootee 3:a9b4b2565a23 43
yootee 3:a9b4b2565a23 44 short gyro_z_now_;
yootee 3:a9b4b2565a23 45 gyro_z_aver_ = 0;
yootee 3:a9b4b2565a23 46 for (int i = 0; i < calibration_; ++i) {
yootee 3:a9b4b2565a23 47 gyro_z_now_ = gyroRead2(GYRO_ZOUT_H);
yootee 3:a9b4b2565a23 48 gyro_z_aver_ += gyro_z_now_;
yootee 3:a9b4b2565a23 49 }
yootee 3:a9b4b2565a23 50 gyro_z_aver_ /= calibration_;
yootee 3:a9b4b2565a23 51
yootee 3:a9b4b2565a23 52 yaw = diff_yaw_ = 0;
yootee 3:a9b4b2565a23 53 timer_.start();
yootee 3:a9b4b2565a23 54 flag_ = false;
yootee 3:a9b4b2565a23 55 }
yootee 3:a9b4b2565a23 56
yootee 3:a9b4b2565a23 57 void GY521::update(){
yootee 3:a9b4b2565a23 58 if(flag_){
yootee 3:a9b4b2565a23 59 return;
yootee 3:a9b4b2565a23 60 }
yootee 3:a9b4b2565a23 61 short gyro_z_now_ = gyroRead2(GYRO_ZOUT_H);
yootee 3:a9b4b2565a23 62 gyro_z_now_ = ((double)gyro_z_now_ - gyro_z_aver_) / gyro_LSB_;
yootee 3:a9b4b2565a23 63 diff_yaw_ = (gyro_z_now_ + gyro_z_prev_) / 2 * timer_.read();
yootee 3:a9b4b2565a23 64 timer_.reset();
yootee 3:a9b4b2565a23 65 yaw += diff_yaw_;
yootee 3:a9b4b2565a23 66 gyro_z_prev_ = gyro_z_now_;
yootee 3:a9b4b2565a23 67 //temp = (double)gyroRead2(TEMPERATURE)/340+36.53;
yootee 3:a9b4b2565a23 68 if (yaw > 180) {
yootee 3:a9b4b2565a23 69 yaw -= 360;
yootee 3:a9b4b2565a23 70 } else if (yaw <= -180) {
yootee 3:a9b4b2565a23 71 yaw += 360;
yootee 3:a9b4b2565a23 72 }
yootee 3:a9b4b2565a23 73 }
yootee 3:a9b4b2565a23 74
yootee 3:a9b4b2565a23 75 void GY521::reset(int user){
yootee 3:a9b4b2565a23 76 yaw = user;
yootee 3:a9b4b2565a23 77 short gyro_z_now_;
yootee 3:a9b4b2565a23 78 flag_ = true;
yootee 3:a9b4b2565a23 79 gyro_z_aver_ = 0;
yootee 3:a9b4b2565a23 80 for (int i = 0; i < calibration_; i++) {
yootee 3:a9b4b2565a23 81 gyro_z_now_ = gyroRead2(GYRO_ZOUT_H);
yootee 3:a9b4b2565a23 82 gyro_z_aver_ += gyro_z_now_;
yootee 3:a9b4b2565a23 83 }
yootee 3:a9b4b2565a23 84 gyro_z_aver_ /= calibration_;
yootee 3:a9b4b2565a23 85 flag_ = false;
yootee 3:a9b4b2565a23 86 }
yootee 3:a9b4b2565a23 87
yootee 3:a9b4b2565a23 88 double GY521::checkStatus(int mode) {
yootee 3:a9b4b2565a23 89 if (mode == 0) {
yootee 3:a9b4b2565a23 90 return gyro_LSB_;
yootee 3:a9b4b2565a23 91 } else if (mode == 1) {
yootee 3:a9b4b2565a23 92 return gyro_z_aver_;
yootee 3:a9b4b2565a23 93 } else if (mode == 2) {
yootee 3:a9b4b2565a23 94 return acos(gyro_LSB_ / GY521_LSB_MAP[bit_]);
yootee 3:a9b4b2565a23 95 }
yootee 3:a9b4b2565a23 96 return 0;
yootee 3:a9b4b2565a23 97 }
yootee 3:a9b4b2565a23 98
yootee 3:a9b4b2565a23 99 int16_t GY521::gyroRead2(enum GY521RegisterMap reg) {
yootee 3:a9b4b2565a23 100 char data[2];
yootee 3:a9b4b2565a23 101 char addr = reg;
yootee 3:a9b4b2565a23 102 i2c_.write(dev_id,&addr,1,true);
yootee 3:a9b4b2565a23 103 i2c_.read(dev_id,data,2);
yootee 3:a9b4b2565a23 104 return (data[0] << 8) + data[1];
yootee 3:a9b4b2565a23 105 }