#include "gd32f4xx.h" #include "ptz_header_file.h" #include "math.h" #include "ptz_default_value.h" #include "ptz_type_select.h" #include "l6235d.h" #include "service_presetbitscan.h" #ifdef PTZ_BLDC_MOTOR static BSP_OS_SEM ptz_hori_stop_mutex;//共享资源锁 static BSP_OS_SEM ptz_vert_stop_mutex; static char ptz_hori_stop_count;//水平停止计数，防止多次停止多次延时 static char ptz_vert_stop_count;//垂直停止计数，防止多次停止多次延时 void ptz_sem_post_stop_mutex()//释放云台启停共享资源锁 { BSP_OS_SemPost(&ptz_hori_stop_mutex); BSP_OS_SemPost(&ptz_vert_stop_mutex); } void ptz_hori_start(char direction, float speed) { BSP_OS_SemWait(&ptz_hori_stop_mutex, 0u); ptz_hori_pid_clear_zero(); //设定转动速度 g_ptz.hori_speed_set = speed; //设定转动方向 g_ptz.hori_direction_last = g_ptz.hori_direction_set; g_ptz.hori_direction_set = direction; g_ptz.hori_direction_actual = g_ptz.hori_direction_set; //启动电机 g_ptz.hori_start_stop_set = PTZ_HORI_START; g_ptz.hori_start_stop_actual = g_ptz.hori_start_stop_set; #ifdef PTZ_MOTOR_CONTROL_L6235D l6235d_hori_set_direction(direction); l6235d_hori_start_stop(PTZ_HORI_START); #endif ptz_hori_stop_count = 0; #ifdef PTZ_ELECTRIC_STABLE_L6235D g_ptz.hori_electric_stable.stable_stop_switch = 0; #endif BSP_OS_SemPost(&ptz_hori_stop_mutex); } void ptz_hori_stop(unsigned short int time) { BSP_OS_SemWait(&ptz_hori_stop_mutex, 0u); //停止电机 g_ptz.hori_start_stop_set = PTZ_HORI_STOP; g_ptz.hori_start_stop_actual = g_ptz.hori_start_stop_set; #ifdef PTZ_MOTOR_CONTROL_L6235D l6235d_hori_start_stop(PTZ_HORI_STOP); #endif //PID清0 ptz_hori_pid_clear_zero(); //设定转动速度 g_ptz.hori_speed_set = 0; g_ptz.hori_speed_actual = 0; if(ptz_hori_stop_count <= 0) { OSTimeDlyHMSM(0u, 0u, 0u, time); ptz_hori_stop_count = 0; } ptz_hori_stop_count ++; //电子稳定 #ifdef PTZ_ELECTRIC_STABLE_L6235D ptz_hori_electric_stable_init(); #endif BSP_OS_SemPost(&ptz_hori_stop_mutex); } void ptz_vert_start(char direction, float speed) { BSP_OS_SemWait(&ptz_vert_stop_mutex, 0u); ptz_vert_pid_clear_zero(); //设定转动速度 g_ptz.vert_speed_set = speed; //设定转动方向 g_ptz.vert_direction_last = g_ptz.vert_direction_set; g_ptz.vert_direction_set = direction; g_ptz.vert_direction_actual = g_ptz.vert_direction_set; //启动电机 g_ptz.vert_start_stop_set = PTZ_VERT_START; g_ptz.vert_start_stop_actual = g_ptz.vert_start_stop_set; #ifdef PTZ_MOTOR_CONTROL_L6235D l6235d_vert_set_direction(direction); l6235d_vert_start_stop(PTZ_VERT_START); #endif ptz_vert_stop_count = 0; #ifdef PTZ_ELECTRIC_STABLE_L6235D g_ptz.vert_electric_stable.stable_stop_switch = 0; #endif BSP_OS_SemPost(&ptz_vert_stop_mutex); } void ptz_vert_stop(unsigned short int time) { BSP_OS_SemWait(&ptz_vert_stop_mutex, 0u); if(g_ptz.vert_start_stop_set == PTZ_VERT_START) { //停止电机 g_ptz.vert_start_stop_set = PTZ_VERT_STOP; g_ptz.vert_start_stop_actual = g_ptz.vert_start_stop_set; #ifdef PTZ_MOTOR_CONTROL_L6235D l6235d_vert_start_stop(PTZ_VERT_STOP); #endif //PID清0 ptz_vert_pid_clear_zero(); //设定转动速度 g_ptz.vert_speed_set = 0; g_ptz.vert_speed_actual = 0; if(ptz_vert_stop_count <= 0) { OSTimeDlyHMSM(0u, 0u, 0u, time); ptz_vert_stop_count = 0; } ptz_vert_stop_count ++; //云台电子稳定 #ifdef PTZ_ELECTRIC_STABLE_L6235D ptz_vert_electric_stable_init(); #endif } BSP_OS_SemPost(&ptz_vert_stop_mutex); } /// 云台水平转动监控 static char ptz_hori_rotate_monitor_task() { static float BNP_D;//刹车最近点与当前位置的距离 static float BFP_D;//刹车最远点与当前位置的距离 static unsigned int k; switch(g_ptz.hori_rotate_monitor_switch) { case 0://关闭 break; case PTZ_HORI_RIGHT_KEEP://向右转动，不刹车 //关闭转动监控 g_ptz.hori_rotate_monitor_switch = 0; //首先判断云台是否在转动 if(g_ptz.hori_start_stop_set == PTZ_HORI_START) { //判断转动方向是否相同 //如果转向相同，则直接修改设置的转速 if(g_ptz.hori_direction_set == PTZ_HORI_DIR_RIGHT) { g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed; } else//如果转向不同 { //则先刹车 ptz_hori_stop(PTZ_HORI_STOP_TIME); //再启动 g_ptz.hori_direction_set = PTZ_HORI_DIR_RIGHT; g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed; ptz_hori_start(PTZ_HORI_DIR_RIGHT, g_ptz.hori_speed_set); } } else//没有转动 { g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed; ptz_hori_start(PTZ_HORI_DIR_RIGHT, g_ptz.hori_speed_set); } break; case PTZ_HORI_LEFT_KEEP://向左转动，不刹车 //关闭转动监控 g_ptz.hori_rotate_monitor_switch = 0; //首先判断云台是否在转动 if(g_ptz.hori_start_stop_set == PTZ_HORI_START) { //判断转动方向是否相同 //如果转向相同，则直接修改设置的转速 if(g_ptz.hori_direction_set == PTZ_HORI_DIR_LEFT) { g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed; } else//如果转向不同 { //则先刹车 ptz_hori_stop(PTZ_HORI_STOP_TIME); //再启动 g_ptz.hori_direction_set = PTZ_HORI_DIR_LEFT; g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed; ptz_hori_start(PTZ_HORI_DIR_LEFT, g_ptz.hori_speed_set); } } else//没有转动 { g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed; ptz_hori_start(PTZ_HORI_DIR_LEFT, g_ptz.hori_speed_set); } break; case PTZ_HORI_GO_TO_ANGLE_A://角度定位 case PTZ_HORI_MIN_DISTANCE: case PTZ_HORI_MAX_DISTANCE: case PTZ_HORI_RIGHT_ANGLE: case PTZ_HORI_LEFT_ANGLE: //首先判断是否需要转动,如果转动距离为0，即同一个位置则不需要转动 if(g_ptz.hori_rotate_plan.rotate_switch == 1) { g_ptz.hori_rotate_monitor_switch = PTZ_HORI_GO_TO_ANGLE_B; //如果不是处于同一个位置 //判断是否转动 //处于转动 if(g_ptz.hori_start_stop_set == PTZ_HORI_START) { //判断转向是否相同 //如果转向相同 if(g_ptz.hori_direction_set == g_ptz.hori_rotate_plan.direction) { g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed; } else//转向不同，则直接修改设置的转速 { //则先刹车 ptz_hori_stop(PTZ_HORI_STOP_TIME); //再启动 g_ptz.hori_direction_set = g_ptz.hori_rotate_plan.direction; g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed; ptz_hori_start(g_ptz.hori_direction_set, g_ptz.hori_speed_set); } } else//没有转动 { g_ptz.hori_direction_set = g_ptz.hori_rotate_plan.direction; g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed; ptz_hori_start(g_ptz.hori_direction_set, g_ptz.hori_speed_set); } //如果离最近刹车距离小于一定程度，则以最小转速转动 if(g_ptz.hori_direction_set == PTZ_HORI_DIR_LEFT) { if(g_ptz.hori_angle_actual >= g_ptz.hori_rotate_plan.stop_near_angle) { if(fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle) <= PTZ_HORI_MIN_SPEED_ANGLE) { g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED; } } else { if((fabs(360.0 - g_ptz.hori_rotate_plan.stop_near_angle) + g_ptz.hori_angle_actual) <= PTZ_HORI_MIN_SPEED_ANGLE) { g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED; } } } if(g_ptz.hori_direction_set == PTZ_HORI_DIR_RIGHT) { if(g_ptz.hori_angle_actual <= g_ptz.hori_rotate_plan.stop_near_angle) { if(fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle) <= PTZ_HORI_MIN_SPEED_ANGLE) { g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED; } } else { if((fabs(360.0 - g_ptz.hori_angle_actual) + g_ptz.hori_rotate_plan.stop_near_angle) <= PTZ_HORI_MIN_SPEED_ANGLE) { g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED; } } } } else//g_ptz.hori_rotate_plan.rotate_switch == 0; { g_ptz.hori_rotate_monitor_switch = 0; //不需要转动则刹车停止，关闭监控 g_ptz.hori_arrive_flag = 0;//转动到指定位置 ptz_location_return_return(LOCATION_HORI);//定位回传 ptz_preset_bit_location_return_return();//预置位到位回传 if(g_ptz.hori_start_stop_set == PTZ_HORI_START) { ptz_hori_stop(PTZ_HORI_STOP_TIME); } } break; case PTZ_HORI_RIGHT_CYCLE: case PTZ_HORI_LEFT_CYCLE: if(g_ptz.hori_start_stop_set == PTZ_HORI_START) { //判断转向是否相同 //如果转向相同 if(g_ptz.hori_direction_set == g_ptz.hori_rotate_plan.direction) { g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed; } else//转向不同，则直接修改设置的转速 { //则先刹车 ptz_hori_stop(PTZ_HORI_STOP_TIME); //再启动 g_ptz.hori_direction_set = g_ptz.hori_rotate_plan.direction; g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed; ptz_hori_start(g_ptz.hori_direction_set, g_ptz.hori_speed_set); } } else//没有转动 { g_ptz.hori_direction_set = g_ptz.hori_rotate_plan.direction; g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed; ptz_hori_start(g_ptz.hori_direction_set, g_ptz.hori_speed_set); } g_ptz.hori_rotate_monitor_switch = PTZ_HORI_CYCLE; break; case PTZ_HORI_CYCLE: BNP_D = fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle); if(BNP_D > 360.0 / 2.0) { BNP_D = 360.0 - BNP_D; } BFP_D = fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_far_angle); if(BFP_D > 360.0 / 2.0) { BFP_D = 360.0 - BFP_D; } /*定位*/ if(BNP_D + BFP_D > (g_ptz.hori_rotate_plan.stop_angle_range + 2.0)) { g_ptz.hori_rotate_monitor_switch = PTZ_HORI_GO_TO_ANGLE_B; } break; case PTZ_HORI_GO_TO_ANGLE_B: BNP_D = fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle); if(BNP_D > 360.0 / 2.0) { BNP_D = 360.0 - BNP_D; } BFP_D = fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_far_angle); if(BFP_D > 360.0 / 2.0) { BFP_D = 360.0 - BFP_D; } /*定位*/ if(BNP_D + BFP_D <= (g_ptz.hori_rotate_plan.stop_angle_range + 0.01)) { if(g_ptz.hori_repeat_locate_switch == 0)//重复定位关闭 { g_ptz.hori_arrive_flag = 0;//转到指定位置 ptz_location_return_return(LOCATION_HORI);//定位回传 ptz_preset_bit_location_return_return();//预置位到位回传 } g_ptz.hori_rotate_monitor_switch = 0; if(g_ptz.hori_repeat_locate_switch == 1)//重复定位打开 { g_ptz.hori_rotate_monitor_switch = PTZ_HORI_REPEAT_LOCATE_A; } ptz_hori_stop(PTZ_HORI_STOP_TIME); } //如果离最近刹车距离小于一定程度，则以最小转速转动 if(g_ptz.hori_direction_set == PTZ_HORI_DIR_LEFT) { if(g_ptz.hori_angle_actual >= g_ptz.hori_rotate_plan.stop_near_angle) { if(fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle) <= PTZ_HORI_MIN_SPEED_ANGLE) { g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED; } } else { if((fabs(360.0 - g_ptz.hori_rotate_plan.stop_near_angle) + g_ptz.hori_angle_actual) <= PTZ_HORI_MIN_SPEED_ANGLE) { g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED; } } } if(g_ptz.hori_direction_set == PTZ_HORI_DIR_RIGHT) { if(g_ptz.hori_angle_actual <= g_ptz.hori_rotate_plan.stop_near_angle) { if(fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle) <= PTZ_HORI_MIN_SPEED_ANGLE) { g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED; } } else { if((fabs(360.0 - g_ptz.hori_angle_actual) + g_ptz.hori_rotate_plan.stop_near_angle) <= PTZ_HORI_MIN_SPEED_ANGLE) { g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED; } } } break; case PTZ_HORI_BRAKE://停止转动 g_ptz.hori_rotate_monitor_switch = 0; ptz_hori_stop(PTZ_HORI_STOP_TIME); break; case PTZ_HORI_DEC_BRAKE_A://减速刹车 g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED; g_ptz.hori_rotate_monitor_switch = PTZ_HORI_DEC_BRAKE_B; k = 0; break; case PTZ_HORI_DEC_BRAKE_B: k++;//K增加1，时间增加1ms if(g_ptz.hori_speed_actual <= PTZ_HORI_MIN_SPEED * 1.5 || k >= 1400) { k = 0; g_ptz.hori_rotate_monitor_switch = 0; ptz_hori_stop(PTZ_HORI_STOP_TIME); } break; //重复定位路径规划 case PTZ_HORI_REPEAT_LOCATE_A: ptz_hori_rotate_plan(PTZ_HORI_MIN_DISTANCE); g_ptz.hori_rotate_monitor_switch = PTZ_HORI_REPEAT_LOCATE_B; break; //重复定位启动控制 case PTZ_HORI_REPEAT_LOCATE_B: //首先判断是否需要转动,如果转动距离为0，即同一个位置则不需要转动 if(g_ptz.hori_rotate_plan.rotate_switch == 1) { g_ptz.hori_rotate_monitor_switch = PTZ_HORI_REPEAT_LOCATE_C; //如果不是处于同一个位置 //判断是否转动 //处于转动 if(g_ptz.hori_start_stop_set == PTZ_HORI_START) { //判断转向是否相同 //如果转向相同 if(g_ptz.hori_direction_set == g_ptz.hori_rotate_plan.direction) { g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed; } else//转向不同，则直接修改设置的转速 { //则先刹车 ptz_hori_stop(PTZ_HORI_STOP_TIME); //再启动 g_ptz.hori_direction_set = g_ptz.hori_rotate_plan.direction; g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed; ptz_hori_start(g_ptz.hori_direction_set, g_ptz.hori_speed_set); } } else//没有转动 { g_ptz.hori_direction_set = g_ptz.hori_rotate_plan.direction; g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed; ptz_hori_start(g_ptz.hori_direction_set, g_ptz.hori_speed_set); } //如果离最近刹车距离小于一定程度，则以最小转速转动 if(g_ptz.hori_direction_set == PTZ_HORI_DIR_LEFT) { if(g_ptz.hori_angle_actual >= g_ptz.hori_rotate_plan.stop_near_angle) { if(fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle) <= PTZ_HORI_MIN_SPEED_ANGLE) { g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED; } } else { if((fabs(360.0 - g_ptz.hori_rotate_plan.stop_near_angle) + g_ptz.hori_angle_actual) <= PTZ_HORI_MIN_SPEED_ANGLE) { g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED; } } } if(g_ptz.hori_direction_set == PTZ_HORI_DIR_RIGHT) { if(g_ptz.hori_angle_actual <= g_ptz.hori_rotate_plan.stop_near_angle) { if(fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle) <= PTZ_HORI_MIN_SPEED_ANGLE) { g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED; } } else { if((fabs(360.0 - g_ptz.hori_angle_actual) + g_ptz.hori_rotate_plan.stop_near_angle) <= PTZ_HORI_MIN_SPEED_ANGLE) { g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED; } } } } else//g_ptz.hori_rotate_plan.rotate_switch == 0; { g_ptz.hori_rotate_monitor_switch = 0; //不需要转动则刹车停止，关闭监控 g_ptz.hori_arrive_flag = 0;//转到指定预位置 ptz_location_return_return(LOCATION_HORI);//定位回传 ptz_preset_bit_location_return_return();//预置位到位回传 if(g_ptz.hori_start_stop_set == PTZ_HORI_START) { ptz_hori_stop(PTZ_HORI_STOP_TIME); } } break; //重复定位判断是否达到定位位置 case PTZ_HORI_REPEAT_LOCATE_C: BNP_D = fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle); if(BNP_D > 360.0 / 2.0) { BNP_D = 360.0 - BNP_D; } BFP_D = fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_far_angle); if(BFP_D > 360.0 / 2.0) { BFP_D = 360.0 - BFP_D; } /*定位*/ if(BNP_D + BFP_D <= (g_ptz.hori_rotate_plan.stop_angle_range + 0.01)) { g_ptz.hori_arrive_flag = 0;//转到指定位置 ptz_location_return_return(LOCATION_HORI);//定位回传 ptz_preset_bit_location_return_return();//预置位到位回传 g_ptz.hori_rotate_monitor_switch = 0; ptz_hori_stop(PTZ_HORI_STOP_TIME); } //如果离最近刹车距离小于一定程度，则以最小转速转动 if(g_ptz.hori_direction_set == PTZ_HORI_DIR_LEFT) { if(g_ptz.hori_angle_actual >= g_ptz.hori_rotate_plan.stop_near_angle) { if(fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle) <= PTZ_HORI_MIN_SPEED_ANGLE) { g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED; } } else { if((fabs(360.0 - g_ptz.hori_rotate_plan.stop_near_angle) + g_ptz.hori_angle_actual) <= PTZ_HORI_MIN_SPEED_ANGLE) { g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED; } } } if(g_ptz.hori_direction_set == PTZ_HORI_DIR_RIGHT) { if(g_ptz.hori_angle_actual <= g_ptz.hori_rotate_plan.stop_near_angle) { if(fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle) <= PTZ_HORI_MIN_SPEED_ANGLE) { g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED; } } else { if((fabs(360.0 - g_ptz.hori_angle_actual) + g_ptz.hori_rotate_plan.stop_near_angle) <= PTZ_HORI_MIN_SPEED_ANGLE) { g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED; } } } break; case PTZ_HORI_RIGHT_ANGLE_INC: //首先判断是否需要转动,如果转动距离为0，即同一个位置则不需要转动 if(g_ptz.hori_rotate_plan.rotate_switch == 1) { g_ptz.hori_rotate_monitor_switch = PTZ_HORI_GO_TO_ANGLE_B; //如果不是处于同一个位置 //判断是否转动 //处于转动 if(g_ptz.hori_start_stop_set == PTZ_HORI_START) { //判断转向是否相同 //如果转向相同 if(g_ptz.hori_direction_set == g_ptz.hori_rotate_plan.direction) { g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed; } else//转向不同，则直接修改设置的转速 { //则先刹车 ptz_hori_stop(PTZ_HORI_STOP_TIME); //再启动 g_ptz.hori_direction_set = g_ptz.hori_rotate_plan.direction; g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed; ptz_hori_start(g_ptz.hori_direction_set, g_ptz.hori_speed_set); } } else//没有转动 { g_ptz.hori_direction_set = g_ptz.hori_rotate_plan.direction; g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed; ptz_hori_start(g_ptz.hori_direction_set, g_ptz.hori_speed_set); } //如果离最近刹车距离小于一定程度，则以最小转速转动 if(g_ptz.hori_direction_set == PTZ_HORI_DIR_LEFT) { if(g_ptz.hori_angle_actual >= g_ptz.hori_rotate_plan.stop_near_angle) { if(fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle) <= PTZ_HORI_MIN_SPEED_ANGLE) { g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED; } } else { if((fabs(360.0 - g_ptz.hori_rotate_plan.stop_near_angle) + g_ptz.hori_angle_actual) <= PTZ_HORI_MIN_SPEED_ANGLE) { g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED; } } } if(g_ptz.hori_direction_set == PTZ_HORI_DIR_RIGHT) { if(g_ptz.hori_angle_actual <= g_ptz.hori_rotate_plan.stop_near_angle) { if(fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle) <= PTZ_HORI_MIN_SPEED_ANGLE) { g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED; } } else { if((fabs(360.0 - g_ptz.hori_angle_actual) + g_ptz.hori_rotate_plan.stop_near_angle) <= PTZ_HORI_MIN_SPEED_ANGLE) { g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED; } } } } else//g_ptz.hori_rotate_plan.rotate_switch == 0; { g_ptz.hori_rotate_monitor_switch = 0; //不需要转动则刹车停止，关闭监控 g_ptz.hori_arrive_flag = 0;//转动到指定位置 ptz_location_return_return(LOCATION_HORI);//定位回传 ptz_preset_bit_location_return_return();//预置位到位回传 if(g_ptz.hori_start_stop_set == PTZ_HORI_START) { ptz_hori_stop(PTZ_HORI_STOP_TIME); } } break; case PTZ_HORI_LEFT_ANGLE_INC: break; } return 1; } /// 云台垂直转动监控 static char ptz_vert_rotate_monitor_task() { static float BNP_D;//刹车最近点与当前位置的距离 static float BFP_D;//刹车最远点与当前位置的距离 static char cmd_type; static unsigned int k; switch(g_ptz.vert_rotate_monitor_switch) { case 0://关闭 break; case PTZ_VERT_GO_TO_ANGLE_A://转动到指定角度启动 case PTZ_VERT_ANGLE: case PTZ_VERT_UP_KEEP: case PTZ_VERT_DOWN_KEEP: cmd_type = g_ptz.vert_rotate_monitor_switch; g_ptz.vert_rotate_monitor_switch = PTZ_VERT_GO_TO_ANGLE_B; if(g_ptz.vert_rotate_plan.rotate_switch == 1) { //首先判断是否转动 //处于转动 if(g_ptz.vert_start_stop_set == PTZ_VERT_START) { //判断转向是否相同 //如果转向相同 if(g_ptz.vert_direction_set == g_ptz.vert_rotate_plan.direction) { g_ptz.vert_speed_set = g_ptz.vert_rotate_plan.max_speed; } else//转向不同，则直接修改设置的转速 { //则先刹车 ptz_vert_stop(PTZ_VERT_STOP_TIME); //再启动 g_ptz.vert_direction_set = g_ptz.vert_rotate_plan.direction; g_ptz.vert_speed_set = g_ptz.vert_rotate_plan.max_speed; ptz_vert_start(g_ptz.vert_direction_set, g_ptz.vert_speed_set); } } else//没有转动 { g_ptz.vert_direction_set = g_ptz.vert_rotate_plan.direction; g_ptz.vert_speed_set = g_ptz.vert_rotate_plan.max_speed; ptz_vert_start(g_ptz.vert_direction_set, g_ptz.vert_speed_set); } //如果离最近刹车距离小于一定程度，则以最小转速转动 if(fabs(g_ptz.vert_angle_actual - g_ptz.vert_rotate_plan.stop_near_angle) <= PTZ_VERT_MIN_SPEED_ANGLE) { g_ptz.vert_speed_set = PTZ_VERT_MIN_SPEED; } } else//g_ptz.vert_rotate_plan.rotate_switch == 0 { //刹车关闭转动 g_ptz.vert_arrive_flag = 0;//转到指定预位置 ptz_location_return_return(LOCATION_VERT);//定位回传 ptz_preset_bit_location_return_return();//预置位到位回传 g_ptz.vert_rotate_monitor_switch = 0; if(g_ptz.vert_start_stop_set == PTZ_VERT_START) { ptz_vert_stop(PTZ_VERT_STOP_TIME); } } break; case PTZ_VERT_GO_TO_ANGLE_B://转动到指定角度定位 BNP_D = fabs(g_ptz.vert_angle_actual - g_ptz.vert_rotate_plan.stop_near_angle); BFP_D = fabs(g_ptz.vert_angle_actual - g_ptz.vert_rotate_plan.stop_far_angle); /*定位*/ if(BNP_D + BFP_D <= (g_ptz.vert_rotate_plan.stop_angle_range + 0.01)) { if(g_ptz.vert_repeat_locate_switch == 0 && cmd_type != PTZ_VERT_UP_KEEP && cmd_type != PTZ_VERT_DOWN_KEEP) { g_ptz.vert_arrive_flag = 0;//转到指定预位置 ptz_location_return_return(LOCATION_VERT);//定位回传 ptz_preset_bit_location_return_return();//预置位到位回传 } g_ptz.vert_rotate_monitor_switch = 0; if(g_ptz.vert_repeat_locate_switch == 1 && cmd_type != PTZ_VERT_UP_KEEP && cmd_type != PTZ_VERT_DOWN_KEEP) { g_ptz.vert_rotate_monitor_switch = PTZ_VERT_REPEAT_LOCATE_A; } ptz_vert_stop(PTZ_VERT_STOP_TIME); } //如果离最近刹车距离小于一定程度，则以最小转速转动 if(fabs(g_ptz.vert_angle_actual - g_ptz.vert_rotate_plan.stop_near_angle) <= PTZ_VERT_MIN_SPEED_ANGLE) { g_ptz.vert_speed_set = PTZ_VERT_MIN_SPEED; } break; case PTZ_VERT_BRAKE://刹车 g_ptz.vert_rotate_monitor_switch = 0; ptz_vert_stop(PTZ_VERT_STOP_TIME); break; case PTZ_VERT_DEC_BRAKE_A://垂直减速刹车 g_ptz.vert_speed_set = PTZ_VERT_MIN_SPEED; g_ptz.vert_rotate_monitor_switch = PTZ_VERT_DEC_BRAKE_B; k = 0; break; case PTZ_VERT_DEC_BRAKE_B: k ++;//K增加1,时间增加1ms if(g_ptz.vert_speed_actual <= PTZ_VERT_MIN_SPEED * 1.5 || k >= 1400) { k = 0; g_ptz.vert_rotate_monitor_switch = 0; ptz_vert_stop(PTZ_VERT_STOP_TIME); } break; //重复定位路径规划 case PTZ_VERT_REPEAT_LOCATE_A: ptz_vert_rotate_plan(PTZ_VERT_ANGLE); g_ptz.vert_rotate_monitor_switch = PTZ_VERT_REPEAT_LOCATE_B; break; //重复定位转动启动 case PTZ_VERT_REPEAT_LOCATE_B: if(g_ptz.vert_rotate_plan.rotate_switch == 1) { g_ptz.vert_rotate_monitor_switch = PTZ_VERT_REPEAT_LOCATE_C; //首先判断是否转动 //处于转动 if(g_ptz.vert_start_stop_set == PTZ_VERT_START) { //判断转向是否相同 //如果转向相同 if(g_ptz.vert_direction_set == g_ptz.vert_rotate_plan.direction) { g_ptz.vert_speed_set = g_ptz.vert_rotate_plan.max_speed; } else//转向不同，则直接修改设置的转速 { //则先刹车 ptz_vert_stop(PTZ_VERT_STOP_TIME); //再启动 g_ptz.vert_direction_set = g_ptz.vert_rotate_plan.direction; g_ptz.vert_speed_set = g_ptz.vert_rotate_plan.max_speed; ptz_vert_start(g_ptz.vert_direction_set, g_ptz.vert_speed_set); } } else//没有转动 { g_ptz.vert_direction_set = g_ptz.vert_rotate_plan.direction; g_ptz.vert_speed_set = g_ptz.vert_rotate_plan.max_speed; ptz_vert_start(g_ptz.vert_direction_set, g_ptz.vert_speed_set); } //如果离最近刹车距离小于一定程度，则以最小转速转动 if(fabs(g_ptz.vert_angle_actual - g_ptz.vert_rotate_plan.stop_near_angle) <= PTZ_VERT_MIN_SPEED_ANGLE) { g_ptz.vert_speed_set = PTZ_VERT_MIN_SPEED; } } else//g_ptz.vert_rotate_plan.rotate_switch == 0 { //刹车关闭转动 g_ptz.vert_arrive_flag = 0;//转到指定位置 ptz_location_return_return(LOCATION_VERT);//定位回传 ptz_preset_bit_location_return_return();//预置位到位回传 g_ptz.vert_rotate_monitor_switch = 0; if(g_ptz.vert_start_stop_set == PTZ_VERT_START) { ptz_vert_stop(PTZ_VERT_STOP_TIME); } } break; case PTZ_VERT_REPEAT_LOCATE_C: BNP_D = fabs(g_ptz.vert_angle_actual - g_ptz.vert_rotate_plan.stop_near_angle); BFP_D = fabs(g_ptz.vert_angle_actual - g_ptz.vert_rotate_plan.stop_far_angle); /*定位*/ if(BNP_D + BFP_D <= (g_ptz.vert_rotate_plan.stop_angle_range + 0.01)) { g_ptz.vert_arrive_flag = 0;//预置位扫描，转到指定预置位 ptz_location_return_return(LOCATION_VERT);//定位回传 ptz_preset_bit_location_return_return();//预置位到位回传 g_ptz.vert_rotate_monitor_switch = 0; ptz_vert_stop(PTZ_VERT_STOP_TIME); } //如果离最近刹车距离小于一定程度，则以最小转速转动 if(fabs(g_ptz.vert_angle_actual - g_ptz.vert_rotate_plan.stop_near_angle) <= PTZ_VERT_MIN_SPEED_ANGLE) { g_ptz.vert_speed_set = PTZ_VERT_MIN_SPEED; } break; } //当云台垂直方向处于极限位置时，防止垂直方向角度越界 #ifdef PTZ_PHOTOELECTRIC_SWITCH if(g_ptz.vert_ps_sw1_state == PS_COVER)//下俯极限位置 { if(g_ptz.vert_start_stop_set == PTZ_VERT_START && g_ptz.vert_direction_set == PTZ_VERT_DIR_DOWN) { g_ptz.vert_arrive_flag = 0; g_ptz.vert_rotate_monitor_switch = 0; ptz_vert_stop(PTZ_VERT_STOP_TIME); } } if(g_ptz.vert_ps_sw2_state == PS_COVER)//上仰极限位置 { if(g_ptz.vert_start_stop_set == PTZ_VERT_START && g_ptz.vert_direction_set == PTZ_VERT_DIR_UP) { g_ptz.vert_arrive_flag = 0; g_ptz.vert_rotate_monitor_switch = 0; ptz_vert_stop(PTZ_VERT_STOP_TIME); } } #endif return 1; } static void ptz_hori_rotate_task() { while(1) { ptz_hori_rotate_monitor_task(); OSTimeDlyHMSM(0u, 0u, 0u, 1u); } } static void ptz_vert_rotate_task() { while(1) { ptz_vert_rotate_monitor_task(); OSTimeDlyHMSM(0u, 0u, 0u, 1u); } } static OS_STK task_hori_rotate_stk[TASK_HORI_ROATE_MONITOR_STK_SIZE]; static void creat_task_hori_rotate(void) { CPU_INT08U task_err; CPU_INT08U name_err; task_err = OSTaskCreateExt((void (*)(void *)) ptz_hori_rotate_task, (void *) 0, (OS_STK *)&task_hori_rotate_stk[TASK_HORI_ROATE_MONITOR_STK_SIZE - 1], (INT8U ) TASK_HORI_ROATE_MONITOR_PRIO, (INT16U ) TASK_HORI_ROATE_MONITOR_PRIO, (OS_STK *)&task_hori_rotate_stk[0], (INT32U ) TASK_HORI_ROATE_MONITOR_STK_SIZE, (void *) 0, (INT16U )(OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR)); #if (OS_TASK_NAME_EN > 0) OSTaskNameSet(TASK_HORI_ROATE_MONITOR_PRIO, "ptz_hori_rotate_task", &name_err); #endif } static OS_STK task_vert_rotate_stk[TASK_VERT_ROATE_MONITOR_STK_SIZE]; static void creat_task_vert_rotate(void) { CPU_INT08U task_err; CPU_INT08U name_err; task_err = OSTaskCreateExt((void (*)(void *)) ptz_vert_rotate_task, (void *) 0, (OS_STK *)&task_vert_rotate_stk[TASK_VERT_ROATE_MONITOR_STK_SIZE - 1], (INT8U ) TASK_VERT_ROATE_MONITOR_PRIO, (INT16U ) TASK_VERT_ROATE_MONITOR_PRIO, (OS_STK *)&task_vert_rotate_stk[0], (INT32U ) TASK_VERT_ROATE_MONITOR_STK_SIZE, (void *) 0, (INT16U )(OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR)); #if (OS_TASK_NAME_EN > 0) OSTaskNameSet(TASK_VERT_ROATE_MONITOR_PRIO, "ptz_vert_rotate_task", &name_err); #endif } void init_rotate_monitor_module(void) { BSP_OS_SemCreate(&ptz_hori_stop_mutex,1u,"ptz_hori_stop_mutex"); BSP_OS_SemCreate(&ptz_vert_stop_mutex,1u,"ptz_vert_stop_mutex"); l6235d_init(); creat_task_hori_rotate(); creat_task_vert_rotate(); } #endif