Diff: GET_ANGLE_FUNC/GET_ANGLE_FUNC.cpp

Revision:

0:f0b20f502059

Child:

1:dd6e70abeb8e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/GET_ANGLE_FUNC/GET_ANGLE_FUNC.cpp	Thu May 24 10:19:44 2018 +0000
@@ -0,0 +1,117 @@
+#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;}
+}
\ No newline at end of file