aurora moon / Mbed 2 deprecated send_copy

1223

Dependencies:   BufferedSerial SoftPWM mbed send

Fork of nRF24L01P_Hello_World by YX ZHANG

JY901.cpp@4:876bfa91934c, 2017-11-08 (annotated)

Committer:
accelerator225
Date:
Wed Nov 08 06:24:17 2017 +0000
Revision:
4:876bfa91934c
1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
accelerator225 4:876bfa91934c 1
accelerator225 4:876bfa91934c 2 #include "JY901.h"
accelerator225 4:876bfa91934c 3 void JY901::receiveData()
accelerator225 4:876bfa91934c 4 {
accelerator225 4:876bfa91934c 5 char ch;
accelerator225 4:876bfa91934c 6 while(mod.readable()){
accelerator225 4:876bfa91934c 7 ch = mod.getc();
accelerator225 4:876bfa91934c 8 parseInput(&ch, 1);
accelerator225 4:876bfa91934c 9 }
accelerator225 4:876bfa91934c 10 }
accelerator225 4:876bfa91934c 11 void JY901::parseCmpt(int token, unsigned char* payloadBuf, int payloadLen)
accelerator225 4:876bfa91934c 12 {
accelerator225 4:876bfa91934c 13 switch(token){
accelerator225 4:876bfa91934c 14 case 0x51:
accelerator225 4:876bfa91934c 15 for (int i = 0; i < 3; ++i)
accelerator225 4:876bfa91934c 16 {
accelerator225 4:876bfa91934c 17 acc[i] = payloadBuf[i*2]|((int)payloadBuf[i*2+1]<<8);
accelerator225 4:876bfa91934c 18 acc[i] = acc[i] * 16 * 9.8 / 32768;
accelerator225 4:876bfa91934c 19 }
accelerator225 4:876bfa91934c 20 // lcd.cls();
accelerator225 4:876bfa91934c 21 // lcd.printf("Ax=%d\n",int(data[0]*100));
accelerator225 4:876bfa91934c 22 //pc.printf("Ax=%.2f\tAy=%.2f\tAz=%.2f\r\n", data[0], data[1], data[2]);
accelerator225 4:876bfa91934c 23 break;
accelerator225 4:876bfa91934c 24 case 0x52:
accelerator225 4:876bfa91934c 25 for (int i = 0; i < 3; ++i)
accelerator225 4:876bfa91934c 26 {
accelerator225 4:876bfa91934c 27 gyo[i] = payloadBuf[i*2]|((int)payloadBuf[i*2+1]<<8);
accelerator225 4:876bfa91934c 28 gyo[i] = gyo[i] * 2000 / 32768;
accelerator225 4:876bfa91934c 29 }
accelerator225 4:876bfa91934c 30 //pc.printf("Wx=%.2f\tWy=%.2f\tWz=%.2f\r\n", data[0], data[1], data[2]);
accelerator225 4:876bfa91934c 31 break;
accelerator225 4:876bfa91934c 32 case 0x53:
accelerator225 4:876bfa91934c 33 for (int i = 0; i < 3; ++i)
accelerator225 4:876bfa91934c 34 {
accelerator225 4:876bfa91934c 35 att[i] = payloadBuf[i*2]|((int)payloadBuf[i*2+1]<<8);
accelerator225 4:876bfa91934c 36 att[i] = att[i] * 180 / 32768;
accelerator225 4:876bfa91934c 37 }
accelerator225 4:876bfa91934c 38 // pc.printf("Roll=%.2f\tPitch=%.2f\tYaw=%.2f\r\n", data[0], data[1], data[2]);
accelerator225 4:876bfa91934c 39 break;
accelerator225 4:876bfa91934c 40 case 0x54:
accelerator225 4:876bfa91934c 41 for (int i = 0; i < 3; ++i)
accelerator225 4:876bfa91934c 42 {
accelerator225 4:876bfa91934c 43 mag[i] = payloadBuf[i*2]|((int)payloadBuf[i*2+1]<<8);
accelerator225 4:876bfa91934c 44 }
accelerator225 4:876bfa91934c 45 //pc.printf("Hx=%.2f\tHy=%.2f\tHz=%.2f\r\n", data[0], data[1], data[2]);
accelerator225 4:876bfa91934c 46 break;
accelerator225 4:876bfa91934c 47 }
accelerator225 4:876bfa91934c 48 }
accelerator225 4:876bfa91934c 49 void JY901::parseInput(const char* data, int len)
accelerator225 4:876bfa91934c 50 {
accelerator225 4:876bfa91934c 51 for (int i = 0; i < len; ++i)
accelerator225 4:876bfa91934c 52 {
accelerator225 4:876bfa91934c 53 unsigned char ch = data[i], sum;
accelerator225 4:876bfa91934c 54 switch(state){
accelerator225 4:876bfa91934c 55 case 0:
accelerator225 4:876bfa91934c 56 if(ch == 0x55)
accelerator225 4:876bfa91934c 57 state = 1;
accelerator225 4:876bfa91934c 58 break;
accelerator225 4:876bfa91934c 59 case 1:
accelerator225 4:876bfa91934c 60 token = ch;
accelerator225 4:876bfa91934c 61 if(0x51 <= token && token <= 0x54){
accelerator225 4:876bfa91934c 62 payloadLen = 8;
accelerator225 4:876bfa91934c 63 recvLen = 0;
accelerator225 4:876bfa91934c 64 state = 2;
accelerator225 4:876bfa91934c 65 }else{
accelerator225 4:876bfa91934c 66 // pc.printf("%s %x\r\n", "unknown token", token);
accelerator225 4:876bfa91934c 67 state = 0;
accelerator225 4:876bfa91934c 68 }
accelerator225 4:876bfa91934c 69 break;
accelerator225 4:876bfa91934c 70 case 2:
accelerator225 4:876bfa91934c 71 payloadBuf[recvLen++] = ch;
accelerator225 4:876bfa91934c 72 if(recvLen == payloadLen){
accelerator225 4:876bfa91934c 73 state = 3;
accelerator225 4:876bfa91934c 74 }
accelerator225 4:876bfa91934c 75 break;
accelerator225 4:876bfa91934c 76 case 3:
accelerator225 4:876bfa91934c 77 sum = 0x55;
accelerator225 4:876bfa91934c 78 sum += token;
accelerator225 4:876bfa91934c 79 for (int i = 0; i < payloadLen; ++i)
accelerator225 4:876bfa91934c 80 {
accelerator225 4:876bfa91934c 81 sum += payloadBuf[i];
accelerator225 4:876bfa91934c 82 }
accelerator225 4:876bfa91934c 83 if(sum != ch){
accelerator225 4:876bfa91934c 84 // pc.printf("wrong checksum\r\n");
accelerator225 4:876bfa91934c 85 }else{
accelerator225 4:876bfa91934c 86 parseCmpt(token, payloadBuf, payloadLen);
accelerator225 4:876bfa91934c 87 //myled = !myled;
accelerator225 4:876bfa91934c 88 }
accelerator225 4:876bfa91934c 89 state = 0;
accelerator225 4:876bfa91934c 90 break;
accelerator225 4:876bfa91934c 91 }
accelerator225 4:876bfa91934c 92 }
accelerator225 4:876bfa91934c 93 }