/*
 * @Author: ookk303
 * @Date: 2024-08-23 17:46:53
 * @LastEditors: Do not edit
 * @LastEditTime: 2024-08-28 15:26:17
 * @Description: 
 * @FilePath: \JZSDK_Linux\Module\Gimbal\Gimbal.c
 */

#include <stdio.h>
#include <pthread.h>
#include <math.h>
#include <stdlib.h>
#include "JZsdkLib.h"
#include "Gimbal/Gimbal_Param.h"
#include "Gimbal/Gimbal.h"
#include "Gimbal/Gimbal_DataDeal/Gimbal_DataDeal.h"
#include "Gimbal/Gimbal_MotorFineTuning/MotorFineTuning.h"
#include "Gimbal/SingleCompensation/SingleCompensation.h"

#include "BaseConfig.h"
#include "Hal_Send/HalSend.h"
#include "DeviceInfo/DebugInfo/DebugInfo.h"



static T_GimbalInfo g_GimbalInfo = {
    .PitchLimitMax = MAX_PITCH,
    .PitchLimitMin = MIN_PITCH,
    .DebugPitch_Left = 0,
    .DebugPitch_Right = 0,
};

//云台使用的模式, 当前有透传模式和独立模式
static int g_Gimbal_Mode = JZ_FLAGCODE_OFF; 

//数值角度记录
static int Gimbal_PitchAngle = 0; //俯仰角度  //控制云台上相机相对于水平面的上下移动。
static int Gimbal_YawAngle = 0;  //偏航角度   //个角度控制云台上相机相对于水平面的上下移动。
static int Gimbal_RollAngle = 0; //滚转角度   //控制云台上相机围绕其前后轴（即与相机镜头方向平行的轴）的旋转。

//微调角度
static int g_MotorFineTuningPitch = 0; //pitch微调值
static int g_MotorFineTuningYaw = 0; //yaw微调值
static int g_MotorFineTuningRoll = 0; //roll微调值

//单边微调角度，并非会在每次调整角度后发送，而是在每次单边微调和初始化时发送
static T_JZ_SingleCompensationInfo g_SingleCompensationInfo = {
    .Left_SingleCompensationValue = 0,
    .Right_SingleCompensationValue = 0,
};

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

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

static int g_Gimbal_Linkage = JZ_FLAGCODE_OFF; //云台联动标志位

static int g_JZsdk_GimbalStatusFlag = JZ_FLAGCODE_OFF; //云台状态，用于判断云台是否已经初始化完毕

static T_JZsdkReturnCode Gimbal_Set_RealPitchAngle(int pitch, int FineTuningP, int UAV_pitch);



//手动刷新角度标志位
static int g_Flush_Pitch = JZ_FLAGCODE_OFF;

/****************
 * 
 *   软件云台限位刷新
 * 
 * 
 * *****************/
T_JZsdkReturnCode Gimbal_LimitAngleFlush()
{
    g_GimbalInfo.PitchLimitMax = MAX_PITCH;
    g_GimbalInfo.PitchLimitMin = MIN_PITCH;

    return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}

/**************************
 * 
 * 
 *  云台角度发送线程
 * 
 * 
 * ***************************/
static void *Gimbal_SendAngleTask(void *arg)
{
    int Old_angle = 0;      //上一次变化的角度
    int Old_UAV_self_angle; //上一次飞机的角度
    int Old_PitchFineTuning;    //上一次微调的角度
    int Old_DebugPitchLeft = 0; //上一次调试角度
    int Old_DebugPitchRight = 0; //上一次调试角度

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

    int test_value =0;

    while (1)
    {
        if (Gimbal_PitchAngle != Old_angle //云台角度发生了变化
        || Gimbal_UavSelfPitch != Old_UAV_self_angle //飞机自身角度发生了变化
        || g_MotorFineTuningPitch != Old_PitchFineTuning  //微调角度发生了变化
        || g_Flush_Pitch == JZ_FLAGCODE_ON
        || g_GimbalInfo.DebugPitch_Left != Old_DebugPitchLeft
        || g_GimbalInfo.DebugPitch_Right != Old_DebugPitchRight
        )
        {
            Old_angle = Gimbal_PitchAngle;
            Old_UAV_self_angle = Gimbal_UavSelfPitch;
            Old_PitchFineTuning = g_MotorFineTuningPitch;
            Old_DebugPitchLeft = g_GimbalInfo.DebugPitch_Left;
            Old_DebugPitchRight = g_GimbalInfo.DebugPitch_Right;
            g_Flush_Pitch = JZ_FLAGCODE_OFF;
            Gimbal_Set_RealPitchAngle(Old_angle, Old_PitchFineTuning, Old_UAV_self_angle);
        }

        //1s检测200次舵机角度
        delayMs(1000/200);
    }
}

