xiaocheng/Er_sentry
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

sentry

Browse Source
This commit is contained in:
Xiaocheng 2026-01-12 21:26:57 +08:00
parent 7003dbed15
commit 88acd92609
9 changed files with 213 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

134
User/device/ai.c
View File

@ -1,23 +1,24 @@
#include "ai.h" #include "device/ai.h"
#include "device/device.h"
#include "bsp/uart.h" #include "bsp/uart.h"
#include "component/crc16.h" #include "component/crc16.h"
int8_t AI_Init(AI_t *ai) {
if (ai == NULL) return DEVICE_ERR_NULL;
// if (inited) return DEVICE_ERR_INITED;
// if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL;
// void ai_init(AI_t* ai){ BSP_UART_RegisterCallback(BSP_UART_NUC, BSP_UART_NUC,
// ai->RX.head[0]="M"; BSP_UART_RX_CPLT_CB);
// ai->RX.head[0]="R"; // inited = true;
// ai->TX.head[0]="M"; return 0;
// ai->TX.head[0]="R"; }
// }
int8_t AI_StartReceiving(AI_t *ai) { int8_t AI_StartReceiving(AI_t *ai) {
if (BSP_UART_Receive(BSP_UART_NUC,&ai->RX,sizeof(ai->RX), false)) if (BSP_UART_Receive(BSP_UART_NUC,&ai->RX,sizeof(ai->RX), true)==HAL_OK) {
return DEVICE_OK; return DEVICE_OK;}
return DEVICE_ERR; return DEVICE_ERR;
} }
@ -26,9 +27,9 @@ int8_t AI_Get_NUC(AI_t *ai,AI_cmd_t* ai_cmd) {
return DEVICE_ERR; return DEVICE_ERR;
} }
// CRC16_Calc(&ai->RX,sizeof(ai->RX),ai->RX.crc16); // CRC16_Calc(&ai->RX,sizeof(ai->RX),ai->RX.crc16);
// if(CRC16_Verify((const uint8_t*)&(ai->RX), sizeof(ai->RX.crc16))!=true){ if(CRC16_Verify((const uint8_t*)&(ai->RX), sizeof(ai->RX))!=true){
// return DEVICE_ERR; return DEVICE_ERR;
// } }
ai_cmd->gimbal_t.setpoint.pit=ai->RX.pitch; ai_cmd->gimbal_t.setpoint.pit=ai->RX.pitch;
ai_cmd->gimbal_t.setpoint.yaw=ai->RX.yaw; ai_cmd->gimbal_t.setpoint.yaw=ai->RX.yaw;
ai_cmd->mode=ai->RX.mode; ai_cmd->mode=ai->RX.mode;
@ -40,24 +41,24 @@ int8_t AI_Get_NUC(AI_t *ai,AI_cmd_t* ai_cmd) {
} }
int8_t AI_ParseHost(AI_t *ai,Gimbal_feedback_t* motor,AHRS_Quaternion_t* quat) { int8_t AI_ParseHost(AI_t* ai,Gimbal_feedback_t* motor,AHRS_Quaternion_t* quat) {
ai->TX.head[0]='M'; ai->TX.head[0]='M';
ai->TX.head[1]='R'; ai->TX.head[1]='R';
// ai->TX.mode=2; // ai->TX.mode=2;
ai->TX.pitch=motor->motor.pitch_4310_motor_feedback.rotor_abs_angle; ai->TX.pitch=motor->motor.pitch_4310_motor_feedback.rotor_abs_angle;
ai->TX.yaw=motor->motor.yaw_4310_motor_feedback.rotor_abs_angle; ai->TX.yaw=motor->motor.yaw_4310_motor_feedback.rotor_abs_angle;
ai->TX.pitch_vel=motor->motor.pitch_4310_motor_feedback.rotor_speed; ai->TX.pitch_vel=motor->motor.pitch_4310_motor_feedback.rotor_speed;
ai->TX.yaw_vel=motor->motor.yaw_4310_motor_feedback.rotor_speed; ai->TX.yaw_vel=motor->motor.yaw_4310_motor_feedback.rotor_speed;
ai->TX.q[0]=motor->imu.quat.q1; ai->TX.q[0]=motor->imu.quat.q0;
ai->TX.q[1]=motor->imu.quat.q2; ai->TX.q[1]=motor->imu.quat.q1;
ai->TX.q[2]=motor->imu.quat.q3; ai->TX.q[2]=motor->imu.quat.q2;
ai->TX.q[3]=motor->imu.quat.q0; ai->TX.q[3]=motor->imu.quat.q3;
ai->TX.bullet_count=10; ai->TX.bullet_count=10;
ai->TX.bullet_speed=10; ai->TX.bullet_speed=22;
ai->TX.crc16=CRC16_Calc(((const uint8_t*)&(ai->TX)),sizeof(ai->TX)-sizeof(uint16_t),ai->TX.crc16); ai->TX.crc16=CRC16_Calc(((const uint8_t*)&(ai->TX)),sizeof(ai->TX)-sizeof(uint16_t), CRC16_INIT );
if(CRC16_Verify(((const uint8_t*)&(ai->TX)), sizeof(ai->TX))!=true){ if(CRC16_Verify(((const uint8_t*)&(ai->TX)), sizeof(ai->TX))!=true){
return DEVICE_ERR; return DEVICE_ERR;
} }
@ -65,8 +66,8 @@ int8_t AI_ParseHost(AI_t *ai,Gimbal_feedback_t* motor,AHRS_Quaternion_t* quat) {
} }
int8_t AI_StartSend(AI_t *ai) { int8_t AI_StartSend(AI_t *ai) {
// if (ref_update) {
if (BSP_UART_Transmit(BSP_UART_NUC,&ai->TX,sizeof(ai->TX), false)) if (BSP_UART_Transmit(BSP_UART_NUC,&ai->TX,sizeof(ai->TX), true)==HAL_OK)
return DEVICE_OK; return DEVICE_OK;
// else // else
// return DEVICE_ERR; // return DEVICE_ERR;
@ -79,3 +80,86 @@ int8_t AI_StartSend(AI_t *ai) {
// return DEVICE_ERR; // return DEVICE_ERR;
// } // }
} }
// #include "ai.h"
// #include "bsp/uart.h"
// #include "component/crc16.h"
// // void ai_init(AI_t* ai){
// // ai->RX.head[0]="M";
// // ai->RX.head[0]="R";
// // ai->TX.head[0]="M";
// // ai->TX.head[0]="R";
// // }
// int8_t AI_StartReceiving(AI_t *ai) {
// if (BSP_UART_Receive(BSP_UART_NUC,&ai->RX,sizeof(ai->RX), false))
// return DEVICE_OK;
// return DEVICE_ERR;
// }
// int8_t AI_Get_NUC(AI_t *ai,AI_cmd_t* ai_cmd) {
// if(ai->RX.head[0]!='M'&&ai->RX.head[1]!='R'){
// return DEVICE_ERR;
// }
// // CRC16_Calc(&ai->RX,sizeof(ai->RX),ai->RX.crc16);
// // if(CRC16_Verify((const uint8_t*)&(ai->RX), sizeof(ai->RX.crc16))!=true){
// // return DEVICE_ERR;
// // }
// ai_cmd->gimbal_t.setpoint.pit=ai->RX.pitch;
// ai_cmd->gimbal_t.setpoint.yaw=ai->RX.yaw;
// ai_cmd->mode=ai->RX.mode;
// ai_cmd->gimbal_t.accl.pit=ai->RX.pitch_acc;
// ai_cmd->gimbal_t.vel.pit=ai->RX.pitch_vel;
// ai_cmd->gimbal_t.accl.yaw=ai->RX.yaw_acc;
// ai_cmd->gimbal_t.vel.yaw=ai->RX.yaw_vel;
// return DEVICE_OK;
// }
// int8_t AI_ParseHost(AI_t *ai,Gimbal_feedback_t* motor,AHRS_Quaternion_t* quat) {
// ai->TX.head[0]='M';
// ai->TX.head[1]='R';
// // ai->TX.mode=2;
// ai->TX.pitch=motor->motor.pitch_4310_motor_feedback.rotor_abs_angle;
// ai->TX.yaw=motor->motor.yaw_4310_motor_feedback.rotor_abs_angle;
// ai->TX.pitch_vel=motor->motor.pitch_4310_motor_feedback.rotor_speed;
// ai->TX.yaw_vel=motor->motor.yaw_4310_motor_feedback.rotor_speed;
// ai->TX.q[0]=motor->imu.quat.q1;
// ai->TX.q[1]=motor->imu.quat.q2;
// ai->TX.q[2]=motor->imu.quat.q3;
// ai->TX.q[3]=motor->imu.quat.q0;
// ai->TX.bullet_count=10;
// ai->TX.bullet_speed=10;
// ai->TX.crc16=CRC16_Calc(((const uint8_t*)&(ai->TX)),sizeof(ai->TX)-sizeof(uint16_t),ai->TX.crc16);
// if(CRC16_Verify(((const uint8_t*)&(ai->TX)), sizeof(ai->TX))!=true){
// return DEVICE_ERR;
// }
// return DEVICE_OK;
// }
// int8_t AI_StartSend(AI_t *ai) {
// // if (ref_update) {
// if (BSP_UART_Transmit(BSP_UART_NUC,&ai->TX,sizeof(ai->TX), false))
// return DEVICE_OK;
// // else
// // return DEVICE_ERR;
// // } else {
// // if (HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_AI),
// // (uint8_t *)&(ai->TX),
// // sizeof(ai->TX)) == HAL_OK)
// // return DEVICE_OK;
// // else
// // return DEVICE_ERR;
// // }
// }

10
User/module/chassis.c
View File

@ -128,8 +128,8 @@ int8_t chassis_init(Chassis_t *c, Chassis_Param_t *param, float target_freq)
} }
// 舵轮安装时的6020机械误差，机械校准时1号轮在左前方，所有轮的编码器朝向右面 // 舵轮安装时的6020机械误差，机械校准时1号轮在左前方，所有轮的编码器朝向右面
MotorOffset_t motor_offset = {{3.67848611 / M_PI * 180.0f, 2.61390328 / M_PI * 180.0f, MotorOffset_t motor_offset = {{1.6467284 / M_PI * 180.0f, 5.77390385 / M_PI * 180.0f,
2.11075759 / M_PI * 180.0f, 2.62694216 / M_PI * 180.0f}}; // 右右右右 1.56696141 / M_PI * 180.0f, 5.77160311 / M_PI * 180.0f}}; // 右右右右
c->motoroffset = motor_offset; c->motoroffset = motor_offset;
/*对3508的速度环和6020的角速度以及位置环pid进行初始化*/ /*对3508的速度环和6020的角速度以及位置环pid进行初始化*/
@ -394,9 +394,9 @@ int8_t Chassis_Control(Chassis_t *c, Chassis_CMD_t *c_cmd,uint32_t now)
case CHASSIS_MODE_FOLLOW_GIMBAL: case CHASSIS_MODE_FOLLOW_GIMBAL:
// 跟随云台模式 // 跟随云台模式
c->move_vec.Vx =c_cmd->y_l; c->move_vec.Vx =-c_cmd->y_l;
c->move_vec.Vy =c_cmd->x_l; c->move_vec.Vy =-c_cmd->x_l;
c->move_vec.Vw = -PID_Calc(&c->pid.chassis_follow_gimbal_pid, 2.06f ,c->motorfeedback.gimbal_yaw_encoder.rotor_abs_angle, 0.0f, c->dt); c->move_vec.Vw = PID_Calc(&c->pid.chassis_follow_gimbal_pid, 2.06f ,c->motorfeedback.gimbal_yaw_encoder.rotor_abs_angle, 0.0f, c->dt);
// c->move_vec.Vw = -PID_Calc(&c->pid.chassis_follow_gimbal_pid, c->motorfeedback.gimbal_yaw_encoder-2.07694483f ,c->motorfeedback.gimbal_yaw_encoder, 0.0f, c->dt); // c->move_vec.Vw = -PID_Calc(&c->pid.chassis_follow_gimbal_pid, c->motorfeedback.gimbal_yaw_encoder-2.07694483f ,c->motorfeedback.gimbal_yaw_encoder, 0.0f, c->dt);
break; break;

16
User/module/cmd.c
View File

@ -142,6 +142,22 @@ int8_t CMD_CtrlSet(Chassis_CMD_t *c_cmd, const CMD_RC_t *rc, Gimbal_CMD_t *g_cmd
break; break;
} }
switch (rc->ET16s.key_B)
{
case CMD_SW_UP:
g_cmd->ctrl_mode = GIMBAL_MODE_REMOTE;
break;
case CMD_SW_MID:
g_cmd->ctrl_mode = GIMBAL_MODE_AI;
break;
case CMD_SW_DOWN:
g_cmd->ctrl_mode = GIMBAL_MODE_AI;
break;
case CMD_SW_ERR:
g_cmd->ctrl_mode = GIMBAL_MODE_REMOTE;
break;
}
switch (rc->ET16s.key_E) switch (rc->ET16s.key_E)
{ {
case CMD_SW_UP: case CMD_SW_UP:

9
User/module/cmd.h
View File

@ -102,11 +102,20 @@ typedef struct
GIMBAL_MODE_ABSOLUTE, /* 绝对坐标系控制，控制在空间内的绝对姿态 */ GIMBAL_MODE_ABSOLUTE, /* 绝对坐标系控制，控制在空间内的绝对姿态 */
GIMBAL_MODE_RELATIVE, /* 相对坐标系控制，控制相对于底盘的姿态 */ GIMBAL_MODE_RELATIVE, /* 相对坐标系控制，控制相对于底盘的姿态 */
} Gimbal_Mode_t; } Gimbal_Mode_t;
typedef enum
{
GIMBAL_MODE_REMOTE,
GIMBAL_MODE_AI,
}GIMBAL_Ctrl_mode_t;
typedef struct { typedef struct {
Gimbal_Mode_t mode; Gimbal_Mode_t mode;
float delta_yaw_4310; float delta_yaw_4310;
float delta_pitch_4310; float delta_pitch_4310;
float delta_yaw_6020; float delta_yaw_6020;
GIMBAL_Ctrl_mode_t ctrl_mode;
float set_yaw;
float set_pitch;
} Gimbal_CMD_t; } Gimbal_CMD_t;
typedef enum { typedef enum {

2
User/module/config.c
View File

@ -198,7 +198,7 @@ static const Config_Param_t config = {
.mech_zero = { .mech_zero = {
.yaw_6020 = 1.31f, .yaw_6020 = 1.31f,
.yaw_4310 = 2.06f, /*大yaw零点*/ .yaw_4310 = 2.06f, /*大yaw零点*/
.pitch_4310 = 1.0f, .pitch_4310 = 0.93f,
}, },
.travel = { .travel = {
.yaw_6020 = 1.4f, .yaw_6020 = 1.4f,

35
User/module/gimbal.c
View File

@ -201,6 +201,25 @@ int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal, const Gimbal_IMU_t *imu){
return GIMBAL_OK; return GIMBAL_OK;
} }
// int8_t Gimbal_Control_mode(Gimbal_t *g,Gimbal_CMD_t *g_cmd){
// if (g == NULL || g_cmd == NULL) {
// return -1;
// }
// switch(g_cmd->ctrl_mode){
// case GIMBAL_MODE_REMOTE:
// g->setpoint.eulr.yaw=g_cmd->delta_yaw_6020;
// g->setpoint.eulr.pit=g_cmd->delta_pitch_4310;
// break;
// case GIMBAL_MODE_AI:
// g->setpoint.eulr.yaw=g_cmd->set_yaw;
// g->setpoint.eulr.pit=g_cmd->set_pitch;
// break;
// default:
// break;
// }
// return GIMBAL_OK;
// };
/** /**
* \brief 运行云台控制逻辑 * \brief 运行云台控制逻辑
* *
@ -294,6 +313,16 @@ switch (g->mode) {
case GIMBAL_MODE_ABSOLUTE: case GIMBAL_MODE_ABSOLUTE:
/*绝对坐标 大地坐标系下云台控制*/ /*绝对坐标 大地坐标系下云台控制*/
/*6020小YAW控制*/ /*6020小YAW控制*/
// Gimbal_Control_mode(g, g_cmd);
if(g_cmd->ctrl_mode == GIMBAL_MODE_REMOTE){
g->setpoint.eulr.yaw = g->setpoint.eulr.yaw;
g->setpoint.eulr.pit = g->setpoint.eulr.pit;
}
else if (g_cmd->ctrl_mode == GIMBAL_MODE_AI) {
g->setpoint.eulr.yaw = g_cmd->set_yaw;
g->setpoint.eulr.pit = g_cmd->set_pitch;
}
yaw_6020_omega_set_point =PID_Calc(&g->pid.yaw_6020_angle,g->setpoint.eulr.yaw, yaw_6020_omega_set_point =PID_Calc(&g->pid.yaw_6020_angle,g->setpoint.eulr.yaw,
g->feedback.imu.eulr.yaw,0.0f,g->dt); g->feedback.imu.eulr.yaw,0.0f,g->dt);
g->out.yaw_6020 = PID_Calc(&g->pid.yaw_6020_omega,yaw_6020_omega_set_point, g->out.yaw_6020 = PID_Calc(&g->pid.yaw_6020_omega,yaw_6020_omega_set_point,
@ -330,6 +359,9 @@ switch (g->mode) {
return 0; return 0;
} }
/** /**
* \brief 云台输出 * \brief 云台输出
* *
@ -338,11 +370,12 @@ return 0;
* void类型的，可以改成int * void类型的，可以改成int
* 4310为力矩输出控制，传统的电机控制方法，两者之积不要大于12.5 * 4310为力矩输出控制，传统的电机控制方法，两者之积不要大于12.5
*/ */
MOTOR_MIT_Output_t output_yaw_4310 = {0};
void Gimbal_Output(Gimbal_t *g) void Gimbal_Output(Gimbal_t *g)
{ {
MOTOR_RM_SetOutput(&g->param->yaw_6020_motor, g->out.yaw_6020); MOTOR_RM_SetOutput(&g->param->yaw_6020_motor, g->out.yaw_6020);
MOTOR_MIT_Output_t output_yaw_4310 = {0};
output_yaw_4310.torque = g->out.yaw_4310 * 1.0f; output_yaw_4310.torque = g->out.yaw_4310 * 1.0f;
output_yaw_4310.kd = 0.1f; output_yaw_4310.kd = 0.1f;
// output_yaw_4310.kd=0.1f; // output_yaw_4310.kd=0.1f;

2
User/module/shoot.c
View File

@ -173,7 +173,7 @@ int8_t Shoot_CaluTargetRPM(Shoot_t *s, float target_speed)
switch(s->param->basic.projectileType) switch(s->param->basic.projectileType)
{ {
case SHOOT_PROJECTILE_17MM: case SHOOT_PROJECTILE_17MM:
s->target_variable.fric_rpm=5000.0f; s->target_variable.fric_rpm=6500.0f;
break; break;
case SHOOT_PROJECTILE_42MM: case SHOOT_PROJECTILE_42MM:
s->target_variable.fric_rpm=4000.0f; s->target_variable.fric_rpm=4000.0f;

2
User/task/Task8.c
View File

@ -1,4 +1,4 @@
/* /*
Task8 Task Task8 Task
*/ */

40
User/task/gimbal.c
View File

@ -8,6 +8,7 @@
/* USER INCLUDE BEGIN */ /* USER INCLUDE BEGIN */
#include "module/gimbal.h" #include "module/gimbal.h"
#include "module/config.h" #include "module/config.h"
#include "device/ai.h"
/* USER INCLUDE END */ /* USER INCLUDE END */
/* Private typedef ---------------------------------------------------------- */ /* Private typedef ---------------------------------------------------------- */
@ -15,9 +16,13 @@
/* Private macro ------------------------------------------------------------ */ /* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */ /* Private variables -------------------------------------------------------- */
/* USER STRUCT BEGIN */ /* USER STRUCT BEGIN */
Gimbal_CMD_t cmd_gimbal; Gimbal_CMD_t cmd_gimbal; //遥控器
Gimbal_IMU_t gimbal_imu; Gimbal_IMU_t gimbal_imu;
Gimbal_t gimbal; Gimbal_t gimbal;
AI_cmd_t ai_gimbal_cmd;
Gimbal_CMD_t final_gimbal_cmd; //最终命令
/* USER STRUCT END */ /* USER STRUCT END */
/* Private function --------------------------------------------------------- */ /* Private function --------------------------------------------------------- */
@ -39,10 +44,39 @@ void Task_gimbal(void *argument) {
while (1) { while (1) {
tick += delay_tick; /* 计算下一个唤醒时刻 */ tick += delay_tick; /* 计算下一个唤醒时刻 */
/* USER CODE BEGIN */ /* USER CODE BEGIN */
osMessageQueueGet(task_runtime.msgq.gimbal.imu, &gimbal_imu,NULL, 0); osMessageQueueGet(task_runtime.msgq.gimbal.imu, &gimbal_imu,NULL, 0);
Gimbal_UpdateIMU(&gimbal, &gimbal_imu); Gimbal_UpdateIMU(&gimbal, &gimbal_imu);
osMessageQueueGet(task_runtime.msgq.gimbal.ai.g_cmd,&ai_gimbal_cmd,NULL, 0);
/* ai指令 */
// if(osMessageQueueGet(task_runtime.msgq.gimbal.ai.g_cmd, &ai_gimbal_cmd, NULL, 0)==osOK){
// if(ai_gimbal_cmd.mode==0){
// final_gimbal_cmd.set_pitch =gimbal_imu.eulr.rol;
// final_gimbal_cmd.set_yaw=gimbal_imu.eulr.yaw;
// }
// if(ai_gimbal_cmd.mode==2){
// final_gimbal_cmd.set_pitch=ai_gimbal_cmd.gimbal_t.setpoint.pit;
// final_gimbal_cmd.set_yaw=ai_gimbal_cmd.gimbal_t.setpoint.yaw;
// }
// }
osMessageQueueGet(task_runtime.msgq.gimbal.cmd, &cmd_gimbal, NULL, 0); osMessageQueueGet(task_runtime.msgq.gimbal.cmd, &cmd_gimbal, NULL, 0);
if(ai_gimbal_cmd.mode==0){
cmd_gimbal.set_pitch =gimbal_imu.eulr.pit;
cmd_gimbal.set_yaw=gimbal_imu.eulr.yaw;
}
if(ai_gimbal_cmd.mode==1)
{
cmd_gimbal.set_pitch=ai_gimbal_cmd.gimbal_t.setpoint.pit;
cmd_gimbal.set_yaw=ai_gimbal_cmd.gimbal_t.setpoint.yaw;
}
if(ai_gimbal_cmd.mode==2){
cmd_gimbal.set_pitch=ai_gimbal_cmd.gimbal_t.setpoint.pit;
cmd_gimbal.set_yaw=ai_gimbal_cmd.gimbal_t.setpoint.yaw;
}
Gimbal_UpdateFeedback(&gimbal); Gimbal_UpdateFeedback(&gimbal);
osMessageQueueReset(task_runtime.msgq.gimbal.yaw6020); osMessageQueueReset(task_runtime.msgq.gimbal.yaw6020);