pengsx
/
MW22-02A
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
MW22-02A/APP/Device/Device_rotate/rotate_plan.c

570 lines
19 KiB
C
Raw Permalink Normal View History

汇源原始工程
2025-05-21 01:55:40 +00:00
#include "ptz_header_file.h"
//#include "rotate_plan.h"
///<2F><>̨<EFBFBD><CCA8>λ<EFBFBD>ش<EFBFBD><D8B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ظ<EFBFBD>
void ptz_location_return_return(char hori_vert)
{
if(g_ptz.location_return.net_udp_location_switch == LOCATION_RETURN_ON)
{//<2F><><EFBFBD><EFBFBD><EFBFBD>ǵ<EFBFBD><C7B5><EFBFBD>ָ<EFBFBD><D6B8>λ<EFBFBD><CEBB><EFBFBD><EFBFBD>Ҫ<EFBFBD>ش<EFBFBD><D8B4><EFBFBD><EFBFBD><EFBFBD>
if(hori_vert == LOCATION_HORI && g_ptz.location_return.hori_cmd == LOCATION_UDP_CMD_ON)
{//ת<><D7AA><EFBFBD><EFBFBD>ˮƽλ<C6BD>ûظ<C3BB>
g_ptz.location_return.hori_angle[2] = 0xc5;//<2F>޸<EFBFBD><DEB8><EFBFBD><EFBFBD><EFBFBD>
g_ptz.location_return.hori_angle[3] = LOCATION_HORI;//<2F>ش<EFBFBD>ˮƽλ<C6BD>õ<EFBFBD><C3B5><EFBFBD>
g_ptz.location_return.hori_angle[6] =
MotorCalPelcoDSUM(g_ptz.location_return.hori_angle,sizeof(g_ptz.location_return.hori_angle));
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
send_udp_data_aim(g_ptz.location_return.hori_angle, sizeof(g_ptz.location_return.hori_angle),
(struct sockaddr*)&g_ptz.location_return.hori_cmd_from, g_ptz.location_return.hori_cmd_fromlen);
g_ptz.location_return.hori_cmd = LOCATION_CMD_OFF;
}
if(hori_vert == LOCATION_VERT && g_ptz.location_return.vert_cmd == LOCATION_UDP_CMD_ON)
{//ת<><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֱλ<D6B1>ûظ<C3BB>
g_ptz.location_return.vert_angle[2] = 0xc5;//<2F>޸<EFBFBD><DEB8><EFBFBD><EFBFBD><EFBFBD>
g_ptz.location_return.vert_angle[3] = LOCATION_VERT;//<2F>ش<EFBFBD><D8B4><EFBFBD>ֱλ<D6B1>õ<EFBFBD><C3B5><EFBFBD>
g_ptz.location_return.vert_angle[6] =
MotorCalPelcoDSUM(g_ptz.location_return.vert_angle,sizeof(g_ptz.location_return.vert_angle));
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
send_udp_data_aim(g_ptz.location_return.vert_angle, sizeof(g_ptz.location_return.vert_angle),
(struct sockaddr*)&g_ptz.location_return.vert_cmd_from, g_ptz.location_return.vert_cmd_fromlen);
g_ptz.location_return.vert_cmd = LOCATION_CMD_OFF;
}
}
if(g_ptz.location_return.uart_422_location_switch == LOCATION_RETURN_ON)
{//<2F><><EFBFBD><EFBFBD><EFBFBD>ǵ<EFBFBD><C7B5><EFBFBD>ָ<EFBFBD><D6B8>λ<EFBFBD><CEBB><EFBFBD><EFBFBD>Ҫ<EFBFBD>ش<EFBFBD><D8B4><EFBFBD><EFBFBD><EFBFBD>
if(hori_vert == LOCATION_HORI && g_ptz.location_return.hori_cmd == LOCATION_UART_422_CMD_ON)
{//ת<><D7AA><EFBFBD><EFBFBD>ˮƽλ<C6BD>ûظ<C3BB>
g_ptz.location_return.hori_angle[2] = 0xc5;//<2F>޸<EFBFBD><DEB8><EFBFBD><EFBFBD><EFBFBD>
g_ptz.location_return.hori_angle[3] = LOCATION_HORI;//<2F>ش<EFBFBD>ˮƽλ<C6BD>õ<EFBFBD><C3B5><EFBFBD>
g_ptz.location_return.hori_angle[6] =
MotorCalPelcoDSUM(g_ptz.location_return.hori_angle,sizeof(g_ptz.location_return.hori_angle));
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
ptz_send_data(PTZ_UART_422, g_ptz.location_return.hori_angle,
sizeof(g_ptz.location_return.hori_angle));
g_ptz.location_return.hori_cmd = LOCATION_CMD_OFF;
}
if(hori_vert == LOCATION_VERT && g_ptz.location_return.vert_cmd == LOCATION_UART_422_CMD_ON)
{//ת<><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֱλ<D6B1>ûظ<C3BB>
g_ptz.location_return.vert_angle[2] = 0xc5;//<2F>޸<EFBFBD><DEB8><EFBFBD><EFBFBD><EFBFBD>
g_ptz.location_return.vert_angle[3] = LOCATION_VERT;//<2F>ش<EFBFBD><D8B4><EFBFBD>ֱλ<D6B1>õ<EFBFBD><C3B5><EFBFBD>
g_ptz.location_return.vert_angle[6] =
MotorCalPelcoDSUM(g_ptz.location_return.vert_angle,sizeof(g_ptz.location_return.vert_angle));
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
ptz_send_data(PTZ_UART_422, g_ptz.location_return.vert_angle,
sizeof(g_ptz.location_return.vert_angle));
g_ptz.location_return.vert_cmd = LOCATION_CMD_OFF;
}
}
if(g_ptz.location_return.uart_485_location_switch == LOCATION_RETURN_ON)
{//<2F><><EFBFBD><EFBFBD><EFBFBD>ǵ<EFBFBD><C7B5><EFBFBD>ָ<EFBFBD><D6B8>λ<EFBFBD><CEBB><EFBFBD><EFBFBD>Ҫ<EFBFBD>ش<EFBFBD><D8B4><EFBFBD><EFBFBD><EFBFBD>
if(hori_vert == LOCATION_HORI && g_ptz.location_return.hori_cmd == LOCATION_UART_485_CMD_ON)
{//ת<><D7AA><EFBFBD><EFBFBD>ˮƽλ<C6BD>ûظ<C3BB>
g_ptz.location_return.hori_angle[2] = 0xc5;//<2F>޸<EFBFBD><DEB8><EFBFBD><EFBFBD><EFBFBD>
g_ptz.location_return.hori_angle[3] = LOCATION_HORI;//<2F>ش<EFBFBD>ˮƽλ<C6BD>õ<EFBFBD><C3B5><EFBFBD>
g_ptz.location_return.hori_angle[6] =
MotorCalPelcoDSUM(g_ptz.location_return.hori_angle,sizeof(g_ptz.location_return.hori_angle));
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
ptz_send_data(PTZ_UART_485, g_ptz.location_return.hori_angle,
sizeof(g_ptz.location_return.hori_angle));
g_ptz.location_return.hori_cmd = LOCATION_CMD_OFF;
}
if(hori_vert == LOCATION_VERT && g_ptz.location_return.vert_cmd == LOCATION_UART_485_CMD_ON)
{//ת<><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֱλ<D6B1>ûظ<C3BB>
g_ptz.location_return.vert_angle[2] = 0xc5;//<2F>޸<EFBFBD><DEB8><EFBFBD><EFBFBD><EFBFBD>
g_ptz.location_return.vert_angle[3] = LOCATION_VERT;//<2F>ش<EFBFBD><D8B4><EFBFBD>ֱλ<D6B1>õ<EFBFBD><C3B5><EFBFBD>
g_ptz.location_return.vert_angle[6] =
MotorCalPelcoDSUM(g_ptz.location_return.vert_angle,sizeof(g_ptz.location_return.vert_angle));
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
ptz_send_data(PTZ_UART_485, g_ptz.location_return.vert_angle,
sizeof(g_ptz.location_return.vert_angle));
g_ptz.location_return.vert_cmd = LOCATION_CMD_OFF;
}
}
}
void ptz_location_return_angle_save(char hori_vert, unsigned char *data, char dev)
{
if(g_ptz.location_return.uart_485_location_switch == LOCATION_RETURN_ON ||
g_ptz.location_return.net_udp_location_switch == LOCATION_RETURN_ON||
g_ptz.location_return.uart_422_location_switch == LOCATION_RETURN_ON)
{
//<2F><>ֹ<EFBFBD>Ƕȶ<C7B6>λ<EFBFBD>ش<EFBFBD><D8B4><EFBFBD>Ԥ<EFBFBD><D4A4>λ<EFBFBD>ش<EFBFBD><D8B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͻ<EFBFBD><CDBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD>ش<EFBFBD><D8B4><EFBFBD>Ԥ<EFBFBD><D4A4>λ<EFBFBD><CEBB>λ<EFBFBD>ش<EFBFBD>ֻ<EFBFBD>ܻش<DCBB>һ<EFBFBD><D2BB>
g_ptz.preset_bit_return.preset_bit_location_cmd = 0;
//<2F><><EFBFBD><EFBFBD><EFBFBD>Ǹոս<D5B8><D5BD>յ<EFBFBD><D5B5>Ƕȿ<C7B6><C8BF><EFBFBD>ָ<EFBFBD><EFBFBD>򱣴<EFBFBD><F2B1A3B4><EFBFBD><EFBFBD><EFBFBD>
if(hori_vert == LOCATION_HORI)
{//<2F>յ<EFBFBD>ˮƽ<CBAE>Ƕȿ<C7B6><C8BF><EFBFBD>ָ<EFBFBD><D6B8>
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
memcpy(g_ptz.location_return.hori_angle, data, sizeof(g_ptz.location_return.hori_angle));
}
if(hori_vert == LOCATION_VERT)
{//<2F>յ<EFBFBD><D5B5><EFBFBD>ֱ<EFBFBD>Ƕȿ<C7B6><C8BF><EFBFBD>ָ<EFBFBD><D6B8>
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
memcpy(g_ptz.location_return.vert_angle, data, sizeof(g_ptz.location_return.vert_angle));
}
if(hori_vert == LOCATION_HORI)
{
switch(dev)
{
case PTZ_UDP:
g_ptz.location_return.hori_cmd = LOCATION_UDP_CMD_ON;
break;
case PTZ_UART_422:
g_ptz.location_return.hori_cmd = LOCATION_UART_422_CMD_ON;
break;
case PTZ_UART_485:
g_ptz.location_return.hori_cmd = LOCATION_UART_485_CMD_ON;
break;
}
}
if(hori_vert == LOCATION_VERT)
{
switch(dev)
{
case PTZ_UDP:
g_ptz.location_return.vert_cmd = LOCATION_UDP_CMD_ON;
break;
case PTZ_UART_422:
g_ptz.location_return.vert_cmd = LOCATION_UART_422_CMD_ON;
break;
case PTZ_UART_485:
g_ptz.location_return.vert_cmd = LOCATION_UART_485_CMD_ON;
break;
}
}
}
}
char ptz_hori_rotate_plan(char rot_mode)
{
float M,M1,M2;
g_ptz.hori_rotate_plan.start_angle = g_ptz.hori_angle_actual;
g_ptz.hori_rotate_plan.stop_angle = g_ptz.hori_angle_control;
g_ptz.hori_rotate_plan.max_speed = g_ptz.hori_speed_control;
g_ptz.hori_rotate_plan.rotate_switch = 1;//<2F><><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>Ҫת<D2AA><D7AA>
//<2F><><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD><D0B6><EFBFBD>̨<EFBFBD>ٶ<EFBFBD><D9B6>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><D2AA>
if(g_ptz.hori_rotate_plan.max_speed > PTZ_HORI_MAX_SPEED)
{
g_ptz.hori_rotate_plan.max_speed = PTZ_HORI_MAX_SPEED;
}
if(g_ptz.hori_rotate_plan.max_speed < PTZ_HORI_MIN_SPEED)
{
g_ptz.hori_rotate_plan.max_speed = PTZ_HORI_MIN_SPEED;
}
//<2F>ж<EFBFBD>ֹͣ<CDA3>Ƕ<EFBFBD><C7B6>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><D2AA>
while(g_ptz.hori_rotate_plan.stop_angle < 0)
{
g_ptz.hori_rotate_plan.stop_angle = 360.0 + g_ptz.hori_rotate_plan.stop_angle;
}
while(g_ptz.hori_rotate_plan.stop_angle > 360.0)
{
g_ptz.hori_rotate_plan.stop_angle = g_ptz.hori_rotate_plan.stop_angle - 360.0;
}
//<2F><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD>ģʽ<C4A3>滮·<E6BBAE><C2B7>
switch(rot_mode)
{
case PTZ_HORI_MIN_DISTANCE://if(rot_mode == PTZ_HORI_MIN_DISTANCE)
M1 = fabs(g_ptz.hori_rotate_plan.start_angle - g_ptz.hori_rotate_plan.stop_angle);
if(g_ptz.hori_rotate_plan.start_angle < g_ptz.hori_rotate_plan.stop_angle)
{
M2 = fabs(360.0 - g_ptz.hori_rotate_plan.stop_angle) +
fabs(g_ptz.hori_rotate_plan.start_angle - 0);
if(M1 <= M2)
{
M = M1;
g_ptz.hori_rotate_plan.direction = PTZ_HORI_DIR_RIGHT;
}
else //if(M1 > M2)
{
M = M2;
g_ptz.hori_rotate_plan.direction = PTZ_HORI_DIR_LEFT;
}
}
if(g_ptz.hori_rotate_plan.start_angle > g_ptz.hori_rotate_plan.stop_angle)
{
M2 = fabs(360.0 - g_ptz.hori_rotate_plan.start_angle) +
fabs(g_ptz.hori_rotate_plan.stop_angle - 0);
if(M1 < M2)
{
M = M1;
g_ptz.hori_rotate_plan.direction = PTZ_HORI_DIR_LEFT;
}
else //if(M1 >= M2)
{
M = M2;
g_ptz.hori_rotate_plan.direction = PTZ_HORI_DIR_RIGHT;
}
}
if(g_ptz.hori_rotate_plan.start_angle == g_ptz.hori_rotate_plan.stop_angle)
{
M = 0;
g_ptz.hori_rotate_plan.rotate_switch = 0;
return 1;
}
//ת<><D7AA><EFBFBD>ܾ<EFBFBD><DCBE><EFBFBD>
g_ptz.hori_rotate_plan.total_rotate_range = M;
break;
case PTZ_HORI_MAX_DISTANCE://if(rot_mode == PTZ_HORI_MAX_DISTANCE)
M1 = fabs(g_ptz.hori_rotate_plan.start_angle - g_ptz.hori_rotate_plan.stop_angle);
if(g_ptz.hori_rotate_plan.start_angle < g_ptz.hori_rotate_plan.stop_angle)
{
M2 = fabs(360.0 - g_ptz.hori_rotate_plan.stop_angle) +
fabs(g_ptz.hori_rotate_plan.start_angle - 0);
if(M1 <= M2)
{
M = M2;
g_ptz.hori_rotate_plan.direction = PTZ_HORI_DIR_LEFT;
}
else //if(M1 > M2)
{
M = M1;
g_ptz.hori_rotate_plan.direction = PTZ_HORI_DIR_RIGHT;
}
}
if(g_ptz.hori_rotate_plan.start_angle > g_ptz.hori_rotate_plan.stop_angle)
{
M2 = fabs(360.0 - g_ptz.hori_rotate_plan.start_angle) +
fabs(g_ptz.hori_rotate_plan.stop_angle - 0);
if(M1 < M2)
{
M = M2;
g_ptz.hori_rotate_plan.direction = PTZ_HORI_DIR_RIGHT;
}
else //if(M1 >= M2)
{
M = M1;
g_ptz.hori_rotate_plan.direction = PTZ_HORI_DIR_LEFT;
}
}
if(g_ptz.hori_rotate_plan.start_angle == g_ptz.hori_rotate_plan.stop_angle)
{
M = 0;
g_ptz.hori_rotate_plan.rotate_switch = 0;
return 1;
}
//ת<><D7AA><EFBFBD>ܾ<EFBFBD><DCBE><EFBFBD>
g_ptz.hori_rotate_plan.total_rotate_range = M;
break;
case PTZ_HORI_RIGHT_ANGLE://if(rot_mode == PTZ_HORI_RIGHT_ANGLE),<2C><>ת<EFBFBD><D7AA>ָ<EFBFBD><D6B8><EFBFBD>Ƕ<EFBFBD>
M1 = fabs(g_ptz.hori_rotate_plan.start_angle - g_ptz.hori_rotate_plan.stop_angle);
g_ptz.hori_rotate_plan.direction = PTZ_HORI_DIR_RIGHT;
if(g_ptz.hori_rotate_plan.start_angle < g_ptz.hori_rotate_plan.stop_angle)
{
M = M1;
}
if(g_ptz.hori_rotate_plan.start_angle > g_ptz.hori_rotate_plan.stop_angle)
{
M2 = fabs(360.0 - g_ptz.hori_rotate_plan.start_angle) +
fabs(g_ptz.hori_rotate_plan.stop_angle - 0);
M = M2;
}
if(g_ptz.hori_rotate_plan.start_angle == g_ptz.hori_rotate_plan.stop_angle)
{
M = 0;
g_ptz.hori_rotate_plan.rotate_switch = 0;
return 1;
}
//ת<><D7AA><EFBFBD>ܾ<EFBFBD><DCBE><EFBFBD>
g_ptz.hori_rotate_plan.total_rotate_range = M;
break;
case PTZ_HORI_LEFT_ANGLE://if(rot_mode == PTZ_HORI_LEFT_ANGLE)<29><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>ָ<EFBFBD><D6B8><EFBFBD>Ƕ<EFBFBD>
M1 = fabs(g_ptz.hori_rotate_plan.start_angle - g_ptz.hori_rotate_plan.stop_angle);
g_ptz.hori_rotate_plan.direction = PTZ_HORI_DIR_LEFT;
if(g_ptz.hori_rotate_plan.start_angle < g_ptz.hori_rotate_plan.stop_angle)
{
M2 = fabs(360.0 - g_ptz.hori_rotate_plan.stop_angle) +
fabs(g_ptz.hori_rotate_plan.start_angle - 0);
M = M2;
}
if(g_ptz.hori_rotate_plan.start_angle > g_ptz.hori_rotate_plan.stop_angle)
{
M = M1;
}
if(g_ptz.hori_rotate_plan.start_angle == g_ptz.hori_rotate_plan.stop_angle)
{
M = 0;
g_ptz.hori_rotate_plan.rotate_switch = 0;
return 1;
}
//ת<><D7AA><EFBFBD>ܾ<EFBFBD><DCBE><EFBFBD>
g_ptz.hori_rotate_plan.total_rotate_range = M;
break;
case PTZ_HORI_RIGHT_CYCLE:
g_ptz.hori_rotate_plan.direction = PTZ_HORI_DIR_RIGHT;
M = 360.0;
g_ptz.hori_rotate_plan.total_rotate_range = M;
break;
case PTZ_HORI_LEFT_CYCLE:
g_ptz.hori_rotate_plan.direction = PTZ_HORI_DIR_LEFT;
M = 360.0;
g_ptz.hori_rotate_plan.total_rotate_range = M;
break;
case PTZ_HORI_RIGHT_KEEP:
g_ptz.hori_rotate_plan.direction = PTZ_HORI_DIR_RIGHT;
return 1;
break;
case PTZ_HORI_LEFT_KEEP:
g_ptz.hori_rotate_plan.direction = PTZ_HORI_DIR_LEFT;
return 1;
break;
case PTZ_HORI_RIGHT_ANGLE_INC://<2F><>ת<EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>滮
g_ptz.hori_rotate_plan.direction = PTZ_HORI_DIR_RIGHT;
g_ptz.hori_rotate_plan.total_rotate_range = g_ptz.hori_angle_right_inc_control;
if(g_ptz.hori_rotate_plan.total_rotate_range == 0)
{
g_ptz.hori_rotate_plan.rotate_switch = 0;
return 1;
}
break;
case PTZ_HORI_LEFT_ANGLE_INC://<2F><>ת<EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>滮
g_ptz.hori_rotate_plan.direction = PTZ_HORI_DIR_LEFT;
g_ptz.hori_rotate_plan.total_rotate_range = g_ptz.hori_angle_left_inc_control;
if(g_ptz.hori_rotate_plan.total_rotate_range == 0)
{
g_ptz.hori_rotate_plan.rotate_switch = 0;
return 1;
}
break;
}
//<2F><><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if(g_ptz.hori_rotate_plan.direction == PTZ_HORI_DIR_RIGHT)
{
/*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2><EFBFBD><EFBFBD>*/
g_ptz.hori_rotate_plan.stop_near_angle =
g_ptz.hori_rotate_plan.stop_angle - PTZ_HORI_STOP_NEAR_DISTANCE ;
if(g_ptz.hori_rotate_plan.stop_near_angle < 0)
{
g_ptz.hori_rotate_plan.stop_near_angle =
g_ptz.hori_rotate_plan.stop_near_angle + 360.0;
}
/*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Զɲ<D4B6><C9B2><EFBFBD><EFBFBD>*/
g_ptz.hori_rotate_plan.stop_far_angle =
g_ptz.hori_rotate_plan.stop_angle + PTZ_HORI_STOP_FAR_DISTANCE;
if(g_ptz.hori_rotate_plan.stop_far_angle > 360.0)
{
g_ptz.hori_rotate_plan.stop_far_angle =
g_ptz.hori_rotate_plan.stop_far_angle - 360.0;
}
}
if(g_ptz.hori_rotate_plan.direction == PTZ_HORI_DIR_LEFT)
{
/*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2><EFBFBD><EFBFBD>*/
g_ptz.hori_rotate_plan.stop_near_angle =
g_ptz.hori_rotate_plan.stop_angle + PTZ_HORI_STOP_NEAR_DISTANCE;
if(g_ptz.hori_rotate_plan.stop_angle > 360.0)
{
g_ptz.hori_rotate_plan.stop_near_angle =
g_ptz.hori_rotate_plan.stop_near_angle - 360.0;
}
/*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Զɲ<D4B6><C9B2><EFBFBD><EFBFBD>*/
g_ptz.hori_rotate_plan.stop_far_angle =
g_ptz.hori_rotate_plan.stop_angle - PTZ_HORI_STOP_FAR_DISTANCE;
if(g_ptz.hori_rotate_plan.stop_far_angle < 0)
{
g_ptz.hori_rotate_plan.stop_far_angle =
g_ptz.hori_rotate_plan.stop_far_angle + 360.0;
}
}
/* ˮƽ<CBAE><C6BD><EFBFBD><EFBFBD><EEB2B9> */
if(g_ptz.hori_angle_erro_switch == 1)
{
if(g_ptz.hori_direction_actual != g_ptz.hori_rotate_plan.direction)
{//<2F><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>ͬ<EFBFBD><CDAC>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>ͬ<EFBFBD><CDAC><EFBFBD>нṹ<D0BD><E1B9B9>϶<EFBFBD><CFB6><EFBFBD><EFBFBD>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD>ṹ<EFBFBD><E1B9B9><EFBFBD><EFBFBD>
if(g_ptz.hori_rotate_plan.direction == PTZ_HORI_DIR_LEFT)
{//<2F><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>ʾ<EFBFBD>ϴ<EFBFBD>Ϊ<EFBFBD><CEAA>ת<EFBFBD><D7AA><EFBFBD>Ƕȼ<C7B6>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD>Ͻṹ<CFBD><E1B9B9><EFBFBD><EFBFBD><EEA3AC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƕȵ<C7B6>λ<EFBFBD><CEBB>ʵ<EFBFBD><CAB5><EFBFBD><EFBFBD>̨<EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD>,ʵ<><CAB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB><EFBFBD><EFBFBD>
g_ptz.offset_angle.offset_anle = (g_ptz.hori_angle_error * 3.2);//0.5;<3B>ṹ<EFBFBD><E1B9B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>зŴ<D0B7><C5B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŵ<EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
}
if(g_ptz.hori_rotate_plan.direction == PTZ_HORI_DIR_RIGHT)
{//<2F><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD>ϴ<EFBFBD>Ϊ<EFBFBD><CEAA>ת<EFBFBD><D7AA>ʵ<EFBFBD><CAB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB><EFBFBD><EFBFBD>
g_ptz.offset_angle.offset_anle = (-1.0)*(g_ptz.hori_angle_error * 3.2);//-0.2;
}
}
if(g_ptz.hori_direction_actual == g_ptz.hori_rotate_plan.direction)
{//<2F><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>ͬ<EFBFBD><CDAC>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>ͬ<EFBFBD><CDAC><EFBFBD>нṹ<D0BD><E1B9B9>϶<EFBFBD><CFB6><EFBFBD><EFBFBD>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD>ṹ<EFBFBD><E1B9B9><EFBFBD><EFBFBD>
if(g_ptz.hori_rotate_plan.direction == PTZ_HORI_DIR_LEFT)
{//<2F><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>ʾ<EFBFBD>ϴ<EFBFBD>Ϊ<EFBFBD><CEAA>ת<EFBFBD><D7AA><EFBFBD>Ƕȼ<C7B6>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD>Ͻṹ<CFBD><E1B9B9><EFBFBD><EFBFBD><EEA3AC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƕȵ<C7B6>λ<EFBFBD><CEBB>ʵ<EFBFBD><CAB5><EFBFBD><EFBFBD>̨<EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD>
g_ptz.offset_angle.offset_anle = 0.0;//0.3;
}
if(g_ptz.hori_rotate_plan.direction == PTZ_HORI_DIR_RIGHT)
{//<2F><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>ʾ<EFBFBD>ϴ<EFBFBD>Ϊ<EFBFBD><CEAA>ת<EFBFBD><D7AA><EFBFBD>Ƕȼ<C7B6>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD>Ͻṹ<CFBD><E1B9B9><EFBFBD><EFBFBD><EEA3AC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƕȵ<C7B6>λ<EFBFBD><CEBB>ʵ<EFBFBD><CAB5><EFBFBD><EFBFBD>̨<EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD>
g_ptz.offset_angle.offset_anle = 0.0;//-0.2;
}
}
}
/*<2A><><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2><EFBFBD><EFBFBD>Χ*/
g_ptz.hori_rotate_plan.stop_angle_range =
PTZ_HORI_STOP_NEAR_DISTANCE + PTZ_HORI_STOP_FAR_DISTANCE;
return 1;
}
char ptz_vert_rotate_plan(char rot_mode)
{
g_ptz.vert_rotate_plan.start_angle = g_ptz.vert_angle_actual;
g_ptz.vert_rotate_plan.stop_angle = g_ptz.vert_angle_control;
g_ptz.vert_rotate_plan.max_speed = g_ptz.vert_speed_control;
g_ptz.vert_rotate_plan.rotate_switch = 1;//<2F><><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>Ҫת<D2AA><D7AA>
//<2F><><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD><D0B6><EFBFBD>̨<EFBFBD>ٶ<EFBFBD><D9B6>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><D2AA>
if(g_ptz.vert_rotate_plan.max_speed > PTZ_VERT_MAX_SPEED)
{
g_ptz.vert_rotate_plan.max_speed = PTZ_VERT_MAX_SPEED;
}
if(g_ptz.vert_rotate_plan.max_speed < PTZ_VERT_MIN_SPEED)
{
g_ptz.vert_rotate_plan.max_speed = PTZ_VERT_MIN_SPEED;
}
//<2F>ж<EFBFBD>ֹͣ<CDA3>Ƕ<EFBFBD><C7B6>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><D2AA>
if(g_ptz.vert_rotate_plan.stop_angle < g_ptz.vert_angleP.angle_allow_min)
{
g_ptz.vert_rotate_plan.stop_angle = g_ptz.vert_angleP.angle_allow_min;
}
if(g_ptz.vert_rotate_plan.stop_angle > g_ptz.vert_angleP.angle_allow_max)
{
g_ptz.vert_rotate_plan.stop_angle = g_ptz.vert_angleP.angle_allow_max;
}
//<2F><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD>ģʽ<C4A3>滮·<E6BBAE><C2B7>
switch(rot_mode)
{
case PTZ_VERT_ANGLE:
#ifdef PTZ_PHOTOELECTRIC_SWITCH
if(g_ptz.vert_rotate_plan.stop_angle > g_ptz.vert_rotate_plan.start_angle)
{
g_ptz.vert_rotate_plan.direction = PTZ_VERT_DIR_UP;
if(g_ptz.vert_ps_sw2_state == PS_COVER)
{
g_ptz.vert_rotate_plan.rotate_switch = 0;//<2F><><EFBFBD><EFBFBD>Ҫת<D2AA><D7AA>
return 1;
}
}
if(g_ptz.vert_rotate_plan.stop_angle < g_ptz.vert_rotate_plan.start_angle)
{
g_ptz.vert_rotate_plan.direction = PTZ_VERT_DIR_DOWN;
if(g_ptz.vert_ps_sw1_state == PS_COVER)
{
g_ptz.vert_rotate_plan.rotate_switch = 0;//<2F><><EFBFBD><EFBFBD>Ҫת<D2AA><D7AA>
return 1;
}
}
if(g_ptz.vert_rotate_plan.stop_angle == g_ptz.vert_rotate_plan.start_angle)
{
g_ptz.vert_rotate_plan.rotate_switch = 0;//<2F><><EFBFBD><EFBFBD>Ҫת<D2AA><D7AA>
return 1;
}
#endif
break;
case PTZ_VERT_UP_KEEP:
#ifdef PTZ_PHOTOELECTRIC_SWITCH
g_ptz.vert_rotate_plan.stop_angle = g_ptz.vert_angleP.angle_allow_max;
g_ptz.vert_rotate_plan.direction = PTZ_VERT_DIR_UP;
if(g_ptz.vert_angle_actual >= g_ptz.vert_angleP.angle_allow_max
|| g_ptz.vert_ps_sw2_state == PS_COVER)
{
g_ptz.vert_rotate_plan.rotate_switch = 0;//<2F><><EFBFBD><EFBFBD>Ҫת<D2AA><D7AA>
return 1;
}
#endif
break;
case PTZ_VERT_DOWN_KEEP:
#ifdef PTZ_PHOTOELECTRIC_SWITCH
g_ptz.vert_rotate_plan.stop_angle = g_ptz.vert_angleP.angle_allow_min;
g_ptz.vert_rotate_plan.direction = PTZ_VERT_DIR_DOWN;
if(g_ptz.vert_angle_actual <= g_ptz.vert_angleP.angle_allow_min
|| g_ptz.vert_ps_sw1_state == PS_COVER)
{
g_ptz.vert_rotate_plan.rotate_switch = 0;//<2F><><EFBFBD><EFBFBD>Ҫת<D2AA><D7AA>
return 1;
}
#endif
break;
}
//<2F><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD>ܾ<EFBFBD><DCBE><EFBFBD>
g_ptz.vert_rotate_plan.total_rotate_range =
fabs(g_ptz.vert_rotate_plan.stop_angle - g_ptz.vert_rotate_plan.start_angle);
//<2F><><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Զ<EFBFBD><D4B6>
if(g_ptz.vert_rotate_plan.direction == PTZ_VERT_DIR_UP)
{
/*ɲ<><C9B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
g_ptz.vert_rotate_plan.stop_near_angle =
g_ptz.vert_rotate_plan.stop_angle - PTZ_VERT_STOP_NEAR_DISTANCE;
/*ɲ<><C9B2><EFBFBD><EFBFBD>Զ<EFBFBD><D4B6>*/
g_ptz.vert_rotate_plan.stop_far_angle =
g_ptz.vert_rotate_plan.stop_angle + PTZ_VERT_STOP_FAR_DISTANCE;
}
if(g_ptz.vert_rotate_plan.direction == PTZ_VERT_DIR_DOWN)
{
/*ɲ<><C9B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
g_ptz.vert_rotate_plan.stop_near_angle =
g_ptz.vert_rotate_plan.stop_angle + PTZ_VERT_STOP_NEAR_DISTANCE;
/*ɲ<><C9B2><EFBFBD><EFBFBD>Զ<EFBFBD><D4B6>*/
g_ptz.vert_rotate_plan.stop_far_angle =
g_ptz.vert_rotate_plan.stop_angle - PTZ_VERT_STOP_FAR_DISTANCE;
}
//<2F><><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2><EFBFBD><EFBFBD>Χ
g_ptz.vert_rotate_plan.stop_angle_range =
PTZ_VERT_STOP_NEAR_DISTANCE + PTZ_VERT_STOP_FAR_DISTANCE;
return 1;
}
//void init_rotate_module()
//{
// task_rotate_monitor_init();
//}