//发送线程初始化
static T_JZsdkReturnCode 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 JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
	}

    JZSDK_LOG_INFO("MODULE_GIMBAL_INIT_COMPLETE");
}

//初始化舵机部分
static T_JZsdkReturnCode Gimal_MotorInit()
{
    //角度发送线程初始化
	Gimbal_SendAngleTask_Init();

    //数据处理初始化
    Gimbal_DataDeal_init();

    JZSDK_LOG_INFO("MODULE_GIMBAL_MOTOR_INIT_COMPLETE");

}



//设置云台俯仰角度
T_JZsdkReturnCode Gimbal_Set_PitchAngle(int angle)
{
    //禁止打印
    //printf("设置云台俯仰角度\n");

    //云台未初始化
    if (g_JZsdk_GimbalStatusFlag != JZ_FLAGCODE_ON)
    {
        return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
    }

    //根据当前的云台模式来选择设置方案
    switch (g_Gimbal_Mode)
    {
        //独立模式
        case JZ_MODULE_CONTROL_WAY_INDEPENDENT:
        {
            if (angle < g_GimbalInfo.PitchLimitMin || angle > g_GimbalInfo.PitchLimitMax || (g_GimbalInfo.PitchLimitMin == g_GimbalInfo.PitchLimitMax))
            {
                printf("云台俯仰输入值范围出错:%d\n", angle);
                return JZ_ERRORCODE_GIMBAL_INVALID_PITCH;
            }
            
            Gimbal_PitchAngle = angle;
        }
        break;

        //透传模式
        case JZ_MODULE_CONTROL_WAY_TRANSPARENT:
        {
            Gimbal_PitchAngle = angle;

            //将云台角度透传出去
            Gimbal_DataDeal_SetRealAngle(Gimbal_PitchAngle);
        }
        break;

        case JZ_FLAGCODE_OFF:
            JZSDK_LOG_ERROR("云台未开启");
            return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
        break;
        
        default:
            JZSDK_LOG_ERROR("云台模式错误");
            return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
        break;
    }

    return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;     
}

//设置云台航向角度
T_JZsdkReturnCode Gimbal_Set_YawAngle(int angle)
{
    printf("没有航向角度\n");
    return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
}

//设置云台转向角度
T_JZsdkReturnCode Gimbal_Set_RollAngle(int angle)
{
    printf("没有转向角度\n");
    return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
}

/*******************
 * 
 *  设置Pitch_云台范围的最大最小值
 *  flag 0 pitch 1 yaw 2 roll
 * 
 * *************************/
T_JZsdkReturnCode Gimbal_SetGimbalRange(int flag, int value)
{
    JZSDK_LOG_INFO("设置云台范围最大最小值 flag:%d 0x%x", flag, value);

    //根据云台模式来选择设置方案
    switch (g_Gimbal_Mode)
    {
        //独立模式
        case JZ_MODULE_CONTROL_WAY_INDEPENDENT:
        {
            switch (flag)
            {
                case 0:
                    return Gimbal_DataDeal_SetPitchRange(value);     
                break;
                
                default:
                    break;
            }
        }
        break;

        //透传模式
        case JZ_MODULE_CONTROL_WAY_TRANSPARENT:
        {
            //透传模式暂时无法设置云台范围
        }
        break;

        case JZ_FLAGCODE_OFF:
            JZSDK_LOG_ERROR("云台未开启");
            return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
        break;

        default:
            JZSDK_LOG_ERROR("云台模式错误");
            return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
        break;
    }

    return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}


