Konkuk asml team / Mbed 2 deprecated GIMBAL_SAMPLE4_PPM

a

Dependencies:   mbed

GET_ANGLE_FUNC/GET_ANGLE_FUNC.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?

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 /*
halusis 0:f0b20f502059 10 Gimbal.putc(0x3e); //3e 49 01 4a 00 00
halusis 0:f0b20f502059 11 Gimbal.putc(0x49);
halusis 0:f0b20f502059 12 Gimbal.putc(0x01);
halusis 0:f0b20f502059 13 Gimbal.putc(0x4a);
halusis 0:f0b20f502059 14 Gimbal.putc(0x00);
halusis 0:f0b20f502059 15 Gimbal.putc(0x00);
halusis 0:f0b20f502059 16 */
halusis 0:f0b20f502059 17 Gimbal.putc(0x3e); //3e 3d 01 3e 00 00
halusis 0:f0b20f502059 18 Gimbal.putc(0x3d);
halusis 0:f0b20f502059 19 Gimbal.putc(0x01);
halusis 0:f0b20f502059 20 Gimbal.putc(0x3e);
halusis 0:f0b20f502059 21 Gimbal.putc(0x00);
halusis 0:f0b20f502059 22 Gimbal.putc(0x00);
halusis 0:f0b20f502059 23 }
halusis 0:f0b20f502059 24
halusis 0:f0b20f502059 25 void GET_ANGLE_FUNC(void)
halusis 0:f0b20f502059 26 {
halusis 0:f0b20f502059 27 buf=Gimbal.getc();
halusis 0:f0b20f502059 28 //pc.putc(buf);
halusis 0:f0b20f502059 29 /*
halusis 0:f0b20f502059 30 if(buf==0x3e&&GimbalCnt==1){GimbalCnt++; GimbalBuf[GimbalCnt]=buf;} //3e 49 01 4a 00 00
halusis 0:f0b20f502059 31 else if(buf==0x49&&GimbalCnt==2){GimbalCnt++; GimbalBuf[GimbalCnt]=buf;}
halusis 0:f0b20f502059 32 else if(GimbalCnt>=2&&GimbalCnt<=23)
halusis 0:f0b20f502059 33 {
halusis 0:f0b20f502059 34 GimbalCnt++;
halusis 0:f0b20f502059 35 GimbalBuf[GimbalCnt]=buf;
halusis 0:f0b20f502059 36 if(GimbalCnt==24)
halusis 0:f0b20f502059 37 {
halusis 0:f0b20f502059 38 BodyChks=0;
halusis 0:f0b20f502059 39 for(char i=5;i>22;i++) BodyChks+=GimbalBuf[i];
halusis 0:f0b20f502059 40 }
halusis 0:f0b20f502059 41 GimbalCnt++;
halusis 0:f0b20f502059 42 }
halusis 0:f0b20f502059 43 else if(GimbalCnt==25&&BodyChks==GimbalBuf[23])
halusis 0:f0b20f502059 44 {
halusis 0:f0b20f502059 45 //pc.putc(GimbalCnt);
halusis 0:f0b20f502059 46
halusis 0:f0b20f502059 47 RollIMU =(GimbalBuf[5]<<8)|GimbalBuf[6];
halusis 0:f0b20f502059 48 RollTarIMU =(GimbalBuf[7]<<8)|GimbalBuf[8];
halusis 0:f0b20f502059 49 PitchIMU =(GimbalBuf[11]<<8)|GimbalBuf[12];
halusis 0:f0b20f502059 50 PitchTarIMU =(GimbalBuf[13]<<8)|GimbalBuf[14];
halusis 0:f0b20f502059 51 YawIMU =(GimbalBuf[17]<<8)|GimbalBuf[18];
halusis 0:f0b20f502059 52 YawTarIMU =(GimbalBuf[19]<<8)|GimbalBuf[20];
halusis 0:f0b20f502059 53
halusis 0:f0b20f502059 54 RollDeg =((double)RollIMU)*ANGLE_UNIT;
halusis 0:f0b20f502059 55 PitchDeg =((double)PitchIMU)*ANGLE_UNIT;
halusis 0:f0b20f502059 56 YawDeg =((double)YawIMU)*ANGLE_UNIT;
halusis 0:f0b20f502059 57 RollTar =((double)RollTarIMU)*ANGLE_UNIT;
halusis 0:f0b20f502059 58 PitchTar =((double)PitchTarIMU)*ANGLE_UNIT;
halusis 0:f0b20f502059 59 YawTar =((double)YawTarIMU)*ANGLE_UNIT;
halusis 0:f0b20f502059 60
halusis 0:f0b20f502059 61 GimbalCnt=1;
halusis 0:f0b20f502059 62
halusis 0:f0b20f502059 63 //pc.printf("a\n");
halusis 0:f0b20f502059 64 }
halusis 0:f0b20f502059 65 else{GimbalCnt=1;}
halusis 0:f0b20f502059 66 */
halusis 0:f0b20f502059 67 if(buf==0x3e&&GimbalCnt==1){GimbalBuf[GimbalCnt]=buf;GimbalCnt++;} //3e 3d 01 3e 00 00
halusis 0:f0b20f502059 68 else if(buf==0x3d&&GimbalCnt==2){GimbalBuf[GimbalCnt]=buf;GimbalCnt++;}
halusis 0:f0b20f502059 69 else if(GimbalCnt>=3&&GimbalCnt<=59)
halusis 0:f0b20f502059 70 {
halusis 0:f0b20f502059 71 GimbalBuf[GimbalCnt]=buf;
halusis 0:f0b20f502059 72
halusis 0:f0b20f502059 73 //pc.putc(GimbalCnt);
halusis 0:f0b20f502059 74 if(GimbalCnt==59)
halusis 0:f0b20f502059 75 {
halusis 0:f0b20f502059 76 //pc.putc(GimbalCnt);
halusis 0:f0b20f502059 77
halusis 0:f0b20f502059 78 BodyChks=0;
halusis 0:f0b20f502059 79 for(char i=5;i<=58;i++)
halusis 0:f0b20f502059 80 {
halusis 0:f0b20f502059 81 BodyChks+=GimbalBuf[i];
halusis 0:f0b20f502059 82 //pc.putc(BodyChks);
halusis 0:f0b20f502059 83 }
halusis 0:f0b20f502059 84
halusis 0:f0b20f502059 85 //pc.putc(BodyChks);
halusis 0:f0b20f502059 86 //pc.putc(GimbalBuf[59]);
halusis 0:f0b20f502059 87 }
halusis 0:f0b20f502059 88 GimbalCnt++;
halusis 0:f0b20f502059 89 }
halusis 0:f0b20f502059 90 else if(GimbalCnt==60&&BodyChks==GimbalBuf[59])
halusis 0:f0b20f502059 91 {
halusis 0:f0b20f502059 92
halusis 0:f0b20f502059 93 RollIMU =(GimbalBuf[6]<<8)|GimbalBuf[5];
halusis 0:f0b20f502059 94 RollTarIMU =(GimbalBuf[8]<<8)|GimbalBuf[7];
halusis 0:f0b20f502059 95 RollIMU2 =(GimbalBuf[12]<<24)|(GimbalBuf[11]<<16)|(GimbalBuf[10]<<8)|GimbalBuf[9];
halusis 0:f0b20f502059 96 PitchIMU =(GimbalBuf[24]<<8)|GimbalBuf[23];
halusis 0:f0b20f502059 97 PitchTarIMU =(GimbalBuf[26]<<8)|GimbalBuf[25];
halusis 0:f0b20f502059 98 PitchIMU2 =(GimbalBuf[30]<<24)|(GimbalBuf[29]<<16)|(GimbalBuf[28]<<8)|GimbalBuf[27];
halusis 0:f0b20f502059 99 YawIMU =(GimbalBuf[42]<<8)|GimbalBuf[41];
halusis 0:f0b20f502059 100 YawTarIMU =(GimbalBuf[44]<<8)|GimbalBuf[43];
halusis 0:f0b20f502059 101 YawIMU2 =(GimbalBuf[48]<<24)|(GimbalBuf[47]<<16)|(GimbalBuf[46]<<8)|GimbalBuf[45];
halusis 0:f0b20f502059 102
halusis 0:f0b20f502059 103
halusis 0:f0b20f502059 104 RollDeg =((double)RollIMU)*ANGLE_UNIT;
halusis 0:f0b20f502059 105 PitchDeg =((double)PitchIMU)*ANGLE_UNIT;
halusis 0:f0b20f502059 106 YawDeg =((double)YawIMU)*ANGLE_UNIT;
halusis 0:f0b20f502059 107 RollTar =((double)RollTarIMU)*ANGLE_UNIT;
halusis 0:f0b20f502059 108 PitchTar =((double)PitchTarIMU)*ANGLE_UNIT;
halusis 0:f0b20f502059 109 YawTar =((double)YawTarIMU)*ANGLE_UNIT;
halusis 0:f0b20f502059 110 RollRel =((double)RollIMU2)*ANGLE_UNIT;
halusis 0:f0b20f502059 111 PitchRel =((double)PitchIMU2)*ANGLE_UNIT;//+180.0f;
halusis 0:f0b20f502059 112 YawRel =((double)YawIMU2)*ANGLE_UNIT;
halusis 0:f0b20f502059 113
halusis 0:f0b20f502059 114 GimbalCnt=1;
halusis 0:f0b20f502059 115 }
halusis 0:f0b20f502059 116 else{GimbalCnt=1;}
halusis 0:f0b20f502059 117 }