Merge branch 'feature/ptz_change_yuntai' into 'master'

drivers/devicecontrol/devcontrol_common.c

@@ -88,7 +88,7 @@ const device_abnormal_close_t devcontrol_config_t[]= {
     {
         .device_id = DEVICE_TANK0202,
         .device_abnormal_stop = tank0202_middle,
-        .device_close = tank_thread_close,
+        .device_close = device_poilthread_close,
         .gpio_pin_pulled=pin_all_default,
         .gpio_pwm_pulled=pwm_all_default
     },
@@ -96,7 +96,7 @@ const device_abnormal_close_t devcontrol_config_t[]= {
     {
         .device_id = DEVICE_TANK0203,
         .device_abnormal_stop = tank0203_middle,
-        .device_close = tank_thread_close,
+        .device_close = device_poilthread_close,
         .gpio_pin_pulled=pin_all_default,
         .gpio_pwm_pulled=pwm_all_default
     },
@@ -104,7 +104,7 @@ const device_abnormal_close_t devcontrol_config_t[]= {
     {
         .device_id = DEVICE_TANK0204,
         .device_abnormal_stop = tank0204_stop,
-        .device_close = tank_thread_close,
+        .device_close = device_poilthread_close,
         .gpio_pin_pulled=pin_all_default,
         .gpio_pwm_pulled=tankpwm_default
     },
@@ -135,8 +135,8 @@ const device_abnormal_close_t devcontrol_config_t[]= {
 
     {
         .device_id = DEVICE_PAO_PTZ0401,
-        .device_abnormal_stop = PTZ_pwm_init,
-        .device_close = NULL,                 // TANK0206没有单独的关闭函数
+        .device_abnormal_stop = ptz_pwm_stop,
+        .device_close = device_poilthread_close,                 // TANK0206没有单独的关闭函数
         .gpio_pin_pulled=pin_all_default,
         .gpio_pwm_pulled=pwm_all_default
     },

drivers/devicecontrol/ptz0401_control.c

@@ -2,135 +2,217 @@
 #include "modules_common.h"
 #include "ptz0401_control.h"
 #include "gpio_common.h"
+#include <math.h>
 
+/* ===================== 硬件/PWM 参数 ===================== */
 #define PWM_PIN_up        21
 #define PWM_PIN_down      2
 
-// 舵机最小脉冲宽度（ms）
-#define MIN_PULSE_WIDTH 0.5
-// 舵机最大脉冲宽度（ms）
-#define MAX_PULSE_WIDTH 2.5
+#define SERVO_FREQ        50.0f
+#define PWM_RANGE         1024
 
-float Initial_value=90;
+// 舵机脉冲宽度（ms）- 适配 270 度舵机
+#define MIN_PULSE_WIDTH   0.5f
+#define MAX_PULSE_WIDTH   2.5f
 
-static int val_change = 90;
-static int val_up_down = 90;
-static unsigned char change_index=0;
-static unsigned char up_down_index=0;
+/* ===================== 运动学参数（平滑控制核心） ===================== */
+#define CONTROL_PERIOD    0.02f      // 20ms 调用一次
+#define SERVO_MAX_SPEED   180.0f     // 最大角速度 °/s
+#define SERVO_ACCEL       800.0f     // 加速度 °/s²
+#define STOP_DEADZONE     0.2f      // 停止死区（度）
 
+/* ===================== 角度限位 ===================== */
+#define PAN_MIN_ANGLE     30.0f
+#define PAN_MAX_ANGLE     150.0f
 
-void pwm_PTZ_hz() {
-    int pwm_clock = 24000000 / (50 * 1000);// 定义 PWM 频率为 50Hz
-    pinMode(PWM_PIN_up,PWM_OUTPUT);
-    pinMode(PWM_PIN_down,PWM_OUTPUT);
-    pwmSetClock(PWM_PIN_up,pwm_clock);//=19200*1000/(hz*2000)
-    pwmSetRange(PWM_PIN_up,1024);//占空比范围
-    pwmSetClock(PWM_PIN_down,pwm_clock);//=19200*1000/(hz*2000)
-    pwmSetRange(PWM_PIN_down,1024);//占空比范围
+#define TILT_MIN_ANGLE    40.0f
+#define TILT_MAX_ANGLE    140.0f
 
+
+
+static ServoAxis servo_pan  = {90.0f, 90.0f, 0.0f, PAN_MIN_ANGLE, PAN_MAX_ANGLE};
+static ServoAxis servo_tilt = {90.0f, 90.0f, 0.0f, TILT_MIN_ANGLE, TILT_MAX_ANGLE};
+
+/* ===================== PWM 初始化 ===================== */
+void pwm_PTZ_hz(void)
+{
+    // 计算时钟频率，确保输出 50Hz PWM
+    int pwm_clock = 24000000 / (SERVO_FREQ * 1000); 
+
+    pinMode(PWM_PIN_up, PWM_OUTPUT);
+    pinMode(PWM_PIN_down, PWM_OUTPUT);
+
+    pwmSetClock(PWM_PIN_up, pwm_clock);
+    pwmSetRange(PWM_PIN_up, PWM_RANGE);
+
+    pwmSetClock(PWM_PIN_down, pwm_clock);
+    pwmSetRange(PWM_PIN_down, PWM_RANGE);
 }
 
-float calculate_pulse_width(int angle) {
-    if (angle < 0) {
-        angle = 0;
-    } else if (angle > 270) {
-        angle = 270;
-    }
-    return ((MAX_PULSE_WIDTH - MIN_PULSE_WIDTH) / 270.0) * angle + MIN_PULSE_WIDTH;
+/* ===================== 角度 → PWM 映射 ===================== */
+static float calculate_pulse_width(float angle)
+{
+    // 硬件物理保护：不允许超过 0-270 度范围
+    if (angle < 0) angle = 0;
+    if (angle > 270) angle = 270;
+
+    return ((MAX_PULSE_WIDTH - MIN_PULSE_WIDTH) / 270.0f) * angle + MIN_PULSE_WIDTH;
 }
 
-int calculate_duty_cycle(float pulse_width) {
-    // 周期（ms）
-    float period = 1000.0 / 50;
-    return (int)((pulse_width / period) * 1024);
+static int calculate_duty_cycle(float pulse_width)
+{
+    float period = 1000.0f / SERVO_FREQ; // 20ms
+    return (int)((pulse_width / period) * PWM_RANGE);
 }
 
-void PTZ_pwm_init() {
-    float pulse_width = calculate_pulse_width(Initial_value);
-    int pulse=calculate_duty_cycle(pulse_width);
-    pwmWrite(PWM_PIN_up,pulse);
-    pwmWrite(PWM_PIN_down,pulse);
+static void servo_apply(int pin, float angle)
+{
+    float pulse_width = calculate_pulse_width(angle);
+    int pulse = calculate_duty_cycle(pulse_width);
+    pwmWrite(pin, pulse);
 }
 
-void PTZ_pwm_left() {
-    float pulse_width;
-    int pulse; 
-    change_index++; 
-    if(change_index>=1) {
-        change_index=0;
-        val_change -= 3;
-        if(val_change <= 30)val_change=30; 
-        pulse_width = calculate_pulse_width(val_change);
-        pulse=calculate_duty_cycle(pulse_width);
+/* ===================== 核心：带减速预测的运动学更新 ===================== */
+static void servo_update(ServoAxis *s)
+{
+    float error = s->target - s->angle;
+    float abs_error = fabs(error);
+
+    // 1. 检查是否进入静止死区
+    // 只有在距离足够近且速度已经降下来时才真正停止，防止突然卡死
+    if (abs_error < STOP_DEADZONE && fabs(s->speed) < (SERVO_ACCEL * CONTROL_PERIOD)) {
+        s->angle = s->target;
+        s->speed = 0;
+        return;
     }
-    printf("%d\n",pulse);
-    pwmWrite(PWM_PIN_down,pulse);
-}
 
-void PTZ_pwm_right() {
-    float pulse_width;
-    int pulse;
-    change_index++;
-    if(change_index>=1) {
-        change_index=0;
-        val_change += 3;
-        if(val_change >= 150)val_change=150; 
-        pulse_width = calculate_pulse_width(val_change);
-        pulse=calculate_duty_cycle(pulse_width);
+    // 2. 计算刹车距离 (物理公式: d = v^2 / 2a)
+    // 预测以当前加速度减速到 0 需要走多少度
+    float braking_dist = (s->speed * s->speed) / (2.0f * SERVO_ACCEL);
+
+    // 3. 速度规划
+    float desired_speed;
+    if (abs_error <= braking_dist) {
+        // 如果剩余距离已经小于等于刹车距离，目标速度应设为 0（开始减速）
+        desired_speed = 0;
+    } else {
+        // 否则，向目标方向以最大速度运行
+        desired_speed = (error > 0) ? SERVO_MAX_SPEED : -SERVO_MAX_SPEED;
     }
-    printf("%d\n",pulse);
-    pwmWrite(PWM_PIN_down,pulse);
-}
 
-void PTZ_pwm_up() {
-    float pulse_width;
-    int pulse;
-    up_down_index++;
-    if(up_down_index>=1) {
-        up_down_index = 0;
-        val_up_down += 3;
-        if(val_up_down >= 110) val_up_down=110;
-        pulse_width = calculate_pulse_width(val_up_down);
-        pulse=calculate_duty_cycle(pulse_width);
+    // 4. 根据加速度计算当前步进速度
+    if (s->speed < desired_speed) {
+        s->speed += SERVO_ACCEL * CONTROL_PERIOD;
+        if (s->speed > desired_speed && desired_speed >= 0) s->speed = desired_speed;
+    } else if (s->speed > desired_speed) {
+        s->speed -= SERVO_ACCEL * CONTROL_PERIOD;
+        if (s->speed < desired_speed && desired_speed <= 0) s->speed = desired_speed;
     }
-    printf("%d\n",pulse); 
-    pwmWrite(PWM_PIN_up,pulse);
-}
 
-void PTZ_pwm_down() {
-    float pulse_width;
-    int pulse;
-    up_down_index++;
-    if(up_down_index>=1) {
-        up_down_index = 0;
-        val_up_down -= 3;
-        if(val_up_down <= 70) val_up_down=70;
-        pulse_width = calculate_pulse_width(val_up_down);
-        pulse=calculate_duty_cycle(pulse_width);
+    // 5. 更新实际位置
+    s->angle += s->speed * CONTROL_PERIOD;
+
+    // 6. 软限位二次约束：防止计算导致的越界抽动
+    if (s->angle < s->min_limit) {
+        s->angle = s->min_limit;
+        s->speed = 0;
+    } else if (s->angle > s->max_limit) {
+        s->angle = s->max_limit;
+        s->speed = 0;
     }
-    printf("%d\n",pulse);
-    pwmWrite(PWM_PIN_up,pulse);
 }
 
-void PTZ_pwm_change(unsigned char *buf) {
-    unsigned char mode=buf[1];
-    unsigned char val=buf[2];
-    if(val != 0) {
-        switch(mode) {
-            case 1:			
-                PTZ_pwm_up();					 
-                break;
-            case 2:	
-                PTZ_pwm_down();
-                break;
-            case 3:
-                PTZ_pwm_right();
-                break;
-            case 4:
-                PTZ_pwm_left();
-                break; 
-            default:
-                break;
+/* ===================== 任务与线程逻辑 ===================== */
+void PTZ_pwm_task(void)
+{
+    servo_update(&servo_pan);
+    servo_update(&servo_tilt);
+
+    servo_apply(PWM_PIN_down, servo_pan.angle);
+    servo_apply(PWM_PIN_up,   servo_tilt.angle);
+}
+
+void ptz_pwm_task_function(void *arg)
+{
+    my_zlog_info("PTZ_pwm_task_function start");
+    while(1) {
+        PTZ_pwm_task();
+        delay_ms(20); // 50Hz 频率更新
     }
+}
+
+/* 云台线程初始化 */
+void device_ptz_pwm_task_threadpoll_init()
+{
+    my_zlog_info("PTZ_pwm_thread_init start");
+    // 假设 g_pool_device_gpio_control_t 是全局线程池变量
+    g_pool_device_gpio_control_t = thread_pool_init(1, 1);
+    thread_pool_add_task(g_pool_device_gpio_control_t, ptz_pwm_task_function, NULL);
+}
+
+/* 全局初始化接口 */
+void PTZ_pwm_init(void)
+{
+    pwm_PTZ_hz(); // 必须先初始化硬件配置
+
+    servo_pan.angle  = servo_pan.target  = 90.0f;
+    servo_tilt.angle = servo_tilt.target = 90.0f;
+
+    servo_apply(PWM_PIN_down, servo_pan.angle);
+    servo_apply(PWM_PIN_up,   servo_tilt.angle);
+
+    device_ptz_pwm_task_threadpoll_init();
+}
+
+/* ===================== 控制接口（修改目标角度） ===================== */
+void PTZ_pwm_left(void)
+{
+    servo_pan.target -= 10.0f; // 步长可以适当加大，因为有平滑过渡
+    if (servo_pan.target < PAN_MIN_ANGLE)
+        servo_pan.target = PAN_MIN_ANGLE;
+}
+
+void PTZ_pwm_right(void)
+{
+    servo_pan.target += 10.0f;
+    if (servo_pan.target > PAN_MAX_ANGLE)
+        servo_pan.target = PAN_MAX_ANGLE;
+}
+
+void PTZ_pwm_up(void)
+{
+    servo_tilt.target += 10.0f;
+    if (servo_tilt.target > TILT_MAX_ANGLE)
+        servo_tilt.target = TILT_MAX_ANGLE;
+}
+
+void PTZ_pwm_down(void)
+{
+    servo_tilt.target -= 10.0f;
+    if (servo_tilt.target < TILT_MIN_ANGLE)
+        servo_tilt.target = TILT_MIN_ANGLE;
+}
+
+void ptz_pwm_stop(void)
+{
+    // 停止不是瞬间清零，而是将目标设为当前位置，让算法平滑减速
+    servo_pan.target  = servo_pan.angle;
+    servo_tilt.target = servo_tilt.angle;
+}
+  
+/* ===================== 指令解析 ===================== */
+void PTZ_pwm_change(unsigned char *buf)
+{
+    // buf[2] 通常作为有效性检测或速度系数
+    if (buf[2] == 0) return;
+
+    my_zlog_info("PTZ Receive CMD: %d", buf[1]);
+
+    switch (buf[1]) {
+        case 1: PTZ_pwm_up();    break;
+        case 2: PTZ_pwm_down();  break;
+        case 3: PTZ_pwm_right(); break;
+        case 4: PTZ_pwm_left();  break;
+        default: break;
     }
 }
\ No newline at end of file

drivers/devicecontrol/ptz0401_control.h

 #ifndef PTZ0401_CONTROL_H__
 #define PTZ0401_CONTROL_H__
 
+
+/* ===================== 轴结构体与全局变量 ===================== */
+// 增加了限位属性，便于在算法内部强制约束
+typedef struct {
+    float angle;        // 当前角度
+    float target;       // 目标角度
+    float speed;        // 当前速度
+    float min_limit;    // 最小限位
+    float max_limit;    // 最大限位
+} ServoAxis;
+
+void device_ptz_pwm_task_threadpoll_init();
+
 void pwm_PTZ_hz();
 void PTZ_pwm_init();
 void PTZ_pwm_change(unsigned char *buf) ;
-
+void ptz_pwm_stop();
 #endif
\ No newline at end of file

drivers/devicecontrol/tank_common.c

@@ -229,9 +229,9 @@ void set_tank_shot_index_cool(bool index){
 /*
 * @brief 销毁坦克使用的线程池，让其正常销毁，只有在tank相关设备号下才有用，最后销毁都会到device——common.h中
 */
-void tank_thread_close(){
-	thread_pool_destroy(pool_tank_t);
-	thread_pool_destroy(g_pool_device_gpio_control_t);
+void device_poilthread_close(){
+	if(pool_tank_t)  thread_pool_destroy(pool_tank_t);
+	if(g_pool_device_gpio_control_t) thread_pool_destroy(g_pool_device_gpio_control_t);
 }
 
 /*坦克射击接口，只有在特定设备号下使用*/

drivers/devicecontrol/tank_common.h

@@ -41,6 +41,6 @@ bool set_backshotstatus(bool flag);
 int tankshot_detection_backcount(int *cool_time,int flag);
 
 /*关闭线程*/
-void tank_thread_close();
+void device_poilthread_close();
 
 #endif
\ No newline at end of file

drivers/gpio/gpio_control.c

@@ -31,6 +31,7 @@ void dog0501_pwm_value(int pin,int value);
 
 TankFireControl g_device_shot_t;   // 真正的结构体变量
 
+
 /**
  * @brief 初始化坦克射击控制器
  * @param this 控制器指针
@@ -198,7 +199,6 @@ void tank_angle_limit_function(){
             delay_ms(5);
         }
              
-    
     }
 }
 
@@ -209,6 +209,7 @@ void device_gpio_control_threadpoll_init(){
     thread_pool_add_task(g_pool_device_gpio_control_t, tank_angle_limit_function, NULL);
 }
 
+
 /*设备拉低引脚结构体数组*/
 const gpiocontrol_t gpio_configs[] = {
     {

drivers/gpio/gpio_control.h

@@ -33,6 +33,8 @@ int device_shot_cooling_init();
 
 extern ThreadPool_t *g_pool_device_gpio_control_t;
 
+void set_gpio_control_config_t(ThreadPool_t *s);
+
 extern int g_tank_shot_index;
 
 void device_fast_read();//快速判断