1417 lines
47 KiB
C
1417 lines
47 KiB
C
#include "bsp_os.h"
|
||
#include "ptz_header_file.h"
|
||
#include "enet_to_udp.h"
|
||
#include "service_areascan.h"
|
||
|
||
static BSP_OS_SEM ptz_area_crc_mutex;
|
||
|
||
PtzArea g_area[AREA_AMOUNT];
|
||
AreaScan g_area_scan;
|
||
|
||
//发送云台区域扫描的区域配置信息
|
||
void ptz_send_area_data(char dev, unsigned short int ScanNum)
|
||
{
|
||
unsigned char AreaData[7];
|
||
unsigned short int data;
|
||
|
||
//水平边界A
|
||
data = (unsigned short int)(g_area[ScanNum].hori_start_angle * 100);
|
||
memset(AreaData,0,sizeof(AreaData));
|
||
AreaData[0] = 0xff;
|
||
AreaData[1] = g_ptz.address;
|
||
AreaData[2] = SCAN_HA;
|
||
AreaData[3] = ScanNum;
|
||
AreaData[4] = (unsigned char)(data >> 8);
|
||
AreaData[5] = (unsigned char)(data & 0x00ff);
|
||
AreaData[6] = MotorCalPelcoDSUM(AreaData,sizeof(AreaData));
|
||
|
||
ptz_send_data(dev, AreaData, sizeof(AreaData));
|
||
|
||
|
||
//水平边界B
|
||
data = (unsigned short int)(g_area[ScanNum].hori_end_angle * 100);
|
||
memset(AreaData,0,sizeof(AreaData));
|
||
AreaData[0] = 0xff;
|
||
AreaData[1] = g_ptz.address;
|
||
AreaData[2] = SCAN_HB;
|
||
AreaData[3] = ScanNum;
|
||
AreaData[4] = (unsigned char)(data >> 8);
|
||
AreaData[5] = (unsigned char)(data & 0x00ff);
|
||
AreaData[6] =MotorCalPelcoDSUM(AreaData,sizeof(AreaData));
|
||
ptz_send_data(dev,AreaData,sizeof(AreaData));
|
||
|
||
|
||
//垂直边界A
|
||
data = (unsigned short int)(g_area[ScanNum].vert_start_angle * 100);
|
||
memset(AreaData,0,sizeof(AreaData));
|
||
AreaData[0] = 0xff;
|
||
AreaData[1] = g_ptz.address;
|
||
AreaData[2] = SCAN_VA;
|
||
AreaData[3] = ScanNum;
|
||
AreaData[4] = (unsigned char)(data >> 8);
|
||
AreaData[5] = (unsigned char)(data & 0x00ff);
|
||
AreaData[6] = MotorCalPelcoDSUM(AreaData,sizeof(AreaData));
|
||
ptz_send_data(dev,AreaData,sizeof(AreaData));
|
||
|
||
|
||
//垂直边界B
|
||
data = (unsigned short int)(g_area[ScanNum].vert_end_angle * 100);
|
||
memset(AreaData,0,sizeof(AreaData));
|
||
AreaData[0] = 0xff;
|
||
AreaData[1] = g_ptz.address;
|
||
AreaData[2] = SCAN_VB;
|
||
AreaData[3] = ScanNum;
|
||
AreaData[4] = (unsigned char)(data >> 8);
|
||
AreaData[5] = (unsigned char)(data & 0x00ff);
|
||
AreaData[6] = MotorCalPelcoDSUM(AreaData,sizeof(AreaData));
|
||
ptz_send_data(dev,AreaData,sizeof(AreaData));
|
||
|
||
|
||
//水平间隔角度
|
||
data = (unsigned short int)(g_area[ScanNum].hori_step_angle * 100);
|
||
memset(AreaData,0,sizeof(AreaData));
|
||
AreaData[0] = 0xff;
|
||
AreaData[1] = g_ptz.address;
|
||
AreaData[2] = SCAN_INT_H;
|
||
AreaData[3] = ScanNum;
|
||
AreaData[4] = (unsigned char)(data >> 8);
|
||
AreaData[5] = (unsigned char)(data & 0x00ff);
|
||
AreaData[6] = MotorCalPelcoDSUM(AreaData,sizeof(AreaData));
|
||
ptz_send_data(dev,AreaData,sizeof(AreaData));
|
||
|
||
|
||
//垂直间隔角度
|
||
data = (unsigned short int)(g_area[ScanNum].vert_step_angle * 100);
|
||
memset(AreaData,0,sizeof(AreaData));
|
||
AreaData[0] = 0xff;
|
||
AreaData[1] = g_ptz.address;
|
||
AreaData[2] = SCAN_INT_V;
|
||
AreaData[3] = ScanNum;
|
||
AreaData[4] = (unsigned char)(data >> 8);
|
||
AreaData[5] = (unsigned char)(data & 0x00ff);
|
||
AreaData[6] = MotorCalPelcoDSUM(AreaData,sizeof(AreaData));
|
||
ptz_send_data(dev,AreaData,sizeof(AreaData));
|
||
|
||
|
||
//转动速度
|
||
memset(AreaData,0,sizeof(AreaData));
|
||
AreaData[0] = 0xff;
|
||
AreaData[1] = g_ptz.address;
|
||
AreaData[2] = SCAN_SPEED;
|
||
AreaData[3] = ScanNum;
|
||
AreaData[4] = (unsigned char)(g_area[ScanNum].hori_scan_speed * 10);
|
||
AreaData[5] = (unsigned char)(g_area[ScanNum].vert_scan_speed * 10);
|
||
AreaData[6] = MotorCalPelcoDSUM(AreaData,sizeof(AreaData));
|
||
ptz_send_data(dev,AreaData,sizeof(AreaData));
|
||
|
||
|
||
//扫描停止时间
|
||
data = (unsigned short int)(g_area[ScanNum].step_stop_time);
|
||
memset(AreaData,0,sizeof(AreaData));
|
||
AreaData[0] = 0xff;
|
||
AreaData[1] = g_ptz.address;
|
||
AreaData[2] = SCAN_STOP_TIME;
|
||
AreaData[3] = ScanNum;
|
||
AreaData[4] = (unsigned char)(data >> 8);
|
||
AreaData[5] = (unsigned char)(data & 0x00ff);
|
||
AreaData[6] = MotorCalPelcoDSUM(AreaData,sizeof(AreaData));
|
||
ptz_send_data(dev,AreaData,sizeof(AreaData));
|
||
|
||
|
||
//区域扫描方式
|
||
memset(AreaData,0,sizeof(AreaData));
|
||
AreaData[0] = 0xff;
|
||
AreaData[1] = g_ptz.address;
|
||
AreaData[2] = SCAN_WAY;
|
||
AreaData[3] = ScanNum;
|
||
AreaData[4] = g_area[ScanNum].scan_mode;
|
||
AreaData[5] = 0;
|
||
AreaData[6] = MotorCalPelcoDSUM(AreaData,sizeof(AreaData));
|
||
ptz_send_data(dev,AreaData, sizeof(AreaData));
|
||
|
||
|
||
//云台扫描使能
|
||
memset(AreaData,0,sizeof(AreaData));
|
||
AreaData[0] = 0xff;
|
||
AreaData[1] = g_ptz.address;
|
||
AreaData[2] = SCAN_ENABLE;
|
||
AreaData[3] = ScanNum;
|
||
AreaData[4] = g_area[ScanNum].enable;
|
||
AreaData[5] = 0;
|
||
AreaData[6] = MotorCalPelcoDSUM(AreaData,sizeof(AreaData));
|
||
ptz_send_data(dev,AreaData, sizeof(AreaData));
|
||
|
||
}
|
||
|
||
|
||
|
||
///区域自动使能
|
||
char ptz_area_auto_enable(PtzArea area)
|
||
{
|
||
if(area.hsa == 1 &&
|
||
area.hea == 1 &&
|
||
area.vsa == 1 &&
|
||
area.vea == 1 &&
|
||
area.h_step == 1 &&
|
||
area.v_step == 1)
|
||
{
|
||
return 1;
|
||
}
|
||
return 0;
|
||
}
|
||
|
||
///计算每个 对应的效验码
|
||
unsigned int ptz_area_crc(PtzArea area)
|
||
{
|
||
BSP_OS_SemWait(&ptz_area_crc_mutex, 0u);
|
||
float crc1 = 0;
|
||
unsigned int crc = 0;
|
||
|
||
crc1 = crc1 + area.hori_start_angle;//水平起始边界
|
||
crc1 = crc1 + area.hsa;//水平边界A是否被设置，1设置。0未被设置
|
||
|
||
crc1 = crc1 + area.hori_end_angle;//水平结束边界
|
||
crc1 = crc1 + area.hea;//水平边界B是否被设置，1设置。0未被设置
|
||
|
||
crc1 = crc1 + area.vert_start_angle;//垂直起始边界
|
||
crc1 = crc1 + area.vsa;//垂直边界A是否被设置，1设置。0未被设置
|
||
|
||
crc1 = crc1 + area.vert_end_angle;//垂直结束边界
|
||
crc1 = crc1 + area.vea;//垂直边界B是否被设置，1设置。0未被设置
|
||
|
||
crc1 = crc1 + area.hori_step_angle;//水平间隔角度
|
||
crc1 = crc1 + area.h_step;//水平间隔角度是否被设置，1设置。0未被设置
|
||
|
||
crc1 = crc1 + area.vert_step_angle;//垂直间隔角度
|
||
crc1 = crc1 + area.v_step;//垂直间隔角度是否被设置，1设置。0未被设置
|
||
|
||
crc1 = crc1 + area.hori_scan_speed;//水平转动速度
|
||
crc1 = crc1 + area.vert_scan_speed;//垂直转动速度
|
||
crc1 = crc1 + area.step_stop_time;//单步转动每个位置的停止时间，单位是ms，并且必须大于电机默认的停止时间
|
||
crc1 = crc1 + area.scan_mode;//0默认连续扫描，1连续扫描，2单步扫描
|
||
crc1 = crc1 + area.enable;//区域扫描是否有效，1有效，0无效
|
||
|
||
BSP_OS_SemPost(&ptz_area_crc_mutex);
|
||
crc = (unsigned int)crc1;
|
||
return crc;
|
||
}
|
||
|
||
|
||
///区域扫描参数保存
|
||
char ptz_area_save()
|
||
{
|
||
char i,j;
|
||
static PtzArea area[AREA_AMOUNT];
|
||
|
||
//写入主存储区
|
||
for(i = 0; i < AREA_SAVE_FLASH_NUM; i++)
|
||
{
|
||
memset(area, 0, sizeof(area));
|
||
write_many_data(sizeof(g_area),(unsigned char *)g_area,AREA_FLASH_ADD);
|
||
OSTimeDlyHMSM(0u, 0u, 0u, 5u);
|
||
Flash_Read((unsigned char *)area,AREA_FLASH_ADD,sizeof(g_area));
|
||
if(memcmp(g_area, area, sizeof(g_area)) == 0)//判断写入数据是否正确
|
||
{
|
||
break;
|
||
}
|
||
OSTimeDlyHMSM(0u, 0u, 0u, 5u);
|
||
}
|
||
//写入备份存储区
|
||
for(j = 0; j < AREA_SAVE_FLASH_NUM; j++)
|
||
{
|
||
memset(area, 0, sizeof(area));
|
||
write_many_data(sizeof(g_area),(unsigned char *)g_area,AREA_BACKUP_FLASH_ADD);
|
||
OSTimeDlyHMSM(0u, 0u, 0u, 5u);
|
||
Flash_Read((unsigned char *)area,AREA_BACKUP_FLASH_ADD,sizeof(g_area));
|
||
if(memcmp(g_area, area, sizeof(g_area)) == 0)//判断写入数据是否正确
|
||
{
|
||
break;
|
||
}
|
||
OSTimeDlyHMSM(0u, 0u, 0u, 5u);
|
||
}
|
||
if(i < AREA_SAVE_FLASH_NUM || j < AREA_SAVE_FLASH_NUM)//判断写入数据是否成功
|
||
{
|
||
//写入成功
|
||
return 1;
|
||
}
|
||
//写入失败
|
||
return 0;
|
||
}
|
||
|
||
///区域扫描参数读取**在初始化中已经全部将参数读出
|
||
char ptz_area_read()
|
||
{
|
||
unsigned int i = 0;
|
||
char flag = 0;
|
||
Flash_Read((unsigned char *)g_area,AREA_FLASH_ADD,sizeof(g_area));
|
||
for(i = 0; i < AREA_AMOUNT; i++)//防止读出来的数据不是一个数
|
||
{
|
||
if(isnan(g_area[i].hori_start_angle) == 1)
|
||
{
|
||
g_area[i].hori_start_angle = 0;
|
||
flag = 1;
|
||
}
|
||
if(isnan(g_area[i].hsa) == 1)
|
||
{
|
||
g_area[i].hsa = 0;
|
||
flag = 1;
|
||
}
|
||
if(isnan(g_area[i].hori_end_angle) == 1)
|
||
{
|
||
g_area[i].hori_end_angle = 0;
|
||
flag = 1;
|
||
}
|
||
if(isnan(g_area[i].hea) == 1)
|
||
{
|
||
g_area[i].hea = 0;
|
||
flag = 1;
|
||
}
|
||
if(isnan(g_area[i].vert_start_angle) == 1)
|
||
{
|
||
g_area[i].vert_start_angle = 0;
|
||
flag = 1;
|
||
}
|
||
if(isnan(g_area[i].vsa) == 1)
|
||
{
|
||
g_area[i].vsa = 0;
|
||
flag = 1;
|
||
}
|
||
if(isnan(g_area[i].vert_end_angle) == 1)
|
||
{
|
||
g_area[i].vert_end_angle = 0;
|
||
flag = 1;
|
||
}
|
||
if(isnan(g_area[i].vea) == 1)
|
||
{
|
||
g_area[i].vea = 0;
|
||
flag = 1;
|
||
}
|
||
if(isnan(g_area[i].hori_step_angle) == 1)
|
||
{
|
||
g_area[i].hori_step_angle = 0;
|
||
flag = 1;
|
||
}
|
||
if(isnan(g_area[i].h_step) == 1)
|
||
{
|
||
g_area[i].h_step = 0;
|
||
flag = 1;
|
||
}
|
||
if(isnan(g_area[i].vert_step_angle) == 1)
|
||
{
|
||
g_area[i].vert_step_angle = 0;
|
||
flag = 1;
|
||
}
|
||
if(isnan(g_area[i].v_step) == 1)
|
||
{
|
||
g_area[i].v_step = 0;
|
||
flag = 1;
|
||
}
|
||
if(isnan(g_area[i].hori_scan_speed) == 1)
|
||
{
|
||
g_area[i].hori_scan_speed = 0;
|
||
flag = 1;
|
||
}
|
||
if(isnan(g_area[i].vert_scan_speed) == 1)
|
||
{
|
||
g_area[i].vert_scan_speed = 0;
|
||
flag = 1;
|
||
}
|
||
if(isnan(g_area[i].step_stop_time) == 1)
|
||
{
|
||
g_area[i].step_stop_time = 0;
|
||
flag = 1;
|
||
}
|
||
if(isnan(g_area[i].scan_mode) == 1)
|
||
{
|
||
g_area[i].scan_mode = 0;
|
||
flag = 1;
|
||
}
|
||
if(isnan(g_area[i].enable) == 1)
|
||
{
|
||
g_area[i].enable = 0;
|
||
flag = 1;
|
||
}
|
||
if(isnan(g_area[i].crc) == 1)
|
||
{
|
||
g_area[i].crc = 0;
|
||
flag = 1;
|
||
}
|
||
|
||
//判断是否需要从新计算效验码
|
||
if(flag == 1)
|
||
{
|
||
flag = 0;
|
||
g_area[i].crc = ptz_area_crc(g_area[i]);//需要重新计算效验码
|
||
}
|
||
|
||
if(i % 10 == 0)
|
||
{
|
||
OSTimeDlyHMSM(0u, 0u, 0u, 1u);
|
||
}
|
||
}
|
||
return 1;
|
||
}
|
||
|
||
|
||
///判断区域扫描区域的数据是否有效
|
||
static char ptz_area_data_check(PtzArea area)
|
||
{
|
||
if(area.crc != ptz_area_crc(area))
|
||
{
|
||
return 0;
|
||
}
|
||
if(area.enable != AREA_ENABLE)
|
||
{
|
||
return 0;
|
||
}
|
||
if(area.hori_start_angle > g_ptz.hori_angleP.angle_allow_max)
|
||
{
|
||
return 0;
|
||
}
|
||
if(area.hori_start_angle < g_ptz.hori_angleP.angle_allow_min)
|
||
{
|
||
return 0;
|
||
}
|
||
if(area.vert_end_angle > g_ptz.vert_angleP.angle_allow_max)
|
||
{
|
||
return 0;
|
||
}
|
||
if(area.vert_end_angle < g_ptz.vert_angleP.angle_allow_min)
|
||
{
|
||
return 0;
|
||
}
|
||
return 1;
|
||
}
|
||
|
||
///区域扫描任务
|
||
static void ptz_area_scan_task()
|
||
{
|
||
short int i;
|
||
float tem;
|
||
while(1)
|
||
{
|
||
switch(g_area_scan.state)
|
||
{
|
||
|
||
///区域扫描启动过程
|
||
case AREA_SCAN_START://启动扫描
|
||
if(g_area_scan.start_num >= AREA_AMOUNT)
|
||
{
|
||
g_area_scan.start_num = AREA_AMOUNT - 1;
|
||
}
|
||
if(g_area_scan.end_num >= AREA_AMOUNT)
|
||
{
|
||
g_area_scan.end_num = AREA_AMOUNT - 1;
|
||
}
|
||
//根据单区域扫描还是多区域扫描，确定扫描顺序
|
||
if(g_area_scan.sin_mul_mode == AREA_MUL_SCAN)
|
||
{
|
||
if(g_area_scan.start_num <= g_area_scan.end_num)
|
||
{
|
||
g_area_scan.order = 1;//由区域编号由小到大扫描
|
||
}
|
||
else
|
||
{
|
||
g_area_scan.order = -1;//由区域编号由大到小扫描
|
||
}
|
||
}
|
||
//总体判断在扫描区域编号范围内，有多少区域满足要求，能够扫描
|
||
g_area_scan.allow_scan_area_num = 0;
|
||
if(g_area_scan.sin_mul_mode == AREA_MUL_SCAN)//如果是多区域扫描
|
||
{
|
||
if(g_area_scan.start_num <= g_area_scan.end_num)
|
||
{
|
||
for(i = g_area_scan.start_num; i <= g_area_scan.end_num; i++)
|
||
{
|
||
if(ptz_area_data_check(g_area[i]) == 1)
|
||
{
|
||
g_area_scan.allow_scan_area_num ++;
|
||
}
|
||
}
|
||
}
|
||
else
|
||
{
|
||
for(i = g_area_scan.start_num; i >= g_area_scan.end_num; i--)
|
||
{
|
||
if(ptz_area_data_check(g_area[i]) == 1)
|
||
{
|
||
g_area_scan.allow_scan_area_num ++;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
else//如果是单区域扫描
|
||
{
|
||
if(ptz_area_data_check(g_area[g_area_scan.start_num]) == 1)
|
||
{
|
||
g_area_scan.allow_scan_area_num ++;
|
||
}
|
||
}
|
||
if(g_area_scan.allow_scan_area_num > 0)
|
||
{//有可以扫描的区域
|
||
g_area_scan.actual_num = g_area_scan.start_num;
|
||
g_area_scan.last_state = g_area_scan.state;
|
||
g_area_scan.state = AREA_SCAN_START + 1;
|
||
g_area_scan.state_a = g_area_scan.state;
|
||
g_area_scan.start_flag = 1;
|
||
}
|
||
else//没有完整可用的区域进行扫描，关闭区域扫描
|
||
{
|
||
g_area_scan.last_state = g_area_scan.state;
|
||
g_area_scan.state = AREA_SCAN_CLOSE_A;
|
||
g_area_scan.state_a = g_area_scan.state;
|
||
}
|
||
break;
|
||
|
||
case AREA_SCAN_START + 1:
|
||
if(g_area_scan.start_flag == 0)
|
||
{//如果不是第一次启动
|
||
if(g_area_scan.sin_mul_mode == AREA_MUL_SCAN)//如果是多区域扫描
|
||
{//直接进入下一个区域
|
||
g_area_scan.actual_num = g_area_scan.actual_num + g_area_scan.order;
|
||
if(g_area_scan.order == 1)
|
||
{
|
||
if(g_area_scan.actual_num > g_area_scan.end_num)
|
||
{
|
||
g_area_scan.actual_num = g_area_scan.start_num;
|
||
}
|
||
}
|
||
else //order == -1
|
||
{
|
||
if(g_area_scan.actual_num < g_area_scan.end_num)
|
||
{
|
||
g_area_scan.actual_num = g_area_scan.start_num;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
else//如果是第一次启动
|
||
{
|
||
g_area_scan.start_flag = 0;
|
||
}
|
||
|
||
//首先判断速度是否在允许范围内
|
||
//提取转速
|
||
g_area_scan.hori_speed = g_area[g_area_scan.actual_num].hori_scan_speed;
|
||
if(g_area_scan.hori_speed < PTZ_HORI_MIN_SPEED)
|
||
{
|
||
g_area_scan.hori_speed = PTZ_HORI_MIN_SPEED;
|
||
}
|
||
if(g_area_scan.hori_speed > PTZ_HORI_MAX_SPEED)
|
||
{
|
||
g_area_scan.hori_speed = PTZ_HORI_MAX_SPEED;
|
||
}
|
||
|
||
g_area_scan.vert_speed = g_area[g_area_scan.actual_num].vert_scan_speed;
|
||
if(g_area_scan.vert_speed < PTZ_VERT_MIN_SPEED)
|
||
{
|
||
g_area_scan.vert_speed = PTZ_VERT_MIN_SPEED;
|
||
}
|
||
if(g_area_scan.vert_speed > PTZ_VERT_MAX_SPEED)
|
||
{
|
||
g_area_scan.vert_speed = PTZ_VERT_MAX_SPEED;
|
||
}
|
||
|
||
//提取转动边界
|
||
g_area_scan.hori_start_angle = g_area[g_area_scan.actual_num].hori_start_angle;
|
||
g_area_scan.hori_end_angle = g_area[g_area_scan.actual_num].hori_end_angle;
|
||
g_area_scan.vert_start_angle = g_area[g_area_scan.actual_num].vert_start_angle;
|
||
g_area_scan.vert_end_angle = g_area[g_area_scan.actual_num].vert_end_angle;
|
||
|
||
//提取扫描模式，判断是单步扫描还是连续扫描
|
||
g_area_scan.scan_mode = g_area[g_area_scan.actual_num].scan_mode;
|
||
|
||
//提取单步时间
|
||
g_area_scan.step_stop_time = g_area[g_area_scan.actual_num].step_stop_time;
|
||
|
||
//提取单步角度
|
||
g_area_scan.hori_step_angle = g_area[g_area_scan.actual_num].hori_step_angle;
|
||
if(g_area_scan.hori_step_angle < 0)
|
||
{//防止水平间隔角度为负数（水平间隔角度永远为正数）
|
||
g_area_scan.hori_step_angle = g_area_scan.hori_step_angle * -1;
|
||
}
|
||
|
||
g_area_scan.vert_step_angle = g_area[g_area_scan.actual_num].vert_step_angle;
|
||
if(g_area_scan.vert_start_angle >= g_area_scan.vert_end_angle)
|
||
{//从上仰到下俯扫描单步角度为负值
|
||
if(g_area_scan.vert_step_angle > 0)
|
||
{
|
||
g_area_scan.vert_step_angle = g_area_scan.vert_step_angle * -1;
|
||
}
|
||
}
|
||
else
|
||
{//从下俯到上仰扫描单步角度为正值
|
||
if(g_area_scan.vert_step_angle < 0)
|
||
{
|
||
g_area_scan.vert_step_angle = g_area_scan.vert_step_angle * -1;
|
||
}
|
||
}
|
||
|
||
//计算水平一层总步数和垂直总步数
|
||
if(g_area_scan.hori_step_angle != 0)
|
||
{//计算水平一层总步数
|
||
//计算水平边界之间的总角度
|
||
if(g_area_scan.hori_start_angle < g_area_scan.hori_end_angle)
|
||
{
|
||
tem = fabs(g_area_scan.hori_end_angle - g_area_scan.hori_start_angle);
|
||
}
|
||
else
|
||
{
|
||
tem = fabs(g_area_scan.hori_end_angle - 0) + fabs(360.0 - g_area_scan.hori_start_angle);
|
||
}
|
||
g_area_scan.hori_step_num_b = tem / g_area_scan.hori_step_angle;
|
||
g_area_scan.hori_step_num_a = (unsigned int)(g_area_scan.hori_step_num_b + 0.5);
|
||
}
|
||
else
|
||
{
|
||
g_area_scan.hori_step_num_a = 0;
|
||
}
|
||
|
||
if(g_area_scan.vert_step_angle != 0)
|
||
{//计算垂直总步数
|
||
tem = fabs(g_area_scan.vert_end_angle - g_area_scan.vert_start_angle);
|
||
g_area_scan.vert_step_num_b = tem / fabs(g_area_scan.vert_step_angle);
|
||
g_area_scan.vert_step_num_a = (unsigned int)(g_area_scan.vert_step_num_b + 0.5);
|
||
}
|
||
else
|
||
{
|
||
g_area_scan.vert_step_num_a = 0;
|
||
}
|
||
|
||
//步数计数清0
|
||
g_area_scan.hori_step_count = 0;
|
||
g_area_scan.vert_step_count = 0;
|
||
|
||
//判断区域数据是否正确
|
||
if(ptz_area_data_check(g_area[g_area_scan.actual_num]) == 1)
|
||
{//区域数据正确，且区域有效，则按照扫描模式进入扫描。
|
||
//首先转动到待扫描区域的起始位置
|
||
g_area_scan.last_state = g_area_scan.state;
|
||
g_area_scan.state = AREA_SCAN_START + 2;
|
||
g_area_scan.state_a = g_area_scan.state;
|
||
}
|
||
else//区域参数不正确，如果是多区域扫描则进入下一个区域，如果是单区域则关闭扫描
|
||
{
|
||
if(g_area_scan.sin_mul_mode == AREA_MUL_SCAN)//如果是多区域扫描
|
||
{//则按照顺序进入下一个区域
|
||
g_area_scan.state = AREA_SCAN_START + 1;
|
||
g_area_scan.state_a = g_area_scan.state;
|
||
}
|
||
else//如果是单区域扫描
|
||
{//直接关闭扫描
|
||
g_area_scan.last_state = g_area_scan.state;
|
||
g_area_scan.state = AREA_SCAN_CLOSE_A;
|
||
g_area_scan.state_a = g_area_scan.state;
|
||
}
|
||
}
|
||
break;
|
||
|
||
case AREA_SCAN_START + 2://转动到待扫描区域的起始位置
|
||
g_area_scan.hori_target_angle = g_area[g_area_scan.actual_num].hori_start_angle;
|
||
g_area_scan.vert_target_angle = g_area[g_area_scan.actual_num].vert_start_angle;
|
||
|
||
g_area_scan.hori_step_count = 0;//步数计数清0
|
||
g_area_scan.vert_step_count = 0;
|
||
g_area_scan.step_stop_time_count = 0;
|
||
g_area_scan.vert_scan_dir = 1;
|
||
g_area_scan.hori_scan_dir = 1;
|
||
|
||
g_ptz.vert_angle_control = g_area_scan.vert_target_angle;
|
||
g_ptz.vert_speed_control = g_area_scan.vert_speed;
|
||
ptz_vert_rotate_plan(PTZ_VERT_ANGLE);
|
||
g_ptz.vert_rotate_monitor_switch = PTZ_VERT_ANGLE;
|
||
|
||
g_ptz.hori_angle_control = g_area_scan.hori_target_angle;
|
||
g_ptz.hori_speed_control = g_area_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_area_scan.last_state = g_area_scan.state;
|
||
g_area_scan.state = AREA_SCAN_START + 3;
|
||
g_area_scan.state_a = g_area_scan.state;
|
||
break;
|
||
|
||
case AREA_SCAN_START + 3://判断是否到达起始位置
|
||
if(g_ptz.hori_arrive_flag == 0 && g_ptz.vert_arrive_flag == 0)
|
||
{//成功到达区域起始位置
|
||
//根据该区域的扫描模式，判断是单步扫描还是连续扫描
|
||
if(g_area_scan.scan_mode == AREA_SCAN_MODE_STEP)
|
||
{//单步扫描模式
|
||
g_area_scan.last_state = g_area_scan.state;
|
||
g_area_scan.state = AREA_SCAN_STEP;
|
||
g_area_scan.state_a = g_area_scan.state;
|
||
}
|
||
else
|
||
{//连续扫描模式
|
||
if(fabs(g_area_scan.hori_start_angle - g_area_scan.hori_end_angle) == 360.0)
|
||
{//360度全范围连续扫描
|
||
g_area_scan.last_state = g_area_scan.state;
|
||
g_area_scan.state = AREA_SCAN_CONT_360;
|
||
g_area_scan.state_a = g_area_scan.state;
|
||
}
|
||
else
|
||
{//连续扇形扫描
|
||
g_area_scan.last_state = g_area_scan.state;
|
||
g_area_scan.state = AREA_SCAN_CONT;
|
||
g_area_scan.state_a = g_area_scan.state;
|
||
}
|
||
}
|
||
|
||
}
|
||
break;
|
||
|
||
|
||
|
||
|
||
|
||
|
||
///单步扫描过程
|
||
case AREA_SCAN_STEP://单步停止时间计时
|
||
//单步扫描到位回传
|
||
ptz_area_step_scan_location_return_return(PTZ_HORI);
|
||
ptz_area_step_scan_location_return_return(PTZ_VERT);
|
||
for(g_area_scan.step_stop_time_count = 0; g_area_scan.step_stop_time_count < g_area_scan.step_stop_time; g_area_scan.step_stop_time_count ++)
|
||
{
|
||
if(g_area_scan.state == AREA_SCAN_STEP)
|
||
{//由于任务优先级不同，因此要加一个这个判断，好实时响应扫描停止、暂停、关闭等指令
|
||
OSTimeDlyHMSM(0u, 0u, 0u, 1u);
|
||
}
|
||
else
|
||
{
|
||
g_area_scan.step_stop_time_count = 0;
|
||
g_area_scan.last_state = g_area_scan.state;
|
||
|
||
break;
|
||
}
|
||
}
|
||
if(g_area_scan.state == AREA_SCAN_STEP)
|
||
{//由于任务优先级不同，因此要加一个这个判断，好实时响应预置位停止、暂停、关闭等指令
|
||
//停止时间到达
|
||
g_area_scan.step_stop_time_count = 0;
|
||
g_area_scan.last_state = g_area_scan.state;
|
||
g_area_scan.state = AREA_SCAN_STEP + 1;
|
||
g_area_scan.state_a = g_area_scan.state;
|
||
}
|
||
break;
|
||
|
||
case AREA_SCAN_STEP + 1://计算下一个需要到达的水平角度角度
|
||
g_area_scan.hori_step_count = g_area_scan.hori_step_count + 1;
|
||
//判断一层是否扫描完成
|
||
if(g_area_scan.hori_step_count > g_area_scan.hori_step_num_a)
|
||
{//如果一层扫描完成
|
||
g_area_scan.hori_step_count = 0;
|
||
g_area_scan.hori_scan_dir = g_area_scan.hori_scan_dir * -1;
|
||
|
||
g_area_scan.last_state = g_area_scan.state;
|
||
g_area_scan.state = AREA_SCAN_STEP + 5;
|
||
g_area_scan.state_a = g_area_scan.state;
|
||
}
|
||
else
|
||
{
|
||
//计算水平下一步的具体角度
|
||
if(g_area_scan.hori_scan_dir == 1)
|
||
{
|
||
if(g_area_scan.hori_step_count == g_area_scan.hori_step_num_a)
|
||
{
|
||
g_area_scan.hori_target_angle = g_area_scan.hori_end_angle;
|
||
}
|
||
else
|
||
{
|
||
g_area_scan.hori_target_angle =
|
||
g_area_scan.hori_start_angle + g_area_scan.hori_step_angle * g_area_scan.hori_step_count;
|
||
if(g_area_scan.hori_target_angle > 360.0)
|
||
{
|
||
g_area_scan.hori_target_angle = g_area_scan.hori_target_angle - 360.0;
|
||
}
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if(g_area_scan.hori_step_count == g_area_scan.hori_step_num_a)
|
||
{
|
||
g_area_scan.hori_target_angle = g_area_scan.hori_start_angle;
|
||
}
|
||
else
|
||
{
|
||
g_area_scan.hori_target_angle =
|
||
g_area_scan.hori_end_angle - g_area_scan.hori_step_angle * g_area_scan.hori_step_count;
|
||
if(g_area_scan.hori_target_angle < 0)
|
||
{
|
||
g_area_scan.hori_target_angle = g_area_scan.hori_target_angle + 360.0;
|
||
}
|
||
}
|
||
}
|
||
|
||
g_ptz.hori_angle_control = g_area_scan.hori_target_angle;
|
||
g_ptz.hori_speed_control = g_area_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_area_scan.last_state = g_area_scan.state;
|
||
g_area_scan.state = AREA_SCAN_STEP + 2;
|
||
g_area_scan.state_a = g_area_scan.state;
|
||
}
|
||
break;
|
||
|
||
case AREA_SCAN_STEP + 2://判断是否到达指定的位置
|
||
if(g_ptz.hori_arrive_flag == 0 && g_ptz.vert_arrive_flag == 0)
|
||
{
|
||
g_area_scan.last_state = g_area_scan.state;
|
||
g_area_scan.state = AREA_SCAN_STEP;
|
||
g_area_scan.state_a = g_area_scan.state;
|
||
}
|
||
break;
|
||
|
||
/***单区域扫描时进行策略修改，使云台转动范围永远只在扫描区域循环转动***/
|
||
case AREA_SCAN_STEP + 5://向下一个水平层转动
|
||
g_area_scan.vert_step_count = g_area_scan.vert_step_count + 1;
|
||
//判断整个区域是否扫描结束
|
||
if(g_area_scan.vert_step_count > g_area_scan.vert_step_num_a)
|
||
{//整个区域扫描结束
|
||
if(g_area_scan.sin_mul_mode == AREA_SIN_SCAN)
|
||
{//如果是单区域扫描，则继续循环
|
||
ptz_area_scan_end_return_return();//通知控制端一个区域扫描结束
|
||
g_area_scan.hori_step_count = 0; //步数清零
|
||
g_area_scan.vert_step_count = 0;
|
||
//转至垂直起始位
|
||
g_ptz.vert_angle_control = g_area_scan.vert_start_angle;
|
||
g_ptz.vert_speed_control = g_area_scan.vert_speed;
|
||
ptz_vert_rotate_plan(PTZ_VERT_ANGLE);
|
||
g_ptz.vert_rotate_monitor_switch = PTZ_VERT_ANGLE;
|
||
|
||
g_ptz.vert_arrive_flag = 1;//用于判断云台是否转动到了指定角度
|
||
|
||
g_area_scan.last_state = g_area_scan.state;
|
||
g_area_scan.state = AREA_SCAN_STEP + 6;//判断是否到达指定垂直位置
|
||
g_area_scan.state_a = g_area_scan.state;
|
||
}else{
|
||
ptz_area_scan_end_return_return();//通知控制端一个区域扫描结束
|
||
g_area_scan.vert_step_count = 0;
|
||
g_area_scan.last_state = g_area_scan.state;
|
||
g_area_scan.state = AREA_SCAN_START + 1;//进入下一个扫描区域
|
||
g_area_scan.state_a = g_area_scan.state;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
//计算水平下一步的具体角度
|
||
g_area_scan.vert_target_angle =
|
||
g_area_scan.vert_start_angle + g_area_scan.vert_step_angle * g_area_scan.vert_step_count;
|
||
if(g_area_scan.vert_step_count == g_area_scan.vert_step_num_a)
|
||
{
|
||
g_area_scan.vert_target_angle = g_area_scan.vert_end_angle;
|
||
}
|
||
g_ptz.vert_angle_control = g_area_scan.vert_target_angle;
|
||
g_ptz.vert_speed_control = g_area_scan.vert_speed;
|
||
ptz_vert_rotate_plan(PTZ_VERT_ANGLE);
|
||
g_ptz.vert_rotate_monitor_switch = PTZ_VERT_ANGLE;
|
||
|
||
g_ptz.vert_arrive_flag = 1;//用于判断云台是否转动到了指定角度
|
||
|
||
g_area_scan.last_state = g_area_scan.state;
|
||
g_area_scan.state = AREA_SCAN_STEP + 6;//判断是否到达指定垂直位置
|
||
g_area_scan.state_a = g_area_scan.state;
|
||
}
|
||
break;
|
||
|
||
case AREA_SCAN_STEP + 6://判断是否到达指定垂直位置
|
||
if(g_ptz.hori_arrive_flag == 0 && g_ptz.vert_arrive_flag == 0)
|
||
{
|
||
g_area_scan.last_state = g_area_scan.state;
|
||
g_area_scan.state = AREA_SCAN_STEP;
|
||
g_area_scan.state_a = g_area_scan.state;
|
||
}
|
||
break;
|
||
|
||
|
||
|
||
|
||
|
||
|
||
///连续扫描过程
|
||
case AREA_SCAN_CONT://到达起始位置，计算下一个需要到达的水平位置，
|
||
//即从水平起始边界转动到水平结束边界
|
||
//或者从水平结束位置转动到水平起始位置
|
||
//计算水平下一步的具体角度
|
||
if(g_area_scan.hori_scan_dir == 1)
|
||
{
|
||
g_area_scan.hori_target_angle = g_area_scan.hori_end_angle;
|
||
|
||
g_ptz.hori_angle_control = g_area_scan.hori_target_angle;
|
||
g_ptz.hori_speed_control = g_area_scan.hori_speed;
|
||
ptz_hori_rotate_plan(PTZ_HORI_RIGHT_ANGLE);
|
||
g_ptz.hori_rotate_monitor_switch = PTZ_HORI_RIGHT_ANGLE;
|
||
}
|
||
else
|
||
{
|
||
g_area_scan.hori_target_angle = g_area_scan.hori_start_angle;
|
||
|
||
g_ptz.hori_angle_control = g_area_scan.hori_target_angle;
|
||
g_ptz.hori_speed_control = g_area_scan.hori_speed;
|
||
ptz_hori_rotate_plan(PTZ_HORI_LEFT_ANGLE);
|
||
g_ptz.hori_rotate_monitor_switch = PTZ_HORI_LEFT_ANGLE;
|
||
}
|
||
|
||
g_area_scan.hori_scan_dir = g_area_scan.hori_scan_dir * -1;
|
||
|
||
g_ptz.hori_arrive_flag = 1;//用于判断云台是否转动到了指定角度
|
||
|
||
g_area_scan.last_state = g_area_scan.state;
|
||
g_area_scan.state = AREA_SCAN_CONT + 2;
|
||
g_area_scan.state_a = g_area_scan.state;
|
||
break;
|
||
|
||
case AREA_SCAN_CONT + 2://判断是否转动到指定的水平边界
|
||
if(g_ptz.hori_arrive_flag == 0 && g_ptz.vert_arrive_flag == 0)
|
||
{
|
||
g_area_scan.last_state = g_area_scan.state;
|
||
g_area_scan.state = AREA_SCAN_CONT + 3;
|
||
g_area_scan.state_a = g_area_scan.state;
|
||
}
|
||
break;
|
||
|
||
/***单区域扫描时进行策略修改，使云台转动范围永远只在扫描区域循环转动***/
|
||
case AREA_SCAN_CONT + 3://向下一个垂直层转动
|
||
g_area_scan.vert_step_count = g_area_scan.vert_step_count + 1;
|
||
//判断整个区域是否扫描结束
|
||
if(g_area_scan.vert_step_count > g_area_scan.vert_step_num_a)
|
||
{//整个区域扫描结束
|
||
if(g_area_scan.sin_mul_mode == AREA_SIN_SCAN)
|
||
{//如果是单区域扫描，则继续循环
|
||
ptz_area_scan_end_return_return();//通知控制端一个区域扫描结束
|
||
g_area_scan.vert_step_count = 0;
|
||
//转至垂直起始位
|
||
g_ptz.vert_angle_control = g_area_scan.vert_start_angle;
|
||
g_ptz.vert_speed_control = g_area_scan.vert_speed;
|
||
ptz_vert_rotate_plan(PTZ_VERT_ANGLE);
|
||
g_ptz.vert_rotate_monitor_switch = PTZ_VERT_ANGLE;
|
||
|
||
g_ptz.vert_arrive_flag = 1;//用于判断云台是否转动到了指定角度
|
||
|
||
g_area_scan.last_state = g_area_scan.state;
|
||
g_area_scan.state = AREA_SCAN_CONT + 4;//判断是否到达指定垂直位置
|
||
g_area_scan.state_a = g_area_scan.state;
|
||
}else{
|
||
ptz_area_scan_end_return_return();//通知控制端一个区域扫描结束
|
||
g_area_scan.vert_step_count = 0;
|
||
g_area_scan.last_state = g_area_scan.state;
|
||
g_area_scan.state = AREA_SCAN_START + 1;//进入下一个扫描区域
|
||
g_area_scan.state_a = g_area_scan.state;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
//计算水平下一步的具体角度
|
||
g_area_scan.vert_target_angle =
|
||
g_area_scan.vert_start_angle + g_area_scan.vert_step_angle * g_area_scan.vert_step_count;
|
||
if(g_area_scan.vert_step_count == g_area_scan.vert_step_num_a)
|
||
{
|
||
g_area_scan.vert_target_angle = g_area_scan.vert_end_angle;
|
||
}
|
||
g_ptz.vert_angle_control = g_area_scan.vert_target_angle;
|
||
g_ptz.vert_speed_control = g_area_scan.vert_speed;
|
||
ptz_vert_rotate_plan(PTZ_VERT_ANGLE);
|
||
g_ptz.vert_rotate_monitor_switch = PTZ_VERT_ANGLE;
|
||
|
||
g_ptz.vert_arrive_flag = 1;//用于判断云台是否转动到了指定角度
|
||
|
||
g_area_scan.last_state = g_area_scan.state;
|
||
g_area_scan.state = AREA_SCAN_CONT + 4;//判断是否到达指定垂直位置
|
||
g_area_scan.state_a = g_area_scan.state;
|
||
}
|
||
break;
|
||
|
||
case AREA_SCAN_CONT + 4://判断是否到达指定垂直位置
|
||
if(g_ptz.hori_arrive_flag == 0 && g_ptz.vert_arrive_flag == 0)
|
||
{
|
||
g_area_scan.last_state = g_area_scan.state;
|
||
g_area_scan.state = AREA_SCAN_CONT;
|
||
g_area_scan.state_a = g_area_scan.state;
|
||
}
|
||
break;
|
||
|
||
|
||
|
||
|
||
|
||
///360度全景连续扫描
|
||
case AREA_SCAN_CONT_360://360度全景扫描首次启动
|
||
g_ptz.hori_speed_control = g_area_scan.hori_speed;
|
||
//判断转动方向
|
||
if(g_area_scan.hori_start_angle <= g_area_scan.hori_end_angle)
|
||
{//右转扫描
|
||
g_area_scan.hori_scan_dir_360 = PTZ_HORI_DIR_RIGHT;
|
||
}
|
||
else
|
||
{//左转扫描
|
||
g_area_scan.hori_scan_dir_360 = PTZ_HORI_DIR_LEFT;
|
||
}
|
||
//开启水平转动
|
||
if(g_area_scan.hori_scan_dir_360 == PTZ_HORI_DIR_RIGHT)
|
||
{
|
||
ptz_hori_rotate_plan(PTZ_HORI_RIGHT_KEEP);
|
||
g_ptz.hori_rotate_monitor_switch = PTZ_HORI_RIGHT_KEEP;
|
||
}
|
||
else
|
||
{
|
||
ptz_hori_rotate_plan(PTZ_HORI_LEFT_KEEP);
|
||
g_ptz.hori_rotate_monitor_switch = PTZ_HORI_LEFT_KEEP;
|
||
}
|
||
g_area_scan.last_state = g_area_scan.state;
|
||
g_area_scan.state = AREA_SCAN_CONT_360 + 1;
|
||
g_area_scan.state_a = g_area_scan.state;
|
||
break;
|
||
|
||
case AREA_SCAN_CONT_360 + 1://判断一层转动起始位置和结束位置
|
||
//计算起始角度
|
||
g_area_scan.hori_start_angle_360 = g_ptz.hori_angle_actual;
|
||
//计算结束角度
|
||
if(g_area_scan.hori_scan_dir_360 == PTZ_HORI_DIR_RIGHT)
|
||
{
|
||
g_area_scan.hori_end_angle_360 = g_area_scan.hori_start_angle_360 + AREA_SCAN_END_ANGLE_INC_360;
|
||
if(g_area_scan.hori_end_angle_360 >= 360.0)
|
||
{
|
||
g_area_scan.hori_end_angle_360 = g_area_scan.hori_end_angle_360 - 360.0;
|
||
}
|
||
}
|
||
else//g_area_scan.hori_scan_dir_360 = PTZ_HORI_DIR_LEFT
|
||
{
|
||
g_area_scan.hori_end_angle_360 = g_area_scan.hori_start_angle_360 - AREA_SCAN_END_ANGLE_INC_360;
|
||
if(g_area_scan.hori_end_angle_360 < 0)
|
||
{
|
||
g_area_scan.hori_end_angle_360 = g_area_scan.hori_end_angle_360 + 360.0;
|
||
}
|
||
}
|
||
g_area_scan.last_state = g_area_scan.state;
|
||
g_area_scan.state = AREA_SCAN_CONT_360 + 2;
|
||
g_area_scan.state_a = g_area_scan.state;
|
||
break;
|
||
|
||
case AREA_SCAN_CONT_360 + 2://判断一层是否扫描完成一半
|
||
if(g_area_scan.hori_scan_dir_360 == PTZ_HORI_DIR_RIGHT)
|
||
{
|
||
if(g_area_scan.hori_start_angle_360 <= g_ptz.hori_angle_actual)
|
||
{
|
||
if(fabs(g_ptz.hori_angle_actual - g_area_scan.hori_start_angle_360) >= 180.0 &&
|
||
fabs(360.0 - g_ptz.hori_angle_actual + g_area_scan.hori_start_angle_360) > 50.0)
|
||
{
|
||
g_area_scan.last_state = g_area_scan.state;
|
||
g_area_scan.state = AREA_SCAN_CONT_360 + 3;
|
||
g_area_scan.state_a = g_area_scan.state;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if(fabs(360.0 - g_area_scan.hori_start_angle_360 + g_ptz.hori_angle_actual) >= 180.0 &&
|
||
fabs(g_area_scan.hori_start_angle_360 - g_ptz.hori_angle_actual) >= 50.0)
|
||
{
|
||
g_area_scan.last_state = g_area_scan.state;
|
||
g_area_scan.state = AREA_SCAN_CONT_360 + 3;
|
||
g_area_scan.state_a = g_area_scan.state;
|
||
}
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if(g_area_scan.hori_start_angle_360 >= g_ptz.hori_angle_actual)
|
||
{
|
||
if(fabs(g_area_scan.hori_start_angle_360 - g_ptz.hori_angle_actual) >= 180.0 &&
|
||
fabs(360.0 - g_area_scan.hori_start_angle_360 + g_ptz.hori_angle_actual) >= 50.0)
|
||
{
|
||
g_area_scan.last_state = g_area_scan.state;
|
||
g_area_scan.state = AREA_SCAN_CONT_360 + 3;
|
||
g_area_scan.state_a = g_area_scan.state;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if(fabs(360.0 - g_ptz.hori_angle_actual + g_area_scan.hori_start_angle_360) >= 180.0 &&
|
||
fabs(g_ptz.hori_angle_actual - g_area_scan.hori_start_angle_360) >= 50.0)
|
||
{
|
||
g_area_scan.last_state = g_area_scan.state;
|
||
g_area_scan.state = AREA_SCAN_CONT_360 + 3;
|
||
g_area_scan.state_a = g_area_scan.state;
|
||
}
|
||
}
|
||
}
|
||
break;
|
||
|
||
case AREA_SCAN_CONT_360 + 3://判断一层剩下的一层是否扫描完成，即判断一整层是否扫描完成
|
||
//判断一层剩下的一层是否扫描完成的原理和转动控制定位的原理是相同的
|
||
//计算最近结束点距离
|
||
g_area_scan.hori_near_angle_360 =
|
||
fabs(g_ptz.hori_angle_actual - g_area_scan.hori_start_angle_360);
|
||
if(g_area_scan.hori_near_angle_360 > 360.0 / 2.0)
|
||
{
|
||
g_area_scan.hori_near_angle_360 = 360.0 - g_area_scan.hori_near_angle_360;
|
||
}
|
||
//计算最远结束点的距离
|
||
g_area_scan.hori_far_angle_360 =
|
||
fabs(g_ptz.hori_angle_actual - g_area_scan.hori_start_angle_360);
|
||
if(g_area_scan.hori_far_angle_360 > 360.0 / 2.0)
|
||
{
|
||
g_area_scan.hori_far_angle_360 = 360.0 - g_area_scan.hori_far_angle_360;
|
||
}
|
||
//判断一层扫描是否结束
|
||
if(g_area_scan.hori_far_angle_360 + g_area_scan.hori_near_angle_360 <=
|
||
AREA_SCAN_END_ANGLE_INC_360)
|
||
{//一层扫描结束
|
||
g_area_scan.hori_far_angle_360 = 0;
|
||
g_area_scan.hori_near_angle_360 = 0;
|
||
g_area_scan.hori_end_angle_360 = 0;
|
||
g_area_scan.last_state = g_area_scan.state;
|
||
g_area_scan.state = AREA_SCAN_CONT_360 + 4;
|
||
g_area_scan.state_a = g_area_scan.state;
|
||
}
|
||
break;
|
||
|
||
case AREA_SCAN_CONT_360 + 4://规划转动到下一个垂直层
|
||
g_area_scan.vert_step_count = g_area_scan.vert_step_count + 1;
|
||
//判断整个区域是否扫描结束
|
||
if(g_area_scan.vert_step_count > g_area_scan.vert_step_num_a)
|
||
{//整个区域扫描结束
|
||
|
||
g_area_scan.vert_step_count = 0;
|
||
|
||
if(g_area_scan.sin_mul_mode == AREA_MUL_SCAN)
|
||
{//如果是多区域扫描
|
||
ptz_area_scan_end_return_return();//通知控制端一个区域扫描结束
|
||
g_area_scan.last_state = g_area_scan.state;
|
||
g_area_scan.state = AREA_SCAN_START + 1;//进入下一个扫描区域
|
||
g_area_scan.state_a = g_area_scan.state;
|
||
}
|
||
else
|
||
{//如果是单区域扫描g_area_scan.sin_mul_mode == AREA_SIN_SCAN
|
||
//则直接回到垂直起始层，水平转动状态不改变
|
||
ptz_area_scan_end_return_return();//通知控制端一个区域扫描结束
|
||
g_area_scan.vert_target_angle = g_area_scan.vert_start_angle;
|
||
g_ptz.vert_angle_control = g_area_scan.vert_target_angle;
|
||
g_ptz.vert_speed_control = g_area_scan.vert_speed;
|
||
ptz_vert_rotate_plan(PTZ_VERT_ANGLE);
|
||
g_ptz.vert_rotate_monitor_switch = PTZ_VERT_ANGLE;
|
||
|
||
g_ptz.vert_arrive_flag = 1;//用于判断云台是否转动到了指定角度
|
||
|
||
g_area_scan.last_state = g_area_scan.state;
|
||
g_area_scan.state = AREA_SCAN_CONT_360 + 5;//判断是否到达指定垂直位置
|
||
g_area_scan.state_a = g_area_scan.state;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
//计算水平下一步的具体角度
|
||
g_area_scan.vert_target_angle =
|
||
g_area_scan.vert_start_angle + g_area_scan.vert_step_angle * g_area_scan.vert_step_count;
|
||
if(g_area_scan.vert_step_count == g_area_scan.vert_step_num_a)
|
||
{
|
||
g_area_scan.vert_target_angle = g_area_scan.vert_end_angle;
|
||
}
|
||
g_ptz.vert_angle_control = g_area_scan.vert_target_angle;
|
||
g_ptz.vert_speed_control = g_area_scan.vert_speed;
|
||
ptz_vert_rotate_plan(PTZ_VERT_ANGLE);
|
||
g_ptz.vert_rotate_monitor_switch = PTZ_VERT_ANGLE;
|
||
|
||
g_ptz.vert_arrive_flag = 1;//用于判断云台是否转动到了指定角度
|
||
|
||
g_area_scan.last_state = g_area_scan.state;
|
||
g_area_scan.state = AREA_SCAN_CONT_360 + 5;//判断是否到达指定垂直位置
|
||
g_area_scan.state_a = g_area_scan.state;
|
||
}
|
||
break;
|
||
|
||
case AREA_SCAN_CONT_360 + 5://判断是否到达指定垂直位置
|
||
if(g_ptz.vert_arrive_flag == 0)
|
||
{
|
||
g_area_scan.last_state = g_area_scan.state;
|
||
g_area_scan.state = AREA_SCAN_CONT_360 + 1;
|
||
g_area_scan.state_a = g_area_scan.state;
|
||
}
|
||
break;
|
||
|
||
|
||
|
||
|
||
|
||
//区域扫描其他功能
|
||
|
||
//暂停扫描
|
||
case AREA_SCAN_PAUSE_A://区域扫描暂停
|
||
//首先保存所有参数
|
||
g_area_scan.pause_state = g_area_scan.state_a;
|
||
g_area_scan.pause_hori_angle = g_ptz.hori_angle_actual;
|
||
g_area_scan.pause_vert_angle = g_ptz.vert_angle_actual;
|
||
g_area_scan.pause_hori_arrive_flag = g_ptz.hori_arrive_flag;
|
||
g_area_scan.pause_vert_arrive_flag = g_ptz.vert_arrive_flag;
|
||
g_area_scan.pause_hori_start_stop = g_ptz.hori_start_stop_set;
|
||
g_area_scan.pause_vert_start_stop = g_ptz.vert_start_stop_set;
|
||
g_area_scan.pause_hori_direction = g_ptz.hori_direction_set;
|
||
g_area_scan.pause_vert_direction = g_ptz.vert_direction_set;
|
||
//停止云台转动
|
||
g_ptz.vert_rotate_monitor_switch = PTZ_VERT_DEC_BRAKE_A;
|
||
g_ptz.hori_rotate_monitor_switch = PTZ_HORI_DEC_BRAKE_A;
|
||
g_area_scan.last_state = g_area_scan.state;
|
||
g_area_scan.state = AREA_SCAN_PAUSE_B;
|
||
g_area_scan.state_a = g_area_scan.state;
|
||
break;
|
||
|
||
case AREA_SCAN_PAUSE_B://区域扫描暂停中
|
||
|
||
break;
|
||
|
||
|
||
|
||
|
||
//恢复扫描
|
||
case AREA_SCAN_RECOVERY://区域扫描暂停恢复
|
||
//首先规划转动到被打断的位置
|
||
g_ptz.vert_angle_control = g_area_scan.pause_vert_angle;
|
||
g_ptz.vert_speed_control = g_area_scan.vert_speed;
|
||
ptz_vert_rotate_plan(PTZ_VERT_ANGLE);
|
||
g_ptz.vert_rotate_monitor_switch = PTZ_VERT_ANGLE;
|
||
|
||
g_ptz.hori_angle_control = g_area_scan.pause_hori_angle;
|
||
g_ptz.hori_speed_control = g_area_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_area_scan.last_state = g_area_scan.state;
|
||
g_area_scan.state = AREA_SCAN_RECOVERY + 1;
|
||
g_area_scan.state_a = g_area_scan.state;
|
||
break;
|
||
|
||
case AREA_SCAN_RECOVERY + 1://到达暂停时的位置
|
||
//恢复云台扫描暂停前的状态//g_area_scan.vert_target_angle
|
||
if(g_ptz.hori_arrive_flag == 0 && g_ptz.vert_arrive_flag == 0)
|
||
{
|
||
if(g_area_scan.pause_hori_arrive_flag == 1)
|
||
{//如果是在向指定位置转动
|
||
g_ptz.hori_angle_control = g_area_scan.hori_target_angle;
|
||
g_ptz.hori_speed_control = g_area_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;
|
||
}
|
||
else
|
||
{//如果不是向指定位置转动
|
||
if(g_area_scan.pause_hori_start_stop == PTZ_HORI_START)
|
||
{//但是却又是一直处于转动状态,主要是针对360度连续扫描
|
||
if(g_area_scan.pause_hori_direction == PTZ_HORI_DIR_RIGHT)
|
||
{
|
||
ptz_hori_rotate_plan(PTZ_HORI_RIGHT_KEEP);
|
||
g_ptz.hori_rotate_monitor_switch = PTZ_HORI_RIGHT_KEEP;
|
||
}
|
||
else
|
||
{
|
||
ptz_hori_rotate_plan(PTZ_HORI_LEFT_KEEP);
|
||
g_ptz.hori_rotate_monitor_switch = PTZ_HORI_LEFT_KEEP;
|
||
}
|
||
}
|
||
}
|
||
|
||
if(g_area_scan.pause_vert_arrive_flag == 1)
|
||
{//如果是在向指定位置转动
|
||
g_ptz.vert_angle_control = g_area_scan.vert_target_angle;
|
||
g_ptz.vert_speed_control = g_area_scan.vert_speed;
|
||
ptz_vert_rotate_plan(PTZ_VERT_ANGLE);
|
||
g_ptz.vert_rotate_monitor_switch = PTZ_VERT_ANGLE;
|
||
g_ptz.vert_arrive_flag = 1;
|
||
}
|
||
//恢复之前扫描状态
|
||
g_area_scan.last_state = g_area_scan.state;
|
||
g_area_scan.state = g_area_scan.pause_state;
|
||
g_area_scan.state_a = g_area_scan.state;
|
||
}
|
||
break;
|
||
|
||
|
||
|
||
//关闭扫描
|
||
case AREA_SCAN_CLOSE_A: //关闭区域扫描
|
||
memset(&g_area_scan, 0, sizeof(g_area_scan));
|
||
g_ptz.vert_rotate_monitor_switch = PTZ_VERT_DEC_BRAKE_A;
|
||
g_ptz.hori_rotate_monitor_switch = PTZ_HORI_DEC_BRAKE_A;
|
||
g_area_scan.last_state = g_area_scan.state;
|
||
g_area_scan.state = AREA_SCAN_CLOSE_B;
|
||
g_area_scan.state_a = g_area_scan.state;
|
||
break;
|
||
|
||
case AREA_SCAN_CLOSE_B://区域扫描关闭中
|
||
|
||
break;
|
||
}
|
||
|
||
OSTimeDlyHMSM(0u, 0u, 0u, 10u);
|
||
|
||
}
|
||
}
|
||
|
||
|
||
///区域扫描区域结束回传数据保存
|
||
void ptz_area_scan_end_return_save(char dev)
|
||
{
|
||
if(dev == PTZ_UDP)
|
||
{
|
||
g_ptz.area_scan_return.area_scan_end_fromlen = ptz_fromlen;
|
||
memcpy(&g_ptz.area_scan_return.area_scan_end_from, &ptz_from, ptz_fromlen);
|
||
}
|
||
}
|
||
///区域扫描区域结束回复
|
||
void ptz_area_scan_end_return_return()
|
||
{
|
||
unsigned char area[7] = {0xff,0x00,0x00,0x00,0x00,0x00,0x00};
|
||
area[1] = g_ptz.address;
|
||
area[2] = 0xc4;
|
||
area[3] = AREA_SCAN_END_RETURN_DATA;//表示回传
|
||
area[4] = g_area_scan.actual_num;//当前扫描完成的区域
|
||
area[6] = MotorCalPelcoDSUM(area,sizeof(area));
|
||
|
||
if(g_ptz.area_scan_return.area_scan_end_udp_switch == AREA_SCAN_RETURN_ON)
|
||
{
|
||
//发送数据
|
||
send_udp_data_aim(area, sizeof(area),
|
||
(struct sockaddr*)&g_ptz.area_scan_return.area_scan_end_from, g_ptz.area_scan_return.area_scan_end_fromlen);
|
||
|
||
}
|
||
|
||
if(g_ptz.area_scan_return.area_scan_end_uart_422_switch == AREA_SCAN_RETURN_ON)
|
||
{
|
||
ptz_send_data(PTZ_UART_422, area, sizeof(area));
|
||
}
|
||
|
||
if(g_ptz.area_scan_return.area_scan_end_uart_485_switch == AREA_SCAN_RETURN_ON)
|
||
{
|
||
ptz_send_data(PTZ_UART_485, area, sizeof(area));
|
||
}
|
||
}
|
||
|
||
|
||
///单步扫描到位回传数据保存
|
||
void ptz_area_step_scan_location_return_save(char dev)
|
||
{
|
||
if(dev == PTZ_UDP)
|
||
{
|
||
g_ptz.area_scan_return.area_step_scan_location_fromlen = ptz_fromlen;
|
||
memcpy(&g_ptz.area_scan_return.area_step_scan_location_from, &ptz_from, ptz_fromlen);
|
||
}
|
||
}
|
||
|
||
///单步扫描到位回传
|
||
void ptz_area_step_scan_location_return_return(char angle_choice)
|
||
{
|
||
unsigned short int uint16_angle = 0;
|
||
unsigned char area[7] = {0xff,0x00,0x00,0x00,0x00,0x00,0x00};
|
||
|
||
area[1] = g_ptz.address;
|
||
area[2] = 0xc4;
|
||
if(angle_choice == PTZ_HORI)
|
||
{
|
||
uint16_angle = (unsigned short int)(g_area_scan.hori_target_angle * 100 + 0.5);
|
||
area[3] = AREA_STEP_SCAN_LOCATION_RETURN_HORI;//表示回传
|
||
area[4] = (u_int16_t)(uint16_angle >> 8);
|
||
area[5] = (u_int16_t)(uint16_angle & 0x00ff);
|
||
area[6] = MotorCalPelcoDSUM(area,sizeof(area));
|
||
}
|
||
else
|
||
{
|
||
uint16_angle = (unsigned short int)(g_area_scan.vert_target_angle * 100 + 0.5);
|
||
area[3] = AREA_STEP_SCAN_LOCATION_RETURN_VERT;//表示回传
|
||
area[4] = (u_int16_t)(uint16_angle >> 8);
|
||
area[5] = (u_int16_t)(uint16_angle & 0x00ff);
|
||
area[6] = MotorCalPelcoDSUM(area,sizeof(area));
|
||
}
|
||
|
||
if(g_ptz.area_scan_return.area_step_scan_location_udp_switch == AREA_SCAN_RETURN_ON)
|
||
{
|
||
//发送数据
|
||
send_udp_data_aim(area, sizeof(area),
|
||
(struct sockaddr*)&g_ptz.area_scan_return.area_step_scan_location_from, g_ptz.area_scan_return.area_step_scan_location_fromlen);
|
||
}
|
||
|
||
if(g_ptz.area_scan_return.area_step_scan_location_uart_422_switch == AREA_SCAN_RETURN_ON)
|
||
{
|
||
ptz_send_data(PTZ_UART_422, area, sizeof(area));
|
||
}
|
||
|
||
if(g_ptz.area_scan_return.area_step_scan_location_uart_485_switch == AREA_SCAN_RETURN_ON)
|
||
{
|
||
ptz_send_data(PTZ_UART_485, area, sizeof(area));
|
||
}
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
static OS_STK task_area_scan_stk[TASK_AREA_SCAN_STK_SIZE];
|
||
static void creat_task_area_scan(void)
|
||
{
|
||
CPU_INT08U task_err;
|
||
CPU_INT08U name_err;
|
||
|
||
task_err = OSTaskCreateExt((void (*)(void *)) ptz_area_scan_task,
|
||
(void *) 0,
|
||
(OS_STK *)&task_area_scan_stk[TASK_AREA_SCAN_STK_SIZE - 1],
|
||
(INT8U ) TASK_AREA_SCAN_PRIO,
|
||
(INT16U ) TASK_AREA_SCAN_PRIO,
|
||
(OS_STK *)&task_area_scan_stk[0],
|
||
(INT32U ) TASK_AREA_SCAN_STK_SIZE,
|
||
(void *) 0,
|
||
(INT16U )(OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR));
|
||
#if (OS_TASK_NAME_EN > 0)
|
||
OSTaskNameSet(TASK_AREA_SCAN_PRIO, "ptz_area_scan_task", &name_err);
|
||
#endif
|
||
if ((task_err == OS_ERR_NONE) && (name_err == OS_ERR_NONE)) {
|
||
pdebug(DEBUG_LEVEL_INFO,"create ptz_area_scan_task success...\n\r");
|
||
} else {
|
||
pdebug(DEBUG_LEVEL_FATAL,"create ptz_area_scan_task failed...\n\r");
|
||
}
|
||
|
||
}
|
||
|
||
void init_area_scan_module()
|
||
{
|
||
ptz_area_read();
|
||
creat_task_area_scan();
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|