a
Dependencies: mbed
Diff: RFDevCOMM/RFDevCOMM.cpp
- Revision:
- 1:dd6e70abeb8e
- Parent:
- 0:f0b20f502059
diff -r f0b20f502059 -r dd6e70abeb8e RFDevCOMM/RFDevCOMM.cpp --- a/RFDevCOMM/RFDevCOMM.cpp Thu May 24 10:19:44 2018 +0000 +++ b/RFDevCOMM/RFDevCOMM.cpp Fri May 25 08:09:17 2018 +0000 @@ -7,12 +7,13 @@ void RF2FCC(void) { signed short yaw, azi; - signed short v2, v3; - unsigned char yawbuf[2],azibuf[2]; + //signed short v2, v3; + unsigned char yawbuf[2],azibuf[2],v2buf[2],v3buf[2]; Fcc.putc(0xfe); Fcc.putc(0xfe); + /* //-------dummy---------- v2=22; RFBuf[8]=v2>>8; @@ -21,26 +22,32 @@ 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; + v2buf[0]=rV2>>8; + v2buf[1]=rV2; + v3buf[0]=rV3>>8; + v3buf[1]=rV3; + + Fcc.putc(azibuf[0]); //Azi + Fcc.putc(azibuf[1]); 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(v2buf[0]); //V2 + Fcc.putc(v2buf[1]); + Fcc.putc(v3buf[0]); //V3 + Fcc.putc(v3buf[1]); Fcc.putc(0xff); Fcc.putc(0xff); @@ -57,45 +64,45 @@ { RFBuf[RFCnt]=RFbuf; - if(RFCnt==35) - { + if(RFCnt==35) + { RFBodyChks=0; - for(char i=5;i<=34;i++) + for(char i=5; i<=34; i++) { RFBodyChks+=RFBuf[i]; } //pc.printf("%d %d %d\n",RFCnt,RFBodyChks,RFBuf[35]); + if(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; } - 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++; } else{RFCnt=1;} }