a
Dependencies: mbed
Revision 1:dd6e70abeb8e, committed 2018-05-25
- Comitter:
- halusis
- Date:
- Fri May 25 08:09:17 2018 +0000
- Parent:
- 0:f0b20f502059
- Commit message:
- a
Changed in this revision
diff -r f0b20f502059 -r dd6e70abeb8e GET_ANGLE_FUNC/GET_ANGLE_FUNC.cpp --- a/GET_ANGLE_FUNC/GET_ANGLE_FUNC.cpp Thu May 24 10:19:44 2018 +0000 +++ b/GET_ANGLE_FUNC/GET_ANGLE_FUNC.cpp Fri May 25 08:09:17 2018 +0000 @@ -6,14 +6,6 @@ void CMD_GET_ANGLE(void) { - /* - Gimbal.putc(0x3e); //3e 49 01 4a 00 00 - Gimbal.putc(0x49); - Gimbal.putc(0x01); - Gimbal.putc(0x4a); - Gimbal.putc(0x00); - Gimbal.putc(0x00); - */ Gimbal.putc(0x3e); //3e 3d 01 3e 00 00 Gimbal.putc(0x3d); Gimbal.putc(0x01); @@ -26,44 +18,7 @@ { buf=Gimbal.getc(); //pc.putc(buf); - /* - if(buf==0x3e&&GimbalCnt==1){GimbalCnt++; GimbalBuf[GimbalCnt]=buf;} //3e 49 01 4a 00 00 - else if(buf==0x49&&GimbalCnt==2){GimbalCnt++; GimbalBuf[GimbalCnt]=buf;} - else if(GimbalCnt>=2&&GimbalCnt<=23) - { - GimbalCnt++; - GimbalBuf[GimbalCnt]=buf; - if(GimbalCnt==24) - { - BodyChks=0; - for(char i=5;i>22;i++) BodyChks+=GimbalBuf[i]; - } - GimbalCnt++; - } - else if(GimbalCnt==25&&BodyChks==GimbalBuf[23]) - { - //pc.putc(GimbalCnt); - - RollIMU =(GimbalBuf[5]<<8)|GimbalBuf[6]; - RollTarIMU =(GimbalBuf[7]<<8)|GimbalBuf[8]; - PitchIMU =(GimbalBuf[11]<<8)|GimbalBuf[12]; - PitchTarIMU =(GimbalBuf[13]<<8)|GimbalBuf[14]; - YawIMU =(GimbalBuf[17]<<8)|GimbalBuf[18]; - YawTarIMU =(GimbalBuf[19]<<8)|GimbalBuf[20]; - RollDeg =((double)RollIMU)*ANGLE_UNIT; - PitchDeg =((double)PitchIMU)*ANGLE_UNIT; - YawDeg =((double)YawIMU)*ANGLE_UNIT; - RollTar =((double)RollTarIMU)*ANGLE_UNIT; - PitchTar =((double)PitchTarIMU)*ANGLE_UNIT; - YawTar =((double)YawTarIMU)*ANGLE_UNIT; - - GimbalCnt=1; - - //pc.printf("a\n"); - } - else{GimbalCnt=1;} - */ if(buf==0x3e&&GimbalCnt==1){GimbalBuf[GimbalCnt]=buf;GimbalCnt++;} //3e 3d 01 3e 00 00 else if(buf==0x3d&&GimbalCnt==2){GimbalBuf[GimbalCnt]=buf;GimbalCnt++;} else if(GimbalCnt>=3&&GimbalCnt<=59) @@ -81,37 +36,35 @@ BodyChks+=GimbalBuf[i]; //pc.putc(BodyChks); } - - //pc.putc(BodyChks); - //pc.putc(GimbalBuf[59]); + if(BodyChks==GimbalBuf[59]) + { + + RollIMU =(GimbalBuf[6]<<8)|GimbalBuf[5]; + RollTarIMU =(GimbalBuf[8]<<8)|GimbalBuf[7]; + RollIMU2 =(GimbalBuf[12]<<24)|(GimbalBuf[11]<<16)|(GimbalBuf[10]<<8)|GimbalBuf[9]; + PitchIMU =(GimbalBuf[24]<<8)|GimbalBuf[23]; + PitchTarIMU =(GimbalBuf[26]<<8)|GimbalBuf[25]; + PitchIMU2 =(GimbalBuf[30]<<24)|(GimbalBuf[29]<<16)|(GimbalBuf[28]<<8)|GimbalBuf[27]; + YawIMU =(GimbalBuf[42]<<8)|GimbalBuf[41]; + YawTarIMU =(GimbalBuf[44]<<8)|GimbalBuf[43]; + YawIMU2 =(GimbalBuf[48]<<24)|(GimbalBuf[47]<<16)|(GimbalBuf[46]<<8)|GimbalBuf[45]; + + + RollDeg =((double)RollIMU)*ANGLE_UNIT; + PitchDeg =((double)PitchIMU)*ANGLE_UNIT; + YawDeg =((double)YawIMU)*ANGLE_UNIT; + RollTar =((double)RollTarIMU)*ANGLE_UNIT; + PitchTar =((double)PitchTarIMU)*ANGLE_UNIT; + YawTar =((double)YawTarIMU)*ANGLE_UNIT; + RollRel =((double)RollIMU2)*ANGLE_UNIT; + PitchRel =((double)PitchIMU2)*ANGLE_UNIT;//+180.0f; + YawRel =((double)YawIMU2)*ANGLE_UNIT; + + GimbalCnt=1; + } + else GimbalCnt=1; } - GimbalCnt++; + else GimbalCnt++; } - else if(GimbalCnt==60&&BodyChks==GimbalBuf[59]) - { - - RollIMU =(GimbalBuf[6]<<8)|GimbalBuf[5]; - RollTarIMU =(GimbalBuf[8]<<8)|GimbalBuf[7]; - RollIMU2 =(GimbalBuf[12]<<24)|(GimbalBuf[11]<<16)|(GimbalBuf[10]<<8)|GimbalBuf[9]; - PitchIMU =(GimbalBuf[24]<<8)|GimbalBuf[23]; - PitchTarIMU =(GimbalBuf[26]<<8)|GimbalBuf[25]; - PitchIMU2 =(GimbalBuf[30]<<24)|(GimbalBuf[29]<<16)|(GimbalBuf[28]<<8)|GimbalBuf[27]; - YawIMU =(GimbalBuf[42]<<8)|GimbalBuf[41]; - YawTarIMU =(GimbalBuf[44]<<8)|GimbalBuf[43]; - YawIMU2 =(GimbalBuf[48]<<24)|(GimbalBuf[47]<<16)|(GimbalBuf[46]<<8)|GimbalBuf[45]; - - - RollDeg =((double)RollIMU)*ANGLE_UNIT; - PitchDeg =((double)PitchIMU)*ANGLE_UNIT; - YawDeg =((double)YawIMU)*ANGLE_UNIT; - RollTar =((double)RollTarIMU)*ANGLE_UNIT; - PitchTar =((double)PitchTarIMU)*ANGLE_UNIT; - YawTar =((double)YawTarIMU)*ANGLE_UNIT; - RollRel =((double)RollIMU2)*ANGLE_UNIT; - PitchRel =((double)PitchIMU2)*ANGLE_UNIT;//+180.0f; - YawRel =((double)YawIMU2)*ANGLE_UNIT; - - GimbalCnt=1; - } else{GimbalCnt=1;} } \ No newline at end of file
diff -r f0b20f502059 -r dd6e70abeb8e GIMBAL_SAMPLE.cpp --- a/GIMBAL_SAMPLE.cpp Thu May 24 10:19:44 2018 +0000 +++ b/GIMBAL_SAMPLE.cpp Fri May 25 08:09:17 2018 +0000 @@ -138,13 +138,13 @@ //pc.printf("%d %d %d\n", (int)YawPWM, (int)PitchPWM, fLock); - pc.printf("%.0f %.0f %.0f %.0f %.0f %.0f %d\n",YawRel,PitchRel,YawT,PitchT,YawPWM,PitchPWM, fLock); + //pc.printf("%.0f %.0f %.0f %.0f %.0f %.0f %d\n",YawRel,PitchRel,YawT,PitchT,YawPWM,PitchPWM, fLock); //pc.printf("%.0f %.0f %.0f %d\n",YawRel,YawT,YawPWM, fLock); //pc.printf("%.0f %.0f %.0f %d\n",PitchRel,PitchT,PitchPWM, fLock); //pc.printf("%.0f %.0f %.1f %.1f %.1f %.1f %d\n",Azi,Ele,V1,V2,V3,V4, fLock); //pc.printf("%.0f %.0f %.1f %.1f %.1f %.1f %d\n",YawRel,PitchRel,V1,V2,V3,V4, fLock); //pc.printf("%.0f %.0f %.0f %.0f %d\n",YawRel,PitchRel,Azi,Ele, fLock); - //pc.printf("%.3f %.3f %.3f %.3f %.3f %.3f %d\n",YawRel,Azi,V1,V2,V3,V4, fLock); + pc.printf("%.3f %.3f %.3f %.3f %.3f %.3f %d\n",YawRel,Azi,V1,V2,V3,V4, fLock); RF2FCC();
diff -r f0b20f502059 -r dd6e70abeb8e RFDevCOMM/RFDevCOMM.cpp --- a/RFDevCOMM/RFDevCOMM.cpp Thu May 24 10:19:44 2018 +0000 +++ b/RFDevCOMM/RFDevCOMM.cpp Fri May 25 08:09:17 2018 +0000 @@ -7,12 +7,13 @@ void RF2FCC(void) { signed short yaw, azi; - signed short v2, v3; - unsigned char yawbuf[2],azibuf[2]; + //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; @@ -21,26 +22,32 @@ 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; + 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(RFBuf[8]); //V2 - Fcc.putc(RFBuf[7]); - Fcc.putc(RFBuf[10]); //V3 - Fcc.putc(RFBuf[9]); + + Fcc.putc(v2buf[0]); //V2 + Fcc.putc(v2buf[1]); + Fcc.putc(v3buf[0]); //V3 + Fcc.putc(v3buf[1]); Fcc.putc(0xff); Fcc.putc(0xff); @@ -57,45 +64,45 @@ { RFBuf[RFCnt]=RFbuf; - if(RFCnt==35) - { + if(RFCnt==35) + { RFBodyChks=0; - for(char i=5;i<=34;i++) + 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; } - 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++; } else{RFCnt=1;} }