/********************************
 * 
 * 
 *  单边云台校准选择
 * 
 * 
 * ************************************/
T_JZsdkReturnCode Gimbal_Set_GimbalSingleCalibration(E_JZ_GimbalSingleCalibrationMode Mode)
{
    JZSDK_LOG_INFO("单边云台校准选择 Mode:%d 0x%x", Mode);

    switch (Mode)
    {
        case JZ_GIMBAL_SINGLE_CALIBRATION_MODE_LEFT_MAX:
        {
#if DEVICE_VERSION == JZ_T40 || DEVICE_VERSION == JZ_T40S
            HalSend_type1Send_SetGimbalRange(UART_DEV_2, 0x11, 0xFF);
#endif
            
        }
        break;

        case JZ_GIMBAL_SINGLE_CALIBRATION_MODE_LEFT_MIN:
        {
#if DEVICE_VERSION == JZ_T40 || DEVICE_VERSION == JZ_T40S
            HalSend_type1Send_SetGimbalRange(UART_DEV_2, 0x11, 0x00);
#endif
        }
        break;  

        case JZ_GIMBAL_SINGLE_CALIBRATION_MODE_LEFT_ZERO:
        {
#if DEVICE_VERSION == JZ_T40 || DEVICE_VERSION == JZ_T40S
            HalSend_type1Send_SetGimbalRange(UART_DEV_2, 0x11, 0x02);
#endif
        }
        break;

        case JZ_GIMBAL_SINGLE_CALIBRATION_MODE_LEFT_RESET:
        {
#if DEVICE_VERSION == JZ_T40 || DEVICE_VERSION == JZ_T40S
            HalSend_type1Send_SetGimbalRange(UART_DEV_2, 0x11, 0x01);
#endif
        }
        break;

        case JZ_GIMBAL_SINGLE_CALIBRATION_MODE_RIGHT_MAX:
        {
#if DEVICE_VERSION == JZ_T40 || DEVICE_VERSION == JZ_T40S
            HalSend_type1Send_SetGimbalRange(UART_DEV_2, 0x12, 0xFF);
#endif
        }
        break;

        case JZ_GIMBAL_SINGLE_CALIBRATION_MODE_RIGHT_MIN:
        {
#if DEVICE_VERSION == JZ_T40 || DEVICE_VERSION == JZ_T40S
            HalSend_type1Send_SetGimbalRange(UART_DEV_2, 0x12, 0x00);
#endif
        }
        break;
        
        case JZ_GIMBAL_SINGLE_CALIBRATION_MODE_RIGHT_ZERO:
        {
#if DEVICE_VERSION == JZ_T40 || DEVICE_VERSION == JZ_T40S
            HalSend_type1Send_SetGimbalRange(UART_DEV_2, 0x12, 0x02);
#endif
        }
        break;

        case JZ_GIMBAL_SINGLE_CALIBRATION_MODE_RIGHT_RESET:
        {
#if DEVICE_VERSION == JZ_T40 || DEVICE_VERSION == JZ_T40S
            HalSend_type1Send_SetGimbalRange(UART_DEV_2, 0x12, 0x01);
#endif
        }
        break;
    }

    return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}

