修改了角度

drivers/devicecontrol/ptz0401_control.c

@@ -15,28 +15,45 @@
 #define MIN_PULSE_WIDTH   0.5f
 #define MAX_PULSE_WIDTH   2.5f
 
-/* ===================== 运动学参数（平滑控制核心） ===================== */
+/* ===================== 运动学参数（分离配置） ===================== */
 #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
 
-#define TILT_MIN_ANGLE    40.0f
-#define TILT_MAX_ANGLE    140.0f
+// --- 水平方向 (左右) 参数 ---
+// 为了配合“立即停止”，建议适当加大加速度，让起步也更灵敏
+#define PAN_MAX_SPEED     120.0f      // 左右最大速度 °/s
+#define PAN_ACCEL         300.0f     // 左右加速度 °/s²
 
+// --- 垂直方向 (上下) 参数 ---
+#define TILT_MAX_SPEED    150.0f     // 上下最大速度 °/s
+#define TILT_ACCEL        700.0f     // 上下加速度 °/s²
 
+#define STOP_DEADZONE     0.2f       // 停止死区（度）
 
-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};
+/* ===================== 角度限位 ===================== */
+#define PAN_MIN_ANGLE     0.0f
+#define PAN_MAX_ANGLE     270.0f
+
+#define TILT_MIN_ANGLE    0.0f
+#define TILT_MAX_ANGLE    80.0f
+
+/* ===================== 对象初始化 ===================== */
+static ServoAxis servo_pan = {
+    .angle = 135.0f, .target = 135.0f, .speed = 0.0f,
+    .min_limit = PAN_MIN_ANGLE, .max_limit = PAN_MAX_ANGLE,
+    .max_speed_cfg = PAN_MAX_SPEED, 
+    .accel_cfg = PAN_ACCEL
+};
+
+static ServoAxis servo_tilt = {
+    .angle = 90.0f, .target = 90.0f, .speed = 0.0f,
+    .min_limit = TILT_MIN_ANGLE, .max_limit = TILT_MAX_ANGLE,
+    .max_speed_cfg = TILT_MAX_SPEED, 
+    .accel_cfg = TILT_ACCEL
+};
 
 /* ===================== PWM 初始化 ===================== */
 void pwm_PTZ_hz(void)
 {
-    // 计算时钟频率，确保输出 50Hz PWM
     int pwm_clock = 24000000 / (SERVO_FREQ * 1000); 
 
     pinMode(PWM_PIN_up, PWM_OUTPUT);
@@ -52,16 +69,14 @@ void pwm_PTZ_hz(void)
 /* ===================== 角度 → 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;
 }
 
 static int calculate_duty_cycle(float pulse_width)
 {
-    float period = 1000.0f / SERVO_FREQ; // 20ms
+    float period = 1000.0f / SERVO_FREQ;
     return (int)((pulse_width / period) * PWM_RANGE);
 }
 
@@ -79,40 +94,38 @@ static void servo_update(ServoAxis *s)
     float abs_error = fabs(error);
 
     // 1. 检查是否进入静止死区
-    // 只有在距离足够近且速度已经降下来时才真正停止，防止突然卡死
-    if (abs_error < STOP_DEADZONE && fabs(s->speed) < (SERVO_ACCEL * CONTROL_PERIOD)) {
+    if (abs_error < STOP_DEADZONE && fabs(s->speed) < (s->accel_cfg * CONTROL_PERIOD)) {
         s->angle = s->target;
         s->speed = 0;
         return;
     }
 
-    // 2. 计算刹车距离 (物理公式: d = v^2 / 2a)
-    // 预测以当前加速度减速到 0 需要走多少度
-    float braking_dist = (s->speed * s->speed) / (2.0f * SERVO_ACCEL);
+    // 2. 计算刹车距离 (d = v^2 / 2a)
+    float braking_dist = (s->speed * s->speed) / (2.0f * s->accel_cfg);
 
     // 3. 速度规划
     float desired_speed;
     if (abs_error <= braking_dist) {
-        // 如果剩余距离已经小于等于刹车距离，目标速度应设为 0（开始减速）
         desired_speed = 0;
     } else {
-        // 否则，向目标方向以最大速度运行
-        desired_speed = (error > 0) ? SERVO_MAX_SPEED : -SERVO_MAX_SPEED;
+        desired_speed = (error > 0) ? s->max_speed_cfg : -s->max_speed_cfg;
     }
 
     // 4. 根据加速度计算当前步进速度
+    float speed_step = s->accel_cfg * CONTROL_PERIOD;
+    
     if (s->speed < desired_speed) {
-        s->speed += SERVO_ACCEL * CONTROL_PERIOD;
+        s->speed += speed_step;
         if (s->speed > desired_speed && desired_speed >= 0) s->speed = desired_speed;
     } else if (s->speed > desired_speed) {
-        s->speed -= SERVO_ACCEL * CONTROL_PERIOD;
+        s->speed -= speed_step;
         if (s->speed < desired_speed && desired_speed <= 0) s->speed = desired_speed;
     }
 
     // 5. 更新实际位置
     s->angle += s->speed * CONTROL_PERIOD;
 
-    // 6. 软限位二次约束：防止计算导致的越界抽动
+    // 6. 软限位二次约束
     if (s->angle < s->min_limit) {
         s->angle = s->min_limit;
         s->speed = 0;
@@ -137,26 +150,25 @@ void ptz_pwm_task_function(void *arg)
     my_zlog_info("PTZ_pwm_task_function start");
     while(1) {
         PTZ_pwm_task();
-        delay_ms(20); // 50Hz 频率更新
+        delay_ms(20); 
     }
 }
 
-/* 云台线程初始化 */
 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(); // 必须先初始化硬件配置
+    pwm_PTZ_hz(); 
 
     servo_pan.angle  = servo_pan.target  = 90.0f;
     servo_tilt.angle = servo_tilt.target = 90.0f;
+    servo_pan.speed = 0;
+    servo_tilt.speed = 0;
 
     servo_apply(PWM_PIN_down, servo_pan.angle);
     servo_apply(PWM_PIN_up,   servo_tilt.angle);
@@ -164,10 +176,10 @@ void PTZ_pwm_init(void)
     device_ptz_pwm_task_threadpoll_init();
 }
 
