179 lines
4.3 KiB
C
179 lines
4.3 KiB
C
/*
|
||
该文件cmd用于对各种命令的中转处理，以防止在remote遥控器任务中过于冗杂堆叠
|
||
*/
|
||
|
||
/* Includes ----------------------------------------------------------------- */
|
||
#include "cmd.h"
|
||
#include "string.h"
|
||
#include "device/device.h"
|
||
|
||
/* Private function -------------------------------------------------------- */
|
||
/*Export function --------------------------------------------------------------*/
|
||
|
||
void CMD_Init(Chassis_CMD_t *cmd, Gimbal_CMD_t *g_cmd)
|
||
{
|
||
|
||
cmd->cmd_power_on_safe = 0;
|
||
cmd->mode = CHASSIS_MODE_BREAK;
|
||
g_cmd->mode = GIMBAL_MODE_RELAX;
|
||
}
|
||
|
||
static void CMD_remote(const CMD_RC_t *rc, Chassis_CMD_t *cmd, Gimbal_CMD_t *g_cmd)
|
||
{
|
||
|
||
if (rc->rc_type == RC_DR16)
|
||
{
|
||
|
||
cmd->Vx = rc->DJ.ch_r_x;
|
||
cmd->Vy = rc->DJ.ch_r_y;
|
||
cmd->Vw = rc->DJ.ch_l_x;
|
||
// cmd->throttle = rc->DJ.ch_l_x;
|
||
g_cmd->delta_yaw_6020 = rc->DJ.ch_r_x;
|
||
g_cmd->delta_pitch_4310 = rc->DJ.ch_r_y;
|
||
}
|
||
if (rc->rc_type == RC_LD)
|
||
{
|
||
|
||
cmd->Vx = rc->LD.ch_r_x;
|
||
cmd->Vy = rc->LD.ch_r_y;
|
||
cmd->Vw = rc->LD.ch_l_x;
|
||
|
||
cmd->x_l = rc->LD.ch_l_x;
|
||
cmd->y_l = rc->LD.ch_l_y;
|
||
|
||
// cmd->throttle = rc->LD.ch_l_x;
|
||
g_cmd->delta_yaw_6020 = rc->LD.ch_r_x;
|
||
// g_cmd->delta_yaw_4310 = rc->LD.ch_r_x;
|
||
g_cmd->delta_pitch_4310 = -rc->LD.ch_r_y;
|
||
}
|
||
}
|
||
/**
|
||
* @brief rc失控时机器人恢复放松模式
|
||
*
|
||
* @param cmd 主结构体
|
||
*/
|
||
static void CMD_RcLostLogic(Chassis_CMD_t *cmd,Gimbal_CMD_t *g_cmd)
|
||
{
|
||
/* 机器人底盘运行模式恢复至放松模式 */
|
||
cmd->mode = STOP;
|
||
g_cmd->mode = GIMBAL_MODE_RELAX;
|
||
}
|
||
|
||
void CMD_ParseRc(Chassis_CMD_t *c_cmd, CMD_RC_t *rc, Gimbal_CMD_t *g_cmd)
|
||
{
|
||
|
||
switch (rc->rc_type)
|
||
{
|
||
case RC_DR16:
|
||
/*c当rc丢控时，恢复机器人至默认状态 */
|
||
if ((rc->DJ.sw_l == CMD_SW_ERR) || (rc->DJ.sw_r == CMD_SW_ERR))
|
||
{
|
||
CMD_RcLostLogic(c_cmd,g_cmd);
|
||
c_cmd->cmd_power_on_safe = 0;
|
||
}
|
||
else
|
||
{
|
||
CMD_remote(rc, c_cmd, g_cmd);
|
||
}
|
||
break;
|
||
case RC_ET16s:
|
||
case RC_LD:
|
||
if (rc->LD.key_D == 0)
|
||
{
|
||
CMD_RcLostLogic(c_cmd,g_cmd);
|
||
}
|
||
else
|
||
{
|
||
CMD_remote(rc, c_cmd, g_cmd);
|
||
}
|
||
break;
|
||
|
||
case Control_loss:
|
||
CMD_RcLostLogic(c_cmd,g_cmd);
|
||
break;
|
||
}
|
||
}
|
||
int8_t CMD_CtrlSet(Chassis_CMD_t *cmd, const CMD_RC_t *rc, Gimbal_CMD_t *g_cmd)
|
||
{
|
||
|
||
// 大疆
|
||
if (rc->rc_type == RC_DR16)
|
||
{
|
||
|
||
switch (rc->DJ.sw_l)
|
||
{
|
||
case CMD_SW_UP:
|
||
cmd->mode = RC;
|
||
|
||
break;
|
||
case CMD_SW_DOWN:
|
||
cmd->mode = CHASSIS_MODE_BREAK;
|
||
break;
|
||
case CMD_SW_MID:
|
||
cmd->mode = CHASSIS_MODE_BREAK;
|
||
break;
|
||
case CMD_SW_ERR:
|
||
cmd->mode = CHASSIS_MODE_BREAK;
|
||
break;
|
||
}
|
||
}
|
||
// 天地飞
|
||
else if (rc->rc_type == RC_LD)
|
||
{
|
||
|
||
// 模式切换
|
||
switch (rc->LD.key_A)
|
||
{
|
||
case CMD_SW_UP:
|
||
cmd->mode = CHASSIS_MODE_BREAK;
|
||
// g_cmd->mode = GIMBAL_MODE_RELAX;
|
||
break;
|
||
case CMD_SW_MID:
|
||
cmd->mode = CHASSIS_MODE_FOLLOW_GIMBAL;
|
||
// g_cmd->mode = GIMBAL_MODE_ABSOLUTE;
|
||
break;
|
||
case CMD_SW_DOWN:
|
||
cmd->mode = CHASSIS_MODE_ROTOR;
|
||
// g_cmd->mode = GIMBAL_MODE_ABSOLUTE;
|
||
break;
|
||
case CMD_SW_ERR:
|
||
cmd->mode = CHASSIS_MODE_BREAK;
|
||
// g_cmd->mode = GIMBAL_MODE_RELAX;
|
||
break;
|
||
}
|
||
switch (rc->LD.key_B)
|
||
{
|
||
case CMD_SW_UP:
|
||
cmd->throttle = 1.0f;
|
||
break;
|
||
case CMD_SW_MID:
|
||
cmd->throttle = 1.5f;
|
||
break;
|
||
case CMD_SW_DOWN:
|
||
cmd->throttle = 2.0f;
|
||
break;
|
||
case CMD_SW_ERR:
|
||
break;
|
||
|
||
default:
|
||
break;
|
||
}
|
||
switch (rc->LD.key_E)
|
||
{
|
||
case CMD_SW_UP:
|
||
g_cmd->mode = GIMBAL_MODE_RELAX;
|
||
break;
|
||
case CMD_SW_MID:
|
||
g_cmd->mode = GIMBAL_MODE_ABSOLUTE;
|
||
break;
|
||
case CMD_SW_DOWN:
|
||
g_cmd->mode = GIMBAL_MODE_RELATIVE;
|
||
break;
|
||
case CMD_SW_ERR:
|
||
g_cmd->mode = GIMBAL_MODE_RELAX;
|
||
break;
|
||
}
|
||
}
|
||
return 0;
|
||
}
|