diff --git a/.vscode/settings.json b/.vscode/settings.json
new file mode 100644
index 0000000..8da1df1
--- /dev/null
+++ b/.vscode/settings.json
@@ -0,0 +1,6 @@
+{
+ "files.associations": {
+ "bsp_os.h": "c",
+ "cmath": "c"
+ }
+}
\ No newline at end of file
diff --git a/APP/Device/Device_speed/speed_to_bldc.c b/APP/Device/Device_speed/speed_to_bldc.c
index 913f112..3fec97d 100644
--- a/APP/Device/Device_speed/speed_to_bldc.c
+++ b/APP/Device/Device_speed/speed_to_bldc.c
@@ -9,6 +9,10 @@
#ifdef PTZ_BLDC_MOTOR
+static volatile float tempSpeedH = 0;
+static volatile float tempSpeedV = 0;
+
+
//发送云台实际转速
void ptz_send_speed(char dev, char speed)
{
@@ -64,9 +68,9 @@ void ptz_pid_init()
g_ptz.hori_pid.TD = PTZ_HORI_PID_HORI_TD;//微分常数
g_ptz.hori_pid.T = PTZ_HORI_PID_T / 1000.0;//采样周期
//三个简化参数A，B，C
- g_ptz.hori_pid.A = g_ptz.hori_pid.KP * (1 + g_ptz.hori_pid.T / g_ptz.hori_pid.TI + g_ptz.hori_pid.TD);
- g_ptz.hori_pid.B = g_ptz.hori_pid.KP * (1 + 2 * g_ptz.hori_pid.TD / g_ptz.hori_pid.T);
- g_ptz.hori_pid.C = g_ptz.hori_pid.KP * (g_ptz.hori_pid.TD / g_ptz.hori_pid.T);
+// g_ptz.hori_pid.A = g_ptz.hori_pid.KP * (1 + g_ptz.hori_pid.T / g_ptz.hori_pid.TI + g_ptz.hori_pid.TD);
+// g_ptz.hori_pid.B = g_ptz.hori_pid.KP * (1 + 2 * g_ptz.hori_pid.TD / g_ptz.hori_pid.T);
+// g_ptz.hori_pid.C = g_ptz.hori_pid.KP * (g_ptz.hori_pid.TD / g_ptz.hori_pid.T);
g_ptz.vert_pid.SumError = 0;//累计误差
g_ptz.vert_pid.LastError = 0;//上一次输入偏差
@@ -78,9 +82,10 @@ void ptz_pid_init()
g_ptz.vert_pid.TD = PTZ_VERT_PID_VERT_TD;//微分常数
g_ptz.vert_pid.T = PTZ_VERT_PID_T / 1000.0;//采样周期
//三个简化参数A，B，C
- g_ptz.vert_pid.A = g_ptz.vert_pid.KP * (1 + g_ptz.vert_pid.T / g_ptz.vert_pid.TI + g_ptz.vert_pid.TD);
- g_ptz.vert_pid.B = g_ptz.vert_pid.KP * (1 + 2 * g_ptz.vert_pid.TD / g_ptz.vert_pid.T);
- g_ptz.vert_pid.C = g_ptz.vert_pid.KP * (g_ptz.vert_pid.TD / g_ptz.vert_pid.T);
+// g_ptz.vert_pid.A = g_ptz.vert_pid.KP * (1 + g_ptz.vert_pid.T / g_ptz.vert_pid.TI + g_ptz.vert_pid.TD);
+// g_ptz.vert_pid.B = g_ptz.vert_pid.KP * (1 + 2 * g_ptz.vert_pid.TD / g_ptz.vert_pid.T);
+// g_ptz.vert_pid.C = g_ptz.vert_pid.KP * (g_ptz.vert_pid.TD / g_ptz.vert_pid.T);
+
}
void ptz_hori_pid_clear_zero()
@@ -108,7 +113,9 @@ 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 = g_ptz.hori_pid.KP * (H_IError - g_ptz.hori_pid.LastError) + g_ptz.hori_pid.TI * H_IError;
+
//存储误差，用于下次计算
g_ptz.hori_pid.PrevError = g_ptz.hori_pid.LastError;
g_ptz.hori_pid.LastError = H_IError;
@@ -121,6 +128,10 @@ static float ptz_hori_pid_calculate(float H_SampSpeed)
//新调速方式
static void ptz_hori_pid_task()
{
+
+
+ OSTimeDlyHMSM(0u, 0u, 0u, 1000u);
+
unsigned int time = 0;
char i = 0;
while(1)
@@ -580,7 +591,36 @@ static void ptz_hori_pid_task()
g_ptz.hori_speed_actual = g_ptz.hori_speed_hall_actual;
#endif
-
+
+#ifdef PTZ_PID_AS5047_SPEED
+
+ static float lastAngle = 0;
+ static float lastTime = 0;
+ float angle = 0;
+ float time = 0;
+
+ OSTimeDlyHMSM(0u, 0u, 0u, 20u);
+/* 读取到正确速度为止 */
+start:
+ angle = as5047d_hori_get_angle_a();
+ time = OSTimeGet();
+ if (angle < 0 || //防止读出的角度为负值
+ angle > 360.0||//防止读出的数值超过360
+ isnan(angle) == 1) {//防止读出的不是一个数
+ //以上错误的数据都该舍弃,从新读取
+ goto start;
+ }
+
+ float angleDifference = fabs(angle - lastAngle);
+ if (angleDifference < 3) {
+ // goto start;
+ return;
+ }
+
+ float timeDifference = fabs(time - lastTime);
+ g_ptz.hori_speed_actual = angleDifference / timeDifference / PTZ_HORI_RATIO * 1000;
+
+#endif
/*由于系统实时调节能力不足将调速分为两个部分：
1.启动时首先进入直线比例快速提速；
@@ -645,7 +685,9 @@ static void ptz_hori_pid_task()
{
g_ptz.hori_pid.PidUT_float = PTZ_HORI_VR_MAX;
}
-
+
+ tempSpeedH = g_ptz.hori_pid.PidUT_float;
+
g_ptz.hori_pid.PidUT_uint = (unsigned int)(g_ptz.hori_pid.PidUT_float + 0.5);
//限制PID的输出值在某个指定的范围
@@ -754,7 +796,9 @@ 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 = g_ptz.vert_pid.KP * (IError - g_ptz.vert_pid.LastError) + g_ptz.vert_pid.TI * IError;
+
//存储误差，用于下次计算
g_ptz.vert_pid.PrevError = g_ptz.vert_pid.LastError;
g_ptz.vert_pid.LastError = IError;
@@ -1196,7 +1240,7 @@ static void ptz_vert_pid_task()
g_ptz.vert_pid.hall_h123_motor_speed =
60000.0 * (float)(g_ptz.vert_pid.hall_h123_count /*-1*/) /
(float)PTZ_VERT_PID_T / PTZ_VERT_ONE_CYCLE_HALL_NUM;
-
+
}
g_ptz.vert_motor_speed_hall_actual = g_ptz.vert_pid.hall_h123_motor_speed;
@@ -1216,10 +1260,37 @@ static void ptz_vert_pid_task()
//根据电机转速计算云台转速
g_ptz.vert_speed_hall_actual = g_ptz.vert_motor_speed_hall_actual / PTZ_VERT_RATIO;
g_ptz.vert_speed_actual = g_ptz.vert_speed_hall_actual;
-
+
#endif
-
-
+
+#ifdef PTZ_PID_AS5047_SPEED
+ static float lastAngle = 0;
+ static float lastTime = 0;
+ float angle = 0;
+ float time = 0;
+
+ OSTimeDlyHMSM(0u, 0u, 0u, 20u);
+/* 读取到正确速度为止 */
+start:
+ angle = as5047d_vert_get_angle_a();
+ time = OSTimeGet();
+ if (angle < 0 || //防止读出的角度为负值
+ angle > 360.0||//防止读出的数值超过360
+ isnan(angle) == 1) {//防止读出的不是一个数
+ //以上错误的数据都该舍弃,从新读取
+ goto start;
+ }
+
+ float angleDifference = fabs(angle - lastAngle);
+ if (angleDifference < 3) {
+ // goto start;
+ return;
+ }
+
+ float timeDifference = fabs(time - lastTime);
+ g_ptz.vert_speed_actual = angleDifference / timeDifference / PTZ_VERT_RATIO * 166.667f;
+#endif
+
}
//PID调速
@@ -1303,7 +1374,9 @@ static void ptz_vert_pid_task()
}
g_ptz.vert_pid.PidUT_uint = (unsigned int)(g_ptz.vert_pid.PidUT_float + 0.5);
-
+
+ tempSpeedV = g_ptz.vert_pid.PidUT_float;
+
//限制PID的输出值在某个指定的范围
if(g_ptz.vert_pid.PidUT_uint <= PTZ_VERT_VR_MIN)//限制输入模拟电压最小值
{
diff --git a/APP/Device/Device_speed/speed_to_bldc.h b/APP/Device/Device_speed/speed_to_bldc.h
index 5d28824..a5d5c79 100644
--- a/APP/Device/Device_speed/speed_to_bldc.h
+++ b/APP/Device/Device_speed/speed_to_bldc.h
@@ -228,23 +228,25 @@
//通过芯片反馈测速
//#define PTZ_HORI_PID_JY02A_SPEED
//#define PTZ_VERT_PID_JY02A_SPEED
- #define PTZ_PID_HALL_SPEED 1
+ // #define PTZ_PID_HALL_SPEED 1
+
+ #define PTZ_PID_AS5047_SPEED 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 100.0//比例系数
+ #define PTZ_HORI_PID_HORI_TI 30.0 //积分系数
#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_HORI_PID_OUTPUT_LIMIT 50.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 100.0//比例系数
+ #define PTZ_VERT_PID_VERT_TI 30.0 //积分系数
#define PTZ_VERT_PID_VERT_TD 0.0 //微分系数
#define PTZ_VERT_PID_INPUT_LIMIT 100.0//PID调速输入值限定
- #define PTZ_VERT_PID_OUTPUT_LIMIT 200.0//PID调速输出值限定，当前输出值和上一次输出值之间的差异
+ #define PTZ_VERT_PID_OUTPUT_LIMIT 50.0//PID调速输出值限定，当前输出值和上一次输出值之间的差异
#define PTZ_HORI_PID_T_MAX 100u//PID调速最大周期
#define PTZ_VERT_PID_T_MAX 100u//PID调速最大周期
diff --git a/APP/Service/service_statusmonitor.c b/APP/Service/service_statusmonitor.c
index c727ad5..33d5d52 100644
--- a/APP/Service/service_statusmonitor.c
+++ b/APP/Service/service_statusmonitor.c
@@ -14,23 +14,35 @@ static char ptz_temp_volt_current_fault_detect_task()
static unsigned short int time_ms;
time_ms ++;
+
+ static unsigned char hCurrCount = 0;
+ static unsigned char vCurrCount = 0;
+
+
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);
+ if (3 < hCurrCount++) {
+ g_ptz.fault_detect.Phase_curr_H = FAULT;//水平电机相电流过大，报警
+ ptz_hori_stop(PTZ_HORI_STOP_TIME);
+ }
}
+ else
+ hCurrCount = 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 )
- {//堵转检测，防止电机堵转烧坏，结合电机卡死故障监测，与云台工作电流监测。
-
- g_ptz.fault_detect.Phase_curr_V = FAULT;//垂直电机相电流过大，报警
-
- ptz_vert_stop(PTZ_VERT_STOP_TIME);
+ {//堵转检测，防止电机堵转烧坏，结合电机卡死故障监测，与云台工作电流监测。
+ if (3 < vCurrCount++) {
+ g_ptz.fault_detect.Phase_curr_V = FAULT;//垂直电机相电流过大，报警
+ ptz_vert_stop(PTZ_VERT_STOP_TIME);
+ }
}
+ else
+ vCurrCount = 0;
+
+
return 1;
}
else
diff --git a/PROJECT/OS2.ewp b/PROJECT/OS2.ewp
index c85f083..68acab8 100644
--- a/PROJECT/OS2.ewp
+++ b/PROJECT/OS2.ewp
@@ -718,11 +718,11 @@