JY901.cpp

00001 
00002 #include "JY901.h"
00003 void JY901::receiveData()
00004 {
00005     char ch;
00006     while(mod.readable()){
00007         ch = mod.getc();
00008         parseInput(&ch, 1);
00009     }
00010 }
00011 void JY901::parseCmpt(int token, unsigned char* payloadBuf, int payloadLen)
00012 {
00013     switch(token){
00014         case 0x51:
00015             for (int i = 0; i < 3; ++i)
00016             {
00017                 acc[i] = payloadBuf[i*2]|((int)payloadBuf[i*2+1]<<8);
00018                 acc[i] = acc[i] * 16 * 9.8 / 32768;
00019             }
00020             // lcd.cls();
00021             // lcd.printf("Ax=%d\n",int(data[0]*100));
00022             //pc.printf("Ax=%.2f\tAy=%.2f\tAz=%.2f\r\n", data[0], data[1], data[2]);
00023             break;
00024         case 0x52:
00025             for (int i = 0; i < 3; ++i)
00026             {
00027                 gyo[i] = payloadBuf[i*2]|((int)payloadBuf[i*2+1]<<8);
00028                 gyo[i] = gyo[i] * 2000 / 32768;
00029             }
00030             //pc.printf("Wx=%.2f\tWy=%.2f\tWz=%.2f\r\n", data[0], data[1], data[2]);
00031             break;
00032         case 0x53:
00033             for (int i = 0; i < 3; ++i)
00034             {
00035                 att[i] = payloadBuf[i*2]|((int)payloadBuf[i*2+1]<<8);
00036                 att[i] = att[i] * 180 / 32768;
00037             }
00038             // pc.printf("Roll=%.2f\tPitch=%.2f\tYaw=%.2f\r\n", data[0], data[1], data[2]);
00039             break;
00040         case 0x54:
00041             for (int i = 0; i < 3; ++i)
00042             {
00043                 mag[i] = payloadBuf[i*2]|((int)payloadBuf[i*2+1]<<8);
00044             }
00045             //pc.printf("Hx=%.2f\tHy=%.2f\tHz=%.2f\r\n", data[0], data[1], data[2]);
00046             break;
00047     }
00048 }
00049 void JY901::parseInput(const char* data, int len)
00050 {
00051     for (int i = 0; i < len; ++i)
00052     {
00053         unsigned char ch = data[i], sum;
00054         switch(state){
00055             case 0:
00056                 if(ch == 0x55)
00057                     state = 1;
00058                 break;
00059             case 1:
00060                 token = ch;
00061                 if(0x51 <= token && token <= 0x54){
00062                     payloadLen = 8;
00063                     recvLen = 0;
00064                     state = 2;
00065                 }else{
00066                     // pc.printf("%s %x\r\n", "unknown token", token);
00067                     state = 0;
00068                 }
00069                 break;
00070             case 2:
00071                 payloadBuf[recvLen++] = ch;
00072                 if(recvLen == payloadLen){
00073                     state = 3;
00074                 }
00075                 break;
00076             case 3:
00077                 sum = 0x55;
00078                 sum += token;
00079                 for (int i = 0; i < payloadLen; ++i)
00080                 {
00081                     sum += payloadBuf[i];
00082                 }
00083                 if(sum != ch){
00084                     // pc.printf("wrong checksum\r\n");
00085                 }else{
00086                     parseCmpt(token, payloadBuf, payloadLen);
00087                     //myled = !myled;
00088                 }
00089                 state = 0;
00090                 break;
00091         }
00092     }
00093 }