Konkuk asml team / Mbed 2 deprecated GIMBAL_SAMPLE4_PPM

a

Dependencies:   mbed

GET_ANGLE_FUNC/GET_ANGLE_FUNC.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 "GET_ANGLE_FUNC.h"
halusis 0:f0b20f502059 2 #include "Global_Variables.h"
halusis 0:f0b20f502059 3 #include "Pin_Assign.h"
halusis 0:f0b20f502059 4
halusis 0:f0b20f502059 5 uint8_t buf;
halusis 0:f0b20f502059 6
halusis 0:f0b20f502059 7 void CMD_GET_ANGLE(void)
halusis 0:f0b20f502059 8 {
halusis 0:f0b20f502059 9 Gimbal.putc(0x3e); //3e 3d 01 3e 00 00
halusis 0:f0b20f502059 10 Gimbal.putc(0x3d);
halusis 0:f0b20f502059 11 Gimbal.putc(0x01);
halusis 0:f0b20f502059 12 Gimbal.putc(0x3e);
halusis 0:f0b20f502059 13 Gimbal.putc(0x00);
halusis 0:f0b20f502059 14 Gimbal.putc(0x00);
halusis 0:f0b20f502059 15 }
halusis 0:f0b20f502059 16
halusis 0:f0b20f502059 17 void GET_ANGLE_FUNC(void)
halusis 0:f0b20f502059 18 {
halusis 0:f0b20f502059 19 buf=Gimbal.getc();
halusis 0:f0b20f502059 20 //pc.putc(buf);
halusis 0:f0b20f502059 21
halusis 0:f0b20f502059 22 if(buf==0x3e&&GimbalCnt==1){GimbalBuf[GimbalCnt]=buf;GimbalCnt++;} //3e 3d 01 3e 00 00
halusis 0:f0b20f502059 23 else if(buf==0x3d&&GimbalCnt==2){GimbalBuf[GimbalCnt]=buf;GimbalCnt++;}
halusis 0:f0b20f502059 24 else if(GimbalCnt>=3&&GimbalCnt<=59)
halusis 0:f0b20f502059 25 {
halusis 0:f0b20f502059 26 GimbalBuf[GimbalCnt]=buf;
halusis 0:f0b20f502059 27
halusis 0:f0b20f502059 28 //pc.putc(GimbalCnt);
halusis 0:f0b20f502059 29 if(GimbalCnt==59)
halusis 0:f0b20f502059 30 {
halusis 0:f0b20f502059 31 //pc.putc(GimbalCnt);
halusis 0:f0b20f502059 32
halusis 0:f0b20f502059 33 BodyChks=0;
halusis 0:f0b20f502059 34 for(char i=5;i<=58;i++)
halusis 0:f0b20f502059 35 {
halusis 0:f0b20f502059 36 BodyChks+=GimbalBuf[i];
halusis 0:f0b20f502059 37 //pc.putc(BodyChks);
halusis 0:f0b20f502059 38 }
halusis 1:dd6e70abeb8e 39 if(BodyChks==GimbalBuf[59])
halusis 1:dd6e70abeb8e 40 {
halusis 1:dd6e70abeb8e 41
halusis 1:dd6e70abeb8e 42 RollIMU =(GimbalBuf[6]<<8)|GimbalBuf[5];
halusis 1:dd6e70abeb8e 43 RollTarIMU =(GimbalBuf[8]<<8)|GimbalBuf[7];
halusis 1:dd6e70abeb8e 44 RollIMU2 =(GimbalBuf[12]<<24)|(GimbalBuf[11]<<16)|(GimbalBuf[10]<<8)|GimbalBuf[9];
halusis 1:dd6e70abeb8e 45 PitchIMU =(GimbalBuf[24]<<8)|GimbalBuf[23];
halusis 1:dd6e70abeb8e 46 PitchTarIMU =(GimbalBuf[26]<<8)|GimbalBuf[25];
halusis 1:dd6e70abeb8e 47 PitchIMU2 =(GimbalBuf[30]<<24)|(GimbalBuf[29]<<16)|(GimbalBuf[28]<<8)|GimbalBuf[27];
halusis 1:dd6e70abeb8e 48 YawIMU =(GimbalBuf[42]<<8)|GimbalBuf[41];
halusis 1:dd6e70abeb8e 49 YawTarIMU =(GimbalBuf[44]<<8)|GimbalBuf[43];
halusis 1:dd6e70abeb8e 50 YawIMU2 =(GimbalBuf[48]<<24)|(GimbalBuf[47]<<16)|(GimbalBuf[46]<<8)|GimbalBuf[45];
halusis 1:dd6e70abeb8e 51
halusis 1:dd6e70abeb8e 52
halusis 1:dd6e70abeb8e 53 RollDeg =((double)RollIMU)*ANGLE_UNIT;
halusis 1:dd6e70abeb8e 54 PitchDeg =((double)PitchIMU)*ANGLE_UNIT;
halusis 1:dd6e70abeb8e 55 YawDeg =((double)YawIMU)*ANGLE_UNIT;
halusis 1:dd6e70abeb8e 56 RollTar =((double)RollTarIMU)*ANGLE_UNIT;
halusis 1:dd6e70abeb8e 57 PitchTar =((double)PitchTarIMU)*ANGLE_UNIT;
halusis 1:dd6e70abeb8e 58 YawTar =((double)YawTarIMU)*ANGLE_UNIT;
halusis 1:dd6e70abeb8e 59 RollRel =((double)RollIMU2)*ANGLE_UNIT;
halusis 1:dd6e70abeb8e 60 PitchRel =((double)PitchIMU2)*ANGLE_UNIT;//+180.0f;
halusis 1:dd6e70abeb8e 61 YawRel =((double)YawIMU2)*ANGLE_UNIT;
halusis 1:dd6e70abeb8e 62
halusis 1:dd6e70abeb8e 63 GimbalCnt=1;
halusis 1:dd6e70abeb8e 64 }
halusis 1:dd6e70abeb8e 65 else GimbalCnt=1;
halusis 0:f0b20f502059 66 }
halusis 1:dd6e70abeb8e 67 else GimbalCnt++;
halusis 0:f0b20f502059 68 }
halusis 0:f0b20f502059 69 else{GimbalCnt=1;}
halusis 0:f0b20f502059 70 }