710 lines
21 KiB
C
710 lines
21 KiB
C
#include "device_photoelectricswitch.h"
|
||
#include "device_interrupt.h"
|
||
#include "ptz_struct.h"
|
||
#include "l6235d.h"
|
||
#include "rotate_bldc.h"
|
||
#include "rotate_step.h"
|
||
#include "speed_to_bldc.h"
|
||
#include "gd32f4xx_it.h"
|
||
#include "ptz_type_select.h"
|
||
#include "get_angle.h"
|
||
/// @brief 外部中断初始化
|
||
/// @param[in] usart_periph:EXTI_IRQ_init
|
||
/// @return none
|
||
/// @note 修改日志
|
||
/// LH于2022-05-25
|
||
void EXTI_IRQ_init()
|
||
{
|
||
#ifdef L6235D
|
||
/*HALL中断，水平——PD13-H1,PD14-H2,PD15-H3，，垂直PC6-H1,PC7-H2,PC8-H3*/
|
||
nvic_irq_enable(EXTI10_15_IRQn, 3U, 3U);//水平，一个中断函数，中断线10-15
|
||
nvic_irq_enable(EXTI5_9_IRQn, 3U, 2U);//垂直，一个中断函数，中断线5-9
|
||
|
||
//对应引脚配置外部中断线
|
||
syscfg_exti_line_config(EXTI_SOURCE_GPIOD, EXTI_SOURCE_PIN13);
|
||
syscfg_exti_line_config(EXTI_SOURCE_GPIOD, EXTI_SOURCE_PIN14);
|
||
syscfg_exti_line_config(EXTI_SOURCE_GPIOD, EXTI_SOURCE_PIN15);
|
||
syscfg_exti_line_config(EXTI_SOURCE_GPIOC, EXTI_SOURCE_PIN6);
|
||
syscfg_exti_line_config(EXTI_SOURCE_GPIOC, EXTI_SOURCE_PIN7);
|
||
syscfg_exti_line_config(EXTI_SOURCE_GPIOC, EXTI_SOURCE_PIN8);
|
||
|
||
//外部中断线初始化
|
||
exti_init(EXTI_6, EXTI_INTERRUPT, EXTI_TRIG_RISING);//上升沿中断
|
||
exti_init(EXTI_7, EXTI_INTERRUPT, EXTI_TRIG_RISING);
|
||
exti_init(EXTI_8, EXTI_INTERRUPT, EXTI_TRIG_RISING);
|
||
exti_init(EXTI_13, EXTI_INTERRUPT, EXTI_TRIG_RISING);
|
||
exti_init(EXTI_14, EXTI_INTERRUPT, EXTI_TRIG_RISING);
|
||
exti_init(EXTI_15, EXTI_INTERRUPT, EXTI_TRIG_RISING);
|
||
//清除标志位
|
||
exti_interrupt_flag_clear(EXTI_6);
|
||
exti_interrupt_flag_clear(EXTI_7);
|
||
exti_interrupt_flag_clear(EXTI_8);
|
||
exti_interrupt_flag_clear(EXTI_13);
|
||
exti_interrupt_flag_clear(EXTI_14);
|
||
exti_interrupt_flag_clear(EXTI_15);
|
||
|
||
BSP_IntVectSet(39,EXTI5_9_IRQHandler);
|
||
BSP_IntEn(39);
|
||
|
||
BSP_IntVectSet(56,EXTI10_15_IRQHandler);
|
||
BSP_IntEn(56);
|
||
#endif
|
||
/*光电开关中断PB0-SW1，PB1-SW2，PB2-SW3*/
|
||
nvic_irq_enable(EXTI0_IRQn, 2U, 0U);//垂直光电开关SW1
|
||
nvic_irq_enable(EXTI1_IRQn, 2U, 1U);//垂直光电开关SW2
|
||
nvic_irq_enable(EXTI2_IRQn, 2U, 2U);//水平光电开关SW3
|
||
|
||
syscfg_exti_line_config(EXTI_SOURCE_GPIOB, EXTI_SOURCE_PIN0);
|
||
syscfg_exti_line_config(EXTI_SOURCE_GPIOB, EXTI_SOURCE_PIN1);
|
||
syscfg_exti_line_config(EXTI_SOURCE_GPIOB, EXTI_SOURCE_PIN2);
|
||
|
||
//垂直光电开关SW1
|
||
#ifdef PTZ_SW1_DOWN_FALL_UPDATE
|
||
exti_init(EXTI_0, EXTI_INTERRUPT, EXTI_TRIG_FALLING);//下降沿中断
|
||
#endif
|
||
|
||
#ifdef PTZ_SW1_UP_RISE_UPDATE
|
||
exti_init(EXTI_0, EXTI_INTERRUPT, EXTI_TRIG_BOTH);//边沿中断
|
||
#endif
|
||
//垂直光电开关SW2
|
||
exti_init(EXTI_1, EXTI_INTERRUPT, EXTI_TRIG_FALLING);//下降沿中断
|
||
//水平光电开关SW3
|
||
#ifdef PTZ_SW3_LEFT_RISE_UPDATE
|
||
exti_init(EXTI_2, EXTI_INTERRUPT, EXTI_TRIG_BOTH);//边沿中断
|
||
#else
|
||
exti_init(EXTI_2, EXTI_INTERRUPT, EXTI_TRIG_FALLING);//下降沿中断
|
||
#endif
|
||
//清除标志位
|
||
exti_interrupt_flag_clear(EXTI_0);
|
||
exti_interrupt_flag_clear(EXTI_1);
|
||
exti_interrupt_flag_clear(EXTI_2);
|
||
|
||
BSP_IntVectSet(22,EXTI0_IRQHandler);
|
||
BSP_IntEn(22);
|
||
|
||
BSP_IntVectSet(23,EXTI1_IRQHandler);
|
||
BSP_IntEn(23);
|
||
|
||
BSP_IntVectSet(24,EXTI2_IRQHandler);
|
||
BSP_IntEn(24);
|
||
}
|
||
|
||
|
||
|
||
/// @brief 光电开关中断处理函数
|
||
/// @param[in] sw_linex:EXTI_X（外部中断线号EXTI_X）
|
||
/// @return none
|
||
/// @note 修改日志
|
||
/// LH于2022-05-26
|
||
void ptz_SW_IRQHandler(exti_line_enum sw_linex)
|
||
{
|
||
|
||
#ifdef PTZ_NO_SELF_CHECK
|
||
if(g_ptz.Voltage > SWITCH_IRQ_V)
|
||
{
|
||
switch(sw_linex)
|
||
{
|
||
case EXTI_0://垂直光电开关SW1
|
||
#ifdef PTZ_PHOTOELECTRIC_SWITCH
|
||
asm("nop");
|
||
asm("nop");
|
||
asm("nop");
|
||
asm("nop");
|
||
asm("nop");
|
||
if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_UP)//光电开关中断增加条件限制，云台转向分三种状态：左（上）、右（下）、停止状态
|
||
{
|
||
if(PS_VERT_SW1_READ == PS_HIGH)
|
||
{
|
||
g_ptz.vert_ps_sw1_up_rise ++;
|
||
}
|
||
}
|
||
if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_DOWN)
|
||
{
|
||
if(PS_VERT_SW1_READ == PS_LOW)
|
||
{
|
||
g_ptz.vert_ps_sw1_down_fall ++;
|
||
}
|
||
}
|
||
#endif
|
||
|
||
break;
|
||
|
||
case EXTI_1://垂直光电开关SW2
|
||
#ifdef PTZ_PHOTOELECTRIC_SWITCH
|
||
asm("nop");
|
||
asm("nop");
|
||
asm("nop");
|
||
asm("nop");
|
||
asm("nop");
|
||
if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_UP)
|
||
{
|
||
if(PS_VERT_SW2_READ == PS_LOW)
|
||
{
|
||
g_ptz.vert_ps_sw2_up_fall ++;
|
||
}
|
||
}
|
||
if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_DOWN)
|
||
{
|
||
if(PS_VERT_SW2_READ == PS_HIGH)
|
||
{
|
||
g_ptz.vert_ps_sw2_down_rise ++;
|
||
}
|
||
}
|
||
#endif
|
||
|
||
|
||
break;
|
||
|
||
case EXTI_2://水平光电开关SW3
|
||
#ifdef PTZ_PHOTOELECTRIC_SWITCH
|
||
asm("nop");
|
||
asm("nop");
|
||
asm("nop");
|
||
asm("nop");
|
||
asm("nop");
|
||
if(PS_HORI_SW3_READ == PS_LOW)
|
||
{
|
||
if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_RIGHT)
|
||
{
|
||
g_ptz.hori_ps_sw3_right_fall ++;
|
||
}
|
||
if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_LEFT)
|
||
{
|
||
g_ptz.hori_ps_sw3_left_fall ++;
|
||
}
|
||
}
|
||
if(PS_HORI_SW3_READ == PS_HIGH)
|
||
{
|
||
if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_RIGHT)
|
||
{
|
||
g_ptz.hori_ps_sw3_right_rise ++;
|
||
}
|
||
if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_LEFT)
|
||
{
|
||
g_ptz.hori_ps_sw3_left_rise ++;
|
||
}
|
||
}
|
||
#endif
|
||
|
||
break;
|
||
|
||
default:break;
|
||
}
|
||
|
||
}
|
||
#else
|
||
|
||
switch(sw_linex)
|
||
{
|
||
case EXTI_0://垂直光电开关SW1
|
||
#ifdef PTZ_PHOTOELECTRIC_SWITCH
|
||
#ifdef CLK_50M
|
||
asm("nop");
|
||
asm("nop");
|
||
#else
|
||
asm("nop");
|
||
asm("nop");
|
||
asm("nop");
|
||
asm("nop");
|
||
#endif
|
||
if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_UP)
|
||
{
|
||
if(PS_VERT_SW1_READ == PS_HIGH)
|
||
{
|
||
g_ptz.vert_ps_sw1_up_rise ++;
|
||
}
|
||
}
|
||
if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_DOWN)
|
||
{
|
||
if(PS_VERT_SW1_READ == PS_LOW)
|
||
{
|
||
g_ptz.vert_ps_sw1_down_fall ++;
|
||
}
|
||
}
|
||
#endif
|
||
|
||
|
||
break;
|
||
|
||
case EXTI_1://垂直光电开关SW2
|
||
#ifdef PTZ_PHOTOELECTRIC_SWITCH
|
||
#ifdef CLK_50M
|
||
asm("nop");
|
||
asm("nop");
|
||
#else
|
||
asm("nop");
|
||
asm("nop");
|
||
asm("nop");
|
||
asm("nop");
|
||
#endif
|
||
if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_UP)
|
||
{
|
||
if(PS_VERT_SW2_READ == PS_LOW)
|
||
{
|
||
g_ptz.vert_ps_sw2_up_fall ++;
|
||
}
|
||
}
|
||
if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_DOWN)
|
||
{
|
||
if(PS_VERT_SW2_READ == PS_HIGH)
|
||
{
|
||
g_ptz.vert_ps_sw2_down_rise ++;
|
||
}
|
||
}
|
||
#endif
|
||
|
||
|
||
break;
|
||
|
||
case EXTI_2://水平光电开关SW3
|
||
#ifdef PTZ_PHOTOELECTRIC_SWITCH
|
||
#ifdef CLK_50M
|
||
asm("nop");
|
||
asm("nop");
|
||
#else
|
||
asm("nop");
|
||
asm("nop");
|
||
asm("nop");
|
||
asm("nop");
|
||
#endif
|
||
if(PS_HORI_SW3_READ == PS_LOW)
|
||
{
|
||
if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_RIGHT)
|
||
{
|
||
g_ptz.hori_ps_sw3_right_fall ++;
|
||
}
|
||
if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_LEFT)
|
||
{
|
||
g_ptz.hori_ps_sw3_left_fall ++;
|
||
}
|
||
}
|
||
if(PS_HORI_SW3_READ == PS_HIGH)
|
||
{
|
||
if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_RIGHT)
|
||
{
|
||
g_ptz.hori_ps_sw3_right_rise ++;
|
||
}
|
||
if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_LEFT)
|
||
{
|
||
g_ptz.hori_ps_sw3_left_rise ++;
|
||
}
|
||
}
|
||
#endif
|
||
|
||
|
||
break;
|
||
|
||
default:break;
|
||
}
|
||
|
||
#endif
|
||
|
||
}
|
||
|
||
/// @brief 水平HALL中断处理函数
|
||
/// @param[in] hall_linex:EXTI_X（外部HALL中断线号EXTI_X）
|
||
/// @return none
|
||
/// @note 修改日志
|
||
/// LH于2022-05-26
|
||
void ptz_H_HALL_IRQHandler(exti_line_enum hall_linex)
|
||
{
|
||
|
||
#ifdef PTZ_BLDC_MOTOR
|
||
#ifdef PTZ_L6235D_HALL_FEEDBACK
|
||
|
||
switch(hall_linex)
|
||
{
|
||
case EXTI_13://水平HALL-1
|
||
g_ptz.hori_l6235d.l6235d_hall_state_actual = L6235D_HALL_H1;//当前霍尔状态
|
||
//保存霍尔状态
|
||
//g_ptz.hori_l6235d.l6235d_hall_state_save[g_ptz.hori_l6235d.l6235d_satae_save_count] = L6235D_HALL_H1;
|
||
//霍尔总数计数
|
||
// g_ptz.hori_l6235d.l6235d_satae_save_count ++;
|
||
// if(g_ptz.hori_l6235d.l6235d_satae_save_count >= L6235D_HALL_SAVE_NUM)
|
||
// {
|
||
// g_ptz.hori_l6235d.l6235d_satae_save_count = 0;
|
||
// }
|
||
//霍尔H1单独计数
|
||
g_ptz.hori_l6235d.l6235d_hall_h1_count++;
|
||
|
||
#ifdef PTZ_PID_HALL_SPEED
|
||
g_ptz.hori_pid.hall_h1_count ++;
|
||
#endif
|
||
// g_ptz.hori_l6235d.l6235d_hall_h123_count++;//霍尔总数计数
|
||
// if(g_ptz.hori_l6235d.l6235d_hall_h123_count >= UNSIGNED_INT_MAX_VALUE)
|
||
// {
|
||
// g_ptz.hori_l6235d.l6235d_hall_h123_count = 0;
|
||
// }
|
||
//判断电机实际转动方向,不是云台实际的转动方向
|
||
if(g_ptz.hori_l6235d.l6235d_hall_state_actual != 0 && g_ptz.hori_l6235d.l6235d_hall_state_last != 0)
|
||
{
|
||
if(g_ptz.hori_l6235d.l6235d_hall_state_actual == L6235D_HALL_H1 &&
|
||
g_ptz.hori_l6235d.l6235d_hall_state_last == L6235D_HALL_H2)//判断方向
|
||
{
|
||
#ifdef PTZ_HALL_DIR
|
||
if(g_ptz.hori_start_stop_set == PTZ_HORI_STOP)
|
||
{
|
||
g_ptz.hori_direction_actual = PTZ_HORI_DIR_RIGHT;
|
||
}
|
||
#endif
|
||
g_ptz.hori_l6235d.l6235d_motor_dir_actual = L6235D_MOTOR_DIR_FWD;
|
||
}
|
||
if(g_ptz.hori_l6235d.l6235d_hall_state_actual == L6235D_HALL_H1 &&
|
||
g_ptz.hori_l6235d.l6235d_hall_state_last == L6235D_HALL_H3)
|
||
{
|
||
#ifdef PTZ_HALL_DIR
|
||
if(g_ptz.hori_start_stop_set == PTZ_HORI_STOP)
|
||
{
|
||
g_ptz.hori_direction_actual = PTZ_HORI_DIR_LEFT;
|
||
}
|
||
#endif
|
||
g_ptz.hori_l6235d.l6235d_motor_dir_actual = L6235D_MOTOR_DIR_REV;
|
||
}
|
||
}
|
||
g_ptz.hori_l6235d.l6235d_hall_state_last = g_ptz.hori_l6235d.l6235d_hall_state_actual;
|
||
|
||
break;
|
||
|
||
case EXTI_14://水平HALL-2
|
||
g_ptz.hori_l6235d.l6235d_hall_state_actual = L6235D_HALL_H2;//当前霍尔状态
|
||
//保存霍尔状态
|
||
//g_ptz.hori_l6235d.l6235d_hall_state_save[g_ptz.hori_l6235d.l6235d_satae_save_count] = L6235D_HALL_H2;
|
||
//霍尔总数计数
|
||
// g_ptz.hori_l6235d.l6235d_satae_save_count ++;
|
||
// if(g_ptz.hori_l6235d.l6235d_satae_save_count >= L6235D_HALL_SAVE_NUM)
|
||
// {
|
||
// g_ptz.hori_l6235d.l6235d_satae_save_count = 0;
|
||
// }
|
||
//霍尔H2单独计数
|
||
g_ptz.hori_l6235d.l6235d_hall_h2_count++;
|
||
|
||
#ifdef PTZ_PID_HALL_SPEED
|
||
g_ptz.hori_pid.hall_h2_count ++;
|
||
#endif
|
||
// g_ptz.hori_l6235d.l6235d_hall_h123_count++;//霍尔总数计数
|
||
// if(g_ptz.hori_l6235d.l6235d_hall_h123_count >= UNSIGNED_INT_MAX_VALUE)
|
||
// {
|
||
// g_ptz.hori_l6235d.l6235d_hall_h123_count = 0;
|
||
// }
|
||
//判断电机实际转动方向,不是云台实际的转动方向
|
||
if(g_ptz.hori_l6235d.l6235d_hall_state_actual != 0 && g_ptz.hori_l6235d.l6235d_hall_state_last != 0)
|
||
{
|
||
if(g_ptz.hori_l6235d.l6235d_hall_state_actual == L6235D_HALL_H2 &&
|
||
g_ptz.hori_l6235d.l6235d_hall_state_last == L6235D_HALL_H3)//判断方向
|
||
{
|
||
#ifdef PTZ_HALL_DIR
|
||
if(g_ptz.hori_start_stop_set == PTZ_HORI_STOP)
|
||
{
|
||
g_ptz.hori_direction_actual = PTZ_HORI_DIR_RIGHT;
|
||
}
|
||
#endif
|
||
g_ptz.hori_l6235d.l6235d_motor_dir_actual = L6235D_MOTOR_DIR_FWD;
|
||
}
|
||
if(g_ptz.hori_l6235d.l6235d_hall_state_actual == L6235D_HALL_H2 &&
|
||
g_ptz.hori_l6235d.l6235d_hall_state_last == L6235D_HALL_H1)
|
||
{
|
||
#ifdef PTZ_HALL_DIR
|
||
if(g_ptz.hori_start_stop_set == PTZ_HORI_STOP)
|
||
{
|
||
g_ptz.hori_direction_actual = PTZ_HORI_DIR_LEFT;
|
||
}
|
||
#endif
|
||
g_ptz.hori_l6235d.l6235d_motor_dir_actual = L6235D_MOTOR_DIR_REV;
|
||
}
|
||
}
|
||
g_ptz.hori_l6235d.l6235d_hall_state_last = g_ptz.hori_l6235d.l6235d_hall_state_actual;
|
||
break;
|
||
|
||
case EXTI_15://水平HALL-3
|
||
g_ptz.hori_l6235d.l6235d_hall_state_actual = L6235D_HALL_H3;//当前霍尔状态
|
||
//保存霍尔状态
|
||
//g_ptz.hori_l6235d.l6235d_hall_state_save[g_ptz.hori_l6235d.l6235d_satae_save_count] = L6235D_HALL_H3;
|
||
//霍尔总数计数
|
||
// g_ptz.hori_l6235d.l6235d_satae_save_count ++;
|
||
// if(g_ptz.hori_l6235d.l6235d_satae_save_count >= L6235D_HALL_SAVE_NUM)
|
||
// {
|
||
// g_ptz.hori_l6235d.l6235d_satae_save_count = 0;
|
||
// }
|
||
//霍尔H3单独计数
|
||
g_ptz.hori_l6235d.l6235d_hall_h3_count++;
|
||
|
||
#ifdef PTZ_PID_HALL_SPEED
|
||
g_ptz.hori_pid.hall_h3_count ++;
|
||
#endif
|
||
// g_ptz.hori_l6235d.l6235d_hall_h123_count++;//霍尔总数计数
|
||
// if(g_ptz.hori_l6235d.l6235d_hall_h123_count >= UNSIGNED_INT_MAX_VALUE)
|
||
// {
|
||
// g_ptz.hori_l6235d.l6235d_hall_h123_count = 0;
|
||
// }
|
||
//判断电机实际转动方向,不是云台实际的转动方向
|
||
if(g_ptz.hori_l6235d.l6235d_hall_state_actual != 0 && g_ptz.hori_l6235d.l6235d_hall_state_last != 0)
|
||
{
|
||
if(g_ptz.hori_l6235d.l6235d_hall_state_actual == L6235D_HALL_H3 &&
|
||
g_ptz.hori_l6235d.l6235d_hall_state_last == L6235D_HALL_H1)//判断方向
|
||
{
|
||
#ifdef PTZ_HALL_DIR
|
||
if(g_ptz.hori_start_stop_set == PTZ_HORI_STOP)
|
||
{
|
||
g_ptz.hori_direction_actual = PTZ_HORI_DIR_RIGHT;
|
||
}
|
||
#endif
|
||
g_ptz.hori_l6235d.l6235d_motor_dir_actual = L6235D_MOTOR_DIR_FWD;
|
||
}
|
||
if(g_ptz.hori_l6235d.l6235d_hall_state_actual == L6235D_HALL_H3 &&
|
||
g_ptz.hori_l6235d.l6235d_hall_state_last == L6235D_HALL_H2)
|
||
{
|
||
#ifdef PTZ_HALL_DIR
|
||
if(g_ptz.hori_start_stop_set == PTZ_HORI_STOP)
|
||
{
|
||
g_ptz.hori_direction_actual = PTZ_HORI_DIR_LEFT;
|
||
}
|
||
#endif
|
||
g_ptz.hori_l6235d.l6235d_motor_dir_actual = L6235D_MOTOR_DIR_REV;
|
||
}
|
||
}
|
||
g_ptz.hori_l6235d.l6235d_hall_state_last = g_ptz.hori_l6235d.l6235d_hall_state_actual;
|
||
break;
|
||
|
||
default:break;
|
||
}
|
||
|
||
|
||
#endif
|
||
|
||
#endif
|
||
|
||
}
|
||
|
||
|
||
|
||
/// @brief 垂直HALL中断处理函数
|
||
/// @param[in] hall_linex:EXTI_X（外部HALL中断线号EXTI_X）
|
||
/// @return none
|
||
/// @note 修改日志
|
||
/// LH于2022-05-26
|
||
void ptz_V_HALL_IRQHandler(exti_line_enum hall_linex)
|
||
{
|
||
|
||
#ifdef PTZ_BLDC_MOTOR
|
||
#ifdef PTZ_L6235D_HALL_FEEDBACK
|
||
|
||
switch(hall_linex)
|
||
{
|
||
|
||
case EXTI_6://垂直HALL-1
|
||
g_ptz.vert_l6235d.l6235d_hall_state_actual = L6235D_HALL_H1;//当前霍尔状态
|
||
//保存霍尔状态
|
||
//g_ptz.vert_l6235d.l6235d_hall_state_save[g_ptz.vert_l6235d.l6235d_satae_save_count] = L6235D_HALL_H1;
|
||
//霍尔总数计数
|
||
// g_ptz.vert_l6235d.l6235d_satae_save_count ++;
|
||
// if(g_ptz.vert_l6235d.l6235d_satae_save_count >= L6235D_HALL_SAVE_NUM)
|
||
// {
|
||
// g_ptz.vert_l6235d.l6235d_satae_save_count = 0;
|
||
// }
|
||
//霍尔H1单独计数
|
||
g_ptz.vert_l6235d.l6235d_hall_h1_count++;
|
||
|
||
#ifdef PTZ_PID_HALL_SPEED
|
||
g_ptz.vert_pid.hall_h1_count ++;
|
||
#endif
|
||
// g_ptz.vert_l6235d.l6235d_hall_h123_count++;//霍尔总数计数
|
||
// if(g_ptz.vert_l6235d.l6235d_hall_h123_count >= UNSIGNED_INT_MAX_VALUE)
|
||
// {
|
||
// g_ptz.vert_l6235d.l6235d_hall_h123_count = 0;
|
||
// }
|
||
//判断电机实际转动方向,不是云台实际的转动方向
|
||
if(g_ptz.vert_l6235d.l6235d_hall_state_actual != 0 && g_ptz.vert_l6235d.l6235d_hall_state_last != 0)
|
||
{
|
||
if(g_ptz.vert_l6235d.l6235d_hall_state_actual == L6235D_HALL_H1 &&
|
||
g_ptz.vert_l6235d.l6235d_hall_state_last == L6235D_HALL_H2)//判断方向
|
||
{
|
||
#ifdef PTZ_HALL_DIR
|
||
if(g_ptz.vert_start_stop_set == PTZ_VERT_STOP)
|
||
{
|
||
g_ptz.vert_direction_actual = PTZ_VERT_DIR_UP;
|
||
}
|
||
#endif
|
||
g_ptz.vert_l6235d.l6235d_motor_dir_actual = L6235D_MOTOR_DIR_FWD;
|
||
}
|
||
if(g_ptz.vert_l6235d.l6235d_hall_state_actual == L6235D_HALL_H1 &&
|
||
g_ptz.vert_l6235d.l6235d_hall_state_last == L6235D_HALL_H3)
|
||
{
|
||
#ifdef PTZ_HALL_DIR
|
||
if(g_ptz.vert_start_stop_set == PTZ_VERT_STOP)
|
||
{
|
||
g_ptz.vert_direction_actual = PTZ_VERT_DIR_DOWN;
|
||
}
|
||
#endif
|
||
g_ptz.vert_l6235d.l6235d_motor_dir_actual = L6235D_MOTOR_DIR_REV;
|
||
}
|
||
}
|
||
g_ptz.vert_l6235d.l6235d_hall_state_last = g_ptz.vert_l6235d.l6235d_hall_state_actual;
|
||
|
||
//电子自锁
|
||
#ifdef PTZ_ELECTRIC_STABLE_ON
|
||
if(g_ptz.vert_start_stop_set == PTZ_VERT_STOP)
|
||
{
|
||
g_ptz.vert_electric_stable.stop_hall_count ++;
|
||
}
|
||
else
|
||
{
|
||
g_ptz.vert_electric_stable.stop_hall_count = 0;
|
||
}
|
||
#endif
|
||
break;
|
||
|
||
case EXTI_7://垂直HALL-2
|
||
g_ptz.vert_l6235d.l6235d_hall_state_actual = L6235D_HALL_H2;//当前霍尔状态
|
||
//保存霍尔状态
|
||
//g_ptz.vert_l6235d.l6235d_hall_state_save[g_ptz.vert_l6235d.l6235d_satae_save_count] = L6235D_HALL_H2;
|
||
//霍尔总数计数
|
||
// g_ptz.vert_l6235d.l6235d_satae_save_count ++;
|
||
// if(g_ptz.vert_l6235d.l6235d_satae_save_count >= L6235D_HALL_SAVE_NUM)
|
||
// {
|
||
// g_ptz.vert_l6235d.l6235d_satae_save_count = 0;
|
||
// }
|
||
//霍尔H2单独计数
|
||
g_ptz.vert_l6235d.l6235d_hall_h2_count++;
|
||
|
||
#ifdef PTZ_PID_HALL_SPEED
|
||
g_ptz.vert_pid.hall_h2_count ++;
|
||
#endif
|
||
// g_ptz.vert_l6235d.l6235d_hall_h123_count++;//霍尔总数计数
|
||
// if(g_ptz.vert_l6235d.l6235d_hall_h123_count >= UNSIGNED_INT_MAX_VALUE)
|
||
// {
|
||
// g_ptz.vert_l6235d.l6235d_hall_h123_count = 0;
|
||
// }
|
||
//判断电机实际转动方向,不是云台实际的转动方向
|
||
if(g_ptz.vert_l6235d.l6235d_hall_state_actual != 0 && g_ptz.vert_l6235d.l6235d_hall_state_last != 0)
|
||
{
|
||
if(g_ptz.vert_l6235d.l6235d_hall_state_actual == L6235D_HALL_H2 &&
|
||
g_ptz.vert_l6235d.l6235d_hall_state_last == L6235D_HALL_H3)//判断方向
|
||
{
|
||
#ifdef PTZ_HALL_DIR
|
||
if(g_ptz.vert_start_stop_set == PTZ_VERT_STOP)
|
||
{
|
||
g_ptz.vert_direction_actual = PTZ_VERT_DIR_UP;
|
||
}
|
||
#endif
|
||
g_ptz.vert_l6235d.l6235d_motor_dir_actual = L6235D_MOTOR_DIR_FWD;
|
||
}
|
||
if(g_ptz.vert_l6235d.l6235d_hall_state_actual == L6235D_HALL_H2 &&
|
||
g_ptz.vert_l6235d.l6235d_hall_state_last == L6235D_HALL_H1)
|
||
{
|
||
#ifdef PTZ_HALL_DIR
|
||
if(g_ptz.vert_start_stop_set == PTZ_VERT_STOP)
|
||
{
|
||
g_ptz.vert_direction_actual = PTZ_VERT_DIR_DOWN;
|
||
}
|
||
#endif
|
||
g_ptz.vert_l6235d.l6235d_motor_dir_actual = L6235D_MOTOR_DIR_REV;
|
||
}
|
||
}
|
||
g_ptz.vert_l6235d.l6235d_hall_state_last = g_ptz.vert_l6235d.l6235d_hall_state_actual;
|
||
|
||
//电子自锁
|
||
#ifdef PTZ_ELECTRIC_STABLE_ON
|
||
if(g_ptz.vert_start_stop_set == PTZ_VERT_STOP)
|
||
{
|
||
g_ptz.vert_electric_stable.stop_hall_count ++;
|
||
}
|
||
else
|
||
{
|
||
g_ptz.vert_electric_stable.stop_hall_count = 0;
|
||
}
|
||
#endif
|
||
break;
|
||
|
||
case EXTI_8://垂直HALL-3
|
||
g_ptz.vert_l6235d.l6235d_hall_state_actual = L6235D_HALL_H3;//当前霍尔状态
|
||
//保存霍尔状态
|
||
//g_ptz.vert_l6235d.l6235d_hall_state_save[g_ptz.vert_l6235d.l6235d_satae_save_count] = L6235D_HALL_H3;
|
||
//霍尔总数计数
|
||
// g_ptz.vert_l6235d.l6235d_satae_save_count ++;
|
||
// if(g_ptz.vert_l6235d.l6235d_satae_save_count >= L6235D_HALL_SAVE_NUM)
|
||
// {
|
||
// g_ptz.vert_l6235d.l6235d_satae_save_count = 0;
|
||
// }
|
||
//霍尔H3单独计数
|
||
g_ptz.vert_l6235d.l6235d_hall_h3_count++;
|
||
|
||
#ifdef PTZ_PID_HALL_SPEED
|
||
g_ptz.vert_pid.hall_h3_count ++;
|
||
#endif
|
||
// g_ptz.vert_l6235d.l6235d_hall_h123_count++;//霍尔总数计数
|
||
// if(g_ptz.vert_l6235d.l6235d_hall_h123_count >= UNSIGNED_INT_MAX_VALUE)
|
||
// {
|
||
// g_ptz.vert_l6235d.l6235d_hall_h123_count = 0;
|
||
// }
|
||
//判断电机实际转动方向,不是云台实际的转动方向
|
||
if(g_ptz.vert_l6235d.l6235d_hall_state_actual != 0 && g_ptz.vert_l6235d.l6235d_hall_state_last != 0)
|
||
{
|
||
if(g_ptz.vert_l6235d.l6235d_hall_state_actual == L6235D_HALL_H3 &&
|
||
g_ptz.vert_l6235d.l6235d_hall_state_last == L6235D_HALL_H1)//判断方向
|
||
{
|
||
#ifdef PTZ_HALL_DIR
|
||
if(g_ptz.vert_start_stop_set == PTZ_VERT_STOP)
|
||
{
|
||
g_ptz.vert_direction_actual = PTZ_VERT_DIR_UP;
|
||
}
|
||
#endif
|
||
g_ptz.vert_l6235d.l6235d_motor_dir_actual = L6235D_MOTOR_DIR_FWD;
|
||
}
|
||
if(g_ptz.vert_l6235d.l6235d_hall_state_actual == L6235D_HALL_H3 &&
|
||
g_ptz.vert_l6235d.l6235d_hall_state_last == L6235D_HALL_H2)
|
||
{
|
||
#ifdef PTZ_HALL_DIR
|
||
if(g_ptz.vert_start_stop_set == PTZ_VERT_STOP)
|
||
{
|
||
g_ptz.vert_direction_actual = PTZ_VERT_DIR_DOWN;
|
||
}
|
||
#endif
|
||
g_ptz.vert_l6235d.l6235d_motor_dir_actual = L6235D_MOTOR_DIR_REV;
|
||
}
|
||
}
|
||
g_ptz.vert_l6235d.l6235d_hall_state_last = g_ptz.vert_l6235d.l6235d_hall_state_actual;
|
||
|
||
//电子自锁
|
||
#ifdef PTZ_ELECTRIC_STABLE_ON
|
||
if(g_ptz.vert_start_stop_set == PTZ_VERT_STOP)
|
||
{
|
||
g_ptz.vert_electric_stable.stop_hall_count ++;
|
||
}
|
||
else
|
||
{
|
||
g_ptz.vert_electric_stable.stop_hall_count = 0;
|
||
}
|
||
break;
|
||
#endif
|
||
default:break;
|
||
}
|
||
|
||
|
||
#endif
|
||
|
||
#endif
|
||
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|