Arduino MPU6050学习验证成功了

davis2016

感谢前辈们的贡献
http://www.geek-workshop.com/thread-1935-1-1.html
1. 前期试验，一直失败，考虑可能是接口连接不好，用电烙铁焊接上，效果终于出现，接线连接上了，模块led会亮起
接线INT不用接
2. Arduino程序录入，需要将三个库都导入，程序如下
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"

MPU6050 accelgyro;

int16_t ax, ay, az;
int16_t gx, gy, gz;

char str[512];

void setup() {
    Wire.begin();
    Serial.begin(9600);
    accelgyro.initialize();
}

void loop() {
    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    sprintf(str, "%d,%d,%d,%d,%d,%d,%d", ax, ay, az, gx, gy, gz);  
    Serial.print(str);
    Serial.write(byte(10));
    delay(50);
}

3. processing 程序输入，程序中有几个地方需要根据自己电脑的情况做调整。后面有说明。

import processing.serial.*;

Serial myPort;  // 创建串口对象myPort

boolean firstSample = true;

float [] RwAcc = new float[3];         // 通过加速度传感器把重力加速度投影在x/y/z三轴上
float [] Gyro = new float[3];          // 陀螺仪读取
float [] RwGyro = new float[3];        // 重新读取陀螺仪
float [] Awz = new float[2];           // XZ/ YZ平面和Z轴（度）R的投影之间的角度
float [] RwEst = new float[3];


int lastTime = 0;
int interval = 0;
float wGyro = 10.0;

int lf = 10; // 10在ASCII表中表示'\n'
byte[] inBuffer = new byte[100];

PFont font;
final int VIEW_SIZE_X = 600, VIEW_SIZE_Y = 600;


void setup()
{
  size(600, 600, P3D);

  //myPort = new Serial(this, Serial.list()[2], 9600); // 设置电脑第三个COM口为连接端口，这个要根据你电脑情况进行设置。
  myPort = new Serial(this, "COM10", 9600);  

  // 加载字体，字体必须在代码文件同目录下的data文件夹中
font=loadFont("Vivaldii-48.vlw");
}


void readSensors() {
  if (myPort.available() > 0) {
    if (myPort.readBytesUntil(lf, inBuffer) > 0) {
      String inputString = new String(inBuffer);
      String [] inputStringArr = split(inputString, ',');

      // 把原始数据转换为G
      RwAcc[0] = float(inputStringArr[0]) / 16384.0;
      RwAcc[1] = float(inputStringArr[1])/ 16384.0;
      RwAcc[2] = float(inputStringArr[2])/ 16384.0;

      // 把原始数据转换为"度/秒"
      Gyro[0] = float(inputStringArr[3]) / 131.0;
      Gyro[1] = float(inputStringArr[4]) / 131.0;
      Gyro[2] = float(inputStringArr[5]) / 131.0;
    }
  }
}


void normalize3DVec(float [] vector) {
  float R;
  R = sqrt(vector[0]*vector[0] + vector[1]*vector[1] + vector[2]*vector[2]);
  vector[0] /= R;
  vector[1] /= R;  
  vector[2] /= R;
}


float squared(float x) {
  return x*x;
}


