#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <pthread.h>

#include "version_choose.h"
#include "Gimbal_H3/Gimbal_H3.h"
#include "Gimbal_V3S/Gimbal_V3S.h"
#include "Gimbal_InAndOut.h"
#include "JZsdkLib.h"

#include "Psdk_UI_io.h"

//数值角度记录
#if DEVICE_VERSION == TF_A1
    static int Gimbal_PitchAngle = 0;
    static int Gimbal_YawAngle = 0;
#else
    static int Gimbal_PitchAngle = 0;
    static int Gimbal_YawAngle = 0;
#endif


//微调角度
static int Gimbal_PitchFineTuning = 0;
static int Gimbal_YawFineTuning = 0;

//飞机自身角度
static int Gimbal_UavSelfPitch = 0;

//实际角度 (数值角度+飞机自身角度+微调角度)
static int Gimbal_RealPitchAngle = 0;

static int Gimbal_LinkageNum;

static int Gimbal_SendAngleTask_Init();

static int g_JZsdk_GimbalStatusFlag = JZ_FLAGCODE_OFF;

/**
 * 
 *    获取gimbal模块状态标志位
 * 
 * **/
int Get_JZsdk_GimbalStatusFlag()
{
    return g_JZsdk_GimbalStatusFlag;
}

/*
*     Gimbal模块初始化
*/

int Gimbal_Init()
{
    //发送角度线程初始化
    Gimbal_SendAngleTask_Init();

    if (DEVICE_VERSION == JZ_H150S || DEVICE_VERSION == JZ_H150T)
    {
        //云台初始化
        Gimbal_H3_H150ST_init_motor();
    }
    else if (DEVICE_VERSION == JZ_H10)
    {
        //注：h3_h10的电机引脚已在引脚初始化中完成
        Gimbal_H3_H10_init_motor();
    }
    else if (DEVICE_VERSION == JZ_H10T)
    {
        //注：h3_h10的电机引脚已在引脚初始化中完成
        Gimbal_V3S_H10T_init_motor();
    }
    else if (DEVICE_VERSION == JZ_U3 || DEVICE_VERSION == JZ_U3S || DEVICE_VERSION == JZ_U3D)
    {
        Gimbal_V3S_U3_init_motor();    
    }
    else if (DEVICE_VERSION == JZ_H1T)
    {
        Gimbal_V3S_H1T_init_motor();
    }
    else if (DEVICE_VERSION == TF_A1)
    {
        Gimbal_V3S_TFA1_init_motor();    
    }  
    else if (DEVICE_VERSION == JZ_C1)
    {
        JZSDK_LOG_INFO("C1云台初始化");
    }
    else
    {
        return -1;
    }

    //读取微调值
    Gimbal_CheckStatus_GimbalFineTuning(&Gimbal_PitchFineTuning, &Gimbal_YawFineTuning);

    //云台flag打开
    g_JZsdk_GimbalStatusFlag = JZ_FLAGCODE_ON;

    printf("云台初始化完毕\n");

}  