//查询云台微调值
T_JZsdkReturnCode Gimbal_CheckStatus_GimbalFineTuning(int *FineTuningPitch, int *FineTuningYaw, int *FineTuningRoll)
{
    JZSDK_LOG_DEBUG("查询云台微调值");

    switch (g_Gimbal_Mode)
    {
        //独立模式
        case JZ_MODULE_CONTROL_WAY_INDEPENDENT:
        {
            //独立模式无需另外操作
        }
        break;

        //透传模式
        case JZ_MODULE_CONTROL_WAY_TRANSPARENT:
        {
            //向透传目标索要刷新微调值
#if DEVICE_VERSION == JZ_H1T
            HalSend_type1Send_CheckStatus_GimbalFineTuningPitch(UART_DEV_1, 0);
            delayMs(10);
#endif
        }
        break;

        case JZ_FLAGCODE_OFF:
            JZSDK_LOG_ERROR("云台未开启");
            return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
        break;

        default:
            JZSDK_LOG_ERROR("云台模式错误");
            return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
        break;

    }

    if (FineTuningPitch != NULL)
    {
        *FineTuningPitch = g_MotorFineTuningPitch;
    }
    
    if (FineTuningYaw != NULL)
    {
        *FineTuningYaw = g_MotorFineTuningYaw;
    }

    if (FineTuningRoll != NULL)
    {
        *FineTuningRoll = g_MotorFineTuningRoll;
    }

    return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}

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

    switch (g_Gimbal_Mode)
    {
        //独立模式
        case JZ_MODULE_CONTROL_WAY_INDEPENDENT:
        {
            //独立模式无该概念
            return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
        }
        break;

        //透传模式
        case JZ_MODULE_CONTROL_WAY_TRANSPARENT:
        {
            g_MotorFineTuningPitch = pitch;
        }
        break;

        case JZ_FLAGCODE_OFF:
            JZSDK_LOG_ERROR("云台未开启");
            return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
        break;

        default:
            JZSDK_LOG_ERROR("云台模式错误");
            return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
        break;
    }

    return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}


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

    switch (g_Gimbal_Mode)
    {
        //独立模式
        case JZ_MODULE_CONTROL_WAY_INDEPENDENT:
        {
            if (pitch == 0)
            {
                return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
            }
            
            //设置本地微调值
            int temp_pitch = g_MotorFineTuningPitch + pitch;
            //微调值要小于20度
            if (temp_pitch >= 200)
            {
                temp_pitch = 200;
            }

            if (temp_pitch <= -200)
            {
                temp_pitch = -200;
            }
            
            g_MotorFineTuningPitch = temp_pitch;

            //存储到文件
            MotorFineTuning_WritePitch(g_MotorFineTuningPitch);
        }
        break;

        //透传模式
        case JZ_MODULE_CONTROL_WAY_TRANSPARENT:
        {
            g_MotorFineTuningPitch = pitch;

            #if DEVICE_VERSION == JZ_H1T
                HalSend_type1Send_Set_GimbalFineTuningPitch(UART_DEV_1, g_MotorFineTuningPitch, 0x00);
            #endif
        }
        break;

        case JZ_FLAGCODE_OFF:
            JZSDK_LOG_ERROR("云台未开启");
            return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
        break;

        default:
            JZSDK_LOG_ERROR("云台模式错误");
            return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
        break;
    }

    return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}

//设置yaw云台微调值
T_JZsdkReturnCode Gimbal_Set_YawFineTuning(int yaw)
{
    JZSDK_LOG_INFO("设置云台yaw云台微调值\n");
    T_JZsdkReturnCode ret;

    if (yaw == 0)
    {
        return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
    }
    
    g_MotorFineTuningYaw = g_MotorFineTuningYaw + yaw;

    //微调后重设角度
}

//设置roll云台微调值
T_JZsdkReturnCode Gimbal_Set_RollFineTuning(int pitch, int yaw, int roll)
{
    JZSDK_LOG_INFO("设置云台pitch云台微调值\n");
    T_JZsdkReturnCode ret;


    if (roll == 0)
    {
        return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
    }
    
    g_MotorFineTuningRoll = g_MotorFineTuningRoll + roll;

    //微调后重设角度
}


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

    switch (g_Gimbal_Mode)
    {
        //独立模式
        case JZ_MODULE_CONTROL_WAY_INDEPENDENT:
        {
            g_Gimbal_Linkage = value;
        }
        break;

        //透传模式
        case JZ_MODULE_CONTROL_WAY_TRANSPARENT:
        {
            //设置本地联动值
            g_Gimbal_Linkage = value;

            #if DEVICE_VERSION == JZ_H1T
                HalSend_type1Send_Set_GimbalLinkageControl(UART_DEV_1, 0, g_Gimbal_Linkage);
            #endif
        }
        break;

        case JZ_FLAGCODE_OFF:
            JZSDK_LOG_ERROR("云台未开启");
            return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
        break;

        default:
            JZSDK_LOG_ERROR("云台模式错误");
            return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
        break;  
    }

    return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}

