a
Dependencies: mbed
GET_ANGLE_FUNC/GET_ANGLE_FUNC.cpp
- Committer:
- halusis
- Date:
- 2018-05-24
- Revision:
- 0:f0b20f502059
- Child:
- 1:dd6e70abeb8e
File content as of revision 0:f0b20f502059:
#include "GET_ANGLE_FUNC.h" #include "Global_Variables.h" #include "Pin_Assign.h" uint8_t buf; 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); Gimbal.putc(0x3e); Gimbal.putc(0x00); Gimbal.putc(0x00); } void GET_ANGLE_FUNC(void) { 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) { GimbalBuf[GimbalCnt]=buf; //pc.putc(GimbalCnt); if(GimbalCnt==59) { //pc.putc(GimbalCnt); BodyChks=0; for(char i=5;i<=58;i++) { BodyChks+=GimbalBuf[i]; //pc.putc(BodyChks); } //pc.putc(BodyChks); //pc.putc(GimbalBuf[59]); } 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;} }