|
...
|
...
|
@@ -18,6 +18,10 @@ |
|
|
|
#include "Gimbal/Gimbal_MotorFineTuning/MotorFineTuning.h"
|
|
|
|
|
|
|
|
#include "BaseConfig.h"
|
|
|
|
#include "Hal_Send/HalSend.h"
|
|
|
|
|
|
|
|
//云台使用的模式, 当前有透传模式和独立模式
|
|
|
|
static int g_Gimbal_Mode = JZ_FLAGCODE_OFF;
|
|
|
|
|
|
|
|
//数值角度记录
|
|
|
|
static int Gimbal_PitchAngle = 0; //俯仰角度 //控制云台上相机相对于水平面的上下移动。
|
|
...
|
...
|
@@ -124,14 +128,40 @@ T_JZsdkReturnCode Gimbal_Set_PitchAngle(int angle) |
|
|
|
{
|
|
|
|
return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (angle < MIN_PITCH || angle > MAX_PITCH || (MIN_PITCH == MAX_PITCH))
|
|
|
|
|
|
|
|
//根据当前的云台模式来选择设置方案
|
|
|
|
switch (g_Gimbal_Mode)
|
|
|
|
{
|
|
|
|
printf("云台俯仰输入值范围出错:%d\n", angle);
|
|
|
|
return JZ_ERRORCODE_GIMBAL_INVALID_PITCH;
|
|
|
|
//独立模式
|
|
|
|
case JZ_MODULE_CONTROL_WAY_INDEPENDENT:
|
|
|
|
{
|
|
|
|
if (angle < MIN_PITCH || angle > MAX_PITCH || (MIN_PITCH == MAX_PITCH))
|
|
|
|
{
|
|
|
|
printf("云台俯仰输入值范围出错:%d\n", angle);
|
|
|
|
return JZ_ERRORCODE_GIMBAL_INVALID_PITCH;
|
|
|
|
}
|
|
|
|
|
|
|
|
Gimbal_PitchAngle = angle;
|
|
|
|
}
|
|
|
|
break;
|
|
|
|
|
|
|
|
//透传模式
|
|
|
|
case JZ_MODULE_CONTROL_WAY_TRANSPARENT:
|
|
|
|
{
|
|
|
|
Gimbal_PitchAngle = angle;
|
|
|
|
}
|
|
|
|
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;
|
|
|
|
}
|
|
|
|
|
|
|
|
Gimbal_PitchAngle = angle;
|
|
|
|
|
|
|
|
return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
|
|
|
|
}
|
|
...
|
...
|
@@ -160,17 +190,43 @@ T_JZsdkReturnCode Gimbal_SetGimbalRange(int flag, int value) |
|
|
|
{
|
|
|
|
JZSDK_LOG_INFO("设置云台范围最大最小值 flag:%d 0x%x", flag, value);
|
|
|
|
|
|
|
|
switch (flag)
|
|
|
|
//根据云台模式来选择设置方案
|
|
|
|
switch (g_Gimbal_Mode)
|
|
|
|
{
|
|
|
|
case 0:
|
|
|
|
return Gimbal_DataDeal_SetPitchRange(value);
|
|
|
|
//独立模式
|
|
|
|
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:
|
|
|
|
break;
|
|
|
|
JZSDK_LOG_ERROR("云台模式错误");
|
|
|
|
return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
|
|
|
return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
|
|
|
|
|
|
|
|
return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
|
|
|
|
}
|
|
|
|
|
|
|
|
//查询云台微调值
|
|
...
|
...
|
@@ -178,13 +234,37 @@ T_JZsdkReturnCode Gimbal_CheckStatus_GimbalFineTuning(int *FineTuningPitch, int |
|
|
|
{
|
|
|
|
JZSDK_LOG_DEBUG("查询云台微调值");
|
|
|
|
|
|
|
|
//组合机需要去获取另外一个设备的值
|
|
|
|
#if DEVICE_VERSION == JZ_H1T
|
|
|
|
//1、刷新微调值
|
|
|
|
JZsdk_Uart_Send_CheckStatus_GimbalFineTuningPitch(UART_DEV_1, 0);
|
|
|
|
switch (g_Gimbal_Mode)
|
|
|
|
{
|
|
|
|
//独立模式
|
|
|
|
case JZ_MODULE_CONTROL_WAY_INDEPENDENT:
|
|
|
|
{
|
|
|
|
//独立模式无需另外操作
|
|
|
|
}
|
|
|
|
break;
|
|
|
|
|
|
|
|
delayMs(10);
|
|
|
|
//透传模式
|
|
|
|
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)
|
|
|
|
{
|
|
...
|
...
|
@@ -209,14 +289,35 @@ T_JZsdkReturnCode Gimbal_Obtain_GimbalFineTuning(int pitch, int yaw, int roll) |
|
|
|
{
|
|
|
|
printf("收到云台pitch云台微调值\n");
|
|
|
|
|
|
|
|
//组合机需要记录另外一个设备的值
|
|
|
|
#if DEVICE_VERSION == JZ_H1T
|
|
|
|
g_MotorFineTuningPitch = pitch;
|
|
|
|
#else
|
|
|
|
return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
|
|
|
|
#endif
|
|
|
|
switch (g_Gimbal_Mode)
|
|
|
|
{
|
|
|
|
//独立模式
|
|
|
|
case JZ_MODULE_CONTROL_WAY_INDEPENDENT:
|
|
|
|
{
|
|
|
|
//独立模式无该概念
|
|
|
|
return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
|
|
|
|
}
|
|
|
|
break;
|
|
|
|
|
|
|
|
return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
|
|
|
|
//透传模式
|
|
|
|
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;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
...
|
...
|
@@ -226,34 +327,57 @@ T_JZsdkReturnCode Gimbal_Set_PitchFineTuning(int pitch) |
|
|
|
JZSDK_LOG_INFO("设置云台pitch云台微调值\n");
|
|
|
|
T_JZsdkReturnCode ret;
|
|
|
|
|
|
|
|
if (pitch == 0)
|
|
|
|
switch (g_Gimbal_Mode)
|
|
|
|
{
|
|
|
|
return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
|
|
|
|
}
|
|
|
|
|
|
|
|
//设置本地微调值
|
|
|
|
int temp_pitch = g_MotorFineTuningPitch + pitch;
|
|
|
|
//微调值要小于20度
|
|
|
|
if (temp_pitch >= 200)
|
|
|
|
{
|
|
|
|
temp_pitch = 200;
|
|
|
|
}
|
|
|
|
//独立模式
|
|
|
|
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;
|
|
|
|
|
|
|
|
if (temp_pitch <= -200)
|
|
|
|
{
|
|
|
|
temp_pitch = -200;
|
|
|
|
}
|
|
|
|
|
|
|
|
g_MotorFineTuningPitch = temp_pitch;
|
|
|
|
|
|
|
|
//组合机是发到另外一个设备
|
|
|
|
#if DEVICE_VERSION == JZ_H1T
|
|
|
|
JZsdk_Uart_Send_SetGimbalFineTuningPitch(UART_DEV_1, g_MotorFineTuningPitch);
|
|
|
|
#else
|
|
|
|
//透传模式
|
|
|
|
case JZ_MODULE_CONTROL_WAY_TRANSPARENT:
|
|
|
|
{
|
|
|
|
g_MotorFineTuningPitch = pitch;
|
|
|
|
|
|
|
|
//存储到文件
|
|
|
|
MotorFineTuning_WritePitch(g_MotorFineTuningPitch);
|
|
|
|
#endif
|
|
|
|
#if DEVICE_VERSION == JZ_H1T
|
|
|
|
HalSend_type1Send_Set_GimbalFineTuningPitch(UART_DEV_1, g_MotorFineTuningPitch);
|
|
|
|
#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;
|
|
|
|
}
|
|
...
|
...
|
@@ -297,16 +421,39 @@ T_JZsdkReturnCode Gimbal_Set_GimbalLinkageControl(int value) |
|
|
|
{
|
|
|
|
JZSDK_LOG_INFO("设置云台联动\n");
|
|
|
|
|
|
|
|
g_Gimbal_Linkage = value;
|
|
|
|
switch (g_Gimbal_Mode)
|
|
|
|
{
|
|
|
|
//独立模式
|
|
|
|
case JZ_MODULE_CONTROL_WAY_INDEPENDENT:
|
|
|
|
{
|
|
|
|
g_Gimbal_Linkage = value;
|
|
|
|
}
|
|
|
|
break;
|
|
|
|
|
|
|
|
#if DEVICE_VERSION == JZ_H1T
|
|
|
|
JZsdk_Uart_Send_Set_GimbalLinkageControl(UART_DEV_1, 0, value);
|
|
|
|
#else
|
|
|
|
//暂时没做接口
|
|
|
|
#endif
|
|
|
|
|
|
|
|
return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
|
|
|
|
//透传模式
|
|
|
|
case JZ_MODULE_CONTROL_WAY_TRANSPARENT:
|
|
|
|
{
|
|
|
|
//设置本地联动值
|
|
|
|
g_Gimbal_Linkage = value;
|
|
|
|
|
|
|
|
#if DEVICE_VERSION == JZ_H1T
|
|
|
|
HalSend_type1Send_Set_GimbalLinkageControl(UART_DEV_1, 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;
|
|
|
|
}
|
|
|
|
|
|
|
|
//查询云台联动
|
|
...
|
...
|
@@ -314,17 +461,37 @@ T_JZsdkReturnCode Gimbal_CheckStatus_GimbalLinkage(int *value) |
|
|
|
{
|
|
|
|
JZSDK_LOG_INFO("查询云台联动值\n");
|
|
|
|
|
|
|
|
#if DEVICE_VERSION == JZ_H1T
|
|
|
|
//1、刷新云台联动值
|
|
|
|
JZsdk_Uart_Send_CheckStatus_GimbalLinkage(UART_DEV_1, 0);
|
|
|
|
switch (g_Gimbal_Mode)
|
|
|
|
{
|
|
|
|
case JZ_MODULE_CONTROL_WAY_INDEPENDENT:
|
|
|
|
{
|
|
|
|
//独立模式,直接返回本地值
|
|
|
|
*value = g_Gimbal_Linkage;
|
|
|
|
}
|
|
|
|
break;
|
|
|
|
|
|
|
|
delayMs(10);
|
|
|
|
case JZ_MODULE_CONTROL_WAY_TRANSPARENT:
|
|
|
|
{
|
|
|
|
//透传模式,查询一次云台联动值
|
|
|
|
#if DEVICE_VERSION == JZ_H1T
|
|
|
|
HalSend_type1Send_CheckStatus_GimbalLinkage(UART_DEV_1, 0);
|
|
|
|
|
|
|
|
//2、回复云台联动值
|
|
|
|
*value = g_Gimbal_Linkage;
|
|
|
|
#else
|
|
|
|
//暂时没做接口
|
|
|
|
#endif
|
|
|
|
delayMs(10);
|
|
|
|
|
|
|
|
*value = g_Gimbal_Linkage;
|
|
|
|
#endif
|
|
|
|
}
|
|
|
|
|
|
|
|
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;
|
|
|
|
}
|
|
...
|
...
|
@@ -577,13 +744,16 @@ T_JZsdkReturnCode Gimbal_Flush_Pitch() |
|
|
|
/**************************
|
|
|
|
*
|
|
|
|
*
|
|
|
|
* 云台独立控制模式初始化
|
|
|
|
* 云台透传控制模式初始化
|
|
|
|
*
|
|
|
|
*
|
|
|
|
* **************************/
|
|
|
|
static T_JZsdkReturnCode Gimbal_Init_Gimbal_Init_TransparentControl()
|
|
|
|
{
|
|
|
|
//
|
|
|
|
|
|
|
|
|
|
|
|
return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
|
|
|
|
}
|
|
|
|
|
|
|
|
/**************************
|
|
...
|
...
|
@@ -600,6 +770,8 @@ static T_JZsdkReturnCode Gimbal_Init_IndependentControl() |
|
|
|
|
|
|
|
//云台微调初始化
|
|
|
|
MotorFineTuning_Init(&g_MotorFineTuningPitch, &g_MotorFineTuningYaw, &g_MotorFineTuningRoll);
|
|
|
|
|
|
|
|
return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
|
|
|
|
}
|
|
|
|
|
|
|
|
/***************************
|
...
|
...
|
|