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

2
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);
OSTimeDlyHMSM(0u, 0u, 0u, 100u);
//蜂鸣器开，初始化开始
beep_enable();
// beep_enable();
//flash引脚初始化
Flash_GPIO_Init();

4
APP/Common/ptz_default_value.h
View File

@ -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
///转速单位 转/每分
///垂直电机最大转速

114
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);
}
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)) {
|| 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的输出值增量，当前输出值与上一次输出值的差值必须在某个范围内
//即防止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;
@ -983,7 +1013,8 @@ 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;
@ -1263,6 +1294,65 @@ 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调速
@ -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的输出值增量，当前输出值与上一次输出值的差值必须在某个范围内
//即防止PID增量过大

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

@ -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调速输出值限定，当前输出值和上一次输出值之间的差异
#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调速输入值限定

18
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) {
// 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;
@ -844,6 +843,23 @@ 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)
{

21
APP/Service/service_statusmonitor.c
View File

@ -13,24 +13,37 @@ static char ptz_temp_volt_current_fault_detect_task()
{//只报故障，不做响应
static unsigned short int time_ms;
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;
}
else