a
Dependencies: mbed
Diff: GET_ANGLE_FUNC/GET_ANGLE_FUNC.cpp
- Revision:
- 1:dd6e70abeb8e
- Parent:
- 0:f0b20f502059
--- 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