Diff: gyro.cpp

Revision:

0:68d5307f4eba

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/gyro.cpp	Sat Jun 02 08:18:12 2018 +0000
@@ -0,0 +1,86 @@
+#include "gyro.h"
+
+#define IMU_TX_PINMAP p28
+#define IMU_RX_PINMAP p27
+Serial device(IMU_TX_PINMAP, IMU_RX_PINMAP);  // tx, rx
+Serial pc(USBTX,USBRX);
+unsigned char Re_buf[11],counter=0;
+unsigned char sign=0;
+float a[3],w[3],angle[3],T;
+float yaw = 0;
+void output();
+void deviceEvent() {
+    Re_buf[counter]=(unsigned char)device.getc();
+    if(counter==0&&Re_buf[0]!=0x55) return;      //第0号数据不是帧头              
+    counter++;       
+    if(counter==11)             //接收到11个数据
+    {    
+       counter=0;               //重新赋值，准备下一帧数据的接收 
+       sign=1;
+    }
+    output();
+}
+
+void output()
+{
+    if(sign)
+  {  
+     sign=0;
+     if(Re_buf[0]==0x55)      //检查帧头
+    {  
+    for (int i=0;i<10;i++)
+    /*
+    {pc.printf("%d",Re_buf[i]);}
+    pc.printf("\n"}*/
+  switch(Re_buf [1])
+    {
+    static float lastYaw = 0;
+    static float turnNum = 0;
+    case 0x51:
+        a[0] = (short(Re_buf [3]<<8| Re_buf [2]))/32768.0*16;
+        a[1] = (short(Re_buf [5]<<8| Re_buf [4]))/32768.0*16;
+        a[2] = (short(Re_buf [7]<<8| Re_buf [6]))/32768.0*16;
+        T = (short(Re_buf [9]<<8| Re_buf [8]))/340.0+36.25;
+        break;
+    case 0x52:
+        w[0] = (short(Re_buf [3]<<8| Re_buf [2]))/32768.0*2000;
+        w[1] = (short(Re_buf [5]<<8| Re_buf [4]))/32768.0*2000;
+        w[2] = (short(Re_buf [7]<<8| Re_buf [6]))/32768.0*2000;
+        T = (short(Re_buf [9]<<8| Re_buf [8]))/340.0+36.25;
+        break;
+    case 0x53:
+        double yaw_;
+        yaw_ = Re_buf[7] << 8 | Re_buf[6];
+        yaw_ = yaw_ / 32768.0 * 180;
+        if(lastYaw < 90 && yaw_ > 270){turnNum -= 1;}
+        else if(lastYaw > 270 && yaw_ < 90){turnNum += 1;}
+        lastYaw = yaw_;
+        yaw = yaw_ + 360*turnNum;
+        pc.printf("      %8.2f\n", yaw_+360*turnNum);
+        angle[0] = (short(Re_buf [3]<<8| Re_buf [2]))/32768.0*180;
+        angle[1] = (short(Re_buf [5]<<8| Re_buf [4]))/32768.0*180;
+        angle[2] = (short(Re_buf [7]<<8| Re_buf [6]))/32768.0*180;
+        T = (short(Re_buf [9]<<8| Re_buf [8]))/340.0+36.25;
+               /* pc.printf("a:");
+                pc.printf("%d",a[0]);pc.printf(" ");
+                pc.printf("%d",a[1]);pc.printf(" ");
+                pc.printf("%d\n",a[2]);pc.printf(" ");
+                pc.printf("w:");
+                pc.printf("%d",w[0]);pc.printf(" ");
+                pc.printf("%d",w[1]);pc.printf(" ");
+                pc.printf("%d\n",w[2]);pc.printf(" ");
+                pc.printf("angle:");
+                pc.printf("%d",angle[0]);pc.printf(" ");
+                pc.printf("%d",angle[1]);pc.printf(" ");
+                pc.printf("%d\n",angle[2]);pc.printf(" ");
+                pc.printf("T:");*/
+                break;
+    } 
+    }
+  } 
+}
+
+void init_gyro(){
+   device.baud(115200);
+   device.attach(&deviceEvent, device.RxIrq);
+}
\ No newline at end of file