Skip to content

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

加入了检测代码，并没有完成

parent 3b8a2211
Hide whitespace changes
Inline Side-by-side
Showing with 216 additions and 100 deletions
build/main
View file @ 5c540ef9
No preview for this file type
drivers/devicecontrol/pg0403_serial.c
View file @ 5c540ef9
......@@ -89,6 +89,8 @@ int serial_open(SerialPort *sp, const char *port_name, int baud_rate) {
sp->port_name, strerror(errno));
return 0; // 失败
}
// 方法1：保持非阻塞标志
//fcntl(sp->fd, F_SETFL, O_NONBLOCK);
// 恢复为阻塞模式（对应原代码中的 fcntl(fd_, F_SETFL, 0)）
fcntl(sp->fd, F_SETFL, 0);
......@@ -230,27 +232,29 @@ void process_serial_data(const uint8_t *buffer, int len) {
uint16_t raw_y = ((uint16_t)buffer[15] << 8) | buffer[16];
// 3. 执行卡尔曼滤波
if (g_filters[i].initialized == 0) {
// 如果是第一次收到这个标签的数据，直接初始化为测量值
// 否则滤波需要很长时间才能从 0 爬升到 实际坐标
g_filters[i].kx.estimate = (float)raw_x;
g_filters[i].ky.estimate = (float)raw_y;
g_filters[i].kx.error_cov = 1.0f;
g_filters[i].ky.error_cov = 1.0f;
g_filters[i].initialized = 1;
x_local[i] = raw_x;
y_local[i] = raw_y;
} else {
// 后续数据进入滤波逻辑
float filtered_x = run_kalman(&g_filters[i].kx, (float)raw_x);
float filtered_y = run_kalman(&g_filters[i].ky, (float)raw_y);
// 4. 将滤波结果转回整数存入全局变量
x_local[i] = (uint16_t)(filtered_x + 0.5f); // +0.5 为了四舍五入
y_local[i] = (uint16_t)(filtered_y + 0.5f);
}
x_local[i] = raw_x; // +0.5 为了四舍五入
y_local[i] = raw_y;
// // 3. 执行卡尔曼滤波
// if (g_filters[i].initialized == 0) {
// // 如果是第一次收到这个标签的数据，直接初始化为测量值
// // 否则滤波需要很长时间才能从 0 爬升到 实际坐标
// g_filters[i].kx.estimate = (float)raw_x;
// g_filters[i].ky.estimate = (float)raw_y;
// g_filters[i].kx.error_cov = 1.0f;
// g_filters[i].ky.error_cov = 1.0f;
// g_filters[i].initialized = 1;
// x_local[i] = raw_x;
// y_local[i] = raw_y;
// } else {
// // 后续数据进入滤波逻辑
// float filtered_x = run_kalman(&g_filters[i].kx, (float)raw_x);
// float filtered_y = run_kalman(&g_filters[i].ky, (float)raw_y);
// // 4. 将滤波结果转回整数存入全局变量
// x_local[i] = (uint16_t)(filtered_x + 0.5f); // +0.5 为了四舍五入
// y_local[i] = (uint16_t)(filtered_y + 0.5f);
// }
tag_id[i]=i;
//my_zlog_debug("串口数据更新 -> 标签ID: %d, X: %d, Y: %d", tag_id[i], x_local[i], y_local[i]);
......@@ -271,7 +275,7 @@ int pg0403_serial_init_close(){
// 新增：构建并处理 JSON 数据的函数
// ==========================================
void pg0403_serial_gpssend_mqtt_json(int id, uint16_t x, uint16_t y) {
// 1. 创建 JSON 对象: { }
cJSON *root = cJSON_CreateObject();
if (root == NULL) {
return;
......@@ -281,40 +285,24 @@ void pg0403_serial_gpssend_mqtt_json(int id, uint16_t x, uint16_t y) {
cJSON_AddNumberToObject(head, "message_type", 1);
// 2. 向对象添加字段
// 这里的键名 ("tag_id", "x", "y") 可以根据你的 MQTT 协议需求修改
cJSON_AddNumberToObject(body, "tag_id", id);
cJSON_AddNumberToObject(body, "x", x);
cJSON_AddNumberToObject(body, "y", y);
cJSON_AddNumberToObject(body, "id", id);
cJSON_AddNumberToObject(body, "resX", x);
cJSON_AddNumberToObject(body, "resY", y);
cJSON_AddItemToObject(root, "body", body);
cJSON_AddItemToObject(root, "head",head);
// 可选：添加时间戳
// cJSON_AddNumberToObject(root, "timestamp", current_timestamp_ms());
// 3. 将 JSON 对象打印为字符串
// cJSON_Print: 带缩进格式化（方便调试看）
// cJSON_PrintUnformatted: 压缩成一行（节省流量，适合 MQTT 发送）
char *json_str = cJSON_PrintUnformatted(root);
if (json_str == NULL) {
cJSON_Delete(root);
return;
}
// -----------------------------------------------------
// TODO: 在这里调用你的 MQTT 发送函数
// mqtt_publish("topic/location", json_str);
// -----------------------------------------------------
my_zlog_info("[MQTT Payload]: %s", json_str);
// 4. 重要：释放内存！
// cJSON_Print 分配了 json_str 的内存，必须 free
// cJSON_CreateObject 分配了 root 的内存，必须 cJSON_Delete
char mqtt_topic_pg[256];
sprintf(mqtt_topic_pg,"positioning/%s",mqtt_topic_pure_number());
......
drivers/devicecontrol/pg0403_serial.h
View file @ 5c540ef9
......@@ -7,10 +7,10 @@
// 卡尔曼参数配置 (根据实际效果调整)
// R (测量噪声): 值越大，滤波越平滑，但滞后越明显。防止漂移建议设大一点 (10 ~ 100)
// Q (过程噪声): 值越大，系统认为物体运动越快。如果对运动响应太慢，把这个调大 (0.01 ~ 1.0)
#define KALMAN_R 50.0f
#define KALMAN_Q 0.1f
#define KALMAN_R 0.01f
#define KALMAN_Q 0.01f
// 定义一个结构体来模拟简单的对象上下文（可选，为了保持整洁）
// 定义一个结构体表示串口，整洁
typedef struct {
int fd;
char port_name[64];
......
drivers/devicecontrol/tank0202_control.c
View file @ 5c540ef9
......@@ -75,6 +75,9 @@ void mode_back(unsigned char gval) {
int tank0202_change_grading(int mode){
int mode_val=0;
switch(mode){
case 0:
mode_val=0;
break;
case 2:
mode_val=10;
break;
......@@ -88,6 +91,7 @@ int tank0202_change_grading(int mode){
mode_val=50;
break;
default:
mode_val=40;
break;
}
return mode_val;
......
drivers/devicecontrol/tank0203_control.c
View file @ 5c540ef9
......@@ -76,6 +76,9 @@ void tank0203_mode_right_flont(unsigned char gval) {
int tank0203_change_grading(int mode){
int mode_val=0;
switch(mode){
case 0:
mode_val=0;
break;
case 2:
mode_val=10;
break;
......@@ -89,6 +92,7 @@ int tank0203_change_grading(int mode){
mode_val=50;
break;
default:
mode_val=30;
break;
}
return mode_val;
......
drivers/devicecontrol/tank0204_control.c
View file @ 5c540ef9
......@@ -72,6 +72,9 @@ void tank0204_mode_back(unsigned char gval) {
int tank0204_change_grading(int mode){
int mode_val=0;
switch(mode){
case 0:
mode_val=0;
break;
case 2:
mode_val=10;
break;
......@@ -85,6 +88,7 @@ int tank0204_change_grading(int mode){
mode_val=50;
break;
default:
mode_val=40;
break;
}
return mode_val;
......
drivers/devicecontrol/tank0206_control.c
View file @ 5c540ef9
......@@ -82,6 +82,9 @@ void tank0206_mode_right_back(unsigned char gval) {
int tank0206_change_grading(int mode){
int mode_val=0;
switch(mode){
case 0:
mode_val=0;
break;
case 2:
mode_val=10;
break;
......@@ -101,6 +104,7 @@ int tank0206_change_grading(int mode){
mode_val=70;
break;
default:
mode_val=30;
break;
}
return mode_val;
......
drivers/devicecontrol/tank_common.c
View file @ 5c540ef9
......@@ -8,7 +8,7 @@ int tank_shot_back_stop(unsigned char pin,unsigned char val);
const tank_common_back *g_tank_common_config_t = NULL;
int g_tank_shot_index=0;//状态机，用于状态机,坦克接收射击倒退状态机全局变量
int g_tank_shot_index_cool=1;//状态机，用于冷却状态机,坦克接收射击倒退状态机全局变量
static bool s_tank_shot_index_cool=1;//状态机，用于冷却状态机,坦克接收射击倒退状态机全局变量
/*击打后退速度设置，混空模式下为单路，单独模式下为双路控制，后续需要优化*/
void tank_shot_back(unsigned char gval) {
......@@ -50,6 +50,70 @@ const tank_common_back tank_common_config_t[]={
{ .device_id = -1 }
};
static bool s_back_status=false;
static int tank_delay_shot_back(){
if(g_device_delay_back_count <= g_tank_common_config_t->back_time) {
tank_shot_back(g_tank_common_config_t->shot_back_speed);
my_zlog_debug("击打成功");
}
else if(g_device_delay_back_count > g_tank_common_config_t->back_time&&g_device_delay_back_count < (g_tank_common_config_t->back_time+30)){
tank_shot_back(0);
my_zlog_debug("击打完成");
}
else if(g_device_delay_back_count>g_tank_common_config_t->back_interval_back){
g_tank_shot_index = 1;
s_back_status=false;
}
}
static bool s_cool_count_index =false;
/*用来判断有射击冷却的检测的状态获取*/
bool getshot_detection_index(){
return s_cool_count_index;
}
/*用来设置有射击冷却的检测的状态获取*/
bool setshot_detection_index(bool flase){
s_cool_count_index=flase;
}
static pthread_mutex_t tankshot_detection_i2c_mutex = PTHREAD_MUTEX_INITIALIZER; // I2C 操作互斥锁
/*flag=0时初始化冷却时间，1是计时，2是返回计时值,3是重置计时*/
int tankshot_detection_backcount(int *cool_time,int flag){
static int cool_count_init=0;
static int cool_count=0;
if(flag==0){
cool_count_init=*cool_time;
my_zlog_info("cool_count_init的值：%d",cool_count_init);
}
else if(flag==1){
pthread_mutex_lock(&tankshot_detection_i2c_mutex);
cool_count++;
if(cool_count>cool_count_init*2){
s_cool_count_index =true;
}
if(cool_count>200000) cool_count>200001;
pthread_mutex_unlock(&tankshot_detection_i2c_mutex);
}else if(flag==2){
return cool_count;
}else if(flag==3){
pthread_mutex_lock(&tankshot_detection_i2c_mutex);
cool_count=0;
pthread_mutex_unlock(&tankshot_detection_i2c_mutex);
}
}
static void tankshot_detectionback(){
if(digitalRead(12)==LOW) {
s_back_status=true;
softPwmWrite(7, 0);
my_zlog_info("检测引脚12拉低");
}
if(s_back_status==true) tank_delay_shot_back();
}
//多线程处理坦克发射后退线程池
void tank_shot_back_stop_task_function(void *arg) {
......@@ -64,31 +128,23 @@ void tank_shot_back_stop_task_function(void *arg) {
while(1){
//下面一部分为检测射击后后退
// if(digitalRead(27)==LOW){
// if(g_device_delay_back_count <= g_tank_common_config_t->back_time) {
// tank_shot_back(g_tank_common_config_t->shot_back_speed);
// my_zlog_debug("操作耗时: %lld 毫秒", interval);
// }
// }else tank_shot_back(0);
if(g_device_delay_back_count <= g_tank_common_config_t->back_time) {
tank_shot_back(g_tank_common_config_t->shot_back_speed);
my_zlog_debug("击打成功");
}
else if(g_device_delay_back_count > g_tank_common_config_t->back_time&&g_device_delay_back_count < (g_tank_common_config_t->back_time+30)){
tank_shot_back(0);
}
else if(g_device_delay_back_count>g_tank_common_config_t->back_interval_back){
g_tank_shot_index = 1;
}else{
delay_ms(1);
switch(g_device_type){
case DEVICE_TANK0202:
case DEVICE_TANK0203:
tank_delay_shot_back();
break;
case DEVICE_TANK0204:
tankshot_detectionback();
tankshot_detection_backcount(0,1);
break;
default:
break;
}
delay_ms(10);
delay_us(500);
}
free(arg);
}
ThreadPool_t *pool_tank_t;//坦克后退线程函数
......@@ -99,7 +155,7 @@ void tank_shot_pthrpoll_task_init(){
*arg = 1;
pool_tank_t=thread_pool_init(1,1);
thread_pool_add_task(pool_tank_t, tank_shot_back_stop_task_function, arg);
my_zlog_debug("线程池打开");
my_zlog_info("线程池打开");
}
/*坦克后退射击*/
......@@ -116,17 +172,22 @@ int tank_shot_back_stop(unsigned char pin,unsigned char val){
if(val != 0) {
if(g_tank_shot_index == 0){
tank_shot_pthrpoll_task_init();
g_tank_shot_index=1;
}
if(g_tank_shot_index == 1 && g_tank_shot_index_cool==0){
if(g_tank_shot_index == 1 && s_tank_shot_index_cool==0){
g_device_delay_back_count =0;
g_tank_shot_index = 2;
g_tank_shot_index_cool=1;
s_tank_shot_index_cool=1;
}
}
}
void set_tank_shot_index_cool(bool index){
s_tank_shot_index_cool=index;
}
/*
* @brief 销毁坦克使用的线程池，让其正常销毁，只有在tank相关设备号下才有用，最后销毁都会到device——common.h中
*/
......
drivers/devicecontrol/tank_common.h
View file @ 5c540ef9
......@@ -11,12 +11,27 @@ typedef struct {
int(*shot_back)(unsigned char pin,unsigned char val);
} tank_common_back;
typedef enum {
shot,
shot_stop
}shot_state;
/*坦克射击接口，只有在特定设备号下使用*/
void tank_shot_stop_control(int device_id,unsigned char pin,unsigned char val);
/*设置后退的时间重置s*/
void set_tank_shot_index_cool(bool index);
/*用来判断有射击冷却的检测的状态获取*/
bool getshot_detection_index();
/*用来设置有射击冷却的检测的状态获取*/
bool setshot_detection_index(bool flase);
/*检测的冷却时间，flag=0时初始化冷却时间，2是计时，3是返回计时值*/
int tankshot_detection_backcount(int *cool_time,int flag);
/*关闭线程*/
void tank_thread_close();
extern int g_tank_shot_index_cool;
#endif
\ No newline at end of file
drivers/gpio/device_init.c
View file @ 5c540ef9
......@@ -58,7 +58,7 @@ const deviceconfig_t device_configs[] = {
.device_name = "tank0202",
.gpio_pins = {6, 16, 20, 22,-1},/* 补充GPIO引脚 */
.gpio_pwms = {5 , 7 ,24,26, 27,-1},
.gpio_inputs={-1},
.gpio_inputs={12,-1},
.device_pwm_init = physics_pwm_init,
.device_control_stop = tank0202_middle,/* 补充速度控制函数 */
.emergency_code = 202
......@@ -68,7 +68,7 @@ const deviceconfig_t device_configs[] = {
.device_name = "tank0203",
.gpio_pins = {6, 16, 20, 22,-1},/* 补充GPIO引脚 */
.gpio_pwms = {5 , 7 ,24, 26, 27,-1},
.gpio_inputs={-1},
.gpio_inputs={12,-1},
.device_pwm_init = physics_pwm_init,
.device_control_stop = tank0203_middle,/* 补充速度控制函数 */
.emergency_code = 203
......@@ -78,7 +78,7 @@ const deviceconfig_t device_configs[] = {
.device_name = "tank0204",
.gpio_pins = {6, 16, 20, 22,-1},/* 补充GPIO引脚 */
.gpio_pwms = {5 , 7 ,24, 26, 27,-1},
.gpio_inputs={-1},
.gpio_inputs={12,-1},
.device_pwm_init = physics_pwm_init,
.device_control_stop = tank0204_stop,/* 补充速度控制函数 */
.emergency_code = 204
......
drivers/gpio/gpio_control.c
View file @ 5c540ef9
......@@ -42,6 +42,13 @@ void device_shot_fire_init(TankFireControl* this) {
this->state = TANK_STATE_READY;
this->shot_interval_ms = 1000;
this->shot_duration_ms = 1000;
}if(g_device_type==DEVICE_TANK0204){
this->last_shot_end_time = 0;
this->shooting_start_time = 0;
this->state = TANK_STATE_READY;
this->shot_interval_ms = 5000;
this->shot_duration_ms = 1500;
tankshot_detection_backcount(&this->shot_interval_ms,0);
}else{
this->last_shot_end_time = 0;
this->shooting_start_time = 0;
......@@ -49,6 +56,7 @@ void device_shot_fire_init(TankFireControl* this) {
this->shot_interval_ms = 5000;
this->shot_duration_ms = 1000;
}
}
......@@ -124,7 +132,8 @@ int device_fire_check(TankFireControl* this) {
this->last_shot_end_time = current_time;
this->state = TANK_STATE_COOLDOWN;
g_tank_shot_index_cool = 0;
set_tank_shot_index_cool(0);//当为0坦克才能进入发炮完后退时间重置
return -1; // 射击刚刚结束
}
......@@ -538,12 +547,19 @@ void tank0204_pwm_value(int pin,int value){
if(value==1) {
if(pin == 27){
device_shoting_check(27,30);
//device_shoting_check(27,100);
if(getshot_detection_index()==true){
softPwmWrite(7, 100);
tankshot_detection_backcount(0,3);
getshot_detection_index(false);
my_zlog_info("冷却完成，射击开始");
}
set_tank_shot_index_cool(0);
}else if(pin == 5) {
softPwmWrite(7, 25);
softPwmWrite(7, 35);
my_zlog_info("pwm:7,1");
}else if(pin == 7) {
softPwmWrite(5, 25);
softPwmWrite(5, 35);
my_zlog_info("pwm:5,1");
}else {
softPwmWrite(pin, 60);
......@@ -568,7 +584,7 @@ void tank0204_pwm_value(int pin,int value){
softPwmWrite(5, 0);
my_zlog_info("pwm:5,0",pin);
}else{
softPwmWrite(pin, 0);
if(pin!=27) softPwmWrite(pin, 0);
my_zlog_info("pwm:%d,0",pin);
}
}
......
drivers/gpio/hardware_iic.c
View file @ 5c540ef9
......@@ -5,13 +5,15 @@
#include <sys/ioctl.h>
#include <linux/i2c-dev.h>
#include <stdint.h>
#include <pthread.h>
#define I2C_DEVICE "/dev/i2c-2" // 示例：使用 I2C 总线 2
static int fd = 0;
static uint8_t current_iic_addr = 0; // 记录当前设置的 I2C 地址
static pthread_mutex_t i2c_mutex = PTHREAD_MUTEX_INITIALIZER; // I2C 操作互斥锁
int hardware_iic_config_init();
int hardware_iic_init() {
......@@ -54,8 +56,14 @@ int set_hardware_iic(uint8_t iic_addr) {
// data: 要写入的数据
// len: 数据长度 (不包括寄存器地址)
int hardware_iic_write(uint8_t device_addr, uint8_t reg_addr, const uint8_t *data, int len) {
// 加锁，确保同一时间只有一个线程访问 I2C
pthread_mutex_lock(&i2c_mutex);
int ret = 1; // 默认失败
do{
if (set_hardware_iic(device_addr) != 0) {
return 1;
break;
}
// 构建发送缓冲区：第一个字节是寄存器地址，后面是数据
......@@ -67,10 +75,15 @@ int hardware_iic_write(uint8_t device_addr, uint8_t reg_addr, const uint8_t *dat
if (write(fd, write_buf, len + 1) != (len + 1)) {
my_zlog_warn("错误：向设备 0x%02X 寄存器 0x%02X 写入数据失败。", device_addr, reg_addr);
return 1;
break;
}
pthread_mutex_unlock(&i2c_mutex);
ret = 0; // 成功
}while(0);
// my_zlog_info("成功向设备 0x%02X 寄存器 0x%02X 写入 %d 字节数据。", device_addr, reg_addr, len);
return 0;
return ret;
}
// 硬件 I2C 读取函数
......@@ -79,23 +92,33 @@ int hardware_iic_write(uint8_t device_addr, uint8_t reg_addr, const uint8_t *dat
// data: 存储读取数据的缓冲区
// len: 要读取的数据长度
int hardware_iic_read(uint8_t device_addr, uint8_t reg_addr, uint8_t *data, int len) {
int ret=1;
pthread_mutex_lock(&i2c_mutex);
do{
if (set_hardware_iic(device_addr) != 0) {
return 1;
break;
}
// 先写入要读取的寄存器地址
if (write(fd, &reg_addr, 1) != 1) {
my_zlog_warn("错误：向设备 0x%02X 设置读取寄存器 0x%02X 失败。", device_addr, reg_addr);
return 1;
break;
}
// 然后从该寄存器读取数据
if (read(fd, data, len) != len) {
my_zlog_warn("错误：从设备 0x%02X 寄存器 0x%02X 读取 %d 字节数据失败。", device_addr, reg_addr, len);
return 1;
break;
}
ret = 0;
}while(0);
pthread_mutex_unlock(&i2c_mutex);
// my_zlog_info("成功从设备 0x%02X 寄存器 0x%02X 读取 %d 字节数据。", device_addr, reg_addr, len);
return 0;
return ret;
}
int hardware_iic_close() {
......
drivers/sensors/tank_angle.c
View file @ 5c540ef9
......@@ -2,8 +2,6 @@
#include "common.h"
#include "ads1115.h"
double tank_angle(){
double angle=0;
float angle_shot=ads1115_read_channel(2);
......
modules/mqtt/mqtt_infor_handle.c
View file @ 5c540ef9
......@@ -104,19 +104,19 @@ void angle_mqtt_send() {
void mqtt_beat_wirte(){
if( g_heartbeat_count >= 30) {
heartbeat_send();
g_heartbeat_count=0;
heartbeat_send();
g_heartbeat_count=0;
}
if(get_tank_angle_count()>2){
switch (g_device_type) {
case DEVICE_TANK0202:
case DEVICE_TANK0203:
case DEVICE_TANK0204:
angle_mqtt_send();
break;
default:
break;
case DEVICE_TANK0202:
case DEVICE_TANK0203:
case DEVICE_TANK0204:
angle_mqtt_send();
break;
default:
break;
}
set_tank_angle_count_clear();
}
......
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