File content as of revision 0:f0b20f502059:

#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);
}