//设置云台俯仰角度
T_JZsdkReturnCode Gimbal_Set_PitchAngle(int angle)
{   
    //printf("设置云台俯仰角度\n");
    if (DEVICE_VERSION == JZ_H150S || DEVICE_VERSION == JZ_H150T)
    {
        //1、判断输入范围是否正确
        if ((angle < -900) || (angle > 0))
        {
            printf("H150s/H150t的云台俯仰输入值范围出错:%d\n", angle);
            return JZ_ERRORCODE_GIMBAL_INVALID_PITCH;
        }
    }
    else if (DEVICE_VERSION == JZ_H10)
    {
        //1、判断输入范围是否正确
        if ((angle < -600) || (angle > 0))
        //if ((angle < -800) || (angle > 0))
        {
            printf("H10的云台俯仰输入值范围出错:%d\n", angle);
            return JZ_ERRORCODE_GIMBAL_INVALID_PITCH;
        }

    }
    else if (DEVICE_VERSION == JZ_H10T)
    {
        //1、判断输入范围是否正确
        if ((angle < -600) || (angle > 0))
        {
            printf("H10T的云台俯仰输入值范围出错:%d\n", angle);
            return JZ_ERRORCODE_GIMBAL_INVALID_PITCH;
        }

    }
    else if (DEVICE_VERSION == JZ_U3 || DEVICE_VERSION == JZ_U3S || DEVICE_VERSION == JZ_U3D)
    {
        //1、判断输入范围是否正确
        if ((angle < -900) || (angle > 0))
        {
            printf("U3的云台俯仰输入值范围出错:%d\n", angle);
            return JZ_ERRORCODE_GIMBAL_INVALID_PITCH;
        }

    }
    else if (DEVICE_VERSION == TF_A1)
    {
        //1、判断输入范围是否正确
        if ((angle < -900) || (angle > 0))
        {
            printf("TFA1的云台俯仰输入值范围出错:%d\n", angle);
            return JZ_ERRORCODE_GIMBAL_INVALID_PITCH;
        }

    }
    else if (DEVICE_VERSION == JZ_H1T)
    {   
        //1、判断输入范围是否正确
        if ((angle < -900) || (angle > 300))
        {
            printf("H1T的云台俯仰输入值范围出错:%d\n", angle);
            return JZ_ERRORCODE_GIMBAL_INVALID_PITCH;
        }

    }
    
    else
    {
        return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
    }

    Gimbal_PitchAngle = angle;

    return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}

//设置云台航向角度
int Gimbal_Set_YawAngle(int angle)
{

    printf("没有航向角度\n");
    return -1;
    
}

//设置云台范围的最大最小值
T_JZsdkReturnCode Gimbal_SetGimbalRange(int value)
{
    printf("设置云台范围最大最小值\n");

    if (DEVICE_VERSION == JZ_H150S || DEVICE_VERSION == JZ_H150T)
    {
        return H3_H150ST_SetGimbalRange(value);
    }
    

    if (DEVICE_VERSION == JZ_U3 || DEVICE_VERSION == JZ_U3S || DEVICE_VERSION == JZ_U3D)
    {
        return V3S_U3_SetGimbalRange(value);
    }

    if (DEVICE_VERSION == TF_A1)
    {
        return V3S_TFA1_SetGimbalRange(value);
    }

    if (DEVICE_VERSION == JZ_H1T)
    {
        /* code */
    }
    
    return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
}

//查询云台微调值
T_JZsdkReturnCode Gimbal_CheckStatus_GimbalFineTuning(int *FineTunigPitch, int *FineTunigYaw)
{
    printf("查询云台微调值\n");

    if (DEVICE_VERSION == JZ_H150S || DEVICE_VERSION == JZ_H150T)
    {
        //1、获取微调值
        Gimbal_H3_H150ST_CheckStatus_GimbalFineTuning(&Gimbal_PitchFineTuning);

        //2、回复微调值
        *FineTunigPitch = Gimbal_PitchFineTuning;
        *FineTunigYaw = Gimbal_YawFineTuning;        

        return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;        
    }
    

    if (DEVICE_VERSION == JZ_U3 || DEVICE_VERSION == JZ_U3S || DEVICE_VERSION == JZ_U3D)
    {   
        //1、获取微调值
        Gimbal_V3S_U3_CheckStatus_GimbalFineTuning(&Gimbal_PitchFineTuning);

        //2、回复微调值
        *FineTunigPitch = Gimbal_PitchFineTuning;
        *FineTunigYaw = Gimbal_YawFineTuning;        

        return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
    }

    if (DEVICE_VERSION == TF_A1)
    {   
        //1、获取微调值
        Gimbal_V3S_TFA1_CheckStatus_GimbalFineTuning(&Gimbal_PitchFineTuning);

        //2、回复微调值
        *FineTunigPitch = Gimbal_PitchFineTuning;
        *FineTunigYaw = Gimbal_YawFineTuning;        

        return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
    }

    if (DEVICE_VERSION == JZ_H1T)
    {
        //1、刷新微调值
        Gimbal_V3S_H1T_CheckStatus_GimbalFineTuning();

        delayMs(10);

        //2、回复微调值
        *FineTunigPitch = Gimbal_PitchFineTuning;
        *FineTunigYaw = Gimbal_YawFineTuning;

        return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
    }

    if (DEVICE_VERSION == JZ_H10)
    {   
        //1、获取微调值
        Gimbal_H3_H10_CheckStatus_GimbalFineTuning(&Gimbal_PitchFineTuning);

        //2、回复微调值
        *FineTunigPitch = Gimbal_PitchFineTuning;
        *FineTunigYaw = Gimbal_YawFineTuning;        

        return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
    }

    if (DEVICE_VERSION == JZ_H10T)
    {   
        //1、获取微调值
        Gimbal_V3S_H10T_CheckStatus_GimbalFineTuning(&Gimbal_PitchFineTuning);

        //2、回复微调值
        *FineTunigPitch = Gimbal_PitchFineTuning;
        *FineTunigYaw = Gimbal_YawFineTuning;        

        return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
    }
    
    return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
}

