#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" #include "speed_to_hall.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 #ifdef Full_bridge //MOS+栅极驱动 rcu_periph_clock_enable(RCU_SYSCFG); //配置引脚时钟 rcu_periph_clock_enable(RCU_GPIOE); //水平——PE10-H1,PE11-H2,PE12-H3，，垂直PE13-H1,PE14-H2,PE15-H3 //设置引脚为输入模式 gpio_mode_set(GPIOE, GPIO_MODE_INPUT, GPIO_PUPD_NONE, GPIO_PIN_10); gpio_mode_set(GPIOE, GPIO_MODE_INPUT, GPIO_PUPD_NONE, GPIO_PIN_11); gpio_mode_set(GPIOE, GPIO_MODE_INPUT, GPIO_PUPD_NONE, GPIO_PIN_12); gpio_mode_set(GPIOE, GPIO_MODE_INPUT, GPIO_PUPD_NONE, GPIO_PIN_13); gpio_mode_set(GPIOE, GPIO_MODE_INPUT, GPIO_PUPD_NONE, GPIO_PIN_14); gpio_mode_set(GPIOE, GPIO_MODE_INPUT, GPIO_PUPD_NONE, GPIO_PIN_15); /*HALL中断，水平——PE10-H1,PE11-H2,PE12-H3，，垂直PE13-H1,PE14-H2,PE15-H3*/ //对应引脚配置外部中断线 syscfg_exti_line_config(EXTI_SOURCE_GPIOE, EXTI_SOURCE_PIN10); syscfg_exti_line_config(EXTI_SOURCE_GPIOE, EXTI_SOURCE_PIN11); syscfg_exti_line_config(EXTI_SOURCE_GPIOE, EXTI_SOURCE_PIN12); syscfg_exti_line_config(EXTI_SOURCE_GPIOE, EXTI_SOURCE_PIN13); syscfg_exti_line_config(EXTI_SOURCE_GPIOE, EXTI_SOURCE_PIN14); syscfg_exti_line_config(EXTI_SOURCE_GPIOE, EXTI_SOURCE_PIN15); //外部中断线初始化 exti_init(EXTI_10, EXTI_INTERRUPT, EXTI_TRIG_RISING);//上升沿中断 exti_init(EXTI_11, EXTI_INTERRUPT, EXTI_TRIG_RISING); exti_init(EXTI_12, 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_enable(EXTI_10); exti_interrupt_enable(EXTI_11); exti_interrupt_enable(EXTI_12); exti_interrupt_enable(EXTI_13); exti_interrupt_enable(EXTI_14); exti_interrupt_enable(EXTI_15); //清除标志位 exti_interrupt_flag_clear(EXTI_10); exti_interrupt_flag_clear(EXTI_11); exti_interrupt_flag_clear(EXTI_12); exti_interrupt_flag_clear(EXTI_13); exti_interrupt_flag_clear(EXTI_14); exti_interrupt_flag_clear(EXTI_15); nvic_irq_enable(EXTI10_15_IRQn, 2U, 2U);//，一个中断函数，中断线10-15 BSP_IntVectSet(56,EXTI10_15_IRQHandler); BSP_IntEn(56); /*光电开关中断PE7-SW1，PE8-SW2，PE9-SW3*/ syscfg_exti_line_config(EXTI_SOURCE_GPIOE, EXTI_SOURCE_PIN7); syscfg_exti_line_config(EXTI_SOURCE_GPIOE, EXTI_SOURCE_PIN8); syscfg_exti_line_config(EXTI_SOURCE_GPIOE, EXTI_SOURCE_PIN9); //垂直光电开关SW1 #ifdef PTZ_SW1_DOWN_FALL_UPDATE exti_init(EXTI_7, EXTI_INTERRUPT, EXTI_TRIG_FALLING);//下降沿中断 #endif #ifdef PTZ_SW1_UP_RISE_UPDATE exti_init(EXTI_7, EXTI_INTERRUPT, EXTI_TRIG_BOTH);//边沿中断 #endif //垂直光电开关SW2 exti_init(EXTI_8, EXTI_INTERRUPT, EXTI_TRIG_FALLING);//下降沿中断 //水平光电开关SW3 #ifdef PTZ_SW3_LEFT_RISE_UPDATE exti_init(EXTI_9, EXTI_INTERRUPT, EXTI_TRIG_BOTH);//边沿中断 #else exti_init(EXTI_9, EXTI_INTERRUPT, EXTI_TRIG_FALLING);//下降沿中断 #endif //清除标志位 exti_interrupt_flag_clear(EXTI_7); exti_interrupt_flag_clear(EXTI_8); exti_interrupt_flag_clear(EXTI_9); nvic_irq_enable(EXTI5_9_IRQn, 2U, 3U);//光电开关 BSP_IntVectSet(39,EXTI5_9_IRQHandler); BSP_IntEn(39); #else /*光电开关中断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); #endif } /// @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_BLDC_MOTOR #ifdef PTZ_NO_SELF_CHECK if(g_ptz.Voltage > SWITCH_IRQ_V) { switch(sw_linex) { case EXTI_7://垂直光电开关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_8://垂直光电开关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_9://水平光电开关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 ++; // term_printf(" hori_ps_sw3_left_rise \r\n");//中断不能使用串口打印，会占用较长时间 } } #endif break; default:break; } } #else switch(sw_linex) { case EXTI_7://垂直光电开关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_8://垂直光电开关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_9://水平光电开关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 ++; // term_printf(" hori_ps_sw3_left_rise \r\n");//中断不能使用串口打印，会占用较长时间 } } #endif break; default:break; } #endif #endif #ifdef PTZ_STEP_MOTOR #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"); 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"); 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"); 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 ++; } } else//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 asm("nop"); asm("nop"); asm("nop"); 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 ++; } } else { 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"); 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 ++; } } else { 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"); 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 ++; } else { g_ptz.hori_ps_sw3_left_fall ++; } } else//PS_HIGH { if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_RIGHT) { g_ptz.hori_ps_sw3_right_rise ++; } else { g_ptz.hori_ps_sw3_left_rise ++; } } #endif break; default:break; } #endif #endif } uint32_t h_hall_time; /// @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_HALL_FEEDBACK // if (g_speed_to_hall.hori_speed_t.flag == calculation_enable) { // g_speed_to_hall.hori_speed_t.startTime_us = TIMER_CNT(TIMER6); // g_speed_to_hall.hori_speed_t.startTime_60ms = g_speed_to_hall.time_60ms; // g_speed_to_hall.hori_speed_t.hallNum = 0; // g_speed_to_hall.hori_speed_t.flag = calculation_start; // } // if (g_speed_to_hall.hori_speed_t.flag == calculation_start) { // g_speed_to_hall.hori_speed_t.hallNum++; // } // if (g_speed_to_hall.hori_speed_t.flag == calculation_ok) { // g_speed_to_hall.hori_speed_t.endTime_us = TIMER_CNT(TIMER6); // g_speed_to_hall.hori_speed_t.endTime_60ms = g_speed_to_hall.time_60ms; // g_speed_to_hall.hori_speed_t.flag = calculation_end; // } g_speed_to_hall.hori_speed_t.endTime_us = TIMER_CNT(TIMER6); g_speed_to_hall.hori_speed_t.endTime_60ms = g_speed_to_hall.time_60ms; // uint32_t h_hall_time; if (g_speed_to_hall.hori_speed_t.endTime_60ms > g_speed_to_hall.hori_speed_t.startTime_60ms) { h_hall_time = (g_speed_to_hall.hori_speed_t.endTime_60ms - g_speed_to_hall.hori_speed_t.startTime_60ms - 1) * SPEED_TIMER_PERIOD + g_speed_to_hall.hori_speed_t.endTime_us + (SPEED_TIMER_PERIOD - g_speed_to_hall.hori_speed_t.startTime_us); } else if (g_speed_to_hall.hori_speed_t.endTime_60ms = g_speed_to_hall.hori_speed_t.startTime_60ms) { h_hall_time = g_speed_to_hall.hori_speed_t.endTime_us - g_speed_to_hall.hori_speed_t.startTime_us; } else { h_hall_time = (g_speed_to_hall.hori_speed_t.endTime_60ms + (TIME_60MS_MAX - g_speed_to_hall.hori_speed_t.startTime_60ms) - 1) * SPEED_TIMER_PERIOD + g_speed_to_hall.hori_speed_t.endTime_us + (SPEED_TIMER_PERIOD - g_speed_to_hall.hori_speed_t.startTime_us); } // if (time > 200000) { // g_speed_to_hall.hori_speed_t.speed = 0; // } // else { // g_speed_to_hall.hori_speed_t.speed = 0.2 * (150000000.0 / (float)time) + 0.8 * g_speed_to_hall.hori_speed_t.lastSpeed; // } g_speed_to_hall.hori_speed_t.speed = 0.8 * (5000000.0 / (float)h_hall_time) + 0.2 * g_speed_to_hall.hori_speed_t.lastSpeed; // g_speed_to_hall.hori_speed_t.speed = (5000000.0 / (float)h_hall_time); g_speed_to_hall.hori_speed_t.lastSpeed = g_speed_to_hall.hori_speed_t.speed; g_speed_to_hall.hori_speed_t.startTime_60ms = g_speed_to_hall.hori_speed_t.endTime_60ms; g_speed_to_hall.hori_speed_t.startTime_us = g_speed_to_hall.hori_speed_t.endTime_us; switch(hall_linex) { case EXTI_10://水平HALL-1 // if(gpio_input_bit_get(GPIOE, GPIO_PIN_10) == SET) // { 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; //电子自锁 #ifdef PTZ_ELECTRIC_STABLE_ON if(g_ptz.hori_start_stop_set == PTZ_HORI_STOP) { g_ptz.hori_electric_stable.stop_hall_count ++; } else { g_ptz.hori_electric_stable.stop_hall_count = 0; } #endif // } break; case EXTI_11://水平HALL-2 // if(gpio_input_bit_get(GPIOE, GPIO_PIN_11) == SET) // { 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; //电子自锁 #ifdef PTZ_ELECTRIC_STABLE_ON if(g_ptz.hori_start_stop_set == PTZ_HORI_STOP) { g_ptz.hori_electric_stable.stop_hall_count ++; } else { g_ptz.hori_electric_stable.stop_hall_count = 0; } #endif // } break; case EXTI_12://水平HALL-3 // if(gpio_input_bit_get(GPIOE, GPIO_PIN_12) == SET) // { 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; //电子自锁 #ifdef PTZ_ELECTRIC_STABLE_ON if(g_ptz.hori_start_stop_set == PTZ_HORI_STOP) { g_ptz.hori_electric_stable.stop_hall_count ++; } else { g_ptz.hori_electric_stable.stop_hall_count = 0; } #endif // } break; default:break; } #endif #endif } /// @brief 垂直HALL中断处理函数 /// @param[in] hall_linex:EXTI_X（外部HALL中断线号EXTI_X） /// @return none /// @note 修改日志 /// LH于2022-05-26 uint32_t v_hall_time; void ptz_V_HALL_IRQHandler(exti_line_enum hall_linex) { #ifdef PTZ_BLDC_MOTOR #ifdef PTZ_HALL_FEEDBACK // if (g_speed_to_hall.vert_speed_t.flag == calculation_enable) { // g_speed_to_hall.vert_speed_t.startTime_us = TIMER_CNT(TIMER6); // g_speed_to_hall.vert_speed_t.startTime_60ms = g_speed_to_hall.time_60ms; // g_speed_to_hall.vert_speed_t.hallNum = 0; // g_speed_to_hall.vert_speed_t.flag = calculation_start; // } // if (g_speed_to_hall.vert_speed_t.flag == calculation_start) { // g_speed_to_hall.vert_speed_t.hallNum++; // } // if (g_speed_to_hall.vert_speed_t.flag == calculation_ok) { // g_speed_to_hall.vert_speed_t.endTime_us = TIMER_CNT(TIMER6); // g_speed_to_hall.vert_speed_t.endTime_60ms = g_speed_to_hall.time_60ms; // g_speed_to_hall.vert_speed_t.flag = calculation_end; // } g_speed_to_hall.vert_speed_t.endTime_us = TIMER_CNT(TIMER6); g_speed_to_hall.vert_speed_t.endTime_60ms = g_speed_to_hall.time_60ms; if (g_speed_to_hall.vert_speed_t.endTime_60ms > g_speed_to_hall.vert_speed_t.startTime_60ms) { v_hall_time = (g_speed_to_hall.vert_speed_t.endTime_60ms - g_speed_to_hall.vert_speed_t.startTime_60ms - 1) * SPEED_TIMER_PERIOD + g_speed_to_hall.vert_speed_t.endTime_us + (SPEED_TIMER_PERIOD - g_speed_to_hall.vert_speed_t.startTime_us); } else if (g_speed_to_hall.vert_speed_t.endTime_60ms = g_speed_to_hall.vert_speed_t.startTime_60ms) { v_hall_time = g_speed_to_hall.vert_speed_t.endTime_us - g_speed_to_hall.vert_speed_t.startTime_us; } else { v_hall_time = (g_speed_to_hall.vert_speed_t.endTime_60ms + (TIME_60MS_MAX - g_speed_to_hall.vert_speed_t.startTime_60ms) - 1) * SPEED_TIMER_PERIOD + g_speed_to_hall.vert_speed_t.endTime_us + (SPEED_TIMER_PERIOD - g_speed_to_hall.vert_speed_t.startTime_us); } g_speed_to_hall.vert_speed_t.speed = 0.8 * (5000000.0 / (float)v_hall_time) + 0.2 * g_speed_to_hall.hori_speed_t.lastSpeed; g_speed_to_hall.vert_speed_t.lastSpeed = g_speed_to_hall.vert_speed_t.speed; g_speed_to_hall.vert_speed_t.startTime_60ms = g_speed_to_hall.vert_speed_t.endTime_60ms; g_speed_to_hall.vert_speed_t.startTime_us = g_speed_to_hall.vert_speed_t.endTime_us; switch(hall_linex) { case EXTI_13://垂直HALL-1 // if(gpio_input_bit_get(GPIOE, GPIO_PIN_13) == SET) // { 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_14://垂直HALL-2 // if(gpio_input_bit_get(GPIOE, GPIO_PIN_14) == SET) // { 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_15://垂直HALL-3 // if(gpio_input_bit_get(GPIOE, GPIO_PIN_15) == SET) // { 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; } #endif // } break; default:break; } #endif #endif }