//查询云台联动
T_JZsdkReturnCode Gimbal_CheckStatus_GimbalLinkage(int *value)
{
    JZSDK_LOG_INFO("查询云台联动值\n");

    switch (g_Gimbal_Mode)
    {
        case JZ_MODULE_CONTROL_WAY_INDEPENDENT:
        {
            //独立模式，直接返回本地值
            *value = g_Gimbal_Linkage;
        }
        break;

        case JZ_MODULE_CONTROL_WAY_TRANSPARENT:
        {
            //透传模式，查询一次云台联动值
            #if DEVICE_VERSION == JZ_H1T
                HalSend_type1Send_CheckStatus_GimbalLinkage(UART_DEV_1, 0);    

                delayMs(10);

                *value = g_Gimbal_Linkage;
            #endif       
        }
        break;

        case JZ_FLAGCODE_OFF:
            JZSDK_LOG_ERROR("云台未开启");
            return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
        break;

        default:
            JZSDK_LOG_ERROR("云台模式错误");
            return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
        break;
    }

    return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}

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

    switch(g_Gimbal_Mode)
    {
        //透传模式
        case JZ_MODULE_CONTROL_WAY_TRANSPARENT:
        {
            g_Gimbal_Linkage = GimbalLinkage;
            return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
        }
        break;

        //独立模式
        case JZ_MODULE_CONTROL_WAY_INDEPENDENT:
        {
            g_Gimbal_Linkage = GimbalLinkage;
            return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
        }
        break;

        case JZ_FLAGCODE_OFF:
            JZSDK_LOG_ERROR("云台未开启");
            return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
        break;

        default:
            JZSDK_LOG_ERROR("云台模式错误");
            return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
        break;
    }

    return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}

/*********
 * 
 *  查询云台角度
 * 
**********/
T_JZsdkReturnCode Gimbal_CheckStatus_Angle(int *PitchAngle, int *YawAngle, int *RollAngle)
{
    
    switch (g_Gimbal_Mode)
    {
        case JZ_MODULE_CONTROL_WAY_INDEPENDENT:
        {
            //无事发生
        }
        break;

        case JZ_MODULE_CONTROL_WAY_TRANSPARENT:
        {
            //透传模式，查询一次云台角度
            //组合机刷新查询角度
            #if DEVICE_VERSION == JZ_H1T
                HalSend_type1Send_CheckStatus_GimbalAngle(UART_DEV_1, 0);  

                //给点延时问题不大
                delayMs(5);
            #endif
        }
        break;

        case JZ_FLAGCODE_OFF:
            JZSDK_LOG_ERROR("云台未开启");
            return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
        break;

        default:
            JZSDK_LOG_ERROR("云台模式错误");
            return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
        break;
    }

    //2、获取一次本地云台角度
    if (PitchAngle != NULL)
    {
        *PitchAngle = Gimbal_PitchAngle;
    }
    
    if (YawAngle != NULL)
    {    
        *YawAngle = Gimbal_YawAngle;
    }

    if (RollAngle != NULL)
    {
        *RollAngle = Gimbal_RollAngle;
    }
    
    return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}


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


/********************************
 * 
 * 
 *  单边云台补偿值设置
 * 
 * 
 * ************************************/
T_JZsdkReturnCode Gimbal_Set_SingleCompensation(int Mode, int Value)
{
    //判断云台是否初始化完成
    if (g_JZsdk_GimbalStatusFlag == JZ_FLAGCODE_OFF)
    {
        JZSDK_LOG_ERROR("云台未初始化");
        return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
    }

    if (Mode == JZ_SINGLE_COMPENSATION_MODE_PITCH_LEFT)
    {
        JZSDK_LOG_INFO("设置单边补偿值，模式：左补偿，值：%d\n", Value);
        g_SingleCompensationInfo.Left_SingleCompensationValue = g_SingleCompensationInfo.Left_SingleCompensationValue + Value;
    
        //设置本地的单边补偿值
        SingleCompensation_SaveValues(&g_SingleCompensationInfo);

        //发送单边补偿值到云台
        HalSend_type1Send_Set_GimbalFineTuningPitch(UART_DEV_2, g_SingleCompensationInfo.Left_SingleCompensationValue, 0x11);
    }

    else if (Mode == JZ_SINGLE_COMPENSATION_MODE_PITCH_RIGHT)
    {
        JZSDK_LOG_INFO("设置单边补偿值，模式：右补偿，值：%d\n", Value);
        g_SingleCompensationInfo.Right_SingleCompensationValue = g_SingleCompensationInfo.Right_SingleCompensationValue + Value;

        //设置本地的单边补偿值
        SingleCompensation_SaveValues(&g_SingleCompensationInfo);

        //发送单边补偿值到云台
        HalSend_type1Send_Set_GimbalFineTuningPitch(UART_DEV_2, g_SingleCompensationInfo.Right_SingleCompensationValue, 0x12);
    }

    return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}
























