Hshine
/
sentry_left
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

新增导航通信测试

This commit is contained in:
宋家齐 2024-03-16 21:20:28 +08:00
parent ced0ef12fe
commit 280746a42d
2 changed files with 39 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

76
application/cmd/robot_cmd.c
View File

@ -220,44 +220,44 @@ static void RemoteControlSet()
*/
static void AutoControlSet()
{
static int8_t gimbal_scan_flag = 1;
static int8_t scan_dir = 1;
trajectory_cal.v0 = 30; //弹速30
//小云台扫描
if(gimbal_scan_flag == 1)
{
if (scan_dir == 1) gimbal_cmd_send.yaw += 0.001f;
else gimbal_cmd_send.yaw -= 0.001f;
if(gimbal_fetch_data.mini_yaw_encode_angle <= YAW_MIN_ENCODE_ANGLE + 0.1f) scan_dir = 1;
if(gimbal_fetch_data.mini_yaw_encode_angle >= YAW_MAX_ENCODE_ANGLE - 0.1f) scan_dir = -1;
}
if (vision_recv_data->x == 0 && vision_recv_data->y == 0 && vision_recv_data->z == 0
&& vision_recv_data->vx == 0 && vision_recv_data->vy == 0 && vision_recv_data->vz == 0) {
aim_select.suggest_fire = 0;
//未发现目标
no_find_cnt++;
if (no_find_cnt >= 2000) {
gimbal_scan_flag = 1;
auto_aim_flag = 0;
}
else
auto_aim_flag = 1;
} else {
//弹道解算
no_find_cnt = 0;
auto_aim_flag = 1;
auto_aim(&aim_select, &trajectory_cal, vision_recv_data);
gimbal_cmd_send.yaw = trajectory_cal.cmd_yaw * 180 / PI;
gimbal_cmd_send.pitch = trajectory_cal.cmd_pitch * 180 / PI;
float yaw_err = fabsf(trajectory_cal.cmd_yaw - gimbal_cmd_send.yaw);
if (yaw_err <= 0.008) //3度
aim_select.suggest_fire = 1;
else
aim_select.suggest_fire = 0;
}
// static int8_t gimbal_scan_flag = 1;
// static int8_t scan_dir = 1;
// trajectory_cal.v0 = 30; //弹速30
//
// //小云台扫描
// if(gimbal_scan_flag == 1)
// {
// if (scan_dir == 1) gimbal_cmd_send.yaw += 0.001f;
// else gimbal_cmd_send.yaw -= 0.001f;
// if(gimbal_fetch_data.mini_yaw_encode_angle <= YAW_MIN_ENCODE_ANGLE + 0.1f) scan_dir = 1;
// if(gimbal_fetch_data.mini_yaw_encode_angle >= YAW_MAX_ENCODE_ANGLE - 0.1f) scan_dir = -1;
// }
//
// if (vision_recv_data->x == 0 && vision_recv_data->y == 0 && vision_recv_data->z == 0
// && vision_recv_data->vx == 0 && vision_recv_data->vy == 0 && vision_recv_data->vz == 0) {
// aim_select.suggest_fire = 0;
// //未发现目标
// no_find_cnt++;
// if (no_find_cnt >= 2000) {
// gimbal_scan_flag = 1;
// auto_aim_flag = 0;
// }
// else
// auto_aim_flag = 1;
// } else {
// //弹道解算
// no_find_cnt = 0;
// auto_aim_flag = 1;
//
// auto_aim(&aim_select, &trajectory_cal, vision_recv_data);
// gimbal_cmd_send.yaw = trajectory_cal.cmd_yaw * 180 / PI;
// gimbal_cmd_send.pitch = trajectory_cal.cmd_pitch * 180 / PI;
// float yaw_err = fabsf(trajectory_cal.cmd_yaw - gimbal_cmd_send.yaw);
// if (yaw_err <= 0.008) //3度
// aim_select.suggest_fire = 1;
// else
// aim_select.suggest_fire = 0;
// }
}
/**

2
modules/master_machine/master_process.c
View File

@ -181,7 +181,7 @@ void VisionSend()
send_data.header = 0x5A;
//VisionSetFlag(1);
VisionSetAim(recv_data.x,recv_data.y,recv_data.z);
send_data.checksum = crc_16(&send_data.header,sizeof(send_data)-2);
memcpy(send_buffer,&send_data,sizeof(send_data));