void buildBoxShape() {
  //box(60, 10, 40);
  noStroke();
  beginShape(QUADS);

  //Z+ (绘图区域)
  fill(#00ff00);
  vertex(-30, -5, 20);
  vertex(30, -5, 20);
  vertex(30, 5, 20);
  vertex(-30, 5, 20);

  //Z-
  fill(#0000ff);
  vertex(-30, -5, -20);
  vertex(30, -5, -20);
  vertex(30, 5, -20);
  vertex(-30, 5, -20);

  //X-
  fill(#ff0000);
  vertex(-30, -5, -20);
  vertex(-30, -5, 20);
  vertex(-30, 5, 20);
  vertex(-30, 5, -20);

  //X+
  fill(#ffff00);
  vertex(30, -5, -20);
  vertex(30, -5, 20);
  vertex(30, 5, 20);
  vertex(30, 5, -20);

  //Y-
  fill(#ff00ff);
  vertex(-30, -5, -20);
  vertex(30, -5, -20);
  vertex(30, -5, 20);
  vertex(-30, -5, 20);

  //Y+
  fill(#00ffff);
  vertex(-30, 5, -20);
  vertex(30, 5, -20);
  vertex(30, 5, 20);
  vertex(-30, 5, 20);

  endShape();
}


void drawCube() {  
  pushMatrix();
  translate(300, 450, 0);
  scale(4, 4, 4);

  rotateX(HALF_PI * -RwEst[0]);
  rotateZ(HALF_PI * RwEst[1]);

  buildBoxShape();

  popMatrix();
}


void getInclination() {
  int w = 0;
  float tmpf = 0.0;
  int currentTime, signRzGyro;


  readSensors();
  normalize3DVec(RwAcc);

  currentTime = millis();
  interval = currentTime - lastTime;
  lastTime = currentTime;

  if (firstSample || Float.isNaN(RwEst[0])) { // NaN用来等待检查从arduino过来的数据
    for (w=0;w<=2;w++) {
      RwEst[w] = RwAcc[w];    // 初始化加速度传感器读数
    }
  }
  else {
    // 对RwGyro进行评估
    if (abs(RwEst[2]) < 0.1) {
      // Rz值非常的小，它的作用是作为Axz与Ayz的计算参照值，防止放大的波动产生错误的结果。
      // 这种情况下就跳过当前的陀螺仪数据，使用以前的。
      for (w=0;w<=2;w++) {
        RwGyro[w] = RwEst[w];
      }
    }
    else {
      // ZX/ZY平面和Z轴R的投影之间的角度，基于最近一次的RwEst值
      for (w=0;w<=1;w++) {
        tmpf = Gyro[w];                        // 获取当前陀螺仪的deg/s
        tmpf *= interval / 1000.0f;                     // 得到角度变化值
        Awz[w] = atan2(RwEst[w], RwEst[2]) * 180 / PI;   // 得到角度并转换为度
        Awz[w] += tmpf;             // 根据陀螺仪的运动得到更新后的角度
      }

      // 判断RzGyro是多少，主要看Axz的弧度是多少
      // 当Axz在-90 ..90 => cos(Awz) >= 0这个范围内的时候RzGyro是准确的
      signRzGyro = ( cos(Awz[0] * PI / 180) >=0 ) ? 1 : -1;

      // 从Awz的角度值反向计算RwGyro的公式请查看网页 http://starlino.com/imu_guide.html
      for (w=0;w<=1;w++) {
        RwGyro[0] = sin(Awz[0] * PI / 180);
        RwGyro[0] /= sqrt( 1 + squared(cos(Awz[0] * PI / 180)) * squared(tan(Awz[1] * PI / 180)) );
        RwGyro[1] = sin(Awz[1] * PI / 180);
        RwGyro[1] /= sqrt( 1 + squared(cos(Awz[1] * PI / 180)) * squared(tan(Awz[0] * PI / 180)) );
      }
      RwGyro[2] = signRzGyro * sqrt(1 - squared(RwGyro[0]) - squared(RwGyro[1]));
    }

    // 把陀螺仪与加速度传感器的值进行结合
    for (w=0;w<=2;w++) RwEst[w] = (RwAcc[w] + wGyro * RwGyro[w]) / (1 + wGyro);

    normalize3DVec(RwEst);
  }

  firstSample = false;
}


void draw() {  
  getInclination();

  background(#000000);
  fill(#ffffff);

  textFont(font, 20);
  //float temp_decoded = 35.0 + ((float) (temp + 13200)) / 280;
  //text("temp:\n" + temp_decoded + " C", 350, 250);
  text("RwAcc (G):\n" + RwAcc[0] + "\n" + RwAcc[1] + "\n" + RwAcc[2] + "\ninterval: " + interval, 20, 50);
  text("Gyro (°/s):\n" + Gyro[0] + "\n" + Gyro[1] + "\n" + Gyro[2], 220, 50);
  text("Awz (°):\n" + Awz[0] + "\n" + Awz[1], 420, 50);
  text("RwGyro (°/s):\n" + RwGyro[0] + "\n" + RwGyro[1] + "\n" + RwGyro[2], 20, 180);
  text("RwEst :\n" + RwEst[0] + "\n" + RwEst[1] + "\n" + RwEst[2], 220, 180);

  // display axes显示轴
  pushMatrix();
  translate(450, 250, 0);
  stroke(#ffffff);
  scale(100, 100, 100);
  line(0, 0, 0, 1, 0, 0);
  line(0, 0, 0, 0, -1, 0);
  line(0, 0, 0, 0, 0, 1);
  line(0, 0, 0, -RwEst[0], RwEst[1], RwEst[2]);
  popMatrix();

  drawCube();
}
1.学习大神的原帖时，编译显示size有问题，直接改为600,600，通过编译
2.大神的原帖，需要更新port口，参看上面更新后程序
3.大神的原帖，font调入需要根据自己的调整下，可在processing 工具创建字体，然后调入。

终于实验成功了，高兴啊：）呵呵

原野动力

学习学习 学习学习

学慧放弃

我只想知道上位机用啥开发的

eysajan

学慧放弃 发表于 2016-5-16 20:55
我只想知道上位机用啥开发的

我看的像processing做的

学慧放弃

eysajan 发表于 2016-5-17 18:42
我看的像processing做的

不是你写的？？

eysajan

学慧放弃 发表于 2016-5-17 19:20
不是你写的？？

楼主写的吧，我看的像用processing写的

学慧放弃

eysajan 发表于 2016-5-18 10:45
楼主写的吧，我看的像用processing写的

嗯，我看看吧

davis2016

用的processing 软件

nick_zm

其实6050库里就有 processing写的样例，画面是个飞机

amaverick

就这样也不滤波直接raw数据搞？

zjbhm

是processing写的

小婧哥哥

我把代码考到arduino  显示mpu6050 does not name a type

刘小白_xl

为什么我的就显示几排数据就没了，arduino的灯也没亮，但也没烧

敢伤~

用之前是要进行卡尔曼滤波的吗？

xiaocaicai

不知楼主是否发现一个问题：
……
app_read_sensor_data_timer_handler,*****RwEst[0]=0.997643,  RwEst[1]=0.000960,  RwEst[2]=0.072435
Awz_X=85, Awz_Y=0, tmpf=-0.007317
app_read_sensor_data_timer_handler,*****RwEst[0]=0.997462,  RwEst[1]=0.001040,  RwEst[2]=0.071624
Awz_X=85, Awz_Y=0, tmpf=-0.035366
app_read_sensor_data_timer_handler,*****RwEst[0]=0.997661,  RwEst[1]=0.000773,  RwEst[2]=0.071494
Awz_X=85, Awz_Y=0, tmpf=-0.015244
app_read_sensor_data_timer_handler,*****RwEst[0]=0.997768,  RwEst[1]=0.001240,  RwEst[2]=0.070883
Awz_X=85, Awz_Y=0, tmpf=-0.006707
app_read_sensor_data_timer_handler,*****RwEst[0]=0.997769,  RwEst[1]=0.001827,  RwEst[2]=0.070504
Awz_X=85, Awz_Y=1, tmpf=0.018293
app_read_sensor_data_timer_handler,*****RwEst[0]=0.997945,  RwEst[1]=0.001789,  RwEst[2]=0.069888
Awz_X=86, Awz_Y=1, tmpf=0.006707
app_read_sensor_data_timer_handler,*****RwEst[0]=0.997266,  RwEst[1]=0.002971,  RwEst[2]=0.064135
Awz_X=86, Awz_Y=2, tmpf=-0.009756
app_read_sensor_data_timer_handler,*****RwEst[0]=0.998143,  RwEst[1]=0.003070,  RwEst[2]=0.064613
Awz_X=86, Awz_Y=2, tmpf=-0.039634
app_read_sensor_data_timer_handler,*****RwEst[0]=0.997942,  RwEst[1]=0.003349,  RwEst[2]=0.064487
Awz_X=86, Awz_Y=2, tmpf=-0.023780
app_read_sensor_data_timer_handler,*****RwEst[0]=0.998270,  RwEst[1]=0.003072,  RwEst[2]=0.064282
Awz_X=86, Awz_Y=2, tmpf=0.001220
app_read_sensor_data_timer_handler,*****RwEst[0]=0.998033,  RwEst[1]=0.003100,  RwEst[2]=0.063912
Awz_X=86, Awz_Y=2, tmpf=0.002439
app_read_sensor_data_timer_handler,*****RwEst[0]=0.997779,  RwEst[1]=0.003178,  RwEst[2]=0.064493
Awz_X=86, Awz_Y=2, tmpf=-0.026829
app_read_sensor_data_timer_handler,*****RwEst[0]=0.998031,  RwEst[1]=0.002954,  RwEst[2]=0.063611
Awz_X=86, Awz_Y=2, tmpf=-0.000610
app_read_sensor_data_timer_handler,*****RwEst[0]=0.997760,  RwEst[1]=0.002992,  RwEst[2]=0.062906
Awz_X=86, Awz_Y=2, tmpf=-0.020732
app_read_sensor_data_timer_handler,*****RwEst[0]=0.997969,  RwEst[1]=0.003058,  RwEst[2]=0.062165
Awz_X=86, Awz_Y=2, tmpf=-0.031707
app_read_sensor_data_timer_handler,*****RwEst[0]=0.998079,  RwEst[1]=0.003039,  RwEst[2]=0.061530
Awz_X=86, Awz_Y=2, tmpf=-0.031098
app_read_sensor_data_timer_handler,*****RwEst[0]=0.998675,  RwEst[1]=0.002709,  RwEst[2]=0.060959
Awz_X=86, Awz_Y=2, tmpf=-0.002439
app_read_sensor_data_timer_handler,*****RwEst[0]=0.998168,  RwEst[1]=0.002552,  RwEst[2]=0.060488
Awz_X=86, Awz_Y=2, tmpf=-0.020122
app_read_sensor_data_timer_handler,*****RwEst[0]=0.998272,  RwEst[1]=0.002785,  RwEst[2]=0.059133
Awz_X=86, Awz_Y=2, tmpf=-0.001220
app_read_sensor_data_timer_handler,*****RwEst[0]=0.998628,  RwEst[1]=0.002930,  RwEst[2]=0.058394
Awz_X=86, Awz_Y=2, tmpf=-0.049390
app_read_sensor_data_timer_handler,*****RwEst[0]=0.998322,  RwEst[1]=0.002543,  RwEst[2]=0.056819
Awz_X=86, Awz_Y=2, tmpf=-0.054268
app_read_sensor_data_timer_handler,*****RwEst[0]=0.999132,  RwEst[1]=0.002437,  RwEst[2]=0.055508
Awz_X=86, Awz_Y=2, tmpf=0.013415
app_read_sensor_data_timer_handler,*****RwEst[0]=0.998938,  RwEst[1]=0.002553,  RwEst[2]=0.054198
Awz_X=86, Awz_Y=2, tmpf=0.003049
app_read_sensor_data_timer_handler,*****RwEst[0]=0.998596,  RwEst[1]=0.002610,  RwEst[2]=0.052495
Awz_X=87, Awz_Y=2, tmpf=0.011585
app_read_sensor_data_timer_handler,*****RwEst[0]=0.998775,  RwEst[1]=0.002728,  RwEst[2]=0.051535
Awz_X=87, Awz_Y=3, tmpf=-0.004878
app_read_sensor_data_timer_handler,*****RwEst[0]=0.998985,  RwEst[1]=0.003079,  RwEst[2]=0.051405
Awz_X=87, Awz_Y=3, tmpf=-0.009756
app_read_sensor_data_timer_handler,*****RwEst[0]=0.998648,  RwEst[1]=0.003357,  RwEst[2]=0.050294
Awz_X=87, Awz_Y=3, tmpf=0.015854
app_read_sensor_data_timer_handler,*****RwEst[0]=0.998734,  RwEst[1]=0.003442,  RwEst[2]=0.049467
Awz_X=87, Awz_Y=3, tmpf=-0.004878
app_read_sensor_data_timer_handler,*****RwEst[0]=0.999089,  RwEst[1]=0.003358,  RwEst[2]=0.048225
Awz_X=87, Awz_Y=3, tmpf=0.003049
app_read_sensor_data_timer_handler,*****RwEst[0]=0.999072,  RwEst[1]=0.003318,  RwEst[2]=0.048395
Awz_X=87, Awz_Y=3, tmpf=0.004268
app_read_sensor_data_timer_handler,*****RwEst[0]=0.999113,  RwEst[1]=0.003212,  RwEst[2]=0.048238
Awz_X=87, Awz_Y=3, tmpf=-0.015244
app_read_sensor_data_timer_handler,*****RwEst[0]=0.999161,  RwEst[1]=0.002884,  RwEst[2]=0.047949
Awz_X=87, Awz_Y=3, tmpf=0.017683
app_read_sensor_data_timer_handler,*****RwEst[0]=0.999114,  RwEst[1]=0.003024,  RwEst[2]=0.048139
Awz_X=87, Awz_Y=3, tmpf=0.009146
app_read_sensor_data_timer_handler,*****RwEst[0]=0.998859,  RwEst[1]=0.002926,  RwEst[2]=0.048163
Awz_X=87, Awz_Y=3, tmpf=-0.033537
app_read_sensor_data_timer_handler,*****RwEst[0]=0.998946,  RwEst[1]=0.002961,  RwEst[2]=0.047872
Awz_X=87, Awz_Y=3, tmpf=-0.023780
app_read_sensor_data_timer_handler,*****RwEst[0]=0.998977,  RwEst[1]=0.002872,  RwEst[2]=0.047647
Awz_X=87, Awz_Y=3, tmpf=-0.006707
app_read_sensor_data_timer_handler,*****RwEst[0]=0.999209,  RwEst[1]=0.003025,  RwEst[2]=0.047099
Awz_X=87, Awz_Y=3, tmpf=-0.019512
app_read_sensor_data_timer_handler,*****RwEst[0]=0.999052,  RwEst[1]=0.003271,  RwEst[2]=0.046146
Awz_X=87, Awz_Y=4, tmpf=-0.015244
app_read_sensor_data_timer_handler,*****RwEst[0]=0.999344,  RwEst[1]=0.003552,  RwEst[2]=0.045682
Awz_X=87, Awz_Y=4, tmpf=-0.034756
app_read_sensor_data_timer_handler,*****RwEst[0]=0.999047,  RwEst[1]=0.003245,  RwEst[2]=0.044880
Awz_X=87, Awz_Y=4, tmpf=0.020122
app_read_sensor_data_timer_handler,*****RwEst[0]=0.999238,  RwEst[1]=0.003603,  RwEst[2]=0.044237
Awz_X=87, Awz_Y=4, tmpf=-0.010976
app_read_sensor_data_timer_handler,*****RwEst[0]=0.998910,  RwEst[1]=0.003835,  RwEst[2]=0.043696
Awz_X=87, Awz_Y=5, tmpf=0.003659
app_read_sensor_data_timer_handler,*****RwEst[0]=0.999546,  RwEst[1]=0.003889,  RwEst[2]=0.043325
Awz_X=87, Awz_Y=5, tmpf=-0.010366
app_read_sensor_data_timer_handler,*****RwEst[0]=0.999281,  RwEst[1]=0.003889,  RwEst[2]=0.043042
Awz_X=87, Awz_Y=5, tmpf=-0.027439
app_read_sensor_data_timer_handler,*****RwEst[0]=0.999374,  RwEst[1]=0.003580,  RwEst[2]=0.042021
Awz_X=87, Awz_Y=4, tmpf=0.011585
app_read_sensor_data_timer_handler,*****RwEst[0]=0.999267,  RwEst[1]=0.004242,  RwEst[2]=0.040887
Awz_X=87, Awz_Y=5, tmpf=0.008537
app_read_sensor_data_timer_handler,*****RwEst[0]=0.999371,  RwEst[1]=0.004291,  RwEst[2]=0.039870
Awz_X=87, Awz_Y=6, tmpf=-0.023780
app_read_sensor_data_timer_handler,*****RwEst[0]=0.999656,  RwEst[1]=0.004327,  RwEst[2]=0.039446
Awz_X=87, Awz_Y=6, tmpf=-0.025000
app_read_sensor_data_timer_handler,*****RwEst[0]=0.999625,  RwEst[1]=0.004361,  RwEst[2]=0.038936
Awz_X=87, Awz_Y=6, tmpf=-0.012195
app_read_sensor_data_timer_handler,*****RwEst[0]=0.999440,  RwEst[1]=0.004720,  RwEst[2]=0.037895
Awz_X=87, Awz_Y=7, tmpf=-0.004878
app_read_sensor_data_timer_handler,*****RwEst[0]=0.999496,  RwEst[1]=0.004453,  RwEst[2]=0.036967
Awz_X=87, Awz_Y=6, tmpf=0.004878
app_read_sensor_data_timer_handler,*****RwEst[0]=0.999730,  RwEst[1]=0.004701,  RwEst[2]=0.036338
Awz_X=87, Awz_Y=7, tmpf=-0.007317
app_read_sensor_data_timer_handler,*****RwEst[0]=0.999681,  RwEst[1]=0.004925,  RwEst[2]=0.036327
Awz_X=87, Awz_Y=7, tmpf=0.012805
app_read_sensor_data_timer_handler,*****RwEst[0]=0.999684,  RwEst[1]=0.004883,  RwEst[2]=0.035767
Awz_X=87, Awz_Y=7, tmpf=-0.007317
app_read_sensor_data_timer_handler,*****RwEst[0]=0.999415,  RwEst[1]=0.005119,  RwEst[2]=0.034917
Awz_X=88, Awz_Y=8, tmpf=0.012195
app_read_sensor_data_timer_handler,*****RwEst[0]=0.999816,  RwEst[1]=0.004731,  RwEst[2]=0.035146
Awz_X=87, Awz_Y=7, tmpf=-0.037195
app_read_sensor_data_timer_handler,*****RwEst[0]=0.999590,  RwEst[1]=0.004701,  RwEst[2]=0.034091
Awz_X=88, Awz_Y=7, tmpf=-0.022561
app_read_sensor_data_timer_handler,*****RwEst[0]=0.999842,  RwEst[1]=0.004847,  RwEst[2]=0.034117
Awz_X=88, Awz_Y=8, tmpf=-0.034756
app_read_sensor_data_timer_handler,*****RwEst[0]=0.999751,  RwEst[1]=0.004777,  RwEst[2]=0.033430
Awz_X=88, Awz_Y=8, tmpf=-0.023780
app_read_sensor_data_timer_handler,*****RwEst[0]=0.999572,  RwEst[1]=0.004965,  RwEst[2]=0.032221
Awz_X=88, Awz_Y=8, tmpf=-0.009756
app_read_sensor_data_timer_handler,*****RwEst[0]=0.999853,  RwEst[1]=0.004714,  RwEst[2]=0.031423
Awz_X=88, Awz_Y=8, tmpf=-0.034146
app_read_sensor_data_timer_handler,*****RwEst[0]=0.999547,  RwEst[1]=0.004996,  RwEst[2]=0.029236
Awz_X=88, Awz_Y=9, tmpf=-0.015244
app_read_sensor_data_timer_handler,*****RwEst[0]=0.999399,  RwEst[1]=0.005361,  RwEst[2]=0.028362
Awz_X=88, Awz_Y=10, tmpf=-0.033537
app_read_sensor_data_timer_handler,*****RwEst[0]=0.999599,  RwEst[1]=0.005438,  RwEst[2]=0.027508
Awz_X=88, Awz_Y=11, tmpf=-0.046951
app_read_sensor_data_timer_handler,*****RwEst[0]=0.999398,  RwEst[1]=0.005344,  RwEst[2]=0.026228
Awz_X=88, Awz_Y=11, tmpf=0.020122
app_read_sensor_data_timer_handler,*****RwEst[0]=1.000078,  RwEst[1]=0.005391,  RwEst[2]=0.025403
Awz_X=88, Awz_Y=11, tmpf=-0.008537
app_read_sensor_data_timer_handler,*****RwEst[0]=1.000051,  RwEst[1]=0.005388,  RwEst[2]=0.024305
Awz_X=88, Awz_Y=12, tmpf=-0.037195
app_read_sensor_data_timer_handler,*****RwEst[0]=0.999877,  RwEst[1]=0.005608,  RwEst[2]=0.023050
Awz_X=88, Awz_Y=13, tmpf=-0.021951
app_read_sensor_data_timer_handler,*****RwEst[0]=1.000059,  RwEst[1]=0.005806,  RwEst[2]=0.022218
Awz_X=88, Awz_Y=14, tmpf=-0.006707
app_read_sensor_data_timer_handler,*****RwEst[0]=0.999695,  RwEst[1]=0.006013,  RwEst[2]=0.020312
Awz_X=88, Awz_Y=16, tmpf=0.024390
app_read_sensor_data_timer_handler,*****RwEst[0]=1.000175,  RwEst[1]=0.005982,  RwEst[2]=0.019931
Awz_X=88, Awz_Y=16, tmpf=-0.035976
app_read_sensor_data_timer_handler,*****RwEst[0]=0.999917,  RwEst[1]=0.006227,  RwEst[2]=0.018821
Awz_X=88, Awz_Y=18, tmpf=-0.040244
app_read_sensor_data_timer_handler,*****RwEst[0]=1.000331,  RwEst[1]=0.007010,  RwEst[2]=0.017425
Awz_X=89, Awz_Y=21, tmpf=0.005488
app_read_sensor_data_timer_handler,*****RwEst[0]=1.000018,  RwEst[1]=0.007056,  RwEst[2]=0.016424
Awz_X=89, Awz_Y=23, tmpf=-0.003049
app_read_sensor_data_timer_handler,*****RwEst[0]=1.000321,  RwEst[1]=0.007238,  RwEst[2]=0.015516
Awz_X=89, Awz_Y=25, tmpf=-0.003659
app_read_sensor_data_timer_handler,*****RwEst[0]=0.999823,  RwEst[1]=0.007268,  RwEst[2]=0.014215
Awz_X=89, Awz_Y=27, tmpf=-0.006707
app_read_sensor_data_timer_handler,*****RwEst[0]=0.999907,  RwEst[1]=0.007333,  RwEst[2]=0.014179
Awz_X=89, Awz_Y=27, tmpf=-0.009146
app_read_sensor_data_timer_handler,*****RwEst[0]=0.999930,  RwEst[1]=0.007545,  RwEst[2]=0.013037
Awz_X=89, Awz_Y=30, tmpf=-0.017683
app_read_sensor_data_timer_handler,*****RwEst[0]=1.000541,  RwEst[1]=0.007376,  RwEst[2]=0.012785
Awz_X=89, Awz_Y=29, tmpf=-0.010976
app_read_sensor_data_timer_handler,*****RwEst[0]=1.000059,  RwEst[1]=0.007363,  RwEst[2]=0.010881
Awz_X=89, Awz_Y=34, tmpf=-0.018902
app_read_sensor_data_timer_handler,*****RwEst[0]=0.999878,  RwEst[1]=0.007401,  RwEst[2]=0.010214
Awz_X=89, Awz_Y=35, tmpf=-0.001220
app_read_sensor_data_timer_handler,*****RwEst[0]=0.999995,  RwEst[1]=0.007706,  RwEst[2]=0.009993
Awz_X=89, Awz_Y=37, tmpf=0.004878
app_read_sensor_data_timer_handler,*****RwEst[0]=1.000105,  RwEst[1]=0.008086,  RwEst[2]=0.009452
Awz_X=89, Awz_Y=40, tmpf=-0.032927
app_read_sensor_data_timer_handler,*****RwEst[0]=1.000152,  RwEst[1]=0.007663,  RwEst[2]=0.008771
Awz_X=89, Awz_Y=41, tmpf=-0.015854
app_read_sensor_data_timer_handler,*****RwEst[0]=1.000495,  RwEst[1]=0.007920,  RwEst[2]=0.007568
Awz_X=89, Awz_Y=46, tmpf=-0.042073
app_read_sensor_data_timer_handler,*****RwEst[0]=1.000236,  RwEst[1]=0.007883,  RwEst[2]=0.005914
Awz_X=89, Awz_Y=53, tmpf=0.006098
app_read_sensor_data_timer_handler,*****RwEst[0]=1.000445,  RwEst[1]=0.007927,  RwEst[2]=0.005326
Awz_X=89, Awz_Y=56, tmpf=-0.035366
app_read_sensor_data_timer_handler,*****RwEst[0]=0.999920,  RwEst[1]=0.007984,  RwEst[2]=0.000342
Awz_X=89, Awz_Y=87, tmpf=-0.037195
app_read_sensor_data_timer_handler,*****RwEst[0]=1.000310,  RwEst[1]=0.007213,  RwEst[2]=-0.000199
Awz_X=90, Awz_Y=91, tmpf=-0.002439
app_read_sensor_data_timer_handler,*****RwEst[0]=1.000368,  RwEst[1]=0.008129,  RwEst[2]=-0.000977
Awz_X=90, Awz_Y=96, tmpf=-0.028049
app_read_sensor_data_timer_handler,*****RwEst[0]=1.000053,  RwEst[1]=0.008663,  RwEst[2]=-0.001985
Awz_X=90, Awz_Y=102, tmpf=-0.004268
app_read_sensor_data_timer_handler,*****RwEst[0]=1.000521,  RwEst[1]=0.008265,  RwEst[2]=-0.001740
Awz_X=90, Awz_Y=101, tmpf=-0.037195
app_read_sensor_data_timer_handler,*****RwEst[0]=1.000165,  RwEst[1]=0.008277,  RwEst[2]=-0.003043
Awz_X=90, Awz_Y=110, tmpf=-0.031098
app_read_sensor_data_timer_handler,*****RwEst[0]=1.000453,  RwEst[1]=0.007758,  RwEst[2]=-0.004518
Awz_X=90, Awz_Y=120, tmpf=-0.018902
app_read_sensor_data_timer_handler,*****RwEst[0]=1.000229,  RwEst[1]=0.007526,  RwEst[2]=-0.005668
Awz_X=90, Awz_Y=126, tmpf=-0.034756
app_read_sensor_data_timer_handler,*****RwEst[0]=1.000092,  RwEst[1]=0.007610,  RwEst[2]=-0.007092
Awz_X=90, Awz_Y=132, tmpf=-0.021951
app_read_sensor_data_timer_handler,*****RwEst[0]=1.000346,  RwEst[1]=0.007714,  RwEst[2]=-0.007423
Awz_X=90, Awz_Y=133, tmpf=-0.020122
app_read_sensor_data_timer_handler,*****RwEst[0]=1.000347,  RwEst[1]=0.007178,  RwEst[2]=-0.008975
Awz_X=90, Awz_Y=141, tmpf=-0.011585
app_read_sensor_data_timer_handler,*****RwEst[0]=1.000006,  RwEst[1]=0.007304,  RwEst[2]=-0.009855
Awz_X=90, Awz_Y=143, tmpf=-0.027439
app_read_sensor_data_timer_handler,*****RwEst[0]=1.000042,  RwEst[1]=0.007156,  RwEst[2]=-0.011051
Awz_X=90, Awz_Y=147, tmpf=-0.000610
app_read_sensor_data_timer_handler,*****RwEst[0]=1.000277,  RwEst[1]=0.007063,  RwEst[2]=-0.012012
Awz_X=90, Awz_Y=149, tmpf=-0.004268
app_read_sensor_data_timer_handler,*****RwEst[0]=1.000287,  RwEst[1]=0.007105,  RwEst[2]=-0.012906
Awz_X=90, Awz_Y=151, tmpf=-0.021951
app_read_sensor_data_timer_handler,*****RwEst[0]=0.999723,  RwEst[1]=0.007188,  RwEst[2]=-0.014132
Awz_X=90, Awz_Y=153, tmpf=-0.014024
app_read_sensor_data_timer_handler,*****RwEst[0]=0.999773,  RwEst[1]=0.006838,  RwEst[2]=-0.015797
Awz_X=90, Awz_Y=156, tmpf=0.002439
app_read_sensor_data_timer_handler,*****RwEst[0]=1.000332,  RwEst[1]=0.007051,  RwEst[2]=-0.016184
Awz_X=90, Awz_Y=156, tmpf=-0.004878
app_read_sensor_data_timer_handler,*****RwEst[0]=1.000590,  RwEst[1]=0.007095,  RwEst[2]=-0.017065
Awz_X=90, Awz_Y=157, tmpf=0.010976
app_read_sensor_data_timer_handler,*****RwEst[0]=1.000043,  RwEst[1]=0.007340,  RwEst[2]=-0.018642
Awz_X=91, Awz_Y=158, tmpf=0.009756
app_read_sensor_data_timer_handler,*****RwEst[0]=1.000504,  RwEst[1]=0.007453,  RwEst[2]=-0.019483
Awz_X=91, Awz_Y=159, tmpf=-0.012195
app_read_sensor_data_timer_handler,*****RwEst[0]=1.000071,  RwEst[1]=0.007995,  RwEst[2]=-0.021469
Awz_X=91, Awz_Y=159, tmpf=-0.042073
app_read_sensor_data_timer_handler,*****RwEst[0]=1.000316,  RwEst[1]=0.008196,  RwEst[2]=-0.022938
Awz_X=91, Awz_Y=160, tmpf=-0.001220
app_read_sensor_data_timer_handler,*****RwEst[0]=0.999861,  RwEst[1]=0.008547,  RwEst[2]=-0.023488
Awz_X=91, Awz_Y=160, tmpf=0.005488
app_read_sensor_data_timer_handler,*****RwEst[0]=1.000117,  RwEst[1]=0.008677,  RwEst[2]=-0.023244
Awz_X=91, Awz_Y=159, tmpf=-0.005488

如上是我打印出来的log，在Awz_X(即Awz[0])旋转达到86°以前，YAwz_Y(即Awz[1])的角度基本不变；但是随着Awz_X逐渐达到90°时，Awz_Y为什么会达到100多度呢？