#include "service_presetbitscan.h" #include "bsp_os.h" #include "ptz_header_file.h" static BSP_OS_SEM ptz_preset_bit_crc_mutex; PresetBitScan g_preset_bit_scan; /////计算每个预置位对应的效验码 ///计算每个预置位对应的效验码 unsigned int ptz_preset_bit_crc(PresetBit bit) { BSP_OS_SemWait(&ptz_preset_bit_crc_mutex, 0u); float crc1 = 0; unsigned int crc = 0; crc1 = bit.hori_angle; crc1 = crc1 + bit.vert_angle; crc1 = crc1 + bit.hori_scan_speed; crc1 = crc1 + bit.vert_scan_speed; crc1 = crc1 + bit.zoom_v; crc1 = crc1 + bit.focus_v; crc1 = crc1 + bit.step_stop_time; crc1 = crc1 + bit.enable; BSP_OS_SemPost(&ptz_preset_bit_crc_mutex); crc = (unsigned int)crc1; return crc; } ///保存预置位参数 char ptz_preset_bit_save(PresetBit bit, unsigned int num) { unsigned int write_add; write_add = PRESET_BIT_FIRST_FLASH_ADD + sizeof(PresetBit) * num; w25q128_random_write(write_add, PRESET_BIT_CACHE_FLASH_ADD, (char *)&bit, sizeof(PresetBit)); return 1; } PresetBit ptz_preset_bit_read(unsigned int num) { unsigned int read_add; PresetBit bit; char flag = 0; read_add = PRESET_BIT_FIRST_FLASH_ADD + sizeof(PresetBit) * num; Flash_Read((unsigned char *)&bit,read_add,sizeof(PresetBit)); //防止读出来的数据不是一个数 if(isnan(bit.hori_angle) == 1) { bit.hori_angle = 0; flag = 1; } if(isnan(bit.vert_angle) == 1) { bit.vert_angle = 0; flag = 1; } if(isnan(bit.hori_scan_speed) == 1) { bit.hori_scan_speed = 0; flag = 1; } if(isnan(bit.vert_scan_speed) == 1) { bit.vert_scan_speed = 0; flag = 1; } if(isnan(bit.step_stop_time) == 1) { bit.step_stop_time = 0; flag = 1; } if(isnan(bit.enable) == 1) { bit.enable = 0; flag = 1; } if(isnan(bit.crc) == 1) { bit.crc = 0; flag = 1; } if(flag == 1) { bit.crc = ptz_preset_bit_crc(bit); } return bit; } //擦除所有预置位 void ptz_preset_bit_all_erase() { Flash_Erase_Block(w25q128_add_to_block_sector(PRESET_BIT_FIRST_FLASH_ADD).block); Flash_Erase_Block(w25q128_add_to_block_sector(PRESET_BIT_FIRST_FLASH_ADD).block + 1); } //云台预置位扫描 static void ptz_preset_bit_scan_task() { static short int scan_order;//扫描顺序 static unsigned short int stop_time; unsigned int i = 0; unsigned int j = 0; static PresetBit bit; while(1) { switch(g_preset_bit_scan.state) { case PRESET_BIT_SCAN_CLOSE_B://扫描关闭 g_preset_bit_scan.state_a = 0; break; case PRESET_BIT_SCAN_START://扫描打开，判断所有参数是否都在允许范围以内 if(g_preset_bit_scan.start_num <= g_preset_bit_scan.end_num) { scan_order = 1;//由小到大扫描 } else { scan_order = -1;//由大到小扫描 } //判断起始预置位和结束预置位之间有多少可用的预置位 j = 0; i = 0; if(scan_order == -1) { for(i = g_preset_bit_scan.end_num; i <= g_preset_bit_scan.start_num; i++) { //从FLASH读取预置位参数 bit = ptz_preset_bit_read(i); if(bit.enable == PRESET_BIT_ENABLE && bit.crc == ptz_preset_bit_crc(bit)) { j ++;//判断有多少可用预置位 } } } if(scan_order == 1) { for(i = g_preset_bit_scan.start_num; i <= g_preset_bit_scan.end_num; i++) { //从FLASH读取预置位参数 bit = ptz_preset_bit_read(i); if(bit.enable == PRESET_BIT_ENABLE && bit.crc == ptz_preset_bit_crc(bit)) { j ++;//判断是否有可用的预置位 } } } if(j == 0)//没有可用预置位，则直接关闭预置位扫描 { g_preset_bit_scan.state = PRESET_BIT_SCAN_CLOSE_A; } else//有可用预置位，则开始启动扫描 { g_preset_bit_scan.actual_num = g_preset_bit_scan.start_num; g_preset_bit_scan.last_state = g_preset_bit_scan.state; g_preset_bit_scan.state = PRESET_BIT_SCAN_START + 1; g_preset_bit_scan.state_a = g_preset_bit_scan.state; } break; case PRESET_BIT_SCAN_START + 1://转到指定预置位 //从FLASH读取预置位参数 bit = ptz_preset_bit_read(g_preset_bit_scan.actual_num); //首先判断转速是否在指定的范围内 g_preset_bit_scan.hori_speed =bit.hori_scan_speed; if(g_preset_bit_scan.hori_speed < PTZ_HORI_MIN_SPEED) { g_preset_bit_scan.hori_speed = PTZ_HORI_MIN_SPEED; } if(g_preset_bit_scan.hori_speed > PTZ_HORI_MAX_SPEED) { g_preset_bit_scan.hori_speed = PTZ_HORI_MAX_SPEED; } g_preset_bit_scan.vert_speed = bit.vert_scan_speed; if(g_preset_bit_scan.vert_speed < PTZ_VERT_MIN_SPEED) { g_preset_bit_scan.vert_speed = PTZ_VERT_MIN_SPEED; } if(g_preset_bit_scan.vert_speed > PTZ_VERT_MAX_SPEED) { g_preset_bit_scan.vert_speed = PTZ_VERT_MAX_SPEED; } //提取停止时间 g_preset_bit_scan.stop_time = bit.step_stop_time; //判断效验码是否正确，以及判断预置位是否有效,有效 if((bit.crc == ptz_preset_bit_crc(bit)) && (bit.enable == PRESET_BIT_ENABLE)) {//调用预置位 g_ptz.vert_angle_control = bit.vert_angle; g_ptz.vert_speed_control = g_preset_bit_scan.vert_speed; ptz_vert_rotate_plan(PTZ_VERT_ANGLE); g_ptz.vert_rotate_monitor_switch = PTZ_VERT_ANGLE; g_ptz.hori_angle_control = bit.hori_angle; g_ptz.hori_speed_control = g_preset_bit_scan.hori_speed; ptz_hori_rotate_plan(PTZ_HORI_MIN_DISTANCE); g_ptz.hori_rotate_monitor_switch = PTZ_HORI_MIN_DISTANCE; g_ptz.hori_arrive_flag = 1; g_ptz.vert_arrive_flag = 1; g_preset_bit_scan.last_state = g_preset_bit_scan.state; g_preset_bit_scan.state = PRESET_BIT_SCAN_START + 2; g_preset_bit_scan.state_a = g_preset_bit_scan.state; } else { g_preset_bit_scan.actual_num = g_preset_bit_scan.actual_num + scan_order; if(scan_order == 1) { if(g_preset_bit_scan.actual_num > g_preset_bit_scan.end_num) { g_preset_bit_scan.actual_num = g_preset_bit_scan.start_num; } } else //order == -1 { if(g_preset_bit_scan.actual_num < g_preset_bit_scan.end_num) { g_preset_bit_scan.actual_num = g_preset_bit_scan.start_num; } } } break; case PRESET_BIT_SCAN_START + 2://判断预置位是否到达 if(g_ptz.hori_arrive_flag == 0 && g_ptz.vert_arrive_flag == 0) { ptz_preset_bit_scan_location_return_return();//回传到达指定预置位 stop_time = 0; g_preset_bit_scan.last_state = g_preset_bit_scan.state; g_preset_bit_scan.state = PRESET_BIT_SCAN_START + 3; g_preset_bit_scan.state_a = g_preset_bit_scan.state; } break; case PRESET_BIT_SCAN_START + 3://到达指定位置，开始停止计时 for(stop_time = 0; stop_time < g_preset_bit_scan.stop_time; stop_time ++) { if(g_preset_bit_scan.state == PRESET_BIT_SCAN_START + 3) {//由于任务优先级不同，因此要加一个这个判断，好实时响应预置位停止、暂停、关闭等指令 OSTimeDlyHMSM(0u, 0u, 0u, 1u); } else { stop_time = 0; g_preset_bit_scan.last_state = g_preset_bit_scan.state; g_preset_bit_scan.state_a = g_preset_bit_scan.state; break; } } if(g_preset_bit_scan.state == PRESET_BIT_SCAN_START + 3) {//由于任务优先级不同，因此要加一个这个判断，好实时响应预置位停止、暂停、关闭等指令 stop_time = 0; g_preset_bit_scan.last_state = g_preset_bit_scan.state; g_preset_bit_scan.state = PRESET_BIT_SCAN_START + 4; g_preset_bit_scan.state_a = g_preset_bit_scan.state; } break; case PRESET_BIT_SCAN_START + 4://时间到达，准备进入下一个预置位 g_preset_bit_scan.actual_num = g_preset_bit_scan.actual_num + scan_order; if(scan_order ==1) { if(g_preset_bit_scan.actual_num > g_preset_bit_scan.end_num) { ptz_preset_bit_scan_end_return_return();//一个周期预置位扫描结束回传 if(g_preset_bit_scan.mode == 1) { g_preset_bit_scan.state = PRESET_BIT_SCAN_CLOSE_A; break; } else { g_preset_bit_scan.actual_num = g_preset_bit_scan.start_num; } } } else //order == -1 { if(g_preset_bit_scan.actual_num < g_preset_bit_scan.end_num) { ptz_preset_bit_scan_end_return_return();//一个周期预置位扫描结束回传 if(g_preset_bit_scan.mode == 1) { g_preset_bit_scan.state = PRESET_BIT_SCAN_CLOSE_A; break; } else { g_preset_bit_scan.actual_num = g_preset_bit_scan.start_num; } } } g_preset_bit_scan.last_state = g_preset_bit_scan.state; g_preset_bit_scan.state = PRESET_BIT_SCAN_START + 1; g_preset_bit_scan.state_a = g_preset_bit_scan.state; break; case PRESET_BIT_SCAN_PAUSE_A://暂停扫描 if(g_preset_bit_scan.last_state > 0 && g_preset_bit_scan.last_state <= PRESET_BIT_SCAN_START + 4) { g_ptz.vert_rotate_monitor_switch = PTZ_VERT_BRAKE; g_ptz.hori_rotate_monitor_switch = PTZ_HORI_BRAKE; g_preset_bit_scan.save_state = g_preset_bit_scan.state_a; g_preset_bit_scan.last_state = g_preset_bit_scan.state; g_preset_bit_scan.state = PRESET_BIT_SCAN_PAUSE_B; g_preset_bit_scan.state_a = g_preset_bit_scan.state; } if(g_preset_bit_scan.last_state == 0) { g_preset_bit_scan.state = 0; } break; case PRESET_BIT_SCAN_PAUSE_B://扫描暂停中 break; case PRESET_BIT_SCAN_RECOVERY://扫描恢复 if(g_preset_bit_scan.state_a == PRESET_BIT_SCAN_PAUSE_A || g_preset_bit_scan.state_a == PRESET_BIT_SCAN_PAUSE_B) { g_preset_bit_scan.last_state = g_preset_bit_scan.state; if(g_preset_bit_scan.save_state == PRESET_BIT_SCAN_START) { g_preset_bit_scan.state = PRESET_BIT_SCAN_START; } else { g_preset_bit_scan.state = PRESET_BIT_SCAN_START + 1; } g_preset_bit_scan.state_a = g_preset_bit_scan.state; } break; case PRESET_BIT_SCAN_CLOSE_A://扫描关闭 if(g_preset_bit_scan.last_state != 0) { g_ptz.vert_rotate_monitor_switch = PTZ_VERT_DEC_BRAKE_A; g_ptz.hori_rotate_monitor_switch = PTZ_HORI_DEC_BRAKE_A; g_preset_bit_scan.last_state = g_preset_bit_scan.state; g_preset_bit_scan.state = PRESET_BIT_SCAN_CLOSE_B; } break; } OSTimeDlyHMSM(0u, 0u, 0u, 20u); } } //云台预置位扫描任务 static OS_STK task_preset_bit_scan_stk[TASK_PRESET_BIT_SCAN_STK_SIZE]; static void creat_task_preset_bit_scan(void) { CPU_INT08U task_err; CPU_INT08U name_err; task_err = OSTaskCreateExt((void (*)(void *)) ptz_preset_bit_scan_task, (void *) 0, (OS_STK *)&task_preset_bit_scan_stk[TASK_PRESET_BIT_SCAN_STK_SIZE - 1], (INT8U ) TASK_PRESET_BIT_SCAN_PRIO, (INT16U ) TASK_PRESET_BIT_SCAN_PRIO, (OS_STK *)&task_preset_bit_scan_stk[0], (INT32U ) TASK_PRESET_BIT_SCAN_STK_SIZE, (void *) 0, (INT16U )(OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR)); #if (OS_TASK_NAME_EN > 0) OSTaskNameSet(TASK_PRESET_BIT_SCAN_PRIO, "ptz_preset_bit_scan_task", &name_err); #endif } /******************************************************************************/ void init_preset_bit_scan_module(void) { creat_task_preset_bit_scan(); } //预置位扫描一个周期结束回传 void ptz_preset_bit_scan_end_return_return() { unsigned char preset_bit[7] = {0xff,0x00,0x00,0x00,0x00,0x00,0x00}; preset_bit[1] = g_ptz.address; preset_bit[2] = 0x9f; preset_bit[3] = PRESET_BIT_SCAN_END_RETURN_DATA;//表示回传,一个扫描周期结束 preset_bit[4] = g_preset_bit_scan.start_num;//预置位起始编号 preset_bit[5] = g_preset_bit_scan.end_num;//预置位结束编号 preset_bit[6] = MotorCalPelcoDSUM(preset_bit,sizeof(preset_bit)); if(g_ptz.preset_bit_return.preset_bit_scan_end_udp_switch == PRESET_BIT_RETURN_ON) { //发送数据 send_udp_data_aim(preset_bit, sizeof(preset_bit), (struct sockaddr*)&g_ptz.preset_bit_return.preset_bit_scan_end_from, g_ptz.preset_bit_return.preset_bit_scan_end_fromlen); } if(g_ptz.preset_bit_return.preset_bit_scan_end_uart_422_switch == PRESET_BIT_RETURN_ON) { ptz_send_data(PTZ_UART_422, preset_bit, sizeof(preset_bit)); } if(g_ptz.preset_bit_return.preset_bit_scan_end_uart_485_switch == PRESET_BIT_RETURN_ON) { ptz_send_data(PTZ_UART_485, preset_bit, sizeof(preset_bit)); } } void ptz_preset_bit_scan_location_return_return() { unsigned char preset_bit[7] = {0xff,0x00,0x00,0x00,0x00,0x00,0x00}; preset_bit[1] = g_ptz.address; preset_bit[2] = 0x9f; preset_bit[3] = PRESET_BIT_SCAN_LOCATION_RETURN_DATA;//表示回传,一个扫描周期结束 preset_bit[4] = g_preset_bit_scan.actual_num;//当前编号 preset_bit[5] = 0; preset_bit[6] = MotorCalPelcoDSUM(preset_bit,sizeof(preset_bit)); if(g_ptz.preset_bit_return.preset_bit_scan_location_udp_switch == PRESET_BIT_RETURN_ON) { //发送数据 send_udp_data_aim(preset_bit, sizeof(preset_bit), (struct sockaddr*)&g_ptz.preset_bit_return.preset_bit_scan_location_from, g_ptz.preset_bit_return.preset_bit_scan_location_fromlen); } if(g_ptz.preset_bit_return.preset_bit_scan_location_uart_422_switch == PRESET_BIT_RETURN_ON) { ptz_send_data(PTZ_UART_422, preset_bit, sizeof(preset_bit)); } if(g_ptz.preset_bit_return.preset_bit_scan_location_uart_485_switch == PRESET_BIT_RETURN_ON) { ptz_send_data(PTZ_UART_485, preset_bit, sizeof(preset_bit)); } } void ptz_preset_bit_location_return_save( unsigned int num, char dev) { if(g_ptz.preset_bit_return.preset_bit_location_udp_switch == PRESET_BIT_RETURN_ON || g_ptz.preset_bit_return.preset_bit_location_uart_422_switch == PRESET_BIT_RETURN_ON|| g_ptz.preset_bit_return.preset_bit_location_uart_485_switch == PRESET_BIT_RETURN_ON) { //防止角度定位回传和预置位回传发生冲突，即定位回传和预置位到位回传只能回传一个 g_ptz.location_return.hori_cmd = 0; g_ptz.location_return.vert_cmd = 0; g_ptz.preset_bit_return.preset_bit_location_cmd = 1; //保存预置位编号 g_ptz.preset_bit_return.preset_bit_location_num = num; switch(dev) { case PTZ_UDP: g_ptz.preset_bit_return.preset_bit_location_cmd_source = PRESET_BIT_LOCATION_UDP_CMD_ON; break; case PTZ_UART_422: g_ptz.preset_bit_return.preset_bit_location_cmd_source = PRESET_BIT_LOCATION_UART_422_CMD_ON; break; case PTZ_UART_485: g_ptz.preset_bit_return.preset_bit_location_cmd_source = PRESET_BIT_LOCATION_UART_485_CMD_ON; break; } } } void ptz_preset_bit_location_return_return() { unsigned char preset_bit[7] = {0xff,0x00,0x00,0x00,0x00,0x00,0x00}; if(g_ptz.preset_bit_return.preset_bit_location_cmd < 1) { return; } else { g_ptz.preset_bit_return.preset_bit_location_cmd ++; } preset_bit[1] = g_ptz.address; preset_bit[2] = 0x9f; preset_bit[3] = PRESET_BIT_LOCATION_RETURN_DATA;//表示回传,一个扫描周期结束 preset_bit[4] = g_ptz.preset_bit_return.preset_bit_location_num;//当前编号 preset_bit[5] = 0; preset_bit[6] = MotorCalPelcoDSUM(preset_bit,sizeof(preset_bit)); if(g_ptz.preset_bit_return.preset_bit_location_cmd == 3) { if(g_ptz.preset_bit_return.preset_bit_location_udp_switch == PRESET_BIT_RETURN_ON && g_ptz.preset_bit_return.preset_bit_location_cmd_source == PRESET_BIT_LOCATION_UDP_CMD_ON) { //发送数据 send_udp_data_aim(preset_bit, sizeof(preset_bit), (struct sockaddr*)&g_ptz.preset_bit_return.preset_bit_location_from, g_ptz.preset_bit_return.preset_bit_location_fromlen); } if(g_ptz.preset_bit_return.preset_bit_location_uart_422_switch == PRESET_BIT_RETURN_ON && g_ptz.preset_bit_return.preset_bit_location_cmd_source == PRESET_BIT_LOCATION_UART_422_CMD_ON) { ptz_send_data(PTZ_UART_422, preset_bit, sizeof(preset_bit)); } if(g_ptz.preset_bit_return.preset_bit_location_uart_485_switch == PRESET_BIT_RETURN_ON && g_ptz.preset_bit_return.preset_bit_location_cmd_source == PRESET_BIT_LOCATION_UART_485_CMD_ON) { ptz_send_data(PTZ_UART_485, preset_bit, sizeof(preset_bit)); } g_ptz.preset_bit_return.preset_bit_location_cmd = 0; } } //发送云台预置位配置信息 void presetbit_data_return(char dev, unsigned char ScanNum) { unsigned char presetbit[7]; unsigned short int data; PresetBit preset_bit; //读取对应编号预置位数据 preset_bit = ptz_preset_bit_read(ScanNum); //预置位水平角度 data = (unsigned short int)(preset_bit.hori_angle * 100 + 0.5); memset(presetbit,0,sizeof(presetbit)); presetbit[0] = 0xff; presetbit[1] = g_ptz.address; presetbit[2] = PRESET_BIT_HORI_ANGLE;//0xe4 presetbit[3] = ScanNum; presetbit[4] = (unsigned char)(data >> 8); presetbit[5] = (unsigned char)(data & 0x00ff); presetbit[6] = MotorCalPelcoDSUM(presetbit,sizeof(presetbit)); ptz_send_data(dev, presetbit, sizeof(presetbit)); //预置位垂直角度 data = (unsigned short int)(preset_bit.vert_angle * 100 + 0.5); memset(presetbit,0,sizeof(presetbit)); presetbit[0] = 0xff; presetbit[1] = g_ptz.address; presetbit[2] = PRESET_BIT_VERT_ANGLE;//0xa1 presetbit[3] = ScanNum; presetbit[4] = (unsigned char)(data >> 8); presetbit[5] = (unsigned char)(data & 0x00ff); presetbit[6] = MotorCalPelcoDSUM(presetbit,sizeof(presetbit)); ptz_send_data(dev, presetbit, sizeof(presetbit)); //预置位停止时间 data = (unsigned short int)(preset_bit.step_stop_time); memset(presetbit,0,sizeof(presetbit)); presetbit[0] = 0xff; presetbit[1] = g_ptz.address; presetbit[2] = PRESET_BIT_STOPTIME;//0xa2 presetbit[3] = ScanNum; presetbit[4] = (unsigned char)(data >> 8); presetbit[5] = (unsigned char)(data & 0x00ff); presetbit[6] = MotorCalPelcoDSUM(presetbit,sizeof(presetbit)); ptz_send_data(dev, presetbit, sizeof(presetbit)); //预置位水平、垂直速度 memset(presetbit,0,sizeof(presetbit)); presetbit[0] = 0xff; presetbit[1] = g_ptz.address; presetbit[2] = PRESET_BIT_SPEED;//0xa3 presetbit[3] = ScanNum; presetbit[4] = (unsigned char)(preset_bit.hori_scan_speed * 10); presetbit[5] = (unsigned char)(preset_bit.vert_scan_speed * 10); presetbit[6] = MotorCalPelcoDSUM(presetbit,sizeof(presetbit)); ptz_send_data(dev, presetbit, sizeof(presetbit)); //预置位使能状态 memset(presetbit,0,sizeof(presetbit)); presetbit[0] = 0xff; presetbit[1] = g_ptz.address; presetbit[2] = PRESET_BIT_ENABLE;//0xa4 presetbit[3] = ScanNum; presetbit[4] = preset_bit.enable;//使能状态 presetbit[6] = MotorCalPelcoDSUM(presetbit,sizeof(presetbit)); ptz_send_data(dev, presetbit, sizeof(presetbit)); }