![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
a
Dependencies: mbed
Fork of GIMBAL_SAMPLE4_PPM by
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?
User | Revision | Line number | New 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 | } |