File content as of revision 1:dd6e70abeb8e:

#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],v2buf[2],v3buf[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;
    
    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(v2buf[0]);         //V2
    Fcc.putc(v2buf[1]);
    Fcc.putc(v3buf[0]);         //V3
    Fcc.putc(v3buf[1]);
    
    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]);
            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;
        }
        else RFCnt++;
    }    
    else{RFCnt=1;}
}


void SndRFCMD(void)
{
    RF.putc(0x3e);
    RF.putc(0x80);
    RF.putc(0x00);
    RF.putc(0x80);
}