wan zhouquan / Mbed 2 deprecated fly_v1

fly_v1

Dependencies:   BufferedSerial SoftPWM mbed send

Fork of withbufferserial_nrftst by aurora moon

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?

UserRevisionLine numberNew 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 }