//收到云台pitch微调值
T_JZsdkReturnCode Gimbal_Obtain_GimbalFineTuning(int pitch)
{
    printf("收到云台pitch云台微调值\n");

    if (DEVICE_VERSION == JZ_H150S || DEVICE_VERSION == JZ_H150T)
    {
        //无操作
    }
    
    if (DEVICE_VERSION == JZ_U3 || DEVICE_VERSION == JZ_U3S || DEVICE_VERSION == JZ_U3D)
    {
        //无操作
    }

    if (DEVICE_VERSION == JZ_H1T)
    {
        Gimbal_PitchFineTuning = pitch;
        return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
    }

    return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
}

//设置云台pitch微调值
T_JZsdkReturnCode Gimbal_Set_PitchFineTuning(int DeviceName ,int pitch)
{
    printf("设置云台pitch云台微调值\n");
    T_JZsdkReturnCode ret;

    //原值+修改值
    Gimbal_PitchFineTuning = Gimbal_PitchFineTuning + pitch;

    if (DEVICE_VERSION == JZ_H150S || DEVICE_VERSION == JZ_H150T)
    {
        ret = Gimbal_H3_H150ST_set_PitchFineTuning(pitch);
    }

    else if (DEVICE_VERSION == JZ_U3 || DEVICE_VERSION == JZ_U3S || DEVICE_VERSION == JZ_U3D)
    {
        ret = Gimbal_V3S_U3_set_PitchFineTuning(pitch);
    }

    else if (DEVICE_VERSION == TF_A1)
    {
        ret = Gimbal_V3S_TFA1_set_PitchFineTuning(pitch);
    }

    else if (DEVICE_VERSION == JZ_H1T)
    {
        ret = Gimbal_V3S_H1T_set_PitchFineTuning(pitch);
    }

    else if (DEVICE_VERSION == JZ_H10)
    {
        ret = Gimbal_H3_H10_set_PitchFineTuning(pitch);
    }

    else if (DEVICE_VERSION == JZ_H10T)
    {
        ret = Gimbal_V3S_H10T_set_PitchFineTuning(pitch);
    }
    else
    {
        return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
    }

    //微调后重设角度
    Gimbal_Set_PitchAngle(Gimbal_PitchAngle);

    return ret;
}

//设置云台联动
T_JZsdkReturnCode Gimbal_Set_GimbalLinkageControl(int value)
{
    printf("设置云台联动\n");

    Gimbal_LinkageNum = value;

    if (DEVICE_VERSION == JZ_H150S || DEVICE_VERSION == JZ_H150T)
    {
        //没这玩意
    }

    if (DEVICE_VERSION == JZ_U3 || DEVICE_VERSION == JZ_U3S || DEVICE_VERSION == JZ_U3D)
    {
        //没这玩意
    }

    if (DEVICE_VERSION == JZ_H1T)
    {
        Gimbal_V3S_H1T_Set_GimbalLinkageControl(Gimbal_LinkageNum);
        return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
    }
    
    return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;

}

