Konkuk asml team / Mbed 2 deprecated GIMBAL_SAMPLE4_PPM

a

Dependencies:   mbed

RFDevCOMM/RFDevCOMM.cpp@0:f0b20f502059, 2018-05-24 (annotated)

Committer:
halusis
Date:
Thu May 24 10:19:44 2018 +0000
Revision:
0:f0b20f502059
Child:
1:dd6e70abeb8e
AA

Who changed what in which revision?

UserRevisionLine numberNew contents of line
halusis 0:f0b20f502059 1 #include "mbed.h"
halusis 0:f0b20f502059 2 #include "Pin_Assign.h"
halusis 0:f0b20f502059 3 #include "Global_Variables.h"
halusis 0:f0b20f502059 4
halusis 0:f0b20f502059 5 uint8_t RFbuf;
halusis 0:f0b20f502059 6
halusis 0:f0b20f502059 7 void RF2FCC(void)
halusis 0:f0b20f502059 8 {
halusis 0:f0b20f502059 9 signed short yaw, azi;
halusis 0:f0b20f502059 10 signed short v2, v3;
halusis 0:f0b20f502059 11 unsigned char yawbuf[2],azibuf[2];
halusis 0:f0b20f502059 12
halusis 0:f0b20f502059 13 Fcc.putc(0xfe);
halusis 0:f0b20f502059 14 Fcc.putc(0xfe);
halusis 0:f0b20f502059 15
halusis 0:f0b20f502059 16 //-------dummy----------
halusis 0:f0b20f502059 17 v2=22;
halusis 0:f0b20f502059 18 RFBuf[8]=v2>>8;
halusis 0:f0b20f502059 19 RFBuf[7]=v2;
halusis 0:f0b20f502059 20 v3=33;
halusis 0:f0b20f502059 21 RFBuf[10]=v3>>8;
halusis 0:f0b20f502059 22 RFBuf[9]=v3;
halusis 0:f0b20f502059 23 //----------------------
halusis 0:f0b20f502059 24
halusis 0:f0b20f502059 25 azi=(short)(Azi*100);
halusis 0:f0b20f502059 26 if(azi<0) azi=36000+azi;
halusis 0:f0b20f502059 27 azibuf[0]=azi>>8;
halusis 0:f0b20f502059 28 azibuf[1]=azi;
halusis 0:f0b20f502059 29
halusis 0:f0b20f502059 30 Fcc.putc(azibuf[0]); //Azi
halusis 0:f0b20f502059 31 Fcc.putc(azibuf[1]);
halusis 0:f0b20f502059 32
halusis 0:f0b20f502059 33 yaw=(short)(YawRel*100);
halusis 0:f0b20f502059 34 if(yaw<0) yaw=36000+yaw;
halusis 0:f0b20f502059 35 yawbuf[0]=yaw>>8;
halusis 0:f0b20f502059 36 yawbuf[1]=yaw;
halusis 0:f0b20f502059 37
halusis 0:f0b20f502059 38 Fcc.putc(yawbuf[0]); //Yaw
halusis 0:f0b20f502059 39 Fcc.putc(yawbuf[1]);
halusis 0:f0b20f502059 40 Fcc.putc(RFBuf[8]); //V2
halusis 0:f0b20f502059 41 Fcc.putc(RFBuf[7]);
halusis 0:f0b20f502059 42 Fcc.putc(RFBuf[10]); //V3
halusis 0:f0b20f502059 43 Fcc.putc(RFBuf[9]);
halusis 0:f0b20f502059 44
halusis 0:f0b20f502059 45 Fcc.putc(0xff);
halusis 0:f0b20f502059 46 Fcc.putc(0xff);
halusis 0:f0b20f502059 47 }
halusis 0:f0b20f502059 48
halusis 0:f0b20f502059 49 void RcvRFCMD(void)
halusis 0:f0b20f502059 50 {
halusis 0:f0b20f502059 51 RFbuf=RF.getc();
halusis 0:f0b20f502059 52 // pc.putc(RFbuf);
halusis 0:f0b20f502059 53
halusis 0:f0b20f502059 54 if(RFbuf==0x3e&&RFCnt==1){RFBuf[RFCnt]=RFbuf;RFCnt++;}
halusis 0:f0b20f502059 55 else if(RFbuf==0x40&&RFCnt==2){RFBuf[RFCnt]=RFbuf;RFCnt++;}
halusis 0:f0b20f502059 56 else if(RFCnt>=3&&RFCnt<=35)
halusis 0:f0b20f502059 57 {
halusis 0:f0b20f502059 58 RFBuf[RFCnt]=RFbuf;
halusis 0:f0b20f502059 59
halusis 0:f0b20f502059 60 if(RFCnt==35)
halusis 0:f0b20f502059 61 {
halusis 0:f0b20f502059 62 RFBodyChks=0;
halusis 0:f0b20f502059 63 for(char i=5;i<=34;i++)
halusis 0:f0b20f502059 64 {
halusis 0:f0b20f502059 65 RFBodyChks+=RFBuf[i];
halusis 0:f0b20f502059 66 }
halusis 0:f0b20f502059 67 //pc.printf("%d %d %d\n",RFCnt,RFBodyChks,RFBuf[35]);
halusis 0:f0b20f502059 68 }
halusis 0:f0b20f502059 69 RFCnt++;
halusis 0:f0b20f502059 70 }
halusis 0:f0b20f502059 71 else if(RFCnt==36&&RFBodyChks==RFBuf[35])
halusis 0:f0b20f502059 72 {
halusis 0:f0b20f502059 73 rV1 =(RFBuf[6]<<8)|RFBuf[5];
halusis 0:f0b20f502059 74 rV2 =(RFBuf[8]<<8)|RFBuf[7];
halusis 0:f0b20f502059 75 rV3 =(RFBuf[10]<<8)|RFBuf[9];
halusis 0:f0b20f502059 76 rV4 =(RFBuf[12]<<8)|RFBuf[11];
halusis 0:f0b20f502059 77 rEle =(RFBuf[14]<<8)|RFBuf[13];
halusis 0:f0b20f502059 78 rAzi =(RFBuf[16]<<8)|RFBuf[15];
halusis 0:f0b20f502059 79 rEle_raw =(RFBuf[18]<<8)|RFBuf[17];
halusis 0:f0b20f502059 80 rAzi_raw =(RFBuf[20]<<8)|RFBuf[19];
halusis 0:f0b20f502059 81 rEle_Offset =(RFBuf[22]<<8)|RFBuf[21];
halusis 0:f0b20f502059 82 rAzi_Offset =(RFBuf[24]<<8)|RFBuf[23];
halusis 0:f0b20f502059 83
halusis 0:f0b20f502059 84
halusis 0:f0b20f502059 85 V1 =(float)rV1*0.001f;
halusis 0:f0b20f502059 86 V2 =(float)rV2*0.001f;
halusis 0:f0b20f502059 87 V3 =(float)rV3*0.001f;
halusis 0:f0b20f502059 88 V4 =(float)rV4*0.001f;
halusis 0:f0b20f502059 89 Ele =(float)rEle*0.01f;
halusis 0:f0b20f502059 90 Azi =(float)rAzi*0.01f;
halusis 0:f0b20f502059 91 Ele_raw =(float)rEle_raw*0.01f;
halusis 0:f0b20f502059 92 Azi_raw =(float)rAzi_raw*0.01f;
halusis 0:f0b20f502059 93 Ele_Offset =(float)rEle_Offset*0.001f;
halusis 0:f0b20f502059 94 Azi_Offset =(float)rAzi_Offset*0.001f;
halusis 0:f0b20f502059 95
halusis 0:f0b20f502059 96 RF2FCC();
halusis 0:f0b20f502059 97 RFCnt=1;
halusis 0:f0b20f502059 98 fRFread=1;
halusis 0:f0b20f502059 99 }
halusis 0:f0b20f502059 100 else{RFCnt=1;}
halusis 0:f0b20f502059 101 }
halusis 0:f0b20f502059 102
halusis 0:f0b20f502059 103
halusis 0:f0b20f502059 104 void SndRFCMD(void)
halusis 0:f0b20f502059 105 {
halusis 0:f0b20f502059 106 RF.putc(0x3e);
halusis 0:f0b20f502059 107 RF.putc(0x80);
halusis 0:f0b20f502059 108 RF.putc(0x00);
halusis 0:f0b20f502059 109 RF.putc(0x80);
halusis 0:f0b20f502059 110 }