File content as of revision 1:dd6e70abeb8e:

#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 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){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);
            }
            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;
        }
        else GimbalCnt++;
    }
    else{GimbalCnt=1;}
}