fly_v1
Dependencies: BufferedSerial SoftPWM mbed send
Fork of withbufferserial_nrftst by
JY901/JY901.cpp@6:6e0f02edb404, 2017-12-24 (annotated)
- Committer:
- wanzq
- Date:
- Sun Dec 24 03:15:15 2017 +0000
- Revision:
- 6:6e0f02edb404
fly pro
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
wanzq | 6:6e0f02edb404 | 1 | #include "JY901.h" |
wanzq | 6:6e0f02edb404 | 2 | void JY901::receiveData() |
wanzq | 6:6e0f02edb404 | 3 | { |
wanzq | 6:6e0f02edb404 | 4 | char ch; |
wanzq | 6:6e0f02edb404 | 5 | while(mod.readable()){ |
wanzq | 6:6e0f02edb404 | 6 | ch = mod.getc(); |
wanzq | 6:6e0f02edb404 | 7 | parseInput(&ch, 1); |
wanzq | 6:6e0f02edb404 | 8 | } |
wanzq | 6:6e0f02edb404 | 9 | } |
wanzq | 6:6e0f02edb404 | 10 | void JY901::parseCmpt(int token, unsigned char* payloadBuf, int payloadLen) |
wanzq | 6:6e0f02edb404 | 11 | { |
wanzq | 6:6e0f02edb404 | 12 | switch(token){ |
wanzq | 6:6e0f02edb404 | 13 | case 0x51: |
wanzq | 6:6e0f02edb404 | 14 | for (int i = 0; i < 3; ++i) |
wanzq | 6:6e0f02edb404 | 15 | { |
wanzq | 6:6e0f02edb404 | 16 | acc[i] = payloadBuf[i*2]|((int)payloadBuf[i*2+1]<<8); |
wanzq | 6:6e0f02edb404 | 17 | acc[i] = acc[i] * 16 * 9.8 / 32768; |
wanzq | 6:6e0f02edb404 | 18 | } |
wanzq | 6:6e0f02edb404 | 19 | break; |
wanzq | 6:6e0f02edb404 | 20 | case 0x52: |
wanzq | 6:6e0f02edb404 | 21 | for (int i = 0; i < 3; ++i) |
wanzq | 6:6e0f02edb404 | 22 | { |
wanzq | 6:6e0f02edb404 | 23 | gyo[i] = payloadBuf[i*2]|((int)payloadBuf[i*2+1]<<8); |
wanzq | 6:6e0f02edb404 | 24 | gyo[i] = gyo[i] * 2000 / 32768; |
wanzq | 6:6e0f02edb404 | 25 | } |
wanzq | 6:6e0f02edb404 | 26 | //pc.printf("Wx=%.2f\tWy=%.2f\tWz=%.2f\r\n", data[0], data[1], data[2]); |
wanzq | 6:6e0f02edb404 | 27 | break; |
wanzq | 6:6e0f02edb404 | 28 | case 0x53: |
wanzq | 6:6e0f02edb404 | 29 | for (int i = 0; i < 3; ++i) |
wanzq | 6:6e0f02edb404 | 30 | { |
wanzq | 6:6e0f02edb404 | 31 | att[i] = payloadBuf[i*2]|((int)payloadBuf[i*2+1]<<8); |
wanzq | 6:6e0f02edb404 | 32 | att[i] = att[i] * 180 / 32768; |
wanzq | 6:6e0f02edb404 | 33 | } |
wanzq | 6:6e0f02edb404 | 34 | // pc.printf("Roll=%.2f\tPitch=%.2f\tYaw=%.2f\r\n", data[0], data[1], data[2]); |
wanzq | 6:6e0f02edb404 | 35 | break; |
wanzq | 6:6e0f02edb404 | 36 | case 0x54: |
wanzq | 6:6e0f02edb404 | 37 | for (int i = 0; i < 3; ++i) |
wanzq | 6:6e0f02edb404 | 38 | { |
wanzq | 6:6e0f02edb404 | 39 | mag[i] = payloadBuf[i*2]|((int)payloadBuf[i*2+1]<<8); |
wanzq | 6:6e0f02edb404 | 40 | } |
wanzq | 6:6e0f02edb404 | 41 | //pc.printf("Hx=%.2f\tHy=%.2f\tHz=%.2f\r\n", data[0], data[1], data[2]); |
wanzq | 6:6e0f02edb404 | 42 | break; |
wanzq | 6:6e0f02edb404 | 43 | } |
wanzq | 6:6e0f02edb404 | 44 | } |
wanzq | 6:6e0f02edb404 | 45 | void JY901::parseInput(const char* data, int len) |
wanzq | 6:6e0f02edb404 | 46 | { |
wanzq | 6:6e0f02edb404 | 47 | for (int i = 0; i < len; ++i) |
wanzq | 6:6e0f02edb404 | 48 | { |
wanzq | 6:6e0f02edb404 | 49 | unsigned char ch = data[i], sum; |
wanzq | 6:6e0f02edb404 | 50 | switch(state){ |
wanzq | 6:6e0f02edb404 | 51 | case 0: |
wanzq | 6:6e0f02edb404 | 52 | if(ch == 0x55) |
wanzq | 6:6e0f02edb404 | 53 | state = 1; |
wanzq | 6:6e0f02edb404 | 54 | break; |
wanzq | 6:6e0f02edb404 | 55 | case 1: |
wanzq | 6:6e0f02edb404 | 56 | token = ch; |
wanzq | 6:6e0f02edb404 | 57 | if(0x51 <= token && token <= 0x54){ |
wanzq | 6:6e0f02edb404 | 58 | payloadLen = 8; |
wanzq | 6:6e0f02edb404 | 59 | recvLen = 0; |
wanzq | 6:6e0f02edb404 | 60 | state = 2; |
wanzq | 6:6e0f02edb404 | 61 | }else{ |
wanzq | 6:6e0f02edb404 | 62 | // pc.printf("%s %x\r\n", "unknown token", token); |
wanzq | 6:6e0f02edb404 | 63 | state = 0; |
wanzq | 6:6e0f02edb404 | 64 | } |
wanzq | 6:6e0f02edb404 | 65 | break; |
wanzq | 6:6e0f02edb404 | 66 | case 2: |
wanzq | 6:6e0f02edb404 | 67 | payloadBuf[recvLen++] = ch; |
wanzq | 6:6e0f02edb404 | 68 | if(recvLen == payloadLen){ |
wanzq | 6:6e0f02edb404 | 69 | state = 3; |
wanzq | 6:6e0f02edb404 | 70 | } |
wanzq | 6:6e0f02edb404 | 71 | break; |
wanzq | 6:6e0f02edb404 | 72 | case 3: |
wanzq | 6:6e0f02edb404 | 73 | sum = 0x55; |
wanzq | 6:6e0f02edb404 | 74 | sum += token; |
wanzq | 6:6e0f02edb404 | 75 | for (int i = 0; i < payloadLen; ++i) |
wanzq | 6:6e0f02edb404 | 76 | { |
wanzq | 6:6e0f02edb404 | 77 | sum += payloadBuf[i]; |
wanzq | 6:6e0f02edb404 | 78 | } |
wanzq | 6:6e0f02edb404 | 79 | if(sum != ch){ |
wanzq | 6:6e0f02edb404 | 80 | // pc.printf("wrong checksum\r\n"); |
wanzq | 6:6e0f02edb404 | 81 | }else{ |
wanzq | 6:6e0f02edb404 | 82 | parseCmpt(token, payloadBuf, payloadLen); |
wanzq | 6:6e0f02edb404 | 83 | //myled = !myled; |
wanzq | 6:6e0f02edb404 | 84 | } |
wanzq | 6:6e0f02edb404 | 85 | state = 0; |
wanzq | 6:6e0f02edb404 | 86 | break; |
wanzq | 6:6e0f02edb404 | 87 | } |
wanzq | 6:6e0f02edb404 | 88 | } |
wanzq | 6:6e0f02edb404 | 89 | } |