Konkuk asml team
a
Dependencies: mbed
Fork of
GIMBAL_SAMPLE4_PPM
by 
Konkuk asml team
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;}
}