603 lines
22 KiB
C
603 lines
22 KiB
C
/*
|
|
控制命令
|
|
*/
|
|
#include "module/cmd.h"
|
|
#include <stdint.h>
|
|
#include <string.h>
|
|
|
|
/*************************************************************************************************************************************/
|
|
/***************************************************************仲裁器****************************************************************/
|
|
/*************************************************************************************************************************************/
|
|
|
|
/* Includes ----------------------------------------------------------------- */
|
|
/* Private typedef ---------------------------------------------------------- */
|
|
/* Private define ----------------------------------------------------------- */
|
|
/* Private macro ------------------------------------------------------------ */
|
|
/* Private variables -------------------------------------------------------- */
|
|
/* Private function -------------------------------------------------------- */
|
|
|
|
bool isREFOnline(CMD_t *c){return c->inputData.ref.online;}
|
|
bool isNUCOnline(CMD_t *c){return c->inputData.nuc.online;}
|
|
bool isRCOnline(CMD_t *c){return c->inputData.rc.online;}
|
|
bool isPCOnline(CMD_t *c){return c->inputData.pc.online;}
|
|
void PriorityConfigsRanking(CMD_Param_t *param) {
|
|
static bool init=false;
|
|
if(init) return;
|
|
for (int i = 0; i < CMD_SRC_NUM - 1; i++) {
|
|
for (int j = 0; j < CMD_SRC_NUM - i - 1; j++) {
|
|
if (param->sourcePriorityConfigs[j].priority < param->sourcePriorityConfigs[j + 1].priority) {
|
|
CMD_SourcePriorityConfig_t temp = param->sourcePriorityConfigs[j];
|
|
param->sourcePriorityConfigs[j] = param->sourcePriorityConfigs[j + 1];
|
|
param->sourcePriorityConfigs[j + 1] = temp;
|
|
}
|
|
}
|
|
}
|
|
init=!init;
|
|
}
|
|
CMD_InputSource_t 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_REFEREE:
|
|
if (isREFOnline(c)) {
|
|
return CMD_SRC_REFEREE;
|
|
}
|
|
break;
|
|
case CMD_SRC_NUC:
|
|
if (isNUCOnline(c)) {
|
|
return CMD_SRC_NUC;
|
|
}
|
|
break;
|
|
case CMD_SRC_RC:
|
|
if (isRCOnline(c)) {
|
|
return CMD_SRC_RC;
|
|
}
|
|
break;
|
|
case CMD_SRC_PC:
|
|
if (isPCOnline(c)) {
|
|
return CMD_SRC_PC;
|
|
}
|
|
}
|
|
}
|
|
return CMD_SRC_ERR;
|
|
}
|
|
|
|
int8_t Cmd_Arbiter(CMD_t *c) {
|
|
if (c == NULL) {
|
|
return CMD_ERR_NULL; // 参数错误
|
|
}
|
|
|
|
PriorityConfigsRanking(&c->params);
|
|
|
|
CMD_InputSource_t source = getHighestPrioritySource(c);
|
|
|
|
c->outCmd.chassis.source = source;
|
|
c->outCmd.gimbal.source = source;
|
|
c->outCmd.shoot.source = source;
|
|
|
|
return CMD_OK;
|
|
}
|
|
/* Exported functions ------------------------------------------------------- */
|
|
|
|
|
|
|
|
/*************************************************************************************************************************************/
|
|
/****************************************************************RC*******************************************************************/
|
|
/*************************************************************************************************************************************/
|
|
/* API ---------------------------------------------------------------------- */
|
|
/* Mrobot生成 */
|
|
#define RC_SELECT_Index 1
|
|
/* 扩展接口 */
|
|
#if RC_SELECT_Index == 0
|
|
#define FOR_EACH_RC(X) X(dr16, DR16)
|
|
#elif RC_SELECT_Index == 1
|
|
#define FOR_EACH_RC(X) X(at9s, AT9S)
|
|
#endif
|
|
/* Includes ----------------------------------------------------------------- */
|
|
#if RC_SELECT_Index == 0
|
|
#include "device/dr16.h"
|
|
#elif RC_SELECT_Index == 1
|
|
#include "device/at9s_pro.h"
|
|
#endif
|
|
/* Private typedef ---------------------------------------------------------- */
|
|
/* Private define ----------------------------------------------------------- */
|
|
/* Private macro ------------------------------------------------------------ */
|
|
#define RC_X_FIELD_MACRO(name, NAME) DEVICE_##NAME##_t name;
|
|
#define RC_X_EXTERN_MACRO(name, NAME) extern DEVICE_##NAME##_t name##_out;
|
|
#define RC_X_COPYDEFINE_MACRO(name, NAME) \
|
|
static void copy_##name(rc_inputdata_u *dst) { dst->name = name##_out; }
|
|
#define RC_X_COPYREFERENCE_MACRO(name, NAME) copy_##name,
|
|
union rc_inputdata_u{
|
|
FOR_EACH_RC(RC_X_FIELD_MACRO)
|
|
};
|
|
FOR_EACH_RC(RC_X_EXTERN_MACRO)
|
|
FOR_EACH_RC(RC_X_COPYDEFINE_MACRO)
|
|
/* Private variables -------------------------------------------------------- */
|
|
/*静态缓冲区*/
|
|
static rc_inputdata_u rc_buffer;
|
|
/* Private function -------------------------------------------------------- */
|
|
int8_t Cmd_Get_rc(CMD_InputData_RC_t *rc)
|
|
{
|
|
FOR_EACH_RC(RC_X_COPYREFERENCE_MACRO)(&rc_buffer);
|
|
rc->inputData = &rc_buffer;
|
|
rc->type = RC_SELECT_Index;
|
|
return CMD_OK;
|
|
}
|
|
|
|
|
|
int8_t Cmd_BuildChassisCommandFromInput_rc(CMD_t *c) {
|
|
if (c == NULL) {
|
|
return CMD_ERR_NULL; // 参数错误
|
|
}
|
|
#if RC_SELECT_Index == 0
|
|
c->input_rc.inputData->dr16.data =
|
|
#elif RC_SELECT_Index == 1
|
|
switch (c->inputData.rc.inputData->at9s.data.key_E) {
|
|
case AT9S_CMD_SW_DOWN:
|
|
c->outCmd.chassis.cmd.mode = CHASSIS_MODE_RELAX;
|
|
break;
|
|
case AT9S_CMD_SW_MID:
|
|
c->outCmd.chassis.cmd.mode = CHASSIS_MODE_FOLLOW_GIMBAL;
|
|
break;
|
|
case AT9S_CMD_SW_UP:
|
|
c->outCmd.chassis.cmd.mode = CHASSIS_MODE_ROTOR;
|
|
break;
|
|
default:
|
|
c->outCmd.chassis.cmd.mode = CHASSIS_MODE_RELAX;
|
|
break;
|
|
}
|
|
c->outCmd.chassis.cmd.ctrl_vec.vx = c->inputData.rc.inputData->at9s.data.ch_l_y;
|
|
c->outCmd.chassis.cmd.ctrl_vec.vy = c->inputData.rc.inputData->at9s.data.ch_l_x;
|
|
c->outCmd.chassis.cmd.ctrl_vec.wz = c->inputData.rc.inputData->at9s.data.ch_r_x;
|
|
#endif
|
|
return CMD_OK;
|
|
}
|
|
|
|
int8_t Cmd_BuildGimbalCommandFromInput_rc(CMD_t *c) {
|
|
if (c == NULL) {
|
|
return CMD_ERR_NULL; // 参数错误
|
|
}
|
|
#if RC_SELECT_Index == 0
|
|
c->input_rc.inputData->dr16.data =
|
|
#elif RC_SELECT_Index == 1
|
|
switch (c->inputData.rc.inputData->at9s.data.key_G) {
|
|
case AT9S_CMD_SW_DOWN:
|
|
c->outCmd.gimbal.cmd.mode = GIMBAL_MODE_RELAX;
|
|
c->outCmd.gimbal.cmd.delta_yaw = 0.0f;
|
|
c->outCmd.gimbal.cmd.delta_pit = 0.0f;
|
|
break;
|
|
case AT9S_CMD_SW_MID:
|
|
c->outCmd.gimbal.cmd.mode = GIMBAL_MODE_ABSOLUTE;
|
|
c->outCmd.gimbal.cmd.delta_yaw = -at9s_out.data.ch_l_x * 2.0f;
|
|
c->outCmd.gimbal.cmd.delta_pit = -at9s_out.data.ch_l_y * 1.5f;
|
|
break;
|
|
case AT9S_CMD_SW_UP:
|
|
c->outCmd.gimbal.cmd.mode = GIMBAL_MODE_ABSOLUTE;
|
|
c->outCmd.gimbal.cmd.delta_yaw = -at9s_out.data.ch_l_x * 2.0f;
|
|
c->outCmd.gimbal.cmd.delta_pit = -at9s_out.data.ch_l_y * 1.5f;
|
|
break;
|
|
default:
|
|
c->outCmd.gimbal.cmd.mode = GIMBAL_MODE_RELAX;
|
|
c->outCmd.gimbal.cmd.delta_yaw = 0.0f;
|
|
c->outCmd.gimbal.cmd.delta_pit = 0.0f;
|
|
break;
|
|
}
|
|
#endif
|
|
return CMD_OK;
|
|
}
|
|
|
|
|
|
int8_t Cmd_BuildShootCommandFromInput_rc(CMD_t *c) {
|
|
if (c == NULL) {
|
|
return CMD_ERR_NULL; // 参数错误
|
|
}
|
|
#if RC_SELECT_Index == 0
|
|
c->input_rc.inputData->dr16.data =
|
|
#elif RC_SELECT_Index == 1
|
|
switch (c->inputData.rc.inputData->at9s.data.key_C) {
|
|
case AT9S_CMD_SW_DOWN:
|
|
c->outCmd.shoot.cmd.online = at9s_out.online;
|
|
c->outCmd.shoot.cmd.ready = true;
|
|
c->outCmd.shoot.cmd.firecmd = true;
|
|
break;
|
|
case AT9S_CMD_SW_MID:
|
|
c->outCmd.shoot.cmd.online = at9s_out.online;
|
|
c->outCmd.shoot.cmd.ready = true;
|
|
c->outCmd.shoot.cmd.firecmd = false;
|
|
break;
|
|
case AT9S_CMD_SW_UP:
|
|
c->outCmd.shoot.cmd.online = at9s_out.online;
|
|
c->outCmd.shoot.cmd.ready = false;
|
|
c->outCmd.shoot.cmd.firecmd = false;
|
|
break;
|
|
default:
|
|
c->outCmd.shoot.cmd.online = at9s_out.online;
|
|
c->outCmd.shoot.cmd.ready = false;
|
|
c->outCmd.shoot.cmd.firecmd = false;
|
|
break;
|
|
}
|
|
#endif
|
|
return CMD_OK;
|
|
}
|
|
/* Exported functions ------------------------------------------------------- */
|
|
|
|
|
|
/*************************************************************************************************************************************/
|
|
/*****************************************************************PC******************************************************************/
|
|
/*************************************************************************************************************************************/
|
|
|
|
/* Includes ----------------------------------------------------------------- */
|
|
/* Private typedef ---------------------------------------------------------- */
|
|
typedef int8_t (*CMD_BehaviorFunc)(CMD_t *c);
|
|
typedef struct {
|
|
CMD_Behavior_t behavior;
|
|
CMD_BehaviorFunc func;
|
|
} CMD_BehaviorHandlerFunc_t;
|
|
|
|
/* Private define ----------------------------------------------------------- */
|
|
/* Private macro ------------------------------------------------------------ */
|
|
#define PC_MACRO_BEHAVIOR_TABLE(X) \
|
|
X(FORE) \
|
|
X(BACK) \
|
|
X(LEFT) \
|
|
X(RIGHT) \
|
|
X(ACCELERATE) \
|
|
X(DECELEBRATE) \
|
|
X(FIRE) \
|
|
X(FIRE_MODE) \
|
|
X(BUFF) \
|
|
X(AUTOAIM) \
|
|
X(OPENCOVER) \
|
|
X(ROTOR) \
|
|
X(REVTRIG) \
|
|
X(FOLLOWGIMBAL35)
|
|
#define PC_MACRO_FOR_DECLARE_HANDLER_FUNCTION(BEHAVIOR) \
|
|
static int8_t Cmd_PC_HandleBehavior##BEHAVIOR(CMD_t *c);
|
|
|
|
#define PC_MACRO_FOR_BUILD_BEHAVIOR_HANDLER_ARRAY(BEHAVIOR) \
|
|
{CMD_BEHAVIOR_##BEHAVIOR, Cmd_PC_HandleBehavior##BEHAVIOR},
|
|
|
|
PC_MACRO_BEHAVIOR_TABLE(PC_MACRO_FOR_DECLARE_HANDLER_FUNCTION)
|
|
/* Private variables -------------------------------------------------------- */
|
|
CMD_BehaviorHandlerFunc_t behaviorHandlerFunc[] = {
|
|
PC_MACRO_BEHAVIOR_TABLE(PC_MACRO_FOR_BUILD_BEHAVIOR_HANDLER_ARRAY)
|
|
};
|
|
/* Private function -------------------------------------------------------- */
|
|
static int8_t Cmd_PC_HandleBehaviorFORE(CMD_t *c){
|
|
c->outCmd.chassis.cmd.ctrl_vec.vy += c->params.pc.sensitivity.move_sense;
|
|
return CMD_OK;
|
|
}
|
|
static int8_t Cmd_PC_HandleBehaviorBACK(CMD_t *c){
|
|
c->outCmd.chassis.cmd.ctrl_vec.vy -= c->params.pc.sensitivity.move_sense;
|
|
return CMD_OK;
|
|
}
|
|
static int8_t Cmd_PC_HandleBehaviorLEFT(CMD_t *c){
|
|
c->outCmd.chassis.cmd.ctrl_vec.vx -= c->params.pc.sensitivity.move_sense;
|
|
return CMD_OK;
|
|
}
|
|
static int8_t Cmd_PC_HandleBehaviorRIGHT(CMD_t *c){
|
|
c->outCmd.chassis.cmd.ctrl_vec.vx += c->params.pc.sensitivity.move_sense;
|
|
return CMD_OK;
|
|
}
|
|
static int8_t Cmd_PC_HandleBehaviorACCELERATE(CMD_t *c){
|
|
c->outCmd.chassis.cmd.ctrl_vec.vx *= c->params.pc.sensitivity.move_fast_sense;
|
|
c->outCmd.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->outCmd.chassis.cmd.ctrl_vec.vx *= c->params.pc.sensitivity.move_slow_sense;
|
|
c->outCmd.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->outCmd.shoot.cmd.firecmd = true;
|
|
return CMD_OK;
|
|
}
|
|
static int8_t Cmd_PC_HandleBehaviorFIRE_MODE(CMD_t *c){
|
|
c->outCmd.shoot.cmd.mode++;
|
|
c->outCmd.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->outCmd.chassis.cmd.mode = CHASSIS_MODE_ROTOR;
|
|
c->outCmd.chassis.cmd.mode_rotor = ROTOR_MODE_RAND;
|
|
return CMD_OK;
|
|
}
|
|
static int8_t Cmd_PC_HandleBehaviorREVTRIG(CMD_t *c){
|
|
// c->outCmd.shoot.cmd.reverse_trig = true;
|
|
return CMD_OK;
|
|
}
|
|
static int8_t Cmd_PC_HandleBehaviorFOLLOWGIMBAL35(CMD_t *c){
|
|
c->outCmd.chassis.cmd.mode = CHASSIS_MODE_FOLLOW_GIMBAL_35;
|
|
return CMD_OK;
|
|
}
|
|
|
|
static inline CMD_KeyValue_t CMD_BehaviorToKey(CMD_t *c,
|
|
CMD_Behavior_t behavior) {
|
|
return c->params.pc.map.key_map[behavior].key;
|
|
}
|
|
|
|
static inline CMD_TriggerType_t CMD_BehaviorToActive(CMD_t *c,
|
|
CMD_Behavior_t behavior) {
|
|
return c->params.pc.map.key_map[behavior].trigger_type;
|
|
}
|
|
|
|
int8_t Cmd_Get_pc(CMD_InputData_PC_t *pc)
|
|
{
|
|
pc->online = false;
|
|
// pc->data=
|
|
return CMD_OK;
|
|
}
|
|
|
|
static bool CMD_BehaviorOccurredRc(CMD_t *c, CMD_Behavior_t behavior) {
|
|
CMD_KeyValue_t key = CMD_BehaviorToKey(c, behavior);
|
|
CMD_TriggerType_t active = CMD_BehaviorToActive(c, behavior);
|
|
|
|
bool now_key_pressed, last_key_pressed;
|
|
|
|
/* 按下按键为鼠标左、右键 */
|
|
if (key == CMD_L_CLICK) {
|
|
now_key_pressed = c->inputData.pc.data.mouse.l_click;
|
|
last_key_pressed = c->inputData.pc.lastData.mouse.l_click;
|
|
} else if (key == CMD_R_CLICK) {
|
|
now_key_pressed = c->inputData.pc.data.mouse.r_click;
|
|
last_key_pressed = c->inputData.pc.lastData.mouse.r_click;
|
|
} else {
|
|
now_key_pressed = c->inputData.pc.data.key & (1u << key);
|
|
last_key_pressed = c->inputData.pc.lastData.key & (1u << key);
|
|
}
|
|
|
|
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;
|
|
}
|
|
}
|
|
|
|
/* Exported functions ------------------------------------------------------- */
|
|
|
|
|
|
|
|
static void CMD_PcLogic(CMD_t *c, float dt_sec) {
|
|
c->outCmd.gimbal.cmd.mode = GIMBAL_MODE_ABSOLUTE;
|
|
|
|
c->outCmd.gimbal.cmd.delta_yaw = (float)c->inputData.pc.data.mouse.x * dt_sec * c->params.pc.sensitivity.sens_mouse;
|
|
c->outCmd.gimbal.cmd.delta_pit = (float)c->inputData.pc.data.mouse.y * dt_sec * c->params.pc.sensitivity.sens_mouse;
|
|
c->outCmd.chassis.cmd.ctrl_vec.vx = c->outCmd.chassis.cmd.ctrl_vec.vy = 0.0f;
|
|
|
|
c->outCmd.shoot.cmd.firecmd = false;
|
|
|
|
for (size_t i = 0; i < CMD_BEHAVIOR_NUM; i++) {
|
|
if (CMD_BehaviorOccurredRc(&c, behaviorHandlerFunc[i].behavior)) {
|
|
behaviorHandlerFunc[i].func(c);
|
|
}
|
|
}
|
|
|
|
c->inputData.pc.lastData.key = c->inputData.pc.data.key;
|
|
c->inputData.pc.lastData.mouse.l_click = c->inputData.pc.data.mouse.l_click;
|
|
c->inputData.pc.lastData.mouse.r_click = c->inputData.pc.data.mouse.r_click;
|
|
}
|
|
|
|
int8_t Cmd_BuildChassisCommandFromInput_pc(CMD_t *c) {
|
|
if (c == NULL) {
|
|
return CMD_ERR_NULL; // 参数错误
|
|
}
|
|
|
|
return CMD_OK;
|
|
}
|
|
|
|
int8_t Cmd_BuildGimbalCommandFromInput_pc(CMD_t *c) {
|
|
if (c == NULL) {
|
|
return CMD_ERR_NULL; // 参数错误
|
|
}
|
|
|
|
return CMD_OK;
|
|
}
|
|
int8_t Cmd_BuildShootCommandFromInput_pc(CMD_t *c) {
|
|
if (c == NULL) {
|
|
return CMD_ERR_NULL; // 参数错误
|
|
}
|
|
|
|
c->outCmd.shoot.cmd.firecmd = false;
|
|
|
|
for (size_t i = 0; i < CMD_BEHAVIOR_NUM; i++) {
|
|
if (CMD_BehaviorOccurredRc(&c, behaviorHandlerFunc[i].behavior)) {
|
|
behaviorHandlerFunc[i].func(c);
|
|
}
|
|
}
|
|
|
|
c->inputData.pc.lastData.key = c->inputData.pc.data.key;
|
|
c->inputData.pc.lastData.mouse.l_click = c->inputData.pc.data.mouse.l_click;
|
|
c->inputData.pc.lastData.mouse.r_click = c->inputData.pc.data.mouse.r_click;
|
|
|
|
return CMD_OK;
|
|
}
|
|
/*************************************************************************************************************************************/
|
|
/****************************************************************NUC******************************************************************/
|
|
/*************************************************************************************************************************************/
|
|
|
|
/* Includes ----------------------------------------------------------------- */
|
|
/* Private typedef ---------------------------------------------------------- */
|
|
/* Private define ----------------------------------------------------------- */
|
|
/* Private macro ------------------------------------------------------------ */
|
|
/* Private variables -------------------------------------------------------- */
|
|
/* Private function -------------------------------------------------------- */
|
|
/* Exported functions ------------------------------------------------------- */
|
|
|
|
int8_t Cmd_BuildChassisCommandFromInput_nuc(CMD_t *c) {
|
|
if (c == NULL) {
|
|
return CMD_ERR_NULL; // 参数错误
|
|
}
|
|
|
|
return CMD_OK;
|
|
}
|
|
|
|
int8_t Cmd_BuildGimbalCommandFromInput_nuc(CMD_t *c) {
|
|
if (c == NULL) {
|
|
return CMD_ERR_NULL; // 参数错误
|
|
}
|
|
|
|
return CMD_OK;
|
|
}
|
|
int8_t Cmd_BuildShootCommandFromInput_nuc(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 -------------------------------------------------------- */
|
|
/* Exported functions ------------------------------------------------------- */
|
|
|
|
int8_t Cmd_BuildChassisCommandFromInput_referee(CMD_t *c) {
|
|
if (c == NULL) {
|
|
return CMD_ERR_NULL; // 参数错误
|
|
}
|
|
|
|
return CMD_OK;
|
|
}
|
|
|
|
int8_t Cmd_BuildGimbalCommandFromInput_referee(CMD_t *c) {
|
|
if (c == NULL) {
|
|
return CMD_ERR_NULL; // 参数错误
|
|
}
|
|
|
|
return CMD_OK;
|
|
}
|
|
int8_t Cmd_BuildShootCommandFromInput_referee(CMD_t *c) {
|
|
if (c == NULL) {
|
|
return CMD_ERR_NULL; // 参数错误
|
|
}
|
|
|
|
return CMD_OK;
|
|
}
|
|
|
|
/*************************************************************************************************************************************/
|
|
/***************************************************************分发命令***************************************************************/
|
|
/*************************************************************************************************************************************/
|
|
|
|
/* Includes ----------------------------------------------------------------- */
|
|
/* Private typedef ---------------------------------------------------------- */
|
|
/* Private define ----------------------------------------------------------- */
|
|
/* Private macro ------------------------------------------------------------ */
|
|
/* Private variables -------------------------------------------------------- */
|
|
/* Private function -------------------------------------------------------- */
|
|
/* Exported functions ------------------------------------------------------- */
|
|
|
|
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;
|
|
|
|
CMD_SourceHandler_t sourceHandlers[CMD_SRC_NUM] = {
|
|
{CMD_SRC_RC, Cmd_BuildChassisCommandFromInput_rc, Cmd_BuildGimbalCommandFromInput_rc, Cmd_BuildShootCommandFromInput_rc},
|
|
{CMD_SRC_PC, Cmd_BuildChassisCommandFromInput_pc, Cmd_BuildGimbalCommandFromInput_pc, Cmd_BuildShootCommandFromInput_pc},
|
|
{CMD_SRC_NUC, Cmd_BuildChassisCommandFromInput_nuc, Cmd_BuildGimbalCommandFromInput_nuc, Cmd_BuildShootCommandFromInput_nuc},
|
|
{CMD_SRC_REFEREE, Cmd_BuildChassisCommandFromInput_referee, Cmd_BuildGimbalCommandFromInput_referee, Cmd_BuildShootCommandFromInput_referee},
|
|
};
|
|
|
|
int8_t Cmd_GenerateCommand(CMD_t *c) {
|
|
if (c == NULL) {
|
|
return CMD_ERR_NULL; // 参数错误
|
|
}
|
|
|
|
Cmd_Arbiter(c);
|
|
switch (c->outCmd.chassis.source) {
|
|
case CMD_SRC_RC:
|
|
sourceHandlers[CMD_SRC_RC].chassisFunc(c);
|
|
break;
|
|
case CMD_SRC_PC:
|
|
sourceHandlers[CMD_SRC_PC].chassisFunc(c);
|
|
break;
|
|
case CMD_SRC_NUC:
|
|
sourceHandlers[CMD_SRC_NUC].chassisFunc(c);
|
|
break;
|
|
case CMD_SRC_REFEREE:
|
|
sourceHandlers[CMD_SRC_REFEREE].chassisFunc(c);
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
switch (c->outCmd.gimbal.source) {
|
|
case CMD_SRC_RC:
|
|
sourceHandlers[CMD_SRC_RC].gimbalFunc(c);
|
|
break;
|
|
case CMD_SRC_PC:
|
|
sourceHandlers[CMD_SRC_PC].gimbalFunc(c);
|
|
break;
|
|
case CMD_SRC_NUC:
|
|
sourceHandlers[CMD_SRC_NUC].gimbalFunc(c);
|
|
break;
|
|
case CMD_SRC_REFEREE:
|
|
sourceHandlers[CMD_SRC_REFEREE].gimbalFunc(c);
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
switch (c->outCmd.shoot.source) {
|
|
case CMD_SRC_RC:
|
|
sourceHandlers[CMD_SRC_RC].shootFunc(c);
|
|
break;
|
|
case CMD_SRC_PC:
|
|
sourceHandlers[CMD_SRC_PC].shootFunc(c);
|
|
break;
|
|
case CMD_SRC_NUC:
|
|
sourceHandlers[CMD_SRC_NUC].shootFunc(c);
|
|
break;
|
|
case CMD_SRC_REFEREE:
|
|
sourceHandlers[CMD_SRC_REFEREE].shootFunc(c);
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
|
|
return CMD_OK;
|
|
}
|