422 lines
16 KiB
C
422 lines
16 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->input_ref.online;}
|
|
bool isNUCOnline(CMD_t *c){return c->input_nuc.online;}
|
|
bool isRCOnline(CMD_t *c){return c->input_rc.online;}
|
|
bool isPCOnline(CMD_t *c){return c->input_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->rcSourcePriorityConfigs[j].priority < param->rcSourcePriorityConfigs[j + 1].priority) {
|
|
CMD_RCSourcePriorityConfig_t temp = param->rcSourcePriorityConfigs[j];
|
|
param->rcSourcePriorityConfigs[j] = param->rcSourcePriorityConfigs[j + 1];
|
|
param->rcSourcePriorityConfigs[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.rcSourcePriorityConfigs[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_PC; /*默认使用键盘鼠标*/
|
|
}
|
|
|
|
int8_t Cmd_Arbiter(CMD_t *c) {
|
|
if (c == NULL) {
|
|
return CMD_ERR_NULL; // 参数错误
|
|
}
|
|
|
|
PriorityConfigsRanking(&c->params);
|
|
|
|
CMD_InputSource_t source = getHighestPrioritySource(c);
|
|
|
|
c->out_chassis.source = source;
|
|
c->out_gimbal.source = source;
|
|
c->out_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(_) _(dr16, DR16)
|
|
#elif RC_SELECT_Index == 1
|
|
#define FOR_EACH_RC(_) _(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->input_rc.inputData->at9s.data.key_E) {
|
|
case AT9S_CMD_SW_DOWN:
|
|
c->out_chassis.cmd.mode = CHASSIS_MODE_RELAX;
|
|
break;
|
|
case AT9S_CMD_SW_MID:
|
|
c->out_chassis.cmd.mode = CHASSIS_MODE_FOLLOW_GIMBAL;
|
|
break;
|
|
case AT9S_CMD_SW_UP:
|
|
c->out_chassis.cmd.mode = CHASSIS_MODE_ROTOR;
|
|
break;
|
|
default:
|
|
c->out_chassis.cmd.mode = CHASSIS_MODE_RELAX;
|
|
break;
|
|
}
|
|
c->out_chassis.cmd.ctrl_vec.vx = c->input_rc.inputData->at9s.data.ch_l_y;
|
|
c->out_chassis.cmd.ctrl_vec.vy = c->input_rc.inputData->at9s.data.ch_l_x;
|
|
c->out_chassis.cmd.ctrl_vec.wz = c->input_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->input_rc.inputData->at9s.data.key_G) {
|
|
case AT9S_CMD_SW_DOWN:
|
|
c->out_gimbal.cmd.mode = GIMBAL_MODE_RELAX;
|
|
c->out_gimbal.cmd.delta_yaw = 0.0f;
|
|
c->out_gimbal.cmd.delta_pit = 0.0f;
|
|
break;
|
|
case AT9S_CMD_SW_MID:
|
|
c->out_gimbal.cmd.mode = GIMBAL_MODE_ABSOLUTE;
|
|
c->out_gimbal.cmd.delta_yaw = -at9s_out.data.ch_l_x * 2.0f;
|
|
c->out_gimbal.cmd.delta_pit = -at9s_out.data.ch_l_y * 1.5f;
|
|
break;
|
|
case AT9S_CMD_SW_UP:
|
|
c->out_gimbal.cmd.mode = GIMBAL_MODE_ABSOLUTE;
|
|
c->out_gimbal.cmd.delta_yaw = -at9s_out.data.ch_l_x * 2.0f;
|
|
c->out_gimbal.cmd.delta_pit = -at9s_out.data.ch_l_y * 1.5f;
|
|
break;
|
|
default:
|
|
c->out_gimbal.cmd.mode = GIMBAL_MODE_RELAX;
|
|
c->out_gimbal.cmd.delta_yaw = 0.0f;
|
|
c->out_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->input_rc.inputData->at9s.data.key_C) {
|
|
case AT9S_CMD_SW_DOWN:
|
|
c->out_shoot.cmd.online = at9s_out.online;
|
|
c->out_shoot.cmd.ready = true;
|
|
c->out_shoot.cmd.firecmd = true;
|
|
break;
|
|
case AT9S_CMD_SW_MID:
|
|
c->out_shoot.cmd.online = at9s_out.online;
|
|
c->out_shoot.cmd.ready = true;
|
|
c->out_shoot.cmd.firecmd = false;
|
|
break;
|
|
case AT9S_CMD_SW_UP:
|
|
c->out_shoot.cmd.online = at9s_out.online;
|
|
c->out_shoot.cmd.ready = false;
|
|
c->out_shoot.cmd.firecmd = false;
|
|
break;
|
|
default:
|
|
c->out_shoot.cmd.online = at9s_out.online;
|
|
c->out_shoot.cmd.ready = false;
|
|
c->out_shoot.cmd.firecmd = false;
|
|
break;
|
|
}
|
|
#endif
|
|
return CMD_OK;
|
|
}
|
|
/* Exported functions ------------------------------------------------------- */
|
|
|
|
|
|
/*************************************************************************************************************************************/
|
|
/*****************************************************************PC******************************************************************/
|
|
/*************************************************************************************************************************************/
|
|
|
|
/* Includes ----------------------------------------------------------------- */
|
|
/* Private typedef ---------------------------------------------------------- */
|
|
/* Private define ----------------------------------------------------------- */
|
|
/* Private macro ------------------------------------------------------------ */
|
|
/* Private variables -------------------------------------------------------- */
|
|
/* Private function -------------------------------------------------------- */
|
|
int8_t Cmd_Get_pc(CMD_InputData_PC_t *pc)
|
|
{
|
|
FOR_EACH_RC(RC_X_COPYREFERENCE_MACRO)(&rc_buffer);
|
|
rc->inputData = &rc_buffer;
|
|
rc->type = RC_SELECT_Index;
|
|
return CMD_OK;
|
|
}
|
|
/* Exported functions ------------------------------------------------------- */
|
|
|
|
|
|
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; // 参数错误
|
|
}
|
|
|
|
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->out_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->out_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->out_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;
|
|
}
|