Er_sentry/User/module/cmd.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 *c_cmd, Gimbal_CMD_t *g_cmd)
{

    if (rc->rc_type == RC_DR16)
    {

        c_cmd->Vx = rc->DJ.ch_r_x;
        c_cmd->Vy = rc->DJ.ch_r_y;
        c_cmd->Vw = rc->DJ.ch_l_x;
        // c_   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_ET16s)
    {

        c_cmd->Vx = rc->ET16s.ch_r_x;
        c_cmd->Vy = rc->ET16s.ch_r_y;
        c_cmd->Vw = rc->ET16s.ch_l_x;

        c_cmd->x_l = rc->ET16s.ch_l_x;
        c_cmd->y_l = rc->ET16s.ch_l_y;

        //   cmd->throttle = rc->ET16s.ch_l_x;
        g_cmd->delta_yaw_6020 = -rc->ET16s.ch_r_x;
        // g_cmd->delta_yaw_4310 = rc->ET16s.ch_r_x;
        g_cmd->delta_pitch_4310 = -rc->ET16s.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_LD:
    case RC_ET16s:
        if (rc->ET16s.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 *c_cmd, const CMD_RC_t *rc, Gimbal_CMD_t *g_cmd,Shoot_CMD_t *s_cmd)
{

    // 大疆
    if (rc->rc_type == RC_DR16)
    {

        switch (rc->DJ.sw_l)
        {
        case CMD_SW_UP:
            c_cmd->mode = RC;

            break;
        case CMD_SW_DOWN:
            c_cmd->mode = CHASSIS_MODE_BREAK;
            break;
        case CMD_SW_MID:
            c_cmd->mode = CHASSIS_MODE_BREAK;
            break;
        case CMD_SW_ERR:
            c_cmd->mode = CHASSIS_MODE_BREAK;
            break;
        }
    }
    // 天地飞
    else if (rc->rc_type == RC_ET16s)
    {

        // 模式切换
        switch (rc->ET16s.key_A)
        {
        case CMD_SW_UP:
            c_cmd->mode = CHASSIS_MODE_BREAK;
            // g_cmd->mode = GIMBAL_MODE_RELAX;
            break;
        case CMD_SW_MID:
            c_cmd->mode = CHASSIS_MODE_FOLLOW_GIMBAL;
            // g_cmd->mode = GIMBAL_MODE_ABSOLUTE;
            break;
        case CMD_SW_DOWN:
            c_cmd->mode = CHASSIS_MODE_ROTOR;
            // g_cmd->mode = GIMBAL_MODE_ABSOLUTE;
            break;
        case CMD_SW_ERR:
            c_cmd->mode = CHASSIS_MODE_BREAK;
            // g_cmd->mode = GIMBAL_MODE_RELAX;
            break;
        }

        switch (rc->ET16s.key_B)
        {
        case CMD_SW_UP:
            g_cmd->ctrl_mode = GIMBAL_MODE_REMOTE;
            s_cmd->control_mode=SHOOT_REMOTE;
            break;
        case CMD_SW_MID:
            g_cmd->ctrl_mode = GIMBAL_MODE_AI;
            s_cmd->control_mode=SHOOT_AI;
            break;  
        case CMD_SW_DOWN:
            g_cmd->ctrl_mode = GIMBAL_MODE_AI;
            s_cmd->control_mode=SHOOT_AI;
            break;
        case CMD_SW_ERR:
            g_cmd->ctrl_mode = GIMBAL_MODE_REMOTE;
            s_cmd->control_mode=SHOOT_REMOTE;
            break;
        }

        switch (rc->ET16s.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;
        }

        switch(rc->ET16s.key_C)
        {
            case CMD_SW_UP:
                s_cmd->mode = SHOOT_MODE_SAFE;
                break;
            case CMD_SW_MID:
                s_cmd->mode = SHOOT_MODE_SINGLE;
                break;
            case CMD_SW_DOWN:
                s_cmd->mode = SHOOT_MODE_CONTINUE;
                break;
            case CMD_SW_ERR:
                s_cmd->mode = SHOOT_MODE_SAFE;
                break;
        }
        switch(rc->ET16s.key_D)
        {
            
            case CMD_SW_UP:
                s_cmd->firecmd = false;
                s_cmd->ready = false;

                break;
            case CMD_SW_MID:
                s_cmd->firecmd = false;
                s_cmd->ready = true;

                break;
            case CMD_SW_DOWN:
                s_cmd->firecmd = true;
                s_cmd->ready = true;
                break;
            case CMD_SW_ERR:
                s_cmd->firecmd = false;
                s_cmd->ready = false;
                break;
        }
    }
    return 0;
}