From 337edab694cf6729fcbedb7970895439e10a2e64 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E8=B5=B7=E5=BA=8A=E5=B0=B1=E7=8A=AF=E5=9B=B0?= <11730503+psx123456@user.noreply.gitee.com> Date: Tue, 10 Jun 2025 17:26:14 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E8=BF=87=E6=B5=81=E4=BF=9D?= =?UTF-8?q?=E6=8A=A4=EF=BC=8C=E6=B7=BB=E5=8A=A0=E4=BF=AF=E4=BB=B0=E9=80=9F?= =?UTF-8?q?=E5=BA=A6=E8=AE=A1=E7=AE=97=EF=BC=88=E6=9C=89=E8=AF=AF=EF=BC=89?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- APP/Appcfg/app.c | 10 +- APP/Common/ptz_default_value.h | 4 +- APP/Device/Device_speed/speed_to_bldc.c | 122 ++++++++++++++++++--- APP/Device/Device_speed/speed_to_bldc.h | 8 +- APP/Device/device_Other/device_interrupt.c | 20 +++- APP/Service/service_statusmonitor.c | 57 ++++++---- 6 files changed, 170 insertions(+), 51 deletions(-) diff --git a/APP/Appcfg/app.c b/APP/Appcfg/app.c index 7d00bbd..e0eeff7 100644 --- a/APP/Appcfg/app.c +++ b/APP/Appcfg/app.c @@ -113,7 +113,7 @@ static void task_start (void *p_arg) // ck_f=rcu_clock_freq_get(CK_APB1); OSTimeDlyHMSM(0u, 0u, 0u, 100u); //蜂鸣器开，初始化开始 - beep_enable(); + // beep_enable(); //flash引脚初始化 Flash_GPIO_Init(); @@ -230,10 +230,10 @@ int main (void) CPU_INT08U name_err; CPU_Init(); - Mem_Init(); - Math_Init(); - BSP_IntDisAll(); - OSInit(); + Mem_Init(); + Math_Init(); + BSP_IntDisAll(); + OSInit(); task_err = OSTaskCreateExt((void (*)(void *)) task_start, (void *) 0, diff --git a/APP/Common/ptz_default_value.h b/APP/Common/ptz_default_value.h index e84962e..d93369c 100644 --- a/APP/Common/ptz_default_value.h +++ b/APP/Common/ptz_default_value.h @@ -345,7 +345,7 @@ ///ˮƽģѹֵ #define PTZ_HORI_VR_MAX 1999 ///ˮƽģѹСֵ - #define PTZ_HORI_VR_MIN 300 + #define PTZ_HORI_VR_MIN 250 ///תٵλ ת/ÿ ///ˮƽת @@ -370,7 +370,7 @@ ///ֱģѹֵ #define PTZ_VERT_VR_MAX 1999 ///ֱģѹСֵ - #define PTZ_VERT_VR_MIN 0 + #define PTZ_VERT_VR_MIN 50 ///תٵλ ת/ÿ ///ֱת diff --git a/APP/Device/Device_speed/speed_to_bldc.c b/APP/Device/Device_speed/speed_to_bldc.c index 8d76405..861cda0 100644 --- a/APP/Device/Device_speed/speed_to_bldc.c +++ b/APP/Device/Device_speed/speed_to_bldc.c @@ -96,7 +96,7 @@ void ptz_hori_pid_clear_zero() // hori_dac0_data_out(0); } -static float ptz_hori_pid_calculate(float H_SampSpeed) +static float ptz_hori_pid_calculate(float H_SampSpeed, uint32_t H_SampTime) { float H_IError,H_IIncPid; H_IError = g_ptz.hori_speed_set - H_SampSpeed;//ǰPID_INPUT_LIMIT @@ -110,7 +110,8 @@ static float ptz_hori_pid_calculate(float H_SampSpeed) // H_IError = PTZ_HORI_PID_INPUT_LIMIT * -1; // } // - H_IIncPid = g_ptz.hori_pid.A * H_IError + g_ptz.hori_pid.B * g_ptz.hori_pid.LastError + g_ptz.hori_pid.C * g_ptz.hori_pid.PrevError; +// H_IIncPid = g_ptz.hori_pid.A * H_IError + g_ptz.hori_pid.B * g_ptz.hori_pid.LastError + g_ptz.hori_pid.C * g_ptz.hori_pid.PrevError; + H_IIncPid = PTZ_HORI_PID_HORI_KP * (H_IError - g_ptz.hori_pid.LastError) + PTZ_HORI_PID_HORI_TI * H_IError * H_SampTime; // H_IIncPid = PTZ_HORI_PID_HORI_KP; @@ -622,22 +623,43 @@ static void ptz_hori_pid_task() h_pwm_duty_change(PTZ_HORI_VR_MIN); } + /* ʹܼٶ */ g_speed_to_hall.hori_speed_t.flag = calculation_enable; + /* ʱ */ + uint32_t timeOut = 0; + + /* ת󣬵ȴhallжٶȼ */ while (!(g_speed_to_hall.hori_speed_t.flag == calculation_start - || g_ptz.hori_start_stop_set == PTZ_HORI_STOP)) { + || g_ptz.hori_start_stop_set == PTZ_HORI_STOP) + && timeOut < 10) { OSTimeDlyHMSM(0u, 0u, 0u, 1u); + timeOut++; + } + if (timeOut >= 10) { + g_ptz.hori_speed_actual = 0; + goto timeOutErr; } - OSTimeDlyHMSM(0u, 0u, 0u, 100u); + /* ʱhallźٶ */ + OSTimeDlyHMSM(0u, 0u, 0u, 50u); g_speed_to_hall.hori_speed_t.flag = calculation_ok; + timeOut = 0; + + /* ȴһhallźŵ */ while (!(g_speed_to_hall.hori_speed_t.flag == calculation_end - || g_ptz.hori_start_stop_set == PTZ_HORI_STOP)) { - OSTimeDlyHMSM(0u, 0u, 0u, 1u); + || g_ptz.hori_start_stop_set == PTZ_HORI_STOP) + && timeOut < 10) { + OSTimeDlyHMSM(0u, 0u, 0u, 1u); + timeOut++; + } + if (timeOut >= 10) { + g_ptz.hori_speed_actual = 0; + goto timeOutErr; } - int32_t time; + uint32_t time; if (g_speed_to_hall.hori_speed_t.endTime_60ms > g_speed_to_hall.hori_speed_t.startTime_60ms) { 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); @@ -651,6 +673,11 @@ static void ptz_hori_pid_task() } g_ptz.hori_speed_actual = (float)g_speed_to_hall.hori_speed_t.hallNum * 5000000.0 / (float)time / PTZ_HORI_RATIO; + +timeOutErr: + ; + + #endif if(g_ptz.hori_speed_actual > g_ptz.hori_speed_set * 2) @@ -663,7 +690,7 @@ static void ptz_hori_pid_task() else { //PIDֵ - g_ptz.hori_pid.PidUT_float = ptz_hori_pid_calculate(g_ptz.hori_speed_actual) + g_ptz.hori_pid.LastUT_float; + g_ptz.hori_pid.PidUT_float = ptz_hori_pid_calculate(g_ptz.hori_speed_actual, time) + g_ptz.hori_pid.LastUT_float; //PIDֵǰֵһֵIJֵijΧ //ֹPID @@ -704,7 +731,7 @@ static void ptz_hori_pid_task() //PIDĵתģѹȼ뵽ģѹоƬ // hori_dac0_data_out(g_ptz.hori_pid.PidUT_uint); - // h_pwm_duty_change(g_ptz.hori_pid.PidUT_uint); + h_pwm_duty_change(g_ptz.hori_pid.PidUT_uint); //ǰPIDֵ g_ptz.hori_pid.LastUT_float = g_ptz.hori_pid.PidUT_float; @@ -782,7 +809,7 @@ void ptz_vert_pid_clear_zero() // vert_dac1_data_out(0); } -static float ptz_vert_pid_calculate(float SampSpeed) +static float ptz_vert_pid_calculate(float SampSpeed, uint32_t H_SampTime) { float IError,IIncPid; IError = g_ptz.vert_speed_set - SampSpeed;//ǰPID_INPUT_LIMIT @@ -797,7 +824,10 @@ static float ptz_vert_pid_calculate(float SampSpeed) // IError = PTZ_VERT_PID_INPUT_LIMIT * -1; // } // - IIncPid = g_ptz.vert_pid.A * IError + g_ptz.vert_pid.B * g_ptz.vert_pid.LastError + g_ptz.vert_pid.C * g_ptz.vert_pid.PrevError; +// IIncPid = g_ptz.vert_pid.A * IError + g_ptz.vert_pid.B * g_ptz.vert_pid.LastError + g_ptz.vert_pid.C * g_ptz.vert_pid.PrevError; + + IIncPid = PTZ_VERT_PID_VERT_KP * (IError - g_ptz.hori_pid.LastError) + PTZ_VERT_PID_VERT_TI * IError * H_SampTime; + //洢´μ g_ptz.vert_pid.PrevError = g_ptz.vert_pid.LastError; g_ptz.vert_pid.LastError = IError; @@ -982,8 +1012,9 @@ static void ptz_vert_pid_task() g_ptz.vert_speed_actual = g_ptz.vert_speed_jy02a_actual; #endif - -#ifdef PTZ_PID_HALL_SPEED // + +// #ifdef PTZ_PID_HALL_SPEED // +#if 0 g_ptz.vert_pid.hall_h1_count = 0; g_ptz.vert_pid.hall_h2_count = 0; @@ -1262,8 +1293,67 @@ static void ptz_vert_pid_task() #endif - - + +#ifdef PTZ_HALL_SPEED_SL + if (g_ptz.vert_speed_actual == 0) { + v_pwm_duty_change(PTZ_VERT_VR_MIN); + } + + /* ʹܼٶ */ + g_speed_to_hall.vert_speed_t.flag = calculation_enable; + + /* ʱ */ + uint32_t timeOut = 0; + + /* ת󣬵ȴhallжٶȼ */ + while (!(g_speed_to_hall.vert_speed_t.flag == calculation_start + || g_ptz.vert_start_stop_set == PTZ_HORI_STOP) + && timeOut < 10) { + OSTimeDlyHMSM(0u, 0u, 0u, 1u); + timeOut++; + } + if (timeOut >= 10) { + g_ptz.vert_speed_actual = 0; + goto timeOutErr; + } + + /* ʱhallźٶ */ + OSTimeDlyHMSM(0u, 0u, 0u, 50u); + g_speed_to_hall.vert_speed_t.flag = calculation_ok; + + timeOut = 0; + + /* ȴһhallźŵ */ + while (!(g_speed_to_hall.vert_speed_t.flag == calculation_end + || g_ptz.vert_start_stop_set == PTZ_HORI_STOP) + && timeOut < 10) { + OSTimeDlyHMSM(0u, 0u, 0u, 1u); + timeOut++; + } + if (timeOut >= 10) { + g_ptz.vert_speed_actual = 0; + goto timeOutErr; + } + + uint32_t time; + if (g_speed_to_hall.vert_speed_t.endTime_60ms > g_speed_to_hall.vert_speed_t.startTime_60ms) { + 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) { + time = g_speed_to_hall.vert_speed_t.endTime_us - g_speed_to_hall.vert_speed_t.startTime_us; + } + else { + 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_ptz.hori_speed_actual = (float)g_speed_to_hall.vert_speed_t.hallNum * 5000000.0 / (float)time / PTZ_VERT_RATIO; + +timeOutErr: + +#endif + } //PID if(g_ptz.vert_start_stop_set == PTZ_VERT_START && @@ -1318,7 +1408,7 @@ static void ptz_vert_pid_task() else { //PIDֵ - g_ptz.vert_pid.PidUT_float = ptz_vert_pid_calculate(g_ptz.vert_speed_actual) + g_ptz.vert_pid.LastUT_float; + g_ptz.vert_pid.PidUT_float = ptz_vert_pid_calculate(g_ptz.vert_speed_actual, time) + g_ptz.vert_pid.LastUT_float; //PIDֵǰֵһֵIJֵijΧ //ֹPID diff --git a/APP/Device/Device_speed/speed_to_bldc.h b/APP/Device/Device_speed/speed_to_bldc.h index 119116c..fb27d9e 100644 --- a/APP/Device/Device_speed/speed_to_bldc.h +++ b/APP/Device/Device_speed/speed_to_bldc.h @@ -233,16 +233,16 @@ #define PTZ_HALL_SPEED_SL 1 #define PTZ_HORI_PID_T 30u - #define PTZ_HORI_PID_HORI_KP 20.0//ϵ - #define PTZ_HORI_PID_HORI_TI 100.0 //ϵ + #define PTZ_HORI_PID_HORI_KP 35.0//ϵ + #define PTZ_HORI_PID_HORI_TI 0.0003 //ϵ #define PTZ_HORI_PID_HORI_TD 0.0 //΢ϵ #define PTZ_HORI_PID_INPUT_LIMIT 100.0//PIDֵ޶ #define PTZ_HORI_PID_OUTPUT_LIMIT 200.0//PIDֵ޶ǰֵһֵ֮IJ #define PTZ_VERT_PID_T 30u - #define PTZ_VERT_PID_VERT_KP 20.0//ϵ - #define PTZ_VERT_PID_VERT_TI 100.0 //ϵ + #define PTZ_VERT_PID_VERT_KP 35.0//ϵ + #define PTZ_VERT_PID_VERT_TI 0.0003 //ϵ #define PTZ_VERT_PID_VERT_TD 0.0 //΢ϵ #define PTZ_VERT_PID_INPUT_LIMIT 100.0//PIDֵ޶ diff --git a/APP/Device/device_Other/device_interrupt.c b/APP/Device/device_Other/device_interrupt.c index 2d02825..8cbc71d 100644 --- a/APP/Device/device_Other/device_interrupt.c +++ b/APP/Device/device_Other/device_interrupt.c @@ -619,7 +619,6 @@ void ptz_H_HALL_IRQHandler(exti_line_enum hall_linex) } if (g_speed_to_hall.hori_speed_t.flag == calculation_ok) { - // g_speed_to_hall.hori_speed_t.hallNum++; 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; @@ -843,7 +842,24 @@ 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; + } + switch(hall_linex) { diff --git a/APP/Service/service_statusmonitor.c b/APP/Service/service_statusmonitor.c index c727ad5..d7feb4d 100644 --- a/APP/Service/service_statusmonitor.c +++ b/APP/Service/service_statusmonitor.c @@ -13,30 +13,43 @@ static char ptz_temp_volt_current_fault_detect_task() {//ֻϣӦ static unsigned short int time_ms; - time_ms ++; - if(time_ms < 50) - { - //ֵ̨5.4A - if(H_ADC_Collect.Phase_curr_V >= PHASE_CURRENT ||H_ADC_Collect.Phase_curr_U >= PHASE_CURRENT ||H_ADC_Collect.Phase_curr_W >= PHASE_CURRENT ) - {//ת⣬ֹתջϵϼ⣬̨⡣ - - g_ptz.fault_detect.Phase_curr_H = FAULT;//ˮƽ󣬱 - - ptz_hori_stop(PTZ_HORI_STOP_TIME); + static uint8_t H_CURR_FAULT_NUM = 0; + static uint8_t V_CURR_FAULT_NUM = 0; + + + time_ms ++; + if(time_ms < 50) + { + //ֵ̨5.4A + if(H_ADC_Collect.Phase_curr_V >= PHASE_CURRENT ||H_ADC_Collect.Phase_curr_U >= PHASE_CURRENT ||H_ADC_Collect.Phase_curr_W >= PHASE_CURRENT ) + {//ת⣬ֹתջϵϼ⣬̨⡣ + if (5 < H_CURR_FAULT_NUM++) { + g_ptz.fault_detect.Phase_curr_H = FAULT;//ˮƽ󣬱 + ptz_hori_stop(PTZ_HORI_STOP_TIME); + } + } + else { + H_CURR_FAULT_NUM = 0; + } + + if(V_ADC_Collect.Phase_curr_V >= PHASE_CURRENT ||V_ADC_Collect.Phase_curr_U >= PHASE_CURRENT ||V_ADC_Collect.Phase_curr_W >= PHASE_CURRENT ) + {//ת⣬ֹתջϵϼ⣬̨⡣ + if (5 < V_CURR_FAULT_NUM++) { + g_ptz.fault_detect.Phase_curr_V = FAULT;//ֱ󣬱 + ptz_vert_stop(PTZ_VERT_STOP_TIME); + } + } + else { + V_CURR_FAULT_NUM = 0; + } + + + return 1; } - if(V_ADC_Collect.Phase_curr_V >= PHASE_CURRENT ||V_ADC_Collect.Phase_curr_U >= PHASE_CURRENT ||V_ADC_Collect.Phase_curr_W >= PHASE_CURRENT ) - {//ת⣬ֹתջϵϼ⣬̨⡣ - - g_ptz.fault_detect.Phase_curr_V = FAULT;//ֱ󣬱 - - ptz_vert_stop(PTZ_VERT_STOP_TIME); + else + { + time_ms = 0; } - return 1; - } - else - { - time_ms = 0; - } #ifdef Full_bridge //NTC¶ȼ⣬ݶ¶ֵ100϶ȣ޸