//查询云台联动
T_JZsdkReturnCode Gimbal_CheckStatus_GimbalLinkage(int *value)
{
    printf("查询云台联动值\n");
    if (DEVICE_VERSION == JZ_H150S || DEVICE_VERSION == JZ_H150T)
    {

    }

    if (DEVICE_VERSION == JZ_U3 || DEVICE_VERSION == JZ_U3S || DEVICE_VERSION == JZ_U3D)
    {
        
    }

    if (DEVICE_VERSION == JZ_H1T)
    {
        //1、刷新云台联动值
        Gimbal_V3S_H1T_CheckStatus_GimbalLinkage();

        delayMs(10);

        //2、回复云台联动值
        *value = Gimbal_LinkageNum;

        return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
    }

    return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
}

//收到云台联动值
T_JZsdkReturnCode Gimbal_Obtain_GimbalLinkage(int GimbalLinkage)
{
    printf("收到云台联动值\n");

    Gimbal_LinkageNum = GimbalLinkage;

    if (DEVICE_VERSION == JZ_H150S || DEVICE_VERSION == JZ_H150T)
    {

    }
    

    if (DEVICE_VERSION == JZ_U3 || DEVICE_VERSION == JZ_U3S || DEVICE_VERSION == JZ_U3D)
    {
        
    }

    if (DEVICE_VERSION == JZ_H1T)
    {
        //无操作
        //毕竟是t60s返回来的值
        return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
    }

    return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
}


//获取数值角度
T_JZsdkReturnCode Gimbal_Get_Angle(int *PitchAngle, int *YawAngle)
{
    *PitchAngle = Gimbal_PitchAngle;
    *YawAngle = Gimbal_YawAngle;

    return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}

/*********
 * 
 *  查询云台角度
 * 
**********/
T_JZsdkReturnCode Gimbal_CheckStatus_Angle(int *PitchAngle, int *YawAngle)
{
    //1、刷新查询角度
    if (DEVICE_VERSION == JZ_H1T)
    {
        Gimbal_V3S_H1T_CheckStatus_GimbalAngle();
    }

    //2、获取一次本地云台角度
    Gimbal_Get_Angle(PitchAngle, YawAngle);

    return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}

int Gimbal_Get_PitchAngle()
{
    return Gimbal_PitchAngle;
}

int Gimbal_Get_PitchFineTuning()
{
    return Gimbal_PitchFineTuning;
}

int Gimbal_Get_PitchRealPitchAngle()
{
    return Gimbal_RealPitchAngle;
}

//获得云台值
T_JZsdkReturnCode Gimbal_Obtain_Gimbal_Pitch(int Pitch)
{

    //目前 U3和 H1T会接受到云台返回值
    if (DEVICE_VERSION == JZ_U3 || DEVICE_VERSION == JZ_U3S || DEVICE_VERSION == JZ_U3D)
    {
        //不处理，避免出现什么问题
    }
    else if (DEVICE_VERSION == JZ_H1T)
    {
        //Gimbal_PitchAngle = Pitch;
        //这里暂时不处理，避免多次把5a帧返还给串口1设备
        //但是会导致4g接收不到最新的角度
    }
    else
    {
        //不处理，避免出现什么问题
    }
    
}

/**************************************************************
 * 
 * 
 *         飞机特殊控制云台方法
 * 
 * 
***************************************************************/
//1、飞机自身角度控制云台
T_JZsdkReturnCode Gimbal_PitchSelfangleMode(int Pitch)
{
    if (Gimbal_UavSelfPitch  == Pitch)
    {
        return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
    }
    
    Gimbal_UavSelfPitch = Pitch;

    return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;

}

//2、滑动条 获取云台角度
int Gimbal_PitchScaleMode(int value)
{
    int angle;
    if (DEVICE_VERSION == TF_A1 || DEVICE_VERSION == JZ_U3 || DEVICE_VERSION == JZ_U3S || DEVICE_VERSION == JZ_U3D || DEVICE_VERSION == JZ_H150S || DEVICE_VERSION == JZ_H150T)
    {
        //0~100 转化为 -900~0
        angle = value*9 -900;
    }
    else if (DEVICE_VERSION == JZ_H10 || DEVICE_VERSION == JZ_H10T)
    {
        //0~100 转化为 -600~0
        angle = value*6 -600;
    }
    else
    {
        return 0;
    }

    return angle;
}