/**************************************************************
 * 
 * 
 *         飞机特殊控制云台方法
 * 
 * 
***************************************************************/
//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 step = (abs(g_GimbalInfo.PitchLimitMax) + abs(g_GimbalInfo.PitchLimitMin)) / 100;

    int angle = 0;

    angle = value * step + g_GimbalInfo.PitchLimitMin;

    return angle;
}

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

    //JZSDK_LOG_DEBUG("拨轮控制");
    if (g_GimbalInfo.PitchLimitMax == g_GimbalInfo.PitchLimitMin)
    {
        return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
    }
    

    angle = Gimbal_PitchAngle + value;
    if (angle > g_GimbalInfo.PitchLimitMax)
    {
        angle = g_GimbalInfo.PitchLimitMax;
    }
    else if (angle < g_GimbalInfo.PitchLimitMin)
    {
        angle = g_GimbalInfo.PitchLimitMin;
    }
    
    if (angle == Gimbal_PitchAngle)
    {
        //printf("拨轮控制无效\n");
        return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;  
    }

    Gimbal_Set_PitchAngle(angle);

    return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;   
}

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

    int step = (abs(g_GimbalInfo.PitchLimitMax) + abs(g_GimbalInfo.PitchLimitMin)) / 100;
    value = (angle - g_GimbalInfo.PitchLimitMin)/step;

    return value;
}


//设置云台俯仰角度
static T_JZsdkReturnCode Gimbal_Set_RealPitchAngle(int pitch, int FineTuningP, int UAV_pitch)
{   
    int temp_angle = 0;

    switch (g_Gimbal_Mode)
    {
        case JZ_MODULE_CONTROL_WAY_INDEPENDENT:
        {
            //叠加飞机角度和微调角度
            temp_angle = pitch - UAV_pitch + FineTuningP;

            if (temp_angle < g_GimbalInfo.PitchLimitMin)
            {
                temp_angle = g_GimbalInfo.PitchLimitMin;
            }

            if (temp_angle > g_GimbalInfo.PitchLimitMax)
            {
                temp_angle = g_GimbalInfo.PitchLimitMax;
            }

            Gimbal_RealPitchAngle = temp_angle;
        }    
        break;

        case JZ_MODULE_CONTROL_WAY_TRANSPARENT:
        {
            //组合机不用堆叠角度
            #if DEVICE_VERSION == JZ_H1T
            temp_angle = pitch;
            Gimbal_RealPitchAngle = temp_angle;
            #endif
        }
        break;


        case JZ_FLAGCODE_OFF:
        {
            JZSDK_LOG_ERROR("云台模式未设置");
            return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
        }
        break;

        default:
        {
            JZSDK_LOG_ERROR("云台模式设置错误");
            return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
        }
        break;
    }

    //printf("设置真实俯仰角度\n"); //禁止打印

    Gimbal_DataDeal_SetRealAngle(temp_angle);

    return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}

/*
    输入调试角度
    pitch加减的值
    flag标志位 =on时 重置调试角度
    direction = 0 左调试角度
    direction = 1 右调试角度
*/
T_JZsdkReturnCode Gimbal_Set_DebugPitch(int pitch, int flag, int direction)
{
    //如果重置标志位开启
    if (flag == JZ_FLAGCODE_ON)
    {
        if (direction == 0)
        {
            g_GimbalInfo.DebugPitch_Left = 0;
        }
        
        else if (direction == 1)
        {
            g_GimbalInfo.DebugPitch_Right = 0;
        }
        
        return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
    }


    if (direction == 0)
    {
        g_GimbalInfo.DebugPitch_Left += pitch;
    }

    else if (direction == 1)
    {
        g_GimbalInfo.DebugPitch_Right += pitch;
    }

    return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}








