gyro.cpp

00001 #include "gyro.h"
00002 
00003 #define IMU_TX_PINMAP p28
00004 #define IMU_RX_PINMAP p27
00005 Serial device(IMU_TX_PINMAP, IMU_RX_PINMAP);  // tx, rx
00006 Serial pc(USBTX,USBRX);
00007 unsigned char Re_buf[11],counter=0;
00008 unsigned char sign=0;
00009 float a[3],w[3],angle[3],T;
00010 float yaw = 0;
00011 void output();
00012 void deviceEvent() {
00013     Re_buf[counter]=(unsigned char)device.getc();
00014     if(counter==0&&Re_buf[0]!=0x55) return;      //第0号数据不是帧头              
00015     counter++;       
00016     if(counter==11)             //接收到11个数据
00017     {    
00018        counter=0;               //重新赋值，准备下一帧数据的接收 
00019        sign=1;
00020     }
00021     output();
00022 }
00023 
00024 void output()
00025 {
00026     if(sign)
00027   {  
00028      sign=0;
00029      if(Re_buf[0]==0x55)      //检查帧头
00030     {  
00031     for (int i=0;i<10;i++)
00032     /*
00033     {pc.printf("%d",Re_buf[i]);}
00034     pc.printf("\n"}*/
00035   switch(Re_buf [1])
00036     {
00037     static float lastYaw = 0;
00038     static float turnNum = 0;
00039     case 0x51:
00040         a[0] = (short(Re_buf [3]<<8| Re_buf [2]))/32768.0*16;
00041         a[1] = (short(Re_buf [5]<<8| Re_buf [4]))/32768.0*16;
00042         a[2] = (short(Re_buf [7]<<8| Re_buf [6]))/32768.0*16;
00043         T = (short(Re_buf [9]<<8| Re_buf [8]))/340.0+36.25;
00044         break;
00045     case 0x52:
00046         w[0] = (short(Re_buf [3]<<8| Re_buf [2]))/32768.0*2000;
00047         w[1] = (short(Re_buf [5]<<8| Re_buf [4]))/32768.0*2000;
00048         w[2] = (short(Re_buf [7]<<8| Re_buf [6]))/32768.0*2000;
00049         T = (short(Re_buf [9]<<8| Re_buf [8]))/340.0+36.25;
00050         break;
00051     case 0x53:
00052         double yaw_;
00053         yaw_ = Re_buf[7] << 8 | Re_buf[6];
00054         yaw_ = yaw_ / 32768.0 * 180;
00055         if(lastYaw < 90 && yaw_ > 270){turnNum -= 1;}
00056         else if(lastYaw > 270 && yaw_ < 90){turnNum += 1;}
00057         lastYaw = yaw_;
00058         yaw = yaw_ + 360*turnNum;
00059         pc.printf("      %8.2f\n", yaw_+360*turnNum);
00060         angle[0] = (short(Re_buf [3]<<8| Re_buf [2]))/32768.0*180;
00061         angle[1] = (short(Re_buf [5]<<8| Re_buf [4]))/32768.0*180;
00062         angle[2] = (short(Re_buf [7]<<8| Re_buf [6]))/32768.0*180;
00063         T = (short(Re_buf [9]<<8| Re_buf [8]))/340.0+36.25;
00064                /* pc.printf("a:");
00065                 pc.printf("%d",a[0]);pc.printf(" ");
00066                 pc.printf("%d",a[1]);pc.printf(" ");
00067                 pc.printf("%d\n",a[2]);pc.printf(" ");
00068                 pc.printf("w:");
00069                 pc.printf("%d",w[0]);pc.printf(" ");
00070                 pc.printf("%d",w[1]);pc.printf(" ");
00071                 pc.printf("%d\n",w[2]);pc.printf(" ");
00072                 pc.printf("angle:");
00073                 pc.printf("%d",angle[0]);pc.printf(" ");
00074                 pc.printf("%d",angle[1]);pc.printf(" ");
00075                 pc.printf("%d\n",angle[2]);pc.printf(" ");
00076                 pc.printf("T:");*/
00077                 break;
00078     } 
00079     }
00080   } 
00081 }
00082 
00083 void init_gyro(){
00084    device.baud(115200);
00085    device.attach(&deviceEvent, device.RxIrq);
00086 }