-/* ===================== 控制接口（修改目标角度） ===================== */
+/* ===================== 控制接口 ===================== */
 void PTZ_pwm_left(void)
 {
-    servo_pan.target -= 10.0f; // 步长可以适当加大，因为有平滑过渡
+    servo_pan.target -= 10.0f; 
     if (servo_pan.target < PAN_MIN_ANGLE)
         servo_pan.target = PAN_MIN_ANGLE;
 }
@@ -179,32 +191,45 @@ void PTZ_pwm_right(void)
         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)
+void PTZ_pwm_up()
 {
     servo_tilt.target -= 10.0f;
     if (servo_tilt.target < TILT_MIN_ANGLE) 
         servo_tilt.target = TILT_MIN_ANGLE;
 }
 
+void PTZ_pwm_down()
+{
+    servo_tilt.target += 10.0f;
+    if (servo_tilt.target > TILT_MAX_ANGLE)
+        servo_tilt.target = TILT_MAX_ANGLE;
+}
+
+/* ===================== 停止逻辑（关键修改） ===================== */
 void ptz_pwm_stop(void)
 {
-    // 停止不是瞬间清零，而是将目标设为当前位置，让算法平滑减速
+    // --- 左右 (Pan) 立即停止 ---
+    // 1. 设置目标为当前角度（不再继续走）
     servo_pan.target  = servo_pan.angle;
+    // 2. 【核心】强制速度清零。
+    //    如果不清零，算法会根据加速度计算刹车距离并滑行一段。
+    //    强制清零后，位置更新立即停止，实现“急停”。
+    servo_pan.speed   = 0.0f; 
+
+    // --- 上下 (Tilt) 保持平滑缓冲 ---
+    // 只重置目标，让算法自动平滑减速到停止
     servo_tilt.target = servo_tilt.angle;
 }
   
 /* ===================== 指令解析 ===================== */
 void PTZ_pwm_change(unsigned char *buf)
 {
-    // buf[2] 通常作为有效性检测或速度系数
-    if (buf[2] == 0) return;
+    // 假设 buf[1] 是命令字，通常协议里会有停止命令（例如 0 或 5）
+    // 如果收到 0 或者未定义的命令，应该调用 stop
+     if (buf[2] == 0) {
+        ptz_pwm_stop();
+        return; // 处理完停止直接退出，不检查 buf[2]
+    }
     
     my_zlog_info("PTZ Receive CMD: %d", buf[1]);
 
@@ -213,6 +238,7 @@ void PTZ_pwm_change(unsigned char *buf)
         case 2: PTZ_pwm_down();  break;
         case 3: PTZ_pwm_right(); break;
         case 4: PTZ_pwm_left();  break;
-        default: break;
+        // 添加停止处理 (假设 0 是停止，具体看您的协议)
+        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;    // 最大限位
+    float speed;        // 当前实时速度
+
+    // --- 静态配置 (每个轴独立) ---
+    float min_limit;    // 最小角度限位
+    float max_limit;    // 最大角度限位
+    float max_speed_cfg;// 最大速度配置 (°/s)
+    float accel_cfg;    // 加速度配置 (°/s²)
 } 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_change(unsigned char *buf);
 void ptz_pwm_stop();
