![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
四轴
Dependencies: BufferedSerial SoftPWM mbed send
Fork of send_copy by
JY901.cpp
- Committer:
- accelerator225
- Date:
- 2017-12-10
- Revision:
- 5:683542d82dd7
- Parent:
- 4:876bfa91934c
File content as of revision 5:683542d82dd7:
#include "JY901.h" void JY901::receiveData() { char ch; while(mod.readable()){ ch = mod.getc(); parseInput(&ch, 1); } } void JY901::parseCmpt(int token, unsigned char* payloadBuf, int payloadLen) { switch(token){ case 0x51: for (int i = 0; i < 3; ++i) { acc[i] = payloadBuf[i*2]|((int)payloadBuf[i*2+1]<<8); acc[i] = acc[i] * 16 * 9.8 / 32768; } // lcd.cls(); // lcd.printf("Ax=%d\n",int(data[0]*100)); //pc.printf("Ax=%.2f\tAy=%.2f\tAz=%.2f\r\n", data[0], data[1], data[2]); break; case 0x52: for (int i = 0; i < 3; ++i) { gyo[i] = payloadBuf[i*2]|((int)payloadBuf[i*2+1]<<8); gyo[i] = gyo[i] * 2000 / 32768; } //pc.printf("Wx=%.2f\tWy=%.2f\tWz=%.2f\r\n", data[0], data[1], data[2]); break; case 0x53: for (int i = 0; i < 3; ++i) { att[i] = payloadBuf[i*2]|((int)payloadBuf[i*2+1]<<8); att[i] = att[i] * 180 / 32768; } // pc.printf("Roll=%.2f\tPitch=%.2f\tYaw=%.2f\r\n", data[0], data[1], data[2]); break; case 0x54: for (int i = 0; i < 3; ++i) { mag[i] = payloadBuf[i*2]|((int)payloadBuf[i*2+1]<<8); } //pc.printf("Hx=%.2f\tHy=%.2f\tHz=%.2f\r\n", data[0], data[1], data[2]); break; } } void JY901::parseInput(const char* data, int len) { for (int i = 0; i < len; ++i) { unsigned char ch = data[i], sum; switch(state){ case 0: if(ch == 0x55) state = 1; break; case 1: token = ch; if(0x51 <= token && token <= 0x54){ payloadLen = 8; recvLen = 0; state = 2; }else{ // pc.printf("%s %x\r\n", "unknown token", token); state = 0; } break; case 2: payloadBuf[recvLen++] = ch; if(recvLen == payloadLen){ state = 3; } break; case 3: sum = 0x55; sum += token; for (int i = 0; i < payloadLen; ++i) { sum += payloadBuf[i]; } if(sum != ch){ // pc.printf("wrong checksum\r\n"); }else{ parseCmpt(token, payloadBuf, payloadLen); //myled = !myled; } state = 0; break; } } }