pengsx
/
LW21-01B
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

修改过流保护，添加俯仰速度计算（有误）

This commit is contained in:
起床就犯困 2025-06-10 17:26:14 +08:00
parent 845e5d73d5
commit 337edab694
6 changed files with 170 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
APP/Appcfg/app.c
View File

@ -113,7 +113,7 @@ static void task_start (void *p_arg)
// ck_f=rcu_clock_freq_get(CK_APB1); // ck_f=rcu_clock_freq_get(CK_APB1);
OSTimeDlyHMSM(0u, 0u, 0u, 100u); OSTimeDlyHMSM(0u, 0u, 0u, 100u);
//蜂鸣器开，初始化开始 //蜂鸣器开，初始化开始
beep_enable(); // beep_enable();
//flash引脚初始化 //flash引脚初始化
Flash_GPIO_Init(); Flash_GPIO_Init();
@ -230,10 +230,10 @@ int main (void)
CPU_INT08U name_err; CPU_INT08U name_err;
CPU_Init(); CPU_Init();
Mem_Init(); Mem_Init();
Math_Init(); Math_Init();
BSP_IntDisAll(); BSP_IntDisAll();
OSInit(); OSInit();
task_err = OSTaskCreateExt((void (*)(void *)) task_start, task_err = OSTaskCreateExt((void (*)(void *)) task_start,
(void *) 0, (void *) 0,

4
APP/Common/ptz_default_value.h
View File

@ -345,7 +345,7 @@
///水平电机调速模拟电压最大值 ///水平电机调速模拟电压最大值
#define PTZ_HORI_VR_MAX 1999 #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_MAX 1999
///垂直电机调速模拟电压最小值 ///垂直电机调速模拟电压最小值
#define PTZ_VERT_VR_MIN 0 #define PTZ_VERT_VR_MIN 50
///转速单位 转/每分 ///转速单位 转/每分
///垂直电机最大转速 ///垂直电机最大转速

122
APP/Device/Device_speed/speed_to_bldc.c
View File

@ -96,7 +96,7 @@ void ptz_hori_pid_clear_zero()
// hori_dac0_data_out(0); // 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; float H_IError,H_IIncPid;
H_IError = g_ptz.hori_speed_set - H_SampSpeed;//当前误差PID_INPUT_LIMIT 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_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; // 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); h_pwm_duty_change(PTZ_HORI_VR_MIN);
} }
/* 使能计算速度 */
g_speed_to_hall.hori_speed_t.flag = calculation_enable; 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 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); 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; g_speed_to_hall.hori_speed_t.flag = calculation_ok;
timeOut = 0;
/* 等待最后一个hall信号到来 */
while (!(g_speed_to_hall.hori_speed_t.flag == calculation_end while (!(g_speed_to_hall.hori_speed_t.flag == calculation_end
|| g_ptz.hori_start_stop_set == PTZ_HORI_STOP)) { || g_ptz.hori_start_stop_set == PTZ_HORI_STOP)
OSTimeDlyHMSM(0u, 0u, 0u, 1u); && 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) { 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 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); + 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; g_ptz.hori_speed_actual = (float)g_speed_to_hall.hori_speed_t.hallNum * 5000000.0 / (float)time / PTZ_HORI_RATIO;
timeOutErr:
;
#endif #endif
if(g_ptz.hori_speed_actual > g_ptz.hori_speed_set * 2) if(g_ptz.hori_speed_actual > g_ptz.hori_speed_set * 2)
@ -663,7 +690,7 @@ static void ptz_hori_pid_task()
else else
{ {
//计算PID控制器输出值 //计算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的输出值增量，当前输出值与上一次输出值的差值必须在某个范围内 //控制PID的输出值增量，当前输出值与上一次输出值的差值必须在某个范围内
//即防止PID增量过大 //即防止PID增量过大
@ -704,7 +731,7 @@ static void ptz_hori_pid_task()
//将PID输出的电机转速模拟电压等级输入到模拟电压输出芯片 //将PID输出的电机转速模拟电压等级输入到模拟电压输出芯片
// hori_dac0_data_out(g_ptz.hori_pid.PidUT_uint); // 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输出值保存 //将当前PID输出值保存
g_ptz.hori_pid.LastUT_float = g_ptz.hori_pid.PidUT_float; 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); // 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; float IError,IIncPid;
IError = g_ptz.vert_speed_set - SampSpeed;//当前误差PID_INPUT_LIMIT 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; // 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.PrevError = g_ptz.vert_pid.LastError;
g_ptz.vert_pid.LastError = IError; 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; g_ptz.vert_speed_actual = g_ptz.vert_speed_jy02a_actual;
#endif #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_h1_count = 0;
g_ptz.vert_pid.hall_h2_count = 0; g_ptz.vert_pid.hall_h2_count = 0;
@ -1262,8 +1293,67 @@ static void ptz_vert_pid_task()
#endif #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调速 //PID调速
if(g_ptz.vert_start_stop_set == PTZ_VERT_START && if(g_ptz.vert_start_stop_set == PTZ_VERT_START &&
@ -1318,7 +1408,7 @@ static void ptz_vert_pid_task()
else else
{ {
//计算PID控制器输出值 //计算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的输出值增量，当前输出值与上一次输出值的差值必须在某个范围内 //控制PID的输出值增量，当前输出值与上一次输出值的差值必须在某个范围内
//即防止PID增量过大 //即防止PID增量过大

8
APP/Device/Device_speed/speed_to_bldc.h
View File

@ -233,16 +233,16 @@
#define PTZ_HALL_SPEED_SL 1 #define PTZ_HALL_SPEED_SL 1
#define PTZ_HORI_PID_T 30u #define PTZ_HORI_PID_T 30u
#define PTZ_HORI_PID_HORI_KP 20.0//比例系数 #define PTZ_HORI_PID_HORI_KP 35.0//比例系数
#define PTZ_HORI_PID_HORI_TI 100.0 //积分系数 #define PTZ_HORI_PID_HORI_TI 0.0003 //积分系数
#define PTZ_HORI_PID_HORI_TD 0.0 //微分系数 #define PTZ_HORI_PID_HORI_TD 0.0 //微分系数
#define PTZ_HORI_PID_INPUT_LIMIT 100.0//PID调速输入值限定 #define PTZ_HORI_PID_INPUT_LIMIT 100.0//PID调速输入值限定
#define PTZ_HORI_PID_OUTPUT_LIMIT 200.0//PID调速输出值限定，当前输出值和上一次输出值之间的差异 #define PTZ_HORI_PID_OUTPUT_LIMIT 200.0//PID调速输出值限定，当前输出值和上一次输出值之间的差异
#define PTZ_VERT_PID_T 30u #define PTZ_VERT_PID_T 30u
#define PTZ_VERT_PID_VERT_KP 20.0//比例系数 #define PTZ_VERT_PID_VERT_KP 35.0//比例系数
#define PTZ_VERT_PID_VERT_TI 100.0 //积分系数 #define PTZ_VERT_PID_VERT_TI 0.0003 //积分系数
#define PTZ_VERT_PID_VERT_TD 0.0 //微分系数 #define PTZ_VERT_PID_VERT_TD 0.0 //微分系数
#define PTZ_VERT_PID_INPUT_LIMIT 100.0//PID调速输入值限定 #define PTZ_VERT_PID_INPUT_LIMIT 100.0//PID调速输入值限定

20
APP/Device/device_Other/device_interrupt.c
View File

@ -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) { 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_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.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.flag = calculation_end;
@ -843,7 +842,24 @@ void ptz_V_HALL_IRQHandler(exti_line_enum hall_linex)
#ifdef PTZ_BLDC_MOTOR #ifdef PTZ_BLDC_MOTOR
#ifdef PTZ_HALL_FEEDBACK #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) switch(hall_linex)
{ {

57
APP/Service/service_statusmonitor.c
View File

@ -13,30 +13,43 @@ static char ptz_temp_volt_current_fault_detect_task()
{//只报故障，不做响应 {//只报故障，不做响应
static unsigned short int time_ms; static unsigned short int time_ms;
time_ms ++; static uint8_t H_CURR_FAULT_NUM = 0;
if(time_ms < 50) static uint8_t V_CURR_FAULT_NUM = 0;
{
//轻型云台峰值电流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 ) time_ms ++;
{//堵转检测，防止电机堵转烧坏，结合电机卡死故障监测，与云台工作电流监测。 if(time_ms < 50)
{
g_ptz.fault_detect.Phase_curr_H = FAULT;//水平电机相电流过大，报警 //轻型云台峰值电流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 )
ptz_hori_stop(PTZ_HORI_STOP_TIME); {//堵转检测，防止电机堵转烧坏，结合电机卡死故障监测，与云台工作电流监测。
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 ) else
{//堵转检测，防止电机堵转烧坏，结合电机卡死故障监测，与云台工作电流监测。 {
time_ms = 0;
g_ptz.fault_detect.Phase_curr_V = FAULT;//垂直电机相电流过大，报警
ptz_vert_stop(PTZ_VERT_STOP_TIME);
} }
return 1;
}
else
{
time_ms = 0;
}
#ifdef Full_bridge //NTC温度监测，暂定温度上限值100摄氏度，后续测试修改 #ifdef Full_bridge //NTC温度监测，暂定温度上限值100摄氏度，后续测试修改