+
 #endif
\ No newline at end of file

drivers/devicecontrol/ptz0402_control.c

+// #include "common.h"
+// #include "modules_common.h"
+// #include "ptz0402_control.h"
+// #include "gpio_common.h"
+// #include <math.h>
+
+// /* ===================== 硬件/PWM 参数 ===================== */
+// #define PWM_PIN_up        21
+// #define PWM_PIN_down      2
+
+// #define SERVO_FREQ        50.0f
+// #define PWM_RANGE         1024
+
+// // 舵机脉冲宽度（ms）- 适配 270 度舵机
+// #define MIN_PULSE_WIDTH   0.5f
+// #define MAX_PULSE_WIDTH   2.5f
+
+// /* ===================== 运动学参数（平滑控制核心） ===================== */
+// #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     0.0f
+// #define PAN_MAX_ANGLE     270.0f
+
+// #define TILT_MIN_ANGLE    0.0f
+// #define TILT_MAX_ANGLE    270.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_ptz0402_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);
+// }
+
+// /* ===================== 角度 → 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;
+// }
+
+// static int calculate_duty_cycle(float pulse_width)
+// {
+//     float period = 1000.0f / SERVO_FREQ; // 20ms
+//     return (int)((pulse_width / period) * PWM_RANGE);
+// }
+
+// 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);
+// }
+
+// /* ===================== 核心：带减速预测的运动学更新 ===================== */
+// 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;
+//     }
+
+//     // 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;
+//     }
+
+//     // 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;
+//     }
+
+//     // 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;
+//     }
+// }
+
+// /* ===================== 任务与线程逻辑 ===================== */
+// 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 ptz0402_pwm_init(void)
+// {
+//     pwm_ptz0402_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()
+// {
+//     servo_tilt.target += 10.0f;
+//     if (servo_tilt.target > TILT_MAX_ANGLE)
+//         servo_tilt.target = TILT_MAX_ANGLE;
+// }
+
+// void PTZ_pwm_down()
+// {
+
+//     servo_tilt.target -= 10.0f;
+//     if (servo_tilt.target < TILT_MIN_ANGLE)
+//         servo_tilt.target = TILT_MIN_ANGLE;
+// }
+
+// void ptz0402_pwm_stop(void)
+// {
+//     // 停止不是瞬间清零，而是将目标设为当前位置，让算法平滑减速
+//     servo_pan.target  = servo_pan.angle;
+//     servo_tilt.target = servo_tilt.angle;
+// }
+  
+// /* ===================== 指令解析 ===================== */
+// void ptz0402_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/ptz0402_control.h

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

drivers/gpio/device_init.c

@@ -127,8 +127,8 @@ const deviceconfig_t device_configs[] = {
     {
         .device_id = DEVICE_PAO_PTZ0401,
         .device_name = "ptz0401",
-        .gpio_pins = {5, 6, 7, 10, 16, 20, 22, 23, 24, 25, 26,-1},/* 补充GPIO引脚 */
-        .gpio_pwms = { 27,-1},
+        .gpio_pins = {5, 6, 7, 10, 16, 20, 22, 23, 24, 25, 26,27,-1},/* 补充GPIO引脚 */
+        .gpio_pwms = { -1},
         .gpio_inputs={-1},
         .device_pwm_init = pwm_PTZ_hz,
         .device_control_stop =  PTZ_pwm_init,/* 补充速度控制函数 */

drivers/gpio/gpio_control.c

@@ -20,6 +20,7 @@ void tank0204_pin_value(int pin,int value);
 void car0102_pin_value(int pin,int value);
 void car0103_pin_value(int pin,int value);
 void car0104_pin_value(int pin,int value);
+void ptz0401_pin_value(int pin,int value);
 
 void public_pwm_value(int pin ,int value);
 void tank0202_pwm_value(int pin,int value);
@@ -285,7 +286,7 @@ const gpiocontrol_t gpio_configs[] = {
     {
         .device_id = DEVICE_PAO_PTZ0401,
         .category_id=0,
-        .device_pin_value =public_pin_value,
+        .device_pin_value =ptz0401_pin_value,
         .device_pwm_value =public_pwm_value
     },
     {
@@ -492,6 +493,12 @@ void car0104_pin_value(int pin,int value){
     }else public_pin_value(pin,value);
 }
 
+void ptz0401_pin_value(int pin,int value){
+    if(pin==27){
+        public_pin_value(23,value);
+    }else public_pin_value(pin,value);
+}
+
 void public_pwm_value(int pin ,int value){
         for(int i = 0 ; i <= g_gpio_softpwmcount ; i++) {
             if(pin == g_gpioPwm[i]) {