Compare commits
2 Commits
Author | SHA1 | Date | |
---|---|---|---|
0eba61c81a | |||
8e7ac722e6 |
@ -392,6 +392,21 @@
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>shoot</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>7</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>ai</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>8</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>cmd</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>9</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>rc</ItemText>
|
||||
</Ww>
|
||||
</WatchWindow1>
|
||||
<Tracepoint>
|
||||
<THDelay>0</THDelay>
|
||||
@ -1255,7 +1270,7 @@
|
||||
|
||||
<Group>
|
||||
<GroupName>User/bsp</GroupName>
|
||||
<tvExp>1</tvExp>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<cbSel>0</cbSel>
|
||||
<RteFlg>0</RteFlg>
|
||||
|
Binary file not shown.
18470
MDK-ARM/DevC/DevC.hex
18470
MDK-ARM/DevC/DevC.hex
File diff suppressed because it is too large
Load Diff
2
Middlewares/Third_Party/Protocol/protocol.h
vendored
2
Middlewares/Third_Party/Protocol/protocol.h
vendored
@ -53,12 +53,14 @@ typedef struct __attribute__((packed)) {
|
||||
|
||||
/* 视觉 -> 电控 数据结构体*/
|
||||
typedef struct __attribute__((packed)) {
|
||||
|
||||
struct __attribute__((packed)) {
|
||||
float yaw; /* 偏航角(Yaw angle) */
|
||||
float pit; /* 俯仰角(Pitch angle) */
|
||||
float rol; /* 翻滚角(Roll angle) */
|
||||
} gimbal; /* 欧拉角 */
|
||||
|
||||
float distance; /*目标距离*/
|
||||
uint8_t notice; /* 控制命令 */
|
||||
|
||||
struct __attribute__((packed)) {
|
||||
|
@ -192,7 +192,7 @@ static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
|
||||
break;
|
||||
|
||||
case CMD_SW_MID:
|
||||
cmd->chassis.mode = CHASSIS_MODE_FOLLOW_GIMBAL;
|
||||
cmd->chassis.mode = CHASSIS_MODE_FOLLOW_GIMBAL_35;
|
||||
break;
|
||||
|
||||
case CMD_SW_DOWN:
|
||||
@ -208,20 +208,26 @@ static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
|
||||
/* 右拨杆相应行为选择和解析*/
|
||||
case CMD_SW_UP:
|
||||
cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
|
||||
cmd->shoot.mode = SHOOT_MODE_SAFE;
|
||||
// cmd->shoot.mode = SHOOT_MODE_SAFE;
|
||||
cmd->host_overwrite = false;
|
||||
cmd->ai_status = AI_STATUS_STOP;
|
||||
break;
|
||||
|
||||
case CMD_SW_MID:
|
||||
cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
|
||||
cmd->shoot.fire = false;
|
||||
cmd->shoot.mode = SHOOT_MODE_LOADED;
|
||||
cmd->host_overwrite = true;
|
||||
cmd->ai_status = AI_STATUS_AUTOMATIC;
|
||||
// cmd->shoot.fire = false;
|
||||
// cmd->shoot.mode = SHOOT_MODE_LOADED;
|
||||
break;
|
||||
|
||||
case CMD_SW_DOWN:
|
||||
cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
|
||||
cmd->shoot.mode = SHOOT_MODE_LOADED;
|
||||
cmd->shoot.fire_mode = FIRE_MODE_SINGLE;
|
||||
cmd->shoot.fire = true;
|
||||
cmd->host_overwrite = true;
|
||||
cmd->ai_status = AI_STATUS_AUTOMATIC;
|
||||
// cmd->shoot.mode = SHOOT_MODE_LOADED;
|
||||
// cmd->shoot.fire_mode = FIRE_MODE_SINGLE;
|
||||
// cmd->shoot.fire = true;
|
||||
break;
|
||||
/*
|
||||
case CMD_SW_UP:
|
||||
@ -244,7 +250,9 @@ static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
|
||||
*/
|
||||
case CMD_SW_ERR:
|
||||
cmd->gimbal.mode = GIMBAL_MODE_RELAX;
|
||||
cmd->shoot.mode = SHOOT_MODE_RELAX;
|
||||
cmd->host_overwrite = false;
|
||||
cmd->ai_status = AI_STATUS_STOP;
|
||||
// cmd->shoot.mode = SHOOT_MODE_RELAX;
|
||||
}
|
||||
/* 将操纵杆的对应值转换为底盘的控制向量和云台变化的欧拉角 */
|
||||
cmd->chassis.ctrl_vec.vx = rc->ch_l_x;
|
||||
@ -345,6 +353,11 @@ int8_t CMD_ParseHost(const CMD_Host_t *host, CMD_t *cmd, float dt_sec) {
|
||||
cmd->gimbal.delta_eulr.yaw = host->gimbal_delta.yaw;
|
||||
cmd->gimbal.delta_eulr.pit = host->gimbal_delta.pit;
|
||||
|
||||
if (cmd->ai_status == AI_STATUS_AUTOMATIC){
|
||||
cmd->chassis.ctrl_vec.vx = host->chassis_move_vec.vx;
|
||||
cmd->chassis.ctrl_vec.vy = host->chassis_move_vec.vy;
|
||||
cmd->chassis.ctrl_vec.wz = host->chassis_move_vec.wz;
|
||||
}
|
||||
/* host射击命令,设置不同的射击频率和弹丸初速度 */
|
||||
if (host->fire) {
|
||||
cmd->shoot.mode = SHOOT_MODE_LOADED;
|
||||
|
@ -112,3 +112,17 @@ inline float CalculateRpm(float bullet_speed, float fric_radius, bool is17mm) {
|
||||
/* 不为裁判系统设定值时,计算转速 */
|
||||
return 60.0f * (float)bullet_speed / (M_2PI * fric_radius);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 断言失败处理
|
||||
*
|
||||
* @param file 文件名
|
||||
* @param line 行号
|
||||
*/
|
||||
void VerifyFailed(const char *file, uint32_t line) {
|
||||
UNUSED(file);
|
||||
UNUSED(line);
|
||||
while (1) {
|
||||
__NOP();
|
||||
}
|
||||
}
|
||||
|
@ -104,3 +104,53 @@ float CalculateRpm(float bullet_speed, float fric_radius, bool is17mm);
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef DEBUG
|
||||
|
||||
/**
|
||||
* @brief 如果表达式的值为假则运行处理函数
|
||||
*
|
||||
*/
|
||||
#define ASSERT(expr) \
|
||||
do { \
|
||||
if (!(expr)) { \
|
||||
VerifyFailed(__FILE__, __LINE__); \
|
||||
} \
|
||||
} while (0)
|
||||
#else
|
||||
|
||||
/**
|
||||
* @brief 未定DEBUG,表达式不会运行,断言被忽略
|
||||
*
|
||||
*/
|
||||
#define ASSERT(expr) ((void)(0))
|
||||
#endif
|
||||
|
||||
#ifdef DEBUG
|
||||
|
||||
/**
|
||||
* @brief 如果表达式的值为假则运行处理函数
|
||||
*
|
||||
*/
|
||||
#define VERIFY(expr) \
|
||||
do { \
|
||||
if (!(expr)) { \
|
||||
VerifyFailed(__FILE__, __LINE__); \
|
||||
} \
|
||||
} while (0)
|
||||
#else
|
||||
|
||||
/**
|
||||
* @brief 表达式会运行,忽略表达式结果
|
||||
*
|
||||
*/
|
||||
#define VERIFY(expr) ((void)(expr))
|
||||
#endif
|
||||
|
||||
/**
|
||||
* @brief 断言失败处理
|
||||
*
|
||||
* @param file 文件名
|
||||
* @param line 行号
|
||||
*/
|
||||
void VerifyFailed(const char *file, uint32_t line);
|
||||
|
@ -14,11 +14,17 @@
|
||||
#include "component\user_math.h"
|
||||
|
||||
/* Private define ----------------------------------------------------------- */
|
||||
#define AI_HOST_MAX_CONTROL_VALUE (1.0f)
|
||||
|
||||
#define AI_LEN_RX_BUFF (sizeof(Protocol_DownPackage_t))
|
||||
|
||||
/* Private macro ------------------------------------------------------------ */
|
||||
/* Private typedef ---------------------------------------------------------- */
|
||||
/* Private variables -------------------------------------------------------- */
|
||||
static volatile uint32_t drop_message = 0;
|
||||
|
||||
static uint8_t rxbuf[AI_LEN_RX_BUFF];
|
||||
|
||||
static osThreadId_t thread_alert;
|
||||
|
||||
static bool inited = false;
|
||||
@ -29,14 +35,21 @@ static void Ai_RxCpltCallback(void) {
|
||||
osThreadFlagsSet(thread_alert, SIGNAL_AI_RAW_REDY);
|
||||
}
|
||||
|
||||
static void Ai_IdleLineCallback(void) {
|
||||
osThreadFlagsSet(thread_alert, SIGNAL_AI_RAW_REDY);
|
||||
}
|
||||
|
||||
/* Exported functions ------------------------------------------------------- */
|
||||
int8_t AI_Init(AI_t *ai) {
|
||||
if (ai == NULL) return DEVICE_ERR_NULL;
|
||||
UNUSED(ai);
|
||||
ASSERT(ai);
|
||||
if (inited) return DEVICE_ERR_INITED;
|
||||
if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL;
|
||||
VERIFY((thread_alert = osThreadGetId()) != NULL);
|
||||
|
||||
BSP_UART_RegisterCallback(BSP_UART_AI, BSP_UART_RX_CPLT_CB,
|
||||
Ai_RxCpltCallback);
|
||||
BSP_UART_RegisterCallback(BSP_UART_AI, BSP_UART_IDLE_LINE_CB,
|
||||
Ai_IdleLineCallback);
|
||||
inited = true;
|
||||
return 0;
|
||||
}
|
||||
@ -48,9 +61,9 @@ int8_t AI_Restart(void) {
|
||||
}
|
||||
|
||||
int8_t AI_StartReceiving(AI_t *ai) {
|
||||
if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_AI),
|
||||
(uint8_t *)&(ai->form_host),
|
||||
sizeof(ai->form_host)) == HAL_OK)
|
||||
UNUSED(ai);
|
||||
if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_AI), rxbuf,
|
||||
AI_LEN_RX_BUFF) == HAL_OK)
|
||||
return DEVICE_OK;
|
||||
return DEVICE_ERR;
|
||||
}
|
||||
@ -60,17 +73,12 @@ bool AI_WaitDmaCplt(void) {
|
||||
SIGNAL_AI_RAW_REDY);
|
||||
}
|
||||
|
||||
int8_t AI_ParseHost(AI_t *ai, CMD_Host_t *cmd_host) {
|
||||
(void)cmd_host;
|
||||
if (!CRC16_Verify((const uint8_t *)&(ai->form_host), sizeof(ai->form_host)))
|
||||
int8_t AI_ParseHost(AI_t *ai) {
|
||||
if (!CRC16_Verify((const uint8_t *)&(rxbuf), sizeof(ai->form_host)))
|
||||
goto error;
|
||||
cmd_host->gimbal_delta.pit = ai->form_host.data.gimbal.pit;
|
||||
cmd_host->gimbal_delta.yaw = ai->form_host.data.gimbal.yaw;
|
||||
cmd_host->gimbal_delta.rol = ai->form_host.data.gimbal.rol;
|
||||
cmd_host->fire = (ai->form_host.data.notice & AI_NOTICE_FIRE);
|
||||
cmd_host->chassis_move_vec.vx = ai->form_host.data.chassis_move_vec.vx;
|
||||
cmd_host->chassis_move_vec.vy = ai->form_host.data.chassis_move_vec.vy;
|
||||
cmd_host->chassis_move_vec.wz = ai->form_host.data.chassis_move_vec.wz;
|
||||
ai->ai_online = true;
|
||||
memcpy(&(ai->form_host), rxbuf, sizeof(ai->form_host));
|
||||
memset(rxbuf, 0, AI_LEN_RX_BUFF);
|
||||
return DEVICE_OK;
|
||||
|
||||
error:
|
||||
@ -78,10 +86,25 @@ error:
|
||||
return DEVICE_ERR;
|
||||
}
|
||||
|
||||
void AI_PackCmd(AI_t *ai, CMD_Host_t *cmd_host) {
|
||||
cmd_host->gimbal_delta.yaw = -ai->form_host.data.gimbal.yaw;
|
||||
cmd_host->gimbal_delta.pit = -ai->form_host.data.gimbal.pit;
|
||||
|
||||
Clip(&(cmd_host->gimbal_delta.yaw), -AI_HOST_MAX_CONTROL_VALUE,
|
||||
AI_HOST_MAX_CONTROL_VALUE);
|
||||
Clip(&(cmd_host->gimbal_delta.pit), -AI_HOST_MAX_CONTROL_VALUE,
|
||||
AI_HOST_MAX_CONTROL_VALUE);
|
||||
|
||||
cmd_host->fire = (ai->form_host.data.notice & AI_NOTICE_FIRE);
|
||||
cmd_host->chassis_move_vec.vx = ai->form_host.data.chassis_move_vec.vx;
|
||||
cmd_host->chassis_move_vec.vy = ai->form_host.data.chassis_move_vec.vy;
|
||||
cmd_host->chassis_move_vec.wz = ai->form_host.data.chassis_move_vec.wz;
|
||||
}
|
||||
|
||||
int8_t AI_HandleOffline(AI_t *ai, CMD_Host_t *cmd_host) {
|
||||
if (ai == NULL) return DEVICE_ERR_NULL;
|
||||
if (cmd_host == NULL) return DEVICE_ERR_NULL;
|
||||
|
||||
ai->ai_online = false;
|
||||
memset(&(ai->form_host), 0, sizeof(ai->form_host));
|
||||
memset(cmd_host, 0, sizeof(*cmd_host));
|
||||
return 0;
|
||||
|
@ -45,6 +45,7 @@ typedef struct __packed {
|
||||
} to_host;
|
||||
|
||||
CMD_AI_Status_t status;
|
||||
bool ai_online;
|
||||
} AI_t;
|
||||
|
||||
/* Exported functions prototypes -------------------------------------------- */
|
||||
@ -53,11 +54,12 @@ int8_t AI_Restart(void);
|
||||
|
||||
int8_t AI_StartReceiving(AI_t *ai);
|
||||
bool AI_WaitDmaCplt(void);
|
||||
int8_t AI_ParseHost(AI_t *ai, CMD_Host_t *cmd_host);
|
||||
int8_t AI_ParseHost(AI_t *ai);
|
||||
int8_t AI_HandleOffline(AI_t *ai, CMD_Host_t *cmd_host);
|
||||
int8_t AI_PackMCU(AI_t *ai, const AHRS_Quaternion_t *quat);
|
||||
int8_t AI_PackRef(AI_t *ai, const Referee_ForAI_t *ref);
|
||||
int8_t AI_StartSend(AI_t *ai, bool option);
|
||||
void AI_PackCmd(AI_t *ai, CMD_Host_t *cmd_host);
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
@ -264,7 +264,7 @@ int8_t Chassis_Control(Chassis_t *c, const CMD_ChassisCmd_t *c_cmd,
|
||||
|
||||
case CHASSIS_MODE_INDENPENDENT: /* 独立模式控制向量与运动向量相等 */
|
||||
c->move_vec.vx = c_cmd->ctrl_vec.vx;
|
||||
c->move_vec.vy = c_cmd->ctrl_vec.vx;
|
||||
c->move_vec.vy = c_cmd->ctrl_vec.vy;
|
||||
break;
|
||||
|
||||
case CHASSIS_MODE_OPEN:
|
||||
|
@ -130,12 +130,14 @@ int8_t Gimbal_Control(Gimbal_t *g, CMD_GimbalCmd_t *g_cmd, uint32_t now) {
|
||||
Gimbal_SetMode(g, g_cmd->mode);
|
||||
|
||||
/* yaw坐标正方向与遥控器操作逻辑相反 */
|
||||
// g_cmd->delta_eulr.pit = g_cmd->delta_eulr.pit;
|
||||
// if (g->param->pit_ctrl_reverse){
|
||||
// g_cmd->delta_eulr.yaw = g_cmd->delta_eulr.yaw;
|
||||
// }else{
|
||||
// g_cmd->delta_eulr.yaw = -g_cmd->delta_eulr.yaw;
|
||||
// }
|
||||
g_cmd->delta_eulr.pit = g_cmd->delta_eulr.pit;
|
||||
if (g->param->pit_ctrl_reverse){
|
||||
g_cmd->delta_eulr.yaw = -g_cmd->delta_eulr.yaw;
|
||||
}else{
|
||||
g_cmd->delta_eulr.yaw = g_cmd->delta_eulr.yaw;
|
||||
}
|
||||
g_cmd->delta_eulr.yaw = -g_cmd->delta_eulr.yaw;
|
||||
|
||||
/* 处理yaw控制命令 */
|
||||
CircleAdd(&(g->setpoint.eulr.yaw), g_cmd->delta_eulr.yaw, M_2PI);
|
||||
|
@ -42,6 +42,7 @@ void Task_Ai(void *argument) {
|
||||
AI_Init(&ai);
|
||||
|
||||
uint32_t tick = osKernelGetTickCount();
|
||||
uint32_t last_online_tick = tick;
|
||||
while (1) {
|
||||
#ifdef DEBUG
|
||||
task_runtime.stack_water_mark.ai = osThreadGetStackSpace(osThreadGetId());
|
||||
@ -51,17 +52,23 @@ void Task_Ai(void *argument) {
|
||||
|
||||
AI_StartReceiving(&ai);
|
||||
if (AI_WaitDmaCplt()) {
|
||||
AI_ParseHost(&ai, &cmd_host);
|
||||
AI_ParseHost(&ai);
|
||||
last_online_tick = tick;
|
||||
} else {
|
||||
AI_HandleOffline(&ai, &cmd_host);
|
||||
if (tick - last_online_tick > 300) AI_HandleOffline(&ai,&cmd_host);
|
||||
}
|
||||
|
||||
if (ai.status != AI_STATUS_STOP && ai.ai_online){
|
||||
AI_PackCmd(&ai, &cmd_host);
|
||||
osMessageQueueReset(task_runtime.msgq.cmd.raw.host);
|
||||
osMessageQueuePut(task_runtime.msgq.cmd.raw.host, &(cmd_host), 0, 0);
|
||||
}
|
||||
osMessageQueueReset(task_runtime.msgq.cmd.raw.host);
|
||||
osMessageQueuePut(task_runtime.msgq.cmd.raw.host, &(cmd_host), 0, 0);
|
||||
|
||||
osMessageQueueGet(task_runtime.msgq.ai.quat, &(quat), NULL, 0);
|
||||
osMessageQueueGet(task_runtime.msgq.cmd.ai, &(ai.status), NULL, 0);
|
||||
bool ref_update = (osMessageQueueGet(task_runtime.msgq.referee.ai,
|
||||
&(referee_ai), NULL, 0) == osOK);
|
||||
|
||||
AI_PackMCU(&ai, &quat);
|
||||
if (ref_update) AI_PackRef(&ai, &(referee_ai));
|
||||
|
||||
|
Loading…
Reference in New Issue
Block a user