//3、拨轮控制云台
T_JZsdkReturnCode Gimbal_PitchRotationMode(int value)
{
    int angle;

    //JZSDK_LOG_DEBUG("拨轮控制");

    if (DEVICE_VERSION == TF_A1 || DEVICE_VERSION == JZ_H150S || DEVICE_VERSION == JZ_H150T )
    {

        angle = Gimbal_PitchAngle + value;
        if (angle > 0)
        {
            angle = 0;
        }
        else if (angle < -900)
        {
            angle = -900;
        }
        
        if (angle == Gimbal_PitchAngle)
        {
            printf("拨轮控制无效\n");
            return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;  
        }
        
    }
    else if (DEVICE_VERSION == JZ_H10 || DEVICE_VERSION == JZ_H10T )
    {
        angle = Gimbal_PitchAngle + value;
        if (angle > 0)
        {
            angle = 0;
        }
        else if (angle < -600)
        {
            angle = -600;
        }
        
        if (angle == Gimbal_PitchAngle)
        {
            return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;  
        }
        
    }
    else
    {
        return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
    }

    Gimbal_Set_PitchAngle(angle);

    return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;   
}

//输入角度，回复滑动条值
int Gimbal_ReplyPitchToUAVScale(int angle)
{
    int value;

    if (DEVICE_VERSION == TF_A1 || DEVICE_VERSION == JZ_H150S || DEVICE_VERSION == JZ_H150T )
    {
        //-900~0 转回 0~100
        value = (angle+ 900)/9;
    }
    if (DEVICE_VERSION == JZ_H10 || DEVICE_VERSION == JZ_H10T)
    {
        //-600~0 转回 0~100
        value = (angle+ 600)/6;
    }
    
    else
    {
        return 0;
    }

    return value;
}


