541 lines
16 KiB
C
541 lines
16 KiB
C
#include "rotate_plan.h"
|
||
#include "ptz_header_file.h"
|
||
#include "gd32f4xx.h"
|
||
#include "angle_poweroffsave.h"
|
||
#include "mb85rc64.h"
|
||
static BSP_OS_SEM ptz_power_off_mutex;//共享资源锁
|
||
|
||
static float power_v;//实时电压
|
||
static float power_v1;//用于保存用于判断电压升高还是降低的基本电压
|
||
|
||
PowerOffData power_off_data;
|
||
|
||
//电压采集
|
||
static float ptz_voltage_adc1()
|
||
{
|
||
unsigned short int adc1_value;
|
||
float float_adc1;
|
||
|
||
adc_software_trigger_enable(ADC1,ADC_INSERTED_CHANNEL);
|
||
adc1_value = ADC_IDATA1(ADC1);
|
||
|
||
float_adc1 = (float)adc1_value / 4096.0 * 36.3;
|
||
return float_adc1;
|
||
}
|
||
|
||
static float ptz_power_off_data_crc(PowerOffData data)
|
||
{
|
||
float crc = 0;
|
||
crc = data.hori_as5047d.as5047d_ptz_init_angle +
|
||
data.hori_as5047d.as5047d_ptz_angle_max +
|
||
data.hori_as5047d.as5047d_ptz_angle_K +
|
||
data.hori_as5047d.as5047d_ptz_angle_wf_actual +
|
||
data.hori_as5047d.as5047d_ptz_angle_wf_last +
|
||
data.hori_as5047d.as5047d_ptz_angle_actual +
|
||
data.hori_as5047d.as5047d_ptz_angle_last +
|
||
data.hori_as5047d.as5047d_remain_angle +
|
||
data.hori_as5047d.as5047d_cycle_num +
|
||
data.hori_as5047d.as5047d_cycle_count +
|
||
data.hori_as5047d.as5047d_cycle_count_last +
|
||
data.hori_as5047d.as5047d_angle_actual +
|
||
data.hori_as5047d.as5047d_dir +
|
||
data.hori_as5047d.as5047d_angle_last +
|
||
data.hori_as5047d.as5047d_angle_difference +
|
||
data.hori_as5047d.as5047d_state +
|
||
data.hori_as5047d.as5047d_data_reset +
|
||
|
||
data.vert_as5047d.as5047d_ptz_init_angle +
|
||
data.vert_as5047d.as5047d_ptz_angle_max +
|
||
data.vert_as5047d.as5047d_ptz_angle_K +
|
||
data.vert_as5047d.as5047d_ptz_angle_wf_actual +
|
||
data.vert_as5047d.as5047d_ptz_angle_wf_last +
|
||
data.vert_as5047d.as5047d_ptz_angle_actual +
|
||
data.vert_as5047d.as5047d_ptz_angle_last +
|
||
data.vert_as5047d.as5047d_remain_angle +
|
||
data.vert_as5047d.as5047d_cycle_num +
|
||
data.vert_as5047d.as5047d_cycle_count +
|
||
data.vert_as5047d.as5047d_cycle_count_last +
|
||
data.vert_as5047d.as5047d_angle_actual +
|
||
data.vert_as5047d.as5047d_dir +
|
||
data.vert_as5047d.as5047d_angle_last +
|
||
data.vert_as5047d.as5047d_angle_difference +
|
||
data.vert_as5047d.as5047d_state +
|
||
data.vert_as5047d.as5047d_data_reset +
|
||
data.vert_angle +
|
||
data.hori_angle +
|
||
data.power_down_offset_anle +
|
||
|
||
|
||
data.num;
|
||
|
||
return crc;
|
||
}
|
||
|
||
|
||
|
||
|
||
char ptz_power_off_data_save(unsigned short int add)
|
||
{
|
||
BSP_OS_SemWait(&ptz_power_off_mutex, 0u);
|
||
PowerOffData power_off_data_b;
|
||
int i;
|
||
memcpy(&power_off_data.hori_as5047d, &g_ptz.hori_as5047d, sizeof(g_ptz.hori_as5047d));
|
||
memcpy(&power_off_data.vert_as5047d, &g_ptz.vert_as5047d, sizeof(g_ptz.vert_as5047d));
|
||
power_off_data.vert_angle = g_ptz.vert_angle_actual;
|
||
power_off_data.hori_angle = g_ptz.hori_angle_actual;
|
||
power_off_data.power_down_offset_anle = g_ptz.offset_angle.offset_anle;//误差补偿参数保存
|
||
power_off_data.crc = ptz_power_off_data_crc(power_off_data);
|
||
|
||
for(i = 0; i < PTZ_POWER_OFF_DATA_SAVE_NUM; i++)
|
||
{
|
||
memset(&power_off_data_b, 0, sizeof(power_off_data_b));
|
||
mb85rc64_page_write(add, (unsigned char *)&power_off_data, sizeof(power_off_data));
|
||
asm("nop");
|
||
asm("nop");
|
||
mb85rc64_add_read(add, (unsigned char *)&power_off_data_b, sizeof(power_off_data_b));
|
||
if(memcmp(&power_off_data_b, &power_off_data, sizeof(power_off_data)) == 0)//判断擦除数据是否正确
|
||
{
|
||
BSP_OS_SemPost(&ptz_power_off_mutex);
|
||
return 1;
|
||
}
|
||
}
|
||
BSP_OS_SemPost(&ptz_power_off_mutex);
|
||
return 0;
|
||
}
|
||
|
||
char ptz_power_off_data_read()
|
||
{
|
||
BSP_OS_SemWait(&ptz_power_off_mutex, 0u);
|
||
char i;
|
||
PowerOffData power_off_data_a;
|
||
PowerOffData power_off_data_b;
|
||
char data_a = 0;
|
||
char data_b = 0;
|
||
|
||
memset(&power_off_data, 0, sizeof(power_off_data));
|
||
//读取存储区域a的数据
|
||
for(i = 0; i < PTZ_POWER_OFF_DATA_READ_NUM; i++)
|
||
{
|
||
memset(&power_off_data_a, 0, sizeof(power_off_data_a));
|
||
mb85rc64_add_read(PTZ_MB85RC64_ADD_A, (unsigned char *)&power_off_data_a, sizeof(power_off_data_a));
|
||
if(power_off_data_a.crc == ptz_power_off_data_crc(power_off_data_a) && power_off_data_a.crc != 0)
|
||
{
|
||
data_a = 1;
|
||
break;
|
||
}
|
||
}
|
||
|
||
//读取存储区域b的数据
|
||
for(i = 0; i < PTZ_POWER_OFF_DATA_READ_NUM; i++)
|
||
{
|
||
memset(&power_off_data_b, 0, sizeof(power_off_data_b));
|
||
mb85rc64_add_read(PTZ_MB85RC64_ADD_B, (unsigned char *)&power_off_data_b, sizeof(power_off_data_b));
|
||
if(power_off_data_b.crc == ptz_power_off_data_crc(power_off_data_b) && power_off_data_b.crc != 0)
|
||
{
|
||
data_b = 1;
|
||
break;
|
||
}
|
||
}
|
||
|
||
if(data_a == 1 && data_b == 0)//只有存储区A的数据是对的,则只用A的数据
|
||
{
|
||
memcpy(&power_off_data, &power_off_data_a, sizeof(power_off_data));
|
||
memcpy(&g_ptz.hori_as5047d, &power_off_data.hori_as5047d, sizeof(g_ptz.hori_as5047d));
|
||
memcpy(&g_ptz.vert_as5047d, &power_off_data.vert_as5047d, sizeof(g_ptz.vert_as5047d));
|
||
|
||
g_ptz.offset_angle.offset_anle = power_off_data.power_down_offset_anle;//读取偏移量
|
||
|
||
if(power_off_data.num == 65535)
|
||
{
|
||
power_off_data.num = 0;
|
||
}
|
||
else
|
||
{
|
||
power_off_data.num ++;
|
||
}
|
||
|
||
BSP_OS_SemPost(&ptz_power_off_mutex);
|
||
return 1;
|
||
}
|
||
|
||
|
||
if(data_a == 0 && data_b == 1)//只有存储区B的数据是对的,则只用B的数据
|
||
{
|
||
memcpy(&power_off_data, &power_off_data_b, sizeof(power_off_data));
|
||
memcpy(&g_ptz.hori_as5047d, &power_off_data.hori_as5047d, sizeof(g_ptz.hori_as5047d));
|
||
memcpy(&g_ptz.vert_as5047d, &power_off_data.vert_as5047d, sizeof(g_ptz.vert_as5047d));
|
||
|
||
g_ptz.offset_angle.offset_anle = power_off_data.power_down_offset_anle;//读取偏移量
|
||
|
||
if(power_off_data.num == 65535)
|
||
{
|
||
power_off_data.num = 0;
|
||
}
|
||
else
|
||
{
|
||
power_off_data.num ++;
|
||
}
|
||
|
||
BSP_OS_SemPost(&ptz_power_off_mutex);
|
||
return 1;
|
||
}
|
||
|
||
if(data_a == 1 && data_b == 1)//如果存储区A的数据是对的，存储区B的数据也是对的
|
||
{
|
||
if(power_off_data_a.num > power_off_data_b.num)
|
||
{
|
||
if((power_off_data_a.num - power_off_data_b.num) == 1)
|
||
{
|
||
memcpy(&power_off_data, &power_off_data_a, sizeof(power_off_data));
|
||
memcpy(&g_ptz.hori_as5047d, &power_off_data.hori_as5047d, sizeof(g_ptz.hori_as5047d));
|
||
memcpy(&g_ptz.vert_as5047d, &power_off_data.vert_as5047d, sizeof(g_ptz.vert_as5047d));
|
||
|
||
g_ptz.offset_angle.offset_anle = power_off_data.power_down_offset_anle;//读取偏移量
|
||
|
||
if(power_off_data.num == 65535)
|
||
{
|
||
power_off_data.num = 0;
|
||
}
|
||
else
|
||
{
|
||
power_off_data.num ++;
|
||
}
|
||
|
||
BSP_OS_SemPost(&ptz_power_off_mutex);
|
||
return 1;
|
||
}
|
||
|
||
if((power_off_data_a.num - power_off_data_b.num) == 65535)
|
||
{
|
||
memcpy(&power_off_data, &power_off_data_b, sizeof(power_off_data));
|
||
memcpy(&g_ptz.hori_as5047d, &power_off_data.hori_as5047d, sizeof(g_ptz.hori_as5047d));
|
||
memcpy(&g_ptz.vert_as5047d, &power_off_data.vert_as5047d, sizeof(g_ptz.vert_as5047d));
|
||
|
||
g_ptz.offset_angle.offset_anle = power_off_data.power_down_offset_anle;//读取偏移量
|
||
|
||
if(power_off_data.num == 65535)
|
||
{
|
||
power_off_data.num = 0;
|
||
}
|
||
else
|
||
{
|
||
power_off_data.num ++;
|
||
}
|
||
|
||
BSP_OS_SemPost(&ptz_power_off_mutex);
|
||
return 1;
|
||
}
|
||
}
|
||
|
||
if(power_off_data_b.num > power_off_data_a.num)
|
||
{
|
||
if((power_off_data_b.num - power_off_data_a.num) == 1)
|
||
{
|
||
memcpy(&power_off_data, &power_off_data_b, sizeof(power_off_data));
|
||
memcpy(&g_ptz.hori_as5047d, &power_off_data.hori_as5047d, sizeof(g_ptz.hori_as5047d));
|
||
memcpy(&g_ptz.vert_as5047d, &power_off_data.vert_as5047d, sizeof(g_ptz.vert_as5047d));
|
||
|
||
g_ptz.offset_angle.offset_anle = power_off_data.power_down_offset_anle;//读取偏移量
|
||
|
||
if(power_off_data.num == 65535)
|
||
{
|
||
power_off_data.num = 0;
|
||
}
|
||
else
|
||
{
|
||
power_off_data.num ++;
|
||
}
|
||
|
||
BSP_OS_SemPost(&ptz_power_off_mutex);
|
||
return 1;
|
||
}
|
||
|
||
if((power_off_data_b.num - power_off_data_a.num) == 65535)
|
||
{
|
||
memcpy(&power_off_data, &power_off_data_a, sizeof(power_off_data));
|
||
memcpy(&g_ptz.hori_as5047d, &power_off_data.hori_as5047d, sizeof(g_ptz.hori_as5047d));
|
||
memcpy(&g_ptz.vert_as5047d, &power_off_data.vert_as5047d, sizeof(g_ptz.vert_as5047d));
|
||
|
||
g_ptz.offset_angle.offset_anle = power_off_data.power_down_offset_anle;//读取偏移量
|
||
|
||
if(power_off_data.num == 65535)
|
||
{
|
||
power_off_data.num = 0;
|
||
}
|
||
else
|
||
{
|
||
power_off_data.num ++;
|
||
}
|
||
|
||
BSP_OS_SemPost(&ptz_power_off_mutex);
|
||
return 1;
|
||
}
|
||
}
|
||
|
||
}
|
||
|
||
|
||
BSP_OS_SemPost(&ptz_power_off_mutex);
|
||
return 0;
|
||
}
|
||
|
||
char ptz_power_off_data_erase(unsigned short int add)
|
||
{
|
||
BSP_OS_SemWait(&ptz_power_off_mutex, 0u);
|
||
PowerOffData power_off_data_a;
|
||
int i;
|
||
memset(&power_off_data, 0, sizeof(power_off_data));
|
||
power_off_data.crc = -100;
|
||
|
||
for(i = 0; i < PTZ_POWER_OFF_DATA_SAVE_NUM; i++)
|
||
{
|
||
memset(&power_off_data_a, 0, sizeof(power_off_data_a));
|
||
mb85rc64_page_write(add, (unsigned char *)&power_off_data, sizeof(power_off_data));
|
||
asm("nop");
|
||
asm("nop");
|
||
mb85rc64_add_read(add, (unsigned char *)&power_off_data_a, sizeof(power_off_data_a));
|
||
if(memcmp(&power_off_data_a, &power_off_data, sizeof(power_off_data)) == 0)//判断擦除数据是否正确
|
||
{
|
||
BSP_OS_SemPost(&ptz_power_off_mutex);
|
||
return 1;
|
||
}
|
||
}
|
||
BSP_OS_SemPost(&ptz_power_off_mutex);
|
||
return 0;
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
#define ADC1_NUM 5 //电压采集次数
|
||
|
||
static char ptz_power_off_task()
|
||
{
|
||
unsigned short int number;
|
||
#ifdef PTZ_POWER_OFF_ADC_MANY
|
||
static float adc1_v[ADC1_NUM];
|
||
float tem;
|
||
char i,j;
|
||
#endif
|
||
while(1)
|
||
{
|
||
#ifdef PTZ_POWER_OFF_ADC_MANY
|
||
//连续读取电压
|
||
for(i = 0; i < ADC1_NUM; i++)
|
||
{
|
||
tem = 0;
|
||
tem = ptz_voltage_adc1();
|
||
if(tem < 0 || isnan(tem) == 1)
|
||
{
|
||
tem = 0;
|
||
}
|
||
adc1_v[i] = tem;
|
||
OSTimeDlyHMSM(0u, 0u, 0u, 1u);
|
||
}
|
||
//对读取电压排序
|
||
for(i = 0; i < ADC1_NUM-1; i++)//采样值由小到大排列
|
||
{
|
||
for(j = 0; j < ADC1_NUM-i-1; j++)
|
||
{
|
||
if(adc1_v[j] > adc1_v[j+1])
|
||
{
|
||
tem = adc1_v[j];
|
||
adc1_v[j] = adc1_v[j+1];
|
||
adc1_v[j+1] = tem;
|
||
}
|
||
}
|
||
}
|
||
//去掉一个最小值和一个最大值求和
|
||
tem = 0;
|
||
for(i = 1; i < ADC1_NUM - 1; i++)
|
||
{
|
||
tem = tem + adc1_v[i];
|
||
}
|
||
tem = tem /((float)(ADC1_NUM - 2));//去掉一个最大值和一个最小值求平均值
|
||
|
||
|
||
g_ptz.Voltage = tem;
|
||
power_v = tem;
|
||
#endif
|
||
|
||
#ifdef PTZ_POWER_OFF_ADC_SINGLE
|
||
|
||
OSTimeDlyHMSM(0u, 0u, 0u, 2u);
|
||
//读取实时电压
|
||
g_ptz.Voltage = ptz_voltage_adc1();
|
||
//判断读出的电压是否大于0以及是否是一个数字
|
||
if(g_ptz.Voltage < 0 || isnan(g_ptz.Voltage) == 1)
|
||
{
|
||
g_ptz.Voltage = 0;
|
||
}
|
||
if(g_ptz.Voltage == 0)//如果采集的值为0（采集数据异常为0或者电压数据确实降到了0），则再从新采集一次，
|
||
//可以防止某个时刻出现电压采集出错，导致云台误判为电源关电
|
||
{
|
||
//OSTimeDlyHMSM(0u, 0u, 0u, 1u);
|
||
asm("nop");asm("nop");asm("nop");asm("nop");asm("nop");
|
||
g_ptz.Voltage = ptz_voltage_adc1();//再采集一次
|
||
asm("nop");asm("nop");asm("nop");asm("nop");asm("nop");
|
||
g_ptz.Voltage = ptz_voltage_adc1();//再采集一次
|
||
//判断读出的电压是否小于于0以及是否是一个数字
|
||
if(g_ptz.Voltage < 0 || isnan(g_ptz.Voltage) == 1)
|
||
{
|
||
g_ptz.Voltage = 0;
|
||
}
|
||
}
|
||
power_v = g_ptz.Voltage;
|
||
|
||
#endif
|
||
|
||
/*****************************************强制保存位置数据********************************************************/
|
||
if(g_ptz.no_self_check_force_save == 1)
|
||
{
|
||
//存储云台当前运行的数据
|
||
if((power_off_data.num & 1) == 0)//偶数
|
||
{
|
||
if(ptz_power_off_data_save(PTZ_MB85RC64_ADD_A) == 1)//存储云台当前运行的数据
|
||
{
|
||
if(power_off_data.num == 65535)
|
||
{
|
||
|
||
power_off_data.num = 0;
|
||
}
|
||
else
|
||
{
|
||
power_off_data.num ++;
|
||
}
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if(ptz_power_off_data_save(PTZ_MB85RC64_ADD_B) == 1)//存储云台当前运行的数据
|
||
{
|
||
if(power_off_data.num == 65535)
|
||
{
|
||
power_off_data.num = 0;
|
||
}
|
||
else
|
||
{
|
||
power_off_data.num ++;
|
||
}
|
||
}
|
||
}
|
||
g_ptz.no_self_check_force_save = 2;//表示强制保存过云台数据
|
||
}
|
||
|
||
if((g_ptz.hori_start_stop_set == PTZ_HORI_START || g_ptz.vert_start_stop_set == PTZ_VERT_START)
|
||
&& g_ptz.no_self_check_force_save == 2)
|
||
{//强制保存数据后，如果云台又转动了，则又恢复自动判断掉电保存功能
|
||
g_ptz.no_self_check_force_save = 0;
|
||
}
|
||
/**************************************强制擦除保存的位置数据******************************************************/
|
||
if(g_ptz.no_self_check_force_erase == 1)
|
||
{
|
||
ptz_power_off_data_erase(PTZ_MB85RC64_ADD_A);
|
||
ptz_power_off_data_erase(PTZ_MB85RC64_ADD_B);
|
||
g_ptz.no_self_check_force_erase = 2;
|
||
}
|
||
/********************************************自动保存位置数据*****************************************************/
|
||
if(g_ptz.hori_self_check == PTZ_HORI_SELF_CHECK_END &&
|
||
g_ptz.vert_self_check == PTZ_VERT_SELF_CHECK_END &&
|
||
g_ptz.no_self_check_force_save != 2 &&
|
||
g_ptz.no_self_check_force_erase != 2 &&
|
||
g_ptz.no_self_check_state != 1 &&
|
||
g_ptz.hori_angle_state == 1 &&
|
||
g_ptz.vert_angle_state == 1 )
|
||
{
|
||
//电压出现大的下降
|
||
if(power_v < power_v1 && ((power_v1 - power_v) >= PTZ_POWER_DOWN_INC))
|
||
{
|
||
if((power_off_data.num & 1) == 0)//偶数
|
||
{
|
||
if(ptz_power_off_data_save(PTZ_MB85RC64_ADD_A) == 1)//存储云台当前运行的数据
|
||
{
|
||
if(power_off_data.num == 65535)
|
||
{
|
||
power_off_data.num = 0;
|
||
}
|
||
else
|
||
{
|
||
power_off_data.num ++;
|
||
}
|
||
|
||
power_v1 = power_v;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if(ptz_power_off_data_save(PTZ_MB85RC64_ADD_B) == 1)//存储云台当前运行的数据
|
||
{
|
||
if(power_off_data.num == 65535)
|
||
{
|
||
power_off_data.num = 0;
|
||
}
|
||
else
|
||
{
|
||
power_off_data.num ++;
|
||
}
|
||
|
||
power_v1 = power_v;
|
||
}
|
||
}
|
||
|
||
|
||
}
|
||
|
||
//电压升高了
|
||
if(power_v > power_v1)
|
||
{
|
||
power_v1 = power_v;
|
||
}
|
||
// term_printf("\n power down save angle \r\n\r\n");//测试
|
||
|
||
}
|
||
|
||
/**********************************启动后，只要云台一转动就要擦除原先保存的位置数据**************************************/
|
||
if(g_ptz.no_self_check_state == 1 &&
|
||
(g_ptz.hori_start_stop_set == PTZ_HORI_START || g_ptz.vert_start_stop_set == PTZ_VERT_START))
|
||
{
|
||
number = power_off_data.num;
|
||
ptz_power_off_data_erase(PTZ_MB85RC64_ADD_A);
|
||
ptz_power_off_data_erase(PTZ_MB85RC64_ADD_B);
|
||
power_off_data.num = number;
|
||
g_ptz.no_self_check_state = 2;
|
||
}
|
||
|
||
}
|
||
}
|
||
|
||
|
||
static OS_STK task_power_off_stk[POWER_OFF_STK_SIZE];
|
||
static void creat_task_power_off(void)
|
||
{
|
||
CPU_INT08U task_err;
|
||
CPU_INT08U name_err;
|
||
|
||
task_err = OSTaskCreateExt((void (*)(void *)) ptz_power_off_task,
|
||
(void *) 0,
|
||
(OS_STK *)&task_power_off_stk[POWER_OFF_STK_SIZE - 1],
|
||
(INT8U ) POWER_OFF_PRIO,
|
||
(INT16U ) POWER_OFF_PRIO,
|
||
(OS_STK *)&task_power_off_stk[0],
|
||
(INT32U ) POWER_OFF_STK_SIZE,
|
||
(void *) 0,
|
||
(INT16U )(OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR));
|
||
#if (OS_TASK_NAME_EN > 0)
|
||
OSTaskNameSet(POWER_OFF_PRIO, "ptz_power_off_task", &name_err);
|
||
#endif
|
||
|
||
|
||
}
|
||
|
||
void init_power_off_module(void)
|
||
{
|
||
#ifdef PTZ_NO_SELF_CHECK
|
||
mb85rc64_gpio_init();
|
||
|
||
BSP_OS_SemCreate(&ptz_power_off_mutex,1u,"ptz_power_off_mutex");//创建信号量
|
||
|
||
creat_task_power_off();
|
||
#endif
|
||
} |