diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index 17f0176..16792ea 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -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; +// } } /** diff --git a/modules/master_machine/master_process.c b/modules/master_machine/master_process.c index 410bae8..ce59852 100644 --- a/modules/master_machine/master_process.c +++ b/modules/master_machine/master_process.c @@ -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));