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

383 lines
13 KiB
C
Raw Normal View History

汇源原始工程
2025-05-21 01:55:40 +00:00
#include "ptz_header_file.h"
#include "angle_zerooffset.h"
///<2F><><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD>ƫ<EFBFBD><C6AB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ч<EFBFBD><D0A7><EFBFBD><EFBFBD>
unsigned int ptz_zero_offset_angle_crc(PtzZeroOffsetAngle offset)
{
float crc = 0;
unsigned int crc1 = 0;
crc = crc + offset.hori_zero_offset_angle;
crc = crc + offset.vert_zero_offset_angle;
crc = crc + offset.hori_offset_switch;
crc = crc + offset.vert_offset_switch;
crc1 = (unsigned int)crc;
return crc1;
}
///<2F>洢<EFBFBD>Ƕ<EFBFBD>ƫ<EFBFBD><C6AB><EFBFBD><EFBFBD>
char ptz_zero_offset_angle_save()
{
PtzZeroOffsetAngle zero_offset_angle;
PtzZeroOffsetAngle zero_offset_angle_a;
char i,j;
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
memcpy(&zero_offset_angle, &g_ptz.offset_angle, sizeof(zero_offset_angle));
if(g_ptz.offset_angle.hori_offset_switch == 2)
{
zero_offset_angle.hori_offset_switch = 1;
}
if(g_ptz.offset_angle.vert_offset_switch == 2)
{
zero_offset_angle.vert_offset_switch = 1;
}
//<2F><><EFBFBD><EFBFBD>Ч<EFBFBD><D0A7><EFBFBD><EFBFBD>
zero_offset_angle.crc = ptz_zero_offset_angle_crc(zero_offset_angle);
//д<><D0B4><EFBFBD><EFBFBD><EFBFBD>洢<EFBFBD><E6B4A2>
for(i = 0; i < ZERO_OFFSET_ANGLE_SAVE_FLASH_NUM; i++)
{
memset(&zero_offset_angle_a, 0, sizeof(zero_offset_angle_a));
write_many_data(sizeof(zero_offset_angle),(unsigned char *)(&zero_offset_angle),ZERO_OFFSET_ANGLE_FLASH_ADD);
asm("nop");
asm("nop");
Flash_Read((unsigned char *)(&zero_offset_angle_a),ZERO_OFFSET_ANGLE_FLASH_ADD,sizeof(zero_offset_angle_a));
if(memcmp(&zero_offset_angle, &zero_offset_angle_a, sizeof(zero_offset_angle)) == 0)//<2F>ж<EFBFBD>д<EFBFBD><D0B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD>ȷ
{
break;
}
OSTimeDlyHMSM(0u, 0u, 0u, 2u);
}
//д<>뱸<EFBFBD>ݴ洢<DDB4><E6B4A2>
for(j = 0; j < ZERO_OFFSET_ANGLE_SAVE_FLASH_NUM; j++)
{
memset(&zero_offset_angle_a, 0, sizeof(zero_offset_angle_a));
write_many_data(sizeof(zero_offset_angle),(unsigned char *)(&zero_offset_angle),ZERO_OFFSET_ANGLE_BACKUP_FLASH_ADD);
asm("nop");
asm("nop");
Flash_Read((unsigned char *)(&zero_offset_angle_a),ZERO_OFFSET_ANGLE_BACKUP_FLASH_ADD,sizeof(zero_offset_angle_a));
if(memcmp(&zero_offset_angle, &zero_offset_angle_a, sizeof(zero_offset_angle)) == 0)//<2F>ж<EFBFBD>д<EFBFBD><D0B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD>ȷ
{
break;
}
OSTimeDlyHMSM(0u, 0u, 0u, 2u);
}
if(i < ZERO_OFFSET_ANGLE_SAVE_FLASH_NUM || j < ZERO_OFFSET_ANGLE_SAVE_FLASH_NUM)//<2F>ж<EFBFBD>д<EFBFBD><D0B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><C7B7>ɹ<EFBFBD>
{
//д<><D0B4><EFBFBD>ɹ<EFBFBD>
return 1;
}
//д<><D0B4>ʧ<EFBFBD><CAA7>
return 0;
}
///<2F><>ȡ<EFBFBD>Ƕ<EFBFBD>ƫ<EFBFBD><C6AB><EFBFBD><EFBFBD>***<2A><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD>get_angle.c<><63>
char ptz_zero_offset_angle_read()
{
PtzZeroOffsetAngle zero_offset_angle;
char i,j;
//<2F><><EFBFBD>ȴ<EFBFBD><C8B4><EFBFBD><EFBFBD>洢<EFBFBD><E6B4A2><EFBFBD><EFBFBD>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD>
for(i = 0; i < ZERO_OFFSET_ANGLE_READ_FLASH_NUM; i++)
{
memset(&zero_offset_angle, 0, sizeof(zero_offset_angle));
Flash_Read((unsigned char *)(&zero_offset_angle),ZERO_OFFSET_ANGLE_FLASH_ADD,sizeof(zero_offset_angle));
if(zero_offset_angle.crc == ptz_zero_offset_angle_crc(zero_offset_angle))
{
g_ptz.offset_angle.hori_zero_offset_angle = zero_offset_angle.hori_zero_offset_angle;
g_ptz.offset_angle.vert_zero_offset_angle = zero_offset_angle.vert_zero_offset_angle;
g_ptz.offset_angle.hori_offset_switch = zero_offset_angle.hori_offset_switch;
g_ptz.offset_angle.vert_offset_switch = zero_offset_angle.vert_offset_switch;
if(g_ptz.offset_angle.hori_offset_switch == 1)
{
g_ptz.offset_angle.hori_offset_switch = 2;
}
else
{
g_ptz.offset_angle.hori_offset_switch = 0;
}
if(g_ptz.offset_angle.vert_offset_switch == 1)
{
g_ptz.offset_angle.vert_offset_switch = 2;
}
else
{
g_ptz.offset_angle.vert_offset_switch = 0;
}
return 1;
}
OSTimeDlyHMSM(0u, 0u, 0u, 2u);
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>洢<EFBFBD><E6B4A2><EFBFBD><EFBFBD>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ǵ<EFBFBD><C7B4><EFBFBD><EFBFBD>ģ<EFBFBD><C4A3><EFBFBD><EFBFBD>ӱ<EFBFBD><D3B1>ݴ洢<DDB4><E6B4A2><EFBFBD><EFBFBD>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD>
for(j = 0; j < ZERO_OFFSET_ANGLE_READ_FLASH_NUM; j++)
{
memset(&zero_offset_angle, 0, sizeof(zero_offset_angle));
Flash_Read((unsigned char *)(&zero_offset_angle),ZERO_OFFSET_ANGLE_BACKUP_FLASH_ADD,sizeof(zero_offset_angle));
if(zero_offset_angle.crc == ptz_zero_offset_angle_crc(zero_offset_angle))
{
g_ptz.offset_angle.hori_zero_offset_angle = zero_offset_angle.hori_zero_offset_angle;
g_ptz.offset_angle.vert_zero_offset_angle = zero_offset_angle.vert_zero_offset_angle;
g_ptz.offset_angle.hori_offset_switch = zero_offset_angle.hori_offset_switch;
g_ptz.offset_angle.vert_offset_switch = zero_offset_angle.vert_offset_switch;
if(g_ptz.offset_angle.hori_offset_switch == 1)
{
g_ptz.offset_angle.hori_offset_switch = 2;
}
else
{
g_ptz.offset_angle.hori_offset_switch = 0;
}
if(g_ptz.offset_angle.vert_offset_switch == 1)
{
g_ptz.offset_angle.vert_offset_switch = 2;
}
else
{
g_ptz.offset_angle.vert_offset_switch = 0;
}
return 1;
}
OSTimeDlyHMSM(0u, 0u, 0u, 2u);
}
return 0;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD>ƫ<EFBFBD><C6AB><EFBFBD><EFBFBD>
char ptz_zero_offset_angle_erase()
{
char i, j;
PtzZeroOffsetAngle zero_offset_angle;
PtzZeroOffsetAngle zero_offset_angle_a;
memset(&zero_offset_angle_a, 0, sizeof(zero_offset_angle_a));
memset(&zero_offset_angle, 0, sizeof(zero_offset_angle));
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>洢<EFBFBD><E6B4A2>
for(i = 0; i < ZERO_OFFSET_ANGLE_SAVE_FLASH_NUM; i++)
{
memset(&zero_offset_angle_a, 0, sizeof(zero_offset_angle_a));
write_many_data(sizeof(zero_offset_angle),(unsigned char *)(&zero_offset_angle),ZERO_OFFSET_ANGLE_FLASH_ADD);
asm("nop");
asm("nop");
Flash_Read((unsigned char *)(&zero_offset_angle_a),ZERO_OFFSET_ANGLE_FLASH_ADD,sizeof(zero_offset_angle_a));
if(memcmp(&zero_offset_angle, &zero_offset_angle_a, sizeof(zero_offset_angle)) == 0)//<2F>жϲ<D0B6><CFB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD>ȷ
{
break;
}
OSTimeDlyHMSM(0u, 0u, 0u, 2u);
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݴ洢<DDB4><E6B4A2>
for(j = 0; j < ZERO_OFFSET_ANGLE_SAVE_FLASH_NUM; j++)
{
memset(&zero_offset_angle_a, 0, sizeof(zero_offset_angle_a));
write_many_data(sizeof(zero_offset_angle),(unsigned char *)(&zero_offset_angle),ZERO_OFFSET_ANGLE_BACKUP_FLASH_ADD);
asm("nop");
asm("nop");
Flash_Read((unsigned char *)(&zero_offset_angle_a),ZERO_OFFSET_ANGLE_BACKUP_FLASH_ADD,sizeof(zero_offset_angle_a));
if(memcmp(&zero_offset_angle, &zero_offset_angle_a, sizeof(zero_offset_angle)) == 0)//<2F>жϲ<D0B6><CFB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD>ȷ
{
break;
}
OSTimeDlyHMSM(0u, 0u, 0u, 2u);
}
if(i < ZERO_OFFSET_ANGLE_SAVE_FLASH_NUM || j < ZERO_OFFSET_ANGLE_SAVE_FLASH_NUM)//<2F>жϲ<D0B6><CFB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><C7B7>ɹ<EFBFBD>
{
//<2F><><EFBFBD><EFBFBD><EFBFBD>ɹ<EFBFBD>
return 1;
}
//<2F><><EFBFBD><EFBFBD>ʧ<EFBFBD><CAA7>
return 0;
}
//ƫ<>ƽǶ<C6BD><C7B6><EFBFBD><EFBFBD>ݻش<DDBB>
void ptz_zero_offset_angle_return(char dev)
{
unsigned short int uint16_angle;
unsigned char data[7];
//<2F><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB><EFBFBD><EFBFBD>
ptz_zero_offset_angle_read();
uint16_angle = (unsigned short int)(g_ptz.offset_angle.hori_zero_offset_angle * 100 + 0.5);
data[0] = 0xff;
data[1] = g_ptz.address;
data[2] = 0x3e;
data[3] = PTZ_HORI;//0x07;//PTZ_HORI;//ˮƽƫ<C6BD>ƽǶ<C6BD>
data[4] = (u_int16_t)(uint16_angle >> 8);
data[5] = (u_int16_t)(uint16_angle & 0x00ff);
data[6] = MotorCalPelcoDSUM(data,sizeof(data));
ptz_send_data(dev, data, sizeof(data));
OSTimeDlyHMSM(0u, 0u, 0u, 10u);
uint16_angle = (unsigned short int)(g_ptz.offset_angle.vert_zero_offset_angle * 100 + 0.5);
data[0] = 0xff;
data[1] = g_ptz.address;
data[2] = 0x3e;
data[3] = PTZ_VERT;//0x08;//PTZ_VERT;//<2F><>ֱƫ<D6B1>ƽǶ<C6BD>
data[4] = (u_int16_t)(uint16_angle >> 8);
data[5] = (u_int16_t)(uint16_angle & 0x00ff);
data[6] = MotorCalPelcoDSUM(data,sizeof(data));
ptz_send_data(dev, data, sizeof(data));
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λƫ<CEBB><C6AB>
void ptz_zero_offset_angle_set(char dev, PTZ_DATA_PACK *pack)
{
int16_t int16_angle;
switch(pack->command[1])
{
case 1://<2F><>Ƶ<EFBFBD><C6B5><EFBFBD><EFBFBD>ˮƽ0λ
g_ptz.offset_angle.hori_zero_offset_angle = g_ptz.hori_angle_actual_a;
g_ptz.offset_angle.hori_offset_switch = 1;
g_ptz.offset_angle.crc = ptz_zero_offset_angle_crc(g_ptz.offset_angle);
if(ptz_zero_offset_angle_save() == 1)
{
ptz_reply(dev, CMD_RIGHT);
g_ptz.offset_angle.hori_offset_switch = 2;
}
else
{
ptz_reply(dev, CMD_FAIL);
}
break;
case 2://<2F><>Ƶ<EFBFBD><C6B5><EFBFBD>ô<EFBFBD>ֱ0λ
g_ptz.offset_angle.vert_zero_offset_angle = g_ptz.vert_angle_actual_a;
g_ptz.offset_angle.vert_offset_switch = 1;
g_ptz.offset_angle.crc = ptz_zero_offset_angle_crc(g_ptz.offset_angle);
if(ptz_zero_offset_angle_save() == 1)
{
ptz_reply(dev, CMD_RIGHT);
g_ptz.offset_angle.vert_offset_switch = 2;
}
else
{
ptz_reply(dev, CMD_FAIL);
}
if(g_ptz.offset_angle.vert_offset_switch == 2)
{
g_ptz.vert_angleP.angle_allow_min = g_ptz.vert_angleP.angle_range / 2.0 * (-1);
g_ptz.vert_angleP.angle_allow_max = g_ptz.vert_angleP.angle_range / 2.0;
g_ptz.vert_angleP.angle_allow_min =
g_ptz.vert_angleP.angle_allow_min - g_ptz.offset_angle.vert_zero_offset_angle;
g_ptz.vert_angleP.angle_allow_max =
g_ptz.vert_angleP.angle_allow_max - g_ptz.offset_angle.vert_zero_offset_angle;
}
break;
case 3://<2F><>Ƶͬʱ<CDAC><CAB1><EFBFBD><EFBFBD>ˮƽ0λ<30>ʹ<EFBFBD>ֱ0λ
g_ptz.offset_angle.hori_zero_offset_angle = g_ptz.hori_angle_actual_a;
g_ptz.offset_angle.hori_offset_switch = 1;
g_ptz.offset_angle.vert_zero_offset_angle = g_ptz.vert_angle_actual_a;
g_ptz.offset_angle.vert_offset_switch = 1;
g_ptz.offset_angle.crc = ptz_zero_offset_angle_crc(g_ptz.offset_angle);
if(ptz_zero_offset_angle_save() == 1)
{
ptz_reply(dev, CMD_RIGHT);
g_ptz.offset_angle.hori_offset_switch = 2;
g_ptz.offset_angle.vert_offset_switch = 2;
}
else
{
ptz_reply(dev, CMD_FAIL);
}
if(g_ptz.offset_angle.vert_offset_switch == 2)
{
g_ptz.vert_angleP.angle_allow_min = g_ptz.vert_angleP.angle_range / 2.0 * (-1);
g_ptz.vert_angleP.angle_allow_max = g_ptz.vert_angleP.angle_range / 2.0;
g_ptz.vert_angleP.angle_allow_min =
g_ptz.vert_angleP.angle_allow_min - g_ptz.offset_angle.vert_zero_offset_angle;
g_ptz.vert_angleP.angle_allow_max =
g_ptz.vert_angleP.angle_allow_max - g_ptz.offset_angle.vert_zero_offset_angle;
}
break;
case 4://ͨ<><CDA8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ˮƽ<CBAE>Ƕȣ<C7B6><C8A3><EFBFBD><EFBFBD><EFBFBD>ˮƽ0λ
g_ptz.offset_angle.hori_zero_offset_angle =
((pack->data[0] << 8) | pack->data[1]) / 100.0;
g_ptz.offset_angle.hori_offset_switch = 1;
g_ptz.offset_angle.crc = ptz_zero_offset_angle_crc(g_ptz.offset_angle);
if(ptz_zero_offset_angle_save() == 1)
{
ptz_reply(dev, CMD_RIGHT);
g_ptz.offset_angle.hori_offset_switch = 2;
}
else
{
ptz_reply(dev, CMD_FAIL);
}
break;
case 5://ͨ<><CDA8><EFBFBD><EFBFBD><EFBFBD>ʹ<EFBFBD>ֱ<EFBFBD>Ƕȣ<C7B6><C8A3><EFBFBD><EFBFBD>ô<EFBFBD>ֱ0λ
int16_angle = (pack->data[0] << 8) | pack->data[1];
g_ptz.offset_angle.vert_zero_offset_angle = int16_angle / 100.0;
g_ptz.offset_angle.vert_offset_switch = 1;
g_ptz.offset_angle.crc = ptz_zero_offset_angle_crc(g_ptz.offset_angle);
if(ptz_zero_offset_angle_save() == 1)
{
ptz_reply(dev, CMD_RIGHT);
g_ptz.offset_angle.vert_offset_switch = 2;
}
else
{
ptz_reply(dev, CMD_FAIL);
}
if(g_ptz.offset_angle.vert_offset_switch == 2)
{
g_ptz.vert_angleP.angle_allow_min = g_ptz.vert_angleP.angle_range / 2.0 * (-1);
g_ptz.vert_angleP.angle_allow_max = g_ptz.vert_angleP.angle_range / 2.0;
g_ptz.vert_angleP.angle_allow_min =
g_ptz.vert_angleP.angle_allow_min - g_ptz.offset_angle.vert_zero_offset_angle;
g_ptz.vert_angleP.angle_allow_max =
g_ptz.vert_angleP.angle_allow_max - g_ptz.offset_angle.vert_zero_offset_angle;
}
break;
case 6://ɾ<><C9BE><EFBFBD><EFBFBD>׼0λƫ<CEBB><C6AB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8><EFBFBD>ʼ0λ
if(ptz_zero_offset_angle_erase() == 1)
{
g_ptz.offset_angle.hori_offset_switch = 0;
g_ptz.offset_angle.vert_offset_switch = 0;
g_ptz.offset_angle.crc = ptz_zero_offset_angle_crc(g_ptz.offset_angle);
ptz_reply(dev, CMD_RIGHT);
}
else
{
ptz_reply(dev, CMD_FAIL);
}
g_ptz.vert_angleP.angle_allow_min =
g_ptz.vert_angleP.angle_range / 2.0 * (-1);
g_ptz.vert_angleP.angle_allow_max =
g_ptz.vert_angleP.angle_range / 2.0;
break;
}
}
//<2F><>ѯƫ<D1AF><C6AB>