///Copyright (c) 2022, 四川汇源光通信有限公司 ///All rights reserved. ///@file service_selfcheck.C ///@brief 云台开机自检程序，包括水平全范围自检、简易自检，垂直全范围、简易 /// 自检 /// ///@details ///@note ///@author ypc ///@date 2019/05/23 /// ///@version v1.0 2019/05/23 初始版本 /// Hyt2.1.0 2022/06/02 更换GD32单片，文件移植 #include "service_selfcheck.h" #include "bsp_os.h" #include "ptz_global_variable.h" #include "angle_poweroffsave.h" #include "comm_cfginfo.h" #include "tmc2160.h" #include "service_error_count.h" #ifdef PTZ_BLDC_MOTOR /// 云台全范围自检 static unsigned char ptz_hori_complete_self_check_task() { switch(g_ptz.hori_self_check) { //首先让云台水平向右转动 case PTZ_HORI_SELF_CHECK_COMPLETE_STEP: if(g_ptz.hori_ps_sw3_state == PS_COVER)//如果水平光电开关被挡住 { ptz_hori_start(PTZ_HORI_DIR_LEFT, PTZ_HORI_SELF_CHECK_SPEED); g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_COMPLETE_STEP + 1; } else//没有被挡住 { g_ptz.hori_ps_sw3_right_fall = 0; g_ptz.hori_ps_sw3_left_rise = 0; g_ptz.hori_ps_sw3_right_rise = 0; g_ptz.hori_ps_sw3_left_fall = 0; ptz_hori_start(PTZ_HORI_DIR_RIGHT, PTZ_HORI_SELF_CHECK_SPEED); g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_COMPLETE_STEP + 2; } break; case PTZ_HORI_SELF_CHECK_COMPLETE_STEP + 1: if(g_ptz.hori_ps_sw3_state == PS_NO_COVER) { OSTimeDlyHMSM(0u, 0u, 3u, 0u);//延时转动 ptz_hori_stop(PTZ_HORI_STOP_TIME); g_ptz.hori_ps_sw3_right_fall = 0; g_ptz.hori_ps_sw3_left_rise = 0; g_ptz.hori_ps_sw3_right_rise = 0; g_ptz.hori_ps_sw3_left_fall = 0; ptz_hori_start(PTZ_HORI_DIR_RIGHT, PTZ_HORI_SELF_CHECK_SPEED); g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_COMPLETE_STEP + 2; } break; //等待第一次水平光电开关下降沿中断 case (PTZ_HORI_SELF_CHECK_COMPLETE_STEP + 2): if(g_ptz.hori_ps_sw3_right_fall >= PS_FALL_ON) { g_ptz.hori_as5047d.as5047d_data_reset = 1; g_ptz.hori_ps_sw3_right_fall = 0; g_ptz.hori_self_check++; OSTimeDlyHMSM(0u, 0u, 5u, 0u); g_ptz.hori_ps_sw3_right_fall = 0; } break; //等待第二次水平光电开关下降沿中断 case (PTZ_HORI_SELF_CHECK_COMPLETE_STEP + 3): if(g_ptz.hori_ps_sw3_right_fall >= PS_FALL_ON) { g_ptz.hori_as5047d.as5047d_cycle_num = g_ptz.hori_as5047d.as5047d_cycle_count; g_ptz.hori_as5047d.as5047d_ptz_angle_max = g_ptz.hori_as5047d.as5047d_ptz_angle_actual; g_ptz.hori_as5047d.as5047d_ptz_angle_K = 360.0 / g_ptz.hori_as5047d.as5047d_ptz_angle_max; g_ptz.hori_as5047d.as5047d_remain_angle = g_ptz.hori_as5047d.as5047d_ptz_angle_max - g_ptz.hori_as5047d.as5047d_cycle_num * 360.0; g_ptz.hori_angleP.angle_allow_max = 360.0; g_ptz.hori_angleP.angle_allow_min = 0.0; g_ptz.hori_as5047d.as5047d_state = 1; g_ptz.hori_ps_sw3_right_fall = 0; g_ptz.hori_ps_sw3_left_rise = 0; g_ptz.hori_ps_sw3_right_rise = 0; g_ptz.hori_ps_sw3_left_fall = 0; ptz_hori_stop(PTZ_HORI_STOP_TIME); g_ptz.hori_angle_state = 1; g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_DATA_SAVE; } break; } return g_ptz.hori_self_check; } /// 云台水平简化自检 static unsigned char ptz_hori_simplify_self_check_task() { switch(g_ptz.hori_self_check) { case PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP: if(g_ptz.hori_ps_sw3_state == PS_COVER)//如果水平光电开关被挡住 { ptz_hori_start(PTZ_HORI_DIR_LEFT, PTZ_HORI_SELF_CHECK_SPEED); g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 1; } else//没有挡住 { g_ptz.hori_ps_sw3_right_fall = 0; g_ptz.hori_ps_sw3_left_rise = 0; g_ptz.hori_ps_sw3_right_rise = 0; g_ptz.hori_ps_sw3_left_fall = 0; ptz_hori_start(PTZ_HORI_DIR_RIGHT, PTZ_HORI_SELF_CHECK_SPEED); g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 2; } break; case PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 1://向左转动，直到光电开关脱离挡片遮挡 if(g_ptz.hori_ps_sw3_state == PS_NO_COVER) { OSTimeDlyHMSM(0u, 0u, 3u, 0u);//延时转动 ptz_hori_stop(PTZ_HORI_STOP_TIME); g_ptz.hori_ps_sw3_right_fall = 0; g_ptz.hori_ps_sw3_left_rise = 0; g_ptz.hori_ps_sw3_right_rise = 0; g_ptz.hori_ps_sw3_left_fall = 0; ptz_hori_start(PTZ_HORI_DIR_RIGHT, PTZ_HORI_SELF_CHECK_SPEED); g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 2; } break; case PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 2: //等待水平光电开关右转下降沿 if(g_ptz.hori_ps_sw3_right_fall >= PS_FALL_ON) { g_ptz.hori_as5047d.as5047d_cycle_count = 0; g_ptz.hori_as5047d.as5047d_ptz_angle_actual = 0; g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last = 0; g_ptz.hori_as5047d.as5047d_ptz_init_angle = g_ptz.hori_as5047d.as5047d_angle_actual; g_ptz.hori_as5047d.as5047d_state = 1; g_ptz.hori_ps_sw3_right_fall = 0; g_ptz.hori_ps_sw3_left_rise = 0; g_ptz.hori_ps_sw3_right_rise = 0; g_ptz.hori_ps_sw3_left_fall = 0; g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 3; } break; case PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 3://转动到0位 OSTimeDlyHMSM(0u, 0u, 1u, 0u);//延时转动 g_ptz.hori_angle_control = 0.0; g_ptz.hori_speed_control = PTZ_HORI_BEST_SPEED; ptz_hori_rotate_plan(PTZ_HORI_MIN_DISTANCE); g_ptz.hori_rotate_monitor_switch = PTZ_HORI_GO_TO_ANGLE_A; g_ptz.hori_angle_state = 1; g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_END; break; } return g_ptz.hori_self_check; } /// 云台垂直全范围自检 static unsigned char ptz_vert_complete_self_check_task() { switch(g_ptz.vert_self_check) { ///首先读取光电开关的状态，初步判断垂直状态 case PTZ_VERT_SELF_CHECK_COMPLETE_STEP: g_ptz.vert_ps_sw1_down_fall = 0; g_ptz.vert_ps_sw1_up_rise = 0; g_ptz.vert_ps_sw2_up_fall = 0; g_ptz.vert_ps_sw2_down_rise = 0; if(g_ptz.vert_ps_sw1_state == PS_COVER)//云台处于下俯极限位置 { ptz_vert_start(PTZ_VERT_DIR_UP, PTZ_VERT_SELF_CHECK_SPEED); g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 1; } if(g_ptz.vert_ps_sw2_state == PS_COVER)//云台处于上仰极限位置 { g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 2; } if(g_ptz.vert_ps_sw1_state == PS_NO_COVER && g_ptz.vert_ps_sw2_state == PS_NO_COVER) //云台处于中间位置 { g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 2; } break; case (PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 1): if(g_ptz.vert_ps_sw1_state == PS_NO_COVER) { OSTimeDlyHMSM(0u, 0u, 3u, 0u);//延时转动 ptz_vert_stop(PTZ_VERT_STOP_TIME); g_ptz.vert_ps_sw1_down_fall = 0; g_ptz.vert_ps_sw1_up_rise = 0; g_ptz.vert_ps_sw2_up_fall = 0; g_ptz.vert_ps_sw2_down_rise = 0; g_ptz.vert_self_check ++; } break; //开启向下转动，查找下俯极限位置 case (PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 2): ptz_vert_start(PTZ_VERT_DIR_DOWN, PTZ_VERT_SELF_CHECK_SPEED); g_ptz.vert_self_check ++; break; #ifdef PTZ_SW1_DOWN_FALL_UPDATE //判断是否到达下俯极限位置,然后云台开始向上转动 case (PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 3): if(g_ptz.vert_ps_sw1_down_fall >= 1 || g_ptz.vert_ps_sw1_state == PS_COVER) { g_ptz.vert_ps_sw1_down_fall = 0; g_ptz.vert_as5047d.as5047d_data_reset = 1; ptz_vert_stop(PTZ_VERT_STOP_TIME); ptz_vert_start(PTZ_VERT_DIR_UP, PTZ_VERT_SELF_CHECK_SPEED); g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 10; } break; #endif #ifdef PTZ_SW1_UP_RISE_UPDATE //判断是否到达下俯极限位置,然后云台开始向上转动 case (PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 3): if(g_ptz.vert_ps_sw1_down_fall >= 1 || g_ptz.vert_ps_sw1_state == PS_COVER) { g_ptz.vert_ps_sw1_down_fall = 0; g_ptz.vert_ps_sw1_up_rise = 0; OSTimeDlyHMSM(0u, 0u, 0u, 300u); ptz_vert_stop(PTZ_VERT_STOP_TIME); ptz_vert_start(PTZ_VERT_DIR_UP, PTZ_VERT_SELF_CHECK_SPEED); g_ptz.vert_self_check++; } break; //等待上仰上升沿 case (PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 4): if(g_ptz.vert_ps_sw1_up_rise >= 1 || g_ptz.vert_ps_sw1_state == PS_NO_COVER) { g_ptz.vert_as5047d.as5047d_data_reset = 1; g_ptz.vert_ps_sw1_down_fall = 0; g_ptz.vert_ps_sw1_up_rise = 0; g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 10; } break; #endif //等待SW2下降沿,计算垂直转动范围 case (PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 10): if(g_ptz.vert_ps_sw2_up_fall >= 1 || g_ptz.vert_ps_sw2_state == PS_COVER) { g_ptz.vert_as5047d.as5047d_cycle_num = g_ptz.vert_as5047d.as5047d_cycle_count; g_ptz.vert_as5047d.as5047d_ptz_angle_max = g_ptz.vert_as5047d.as5047d_ptz_angle_actual; g_ptz.vert_angleP.angle_range = g_ptz.vert_as5047d.as5047d_ptz_angle_max / PTZ_VERT_BIG_GEAR_RATIO; 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_as5047d.as5047d_ptz_angle_K = 1 / PTZ_VERT_BIG_GEAR_RATIO; g_ptz.vert_as5047d.as5047d_remain_angle = g_ptz.vert_as5047d.as5047d_ptz_angle_max - g_ptz.vert_as5047d.as5047d_cycle_num * 360.0; g_ptz.vert_as5047d.as5047d_state = 1; g_ptz.vert_ps_sw1_down_fall = 0; g_ptz.vert_ps_sw1_up_rise = 0; g_ptz.vert_ps_sw2_up_fall = 0; g_ptz.vert_ps_sw2_down_rise = 0; ptz_vert_stop(PTZ_VERT_STOP_TIME); g_ptz.vert_angle_state = 1; g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_DATA_SAVE; } break; } return g_ptz.vert_self_check; } /// 云台垂直简化自检 static unsigned char ptz_vert_simplify_self_check_task() { switch(g_ptz.vert_self_check) { case PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP: g_ptz.vert_ps_sw1_down_fall = 0; g_ptz.vert_ps_sw1_up_rise = 0; g_ptz.vert_ps_sw2_up_fall = 0; g_ptz.vert_ps_sw2_down_rise = 0; if(g_ptz.vert_ps_sw1_state == PS_COVER)//云台处于下俯极限位置 { ptz_vert_start(PTZ_VERT_DIR_UP, PTZ_VERT_SELF_CHECK_SPEED); g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 1; } if(g_ptz.vert_ps_sw2_state == PS_COVER)//云台处于上仰极限位置 { ptz_vert_start(PTZ_VERT_DIR_DOWN, PTZ_VERT_SELF_CHECK_SPEED); g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 2; } if(g_ptz.vert_ps_sw1_state == PS_NO_COVER && g_ptz.vert_ps_sw2_state == PS_NO_COVER) //云台处于中间位置 { ptz_vert_start(PTZ_VERT_DIR_DOWN, PTZ_VERT_SELF_CHECK_SPEED); g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 2; } break; case PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 1: if(g_ptz.vert_ps_sw1_state == PS_NO_COVER) { OSTimeDlyHMSM(0u, 0u, 3u, 0u);//延时转动 ptz_vert_stop(PTZ_VERT_STOP_TIME); g_ptz.vert_ps_sw1_down_fall = 0; g_ptz.vert_ps_sw1_up_rise = 0; g_ptz.vert_ps_sw2_up_fall = 0; g_ptz.vert_ps_sw2_down_rise = 0; ptz_vert_start(PTZ_VERT_DIR_DOWN, PTZ_VERT_SELF_CHECK_SPEED); g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 2; } break; #ifdef PTZ_SW1_DOWN_FALL_UPDATE case PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 2: if(g_ptz.vert_ps_sw1_down_fall >= 1 || g_ptz.vert_ps_sw1_state == PS_COVER) {//等待SW1光电开关被遮挡，同时更新当前位置 g_ptz.vert_as5047d.as5047d_cycle_count = 0; g_ptz.vert_as5047d.as5047d_ptz_angle_actual = 0; g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = 0; g_ptz.vert_as5047d.as5047d_ptz_init_angle = g_ptz.vert_as5047d.as5047d_angle_actual; g_ptz.vert_as5047d.as5047d_state = 1; g_ptz.vert_ps_sw1_down_fall = 0; g_ptz.vert_ps_sw1_up_rise = 0; g_ptz.vert_ps_sw2_up_fall = 0; g_ptz.vert_ps_sw2_down_rise = 0; ptz_vert_stop(PTZ_VERT_STOP_TIME); g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 10; } break; #endif #ifdef PTZ_SW1_UP_RISE_UPDATE //判断是否到达下俯极限位置,然后云台开始向上转动 case (PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 2): if(g_ptz.vert_ps_sw1_down_fall >= 1 || g_ptz.vert_ps_sw1_state == PS_COVER) { g_ptz.vert_ps_sw1_down_fall = 0; g_ptz.vert_ps_sw1_up_rise = 0; g_ptz.vert_ps_sw2_up_fall = 0; g_ptz.vert_ps_sw2_down_rise = 0; OSTimeDlyHMSM(0u, 0u, 0u, 300u); ptz_vert_stop(PTZ_VERT_STOP_TIME); ptz_vert_start(PTZ_VERT_DIR_UP, PTZ_VERT_SELF_CHECK_SPEED); g_ptz.vert_self_check++; } break; //等待上仰上升沿 case (PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 3): if(g_ptz.vert_ps_sw1_up_rise >= 1 || g_ptz.vert_ps_sw1_state == PS_NO_COVER) { //等待SW1光电开关被遮挡，同时更新当前位置 g_ptz.vert_as5047d.as5047d_cycle_count = 0; g_ptz.vert_as5047d.as5047d_ptz_angle_actual = 0; g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = 0; g_ptz.vert_as5047d.as5047d_ptz_init_angle = g_ptz.vert_as5047d.as5047d_angle_actual; g_ptz.vert_as5047d.as5047d_state = 1; g_ptz.vert_ps_sw1_down_fall = 0; g_ptz.vert_ps_sw1_up_rise = 0; g_ptz.vert_ps_sw2_up_fall = 0; g_ptz.vert_ps_sw2_down_rise = 0; g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 10; } break; #endif case PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 10: OSTimeDlyHMSM(0u, 0u, 1u, 0u); g_ptz.vert_angle_control = 0.0; g_ptz.vert_speed_control = PTZ_VERT_BEST_SPEED; ptz_vert_rotate_plan(PTZ_VERT_ANGLE); g_ptz.vert_rotate_monitor_switch = PTZ_VERT_GO_TO_ANGLE_A; g_ptz.vert_angle_state = 1; g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_END; break; } return g_ptz.vert_self_check; } #endif #ifdef PTZ_STEP_MOTOR /// 云台全范围自检 static unsigned char ptz_hori_complete_self_check_task() { TMC2160Control data; switch(g_ptz.hori_self_check) { //首先让云台水平向右转动 case PTZ_HORI_SELF_CHECK_COMPLETE_STEP: data = ptz_hori_choice_microstep(PTZ_HORI_SELF_CHECK_SPEED); g_ptz.hori_tmc2160 = data; if(g_ptz.hori_ps_sw3_state == PS_COVER)//如果水平光电开关被挡住 { ptz_hori_start(PTZ_HORI_DIR_LEFT, PTZ_HORI_SELF_CHECK_SPEED); g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_COMPLETE_STEP + 1; } else//没有被挡住 { g_ptz.hori_ps_sw3_right_fall = 0; g_ptz.hori_ps_sw3_left_rise = 0; g_ptz.hori_ps_sw3_right_rise = 0; g_ptz.hori_ps_sw3_left_fall = 0; ptz_hori_start(PTZ_HORI_DIR_RIGHT, PTZ_HORI_SELF_CHECK_SPEED); g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_COMPLETE_STEP + 2; } break; case PTZ_HORI_SELF_CHECK_COMPLETE_STEP + 1: if(g_ptz.hori_ps_sw3_state == PS_NO_COVER) { OSTimeDlyHMSM(0u, 0u, 3u, 0u);//延时转动 ptz_hori_stop(PTZ_HORI_STOP_TIME); g_ptz.hori_ps_sw3_right_fall = 0; g_ptz.hori_ps_sw3_left_rise = 0; g_ptz.hori_ps_sw3_right_rise = 0; g_ptz.hori_ps_sw3_left_fall = 0; ptz_hori_start(PTZ_HORI_DIR_RIGHT, PTZ_HORI_SELF_CHECK_SPEED); g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_COMPLETE_STEP + 2; } break; //等待第一次水平光电开关下降沿中断 case (PTZ_HORI_SELF_CHECK_COMPLETE_STEP + 2): if(g_ptz.hori_ps_sw3_right_fall >= PS_FALL_ON) { g_ptz.hori_as5047d.as5047d_data_reset = 1; g_ptz.hori_ps_sw3_right_fall = 0; g_ptz.hori_self_check++; OSTimeDlyHMSM(0u, 0u, 5u, 0u); g_ptz.hori_ps_sw3_right_fall = 0; } break; //等待第二次水平光电开关下降沿中断 case (PTZ_HORI_SELF_CHECK_COMPLETE_STEP + 3): if(g_ptz.hori_ps_sw3_right_fall >= PS_FALL_ON) { g_ptz.hori_as5047d.as5047d_cycle_num = g_ptz.hori_as5047d.as5047d_cycle_count; g_ptz.hori_as5047d.as5047d_ptz_angle_max = g_ptz.hori_as5047d.as5047d_ptz_angle_actual; g_ptz.hori_as5047d.as5047d_ptz_angle_K = 360.0 / g_ptz.hori_as5047d.as5047d_ptz_angle_max; g_ptz.hori_as5047d.as5047d_remain_angle = g_ptz.hori_as5047d.as5047d_ptz_angle_max - g_ptz.hori_as5047d.as5047d_cycle_num * 360.0; g_ptz.hori_angleP.angle_allow_max = 360.0; g_ptz.hori_angleP.angle_allow_min = 0.0; g_ptz.hori_as5047d.as5047d_state = 1; g_ptz.hori_ps_sw3_right_fall = 0; g_ptz.hori_ps_sw3_left_rise = 0; g_ptz.hori_ps_sw3_right_rise = 0; g_ptz.hori_ps_sw3_left_fall = 0; ptz_hori_stop(PTZ_HORI_STOP_TIME); g_ptz.hori_angle_state = 1; g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_DATA_SAVE; } break; } return g_ptz.hori_self_check; } /// 云台水平简化自检 static unsigned char ptz_hori_simplify_self_check_task() { TMC2160Control data; switch(g_ptz.hori_self_check) { case PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP: data = ptz_hori_choice_microstep(PTZ_HORI_SELF_CHECK_SPEED); g_ptz.hori_tmc2160 = data; if(g_ptz.hori_ps_sw3_state == PS_COVER)//如果水平光电开关被挡住 { ptz_hori_start(PTZ_HORI_DIR_LEFT, PTZ_HORI_SELF_CHECK_SPEED); g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 1; } else//没有挡住 { g_ptz.hori_ps_sw3_right_fall = 0; g_ptz.hori_ps_sw3_left_rise = 0; g_ptz.hori_ps_sw3_right_rise = 0; g_ptz.hori_ps_sw3_left_fall = 0; ptz_hori_start(PTZ_HORI_DIR_RIGHT, PTZ_HORI_SELF_CHECK_SPEED); g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 2; } break; case PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 1://向左转动，直到光电开关脱离挡片遮挡 if(g_ptz.hori_ps_sw3_state == PS_NO_COVER) { OSTimeDlyHMSM(0u, 0u, 3u, 0u);//延时转动 ptz_hori_stop(PTZ_HORI_STOP_TIME); g_ptz.hori_ps_sw3_right_fall = 0; g_ptz.hori_ps_sw3_left_rise = 0; g_ptz.hori_ps_sw3_right_rise = 0; g_ptz.hori_ps_sw3_left_fall = 0; ptz_hori_start(PTZ_HORI_DIR_RIGHT, PTZ_HORI_SELF_CHECK_SPEED); g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 2; } break; case PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 2: //等待水平光电开关右转下降沿 if(g_ptz.hori_ps_sw3_right_fall >= PS_FALL_ON) { g_ptz.hori_as5047d.as5047d_cycle_count = 0; g_ptz.hori_as5047d.as5047d_ptz_angle_actual = 0; g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last = 0; g_ptz.hori_as5047d.as5047d_ptz_init_angle = g_ptz.hori_as5047d.as5047d_angle_actual; g_ptz.hori_as5047d.as5047d_state = 1; g_ptz.hori_ps_sw3_right_fall = 0; g_ptz.hori_ps_sw3_left_rise = 0; g_ptz.hori_ps_sw3_right_rise = 0; g_ptz.hori_ps_sw3_left_fall = 0; g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 3; } break; case PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 3://转动到0位 OSTimeDlyHMSM(0u, 0u, 1u, 0u);//延时转动 g_ptz.hori_angle_control = 0.0; g_ptz.hori_speed_control = PTZ_HORI_BEST_SPEED; ptz_hori_rotate_plan(PTZ_HORI_MIN_DISTANCE); g_ptz.hori_rotate_monitor_switch = PTZ_HORI_GO_TO_ANGLE_A; g_ptz.hori_angle_state = 1; g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_END; break; } return g_ptz.hori_self_check; } /// 云台垂直全范围自检 static unsigned char ptz_vert_complete_self_check_task() { TMC2160Control data; switch(g_ptz.vert_self_check) { ///首先读取光电开关的状态，初步判断垂直状态 case PTZ_VERT_SELF_CHECK_COMPLETE_STEP: data = ptz_vert_choice_microstep(PTZ_VERT_SELF_CHECK_SPEED); g_ptz.vert_tmc2160 = data; g_ptz.vert_ps_sw1_down_fall = 0; g_ptz.vert_ps_sw1_up_rise = 0; g_ptz.vert_ps_sw2_up_fall = 0; g_ptz.vert_ps_sw2_down_rise = 0; if(g_ptz.vert_ps_sw1_state == PS_COVER)//云台处于下俯极限位置 { ptz_vert_start(PTZ_VERT_DIR_UP, PTZ_VERT_SELF_CHECK_SPEED); g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 1; } if(g_ptz.vert_ps_sw2_state == PS_COVER)//云台处于上仰极限位置 { g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 2; } if(g_ptz.vert_ps_sw1_state == PS_NO_COVER && g_ptz.vert_ps_sw2_state == PS_NO_COVER) //云台处于中间位置 { g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 2; } break; case (PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 1): if(g_ptz.vert_ps_sw1_state == PS_NO_COVER) { OSTimeDlyHMSM(0u, 0u, 3u, 0u);//延时转动 ptz_vert_stop(PTZ_VERT_STOP_TIME); g_ptz.vert_ps_sw1_down_fall = 0; g_ptz.vert_ps_sw1_up_rise = 0; g_ptz.vert_ps_sw2_up_fall = 0; g_ptz.vert_ps_sw2_down_rise = 0; g_ptz.vert_self_check ++; } break; //开启向下转动，查找下俯极限位置 case (PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 2): ptz_vert_start(PTZ_VERT_DIR_DOWN, PTZ_VERT_SELF_CHECK_SPEED); g_ptz.vert_self_check ++; break; #ifdef PTZ_SW1_DOWN_FALL_UPDATE //判断是否到达下俯极限位置,然后云台开始向上转动 case (PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 3): if(g_ptz.vert_ps_sw1_down_fall >= 1 || g_ptz.vert_ps_sw1_state == PS_COVER) { g_ptz.vert_ps_sw1_down_fall = 0; g_ptz.vert_as5047d.as5047d_data_reset = 1; ptz_vert_stop(PTZ_VERT_STOP_TIME); ptz_vert_start(PTZ_VERT_DIR_UP, PTZ_VERT_SELF_CHECK_SPEED); g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 10; } break; #endif #ifdef PTZ_SW1_UP_RISE_UPDATE //判断是否到达下俯极限位置,然后云台开始向上转动 case (PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 3): if(g_ptz.vert_ps_sw1_down_fall >= 1 || g_ptz.vert_ps_sw1_state == PS_COVER) { g_ptz.vert_ps_sw1_down_fall = 0; g_ptz.vert_ps_sw1_up_rise = 0; OSTimeDlyHMSM(0u, 0u, 0u, 300u); ptz_vert_stop(PTZ_VERT_STOP_TIME); ptz_vert_start(PTZ_VERT_DIR_UP, PTZ_VERT_SELF_CHECK_SPEED); g_ptz.vert_self_check++; } break; //等待上仰上升沿 case (PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 4): if(g_ptz.vert_ps_sw1_up_rise >= 1 || g_ptz.vert_ps_sw1_state == PS_NO_COVER) { g_ptz.vert_as5047d.as5047d_data_reset = 1; g_ptz.vert_ps_sw1_down_fall = 0; g_ptz.vert_ps_sw1_up_rise = 0; g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 10; } break; #endif //等待SW2下降沿,计算垂直转动范围 case (PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 10): if(g_ptz.vert_ps_sw2_up_fall >= 1 || g_ptz.vert_ps_sw2_state == PS_COVER) { g_ptz.vert_as5047d.as5047d_cycle_num = g_ptz.vert_as5047d.as5047d_cycle_count; g_ptz.vert_as5047d.as5047d_ptz_angle_max = g_ptz.vert_as5047d.as5047d_ptz_angle_actual; g_ptz.vert_angleP.angle_range = g_ptz.vert_as5047d.as5047d_ptz_angle_max / PTZ_VERT_BIG_GEAR_RATIO; 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_as5047d.as5047d_ptz_angle_K = 1 / PTZ_VERT_BIG_GEAR_RATIO; g_ptz.vert_as5047d.as5047d_remain_angle = g_ptz.vert_as5047d.as5047d_ptz_angle_max - g_ptz.vert_as5047d.as5047d_cycle_num * 360.0; g_ptz.vert_as5047d.as5047d_state = 1; g_ptz.vert_ps_sw1_down_fall = 0; g_ptz.vert_ps_sw1_up_rise = 0; g_ptz.vert_ps_sw2_up_fall = 0; g_ptz.vert_ps_sw2_down_rise = 0; ptz_vert_stop(PTZ_VERT_STOP_TIME); g_ptz.vert_angle_state = 1; g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_DATA_SAVE; } break; } return g_ptz.vert_self_check; } /// 云台垂直简化自检 static unsigned char ptz_vert_simplify_self_check_task() { TMC2160Control data; switch(g_ptz.vert_self_check) { case PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP: data = ptz_vert_choice_microstep(PTZ_VERT_SELF_CHECK_SPEED); g_ptz.vert_tmc2160 = data; g_ptz.vert_ps_sw1_down_fall = 0; g_ptz.vert_ps_sw1_up_rise = 0; g_ptz.vert_ps_sw2_up_fall = 0; g_ptz.vert_ps_sw2_down_rise = 0; if(g_ptz.vert_ps_sw1_state == PS_COVER)//云台处于下俯极限位置 { ptz_vert_start(PTZ_VERT_DIR_UP, PTZ_VERT_SELF_CHECK_SPEED); g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 1; } if(g_ptz.vert_ps_sw2_state == PS_COVER)//云台处于上仰极限位置 { ptz_vert_start(PTZ_VERT_DIR_DOWN, PTZ_VERT_SELF_CHECK_SPEED); g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 2; } if(g_ptz.vert_ps_sw1_state == PS_NO_COVER && g_ptz.vert_ps_sw2_state == PS_NO_COVER) //云台处于中间位置 { ptz_vert_start(PTZ_VERT_DIR_DOWN, PTZ_VERT_SELF_CHECK_SPEED); g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 2; } break; case PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 1: if(g_ptz.vert_ps_sw1_state == PS_NO_COVER) { OSTimeDlyHMSM(0u, 0u, 3u, 0u);//延时转动 ptz_vert_stop(PTZ_VERT_STOP_TIME); g_ptz.vert_ps_sw1_down_fall = 0; g_ptz.vert_ps_sw1_up_rise = 0; g_ptz.vert_ps_sw2_up_fall = 0; g_ptz.vert_ps_sw2_down_rise = 0; ptz_vert_start(PTZ_VERT_DIR_DOWN, PTZ_VERT_SELF_CHECK_SPEED); g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 2; } break; #ifdef PTZ_SW1_DOWN_FALL_UPDATE case PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 2: if(g_ptz.vert_ps_sw1_down_fall >= 1 || g_ptz.vert_ps_sw1_state == PS_COVER) {//等待SW1光电开关被遮挡，同时更新当前位置 g_ptz.vert_as5047d.as5047d_cycle_count = 0; g_ptz.vert_as5047d.as5047d_ptz_angle_actual = 0; g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = 0; g_ptz.vert_as5047d.as5047d_ptz_init_angle = g_ptz.vert_as5047d.as5047d_angle_actual; g_ptz.vert_as5047d.as5047d_state = 1; g_ptz.vert_ps_sw1_down_fall = 0; g_ptz.vert_ps_sw1_up_rise = 0; g_ptz.vert_ps_sw2_up_fall = 0; g_ptz.vert_ps_sw2_down_rise = 0; ptz_vert_stop(PTZ_VERT_STOP_TIME); g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 10; } break; #endif #ifdef PTZ_SW1_UP_RISE_UPDATE //判断是否到达下俯极限位置,然后云台开始向上转动 case (PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 2): if(g_ptz.vert_ps_sw1_down_fall >= 1 || g_ptz.vert_ps_sw1_state == PS_COVER) { g_ptz.vert_ps_sw1_down_fall = 0; g_ptz.vert_ps_sw1_up_rise = 0; g_ptz.vert_ps_sw2_up_fall = 0; g_ptz.vert_ps_sw2_down_rise = 0; OSTimeDlyHMSM(0u, 0u, 0u, 300u); ptz_vert_stop(PTZ_VERT_STOP_TIME); ptz_vert_start(PTZ_VERT_DIR_UP, PTZ_VERT_SELF_CHECK_SPEED); g_ptz.vert_self_check++; } break; //等待上仰上升沿 case (PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 3): if(g_ptz.vert_ps_sw1_up_rise >= 1 || g_ptz.vert_ps_sw1_state == PS_NO_COVER) { //等待SW1光电开关被遮挡，同时更新当前位置 g_ptz.vert_as5047d.as5047d_cycle_count = 0; g_ptz.vert_as5047d.as5047d_ptz_angle_actual = 0; g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = 0; g_ptz.vert_as5047d.as5047d_ptz_init_angle = g_ptz.vert_as5047d.as5047d_angle_actual; g_ptz.vert_as5047d.as5047d_state = 1; g_ptz.vert_ps_sw1_down_fall = 0; g_ptz.vert_ps_sw1_up_rise = 0; g_ptz.vert_ps_sw2_up_fall = 0; g_ptz.vert_ps_sw2_down_rise = 0; g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 10; } break; #endif case PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 10: OSTimeDlyHMSM(0u, 0u, 1u, 0u); data = ptz_vert_choice_microstep(PTZ_VERT_BEST_SPEED); g_ptz.vert_tmc2160 = data; g_ptz.vert_angle_control = 0.0; g_ptz.vert_speed_control = PTZ_VERT_BEST_SPEED; ptz_vert_rotate_plan(PTZ_VERT_ANGLE); g_ptz.vert_rotate_monitor_switch = PTZ_VERT_GO_TO_ANGLE_A; g_ptz.vert_angle_state = 1; g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_END; break; } return g_ptz.vert_self_check; } #endif static void ptz_hori_self_check_task() { while(1) { if(error_conut_state == 0) {//误差计算结束后开始自检 ptz_hori_complete_self_check_task(); ptz_hori_simplify_self_check_task(); } //保存自检参数 if(g_ptz.vert_self_check == PTZ_VERT_SELF_CHECK_DATA_SAVE && g_ptz.hori_self_check == PTZ_HORI_SELF_CHECK_DATA_SAVE) { ptz_self_check_data_save();//保存数据 //转到垂直0位 g_ptz.vert_angle_control = 0.0; g_ptz.vert_speed_control = PTZ_VERT_BEST_SPEED; ptz_vert_rotate_plan(PTZ_VERT_ANGLE); g_ptz.vert_rotate_monitor_switch = PTZ_VERT_GO_TO_ANGLE_A; g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_END; //转到水平0位 g_ptz.hori_angle_control = 0.0; g_ptz.hori_speed_control = PTZ_HORI_BEST_SPEED; ptz_hori_rotate_plan(PTZ_HORI_MIN_DISTANCE); g_ptz.hori_rotate_monitor_switch = PTZ_HORI_GO_TO_ANGLE_A; g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_END; } if(g_ptz.hori_self_check == PTZ_HORI_SELF_CHECK_END && g_ptz.vert_self_check == PTZ_VERT_SELF_CHECK_END) { #ifdef PTZ_CMD_SAVE_BEFORE ptz_cmd_before_start();//查看是否让云台自动进行工作 #endif OSTaskDel(OS_PRIO_SELF);//关闭该任务 } OSTimeDlyHMSM(0u, 0u, 0u, 1u); } } static void ptz_vert_self_check_task() { while(1) { if(error_conut_state == 0) {//误差计算结束后开始自检 ptz_vert_complete_self_check_task(); ptz_vert_simplify_self_check_task(); } if(g_ptz.vert_self_check == PTZ_VERT_SELF_CHECK_END) { 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; } OSTaskDel(OS_PRIO_SELF);//关闭该任务 } OSTimeDlyHMSM(0u, 0u, 0u, 1u); } } //水平自检任务 static OS_STK task_hori_self_check_stk[TASK_HORI_SELF_CHECK_STK_SIZE]; static void creat_task_hori_self_check(void) { CPU_INT08U task_err; CPU_INT08U name_err; task_err = OSTaskCreateExt((void (*)(void *)) ptz_hori_self_check_task, (void *) 0, (OS_STK *)&task_hori_self_check_stk[TASK_HORI_SELF_CHECK_STK_SIZE - 1], (INT8U ) TASK_HORI_SELF_CHECK_PRIO, (INT16U ) TASK_HORI_SELF_CHECK_PRIO, (OS_STK *)&task_hori_self_check_stk[0], (INT32U ) TASK_HORI_SELF_CHECK_STK_SIZE, (void *) 0, (INT16U )(OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR)); #if (OS_TASK_NAME_EN > 0) OSTaskNameSet(TASK_HORI_SELF_CHECK_PRIO, "ptz_hori_self_check_task", &name_err); #endif if ((task_err == OS_ERR_NONE) && (name_err == OS_ERR_NONE)) { pdebug(DEBUG_LEVEL_INFO,"create ptz_hori_self_check_task success...\n\r"); } else { pdebug(DEBUG_LEVEL_FATAL,"create ptz_hori_self_check_task failed...\n\r"); } } //垂直自检任务 static OS_STK task_vert_self_check_stk[TASK_VERT_SELF_CHECK_STK_SIZE]; static void creat_task_vert_self_check(void) { CPU_INT08U task_err; CPU_INT08U name_err; task_err = OSTaskCreateExt((void (*)(void *)) ptz_vert_self_check_task, (void *) 0, (OS_STK *)&task_vert_self_check_stk[TASK_VERT_SELF_CHECK_STK_SIZE - 1], (INT8U ) TASK_VERT_SELF_CHECK_PRIO, (INT16U ) TASK_VERT_SELF_CHECK_PRIO, (OS_STK *)&task_vert_self_check_stk[0], (INT32U ) TASK_VERT_SELF_CHECK_STK_SIZE, (void *) 0, (INT16U )(OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR)); #if (OS_TASK_NAME_EN > 0) OSTaskNameSet(TASK_VERT_SELF_CHECK_PRIO, "ptz_vert_self_check_task", &name_err); #endif if ((task_err == OS_ERR_NONE) && (name_err == OS_ERR_NONE)) { pdebug(DEBUG_LEVEL_INFO,"create ptz_vert_self_check_task success...\n\r"); } else { pdebug(DEBUG_LEVEL_FATAL,"create ptz_vert_self_check_task failed...\n\r"); } } /* @brief 自检参数初始化和自检方式选择 @param @return @note 20180903 v1.0 20220602 v2.0 lqc */ /* static void ptz_self_check_data_init() { if(ptz_self_check_data_read() == 1)//自检参数读取成功，进行简易自检 { #ifdef PTZ_NO_SELF_CHECK if(ptz_power_off_data_read() == 1) { g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_END; g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_END; g_ptz.hori_angle_state = 1; g_ptz.vert_angle_state = 1; g_ptz.no_self_check_state = 1;//读取成功 return; } g_ptz.no_self_check_state = 0;//读取失败 #endif g_ptz.hori_angle_state = 0; g_ptz.vert_angle_state = 0; g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP; g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP; #ifdef PTZ_VERT_SIMPLIFY_CHECK_OFF//不要垂直简易自检 g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_END; g_ptz.vert_as5047d.as5047d_cycle_count = g_ptz.vert_as5047d.as5047d_cycle_num / 2; g_ptz.vert_as5047d.as5047d_state = 1; #endif #ifdef PTZ_HORI_SIMPLIFY_CHECK_OFF//不要水平简易自检 g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_END; g_ptz.hori_as5047d.as5047d_cycle_count = g_ptz.hori_as5047d.as5047d_cycle_num / 2; g_ptz.hori_as5047d.as5047d_state = 1; #endif } else//自检参数失败进行全范围自检 { g_ptz.hori_angle_state = 0; g_ptz.vert_angle_state = 0; g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_COMPLETE_STEP; g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_COMPLETE_STEP; #ifdef PTZ_VERT_COMPLETE_CHECK_OFF//不要垂直全范围自检 g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_DATA_SAVE; g_ptz.vert_as5047d.as5047d_cycle_count = g_ptz.vert_as5047d.as5047d_cycle_num / 2; g_ptz.vert_as5047d.as5047d_state = 1; #endif #ifdef PTZ_HORI_COMPLETE_CHECK_OFF//不要水平全范围自检 g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_DATA_SAVE; g_ptz.hori_as5047d.as5047d_cycle_count = g_ptz.hori_as5047d.as5047d_cycle_num / 2; g_ptz.hori_as5047d.as5047d_state = 1; #endif } } */ /************通过判断配置信息来初始化自检方式***********/ #ifdef ONE //自检参数初始化和自检方式选择 static void ptz_self_check_data_init() { if(strstr((const char*)system_info.complete_self_check, "enable")) {//全自检使能 if(ptz_self_check_data_read() == 1)//自检参数读取成功，进行简易自检 {//自检参数读取成功，进入简易自检 if(strstr((const char*)system_info.simplify_self_check, "both_enable")) {//双轴简易自检 g_ptz.no_self_check_state = 0;//读取失败 g_ptz.hori_angle_state = 0; g_ptz.vert_angle_state = 0; g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP; g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP; g_ptz.hori_as5047d.as5047d_state = 0; g_ptz.vert_as5047d.as5047d_state = 0; return; } if(strstr((const char*)system_info.simplify_self_check, "hori_enable")) {//水平简易自检、垂直不自检 if(ptz_power_off_data_read() == 1) {//掉电数据读取成功 g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP; g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_END; g_ptz.hori_angle_state = 0; g_ptz.vert_angle_state = 1; g_ptz.no_self_check_state = 1;//读取成功 g_ptz.hori_as5047d.as5047d_state = 0; }else{//掉电数据读取失败，垂直赋默认值，水平简易自检 g_ptz.no_self_check_state = 0;//读取失败 g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_END; g_ptz.vert_as5047d.as5047d_cycle_count = g_ptz.vert_as5047d.as5047d_cycle_num / 2; g_ptz.vert_as5047d.as5047d_state = 1; g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP; g_ptz.hori_as5047d.as5047d_state = 0; } return; } if(strstr((const char*)system_info.simplify_self_check, "vert_enable")) {//垂直简易自检、水平不自检 if(ptz_power_off_data_read() == 1) {//掉电数据读取成功 g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_END; g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP; g_ptz.hori_angle_state = 1; g_ptz.vert_angle_state = 0; g_ptz.no_self_check_state = 1;//读取成功 g_ptz.vert_as5047d.as5047d_state = 0; }else{//掉电数据读取失败，垂直赋默认值，水平简易自检 g_ptz.no_self_check_state = 0;//读取失败 g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP; g_ptz.vert_as5047d.as5047d_state = 0; g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_END; g_ptz.hori_as5047d.as5047d_cycle_count = g_ptz.hori_as5047d.as5047d_cycle_num / 2; g_ptz.hori_as5047d.as5047d_state = 1; } return; } if(strstr((const char*)system_info.simplify_self_check, "both_disable")) {//双轴不简易自检 if(ptz_power_off_data_read() == 1) {//掉电数据读取成功，结束自检 g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_END; g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_END; g_ptz.hori_angle_state = 1; g_ptz.vert_angle_state = 1; g_ptz.no_self_check_state = 1;//读取成功 }else{//掉电数据读取失败，赋默认值，结束自检 g_ptz.no_self_check_state = 0;//读取失败 g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_END; g_ptz.vert_as5047d.as5047d_cycle_count = g_ptz.vert_as5047d.as5047d_cycle_num / 2; g_ptz.vert_as5047d.as5047d_state = 1; g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_END; g_ptz.hori_as5047d.as5047d_cycle_count = g_ptz.hori_as5047d.as5047d_cycle_num / 2; g_ptz.hori_as5047d.as5047d_state = 1; } return; } }else{ //自检参数失败进行全范围自检 g_ptz.hori_angle_state = 0; g_ptz.vert_angle_state = 0; g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_COMPLETE_STEP; g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_COMPLETE_STEP; return; } }else{//任何情况下都不自检 if((ptz_self_check_data_read() == 1) && (ptz_power_off_data_read() == 1)) {//自检参数读取成功、且掉电数据读取成功 g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_END; g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_END; g_ptz.hori_angle_state = 1; g_ptz.vert_angle_state = 1; g_ptz.no_self_check_state = 1;//读取成功 return; }else{ g_ptz.no_self_check_state = 0;//读取失败 g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_DATA_SAVE; g_ptz.vert_as5047d.as5047d_cycle_count = g_ptz.vert_as5047d.as5047d_cycle_num / 2; g_ptz.vert_as5047d.as5047d_state = 1; g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_DATA_SAVE; g_ptz.hori_as5047d.as5047d_cycle_count = g_ptz.hori_as5047d.as5047d_cycle_num / 2; g_ptz.hori_as5047d.as5047d_state = 1; return; } } } #endif //自检参数初始化和自检方式选择 static void ptz_self_check_data_init() { if(strstr((const char*)system_info.complete_self_check, "enable")) {//全自检使能 if(ptz_self_check_data_read() == 1)//自检参数读取成功，进行简易自检 {//自检参数读取成功，进入简易自检 error_conut_state = 0;//不进行误差计算 if(strstr((const char*)system_info.simplify_self_check, "both_enabled")) {//双轴简易自检 g_ptz.no_self_check_state = 0;//读取失败 g_ptz.hori_angle_state = 0; g_ptz.vert_angle_state = 0; g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP; g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP; g_ptz.hori_as5047d.as5047d_state = 0;//编码器状态为0时，角度计算不可用，该值保存于自检参数以及掉电保护数据中，自检完成后置1， g_ptz.vert_as5047d.as5047d_state = 0; return; } if(strstr((const char*)system_info.simplify_self_check, "hori_enabled")) {//水平简易自检、垂直不自检 if(ptz_power_off_data_read() == 1) {//掉电数据读取成功 g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP; g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_END; g_ptz.hori_angle_state = 0; g_ptz.vert_angle_state = 1; g_ptz.no_self_check_state = 1;//读取成功 g_ptz.hori_as5047d.as5047d_state = 0; }else{//掉电数据读取失败，垂直赋默认值，水平简易自检 g_ptz.no_self_check_state = 0;//读取失败 g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_END; g_ptz.vert_as5047d.as5047d_cycle_count = g_ptz.vert_as5047d.as5047d_cycle_num / 2; g_ptz.vert_as5047d.as5047d_state = 1; g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP; g_ptz.hori_as5047d.as5047d_state = 0; g_ptz.hori_angle_state = 0;//掉电参数读取失败，角度状态先置故障 g_ptz.vert_angle_state = 0; } return; } if(strstr((const char*)system_info.simplify_self_check, "vert_enabled")) {//垂直简易自检、水平不自检 if(ptz_power_off_data_read() == 1) {//掉电数据读取成功 g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_END; g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP; g_ptz.hori_angle_state = 1; g_ptz.vert_angle_state = 0; g_ptz.no_self_check_state = 1;//读取成功 g_ptz.vert_as5047d.as5047d_state = 0; }else{//掉电数据读取失败，水平赋默认值，垂直简易自检 g_ptz.no_self_check_state = 0;//读取失败 g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP; g_ptz.vert_as5047d.as5047d_state = 0; g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_END; g_ptz.hori_as5047d.as5047d_cycle_count = g_ptz.hori_as5047d.as5047d_cycle_num / 2; g_ptz.hori_as5047d.as5047d_state = 1; g_ptz.hori_angle_state = 0;//掉电参数读取失败，角度状态先置故障 g_ptz.vert_angle_state = 0; } return; } if(strstr((const char*)system_info.simplify_self_check, "both_disabled")) {//双轴不简易自检 if(ptz_power_off_data_read() == 1) {//掉电数据读取成功，结束自检 g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_END; g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_END; g_ptz.hori_angle_state = 1; g_ptz.vert_angle_state = 1; g_ptz.no_self_check_state = 1;//读取成功 }else{//掉电数据读取失败，赋默认值，结束自检 g_ptz.no_self_check_state = 0;//读取失败 g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_END; g_ptz.vert_as5047d.as5047d_cycle_count = g_ptz.vert_as5047d.as5047d_cycle_num / 2; g_ptz.vert_as5047d.as5047d_state = 1; g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_END; g_ptz.hori_as5047d.as5047d_cycle_count = g_ptz.hori_as5047d.as5047d_cycle_num / 2; g_ptz.hori_as5047d.as5047d_state = 1; g_ptz.hori_angle_state = 0;//掉电参数读取失败，角度状态先置故障 g_ptz.vert_angle_state = 0; } return; } }else{ error_conut_state = 1;//开始进行误差计算 //自检参数失败进行全范围自检 g_ptz.hori_angle_state = 0; g_ptz.vert_angle_state = 0; g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_COMPLETE_STEP; g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_COMPLETE_STEP; return; } }else{//任何情况下都不自检 error_conut_state = 0;//不进行误差计算 if(ptz_self_check_data_read() == 1) {//自检参数读取成功 if(ptz_power_off_data_read() == 1) {//掉电数据读取成功 g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_END; g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_END; g_ptz.hori_angle_state = 1; g_ptz.vert_angle_state = 1; g_ptz.no_self_check_state = 1;//读取成功 return; }else{//掉电数据读取失败 g_ptz.no_self_check_state = 0;//读取失败 g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_END; g_ptz.vert_as5047d.as5047d_cycle_count = g_ptz.vert_as5047d.as5047d_cycle_num / 2; g_ptz.vert_as5047d.as5047d_state = 1; g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_END; g_ptz.hori_as5047d.as5047d_cycle_count = g_ptz.hori_as5047d.as5047d_cycle_num / 2; g_ptz.hori_as5047d.as5047d_state = 1; g_ptz.hori_angle_state = 0;//角度状态先置故障 g_ptz.vert_angle_state = 0; return; } }else{//自检参数读取失败 g_ptz.no_self_check_state = 0;//读取失败 g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_DATA_SAVE; /* 自检参数读取失败，重新赋值经验值进行保存 */ //中型蜗轮蜗杆步进电机云台 g_ptz.vert_as5047d.as5047d_ptz_angle_max = 9600.0; g_ptz.vert_as5047d.as5047d_ptz_angle_K = 1.0/80; g_ptz.vert_as5047d.as5047d_remain_angle = 230.0; g_ptz.vert_as5047d.as5047d_cycle_num = 26; g_ptz.vert_angleP.angle_min = -60.0;//云台角度最小值 g_ptz.vert_angleP.angle_max = 60.0;//云台角度最大值 g_ptz.vert_angleP.angle_allow_min = -60.0;//云台可用角度最小值 g_ptz.vert_angleP.angle_allow_max = 60.0;//云台可用角度最大值 g_ptz.vert_angleP.angle_range = 120.0;//云台角度范围 g_ptz.vert_as5047d.as5047d_state = 1; g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_DATA_SAVE; /* 自检参数读取失败，重新赋值经验值进行保存 */ g_ptz.hori_as5047d.as5047d_ptz_angle_max = 28800.0; g_ptz.hori_as5047d.as5047d_ptz_angle_K = 1.0/80; g_ptz.hori_as5047d.as5047d_remain_angle = 0.0; g_ptz.hori_as5047d.as5047d_cycle_num = 80; g_ptz.hori_angleP.angle_min = 0.0;//云台角度最小值 g_ptz.hori_angleP.angle_max = 360.0;//云台角度最大值 g_ptz.hori_angleP.angle_allow_min = 0.0;//云台可用角度最小值 g_ptz.hori_angleP.angle_allow_max = 360.0;//云台可用角度最大值 g_ptz.hori_angleP.angle_range = 360.0;//云台角度范围 g_ptz.hori_as5047d.as5047d_state = 1; g_ptz.hori_angle_state = 0;//角度状态先置故障 g_ptz.vert_angle_state = 0; } } } //自检任务初始化 void init_self_check_module(void) { // g_ptz.hori_self_check = 255; // g_ptz.vert_self_check = 255; ptz_self_check_data_init(); // init_error_count_task();//误差计算初始化 creat_task_hori_self_check(); creat_task_vert_self_check(); } //计算效验码 unsigned int ptz_self_check_data_crc(PtzSelfCheckData data) { float crc = 0; unsigned int crc1 = 0; crc = crc + data.hori_as5047d.as5047d_ptz_angle_max; crc = crc + data.hori_as5047d.as5047d_ptz_angle_K; crc = crc + data.hori_as5047d.as5047d_remain_angle; crc = crc + data.hori_as5047d.as5047d_cycle_num; crc = crc + data.vert_as5047d.as5047d_ptz_angle_max; crc = crc + data.vert_as5047d.as5047d_ptz_angle_K; crc = crc + data.vert_as5047d.as5047d_remain_angle; crc = crc + data.vert_as5047d.as5047d_cycle_num; crc = crc + data.hori_angleP.angle_min;//云台角度最小值 crc = crc + data.hori_angleP.angle_max;//云台角度最大值 crc = crc + data.hori_angleP.angle_allow_min;//云台可用角度最小值 crc = crc + data.hori_angleP.angle_allow_max;//云台可用角度最大值 crc = crc + data.hori_angleP.angle_range;//云台角度范围 crc = crc + data.vert_angleP.angle_min;//云台角度最小值 crc = crc + data.vert_angleP.angle_max;//云台角度最大值 crc = crc + data.vert_angleP.angle_allow_min;//云台可用角度最小值 crc = crc + data.vert_angleP.angle_allow_max;//云台可用角度最大值 crc = crc + data.vert_angleP.angle_range;//云台角度范围 crc = crc + data.hori_angle_error;//水平结构误差 crc1 = (unsigned int)crc; return crc1; } static PtzSelfCheckData self_check_data; static PtzSelfCheckData self_check_data_a; //保存自检参数 char ptz_self_check_data_save() { char i,j; //复制数据 memcpy(&self_check_data.hori_as5047d, &g_ptz.hori_as5047d, sizeof(g_ptz.hori_as5047d)); memcpy(&self_check_data.hori_angleP, &g_ptz.hori_angleP, sizeof(g_ptz.hori_angleP)); memcpy(&self_check_data.vert_as5047d, &g_ptz.vert_as5047d, sizeof(g_ptz.vert_as5047d)); memcpy(&self_check_data.vert_angleP, &g_ptz.vert_angleP, sizeof(g_ptz.vert_angleP)); self_check_data.hori_angle_error = g_ptz.hori_angle_error;//存储水平结构误差 //计算效验码 self_check_data.crc = ptz_self_check_data_crc(self_check_data); //写入主存储区 for(i = 0; i < SELF_CHECK_DATA_SAVE_FLASH_NUM; i++) { memset(&self_check_data_a, 0, sizeof(self_check_data_a)); write_many_data(sizeof(self_check_data),(unsigned char *)(&self_check_data),SELF_CHECK_DATA_FLASH_ADD); OSTimeDlyHMSM(0u, 0u, 0u, 5u); Flash_Read((unsigned char *)(&self_check_data_a),SELF_CHECK_DATA_FLASH_ADD,sizeof(self_check_data_a)); if(memcmp(&self_check_data, &self_check_data_a, sizeof(self_check_data)) == 0)//判断写入数据是否正确 { break; } OSTimeDlyHMSM(0u, 0u, 0u, 5u); } //写入备份存储区 for(j = 0; j < SELF_CHECK_DATA_SAVE_FLASH_NUM; j++) { memset(&self_check_data_a, 0, sizeof(self_check_data_a)); write_many_data(sizeof(self_check_data),(unsigned char *)(&self_check_data),SELF_CHECK_DATA_BACKUP_FLASH_ADD); OSTimeDlyHMSM(0u, 0u, 0u, 5u); Flash_Read((unsigned char *)(&self_check_data_a),SELF_CHECK_DATA_BACKUP_FLASH_ADD,sizeof(self_check_data_a)); if(memcmp(&self_check_data, &self_check_data_a, sizeof(self_check_data)) == 0)//判断写入数据是否正确 { break; } OSTimeDlyHMSM(0u, 0u, 0u, 5u); } if(i < SELF_CHECK_DATA_SAVE_FLASH_NUM || j < SELF_CHECK_DATA_SAVE_FLASH_NUM)//判断写入数据是否成功 { //写入成功 return 1; } //写入失败 return 0; } //自检参数读取 char ptz_self_check_data_read() { char i,j; //首先从主存储区读取数据 for(i = 0; i < SELF_CHECK_DATA_READ_FLASH_NUM; i++) { memset(&self_check_data, 0, sizeof(self_check_data)); Flash_Read((unsigned char *)(&self_check_data),SELF_CHECK_DATA_FLASH_ADD,sizeof(self_check_data)); if(self_check_data.crc == ptz_self_check_data_crc(self_check_data) && self_check_data.crc > 0) { g_ptz.hori_as5047d.as5047d_ptz_angle_max = self_check_data.hori_as5047d.as5047d_ptz_angle_max; g_ptz.hori_as5047d.as5047d_ptz_angle_K = self_check_data.hori_as5047d.as5047d_ptz_angle_K; g_ptz.hori_as5047d.as5047d_remain_angle = self_check_data.hori_as5047d.as5047d_remain_angle; g_ptz.hori_as5047d.as5047d_cycle_num = self_check_data.hori_as5047d.as5047d_cycle_num; g_ptz.vert_as5047d.as5047d_ptz_angle_max = self_check_data.vert_as5047d.as5047d_ptz_angle_max; g_ptz.vert_as5047d.as5047d_ptz_angle_K = self_check_data.vert_as5047d.as5047d_ptz_angle_K; g_ptz.vert_as5047d.as5047d_remain_angle = self_check_data.vert_as5047d.as5047d_remain_angle; g_ptz.vert_as5047d.as5047d_cycle_num = self_check_data.vert_as5047d.as5047d_cycle_num; g_ptz.hori_angleP.angle_min = self_check_data.hori_angleP.angle_min;//云台角度最小值 g_ptz.hori_angleP.angle_max = self_check_data.hori_angleP.angle_max;//云台角度最大值 g_ptz.hori_angleP.angle_allow_min = self_check_data.hori_angleP.angle_allow_min;//云台可用角度最小值 g_ptz.hori_angleP.angle_allow_max = self_check_data.hori_angleP.angle_allow_max;//云台可用角度最大值 g_ptz.hori_angleP.angle_range = self_check_data.hori_angleP.angle_range;//云台角度范围 g_ptz.vert_angleP.angle_min = self_check_data.vert_angleP.angle_min;//云台角度最小值 g_ptz.vert_angleP.angle_max = self_check_data.vert_angleP.angle_max;//云台角度最大值 g_ptz.vert_angleP.angle_allow_min = self_check_data.vert_angleP.angle_allow_min;//云台可用角度最小值 g_ptz.vert_angleP.angle_allow_max = self_check_data.vert_angleP.angle_allow_max;//云台可用角度最大值 g_ptz.vert_angleP.angle_range = self_check_data.vert_angleP.angle_range;//云台角度范围 g_ptz.hori_angle_error = self_check_data.hori_angle_error;//赋值水平结构误差 return 1; } OSTimeDlyHMSM(0u, 0u, 0u, 2u); } //如果主存储区读取的数据是错误的，则从备份存储区读取数据 for(j = 0; j < SELF_CHECK_DATA_READ_FLASH_NUM; j++) { memset(&self_check_data, 0, sizeof(self_check_data)); Flash_Read((unsigned char *)(&self_check_data),SELF_CHECK_DATA_BACKUP_FLASH_ADD,sizeof(self_check_data)); if(self_check_data.crc == ptz_self_check_data_crc(self_check_data) && self_check_data.crc > 0) { g_ptz.hori_as5047d.as5047d_ptz_angle_max = self_check_data.hori_as5047d.as5047d_ptz_angle_max; g_ptz.hori_as5047d.as5047d_ptz_angle_K = self_check_data.hori_as5047d.as5047d_ptz_angle_K; g_ptz.hori_as5047d.as5047d_remain_angle = self_check_data.hori_as5047d.as5047d_remain_angle; g_ptz.hori_as5047d.as5047d_cycle_num = self_check_data.hori_as5047d.as5047d_cycle_num; g_ptz.vert_as5047d.as5047d_ptz_angle_max = self_check_data.vert_as5047d.as5047d_ptz_angle_max; g_ptz.vert_as5047d.as5047d_ptz_angle_K = self_check_data.vert_as5047d.as5047d_ptz_angle_K; g_ptz.vert_as5047d.as5047d_remain_angle = self_check_data.vert_as5047d.as5047d_remain_angle; g_ptz.vert_as5047d.as5047d_cycle_num = self_check_data.vert_as5047d.as5047d_cycle_num; g_ptz.hori_angleP.angle_min = self_check_data.hori_angleP.angle_min;//云台角度最小值 g_ptz.hori_angleP.angle_max = self_check_data.hori_angleP.angle_max;//云台角度最大值 g_ptz.hori_angleP.angle_allow_min = self_check_data.hori_angleP.angle_allow_min;//云台可用角度最小值 g_ptz.hori_angleP.angle_allow_max = self_check_data.hori_angleP.angle_allow_max;//云台可用角度最大值 g_ptz.hori_angleP.angle_range = self_check_data.hori_angleP.angle_range;//云台角度范围 g_ptz.vert_angleP.angle_min = self_check_data.vert_angleP.angle_min;//云台角度最小值 g_ptz.vert_angleP.angle_max = self_check_data.vert_angleP.angle_max;//云台角度最大值 g_ptz.vert_angleP.angle_allow_min = self_check_data.vert_angleP.angle_allow_min;//云台可用角度最小值 g_ptz.vert_angleP.angle_allow_max = self_check_data.vert_angleP.angle_allow_max;//云台可用角度最大值 g_ptz.vert_angleP.angle_range = self_check_data.vert_angleP.angle_range;//云台角度范围 g_ptz.hori_angle_error = self_check_data.hori_angle_error;//赋值水平结构误差 return 1; } OSTimeDlyHMSM(0u, 0u, 0u, 2u); } return 0; } //擦除自检数据 char ptz_self_check_data_erase() { char i, j; memset(&self_check_data_a, 0, sizeof(self_check_data_a)); memset(&self_check_data, 0, sizeof(self_check_data)); #ifdef PTZ_NO_SELF_CHECK ptz_power_off_data_erase(PTZ_MB85RC64_ADD_A); ptz_power_off_data_erase(PTZ_MB85RC64_ADD_B); OSTimeDlyHMSM(0u, 0u, 0u, 5u); #endif //擦除主存储区 for(i = 0; i < SELF_CHECK_DATA_SAVE_FLASH_NUM; i++) { memset(&self_check_data_a, 0, sizeof(self_check_data_a)); write_many_data(sizeof(self_check_data),(unsigned char *)(&self_check_data),SELF_CHECK_DATA_FLASH_ADD); asm("nop"); asm("nop"); Flash_Read((unsigned char *)(&self_check_data_a),SELF_CHECK_DATA_FLASH_ADD,sizeof(self_check_data_a)); if(memcmp(&self_check_data, &self_check_data_a, sizeof(self_check_data)) == 0)//判断擦除数据是否正确 { break; } OSTimeDlyHMSM(0u, 0u, 0u, 5u); } //擦除备份存储区 for(j = 0; j < SELF_CHECK_DATA_SAVE_FLASH_NUM; j++) { memset(&self_check_data_a, 0, sizeof(self_check_data_a)); write_many_data(sizeof(self_check_data),(unsigned char *)(&self_check_data),SELF_CHECK_DATA_BACKUP_FLASH_ADD); asm("nop"); asm("nop"); Flash_Read((unsigned char *)(&self_check_data_a),SELF_CHECK_DATA_BACKUP_FLASH_ADD,sizeof(self_check_data_a)); if(memcmp(&self_check_data, &self_check_data_a, sizeof(self_check_data)) == 0)//判断擦除数据是否正确 { break; } OSTimeDlyHMSM(0u, 0u, 0u, 5u); } if(i < SELF_CHECK_DATA_SAVE_FLASH_NUM || j < SELF_CHECK_DATA_SAVE_FLASH_NUM)//判断擦除数据是否成功 { //擦除成功 return 1; } //擦除失败 return 0; }