//设置云台俯仰角度
static T_JZsdkReturnCode Gimbal_Set_RealPitchAngle(int angle)
{   
    int temp_angle;

    //printf("设置真实俯仰角度\n");
    if (DEVICE_VERSION == JZ_H150S || DEVICE_VERSION == JZ_H150T)
    {
        //获取飞机微调角度
        int AdjustmentPitch;
        Gimbal_H3_H150ST_CheckStatus_GimbalFineTuning(&AdjustmentPitch);

        //叠加飞机角度和微调角度
        temp_angle = angle - Gimbal_UavSelfPitch + AdjustmentPitch;

        //修正angle值
        if (temp_angle < -900)
        {
            angle = -900;
        }
        else if (temp_angle > 0)
        {
            angle = 0;
        }
        else
        {
            angle = temp_angle;
        }

        Gimbal_RealPitchAngle = temp_angle;
        
        //发送到云台函数
        H3_H150ST_Gimbal_SetAngle(temp_angle);

        return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;

    }
    else if (DEVICE_VERSION == JZ_H10)
    {
        //获取飞机微调角度
        int AdjustmentPitch;
        Gimbal_H3_H10_CheckStatus_GimbalFineTuning(&AdjustmentPitch);

        //叠加飞机角度和微调角度
        temp_angle = angle - Gimbal_UavSelfPitch + AdjustmentPitch;

        //修正angle值
        if (temp_angle < -600)
        {
            angle = -600;
        }
        else if (temp_angle > 0)
        {
            angle = 0;
        }
        else
        {
            angle = temp_angle;
        }

        Gimbal_RealPitchAngle = temp_angle;
        
        //发送到云台函数
        Gimbal_H3_H10_set_angle(temp_angle);

        return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;

    }
    else if (DEVICE_VERSION == JZ_H10T)
    {
        //获取飞机微调角度
        int AdjustmentPitch;
        Gimbal_V3S_H10T_CheckStatus_GimbalFineTuning(&AdjustmentPitch);

        //叠加飞机角度和微调角度
        temp_angle = angle - Gimbal_UavSelfPitch + AdjustmentPitch;

        //修正angle值
        if (temp_angle < -600)
        {
            angle = -600;
        }
        else if (temp_angle > 0)
        {
            angle = 0;
        }
        else
        {
            angle = temp_angle;
        }

        Gimbal_RealPitchAngle = temp_angle;
        
        //发送到云台函数
        Gimbal_V3S_H10T_set_angle(temp_angle);

        return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
    }
    else if (DEVICE_VERSION == JZ_U3 || DEVICE_VERSION == JZ_U3S || DEVICE_VERSION == JZ_U3D)
    { 
        //获取飞机微调角度
        int AdjustmentPitch;
        Gimbal_V3S_U3_CheckStatus_GimbalFineTuning(&AdjustmentPitch);

        //叠加飞机角度和微调角度
        temp_angle = angle - Gimbal_UavSelfPitch + AdjustmentPitch;

        //修正angle值
        if (temp_angle < -900)
        {
            angle = -900;
        }
        else if (temp_angle > 0)
        {
            angle = 0;
        }
        else
        {
            angle = temp_angle;
        }

        Gimbal_RealPitchAngle = temp_angle;
        
        //发送到控制云台函数
        Gimbal_V3S_U3_set_angle(temp_angle);

        return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
    }
    else if (DEVICE_VERSION == TF_A1)
    {
        //叠加飞机角度和微调角度
        temp_angle = angle - Gimbal_UavSelfPitch + Get_V3S_TFA1_motor_precise_adjustment_pitch();

        //修正angle值
        if (temp_angle < -900)
        {
            angle = -900;
        }
        else if (temp_angle > 0)
        {
            angle = 0;
        }
        else
        {
            angle = temp_angle;
        }

        Gimbal_RealPitchAngle = temp_angle;
        
        //发送到V3S_TFA1控制云台函数
        Gimbal_V3S_TFA1_set_angle(temp_angle);

        return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
    }
    else if (DEVICE_VERSION == JZ_H1T)
    {   
        temp_angle = angle;
        Gimbal_RealPitchAngle = temp_angle;

        Gimbal_V3S_H1T_set_PitchAngle(temp_angle);
    }
    else if (DEVICE_VERSION == JZ_C1)
    {
        JZSDK_LOG_INFO("C1相机云台变化");
    }
    
    
}


/**************************
 * 
 * 
 *  云台角度发送线程
 * 
 * 
 * ***************************/
static void *Gimbal_SendAngleTask(void *arg)
{
    int angle = 0;
    int UAV_self_angle;
    int PitchFineTuning;

    JZSDK_LOG_DEBUG("云台角度发送初始化完成");

    int test_value =0;

    while (1)
    {
        if (Gimbal_PitchAngle != angle //云台角度发生了变化
        || Gimbal_UavSelfPitch != UAV_self_angle //飞机自身角度发生了变化
        || Gimbal_PitchFineTuning != PitchFineTuning  //微调角度发生了变化
        )
        {
            angle = Gimbal_PitchAngle;
            UAV_self_angle = Gimbal_UavSelfPitch;
            PitchFineTuning = Gimbal_PitchFineTuning;
            Gimbal_Set_RealPitchAngle(angle);
        }

        delayMs(6);
    }
}

static int Gimbal_SendAngleTask_Init()
{
	pthread_t WriteDataTask;
	pthread_attr_t task_attribute; //线程属性
	pthread_attr_init(&task_attribute);  //初始化线程属性
	pthread_attr_setdetachstate(&task_attribute, PTHREAD_CREATE_DETACHED);      //设置线程分离属性
	int opus_Protection = pthread_create(&WriteDataTask,&task_attribute,Gimbal_SendAngleTask,NULL);		//线程
	if(opus_Protection != 0)
	{
		JZSDK_LOG_ERROR("创建控件写入线程失败！");
		return -1;
	}

    JZSDK_LOG_INFO("MODULE_GIMBAL_INIT_COMPLETE");
}