gy521.cpp

00001 #include "gy521.hpp"
00002 
00003 const double GY521_LSB_MAP[4] = {131, 65.5, 32.8, 16.4};
00004 const unsigned int dev_id = 0x68 <<  1;
00005 
00006 GY521::GY521(I2C &i2c,int bit,int calibration,double user_reg):i2c_(i2c),bit_(bit),calibration_(calibration){
00007     char check;
00008     char data[2] = {WHO_AM_I,0x00};
00009     i2c_.write(dev_id,data,1,true);
00010     i2c_.read(dev_id,&check,1);
00011     if(check == dev_id >> 1){
00012         data[0] = PWR_MGMT_1;
00013         i2c_.write(dev_id,data,1,true);
00014         i2c_.read(dev_id,&check,1);
00015         if(check == 0x40){
00016             i2c_.write(dev_id,data,2);
00017         }
00018     }
00019     data[0] = LPF;
00020     data[1] = 0x00;
00021     i2c_.write(dev_id,data,2);
00022     data[0] = AFS_SEL;
00023     data[1] = 0x00;
00024     i2c_.write(dev_id,data,2);
00025     short accel_x_now = 0,accel_y_now = 0,accel_z_now = 0;
00026     double accel_x_aver = 0, accel_y_aver = 0, accel_z_aver = 0;
00027     for (int i = 0; i < calibration_; ++i) {
00028         accel_x_now = gyroRead2(ACCEL_XOUT_H);
00029         accel_y_now = gyroRead2(ACCEL_YOUT_H);
00030         accel_z_now = gyroRead2(ACCEL_ZOUT_H);
00031         accel_x_aver += accel_x_now;
00032         accel_y_aver += accel_y_now;
00033         accel_z_aver += accel_z_now;
00034     }
00035     accel_x_aver /= calibration_;
00036     accel_y_aver /= calibration_;
00037     accel_z_aver /= calibration_;
00038     double gyro_reg = hypot(hypot(accel_x_aver, accel_y_aver), accel_z_aver) / accel_z_aver;
00039     data[0] = FS_SEL;
00040     data[1] = bit << 3;
00041     i2c_.write(dev_id,data,2);
00042     gyro_LSB_ = GY521_LSB_MAP[bit_] / gyro_reg / user_reg;
00043     
00044     short gyro_z_now_;
00045     gyro_z_aver_ = 0;
00046     for (int i = 0; i < calibration_; ++i) {
00047         gyro_z_now_ = gyroRead2(GYRO_ZOUT_H);
00048         gyro_z_aver_ += gyro_z_now_;
00049     }
00050     gyro_z_aver_ /= calibration_;
00051 
00052     yaw = diff_yaw_ = 0;
00053     timer_.start();
00054     flag_ = false;
00055 }
00056 
00057 void GY521::update(){
00058     if(flag_){
00059         return;
00060     }
00061     short gyro_z_now_ = gyroRead2(GYRO_ZOUT_H);
00062     gyro_z_now_ = ((double)gyro_z_now_ - gyro_z_aver_) / gyro_LSB_;
00063     diff_yaw_ = (gyro_z_now_ + gyro_z_prev_) / 2 * timer_.read();
00064     timer_.reset();
00065     yaw += diff_yaw_;
00066     gyro_z_prev_ = gyro_z_now_;
00067     //temp = (double)gyroRead2(TEMPERATURE)/340+36.53;
00068     if (yaw > 180) {
00069         yaw -= 360;
00070     } else if (yaw <= -180) {
00071         yaw += 360;
00072     }
00073 }
00074 
00075 void GY521::reset(int user){
00076     yaw = user;
00077     short gyro_z_now_;
00078     flag_ = true;
00079     gyro_z_aver_ = 0;
00080     for (int i = 0; i < calibration_; i++) {
00081         gyro_z_now_ = gyroRead2(GYRO_ZOUT_H);
00082         gyro_z_aver_ += gyro_z_now_;
00083     }
00084     gyro_z_aver_ /= calibration_;
00085     flag_ = false;
00086 }
00087     
00088 double GY521::checkStatus(int mode) {
00089   if (mode == 0) {
00090     return gyro_LSB_;
00091   } else if (mode == 1) {
00092     return gyro_z_aver_;
00093   } else if (mode == 2) {
00094     return acos(gyro_LSB_ / GY521_LSB_MAP[bit_]);
00095   }
00096   return 0;
00097 }
00098 
00099 int16_t GY521::gyroRead2(enum GY521RegisterMap reg) {
00100     char data[2];
00101     char addr = reg;
00102     i2c_.write(dev_id,&addr,1,true);
00103     i2c_.read(dev_id,data,2);
00104     return (data[0] << 8) + data[1];
00105 }