T_JZsdkReturnCode Get_JZsdk_GimbalStatusFlag()
{
    return g_JZsdk_GimbalStatusFlag;
}


int Gimbal_Get_PitchAngle()
{
    return Gimbal_PitchAngle;
}

int Gimbal_Get_PitchFineTuning()
{
    return g_MotorFineTuningPitch;
}

int Gimbal_Get_PitchRealPitchAngle()
{
    return Gimbal_RealPitchAngle;
}

T_JZsdkReturnCode Gimbal_Flush_Pitch()
{
    g_Flush_Pitch = JZ_FLAGCODE_ON;

    return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}

T_JZsdkReturnCode Gimbal_Get_GimbalInfo(T_GimbalInfo *GimbalInfo)
{
    memcpy(GimbalInfo, &g_GimbalInfo, sizeof(T_GimbalInfo));

    return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}




























/**************************
 * 
 * 
 *  云台透传控制模式初始化
 * 
 * 
 * **************************/
static T_JZsdkReturnCode Gimbal_Init_Gimbal_Init_TransparentControl()
{
    //
    g_Gimbal_Mode = JZ_MODULE_CONTROL_WAY_TRANSPARENT;


    return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}

/**************************
 * 
 * 
 *  云台独立控制模式初始化
 * 
 * 
 * **************************/
static T_JZsdkReturnCode Gimbal_Init_IndependentControl()
{
    g_Gimbal_Mode = JZ_MODULE_CONTROL_WAY_INDEPENDENT;

    T_JZsdkReturnCode ret;

    //舵机初始化
    Gimal_MotorInit();

    //云台微调初始化
    MotorFineTuning_Init(&g_MotorFineTuningPitch, &g_MotorFineTuningYaw, &g_MotorFineTuningRoll);

    //单边云台补偿初始化
    ret = JZsdk_SingleCompensation_Init(&g_SingleCompensationInfo);
    if ((JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS == ret) && (DEVICE_VERSION == JZ_T40 || DEVICE_VERSION == JZ_T40S))
    {
        JZSDK_LOG_INFO("单边云台补偿初始化成功, 读取到补偿值，左补偿值：%d，右补偿值：%d", g_SingleCompensationInfo.Left_SingleCompensationValue, g_SingleCompensationInfo.Right_SingleCompensationValue);

        //发送单边补偿值到云台
        HalSend_type1Send_Set_GimbalFineTuningPitch(UART_DEV_2, g_SingleCompensationInfo.Left_SingleCompensationValue, 0x11);

        delayMs(500);

        HalSend_type1Send_Set_GimbalFineTuningPitch(UART_DEV_2, g_SingleCompensationInfo.Right_SingleCompensationValue, 0x12);
    }

    //云台角度初始化
    Gimbal_Flush_Pitch();

    return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}

/***************************
 * 
 *  初始化云台模块
 *  云台模块目前有两种方案
 *  
 *  第一种
 *    单一机子，如u3，使用的是独立控制
 *  
 *  第二种 组合机
 *    机子作为中继器，使用的是透传
 * 
 * ******************************/
T_JZsdkReturnCode Gimbal_Init(enum JZ_MODULE_CONTROL_WAY Mode)
{
    //限位初始化
    Gimbal_LimitAngleFlush();

    switch (Mode)
    {
        //独立控制模式
        case JZ_MODULE_CONTROL_WAY_INDEPENDENT:
            Gimbal_Init_IndependentControl();
        break;

        //透传模式
        case JZ_MODULE_CONTROL_WAY_TRANSPARENT:
            Gimbal_Init_Gimbal_Init_TransparentControl();
        break;
        
        default:
            JZSDK_LOG_ERROR("云台初始化失败，初始化模式选择错误");
            return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
        break;
    }

    //云台flag打开
    g_JZsdk_GimbalStatusFlag = JZ_FLAGCODE_ON;

    JZSDK_LOG_INFO("JZsdk_Gimbal_Module_init");
}