fly_v1
Dependencies: BufferedSerial SoftPWM mbed send
Fork of withbufferserial_nrftst by
Diff: JY901/JY901.cpp
- Revision:
- 6:6e0f02edb404
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/JY901/JY901.cpp Sun Dec 24 03:15:15 2017 +0000 @@ -0,0 +1,89 @@ +#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; + } + 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; + } + } +} \ No newline at end of file