a

Dependencies:   mbed

Revision:
0:f0b20f502059
Child:
1:dd6e70abeb8e
--- /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