757 lines
29 KiB
C
757 lines
29 KiB
C
/*
|
|
控制命令
|
|
*/
|
|
#include "module/cmd.h"
|
|
|
|
#include <stdint.h>
|
|
#include <string.h>
|
|
#include "bsp/time.h"
|
|
#include "device/vt13.h"
|
|
#include "module/shoot.h"
|
|
|
|
/*************************************************************************************************************************************/
|
|
/****************************************************************RC*******************************************************************/
|
|
/*************************************************************************************************************************************/
|
|
/* 扩展接口 */
|
|
#if CMD_RCTypeTable_Index == 0
|
|
#define CMD_RCType_TABLE(X) X(dr16, DR16)
|
|
#elif CMD_RCTypeTable_Index == 1
|
|
#define CMD_RCType_TABLE(X) X(at9s, AT9S)
|
|
#elif CMD_RCTypeTable_Index == 2
|
|
#define CMD_RCType_TABLE(X) X(vt13, VT13)
|
|
#endif
|
|
/* Includes ----------------------------------------------------------------- */
|
|
#if CMD_RCTypeTable_Index == 0
|
|
#include "device/dr16.h"
|
|
#elif CMD_RCTypeTable_Index == 1
|
|
#include "device/at9s_pro.h"
|
|
#elif CMD_RCTypeTable_Index == 2
|
|
#include "device/vt13.h"
|
|
#endif
|
|
/* Private macro ------------------------------------------------------------ */
|
|
/* 声明外部rc数据变量 */
|
|
#define CMDMACRO_EXTERNAL_RCDATA(name, NAME) extern NAME##_t name;
|
|
/* 声明CMD结构体的RC数据变量 */
|
|
#define CMDMACRO_VAR_RCDATA(name, NAME) NAME##_DataRC_t name;
|
|
/* 宏生成Cmd_RC_Get函数内容 */
|
|
#define CMDMACRO_Cmd_RC_Get(name, NAME) \
|
|
bool online= name.header.online; \
|
|
rc_buffer.name= name.data.rc; \
|
|
rc->online=online; \
|
|
rc->type = CMD_RCTypeTable_Index;
|
|
union CMD_RC_Data_t{
|
|
CMD_RCType_TABLE(CMDMACRO_VAR_RCDATA)
|
|
};
|
|
|
|
CMD_RCType_TABLE(CMDMACRO_EXTERNAL_RCDATA)
|
|
/* Private typedef ---------------------------------------------------------- */
|
|
/* Private define ----------------------------------------------------------- */
|
|
|
|
/* Private variables -------------------------------------------------------- */
|
|
/*静态缓冲区*/
|
|
CMD_RC_Data_t rc_buffer;
|
|
|
|
/* Private function -------------------------------------------------------- */
|
|
int8_t Cmd_RC_Get(CMD_Input_RC_t *rc){
|
|
CMD_RCType_TABLE(CMDMACRO_Cmd_RC_Get);
|
|
return CMD_OK;
|
|
}
|
|
|
|
int8_t Cmd_RC_BuildChassisCommandFromInput(CMD_t *c) {
|
|
if (c == NULL) {
|
|
return CMD_ERR_NULL; // 参数错误
|
|
}
|
|
#if CMD_RCTypeTable_Index == 0
|
|
switch (c->input.rc.data->dr16.sw_l) {
|
|
case DR16_SW_DOWN:
|
|
c->output.chassis.cmd.mode = CHASSIS_MODE_ROTOR;
|
|
break;
|
|
case DR16_SW_MID:
|
|
c->output.chassis.cmd.mode = CHASSIS_MODE_FOLLOW_GIMBAL;
|
|
break;
|
|
case DR16_SW_UP:
|
|
c->output.chassis.cmd.mode = CHASSIS_MODE_BREAK;
|
|
break;
|
|
default:
|
|
c->output.chassis.cmd.mode = CHASSIS_MODE_RELAX;
|
|
break;
|
|
}
|
|
c->output.chassis.cmd.ctrl_vec.vx = c->input.rc.data->dr16.ch_r_x;
|
|
c->output.chassis.cmd.ctrl_vec.vy = c->input.rc.data->dr16.ch_r_y;
|
|
#elif CMD_RCTypeTable_Index == 1
|
|
switch (c->input.rc.data->at9s.key_E) {
|
|
case AT9S_CMD_SW_DOWN:
|
|
c->output.chassis.cmd.mode = CHASSIS_MODE_RELAX;
|
|
break;
|
|
case AT9S_CMD_SW_MID:
|
|
c->output.chassis.cmd.mode = CHASSIS_MODE_FOLLOW_GIMBAL;
|
|
break;
|
|
case AT9S_CMD_SW_UP:
|
|
c->output.chassis.cmd.mode = CHASSIS_MODE_ROTOR;
|
|
break;
|
|
default:
|
|
c->output.chassis.cmd.mode = CHASSIS_MODE_RELAX;
|
|
break;
|
|
}
|
|
c->output.chassis.cmd.ctrl_vec.vx = c->input.rc.data->at9s.ch_r_x;
|
|
c->output.chassis.cmd.ctrl_vec.vy = c->input.rc.data->at9s.ch_r_y;
|
|
#endif
|
|
return CMD_OK;
|
|
}
|
|
|
|
int8_t Cmd_RC_BuildGimbalCommandFromInput(CMD_t *c) {
|
|
if (c == NULL) {
|
|
return CMD_ERR_NULL; // 参数错误
|
|
}
|
|
#if CMD_RCTypeTable_Index == 0
|
|
switch (c->input.rc.data->dr16.sw_l) {
|
|
case DR16_SW_DOWN:
|
|
c->output.gimbal.cmd.mode = GIMBAL_MODE_ABSOLUTE;
|
|
c->output.gimbal.cmd.delta_yaw = 0.0f;
|
|
c->output.gimbal.cmd.delta_pit = 0.0f;
|
|
break;
|
|
case DR16_SW_MID:
|
|
c->output.gimbal.cmd.mode = GIMBAL_MODE_ABSOLUTE;
|
|
c->output.gimbal.cmd.delta_yaw = -c->input.rc.data->dr16.ch_l_x * 2.0f;
|
|
c->output.gimbal.cmd.delta_pit = -c->input.rc.data->dr16.ch_l_y * 1.5f;
|
|
break;
|
|
case DR16_SW_UP:
|
|
c->output.gimbal.cmd.mode = GIMBAL_MODE_RELAX;
|
|
c->output.gimbal.cmd.delta_yaw = -c->input.rc.data->dr16.ch_l_x * 2.0f;
|
|
c->output.gimbal.cmd.delta_pit = -c->input.rc.data->dr16.ch_l_y * 1.5f;
|
|
break;
|
|
default:
|
|
c->output.gimbal.cmd.mode = GIMBAL_MODE_RELAX;
|
|
c->output.gimbal.cmd.delta_yaw = 0.0f;
|
|
c->output.gimbal.cmd.delta_pit = 0.0f;
|
|
break;
|
|
}
|
|
#elif CMD_RCTypeTable_Index == 1
|
|
switch (c->input.rc.data->at9s.key_G) {
|
|
case AT9S_CMD_SW_DOWN:
|
|
c->output.gimbal.cmd.mode = GIMBAL_MODE_RELAX;
|
|
c->output.gimbal.cmd.delta_yaw = 0.0f;
|
|
c->output.gimbal.cmd.delta_pit = 0.0f;
|
|
break;
|
|
case AT9S_CMD_SW_MID:
|
|
c->output.gimbal.cmd.mode = GIMBAL_MODE_ABSOLUTE;
|
|
c->output.gimbal.cmd.delta_yaw = -c->input.rc.data->at9s.ch_l_x * 2.0f;
|
|
c->output.gimbal.cmd.delta_pit = -c->input.rc.data->at9s.ch_l_y * 1.5f;
|
|
break;
|
|
case AT9S_CMD_SW_UP:
|
|
c->output.gimbal.cmd.mode = GIMBAL_MODE_ABSOLUTE;
|
|
c->output.gimbal.cmd.delta_yaw = -c->input.rc.data->at9s.ch_l_x * 2.0f;
|
|
c->output.gimbal.cmd.delta_pit = -c->input.rc.data->at9s.ch_l_y * 1.5f;
|
|
break;
|
|
default:
|
|
c->output.gimbal.cmd.mode = GIMBAL_MODE_RELAX;
|
|
c->output.gimbal.cmd.delta_yaw = 0.0f;
|
|
c->output.gimbal.cmd.delta_pit = 0.0f;
|
|
break;
|
|
}
|
|
#endif
|
|
return CMD_OK;
|
|
}
|
|
|
|
|
|
int8_t Cmd_RC_BuildShootCommandFromInput(CMD_t *c) {
|
|
if (c == NULL) {
|
|
return CMD_ERR_NULL; // 参数错误
|
|
}
|
|
#if CMD_RCTypeTable_Index == 0
|
|
c->output.shoot.cmd.online = c->input.rc.online;
|
|
if (c->output.shoot.cmd.online) {
|
|
c->output.shoot.cmd.mode=SHOOT_MODE_SINGLE;
|
|
} else {
|
|
c->output.shoot.cmd.mode=SHOOT_MODE_SAFE;
|
|
}
|
|
switch (c->input.rc.data->dr16.sw_r) {
|
|
case DR16_SW_DOWN:
|
|
c->output.shoot.cmd.ready = true;
|
|
c->output.shoot.cmd.firecmd = true;
|
|
break;
|
|
case DR16_SW_MID:
|
|
c->output.shoot.cmd.ready = true;
|
|
c->output.shoot.cmd.firecmd = false;
|
|
break;
|
|
case DR16_SW_UP:
|
|
c->output.shoot.cmd.ready = false;
|
|
c->output.shoot.cmd.firecmd = false;
|
|
break;
|
|
default:
|
|
c->output.shoot.cmd.ready = false;
|
|
c->output.shoot.cmd.firecmd = false;
|
|
break;
|
|
}
|
|
#elif CMD_RCTypeTable_Index == 1
|
|
c->output.shoot.cmd.online = c->input.rc.online;
|
|
switch (c->input.rc.data->at9s.key_C) {
|
|
case AT9S_CMD_SW_DOWN:
|
|
c->output.shoot.cmd.ready = true;
|
|
c->output.shoot.cmd.firecmd = true;
|
|
break;
|
|
case AT9S_CMD_SW_MID:
|
|
c->output.shoot.cmd.ready = true;
|
|
c->output.shoot.cmd.firecmd = false;
|
|
break;
|
|
case AT9S_CMD_SW_UP:
|
|
c->output.shoot.cmd.ready = false;
|
|
c->output.shoot.cmd.firecmd = false;
|
|
break;
|
|
default:
|
|
c->output.shoot.cmd.ready = false;
|
|
c->output.shoot.cmd.firecmd = false;
|
|
break;
|
|
}
|
|
switch (c->input.rc.data->at9s.key_D) {
|
|
case AT9S_CMD_SW_DOWN:
|
|
c->output.shoot.cmd.mode=SHOOT_MODE_SINGLE;
|
|
break;
|
|
case AT9S_CMD_SW_UP:
|
|
c->output.shoot.cmd.mode=SHOOT_MODE_BURST;
|
|
break;
|
|
default:
|
|
c->output.shoot.cmd.mode=SHOOT_MODE_SAFE;
|
|
break;
|
|
}
|
|
#endif
|
|
return CMD_OK;
|
|
}
|
|
/* Exported functions ------------------------------------------------------- */
|
|
|
|
|
|
/*************************************************************************************************************************************/
|
|
/*****************************************************************PC******************************************************************/
|
|
/*************************************************************************************************************************************/
|
|
#ifndef CMD_NOPC_FLAG
|
|
/* Private macro ------------------------------------------------------------ */
|
|
#define CMD_PCBehavior_TABLE(X) \
|
|
X(FORE, CMD_MODULE_CHASSIS) \
|
|
X(BACK, CMD_MODULE_CHASSIS) \
|
|
X(LEFT, CMD_MODULE_CHASSIS) \
|
|
X(RIGHT, CMD_MODULE_CHASSIS) \
|
|
X(ACCELERATE, CMD_MODULE_CHASSIS) \
|
|
X(DECELEBRATE, CMD_MODULE_CHASSIS) \
|
|
X(FIRE, CMD_MODULE_SHOOT) \
|
|
X(FIRE_MODE, CMD_MODULE_SHOOT) \
|
|
X(BUFF, CMD_MODULE_SHOOT) \
|
|
X(AUTOAIM, CMD_MODULE_GIMBAL | CMD_MODULE_SHOOT) \
|
|
X(OPENCOVER, CMD_MODULE_SHOOT) \
|
|
X(ROTOR, CMD_MODULE_CHASSIS) \
|
|
X(REVTRIG, CMD_MODULE_SHOOT) \
|
|
X(FOLLOWGIMBAL35, CMD_MODULE_CHASSIS)
|
|
|
|
/* 行为处理函数声明宏 */
|
|
#define CMDMACRO_FOR_DECLARE_BEHAVIOR_HANDLER_FUNCTION(BEHAVIOR, MODULE_MASK) \
|
|
static int8_t Cmd_PC_HandleBehavior##BEHAVIOR(CMD_t *c);
|
|
/* 行为处理函数指针数组构建宏 */
|
|
#define CMDMACRO_FOR_BUILD_BEHAVIOR_HANDLER_ARRAY(BEHAVIOR, MODULE_MASK) \
|
|
{CMD_BEHAVIOR_##BEHAVIOR, Cmd_PC_HandleBehavior##BEHAVIOR},
|
|
/* 行为模块映射表构建宏 */
|
|
#define CMDMACRO_FOR_BUILD_MODULE_TABLE(BEHAVIOR, MODULE_MASK) \
|
|
[CMD_BEHAVIOR_##BEHAVIOR] = MODULE_MASK,
|
|
/* 行为处理函数声明 */
|
|
CMD_PCBehavior_TABLE(CMDMACRO_FOR_DECLARE_BEHAVIOR_HANDLER_FUNCTION)
|
|
|
|
#define CMDMACRO_VAR_PCDATA(name, NAME) NAME##_DataPC_t name;
|
|
/* 宏展开函数内容 */
|
|
#define CMDMACRO_Cmd_PC_Get(name, NAME) \
|
|
bool online= name.header.online; \
|
|
pc_buffer.name= name.data.pc; \
|
|
pc->online=online;
|
|
#define CMDMACRO_PC_BuildChassisCommandFromInput(name, NAME) \
|
|
c->output.chassis.cmd.ctrl_vec.vx = 0.0f; \
|
|
c->output.chassis.cmd.ctrl_vec.vy = 0.0f; \
|
|
for (size_t i = 0; i < CMD_BEHAVIOR_NUM; i++) { \
|
|
CMD_ModuleMask_t moduleMask = behaviorModuleTable[i]; \
|
|
if (CMD_PC_IsMaskMatch(c, moduleMask)) { \
|
|
if (CMD_PC_IsBehaviorTriggered(c, i)) { \
|
|
behaviorHandlerFuncTable[i].func(c); \
|
|
} \
|
|
} \
|
|
}
|
|
#define CMDMACRO_PC_BuildGimbalCommandFromInput(name, NAME) \
|
|
c->output.gimbal.cmd.mode = GIMBAL_MODE_ABSOLUTE; \
|
|
c->output.gimbal.cmd.delta_yaw = (float)c->input.pc.data->name.mouse.x * c->timer.dt * c->params->pc.sensitivity.sens_mouse; \
|
|
c->output.gimbal.cmd.delta_pit = (float)c->input.pc.data->name.mouse.y * c->timer.dt * c->params->pc.sensitivity.sens_mouse; \
|
|
for (size_t i = 0; i < CMD_BEHAVIOR_NUM; i++) { \
|
|
CMD_ModuleMask_t moduleMask = behaviorModuleTable[i]; \
|
|
if (CMD_PC_IsMaskMatch(c, moduleMask)) { \
|
|
if (CMD_PC_IsBehaviorTriggered(c, i)) { \
|
|
behaviorHandlerFuncTable[i].func(c); \
|
|
} \
|
|
} \
|
|
}
|
|
#define CMDMACRO_PC_BuildShootCommandFromInput(name, NAME) \
|
|
c->output.shoot.cmd.firecmd = false; \
|
|
for (size_t i = 0; i < CMD_BEHAVIOR_NUM; i++) { \
|
|
CMD_ModuleMask_t moduleMask = behaviorModuleTable[i]; \
|
|
if (CMD_PC_IsMaskMatch(c, moduleMask)) { \
|
|
if (CMD_PC_IsBehaviorTriggered(c, i)) { \
|
|
behaviorHandlerFuncTable[i].func(c); \
|
|
} \
|
|
} \
|
|
} \
|
|
memcpy(c->input.pc.lastData->name.keyboard.key, c->input.pc.data->name.keyboard.key, sizeof(c->input.pc.data->name.keyboard.key)); \
|
|
c->input.pc.lastData->name.mouse.l_click = c->input.pc.data->name.mouse.l_click; \
|
|
c->input.pc.lastData->name.mouse.r_click = c->input.pc.data->name.mouse.r_click;
|
|
/* Private typedef ---------------------------------------------------------- */
|
|
typedef int8_t (*CMD_BehaviorFunc)(CMD_t *c);
|
|
typedef struct {
|
|
CMD_Behavior_t behavior;
|
|
CMD_BehaviorFunc func;
|
|
} CMD_BehaviorHandlerFunc_t;
|
|
|
|
union CMD_PC_Data_t {
|
|
CMD_RCType_TABLE(CMDMACRO_VAR_PCDATA)
|
|
};
|
|
union CMD_PC_LastData_t {
|
|
CMD_RCType_TABLE(CMDMACRO_VAR_PCDATA)
|
|
};
|
|
/* Private variables -------------------------------------------------------- */
|
|
/* 行为处理函数指针数组 */
|
|
CMD_BehaviorHandlerFunc_t behaviorHandlerFuncTable[CMD_BEHAVIOR_NUM] = {
|
|
CMD_PCBehavior_TABLE(CMDMACRO_FOR_BUILD_BEHAVIOR_HANDLER_ARRAY)
|
|
};
|
|
/* 行为模块映射表 */
|
|
static const CMD_ModuleMask_t behaviorModuleTable[CMD_BEHAVIOR_NUM] = {
|
|
CMD_PCBehavior_TABLE(CMDMACRO_FOR_BUILD_MODULE_TABLE)
|
|
};
|
|
|
|
/* 静态缓冲区 */
|
|
CMD_PC_Data_t pc_buffer;
|
|
CMD_PC_LastData_t pc_lastdata_buffer;
|
|
/* Private function -------------------------------------------------------- */
|
|
int8_t Cmd_PC_Get(CMD_Input_PC_t *pc){
|
|
CMD_RCType_TABLE(CMDMACRO_Cmd_PC_Get);
|
|
return CMD_OK;
|
|
}
|
|
|
|
static int8_t Cmd_PC_HandleBehaviorFORE(CMD_t *c){
|
|
c->output.chassis.cmd.ctrl_vec.vy += c->params->pc.sensitivity.move_sense;
|
|
return CMD_OK;
|
|
}
|
|
static int8_t Cmd_PC_HandleBehaviorBACK(CMD_t *c){
|
|
c->output.chassis.cmd.ctrl_vec.vy -= c->params->pc.sensitivity.move_sense;
|
|
return CMD_OK;
|
|
}
|
|
static int8_t Cmd_PC_HandleBehaviorLEFT(CMD_t *c){
|
|
c->output.chassis.cmd.ctrl_vec.vx -= c->params->pc.sensitivity.move_sense;
|
|
return CMD_OK;
|
|
}
|
|
static int8_t Cmd_PC_HandleBehaviorRIGHT(CMD_t *c){
|
|
c->output.chassis.cmd.ctrl_vec.vx += c->params->pc.sensitivity.move_sense;
|
|
return CMD_OK;
|
|
}
|
|
static int8_t Cmd_PC_HandleBehaviorACCELERATE(CMD_t *c){
|
|
c->output.chassis.cmd.ctrl_vec.vx *= c->params->pc.sensitivity.move_fast_sense;
|
|
c->output.chassis.cmd.ctrl_vec.vy *= c->params->pc.sensitivity.move_fast_sense;
|
|
return CMD_OK;
|
|
}
|
|
static int8_t Cmd_PC_HandleBehaviorDECELEBRATE(CMD_t *c){
|
|
c->output.chassis.cmd.ctrl_vec.vx *= c->params->pc.sensitivity.move_slow_sense;
|
|
c->output.chassis.cmd.ctrl_vec.vy *= c->params->pc.sensitivity.move_slow_sense;
|
|
return CMD_OK;
|
|
}
|
|
static int8_t Cmd_PC_HandleBehaviorFIRE(CMD_t *c){
|
|
c->output.shoot.cmd.firecmd = true;
|
|
return CMD_OK;
|
|
}
|
|
static int8_t Cmd_PC_HandleBehaviorFIRE_MODE(CMD_t *c){
|
|
c->output.shoot.cmd.mode++;
|
|
c->output.shoot.cmd.mode %= SHOOT_MODE_NUM;
|
|
return CMD_OK;
|
|
}
|
|
static int8_t Cmd_PC_HandleBehaviorBUFF(CMD_t *c){
|
|
// if (cmd->ai_status == AI_STATUS_HITSWITCH) {
|
|
// CMD_RefereeAdd(&(cmd->referee), CMD_UI_HIT_SWITCH_STOP);
|
|
// cmd->host_overwrite = false;
|
|
// cmd->ai_status = AI_STATUS_STOP;
|
|
// } else if (cmd->ai_status == AI_STATUS_AUTOAIM) {
|
|
// // 自瞄模式中切换失败提醒
|
|
// } else {
|
|
// CMD_RefereeAdd(&(cmd->referee), CMD_UI_HIT_SWITCH_START);
|
|
// cmd->ai_status = AI_STATUS_HITSWITCH;
|
|
// cmd->host_overwrite = true;
|
|
// }
|
|
return CMD_OK;
|
|
}
|
|
static int8_t Cmd_PC_HandleBehaviorAUTOAIM(CMD_t *c){
|
|
// if (cmd->ai_status == AI_STATUS_AUTOAIM) {
|
|
// cmd->host_overwrite = false;
|
|
// cmd->ai_status = AI_STATUS_STOP;
|
|
// CMD_RefereeAdd(&(cmd->referee), CMD_UI_AUTO_AIM_STOP);
|
|
// } else {
|
|
// cmd->ai_status = AI_STATUS_AUTOAIM;
|
|
// cmd->host_overwrite = true;
|
|
// CMD_RefereeAdd(&(cmd->referee), CMD_UI_AUTO_AIM_START);
|
|
// }
|
|
return CMD_OK;
|
|
}
|
|
static int8_t Cmd_PC_HandleBehaviorOPENCOVER(CMD_t *c){
|
|
// c->shoot.cover_open = !c->shoot.cover_open;
|
|
return CMD_OK;
|
|
}
|
|
static int8_t Cmd_PC_HandleBehaviorROTOR(CMD_t *c){
|
|
c->output.chassis.cmd.mode = CHASSIS_MODE_ROTOR;
|
|
c->output.chassis.cmd.mode_rotor = ROTOR_MODE_RAND;
|
|
return CMD_OK;
|
|
}
|
|
static int8_t Cmd_PC_HandleBehaviorREVTRIG(CMD_t *c){
|
|
// c->output.shoot.cmd.reverse_trig = true;
|
|
return CMD_OK;
|
|
}
|
|
static int8_t Cmd_PC_HandleBehaviorFOLLOWGIMBAL35(CMD_t *c){
|
|
c->output.chassis.cmd.mode = CHASSIS_MODE_FOLLOW_GIMBAL_35;
|
|
return CMD_OK;
|
|
}
|
|
|
|
static inline CMD_PCValue_t CMD_PC_BehaviorToValue(CMD_t *c,
|
|
CMD_Behavior_t behavior) {
|
|
return c->params->pc.map.key_map[behavior].key;
|
|
}
|
|
|
|
static inline CMD_TriggerType_t CMD_PC_BehaviorToActive(CMD_t *c,
|
|
CMD_Behavior_t behavior) {
|
|
return c->params->pc.map.key_map[behavior].trigger_type;
|
|
}
|
|
|
|
static inline bool CMD_PC_IsMaskMatch(CMD_t *c, CMD_ModuleMask_t module_mask) {
|
|
/* 构建当前各模块输入源状态的掩码 */
|
|
CMD_ModuleMask_t current_pc_mask = 0;
|
|
if (c->output.chassis.source == CMD_SRC_PC) current_pc_mask |= CMD_MODULE_CHASSIS;
|
|
if (c->output.gimbal.source == CMD_SRC_PC) current_pc_mask |= CMD_MODULE_GIMBAL;
|
|
if (c->output.shoot.source == CMD_SRC_PC) current_pc_mask |= CMD_MODULE_SHOOT;
|
|
|
|
/* 检测并集 */
|
|
return (module_mask & current_pc_mask) == module_mask;
|
|
/* 检测交集 */
|
|
// return (module_mask & current_pc_mask) != 0;
|
|
}
|
|
|
|
static inline bool CMD_PC_IsBehaviorTriggered(CMD_t *c, CMD_Behavior_t behavior) {
|
|
CMD_PCValue_t value = CMD_PC_BehaviorToValue(c, behavior);
|
|
CMD_TriggerType_t active = CMD_PC_BehaviorToActive(c, behavior);
|
|
|
|
bool now_key_pressed, last_key_pressed;
|
|
|
|
/* 按下按键为鼠标左、右键 */
|
|
if (value == CMD_L_CLICK) {
|
|
now_key_pressed = c->input.pc.data->dr16.mouse.l_click;
|
|
last_key_pressed = c->input.pc.lastData->dr16.mouse.l_click;
|
|
} else if (value == CMD_R_CLICK) {
|
|
now_key_pressed = c->input.pc.data->dr16.mouse.r_click;
|
|
last_key_pressed = c->input.pc.lastData->dr16.mouse.r_click;
|
|
} else {
|
|
/* 直接用布尔值 */
|
|
now_key_pressed = c->input.pc.data->dr16.keyboard.key[value];
|
|
last_key_pressed = c->input.pc.lastData->dr16.keyboard.key[value];
|
|
/* 或者用位掩码 */
|
|
// now_key_pressed = c->input.pc.data.keyboard.bitmask & (1u << value);
|
|
// last_key_pressed = c->input.pc.lastData.keyboard.bitmask & (1u << value);
|
|
}
|
|
|
|
switch (active) {
|
|
case CMD_ACTIVE_RISING_EDGE:
|
|
return !now_key_pressed && last_key_pressed;
|
|
case CMD_ACTIVE_FALLING_EDGE:
|
|
return now_key_pressed && !last_key_pressed;
|
|
case CMD_ACTIVE_PRESSED:
|
|
return now_key_pressed;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
static int8_t Cmd_PC_BuildChassisCommandFromInput(CMD_t *c) {
|
|
if (c == NULL) {
|
|
return CMD_ERR_NULL; // 参数错误
|
|
}
|
|
// c->output.chassis.cmd.ctrl_vec.vx = c->output.chassis.cmd.ctrl_vec.vy = 0.0f;
|
|
// for (size_t i = 0; i < CMD_RCType_TABLE(CMDMACRO_KEYNUM); i++) {
|
|
// CMD_ModuleMask_t moduleMask = behaviorModuleTable[i];
|
|
// if (CMD_PC_IsMaskMatch(c, moduleMask)) {
|
|
// if (CMD_PC_IsBehaviorTriggered(c, i)) {
|
|
// behaviorHandlerFuncTable[i].func(c);
|
|
// }
|
|
// }
|
|
// }
|
|
CMD_RCType_TABLE(CMDMACRO_PC_BuildChassisCommandFromInput);
|
|
return CMD_OK;
|
|
}
|
|
|
|
static int8_t Cmd_PC_BuildGimbalCommandFromInput(CMD_t *c) {
|
|
if (c == NULL) {
|
|
return CMD_ERR_NULL; // 参数错误
|
|
}
|
|
// c->output.gimbal.cmd.mode = GIMBAL_MODE_ABSOLUTE;
|
|
// c->output.gimbal.cmd.delta_yaw = (float)c->input.pc.data->dr16.mouse.x * c->timer.dt * c->params->pc.sensitivity.sens_mouse;
|
|
// c->output.gimbal.cmd.delta_pit = (float)c->input.pc.data->dr16.mouse.y * c->timer.dt * c->params->pc.sensitivity.sens_mouse;
|
|
// for (size_t i = 0; i < CMD_RCType_TABLE(CMDMACRO_KEYNUM); i++) {
|
|
// CMD_ModuleMask_t moduleMask = behaviorModuleTable[i];
|
|
// if (CMD_PC_IsMaskMatch(c, moduleMask)) {
|
|
// if (CMD_PC_IsBehaviorTriggered(c, i)) {
|
|
// behaviorHandlerFuncTable[i].func(c);
|
|
// }
|
|
// }
|
|
// }
|
|
CMD_RCType_TABLE(CMDMACRO_PC_BuildGimbalCommandFromInput);
|
|
return CMD_OK;
|
|
}
|
|
|
|
static int8_t Cmd_PC_BuildShootCommandFromInput(CMD_t *c) {
|
|
if (c == NULL) {
|
|
return CMD_ERR_NULL; // 参数错误
|
|
}
|
|
// c->output.shoot.cmd.firecmd = false;
|
|
// for (size_t i = 0; i < CMD_RCType_TABLE(CMDMACRO_KEYNUM); i++) {
|
|
// CMD_ModuleMask_t moduleMask = behaviorModuleTable[i];
|
|
// if (CMD_PC_IsMaskMatch(c, moduleMask)) {
|
|
// if (CMD_PC_IsBehaviorTriggered(c, i)) {
|
|
// behaviorHandlerFuncTable[i].func(c);
|
|
// }
|
|
// }
|
|
// }
|
|
// memcpy(c->input.pc.lastData->dr16.keyboard.key, c->input.pc.data->dr16.keyboard.key, sizeof(c->input.pc.data->dr16.keyboard.key));
|
|
// c->input.pc.lastData->dr16.mouse.l_click = c->input.pc.data->dr16.mouse.l_click;
|
|
// c->input.pc.lastData->dr16.mouse.r_click = c->input.pc.data->dr16.mouse.r_click;
|
|
CMD_RCType_TABLE(CMDMACRO_PC_BuildShootCommandFromInput);
|
|
return CMD_OK;
|
|
}
|
|
#else
|
|
static int8_t Cmd_PC_Get(CMD_Input_PC_t *pc) {
|
|
pc->online=false;
|
|
return CMD_OK;
|
|
}
|
|
static int8_t Cmd_PC_BuildChassisCommandFromInput(CMD_t *c) {
|
|
if (c == NULL) {
|
|
return CMD_ERR_NULL; // 参数错误
|
|
}
|
|
|
|
return CMD_OK;
|
|
}
|
|
static int8_t Cmd_PC_BuildGimbalCommandFromInput(CMD_t *c) {
|
|
if (c == NULL) {
|
|
return CMD_ERR_NULL; // 参数错误
|
|
}
|
|
|
|
return CMD_OK;
|
|
}
|
|
static int8_t Cmd_PC_BuildShootCommandFromInput(CMD_t *c) {
|
|
if (c == NULL) {
|
|
return CMD_ERR_NULL; // 参数错误
|
|
}
|
|
|
|
return CMD_OK;
|
|
}
|
|
#endif
|
|
|
|
/*************************************************************************************************************************************/
|
|
/****************************************************************NUC******************************************************************/
|
|
/*************************************************************************************************************************************/
|
|
|
|
/* Includes ----------------------------------------------------------------- */
|
|
/* Private typedef ---------------------------------------------------------- */
|
|
/* Private define ----------------------------------------------------------- */
|
|
/* Private macro ------------------------------------------------------------ */
|
|
/* Private variables -------------------------------------------------------- */
|
|
/* Private function -------------------------------------------------------- */
|
|
int8_t Cmd_NUC_Get(CMD_Input_NUC_t *nuc) {
|
|
nuc->online=0;
|
|
return CMD_OK;
|
|
}
|
|
/* Exported functions ------------------------------------------------------- */
|
|
|
|
int8_t Cmd_NUC_BuildChassisCommandFromInput(CMD_t *c) {
|
|
if (c == NULL) {
|
|
return CMD_ERR_NULL; // 参数错误
|
|
}
|
|
|
|
return CMD_OK;
|
|
}
|
|
|
|
int8_t Cmd_NUC_BuildGimbalCommandFromInput(CMD_t *c) {
|
|
if (c == NULL) {
|
|
return CMD_ERR_NULL; // 参数错误
|
|
}
|
|
|
|
return CMD_OK;
|
|
}
|
|
int8_t Cmd_NUC_BuildShootCommandFromInput(CMD_t *c) {
|
|
if (c == NULL) {
|
|
return CMD_ERR_NULL; // 参数错误
|
|
}
|
|
|
|
return CMD_OK;
|
|
}
|
|
|
|
/*************************************************************************************************************************************/
|
|
/***************************************************************REF*******************************************************************/
|
|
/*************************************************************************************************************************************/
|
|
|
|
/* Includes ----------------------------------------------------------------- */
|
|
/* Private typedef ---------------------------------------------------------- */
|
|
/* Private define ----------------------------------------------------------- */
|
|
/* Private macro ------------------------------------------------------------ */
|
|
/* Private variables -------------------------------------------------------- */
|
|
/* Private function -------------------------------------------------------- */
|
|
int8_t Cmd_REF_Get(CMD_Input_REF_t *nuc) {
|
|
nuc->online=0;
|
|
return CMD_OK;
|
|
}
|
|
/* Exported functions ------------------------------------------------------- */
|
|
|
|
int8_t Cmd_REF_BuildChassisCommandFromInput(CMD_t *c) {
|
|
if (c == NULL) {
|
|
return CMD_ERR_NULL; // 参数错误
|
|
}
|
|
|
|
return CMD_OK;
|
|
}
|
|
|
|
int8_t Cmd_REF_BuildGimbalCommandFromInput(CMD_t *c) {
|
|
if (c == NULL) {
|
|
return CMD_ERR_NULL; // 参数错误
|
|
}
|
|
|
|
return CMD_OK;
|
|
}
|
|
int8_t Cmd_REF_BuildShootCommandFromInput(CMD_t *c) {
|
|
if (c == NULL) {
|
|
return CMD_ERR_NULL; // 参数错误
|
|
}
|
|
|
|
return CMD_OK;
|
|
}
|
|
/*************************************************************************************************************************************/
|
|
/***************************************************************仲裁器****************************************************************/
|
|
/*************************************************************************************************************************************/
|
|
|
|
/* Includes ----------------------------------------------------------------- */
|
|
/* Private typedef ---------------------------------------------------------- */
|
|
/* Private define ----------------------------------------------------------- */
|
|
/* Private macro ------------------------------------------------------------ */
|
|
/* Private variables -------------------------------------------------------- */
|
|
/* Private function -------------------------------------------------------- */
|
|
static inline bool Cmd_isREFOnline(CMD_t *c){return c->input.ref.online;}
|
|
static inline bool Cmd_isNUCOnline(CMD_t *c){return c->input.nuc.online;}
|
|
static inline bool Cmd_isRCOnline(CMD_t *c){return c->input.rc.online;}
|
|
static inline bool Cmd_isPCOnline(CMD_t *c){return c->input.pc.online;}
|
|
CMD_InputSource_t Cmd_GetHighestPrioritySource(CMD_t *c) {
|
|
for (int i = 0; i < CMD_SRC_NUM; i++) {
|
|
CMD_InputSource_t source = c->params->sourcePriorityConfigs[i].source;
|
|
switch (source) {
|
|
case CMD_SRC_REF:
|
|
if (Cmd_isREFOnline(c)) {
|
|
return CMD_SRC_REF;
|
|
}
|
|
break;
|
|
case CMD_SRC_NUC:
|
|
if (Cmd_isNUCOnline(c)) {
|
|
return CMD_SRC_NUC;
|
|
}
|
|
break;
|
|
case CMD_SRC_RC:
|
|
if (Cmd_isRCOnline(c)) {
|
|
return CMD_SRC_RC;
|
|
}
|
|
break;
|
|
case CMD_SRC_PC:
|
|
if (Cmd_isPCOnline(c)) {
|
|
return CMD_SRC_PC;
|
|
}
|
|
}
|
|
}
|
|
return CMD_SRC_ERR;
|
|
}
|
|
|
|
int8_t Cmd_Arbiter(CMD_t *c) {
|
|
if (c == NULL) {
|
|
return CMD_ERR_NULL; // 参数错误
|
|
}
|
|
CMD_InputSource_t source = Cmd_GetHighestPrioritySource(c);
|
|
|
|
c->output.chassis.source = source;
|
|
c->output.gimbal.source = source;
|
|
c->output.shoot.source = source;
|
|
|
|
|
|
|
|
return CMD_OK;
|
|
}
|
|
|
|
int8_t Cmd_Switch_RCorPC(CMD_t *c) {
|
|
if (c == NULL) {
|
|
return CMD_ERR_NULL; // 参数错误
|
|
}
|
|
|
|
return CMD_OK;
|
|
}
|
|
|
|
/*************************************************************************************************************************************/
|
|
/***************************************************************主结构*****************************************************************/
|
|
/*************************************************************************************************************************************/
|
|
|
|
/* Includes ----------------------------------------------------------------- */
|
|
/* Private typedef ---------------------------------------------------------- */
|
|
typedef int8_t (*CMD_BuildCommandFunc)(CMD_t *c);
|
|
typedef struct {
|
|
CMD_InputSource_t source;
|
|
CMD_BuildCommandFunc chassisFunc;
|
|
CMD_BuildCommandFunc gimbalFunc;
|
|
CMD_BuildCommandFunc shootFunc;
|
|
} CMD_SourceHandler_t;
|
|
/* Private macro ------------------------------------------------------------ */
|
|
/* Private variables -------------------------------------------------------- */
|
|
CMD_SourceHandler_t sourceHandlers[CMD_SRC_NUM] = {
|
|
{CMD_SRC_RC, Cmd_RC_BuildChassisCommandFromInput, Cmd_RC_BuildGimbalCommandFromInput, Cmd_RC_BuildShootCommandFromInput},
|
|
{CMD_SRC_PC, Cmd_PC_BuildChassisCommandFromInput, Cmd_PC_BuildGimbalCommandFromInput, Cmd_PC_BuildShootCommandFromInput},
|
|
{CMD_SRC_NUC, Cmd_NUC_BuildChassisCommandFromInput, Cmd_NUC_BuildGimbalCommandFromInput, Cmd_NUC_BuildShootCommandFromInput},
|
|
{CMD_SRC_REF, Cmd_REF_BuildChassisCommandFromInput, Cmd_REF_BuildGimbalCommandFromInput, Cmd_REF_BuildShootCommandFromInput},
|
|
};
|
|
/* Private function -------------------------------------------------------- */
|
|
int8_t Cmd_OFFLINE(CMD_t *c) {
|
|
if (c == NULL) {
|
|
return CMD_ERR_NULL; // 参数错误
|
|
}
|
|
c->output.chassis.cmd.mode =CHASSIS_MODE_RELAX;
|
|
c->output.gimbal.cmd.mode =GIMBAL_MODE_RELAX;
|
|
c->output.shoot.cmd.mode =SHOOT_MODE_SAFE;
|
|
return CMD_OK;
|
|
}
|
|
/* Exported functions ------------------------------------------------------- */
|
|
int8_t Cmd_Init(CMD_t *c, CMD_Params_t *params) {
|
|
|
|
if (c == NULL || params == NULL) {
|
|
return CMD_ERR_NULL; // 参数错误
|
|
}
|
|
c->params = params;
|
|
c->input.rc.data=&rc_buffer;
|
|
#ifndef CMD_NOPC_FLAG
|
|
c->input.pc.lastData=&pc_lastdata_buffer;
|
|
c->input.pc.data=&pc_buffer;
|
|
#endif
|
|
return CMD_OK;
|
|
}
|
|
|
|
int8_t Cmd_GenerateCommand(CMD_t *c) {
|
|
if (c == NULL) {
|
|
return CMD_ERR_NULL; // 参数错误
|
|
}
|
|
Cmd_Arbiter(c);
|
|
Cmd_RC_Get(&c->input.rc);
|
|
Cmd_PC_Get(&c->input.pc);
|
|
Cmd_NUC_Get(&c->input.nuc);
|
|
Cmd_REF_Get(&c->input.ref);
|
|
c->timer.now =BSP_TIME_Get_us() / 1000000.0f;
|
|
c->timer.dt =(BSP_TIME_Get_us() - c->timer.last) / 1000000.0f;
|
|
c->timer.last =BSP_TIME_Get_us();
|
|
if (c->output.chassis.source >= CMD_SRC_NUM || c->output.gimbal.source >= CMD_SRC_NUM || c->output.shoot.source >= CMD_SRC_NUM) {
|
|
Cmd_OFFLINE(c);
|
|
return CMD_ERR_SOURCE; // 输入源错误
|
|
}
|
|
sourceHandlers[c->output.chassis.source].chassisFunc(c);
|
|
sourceHandlers[c->output.gimbal.source].gimbalFunc(c);
|
|
sourceHandlers[c->output.shoot.source].shootFunc(c);
|
|
return CMD_OK;
|
|
}
|