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 void AutoControlSet()
{ {
static int8_t gimbal_scan_flag = 1; // static int8_t gimbal_scan_flag = 1;
static int8_t scan_dir = 1; // static int8_t scan_dir = 1;
trajectory_cal.v0 = 30; //弹速30 // trajectory_cal.v0 = 30; //弹速30
//
//小云台扫描 // //小云台扫描
if(gimbal_scan_flag == 1) // if(gimbal_scan_flag == 1)
{ // {
if (scan_dir == 1) gimbal_cmd_send.yaw += 0.001f; // if (scan_dir == 1) gimbal_cmd_send.yaw += 0.001f;
else 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_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(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 // 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) { // && vision_recv_data->vx == 0 && vision_recv_data->vy == 0 && vision_recv_data->vz == 0) {
aim_select.suggest_fire = 0; // aim_select.suggest_fire = 0;
//未发现目标 // //未发现目标
no_find_cnt++; // no_find_cnt++;
if (no_find_cnt >= 2000) { // if (no_find_cnt >= 2000) {
gimbal_scan_flag = 1; // gimbal_scan_flag = 1;
auto_aim_flag = 0; // auto_aim_flag = 0;
} // }
else // else
auto_aim_flag = 1; // auto_aim_flag = 1;
} else { // } else {
//弹道解算 // //弹道解算
no_find_cnt = 0; // no_find_cnt = 0;
auto_aim_flag = 1; // auto_aim_flag = 1;
//
auto_aim(&aim_select, &trajectory_cal, vision_recv_data); // auto_aim(&aim_select, &trajectory_cal, vision_recv_data);
gimbal_cmd_send.yaw = trajectory_cal.cmd_yaw * 180 / PI; // gimbal_cmd_send.yaw = trajectory_cal.cmd_yaw * 180 / PI;
gimbal_cmd_send.pitch = trajectory_cal.cmd_pitch * 180 / PI; // gimbal_cmd_send.pitch = trajectory_cal.cmd_pitch * 180 / PI;
float yaw_err = fabsf(trajectory_cal.cmd_yaw - gimbal_cmd_send.yaw); // float yaw_err = fabsf(trajectory_cal.cmd_yaw - gimbal_cmd_send.yaw);
if (yaw_err <= 0.008) //3度 // if (yaw_err <= 0.008) //3度
aim_select.suggest_fire = 1; // aim_select.suggest_fire = 1;
else // else
aim_select.suggest_fire = 0; // aim_select.suggest_fire = 0;
} // }
} }
/** /**

2
modules/master_machine/master_process.c
View File

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