a
Dependencies: mbed
Diff: RFDevCOMM/RFDevCOMM.cpp
- Revision:
- 0:f0b20f502059
- Child:
- 1:dd6e70abeb8e
diff -r 000000000000 -r f0b20f502059 RFDevCOMM/RFDevCOMM.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/RFDevCOMM/RFDevCOMM.cpp Thu May 24 10:19:44 2018 +0000 @@ -0,0 +1,110 @@ +#include "mbed.h" +#include "Pin_Assign.h" +#include "Global_Variables.h" + +uint8_t RFbuf; + +void RF2FCC(void) +{ + signed short yaw, azi; + signed short v2, v3; + unsigned char yawbuf[2],azibuf[2]; + + Fcc.putc(0xfe); + Fcc.putc(0xfe); + + //-------dummy---------- + v2=22; + RFBuf[8]=v2>>8; + RFBuf[7]=v2; + v3=33; + RFBuf[10]=v3>>8; + RFBuf[9]=v3; + //---------------------- + + azi=(short)(Azi*100); + if(azi<0) azi=36000+azi; + azibuf[0]=azi>>8; + azibuf[1]=azi; + + Fcc.putc(azibuf[0]); //Azi + Fcc.putc(azibuf[1]); + + yaw=(short)(YawRel*100); + if(yaw<0) yaw=36000+yaw; + yawbuf[0]=yaw>>8; + yawbuf[1]=yaw; + + Fcc.putc(yawbuf[0]); //Yaw + Fcc.putc(yawbuf[1]); + Fcc.putc(RFBuf[8]); //V2 + Fcc.putc(RFBuf[7]); + Fcc.putc(RFBuf[10]); //V3 + Fcc.putc(RFBuf[9]); + + Fcc.putc(0xff); + Fcc.putc(0xff); +} + +void RcvRFCMD(void) +{ + RFbuf=RF.getc(); + // pc.putc(RFbuf); + + if(RFbuf==0x3e&&RFCnt==1){RFBuf[RFCnt]=RFbuf;RFCnt++;} + else if(RFbuf==0x40&&RFCnt==2){RFBuf[RFCnt]=RFbuf;RFCnt++;} + else if(RFCnt>=3&&RFCnt<=35) + { + RFBuf[RFCnt]=RFbuf; + + if(RFCnt==35) + { + RFBodyChks=0; + for(char i=5;i<=34;i++) + { + RFBodyChks+=RFBuf[i]; + } + //pc.printf("%d %d %d\n",RFCnt,RFBodyChks,RFBuf[35]); + } + RFCnt++; + } + else if(RFCnt==36&&RFBodyChks==RFBuf[35]) + { + rV1 =(RFBuf[6]<<8)|RFBuf[5]; + rV2 =(RFBuf[8]<<8)|RFBuf[7]; + rV3 =(RFBuf[10]<<8)|RFBuf[9]; + rV4 =(RFBuf[12]<<8)|RFBuf[11]; + rEle =(RFBuf[14]<<8)|RFBuf[13]; + rAzi =(RFBuf[16]<<8)|RFBuf[15]; + rEle_raw =(RFBuf[18]<<8)|RFBuf[17]; + rAzi_raw =(RFBuf[20]<<8)|RFBuf[19]; + rEle_Offset =(RFBuf[22]<<8)|RFBuf[21]; + rAzi_Offset =(RFBuf[24]<<8)|RFBuf[23]; + + + V1 =(float)rV1*0.001f; + V2 =(float)rV2*0.001f; + V3 =(float)rV3*0.001f; + V4 =(float)rV4*0.001f; + Ele =(float)rEle*0.01f; + Azi =(float)rAzi*0.01f; + Ele_raw =(float)rEle_raw*0.01f; + Azi_raw =(float)rAzi_raw*0.01f; + Ele_Offset =(float)rEle_Offset*0.001f; + Azi_Offset =(float)rAzi_Offset*0.001f; + + RF2FCC(); + RFCnt=1; + fRFread=1; + } + else{RFCnt=1;} +} + + +void SndRFCMD(void) +{ + RF.putc(0x3e); + RF.putc(0x80); + RF.putc(0x00); + RF.putc(0x80); +} \ No newline at end of file