Hiroki Tanaka / Mbed 2 deprecated L432KC_temp

nucleo-l432kcのテンプレートです。

Dependencies:   mbed ros_lib_kinetic

library/GY521.cpp@0:a8a56075e947, 2019-05-18 (annotated)

Committer:
TanakaRobo
Date:
Sat May 18 12:09:43 2019 +0000
Revision:
0:a8a56075e947
Child:
1:17051435cfc5
first commit

Who changed what in which revision?

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