Konkuk asml team / Mbed 2 deprecated GIMBAL_SAMPLE4_PPM

a

Dependencies:   mbed

RFDevCOMM/RFDevCOMM.cpp@1:dd6e70abeb8e, 2018-05-25 (annotated)

Committer:
halusis
Date:
Fri May 25 08:09:17 2018 +0000
Revision:
1:dd6e70abeb8e
Parent:
0:f0b20f502059 
a

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