a

Dependencies:   mbed

Files at this revision

API Documentation at this revision

Comitter:
halusis
Date:
Fri May 25 08:09:17 2018 +0000
Parent:
0:f0b20f502059
Commit message:
a

Changed in this revision

GET_ANGLE_FUNC/GET_ANGLE_FUNC.cpp Show annotated file Show diff for this revision Revisions of this file
GIMBAL_SAMPLE.cpp Show annotated file Show diff for this revision Revisions of this file
RFDevCOMM/RFDevCOMM.cpp Show annotated file Show diff for this revision Revisions of this file
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;}
 }