Skip to content

C
car-controlserver
Commit df3c6dae authored by 957dd's avatar 957dd
Browse files
Options

加入了新设备，还未测试

parent 96051595
Hide whitespace changes
Inline Side-by-side
Showing with 440 additions and 322 deletions
app/device_identity/device_identity.c
View file @ df3c6dae
......@@ -141,6 +141,9 @@ int device_judg(CodeEnum_t code,char *sub_str) {
}else if(code ==PAO_0401) {
device_init(DEVICE_PAO_PTZ0401);
my_zlog_info("使用型号%s",sub_str);
}else if(code ==PAO_0404) {
device_init(DEVICE_PAO_PTZ0404);
my_zlog_info("使用型号%s",sub_str);
}else if(code ==PGGPS_0403) {
device_init(DEVICE_PG_GPS0403);
my_zlog_info("使用定位设备%s",sub_str);
......
app/device_identity/device_identity.h
View file @ df3c6dae
......@@ -21,6 +21,7 @@ typedef enum {
TANK_0207,
SHIP_0301,
PAO_0401,
PAO_0404,
PGGPS_0403,
ROBOT_DOG0501
} CodeEnum_t;
......
build/Makefile
View file @ df3c6dae
......@@ -713,6 +713,54 @@ drivers/devicecontrol/ptz0401_control.c.s:
$(MAKE) $(MAKESILENT) -f CMakeFiles/main.dir/build.make CMakeFiles/main.dir/drivers/devicecontrol/ptz0401_control.c.s
.PHONY : drivers/devicecontrol/ptz0401_control.c.s
drivers/devicecontrol/ptz0404_control.o: drivers/devicecontrol/ptz0404_control.c.o
.PHONY : drivers/devicecontrol/ptz0404_control.o
# target to build an object file
drivers/devicecontrol/ptz0404_control.c.o:
$(MAKE) $(MAKESILENT) -f CMakeFiles/main.dir/build.make CMakeFiles/main.dir/drivers/devicecontrol/ptz0404_control.c.o
.PHONY : drivers/devicecontrol/ptz0404_control.c.o
drivers/devicecontrol/ptz0404_control.i: drivers/devicecontrol/ptz0404_control.c.i
.PHONY : drivers/devicecontrol/ptz0404_control.i
# target to preprocess a source file
drivers/devicecontrol/ptz0404_control.c.i:
$(MAKE) $(MAKESILENT) -f CMakeFiles/main.dir/build.make CMakeFiles/main.dir/drivers/devicecontrol/ptz0404_control.c.i
.PHONY : drivers/devicecontrol/ptz0404_control.c.i
drivers/devicecontrol/ptz0404_control.s: drivers/devicecontrol/ptz0404_control.c.s
.PHONY : drivers/devicecontrol/ptz0404_control.s
# target to generate assembly for a file
drivers/devicecontrol/ptz0404_control.c.s:
$(MAKE) $(MAKESILENT) -f CMakeFiles/main.dir/build.make CMakeFiles/main.dir/drivers/devicecontrol/ptz0404_control.c.s
.PHONY : drivers/devicecontrol/ptz0404_control.c.s
drivers/devicecontrol/ptz_common.o: drivers/devicecontrol/ptz_common.c.o
.PHONY : drivers/devicecontrol/ptz_common.o
# target to build an object file
drivers/devicecontrol/ptz_common.c.o:
$(MAKE) $(MAKESILENT) -f CMakeFiles/main.dir/build.make CMakeFiles/main.dir/drivers/devicecontrol/ptz_common.c.o
.PHONY : drivers/devicecontrol/ptz_common.c.o
drivers/devicecontrol/ptz_common.i: drivers/devicecontrol/ptz_common.c.i
.PHONY : drivers/devicecontrol/ptz_common.i
# target to preprocess a source file
drivers/devicecontrol/ptz_common.c.i:
$(MAKE) $(MAKESILENT) -f CMakeFiles/main.dir/build.make CMakeFiles/main.dir/drivers/devicecontrol/ptz_common.c.i
.PHONY : drivers/devicecontrol/ptz_common.c.i
drivers/devicecontrol/ptz_common.s: drivers/devicecontrol/ptz_common.c.s
.PHONY : drivers/devicecontrol/ptz_common.s
# target to generate assembly for a file
drivers/devicecontrol/ptz_common.c.s:
$(MAKE) $(MAKESILENT) -f CMakeFiles/main.dir/build.make CMakeFiles/main.dir/drivers/devicecontrol/ptz_common.c.s
.PHONY : drivers/devicecontrol/ptz_common.c.s
drivers/devicecontrol/ship0301_control.o: drivers/devicecontrol/ship0301_control.c.o
.PHONY : drivers/devicecontrol/ship0301_control.o
......@@ -2105,6 +2153,12 @@ help:
@echo "... drivers/devicecontrol/ptz0401_control.o"
@echo "... drivers/devicecontrol/ptz0401_control.i"
@echo "... drivers/devicecontrol/ptz0401_control.s"
@echo "... drivers/devicecontrol/ptz0404_control.o"
@echo "... drivers/devicecontrol/ptz0404_control.i"
@echo "... drivers/devicecontrol/ptz0404_control.s"
@echo "... drivers/devicecontrol/ptz_common.o"
@echo "... drivers/devicecontrol/ptz_common.i"
@echo "... drivers/devicecontrol/ptz_common.s"
@echo "... drivers/devicecontrol/ship0301_control.o"
@echo "... drivers/devicecontrol/ship0301_control.i"
@echo "... drivers/devicecontrol/ship0301_control.s"
......
build/main
View file @ df3c6dae
No preview for this file type
build/third_party/mosquitto/lib/CMakeFiles/libmosquitto.dir/progress.make
View file @ df3c6dae
......@@ -26,14 +26,14 @@ CMAKE_PROGRESS_25 =
CMAKE_PROGRESS_26 =
CMAKE_PROGRESS_27 = 9
CMAKE_PROGRESS_28 =
CMAKE_PROGRESS_29 = 10
CMAKE_PROGRESS_30 =
CMAKE_PROGRESS_29 =
CMAKE_PROGRESS_30 = 10
CMAKE_PROGRESS_31 =
CMAKE_PROGRESS_32 = 11
CMAKE_PROGRESS_33 =
CMAKE_PROGRESS_32 =
CMAKE_PROGRESS_33 = 11
CMAKE_PROGRESS_34 =
CMAKE_PROGRESS_35 = 12
CMAKE_PROGRESS_36 =
CMAKE_PROGRESS_35 =
CMAKE_PROGRESS_36 = 12
CMAKE_PROGRESS_37 =
CMAKE_PROGRESS_38 = 13
CMAKE_PROGRESS_39 =
......
build/third_party/mosquitto/lib/CMakeFiles/libmosquitto_static.dir/progress.make
View file @ df3c6dae
......@@ -11,20 +11,20 @@ CMAKE_PROGRESS_10 =
CMAKE_PROGRESS_11 =
CMAKE_PROGRESS_12 = 19
CMAKE_PROGRESS_13 =
CMAKE_PROGRESS_14 = 20
CMAKE_PROGRESS_15 =
CMAKE_PROGRESS_14 =
CMAKE_PROGRESS_15 = 20
CMAKE_PROGRESS_16 =
CMAKE_PROGRESS_17 = 21
CMAKE_PROGRESS_18 =
CMAKE_PROGRESS_17 =
CMAKE_PROGRESS_18 = 21
CMAKE_PROGRESS_19 =
CMAKE_PROGRESS_20 = 22
CMAKE_PROGRESS_21 =
CMAKE_PROGRESS_20 =
CMAKE_PROGRESS_21 = 22
CMAKE_PROGRESS_22 =
CMAKE_PROGRESS_23 = 23
CMAKE_PROGRESS_24 =
CMAKE_PROGRESS_23 =
CMAKE_PROGRESS_24 = 23
CMAKE_PROGRESS_25 =
CMAKE_PROGRESS_26 = 24
CMAKE_PROGRESS_27 =
CMAKE_PROGRESS_26 =
CMAKE_PROGRESS_27 = 24
CMAKE_PROGRESS_28 =
CMAKE_PROGRESS_29 = 25
CMAKE_PROGRESS_30 =
......@@ -40,6 +40,6 @@ CMAKE_PROGRESS_39 =
CMAKE_PROGRESS_40 =
CMAKE_PROGRESS_41 = 29
CMAKE_PROGRESS_42 =
CMAKE_PROGRESS_43 = 30
CMAKE_PROGRESS_44 =
CMAKE_PROGRESS_43 =
CMAKE_PROGRESS_44 = 30
build/third_party/mosquitto/plugins/dynamic-security/CMakeFiles/mosquitto_dynamic_security.dir/progress.make
View file @ df3c6dae
......@@ -5,9 +5,9 @@ CMAKE_PROGRESS_4 =
CMAKE_PROGRESS_5 =
CMAKE_PROGRESS_6 = 87
CMAKE_PROGRESS_7 =
CMAKE_PROGRESS_8 =
CMAKE_PROGRESS_9 = 88
CMAKE_PROGRESS_8 = 88
CMAKE_PROGRESS_9 =
CMAKE_PROGRESS_10 =
CMAKE_PROGRESS_11 =
CMAKE_PROGRESS_12 = 89
CMAKE_PROGRESS_11 = 89
CMAKE_PROGRESS_12 =
build/third_party/mosquitto/src/CMakeFiles/mosquitto.dir/progress.make
View file @ df3c6dae
CMAKE_PROGRESS_1 = 55
CMAKE_PROGRESS_1 =
CMAKE_PROGRESS_2 =
CMAKE_PROGRESS_3 =
CMAKE_PROGRESS_4 = 56
CMAKE_PROGRESS_3 = 56
CMAKE_PROGRESS_4 =
CMAKE_PROGRESS_5 =
CMAKE_PROGRESS_6 =
CMAKE_PROGRESS_7 = 57
CMAKE_PROGRESS_6 = 57
CMAKE_PROGRESS_7 =
CMAKE_PROGRESS_8 =
CMAKE_PROGRESS_9 =
CMAKE_PROGRESS_10 = 58
CMAKE_PROGRESS_9 = 58
CMAKE_PROGRESS_10 =
CMAKE_PROGRESS_11 =
CMAKE_PROGRESS_12 =
CMAKE_PROGRESS_13 = 59
CMAKE_PROGRESS_12 = 59
CMAKE_PROGRESS_13 =
CMAKE_PROGRESS_14 =
CMAKE_PROGRESS_15 = 60
CMAKE_PROGRESS_16 =
......@@ -20,26 +20,26 @@ CMAKE_PROGRESS_19 =
CMAKE_PROGRESS_20 =
CMAKE_PROGRESS_21 = 62
CMAKE_PROGRESS_22 =
CMAKE_PROGRESS_23 =
CMAKE_PROGRESS_24 = 63
CMAKE_PROGRESS_23 = 63
CMAKE_PROGRESS_24 =
CMAKE_PROGRESS_25 =
CMAKE_PROGRESS_26 =
CMAKE_PROGRESS_27 = 64
CMAKE_PROGRESS_26 = 64
CMAKE_PROGRESS_27 =
CMAKE_PROGRESS_28 =
CMAKE_PROGRESS_29 =
CMAKE_PROGRESS_30 = 65
CMAKE_PROGRESS_29 = 65
CMAKE_PROGRESS_30 =
CMAKE_PROGRESS_31 =
CMAKE_PROGRESS_32 =
CMAKE_PROGRESS_33 = 66
CMAKE_PROGRESS_32 = 66
CMAKE_PROGRESS_33 =
CMAKE_PROGRESS_34 =
CMAKE_PROGRESS_35 =
CMAKE_PROGRESS_36 = 67
CMAKE_PROGRESS_35 = 67
CMAKE_PROGRESS_36 =
CMAKE_PROGRESS_37 =
CMAKE_PROGRESS_38 =
CMAKE_PROGRESS_39 = 68
CMAKE_PROGRESS_38 = 68
CMAKE_PROGRESS_39 =
CMAKE_PROGRESS_40 =
CMAKE_PROGRESS_41 =
CMAKE_PROGRESS_42 = 69
CMAKE_PROGRESS_41 = 69
CMAKE_PROGRESS_42 =
CMAKE_PROGRESS_43 =
CMAKE_PROGRESS_44 = 70
CMAKE_PROGRESS_45 =
......@@ -55,20 +55,20 @@ CMAKE_PROGRESS_54 =
CMAKE_PROGRESS_55 =
CMAKE_PROGRESS_56 = 74
CMAKE_PROGRESS_57 =
CMAKE_PROGRESS_58 =
CMAKE_PROGRESS_59 = 75
CMAKE_PROGRESS_58 = 75
CMAKE_PROGRESS_59 =
CMAKE_PROGRESS_60 =
CMAKE_PROGRESS_61 =
CMAKE_PROGRESS_62 = 76
CMAKE_PROGRESS_61 = 76
CMAKE_PROGRESS_62 =
CMAKE_PROGRESS_63 =
CMAKE_PROGRESS_64 =
CMAKE_PROGRESS_65 = 77
CMAKE_PROGRESS_64 = 77
CMAKE_PROGRESS_65 =
CMAKE_PROGRESS_66 =
CMAKE_PROGRESS_67 =
CMAKE_PROGRESS_68 = 78
CMAKE_PROGRESS_67 = 78
CMAKE_PROGRESS_68 =
CMAKE_PROGRESS_69 =
CMAKE_PROGRESS_70 =
CMAKE_PROGRESS_71 = 79
CMAKE_PROGRESS_70 = 79
CMAKE_PROGRESS_71 =
CMAKE_PROGRESS_72 =
CMAKE_PROGRESS_73 = 80
CMAKE_PROGRESS_74 =
......
drivers/devicecontrol/devcontrol_common.c
View file @ df3c6dae
......@@ -52,6 +52,10 @@ const device_didrive device_didrive_control_config_t[]={
.device_id = DEVICE_PAO_PTZ0401,
.device_didrive_control = PTZ_pwm_change
},
{
.device_id = DEVICE_PAO_PTZ0404,
.device_didrive_control = ptz0404_pwm_change
},
// 结束标记
{ .device_id = -1 }
};
......@@ -154,6 +158,14 @@ const device_abnormal_close_t devcontrol_config_t[]= {
},
{
.device_id = DEVICE_PAO_PTZ0404,
.device_abnormal_stop = ptz0404_pwm_stop,
.device_close = device_poilthread_close, // TANK0206没有单独的关闭函数
.gpio_pin_pulled=pin_all_default,
.gpio_pwm_pulled=pwm_all_default
},
{
.device_id = DEVICE_ROBOT_DOG0501,
.device_abnormal_stop = car0101_middle_pwm,
.device_close = NULL, // TANK0206没有单独的关闭函数
......
drivers/devicecontrol/devcontrol_common.h
View file @ df3c6dae
......@@ -8,7 +8,7 @@
#include "car0103_control.h"
#include "car0104_control.h"
#include "car0105_control.h"
#include "ptz0401_control.h"
#include "ptz_common.h"
#include "tank0202_control.h"
#include "tank0203_control.h"
#include "tank0204_control.h"
......@@ -32,6 +32,7 @@
#define DEVICE_TANK0207 207 //可以发射水弹坦克
#define DEVICE_SHIP0301 301 // 32号船
#define DEVICE_PAO_PTZ0401 401 //云台
#define DEVICE_PAO_PTZ0404 404 //第二个烟花大云台
#define DEVICE_PG_GPS0403 403 //定位设备
#define DEVICE_ROBOT_DOG0501 501 //机械狗
......
drivers/devicecontrol/ptz0401_control.c
View file @ df3c6dae
......@@ -2,6 +2,7 @@
#include "modules_common.h"
#include "ptz0401_control.h"
#include "gpio_common.h"
#include "ptz_common.h"
#include <math.h>
/* ===================== 硬件/PWM 参数 ===================== */
......@@ -147,7 +148,7 @@ void PTZ_pwm_task(void)
servo_apply(PWM_PIN_up, servo_tilt.angle);
}
void ptz_pwm_task_function(void *arg)
void ptz0401_pwm_task_function(void *arg)
{
my_zlog_info("PTZ_pwm_task_function start");
while(1) {
......@@ -156,12 +157,6 @@ void ptz_pwm_task_function(void *arg)
}
}
void device_ptz_pwm_task_threadpoll_init()
{
my_zlog_info("PTZ_pwm_thread_init start");
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)
{
......
drivers/devicecontrol/ptz0401_control.h
View file @ df3c6dae
#ifndef PTZ0401_CONTROL_H__
#define PTZ0401_CONTROL_H__
/* ===================== 轴结构体与全局变量 ===================== */
typedef struct {
// --- 实时状态 ---
float angle; // 当前角度
float target; // 目标角度
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 ptz0401_pwm_task_function(void *arg);
void pwm_PTZ_hz();
void PTZ_pwm_init();
void PTZ_pwm_change(unsigned char *buf);
......
drivers/devicecontrol/ptz0402_control.c deleted 100755 → 0
View file @ 96051595
// #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 deleted 100755 → 0
View file @ 96051595
// #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/devicecontrol/ptz0404_control.c 0 → 100755
View file @ df3c6dae
#include "common.h"
#include "modules_common.h"
#include "ptz0404_control.h"
#include "gpio_common.h"
#include "ptz_common.h"
#include <math.h>
/* ===================== 运动学参数（分离配置） ===================== */
#define CONTROL_PERIOD 0.02f // 20ms 调用一次
// --- 水平方向 (左右) 参数 ---
// 为了配合“立即停止”，建议适当加大加速度，让起步也更灵敏
#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 // 停止死区（度）
/* ===================== 角度限位 ===================== */
#define PAN_ANGLE_INITIAL 135.0f
#define PAN_MIN_ANGLE 75.0f
#define PAN_MAX_ANGLE 195.0f
#define TILT_ANGLE_INITIAL 90.0f
#define TILT_MIN_ANGLE 0.0f
#define TILT_MAX_ANGLE 100.0f
/* ===================== 对象初始化 ===================== */
static ServoAxis servo_pan = {
.angle = PAN_ANGLE_INITIAL, .target = PAN_ANGLE_INITIAL, .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 = TILT_ANGLE_INITIAL, .target = TILT_ANGLE_INITIAL, .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_ptz0404_hz(void)
{
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 ptz0404_calculate_pulse_width(float angle)
{
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 ptz0404_calculate_duty_cycle(float pulse_width)
{
float period = 1000.0f / SERVO_FREQ;
return (int)((pulse_width / period) * PWM_RANGE);
}
static void ptz0404_servo_apply(int pin, float angle)
{
float pulse_width = ptz0404_calculate_pulse_width(angle);
int pulse = ptz0404_calculate_duty_cycle(pulse_width);
pwmWrite(pin, pulse);
}
/* ===================== 核心：带减速预测的运动学更新 ===================== */
static void ptz0404_servo_update(ServoAxis *s)
{
float error = s->target - s->angle;
float abs_error = fabs(error);
// 1. 检查是否进入静止死区
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)
float braking_dist = (s->speed * s->speed) / (2.0f * s->accel_cfg);
// 3. 速度规划
float desired_speed;
if (abs_error <= braking_dist) {
desired_speed = 0;
} else {
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 += speed_step;
if (s->speed > desired_speed && desired_speed >= 0) s->speed = desired_speed;
} else if (s->speed > desired_speed) {
s->speed -= speed_step;
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 ptz0404_pwm_task(void)
{
ptz0404_servo_update(&servo_pan);
ptz0404_servo_update(&servo_tilt);
ptz0404_servo_apply(PWM_PIN_down, servo_pan.angle);
ptz0404_servo_apply(PWM_PIN_up, servo_tilt.angle);
}
void ptz0404_pwm_task_function(void *arg)
{
my_zlog_info("PTZ_pwm_task_function start");
while(1) {
ptz0404_pwm_task();
delay_ms(20);
}
}
void ptz0404_pwm_init(void)
{
pwm_ptz0404_hz();
servo_pan.angle = servo_pan.target = PAN_ANGLE_INITIAL;
servo_tilt.angle = servo_tilt.target = TILT_ANGLE_INITIAL;
servo_pan.speed = 0;
servo_tilt.speed = 0;
ptz0404_servo_apply(PWM_PIN_down, servo_pan.angle);
ptz0404_servo_apply(PWM_PIN_up, servo_tilt.angle);
device_ptz_pwm_task_threadpoll_init();
}
/* ===================== 控制接口 ===================== */
void ptz0404_pwm_left(void)
{
servo_pan.target -= 10.0f;
if (servo_pan.target < PAN_MIN_ANGLE)
servo_pan.target = PAN_MIN_ANGLE;
}
void ptz0404_pwm_right(void)
{
servo_pan.target += 10.0f;
if (servo_pan.target > PAN_MAX_ANGLE)
servo_pan.target = PAN_MAX_ANGLE;
}
void ptz0404_pwm_up()
{
servo_tilt.target -= 10.0f;
if (servo_tilt.target < TILT_MIN_ANGLE)
servo_tilt.target = TILT_MIN_ANGLE;
}
void ptz0404_pwm_down()
{
servo_tilt.target += 10.0f;
if (servo_tilt.target > TILT_MAX_ANGLE)
servo_tilt.target = TILT_MAX_ANGLE;
}
/* ===================== 停止逻辑（关键修改） ===================== */
void ptz0404_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 ptz0404_pwm_change(unsigned char *buf)
{
// 假设 buf[1] 是命令字，通常协议里会有停止命令（例如 0 或 5）
// 如果收到 0 或者未定义的命令，应该调用 stop
if (buf[2] == 0) {
ptz0404_pwm_stop();
return; // 处理完停止直接退出，不检查 buf[2]
}
my_zlog_info("PTZ Receive CMD: %d", buf[1]);
switch (buf[1]) {
case 1: ptz0404_pwm_up(); break;
case 2: ptz0404_pwm_down(); break;
case 3: ptz0404_pwm_right(); break;
case 4: ptz0404_pwm_left(); break;
// 添加停止处理 (假设 0 是停止，具体看您的协议)
default: break; // 其他情况也停止，保证安全
}
}
\ No newline at end of file
drivers/devicecontrol/ptz0404_control.h 0 → 100755
View file @ df3c6dae
#ifndef PTZ0404_CONTROL_H__
#define PTZ0404_CONTROL_H__
void ptz0404_pwm_task_function(void *arg);
void pwm_ptz0404_hz();
void ptz0404_pwm_init();
void ptz0404_pwm_change(unsigned char *buf);
void ptz0404_pwm_stop();
#endif
\ No newline at end of file
drivers/devicecontrol/ptz_common.c 0 → 100644
View file @ df3c6dae
#include "ptz_common.h"
#include "ptz0401_control.h"
#include "ptz0404_control.h"
#include "common.h"
#include "modules_common.h"
#include "devcontrol_common.h"
#include "gpio_common.h"
#include <math.h>
void device_ptz_pwm_task_threadpoll_init()
{
my_zlog_info("PTZ_pwm_thread_init start");
g_pool_device_gpio_control_t = thread_pool_init(1, 1);
if(g_device_type == DEVICE_PAO_PTZ0401)thread_pool_add_task(g_pool_device_gpio_control_t, ptz0401_pwm_task_function, NULL);
if(g_device_type == DEVICE_PAO_PTZ0404)thread_pool_add_task(g_pool_device_gpio_control_t, ptz0404_pwm_task_function, NULL);
}
\ No newline at end of file
drivers/devicecontrol/ptz_common.h 0 → 100644
View file @ df3c6dae
#ifndef PTZ_COMMON_H
#define PTZ_COMMON_H
#include "ptz0401_control.h"
#include "ptz0404_control.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
/* ===================== 轴结构体与全局变量 ===================== */
typedef struct {
// --- 实时状态 ---
float angle; // 当前角度
float target; // 目标角度
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();
#endif
\ No newline at end of file
drivers/gpio/device_init.c
View file @ df3c6dae
......@@ -134,6 +134,18 @@ const deviceconfig_t device_configs[] = {
.device_control_stop = PTZ_pwm_init,/* 补充速度控制函数 */
.emergency_code = 401
},
{
.device_id = DEVICE_PAO_PTZ0404,
.device_name = "ptz0404",
.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_ptz0404_hz,
.device_control_stop = ptz0404_pwm_init,/* 补充速度控制函数 */
.emergency_code = 404
},
{
.device_id = DEVICE_PG_GPS0403,
.device_name = "gps0403",
......
fcsgdevintroduce.md
View file @ df3c6dae
......@@ -31,7 +31,7 @@
- car0103 为挖机 最大速度为200，更据电池电压具体调速。原电池为7.6v，大概为140左右
- car0104 为推土机 最大速度为200，更据电池电压具体调速。原电池为7.6v，大概为140左右
- car0105 为超大车，改遥控器，很多都没有
- ptz0401 为炮台，有限位。
- ptz0401 ptz0404 为炮台，有限位。
- 0403 为定位设备
- 0501 为机械狗
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment