Compare commits
5 Commits
Author | SHA1 | Date | |
---|---|---|---|
be3d2120bb | |||
4703c39639 | |||
fa5dae4e9f | |||
6c7e02f5c7 | |||
9e763bae27 |
6
.vscode/settings.json
vendored
Normal file
6
.vscode/settings.json
vendored
Normal file
@ -0,0 +1,6 @@
|
||||
{
|
||||
"files.associations": {
|
||||
"user_math.h": "c",
|
||||
"shoot.h": "c"
|
||||
}
|
||||
}
|
@ -74,7 +74,7 @@ void MX_FREERTOS_Init(void);
|
||||
* @retval int
|
||||
*/
|
||||
int main(void)
|
||||
{
|
||||
{
|
||||
/* USER CODE BEGIN 1 */
|
||||
|
||||
/* USER CODE END 1 */
|
||||
|
@ -407,6 +407,36 @@
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>rc</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>10</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>cmd_host</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>11</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>gimbal_ahrs</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>12</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>eulr_to_send</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>13</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>up_data</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>14</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>AHRS_Eulr_t</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>15</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>gimbal_encoder</ItemText>
|
||||
</Ww>
|
||||
</WatchWindow1>
|
||||
<Tracepoint>
|
||||
<THDelay>0</THDelay>
|
||||
@ -658,7 +688,7 @@
|
||||
|
||||
<Group>
|
||||
<GroupName>Application/User/USB_DEVICE/App</GroupName>
|
||||
<tvExp>1</tvExp>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<cbSel>0</cbSel>
|
||||
<RteFlg>0</RteFlg>
|
||||
@ -1214,7 +1244,7 @@
|
||||
|
||||
<Group>
|
||||
<GroupName>Middlewares/USB_Device_Library</GroupName>
|
||||
<tvExp>1</tvExp>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<cbSel>0</cbSel>
|
||||
<RteFlg>0</RteFlg>
|
||||
@ -1494,7 +1524,7 @@
|
||||
|
||||
<Group>
|
||||
<GroupName>User/component</GroupName>
|
||||
<tvExp>1</tvExp>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<cbSel>0</cbSel>
|
||||
<RteFlg>0</RteFlg>
|
||||
|
Binary file not shown.
18600
MDK-ARM/DevC/DevC.hex
18600
MDK-ARM/DevC/DevC.hex
File diff suppressed because it is too large
Load Diff
30
Middlewares/Third_Party/Protocol/protocol.h
vendored
30
Middlewares/Third_Party/Protocol/protocol.h
vendored
@ -10,12 +10,15 @@
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#define AI_NOTICE_AUTOAIM (1 << 0)
|
||||
#define AI_NOTICE_HITBUFF (1 << 1)
|
||||
#define AI_NOTICE_AUTOMATIC (1 << 2)
|
||||
#define AI_NOTICE_FIRE (1 << 3)
|
||||
#define AI_NOTICE_FIRE (1 << 0)
|
||||
#define AI_NOTICE_TRACKING (1 << 1)
|
||||
|
||||
#define AI_NOTICE_AUTOAIM (1 << 2)
|
||||
#define AI_NOTICE_HITBUFF (1 << 3)
|
||||
#define AI_NOTICE_AUTOMATIC (1 << 4)
|
||||
|
||||
#define AI_ID_MCU (0xC4)
|
||||
#define AI_ID_MCU2 (0xD4)
|
||||
#define AI_ID_REF (0xA8)
|
||||
|
||||
#define AI_TEAM_RED (0x01)
|
||||
@ -33,16 +36,15 @@ typedef struct __attribute__((packed)) {
|
||||
float q3;
|
||||
} quat; /* 四元数 */
|
||||
|
||||
// float chassis_speed; /* 底盘速度(哨兵) */
|
||||
struct __attribute__((packed)) {
|
||||
float yaw; /* 偏航角(Yaw angle) */
|
||||
float pit; /* 俯仰角(Pitch angle) */
|
||||
float rol; /* 翻滚角(Roll angle) */
|
||||
} gimbal; /* 欧拉角 */
|
||||
|
||||
uint8_t notice; /* 控制命令 */
|
||||
|
||||
float ball_speed; /* 子弹初速度 */
|
||||
|
||||
struct __attribute__((packed)) {
|
||||
float left;
|
||||
float right;
|
||||
} distance; /* 左右距离(哨兵) */
|
||||
|
||||
float chassis_speed; /* 底盘速度(哨兵) */
|
||||
} Protocol_UpDataMCU_t;
|
||||
|
||||
/* 电控 -> 视觉 裁判系统数据结构体*/
|
||||
@ -53,19 +55,21 @@ typedef struct __attribute__((packed)) {
|
||||
|
||||
/* 视觉 -> 电控 数据结构体*/
|
||||
typedef struct __attribute__((packed)) {
|
||||
|
||||
struct __attribute__((packed)) {
|
||||
float yaw; /* 偏航角(Yaw angle) */
|
||||
float pit; /* 俯仰角(Pitch angle) */
|
||||
float rol; /* 翻滚角(Roll angle) */
|
||||
} gimbal; /* 欧拉角 */
|
||||
|
||||
uint8_t notice; /* 控制命令 */
|
||||
|
||||
struct __attribute__((packed)) {
|
||||
float vx; /* x轴移动速度 */
|
||||
float vy; /* y轴移动速度 */
|
||||
float wz; /* z轴转动速度 */
|
||||
} chassis_move_vec; /* 底盘移动向量 */
|
||||
uint8_t notice; /* 控制命令 */
|
||||
|
||||
} Protocol_DownData_t;
|
||||
|
||||
typedef struct __attribute__((packed)) {
|
||||
|
@ -13,6 +13,10 @@ extern "C" {
|
||||
/* Exported types ----------------------------------------------------------- */
|
||||
/* Exported functions prototypes -------------------------------------------- */
|
||||
void *BSP_Malloc(size_t size);
|
||||
/**
|
||||
* @brief
|
||||
* @param pv
|
||||
*/
|
||||
void BSP_Free(void *pv);
|
||||
|
||||
#ifdef __cplusplus
|
||||
|
@ -4,9 +4,40 @@
|
||||
|
||||
#include "ballistics.h"
|
||||
|
||||
/**
|
||||
* @brief
|
||||
* @param b
|
||||
*/
|
||||
void Ballistics_Init(Ballistics_t *b) { (void)b; }
|
||||
void Ballistics_Apply(Ballistics_t *b, float bullet_speed) {
|
||||
|
||||
|
||||
/**
|
||||
* @brief
|
||||
* @param b
|
||||
* @param bullet_speed
|
||||
*/
|
||||
void Ballistics_Apply(Ballistics_t *b, float bullet_speed)
|
||||
{
|
||||
(void)b;
|
||||
(void)bullet_speed;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @brief
|
||||
* @param bullet_speed 弹丸速度
|
||||
* @param distance 距离
|
||||
* @return 高度补偿值
|
||||
*/
|
||||
float Simpal_Ballistics_Apply(float bullet_speed, float distance)
|
||||
{
|
||||
return distance /1000 / bullet_speed;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief
|
||||
* @param b
|
||||
*/
|
||||
void Ballistics_Reset(Ballistics_t *b) { (void)b; }
|
||||
|
@ -16,6 +16,14 @@ typedef struct {
|
||||
|
||||
void Ballistics_Init(Ballistics_t *b);
|
||||
void Ballistics_Apply(Ballistics_t *b, float bullet_speed);
|
||||
|
||||
/**
|
||||
* @brief
|
||||
* @param bullet_speed 弹丸速度
|
||||
* @param distance 距离
|
||||
* @return 高度补偿值
|
||||
*/
|
||||
float Simpal_Ballistics_Apply(float bullet_speed, float distance);
|
||||
void Ballistics_Reset(Ballistics_t *b);
|
||||
|
||||
#ifdef __cplusplus
|
||||
|
@ -158,8 +158,8 @@ static void CMD_PcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
|
||||
CMD_RefereeAdd(&(cmd->referee), CMD_UI_AUTO_AIM_STOP);
|
||||
} else {
|
||||
/* ai切换至自瞄模式,启用host控制 */
|
||||
cmd->ai_status = AI_STATUS_AUTOAIM;
|
||||
cmd->host_overwrite = true;
|
||||
cmd->ai_status = AI_STATUS_AUTOAIM;
|
||||
CMD_RefereeAdd(&(cmd->referee), CMD_UI_AUTO_AIM_START);
|
||||
}
|
||||
} else {
|
||||
@ -190,27 +190,19 @@ static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
|
||||
/* 左拨杆相应行为选择和解析 */
|
||||
case CMD_SW_UP:
|
||||
cmd->chassis.mode = CHASSIS_MODE_BREAK;
|
||||
cmd->host_overwrite = false;
|
||||
cmd->shoot.ai_fire = false;
|
||||
break;
|
||||
|
||||
case CMD_SW_MID:
|
||||
cmd->chassis.mode = CHASSIS_MODE_FOLLOW_GIMBAL;
|
||||
cmd->host_overwrite = true;
|
||||
cmd->shoot.ai_fire = false;
|
||||
break;
|
||||
|
||||
case CMD_SW_DOWN:
|
||||
cmd->chassis.mode = CHASSIS_MODE_ROTOR;
|
||||
cmd->chassis.mode_rotor = ROTOR_MODE_CW;
|
||||
cmd->host_overwrite = true;
|
||||
cmd->shoot.ai_fire = true;
|
||||
break;
|
||||
|
||||
case CMD_SW_ERR:
|
||||
cmd->chassis.mode = CHASSIS_MODE_RELAX;
|
||||
cmd->host_overwrite = false;
|
||||
cmd->shoot.ai_fire = false;
|
||||
break;
|
||||
}
|
||||
switch (rc->sw_r) {
|
||||
@ -231,36 +223,19 @@ static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
|
||||
cmd->shoot.mode = SHOOT_MODE_LOADED;
|
||||
cmd->shoot.fire_mode = FIRE_MODE_SINGLE;
|
||||
cmd->shoot.fire = true;
|
||||
break;
|
||||
/*
|
||||
case CMD_SW_UP:
|
||||
cmd->gimbal.mode = GIMBAL_MODE_RELAX;
|
||||
cmd->shoot.mode = SHOOT_MODE_SAFE;
|
||||
break;
|
||||
|
||||
case CMD_SW_MID:
|
||||
cmd->gimbal.mode = GIMBAL_MODE_RELAX;
|
||||
cmd->shoot.fire = false;
|
||||
cmd->shoot.mode = SHOOT_MODE_LOADED;
|
||||
break;
|
||||
|
||||
case CMD_SW_DOWN:
|
||||
cmd->gimbal.mode = GIMBAL_MODE_RELAX;
|
||||
cmd->shoot.mode = SHOOT_MODE_LOADED;
|
||||
cmd->shoot.fire_mode = FIRE_MODE_SINGLE;
|
||||
cmd->shoot.fire = true;
|
||||
break;
|
||||
*/
|
||||
case CMD_SW_ERR:
|
||||
cmd->gimbal.mode = GIMBAL_MODE_RELAX;
|
||||
cmd->shoot.mode = SHOOT_MODE_RELAX;
|
||||
cmd->host_overwrite = false;
|
||||
}
|
||||
/* 将操纵杆的对应值转换为底盘的控制向量和云台变化的欧拉角 */
|
||||
cmd->chassis.ctrl_vec.vx = rc->ch_l_x;
|
||||
cmd->chassis.ctrl_vec.vy = rc->ch_l_y;
|
||||
cmd->gimbal.delta_eulr.yaw = rc->ch_r_x * dt_sec * cmd->param->sens_rc;
|
||||
cmd->gimbal.delta_eulr.pit = rc->ch_r_y * dt_sec * cmd->param->sens_rc;
|
||||
if (cmd->ai_status == AI_STATUS_OFFLINE || cmd->host_overwrite == false) {
|
||||
cmd->chassis.ctrl_vec.vx = rc->ch_l_x;
|
||||
cmd->chassis.ctrl_vec.vy = rc->ch_l_y;
|
||||
cmd->gimbal.delta_eulr.yaw = rc->ch_r_x * dt_sec * cmd->param->sens_rc;
|
||||
cmd->gimbal.delta_eulr.pit = rc->ch_r_y * dt_sec * cmd->param->sens_rc;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
@ -356,11 +331,16 @@ int8_t CMD_ParseHost(const CMD_Host_t *host, CMD_t *cmd, float dt_sec) {
|
||||
cmd->gimbal.delta_eulr.pit = host->gimbal_delta.pit;
|
||||
|
||||
cmd->shoot.mode = SHOOT_MODE_LOADED;
|
||||
if (host->tracking){
|
||||
cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
|
||||
}else{
|
||||
cmd->gimbal.mode = GIMBAL_MODE_SEARCH;
|
||||
}
|
||||
/* host射击命令,设置不同的射击频率和弹丸初速度 */
|
||||
if (host->fire && cmd->shoot.ai_fire) {
|
||||
cmd->shoot.fire = true;
|
||||
} else {
|
||||
cmd->shoot.fire = false;
|
||||
cmd->shoot.fire = true;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
@ -42,6 +42,7 @@ typedef enum {
|
||||
typedef enum {
|
||||
GIMBAL_MODE_RELAX, /* 放松模式,电机不输出。一般情况云台初始化之后的模式 */
|
||||
GIMBAL_MODE_ABSOLUTE, /* 绝对坐标系控制,控制在空间内的绝对姿态 */
|
||||
GIMBAL_MODE_SEARCH, /* 搜索模式,通过云台旋转搜索目标 */
|
||||
GIMBAL_MODE_RELATIVE, /* 相对坐标系控制,控制相对于底盘的姿态 */
|
||||
} CMD_GimbalMode_t;
|
||||
|
||||
@ -178,6 +179,7 @@ typedef struct {
|
||||
|
||||
/* AI行为状态 */
|
||||
typedef enum {
|
||||
AI_STATUS_OFFLINE, /* 离线状态 */
|
||||
AI_STATUS_STOP, /* 停止状态 */
|
||||
AI_STATUS_AUTOAIM, /* 自瞄状态 */
|
||||
AI_STATUS_HITSWITCH, /* 打符状态 */
|
||||
@ -254,6 +256,7 @@ typedef struct {
|
||||
float wz; /* z轴转动速度 */
|
||||
} chassis_move_vec; /* 底盘移动向量 */
|
||||
|
||||
bool tracking; /* 是否追踪目标 */
|
||||
bool fire; /* 开火状态 */
|
||||
} CMD_Host_t;
|
||||
|
||||
|
@ -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);
|
||||
|
@ -12,34 +12,51 @@
|
||||
#include "component\crc16.h"
|
||||
#include "component\crc8.h"
|
||||
#include "component\user_math.h"
|
||||
#include "component\filter.h"
|
||||
|
||||
|
||||
/* Private define ----------------------------------------------------------- */
|
||||
#define AI_HOST_MAX_CONTROL_VALUE (0.001f)
|
||||
|
||||
#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;
|
||||
|
||||
static LowPassFilter2p_t low_pass_filter;
|
||||
|
||||
/* Private function -------------------------------------------------------- */
|
||||
|
||||
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);
|
||||
inited = true;
|
||||
BSP_UART_RegisterCallback(BSP_UART_AI, BSP_UART_IDLE_LINE_CB,
|
||||
Ai_IdleLineCallback);
|
||||
LowPassFilter2p_Init(&low_pass_filter, 1000.0f, 5.0f); // 示例参数
|
||||
|
||||
ai->ai_online = false;
|
||||
inited = true;
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -50,9 +67,9 @@ int8_t AI_Restart(void) {
|
||||
}
|
||||
|
||||
int8_t AI_StartReceiving(AI_t *ai) {
|
||||
if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_AI),
|
||||
ai->form_host.rx_buffer,
|
||||
sizeof(ai->form_host.rx_buffer)) == 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;
|
||||
}
|
||||
@ -62,27 +79,19 @@ bool AI_WaitDmaCplt(void) {
|
||||
SIGNAL_AI_RAW_REDY);
|
||||
}
|
||||
|
||||
int8_t AI_ParseHost(AI_t *ai, CMD_Host_t *cmd_host) {
|
||||
(void)cmd_host;
|
||||
ai->ai_online = true;
|
||||
if (!CRC16_Verify(ai->form_host.rx_buffer, sizeof(ai->form_host.rx_buffer)))
|
||||
int8_t AI_ParseHost(AI_t *ai) {
|
||||
if (!CRC16_Verify((const uint8_t *)&(rxbuf), sizeof(ai->form_host)))
|
||||
goto error;
|
||||
memcpy(&(ai->form_host.data), ai->form_host.rx_buffer, sizeof(ai->form_host.data));
|
||||
// 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;
|
||||
cmd_host->gimbal_delta.pit = ai->form_host.data.data.gimbal.pit;
|
||||
cmd_host->gimbal_delta.yaw = ai->form_host.data.data.gimbal.yaw;
|
||||
cmd_host->gimbal_delta.rol = ai->form_host.data.data.gimbal.rol;
|
||||
cmd_host->fire = (ai->form_host.data.data.notice & AI_NOTICE_FIRE);
|
||||
cmd_host->chassis_move_vec.vx = ai->form_host.data.data.chassis_move_vec.vx;
|
||||
cmd_host->chassis_move_vec.vy = ai->form_host.data.data.chassis_move_vec.vy;
|
||||
cmd_host->chassis_move_vec.wz = ai->form_host.data.data.chassis_move_vec.wz;
|
||||
ai->ai_online = true;
|
||||
|
||||
// 对接收到的数据进行滤波处理
|
||||
for (size_t i = 0; i < sizeof(ai->form_host.data.gimbal); ++i) {
|
||||
((float*)&ai->form_host.data.gimbal)[i] =
|
||||
LowPassFilter2p_Apply(&low_pass_filter, ((float*)&ai->form_host.data.gimbal)[i]);
|
||||
}
|
||||
|
||||
memcpy(&(ai->form_host), rxbuf, sizeof(ai->form_host));
|
||||
memset(rxbuf, 0, AI_LEN_RX_BUFF);
|
||||
return DEVICE_OK;
|
||||
|
||||
error:
|
||||
@ -90,20 +99,37 @@ 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.yaw = cmd_host->gimbal_delta.yaw - 0.00025;
|
||||
cmd_host->gimbal_delta.pit = -ai->form_host.data.gimbal.pit;
|
||||
cmd_host->gimbal_delta.pit = cmd_host->gimbal_delta.pit + 0.0005;
|
||||
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.rx_buffer, 0, sizeof(ai->form_host.rx_buffer));
|
||||
memset(&(ai->form_host.data), 0, sizeof(ai->form_host.data));
|
||||
memset(&(ai->form_host), 0, sizeof(ai->form_host));
|
||||
memset(cmd_host, 0, sizeof(*cmd_host));
|
||||
return 0;
|
||||
}
|
||||
|
||||
int8_t AI_PackMCU(AI_t *ai, const AHRS_Quaternion_t *quat) {
|
||||
int8_t AI_PackMCU(AI_t *ai, const AHRS_Quaternion_t *quat, const AHRS_Eulr_t *gimbal_ai) {
|
||||
ai->to_host.mcu.id = AI_ID_MCU;
|
||||
memcpy((void *)&(ai->to_host.mcu.package.data.quat), (const void *)quat,
|
||||
sizeof(*quat));
|
||||
memcpy((void *)&(ai->to_host.mcu.package.data.gimbal), (const void *)gimbal_ai,
|
||||
sizeof(*gimbal_ai));
|
||||
ai->to_host.mcu.package.data.notice = 0;
|
||||
if (ai->status == AI_STATUS_AUTOAIM)
|
||||
ai->to_host.mcu.package.data.notice |= AI_NOTICE_AUTOAIM;
|
||||
|
@ -16,6 +16,7 @@ extern "C" {
|
||||
#include "component\ahrs.h"
|
||||
#include "component\cmd.h"
|
||||
#include "component\user_math.h"
|
||||
#include "component\filter.h"
|
||||
#include "device\device.h"
|
||||
#include "device\referee.h"
|
||||
#include "protocol.h"
|
||||
@ -36,10 +37,8 @@ typedef struct __packed {
|
||||
|
||||
typedef struct __packed {
|
||||
osThreadId_t thread_alert;
|
||||
struct {
|
||||
uint8_t rx_buffer[sizeof(Protocol_DownPackage_t)];
|
||||
Protocol_DownPackage_t data;
|
||||
}form_host;
|
||||
|
||||
Protocol_DownPackage_t form_host;
|
||||
|
||||
struct {
|
||||
AI_UpPackageReferee_t ref;
|
||||
@ -47,7 +46,6 @@ typedef struct __packed {
|
||||
} to_host;
|
||||
|
||||
CMD_AI_Status_t status;
|
||||
|
||||
bool ai_online;
|
||||
} AI_t;
|
||||
|
||||
@ -57,11 +55,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_PackMCU(AI_t *ai, const AHRS_Quaternion_t *quat, const AHRS_Eulr_t *gimbal_ai);
|
||||
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
|
||||
|
@ -200,7 +200,7 @@ typedef union {
|
||||
} CAN_GimbalMotor_t;
|
||||
|
||||
typedef union {
|
||||
CAN_MotorFeedback_t as_array[3];
|
||||
CAN_MotorFeedback_t as_array[4];
|
||||
struct {
|
||||
CAN_MotorFeedback_t fric1;
|
||||
CAN_MotorFeedback_t fric2;
|
||||
|
@ -20,12 +20,12 @@ extern "C" {
|
||||
|
||||
/* Exported constants ------------------------------------------------------- */
|
||||
/* Exported macro ----------------------------------------------------------- */
|
||||
#define REF_SWITCH_STATUS(ref, stat) ((ref).ref_status = (stat))
|
||||
#define REF_SWITCH_STATUS(ref, stat) ((ref).ref_status = (stat))
|
||||
#define CHASSIS_POWER_MAX_WITHOUT_REF 2000.0f /* 裁判系统离线底盘最大功率 */
|
||||
|
||||
#define REF_UI_MAX_GRAPIC_NUM (7)
|
||||
#define REF_UI_MAX_STRING_NUM (7)
|
||||
#define REF_UI_MAX_DEL_NUM (3)
|
||||
#define REF_UI_MAX_GRAPIC_NUM (7) /* 最大图形数量 */
|
||||
#define REF_UI_MAX_STRING_NUM (7) /* 最大字符串数量 */
|
||||
#define REF_UI_MAX_DEL_NUM (3) /* 最大删除数量 */
|
||||
|
||||
/* Exported types ----------------------------------------------------------- */
|
||||
typedef struct __packed {
|
||||
@ -38,7 +38,7 @@ typedef struct __packed {
|
||||
typedef enum { REF_STATUS_OFFLINE = 0, REF_STATUS_RUNNING } Referee_Status_t;
|
||||
|
||||
typedef enum {
|
||||
REF_CMD_ID_GAME_STATUS = 0x0001,
|
||||
REF_CMD_ID_GAME_STATUS = 0x0001,
|
||||
REF_CMD_ID_GAME_RESULT = 0x0002,
|
||||
REF_CMD_ID_GAME_ROBOT_HP = 0x0003,
|
||||
REF_CMD_ID_DART_STATUS = 0x0004,
|
||||
|
@ -138,7 +138,12 @@ static const Config_RobotParam_t param_default = {
|
||||
},
|
||||
}, /* pid */
|
||||
|
||||
.pitch_travel_rad = 1.05f,
|
||||
.limit_and_imu = {
|
||||
.yaw_travel_rad = -1.0f,
|
||||
.pitch_travel_rad = 2.0f,
|
||||
.yaw_imu = false,
|
||||
.pit_imu = false,
|
||||
},
|
||||
|
||||
.low_pass_cutoff_freq = {
|
||||
.out = -1.0f,
|
||||
@ -150,7 +155,6 @@ static const Config_RobotParam_t param_default = {
|
||||
.pit = true,
|
||||
},
|
||||
|
||||
.pit_ctrl_reverse = false,
|
||||
}, /* gimbal */
|
||||
|
||||
.shoot = { /* 射击模块参数 */
|
||||
@ -191,7 +195,7 @@ static const Config_RobotParam_t param_default = {
|
||||
.cover_close_duty = 0.050f,
|
||||
.model = SHOOT_MODEL_17MM,
|
||||
.bullet_speed = 30.f,
|
||||
.min_shoot_delay = (uint32_t)(1000.0f / 10.0f),
|
||||
.min_shoot_delay = (uint32_t)(1000.0f / 5.0f),
|
||||
}, /* shoot */
|
||||
|
||||
.can = {
|
||||
@ -285,7 +289,12 @@ static const Config_RobotParam_t param_hero = {
|
||||
},
|
||||
}, /* pid */
|
||||
|
||||
.pitch_travel_rad = 1.07685447f,
|
||||
.limit_and_imu = {
|
||||
.yaw_travel_rad = -1.0f,
|
||||
.pitch_travel_rad = 1.0f,
|
||||
.yaw_imu = true,
|
||||
.pit_imu = true,
|
||||
},
|
||||
|
||||
.low_pass_cutoff_freq = {
|
||||
.out = -1.0f,
|
||||
@ -297,7 +306,6 @@ static const Config_RobotParam_t param_hero = {
|
||||
.pit = true,
|
||||
},
|
||||
|
||||
.pit_ctrl_reverse = false,
|
||||
}, /* gimbal */
|
||||
|
||||
.shoot = { /* 射击模块参数 */
|
||||
@ -395,26 +403,6 @@ static const Config_RobotParam_t param_sentry_chassis = {
|
||||
.gimbal = { /* 云台模块参数 */
|
||||
.pid = {
|
||||
{
|
||||
// /* GIMBAL_PID_YAW_OMEGA_IDX */
|
||||
// .k = 0.25f,
|
||||
// .p = 1.0f,
|
||||
// .i = 1.0f,
|
||||
// .d = 0.0f,
|
||||
// .i_limit = 1.0f,
|
||||
// .out_limit = 1.0f,
|
||||
// .d_cutoff_freq = -1.0f,
|
||||
// .range = -1.0f,
|
||||
// }, {
|
||||
// /* GIMBAL_PID_YAW_ANGLE_IDX */
|
||||
// .k = 12.0f,
|
||||
// .p = 1.0f,
|
||||
// .i = 0.0f,
|
||||
// .d = 0.05f,
|
||||
// .i_limit = 0.0f,
|
||||
// .out_limit = 10.0f,
|
||||
// .d_cutoff_freq = -1.0f,
|
||||
// .range = M_2PI,
|
||||
/* GIMBAL_PID_YAW_OMEGA_IDX */
|
||||
.k = 0.24f,
|
||||
.p = 1.0f,
|
||||
.i = 0.5f,
|
||||
@ -434,25 +422,6 @@ static const Config_RobotParam_t param_sentry_chassis = {
|
||||
.d_cutoff_freq = -1.0f,
|
||||
.range = M_2PI,
|
||||
}, {
|
||||
// /* GIMBAL_PID_PIT_OMEGA_IDX */
|
||||
// .k = 0.35f,
|
||||
// .p = 1.0f,
|
||||
// .i = 0.f,
|
||||
// .d = 0.003f,
|
||||
// .i_limit = 1.0f,
|
||||
// .out_limit = 1.0f,
|
||||
// .d_cutoff_freq = -1.0f,
|
||||
// .range = -1.0f,
|
||||
// }, {
|
||||
// /* GIMBAL_PID_PIT_ANGLE_IDX */
|
||||
// .k = 15.0f,
|
||||
// .p = 1.0f,
|
||||
// .i = 0.0f,
|
||||
// .d = 0.0f,
|
||||
// .i_limit = 0.0f,
|
||||
// .out_limit = 10.0f,
|
||||
// .d_cutoff_freq = -1.0f,
|
||||
// .range = M_2PI,
|
||||
/* GIMBAL_PID_PIT_OMEGA_IDX */
|
||||
.k = 0.25f,
|
||||
.p = 1.0f,
|
||||
@ -475,7 +444,12 @@ static const Config_RobotParam_t param_sentry_chassis = {
|
||||
},
|
||||
}, /* pid */
|
||||
|
||||
.pitch_travel_rad = 0.85f,
|
||||
.limit_and_imu = {
|
||||
.yaw_travel_rad = -1.0f,
|
||||
.pitch_travel_rad = 1.0f,
|
||||
.yaw_imu = true,
|
||||
.pit_imu = true,
|
||||
},
|
||||
|
||||
.low_pass_cutoff_freq = {
|
||||
.out = -1.0f,
|
||||
@ -487,7 +461,6 @@ static const Config_RobotParam_t param_sentry_chassis = {
|
||||
.pit = true,
|
||||
},
|
||||
|
||||
.pit_ctrl_reverse = true,
|
||||
}, /* gimbal */
|
||||
|
||||
.shoot = { /* 射击模块参数 */
|
||||
@ -585,25 +558,6 @@ static const Config_RobotParam_t param_sentry_gimbal = {
|
||||
.gimbal = { /* 云台模块参数 */
|
||||
.pid = {
|
||||
{
|
||||
// /* GIMBAL_PID_YAW_OMEGA_IDX */
|
||||
// .k = 0.25f,
|
||||
// .p = 1.0f,
|
||||
// .i = 1.0f,
|
||||
// .d = 0.0f,
|
||||
// .i_limit = 1.0f,
|
||||
// .out_limit = 1.0f,
|
||||
// .d_cutoff_freq = -1.0f,
|
||||
// .range = -1.0f,
|
||||
// }, {
|
||||
// /* GIMBAL_PID_YAW_ANGLE_IDX */
|
||||
// .k = 12.0f,
|
||||
// .p = 1.0f,
|
||||
// .i = 0.0f,
|
||||
// .d = 0.05f,
|
||||
// .i_limit = 0.0f,
|
||||
// .out_limit = 10.0f,
|
||||
// .d_cutoff_freq = -1.0f,
|
||||
// .range = M_2PI,
|
||||
/* GIMBAL_PID_YAW_OMEGA_IDX */
|
||||
.k = 0.24f,
|
||||
.p = 1.0f,
|
||||
@ -624,25 +578,6 @@ static const Config_RobotParam_t param_sentry_gimbal = {
|
||||
.d_cutoff_freq = -1.0f,
|
||||
.range = M_2PI,
|
||||
}, {
|
||||
// /* GIMBAL_PID_PIT_OMEGA_IDX */
|
||||
// .k = 0.35f,
|
||||
// .p = 1.0f,
|
||||
// .i = 0.f,
|
||||
// .d = 0.003f,
|
||||
// .i_limit = 1.0f,
|
||||
// .out_limit = 1.0f,
|
||||
// .d_cutoff_freq = -1.0f,
|
||||
// .range = -1.0f,
|
||||
// }, {
|
||||
// /* GIMBAL_PID_PIT_ANGLE_IDX */
|
||||
// .k = 15.0f,
|
||||
// .p = 1.0f,
|
||||
// .i = 0.0f,
|
||||
// .d = 0.0f,
|
||||
// .i_limit = 0.0f,
|
||||
// .out_limit = 10.0f,
|
||||
// .d_cutoff_freq = -1.0f,
|
||||
// .range = M_2PI,
|
||||
/* GIMBAL_PID_PIT_OMEGA_IDX */
|
||||
.k = 0.25f,
|
||||
.p = 1.0f,
|
||||
@ -665,8 +600,13 @@ static const Config_RobotParam_t param_sentry_gimbal = {
|
||||
},
|
||||
}, /* pid */
|
||||
|
||||
.pitch_travel_rad = 0.85f,
|
||||
|
||||
.limit_and_imu = {
|
||||
.yaw_travel_rad = -1.0f,
|
||||
.pitch_travel_rad = 1.0f,
|
||||
.yaw_imu = true,
|
||||
.pit_imu = false,
|
||||
},
|
||||
|
||||
.low_pass_cutoff_freq = {
|
||||
.out = -1.0f,
|
||||
.gyro = 1000.0f,
|
||||
@ -677,7 +617,6 @@ static const Config_RobotParam_t param_sentry_gimbal = {
|
||||
.pit = true,
|
||||
},
|
||||
|
||||
.pit_ctrl_reverse = true,
|
||||
}, /* gimbal */
|
||||
|
||||
.shoot = { /* 射击模块参数 */
|
||||
@ -718,7 +657,7 @@ static const Config_RobotParam_t param_sentry_gimbal = {
|
||||
.cover_close_duty = 0.050f,
|
||||
.model = SHOOT_MODEL_17MM,
|
||||
.bullet_speed = 25.f,
|
||||
.min_shoot_delay = (uint32_t)(1000.0f / 20.0f),
|
||||
.min_shoot_delay = (uint32_t)(1000.0f / 1.0f),
|
||||
}, /* shoot */
|
||||
|
||||
.can = {
|
||||
|
@ -47,8 +47,8 @@ typedef struct {
|
||||
} cali; /* 校准 */
|
||||
|
||||
AHRS_Eulr_t mech_zero; /* 机械零点 */
|
||||
float gimbal_limit; /* 云台pitch轴软件限位最高点 */
|
||||
|
||||
float gimbal_pitch_limit; /* 云台pitch轴软件限位最高点 */
|
||||
float gimbal_yaw_limit; /* 云台yaw轴软件限位最高点 */
|
||||
} Config_t;
|
||||
|
||||
/* 机器人参数和对应字符串的映射 */
|
||||
|
@ -25,6 +25,10 @@ static int8_t Gimbal_SetMode(Gimbal_t *g, CMD_GimbalMode_t mode) {
|
||||
if (g == NULL) return -1;
|
||||
if (mode == g->mode) return GIMBAL_OK;
|
||||
|
||||
/* 获取当前反馈值,防止位置突变 */
|
||||
g->setpoint.eulr.yaw = g->feedback.eulr.encoder.yaw;
|
||||
g->setpoint.eulr.pit = g->feedback.eulr.encoder.pit;
|
||||
|
||||
/* 切换模式后重置PID和滤波器 */
|
||||
for (uint8_t i = 0; i < GIMBAL_PID_NUM; i++) {
|
||||
PID_Reset(g->pid + i);
|
||||
@ -36,9 +40,15 @@ static int8_t Gimbal_SetMode(Gimbal_t *g, CMD_GimbalMode_t mode) {
|
||||
AHRS_ResetEulr(&(g->setpoint.eulr)); /* 切换模式后重置设定值 */
|
||||
if (g->mode == GIMBAL_MODE_RELAX) {
|
||||
if (mode == GIMBAL_MODE_ABSOLUTE) {
|
||||
g->setpoint.eulr.yaw = g->feedback.eulr.imu.yaw;
|
||||
if (g->param->limit_and_imu.yaw_imu) {
|
||||
g->setpoint.eulr.yaw = g->feedback.eulr.imu.yaw;
|
||||
} else {
|
||||
g->setpoint.eulr.yaw = g->feedback.eulr.encoder.yaw;
|
||||
}
|
||||
} else if (mode == GIMBAL_MODE_RELATIVE) {
|
||||
g->setpoint.eulr.yaw = g->feedback.eulr.encoder.yaw;
|
||||
} else if (mode == GIMBAL_MODE_SEARCH) {
|
||||
g->setpoint.eulr.yaw = g->feedback.eulr.encoder.yaw;
|
||||
}
|
||||
}
|
||||
|
||||
@ -66,9 +76,12 @@ int8_t Gimbal_Init(Gimbal_t *g, const Gimbal_Params_t *param, float limit_max,
|
||||
|
||||
/* 设置软件限位 */
|
||||
if (g->param->reverse.pit) CircleReverse(&limit_max);
|
||||
g->limit.min = g->limit.max = limit_max;
|
||||
CircleAdd(&(g->limit.min), -g->param->pitch_travel_rad, M_2PI);
|
||||
g->limit.pit_min = g->limit.pit_max = limit_max;
|
||||
CircleAdd(&(g->limit.pit_min), -g->param->limit_and_imu.pitch_travel_rad, M_2PI);
|
||||
|
||||
if (g->param->reverse.yaw) CircleReverse(&limit_max);
|
||||
g->limit.yaw_min = g->limit.yaw_max = limit_max;
|
||||
CircleAdd(&(g->limit.yaw_min), -g->param->limit_and_imu.yaw_travel_rad, M_2PI);
|
||||
/* 初始化云台电机控制PID和LPF */
|
||||
PID_Init(&(g->pid[GIMBAL_PID_YAW_ANGLE_IDX]), KPID_MODE_NO_D, target_freq,
|
||||
&(g->param->pid[GIMBAL_PID_YAW_ANGLE_IDX]));
|
||||
@ -130,38 +143,80 @@ 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;
|
||||
g_cmd->delta_eulr.yaw = -g_cmd->delta_eulr.yaw;
|
||||
|
||||
/* 处理yaw控制命令 */
|
||||
CircleAdd(&(g->setpoint.eulr.yaw), g_cmd->delta_eulr.yaw, M_2PI);
|
||||
// /* 处理yaw控制命令 */
|
||||
// CircleAdd(&(g->setpoint.eulr.yaw), g_cmd->delta_eulr.yaw, M_2PI);
|
||||
|
||||
if (g->param->limit_and_imu.yaw_travel_rad > 0.0f) {
|
||||
/* 处理yaw控制命令,软件限位 */
|
||||
float delta_yaw_max;
|
||||
float delta_yaw_min;
|
||||
|
||||
if (g->param->limit_and_imu.yaw_imu)
|
||||
{
|
||||
delta_yaw_max =
|
||||
CircleError(g->limit.yaw_max,
|
||||
(g->feedback.eulr.encoder.yaw + g->setpoint.eulr.yaw -
|
||||
g->feedback.eulr.imu.yaw),
|
||||
M_2PI);
|
||||
delta_yaw_min =
|
||||
CircleError(g->limit.yaw_min,
|
||||
(g->feedback.eulr.encoder.yaw + g->setpoint.eulr.yaw -
|
||||
g->feedback.eulr.imu.yaw),
|
||||
M_2PI);
|
||||
}else
|
||||
{
|
||||
delta_yaw_max =
|
||||
CircleError(g->limit.yaw_max,
|
||||
(g->feedback.eulr.encoder.yaw + g->setpoint.eulr.yaw -
|
||||
g->feedback.eulr.encoder.yaw),
|
||||
M_2PI);
|
||||
delta_yaw_min =
|
||||
CircleError(g->limit.yaw_min,
|
||||
(g->feedback.eulr.encoder.yaw + g->setpoint.eulr.yaw -
|
||||
g->feedback.eulr.encoder.yaw),
|
||||
M_2PI);
|
||||
}
|
||||
|
||||
Clip(&(g_cmd->delta_eulr.yaw), delta_yaw_min, delta_yaw_max);
|
||||
}else (CircleAdd(&(g->setpoint.eulr.yaw), g_cmd->delta_eulr.yaw, M_2PI));
|
||||
|
||||
g->setpoint.eulr.yaw += g_cmd->delta_eulr.yaw;
|
||||
if (g->param->limit_and_imu.pitch_travel_rad > 0.0f) {
|
||||
/* 处理pitch控制命令,软件限位 */
|
||||
float delta_pitch_max;
|
||||
float delta_pitch_min;
|
||||
|
||||
if (g->param->limit_and_imu.pit_imu)
|
||||
{
|
||||
delta_pitch_max =
|
||||
CircleError(g->limit.pit_max,
|
||||
(g->feedback.eulr.encoder.pit + g->setpoint.eulr.pit -
|
||||
g->feedback.eulr.imu.pit),
|
||||
M_2PI);
|
||||
delta_pitch_min =
|
||||
CircleError(g->limit.pit_min,
|
||||
(g->feedback.eulr.encoder.pit + g->setpoint.eulr.pit -
|
||||
g->feedback.eulr.imu.pit),
|
||||
M_2PI);
|
||||
}else
|
||||
{
|
||||
delta_pitch_max =
|
||||
CircleError(g->limit.pit_max,
|
||||
(g->feedback.eulr.encoder.pit + g->setpoint.eulr.pit -
|
||||
g->feedback.eulr.encoder.pit),
|
||||
M_2PI);
|
||||
delta_pitch_min =
|
||||
CircleError(g->limit.pit_min,
|
||||
(g->feedback.eulr.encoder.pit + g->setpoint.eulr.pit -
|
||||
g->feedback.eulr.encoder.pit),
|
||||
M_2PI);
|
||||
}
|
||||
Clip(&(g_cmd->delta_eulr.pit), delta_pitch_min, delta_pitch_max);
|
||||
}else (CircleAdd(&(g->setpoint.eulr.pit), g_cmd->delta_eulr.pit, M_2PI));
|
||||
|
||||
/* 处理pitch控制命令,软件限位 */
|
||||
// const float delta_max =
|
||||
// CircleError(g->limit.max,
|
||||
// (g->feedback.eulr.encoder.pit + g->setpoint.eulr.pit -
|
||||
// g->feedback.eulr.imu.pit),
|
||||
// M_2PI);
|
||||
// const float delta_min =
|
||||
// CircleError(g->limit.min,
|
||||
// (g->feedback.eulr.encoder.pit + g->setpoint.eulr.pit -
|
||||
// g->feedback.eulr.imu.pit),
|
||||
// M_2PI);
|
||||
const float delta_max =
|
||||
CircleError(g->limit.max,
|
||||
(g->feedback.eulr.encoder.pit + g->setpoint.eulr.pit -
|
||||
g->feedback.eulr.encoder.pit),
|
||||
M_2PI);
|
||||
const float delta_min =
|
||||
CircleError(g->limit.min,
|
||||
(g->feedback.eulr.encoder.pit + g->setpoint.eulr.pit -
|
||||
g->feedback.eulr.encoder.pit),
|
||||
M_2PI);
|
||||
Clip(&(g_cmd->delta_eulr.pit), delta_min, delta_max);
|
||||
g->setpoint.eulr.pit += g_cmd->delta_eulr.pit;
|
||||
|
||||
/* 重置输入指令,防止重复处理 */
|
||||
@ -174,24 +229,42 @@ int8_t Gimbal_Control(Gimbal_t *g, CMD_GimbalCmd_t *g_cmd, uint32_t now) {
|
||||
for (uint8_t i = 0; i < GIMBAL_ACTR_NUM; i++) g->out[i] = 0.0f;
|
||||
break;
|
||||
|
||||
case GIMBAL_MODE_SEARCH:
|
||||
g->setpoint.eulr.pit = g->limit.pit_min + (g->limit.pit_max - g->limit.pit_min) * (sinf(now / 100.0f) + 1.0f) / 2.0f; // 在min和max之间摆动
|
||||
g->setpoint.eulr.yaw = g->limit.yaw_min + (g->limit.yaw_max - g->limit.yaw_min) * (sinf(now / 100.0f) + 1.0f) / 2.0f; // 在min和max之间摆动
|
||||
case GIMBAL_MODE_ABSOLUTE:
|
||||
yaw_omega_set_point =
|
||||
PID_Calc(&(g->pid[GIMBAL_PID_YAW_ANGLE_IDX]), g->setpoint.eulr.yaw,
|
||||
g->feedback.eulr.imu.yaw, 0.0f, g->dt);
|
||||
g->out[GIMBAL_ACTR_YAW_IDX] =
|
||||
PID_Calc(&(g->pid[GIMBAL_PID_YAW_OMEGA_IDX]), yaw_omega_set_point,
|
||||
g->feedback.gyro.z, 0.f, g->dt);
|
||||
if (g->param->limit_and_imu.yaw_imu && g->param->limit_and_imu.pit_imu){
|
||||
yaw_omega_set_point =
|
||||
PID_Calc(&(g->pid[GIMBAL_PID_YAW_ANGLE_IDX]), g->setpoint.eulr.yaw,
|
||||
g->feedback.eulr.imu.yaw, 0.0f, g->dt);
|
||||
g->out[GIMBAL_ACTR_YAW_IDX] =
|
||||
PID_Calc(&(g->pid[GIMBAL_PID_YAW_OMEGA_IDX]), yaw_omega_set_point,
|
||||
g->feedback.gyro.z, 0.f, g->dt);
|
||||
|
||||
pit_omega_set_point =
|
||||
PID_Calc(&(g->pid[GIMBAL_PID_PIT_ANGLE_IDX]), g->setpoint.eulr.pit,
|
||||
g->feedback.eulr.imu.pit, 0.0f, g->dt);
|
||||
|
||||
g->out[GIMBAL_ACTR_PIT_IDX] =
|
||||
PID_Calc(&(g->pid[GIMBAL_PID_PIT_OMEGA_IDX]), pit_omega_set_point,
|
||||
0.0, 0.f, g->dt);
|
||||
}else{
|
||||
yaw_omega_set_point =
|
||||
PID_Calc(&(g->pid[GIMBAL_PID_YAW_ANGLE_IDX]), g->setpoint.eulr.yaw,
|
||||
g->feedback.eulr.encoder.yaw, 0.0f, g->dt);
|
||||
g->out[GIMBAL_ACTR_YAW_IDX] =
|
||||
PID_Calc(&(g->pid[GIMBAL_PID_YAW_OMEGA_IDX]), yaw_omega_set_point,
|
||||
g->feedback.gyro.z, 0.f, g->dt);
|
||||
|
||||
pit_omega_set_point =
|
||||
PID_Calc(&(g->pid[GIMBAL_PID_PIT_ANGLE_IDX]), g->setpoint.eulr.pit,
|
||||
g->feedback.eulr.encoder.pit, 0.0f, g->dt);
|
||||
|
||||
g->out[GIMBAL_ACTR_PIT_IDX] =
|
||||
PID_Calc(&(g->pid[GIMBAL_PID_PIT_OMEGA_IDX]), pit_omega_set_point,
|
||||
0.0, 0.f, g->dt);
|
||||
}
|
||||
|
||||
// pit_omega_set_point =
|
||||
// PID_Calc(&(g->pid[GIMBAL_PID_PIT_ANGLE_IDX]), g->setpoint.eulr.pit,
|
||||
// g->feedback.eulr.imu.pit, 0.0f, g->dt);
|
||||
pit_omega_set_point =
|
||||
PID_Calc(&(g->pid[GIMBAL_PID_PIT_ANGLE_IDX]), g->setpoint.eulr.pit,
|
||||
g->feedback.eulr.encoder.pit, 0.0f, g->dt);
|
||||
|
||||
g->out[GIMBAL_ACTR_PIT_IDX] =
|
||||
PID_Calc(&(g->pid[GIMBAL_PID_PIT_OMEGA_IDX]), pit_omega_set_point,
|
||||
0.0, 0.f, g->dt);
|
||||
break;
|
||||
|
||||
case GIMBAL_MODE_RELATIVE:
|
||||
@ -219,6 +292,13 @@ int8_t Gimbal_Control(Gimbal_t *g, CMD_GimbalCmd_t *g_cmd, uint32_t now) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
uint8_t Gimbal_PackAI(Gimbal_t *gimbal, const AHRS_Eulr_t *eulr_gimbal_for_ai) {
|
||||
memcpy((void *)eulr_gimbal_for_ai, (const void *)&(gimbal->feedback.eulr.encoder),
|
||||
sizeof(gimbal->feedback.eulr.encoder));
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief 复制云台输出值
|
||||
*
|
||||
|
@ -52,22 +52,28 @@ typedef struct {
|
||||
float gyro; /* 陀螺仪数据 */
|
||||
} low_pass_cutoff_freq;
|
||||
|
||||
float pitch_travel_rad; /* 云台pitch轴行程弧度 */
|
||||
/* 设置云台限位和是否启用imu */
|
||||
struct {
|
||||
float yaw_travel_rad; /* 云台yaw轴行程弧度 */
|
||||
float pitch_travel_rad; /* 云台pitch轴行程弧度 */
|
||||
bool yaw_imu;
|
||||
bool pit_imu;
|
||||
} limit_and_imu;
|
||||
|
||||
/* 设置默认运动方向 */
|
||||
struct {
|
||||
bool yaw;
|
||||
bool pit;
|
||||
} reverse;
|
||||
|
||||
bool pit_ctrl_reverse; /* 云台pitch遥控方向 */
|
||||
|
||||
} Gimbal_Params_t;
|
||||
|
||||
/* 软件限位 */
|
||||
typedef struct {
|
||||
float max;
|
||||
float min;
|
||||
float yaw_max;
|
||||
float yaw_min;
|
||||
float pit_max;
|
||||
float pit_min;
|
||||
} Gimbal_Limit_t;
|
||||
|
||||
/* 云台反馈数据的结构体,包含反馈控制用的反馈数据 */
|
||||
@ -135,6 +141,9 @@ int8_t Gimbal_Init(Gimbal_t *g, const Gimbal_Params_t *param, float limit,
|
||||
*/
|
||||
int8_t Gimbal_UpdateFeedback(Gimbal_t *gimbal, const CAN_t *can);
|
||||
|
||||
uint8_t Gimbal_PackAI(Gimbal_t *gimbal, const AHRS_Eulr_t *eulr_gimbal_for_ai);
|
||||
|
||||
|
||||
/**
|
||||
* \brief 运行云台控制逻辑
|
||||
*
|
||||
@ -147,6 +156,9 @@ int8_t Gimbal_UpdateFeedback(Gimbal_t *gimbal, const CAN_t *can);
|
||||
*/
|
||||
int8_t Gimbal_Control(Gimbal_t *g, CMD_GimbalCmd_t *g_cmd, uint32_t now);
|
||||
|
||||
uint8_t Gimbal_PackAI(Gimbal_t *gimbal, const AHRS_Eulr_t *eulr_gimbal_for_ai);
|
||||
|
||||
|
||||
/**
|
||||
* \brief 复制云台输出值
|
||||
*
|
||||
|
@ -17,11 +17,13 @@ AI_t ai;
|
||||
CMD_Host_t cmd_host;
|
||||
AHRS_Quaternion_t quat;
|
||||
Referee_ForAI_t referee_ai;
|
||||
AHRS_Eulr_t gimbal_ai;
|
||||
#else
|
||||
static AI_t ai;
|
||||
static CMD_Host_t cmd_host;
|
||||
static AHRS_Quaternion_t quat;
|
||||
static Referee_ForAI_t referee_ai;
|
||||
static AHRS_Eulr_t gimbal_ai;
|
||||
#endif
|
||||
|
||||
/* Private function --------------------------------------------------------- */
|
||||
@ -42,9 +44,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());
|
||||
@ -54,19 +54,25 @@ void Task_Ai(void *argument) {
|
||||
|
||||
AI_StartReceiving(&ai);
|
||||
if (AI_WaitDmaCplt()) {
|
||||
AI_ParseHost(&ai, &cmd_host);
|
||||
AI_ParseHost(&ai);
|
||||
last_online_tick = tick;
|
||||
} else {
|
||||
if (tick - last_online_tick > 300) 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);
|
||||
osMessageQueueGet(task_runtime.msgq.gimbal.eulr_encoder, &(gimbal_ai), NULL, 0);
|
||||
bool ref_update = (osMessageQueueGet(task_runtime.msgq.referee.ai,
|
||||
&(referee_ai), NULL, 0) == osOK);
|
||||
AI_PackMCU(&ai, &quat);
|
||||
|
||||
AI_PackMCU(&ai, &quat, &gimbal_ai);
|
||||
if (ref_update) AI_PackRef(&ai, &(referee_ai));
|
||||
|
||||
AI_StartSend(&(ai), ref_update);
|
||||
|
@ -126,10 +126,10 @@ void Task_AttiEsti(void *argument) {
|
||||
osMessageQueuePut(task_runtime.msgq.gimbal.accl, &bmi088.accl, 0, 0);
|
||||
osMessageQueueReset(task_runtime.msgq.gimbal.eulr_imu);
|
||||
osMessageQueuePut(task_runtime.msgq.gimbal.eulr_imu, &eulr_to_send, 0, 0);
|
||||
osMessageQueueReset(task_runtime.msgq.ai.quat);
|
||||
osMessageQueuePut(task_runtime.msgq.ai.quat, &(gimbal_ahrs.quat), 0, 0);
|
||||
osMessageQueueReset(task_runtime.msgq.gimbal.gyro);
|
||||
osMessageQueuePut(task_runtime.msgq.gimbal.gyro, &bmi088.gyro, 0, 0);
|
||||
osMessageQueueReset(task_runtime.msgq.ai.quat);
|
||||
osMessageQueuePut(task_runtime.msgq.ai.quat, &(gimbal_ahrs.quat), 0, 0);
|
||||
|
||||
/* PID控制IMU温度,PWM输出 */
|
||||
BSP_PWM_Set(BSP_PWM_IMU_HEAT,
|
||||
|
@ -32,10 +32,10 @@ typedef struct {
|
||||
|
||||
static const char *const CLI_WELCOME_MESSAGE =
|
||||
"\r\n"
|
||||
// " ______ __ _______ __ \r\n"
|
||||
// " | __ \\.-----.| |--.-----.| | |.---.-.-----.| |_.-----.----.\r\n"
|
||||
// " | <| _ || _ | _ || || _ |__ --|| _| -__| _|\r\n"
|
||||
// " |___|__||_____||_____|_____||__|_|__||___._|_____||____|_____|__| \r\n"
|
||||
" ______ __ _______ __ \r\n"
|
||||
" | __ \\.-----.| |--.-----.| | |.---.-.-----.| |_.-----.----.\r\n"
|
||||
" | <| _ || _ | _ || || _ |__ --|| _| -__| _|\r\n"
|
||||
" |___|__||_____||_____|_____||__|_|__||___._|_____||____|_____|__| \r\n"
|
||||
" -------------------------------------------------------------------\r\n"
|
||||
" FreeRTOS CLI. Type 'help' to view a list of registered commands. \r\n"
|
||||
"\r\n";
|
||||
@ -523,7 +523,7 @@ static BaseType_t Command_SetGimbalLim(char *out_buffer, size_t len,
|
||||
switch (fsm.stage) {
|
||||
case 0:
|
||||
Config_Get(&cfg);
|
||||
snprintf(out_buffer, len, "(old)limit:%f\r\n", cfg.gimbal_limit);
|
||||
snprintf(out_buffer, len, "(old)pit_limit:%f,(old)yaw_limit:%f\r\n", cfg.gimbal_pitch_limit, cfg.gimbal_yaw_limit);
|
||||
fsm.stage = 1;
|
||||
return pdPASS;
|
||||
case 1:
|
||||
@ -538,12 +538,12 @@ static BaseType_t Command_SetGimbalLim(char *out_buffer, size_t len,
|
||||
}
|
||||
Config_Get(&cfg);
|
||||
osThreadResume(task_runtime.thread.ctrl_gimbal);
|
||||
cfg.gimbal_limit = can.motor.gimbal.named.pit.rotor_angle;
|
||||
cfg.gimbal_pitch_limit = can.motor.gimbal.named.pit.rotor_angle;
|
||||
cfg.gimbal_yaw_limit = can.motor.gimbal.named.yaw.rotor_angle;
|
||||
|
||||
Config_Set(&cfg);
|
||||
Config_Get(&cfg);
|
||||
snprintf(out_buffer, len, "(new)limit:%f\r\nDone.", cfg.gimbal_limit);
|
||||
|
||||
snprintf(out_buffer, len, "(new)limit_pitch:%f,limit_yaw%f\r\nDone.", cfg.gimbal_pitch_limit, cfg.gimbal_yaw_limit);
|
||||
fsm.stage = 2;
|
||||
return pdPASS;
|
||||
case 2:
|
||||
@ -692,7 +692,7 @@ void Task_CLI(void *argument) {
|
||||
BSP_USB_Printf(CLI_WELCOME_MESSAGE);
|
||||
|
||||
/* 开始运行命令行界面 */
|
||||
BSP_USB_Printf("rm>");
|
||||
BSP_USB_Printf("qut-move-rm>");
|
||||
while (1) {
|
||||
#ifdef DEBUG
|
||||
/* 记录任务所使用的的栈空间 */
|
||||
@ -730,7 +730,7 @@ void Task_CLI(void *argument) {
|
||||
index = 0; /* 重置索引,准备接收下一段命令 */
|
||||
memset(input, 0x00, strlen(input)); /* 清空输入缓存 */
|
||||
}
|
||||
BSP_USB_Printf("rm>");
|
||||
BSP_USB_Printf("qut-move-rm>");
|
||||
} else if (rx_char == '\b' || rx_char == 0x7Fu) {
|
||||
/* 如果输入的是退格键则清空一位输入缓存,同时进行界限保护 */
|
||||
if (index > 0) {
|
||||
|
@ -22,11 +22,13 @@ CMD_GimbalCmd_t gimbal_cmd;
|
||||
Gimbal_t gimbal;
|
||||
CAN_GimbalOutput_t gimbal_out;
|
||||
Referee_GimbalUI_t gimbal_ui;
|
||||
AHRS_Eulr_t eulr_gimbal_for_ai;
|
||||
#else
|
||||
static CMD_GimbalCmd_t gimbal_cmd;
|
||||
static Gimbal_t gimbal;
|
||||
static CAN_GimbalOutput_t gimbal_out;
|
||||
static Referee_GimbalUI_t gimbal_ui;
|
||||
static AHRS_Eulr_t eulr_gimbal_for_ai;
|
||||
#endif
|
||||
|
||||
/* Private function --------------------------------------------------------- */
|
||||
@ -43,7 +45,7 @@ void Task_CtrlGimbal(void *argument) {
|
||||
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CTRL_GIMBAL;
|
||||
/* 初始化云台 */
|
||||
Gimbal_Init(&gimbal, &(task_runtime.cfg.robot_param->gimbal),
|
||||
task_runtime.cfg.gimbal_limit, (float)TASK_FREQ_CTRL_GIMBAL);
|
||||
task_runtime.cfg.gimbal_pitch_limit, (float)TASK_FREQ_CTRL_GIMBAL);
|
||||
|
||||
/* 延时一段时间再开启任务 */
|
||||
osMessageQueueGet(task_runtime.msgq.can.feedback.gimbal, &can, NULL,
|
||||
@ -66,12 +68,15 @@ void Task_CtrlGimbal(void *argument) {
|
||||
|
||||
osKernelLock(); /* 锁住RTOS内核防止控制过程中断,造成错误 */
|
||||
Gimbal_UpdateFeedback(&gimbal, &can);
|
||||
Gimbal_PackAI(&gimbal, &eulr_gimbal_for_ai);
|
||||
Gimbal_Control(&gimbal, &gimbal_cmd, tick);
|
||||
Gimbal_DumpOutput(&gimbal, &gimbal_out);
|
||||
|
||||
osKernelUnlock();
|
||||
osMessageQueueReset(task_runtime.msgq.can.output.gimbal);
|
||||
osMessageQueuePut(task_runtime.msgq.can.output.gimbal, &gimbal_out, 0, 0);
|
||||
osMessageQueueReset(task_runtime.msgq.gimbal.eulr_encoder);
|
||||
osMessageQueuePut(task_runtime.msgq.gimbal.eulr_encoder, &eulr_gimbal_for_ai, 0, 0);
|
||||
|
||||
Gimbal_DumpUI(&gimbal, &gimbal_ui);
|
||||
osMessageQueueReset(task_runtime.msgq.ui.gimbal);
|
||||
|
176
User/task/init.c
176
User/task/init.c
@ -30,101 +30,103 @@
|
||||
*
|
||||
* \param argument 未使用
|
||||
*/
|
||||
void Task_Init(void *argument) {
|
||||
(void)argument; /* 未使用argument,消除警告 */
|
||||
void Task_Init(void *argument)
|
||||
{
|
||||
(void)argument; /* 未使用argument,消除警告 */
|
||||
|
||||
Config_Get(&task_runtime.cfg); /* 获取机器人配置 */
|
||||
Config_Get(&task_runtime.cfg); /* 获取机器人配置 */
|
||||
|
||||
osKernelLock();
|
||||
/* 创建任务 */
|
||||
task_runtime.thread.atti_esti =
|
||||
osThreadNew(Task_AttiEsti, NULL, &attr_atti_esti);
|
||||
task_runtime.thread.cli = osThreadNew(Task_CLI, NULL, &attr_cli);
|
||||
task_runtime.thread.command = osThreadNew(Task_Command, NULL, &attr_command);
|
||||
task_runtime.thread.ctrl_chassis =
|
||||
osThreadNew(Task_CtrlChassis, NULL, &attr_ctrl_chassis);
|
||||
task_runtime.thread.ctrl_gimbal =
|
||||
osThreadNew(Task_CtrlGimbal, NULL, &attr_ctrl_gimbal);
|
||||
task_runtime.thread.ctrl_shoot =
|
||||
osThreadNew(Task_CtrlShoot, NULL, &attr_ctrl_shoot);
|
||||
task_runtime.thread.info = osThreadNew(Task_Info, NULL, &attr_info);
|
||||
task_runtime.thread.monitor = osThreadNew(Task_Monitor, NULL, &attr_monitor);
|
||||
task_runtime.thread.can = osThreadNew(Task_Can, NULL, &attr_can);
|
||||
task_runtime.thread.referee = osThreadNew(Task_Referee, NULL, &attr_referee);
|
||||
task_runtime.thread.ai = osThreadNew(Task_Ai, NULL, &attr_ai);
|
||||
task_runtime.thread.rc = osThreadNew(Task_RC, NULL, &attr_rc);
|
||||
task_runtime.thread.cap = osThreadNew(Task_Cap, NULL, &attr_cap);
|
||||
osKernelLock();
|
||||
/* 创建任务 */
|
||||
task_runtime.thread.atti_esti =
|
||||
osThreadNew(Task_AttiEsti, NULL, &attr_atti_esti);
|
||||
task_runtime.thread.cli = osThreadNew(Task_CLI, NULL, &attr_cli);
|
||||
task_runtime.thread.command = osThreadNew(Task_Command, NULL, &attr_command);
|
||||
task_runtime.thread.ctrl_chassis =
|
||||
osThreadNew(Task_CtrlChassis, NULL, &attr_ctrl_chassis);
|
||||
task_runtime.thread.ctrl_gimbal =
|
||||
osThreadNew(Task_CtrlGimbal, NULL, &attr_ctrl_gimbal);
|
||||
task_runtime.thread.ctrl_shoot =
|
||||
osThreadNew(Task_CtrlShoot, NULL, &attr_ctrl_shoot);
|
||||
task_runtime.thread.info = osThreadNew(Task_Info, NULL, &attr_info);
|
||||
task_runtime.thread.monitor = osThreadNew(Task_Monitor, NULL, &attr_monitor);
|
||||
task_runtime.thread.can = osThreadNew(Task_Can, NULL, &attr_can);
|
||||
task_runtime.thread.referee = osThreadNew(Task_Referee, NULL, &attr_referee);
|
||||
task_runtime.thread.ai = osThreadNew(Task_Ai, NULL, &attr_ai);
|
||||
task_runtime.thread.rc = osThreadNew(Task_RC, NULL, &attr_rc);
|
||||
task_runtime.thread.cap = osThreadNew(Task_Cap, NULL, &attr_cap);
|
||||
|
||||
/* 创建消息队列 */
|
||||
/* motor */
|
||||
task_runtime.msgq.can.feedback.chassis =
|
||||
osMessageQueueNew(2u, sizeof(CAN_t), NULL);
|
||||
task_runtime.msgq.can.feedback.gimbal =
|
||||
osMessageQueueNew(2u, sizeof(CAN_t), NULL);
|
||||
task_runtime.msgq.can.feedback.shoot =
|
||||
osMessageQueueNew(2u, sizeof(CAN_t), NULL);
|
||||
task_runtime.msgq.can.feedback.cap =
|
||||
osMessageQueueNew(2u, sizeof(CAN_t), NULL);
|
||||
task_runtime.msgq.can.output.chassis =
|
||||
osMessageQueueNew(2u, sizeof(CAN_ChassisOutput_t), NULL);
|
||||
task_runtime.msgq.can.output.gimbal =
|
||||
osMessageQueueNew(2u, sizeof(CAN_GimbalOutput_t), NULL);
|
||||
task_runtime.msgq.can.output.shoot =
|
||||
osMessageQueueNew(2u, sizeof(CAN_ShootOutput_t), NULL);
|
||||
task_runtime.msgq.can.output.cap =
|
||||
osMessageQueueNew(2u, sizeof(CAN_CapOutput_t), NULL);
|
||||
/* 创建消息队列 */
|
||||
/* motor */
|
||||
task_runtime.msgq.can.feedback.chassis =
|
||||
osMessageQueueNew(2u, sizeof(CAN_t), NULL);
|
||||
task_runtime.msgq.can.feedback.gimbal =
|
||||
osMessageQueueNew(2u, sizeof(CAN_t), NULL);
|
||||
task_runtime.msgq.can.feedback.shoot =
|
||||
osMessageQueueNew(2u, sizeof(CAN_t), NULL);
|
||||
task_runtime.msgq.can.feedback.cap =
|
||||
osMessageQueueNew(2u, sizeof(CAN_t), NULL);
|
||||
task_runtime.msgq.can.output.chassis =
|
||||
osMessageQueueNew(2u, sizeof(CAN_ChassisOutput_t), NULL);
|
||||
task_runtime.msgq.can.output.gimbal =
|
||||
osMessageQueueNew(2u, sizeof(CAN_GimbalOutput_t), NULL);
|
||||
task_runtime.msgq.can.output.shoot =
|
||||
osMessageQueueNew(2u, sizeof(CAN_ShootOutput_t), NULL);
|
||||
task_runtime.msgq.can.output.cap =
|
||||
osMessageQueueNew(2u, sizeof(CAN_CapOutput_t), NULL);
|
||||
|
||||
/* command */
|
||||
task_runtime.msgq.cmd.chassis =
|
||||
osMessageQueueNew(3u, sizeof(CMD_ChassisCmd_t), NULL);
|
||||
task_runtime.msgq.cmd.gimbal =
|
||||
osMessageQueueNew(3u, sizeof(CMD_GimbalCmd_t), NULL);
|
||||
task_runtime.msgq.cmd.shoot =
|
||||
osMessageQueueNew(3u, sizeof(CMD_ShootCmd_t), NULL);
|
||||
task_runtime.msgq.cmd.ai =
|
||||
osMessageQueueNew(3u, sizeof(CMD_AI_Status_t), NULL);
|
||||
task_runtime.msgq.cmd.referee = osMessageQueueNew(6u, sizeof(CMD_UI_t), NULL);
|
||||
/* command */
|
||||
task_runtime.msgq.cmd.chassis =
|
||||
osMessageQueueNew(3u, sizeof(CMD_ChassisCmd_t), NULL);
|
||||
task_runtime.msgq.cmd.gimbal =
|
||||
osMessageQueueNew(3u, sizeof(CMD_GimbalCmd_t), NULL);
|
||||
task_runtime.msgq.cmd.shoot =
|
||||
osMessageQueueNew(3u, sizeof(CMD_ShootCmd_t), NULL);
|
||||
task_runtime.msgq.cmd.ai =
|
||||
osMessageQueueNew(3u, sizeof(CMD_AI_Status_t), NULL);
|
||||
task_runtime.msgq.cmd.referee = osMessageQueueNew(6u, sizeof(CMD_UI_t), NULL);
|
||||
|
||||
/* atti_esti */
|
||||
task_runtime.msgq.cmd.raw.rc = osMessageQueueNew(3u, sizeof(CMD_RC_t), NULL);
|
||||
task_runtime.msgq.cmd.raw.host =
|
||||
osMessageQueueNew(3u, sizeof(CMD_Host_t), NULL);
|
||||
/* atti_esti */
|
||||
task_runtime.msgq.cmd.raw.rc = osMessageQueueNew(3u, sizeof(CMD_RC_t), NULL);
|
||||
task_runtime.msgq.cmd.raw.host =
|
||||
osMessageQueueNew(3u, sizeof(CMD_Host_t), NULL);
|
||||
|
||||
task_runtime.msgq.gimbal.accl =
|
||||
osMessageQueueNew(2u, sizeof(AHRS_Accl_t), NULL);
|
||||
task_runtime.msgq.gimbal.eulr_imu =
|
||||
osMessageQueueNew(2u, sizeof(AHRS_Eulr_t), NULL);
|
||||
task_runtime.msgq.gimbal.gyro =
|
||||
osMessageQueueNew(2u, sizeof(AHRS_Gyro_t), NULL);
|
||||
task_runtime.msgq.gimbal.accl =
|
||||
osMessageQueueNew(2u, sizeof(AHRS_Accl_t), NULL);
|
||||
task_runtime.msgq.gimbal.eulr_imu =
|
||||
osMessageQueueNew(2u, sizeof(AHRS_Eulr_t), NULL);
|
||||
task_runtime.msgq.gimbal.gyro =
|
||||
osMessageQueueNew(2u, sizeof(AHRS_Gyro_t), NULL);
|
||||
task_runtime.msgq.gimbal.eulr_encoder =
|
||||
osMessageQueueNew(2u, sizeof(AHRS_Eulr_t), NULL);
|
||||
task_runtime.msgq.cap_info =
|
||||
osMessageQueueNew(2u, sizeof(CAN_Capacitor_t), NULL);
|
||||
|
||||
task_runtime.msgq.cap_info =
|
||||
osMessageQueueNew(2u, sizeof(CAN_Capacitor_t), NULL);
|
||||
/* AI */
|
||||
task_runtime.msgq.ai.quat =
|
||||
osMessageQueueNew(2u, sizeof(AHRS_Quaternion_t), NULL);
|
||||
|
||||
/* AI */
|
||||
task_runtime.msgq.ai.quat =
|
||||
osMessageQueueNew(2u, sizeof(AHRS_Quaternion_t), NULL);
|
||||
/* 裁判系统 */
|
||||
task_runtime.msgq.referee.ai =
|
||||
osMessageQueueNew(2u, sizeof(Referee_ForAI_t), NULL);
|
||||
task_runtime.msgq.referee.chassis =
|
||||
osMessageQueueNew(2u, sizeof(Referee_ForChassis_t), NULL);
|
||||
task_runtime.msgq.referee.cap =
|
||||
osMessageQueueNew(2u, sizeof(Referee_ForCap_t), NULL);
|
||||
task_runtime.msgq.referee.shoot =
|
||||
osMessageQueueNew(2u, sizeof(Referee_ForShoot_t), NULL);
|
||||
|
||||
/* 裁判系统 */
|
||||
task_runtime.msgq.referee.ai =
|
||||
osMessageQueueNew(2u, sizeof(Referee_ForAI_t), NULL);
|
||||
task_runtime.msgq.referee.chassis =
|
||||
osMessageQueueNew(2u, sizeof(Referee_ForChassis_t), NULL);
|
||||
task_runtime.msgq.referee.cap =
|
||||
osMessageQueueNew(2u, sizeof(Referee_ForCap_t), NULL);
|
||||
task_runtime.msgq.referee.shoot =
|
||||
osMessageQueueNew(2u, sizeof(Referee_ForShoot_t), NULL);
|
||||
/* UI */
|
||||
task_runtime.msgq.ui.chassis =
|
||||
osMessageQueueNew(2u, sizeof(Referee_ChassisUI_t), NULL);
|
||||
task_runtime.msgq.ui.cap =
|
||||
osMessageQueueNew(2u, sizeof(Referee_CapUI_t), NULL);
|
||||
task_runtime.msgq.ui.gimbal =
|
||||
osMessageQueueNew(2u, sizeof(Referee_GimbalUI_t), NULL);
|
||||
task_runtime.msgq.ui.shoot =
|
||||
osMessageQueueNew(2u, sizeof(Referee_ShootUI_t), NULL);
|
||||
task_runtime.msgq.ui.cmd = osMessageQueueNew(2u, sizeof(bool), NULL);
|
||||
|
||||
/* UI */
|
||||
task_runtime.msgq.ui.chassis =
|
||||
osMessageQueueNew(2u, sizeof(Referee_ChassisUI_t), NULL);
|
||||
task_runtime.msgq.ui.cap =
|
||||
osMessageQueueNew(2u, sizeof(Referee_CapUI_t), NULL);
|
||||
task_runtime.msgq.ui.gimbal =
|
||||
osMessageQueueNew(2u, sizeof(Referee_GimbalUI_t), NULL);
|
||||
task_runtime.msgq.ui.shoot =
|
||||
osMessageQueueNew(2u, sizeof(Referee_ShootUI_t), NULL);
|
||||
task_runtime.msgq.ui.cmd = osMessageQueueNew(2u, sizeof(bool), NULL);
|
||||
|
||||
osKernelUnlock();
|
||||
osThreadTerminate(osThreadGetId()); /* 结束自身 */
|
||||
osKernelUnlock();
|
||||
osThreadTerminate(osThreadGetId()); /* 结束自身 */
|
||||
}
|
||||
|
@ -58,6 +58,8 @@ typedef struct {
|
||||
osMessageQueueId_t accl; /* IMU读取 */
|
||||
osMessageQueueId_t gyro; /* IMU读取 */
|
||||
osMessageQueueId_t eulr_imu; /* 姿态解算得到 */
|
||||
osMessageQueueId_t eulr_encoder; /* 读编码器得到 */
|
||||
|
||||
} gimbal;
|
||||
|
||||
/* 控制指令 */
|
||||
|
Loading…
Reference in New Issue
Block a user