/* * @Descripttion: * @version: * @Author: Joe * @Date: 2021-11-30 10:59:13 * @LastEditors: Joe * @LastEditTime: 2021-11-30 17:49:20 */ #include "tfmini.h" #include "env.h" #define DBG_TAG "tfmini" #define DBG_LVL DBG_INFO #include // 操作S.ObsStatus /* 设备名称 */ #define DEV_NAME "can2" #define rx_size 50 #define CAN2_RX_THREAD_PRIORITY 6 #define TFMINI_RORWARD_ID 0X11 #define TFMINI_RIGHT_ID 0x12 #define TFMINI_BACK_ID 0x13 #define TFMINI_LEFT_ID 0x14 /* 定义设备控制块 */ static rt_device_t dev; /* CAN 设备句柄 */ static rt_thread_t can2_rx_thread = RT_NULL; //解析 static rt_sem_t sem = RT_NULL; /*CAN相关*/ typedef struct { rt_uint16_t rxcnt; //接收数 rt_uint16_t delcnt; //处理数 }RXDATA_TypeDef; static RXDATA_TypeDef rx = {0}; static struct rt_can_msg rx_msg[rx_size]; static TFMINI_TypeDef tfmini_for; static TFMINI_TypeDef tfmini_back; static TFMINI_TypeDef tfmini_left; static TFMINI_TypeDef tfmini_right; static uint16_t flag = 0; TFMINI_TypeDef get_tfmini_for(void) { return tfmini_for; } TFMINI_TypeDef get_tfmini_back(void) { return tfmini_back; } TFMINI_TypeDef get_tfmini_left(void) { return tfmini_left; } TFMINI_TypeDef get_tfmini_right(void) { return tfmini_right; } void dist_parse(TFMINI_TypeDef *tfmini,struct rt_can_msg msg) { tfmini->dist = (msg.data[1]<<8) + msg.data[0]; /* 距离 */ tfmini->strength = (msg.data[3]<<8) + msg.data[2]; /* 信号 */ if((tfmini->strength >100) && (tfmini->strength <65533)) //正常,不正常的值不取 { if((tfmini->dist < Cfg.reduce_dist) && (tfmini->dist > Cfg.stop_dist+5)) { tfmini->reduce_cnt++; if(tfmini->reduce_cnt > 3) { /* 减速 */ S.ObsStatus = OBS_STATUS_FAR; } } else { tfmini->reduce_cnt = 0; if(tfmini->dist < Cfg.stop_dist) { tfmini->stop_cnt++; if(tfmini->stop_cnt > 3) { /* 停止 */ S.ObsStatus = OBS_STATUS_NEAR; } } else { tfmini->stop_cnt = 0; } } } } uint8_t tfmini_parse(struct rt_can_msg msg) { uint8_t temp = 1; if(msg.ide!=RT_CAN_STDID) return temp; switch (msg.id) { case TFMINI_RORWARD_ID : /* 前 */ dist_parse(&tfmini_for,msg); break; case TFMINI_RIGHT_ID: /* 右 */ dist_parse(&tfmini_right,msg); break; case TFMINI_BACK_ID: /* 后 */ dist_parse(&tfmini_back,msg); break; case TFMINI_LEFT_ID: /* 左 */ dist_parse(&tfmini_left,msg); break; default: break; } //数据解析 return temp; } /* 接收数据回调函数 */ static rt_err_t rx_callback(rt_device_t dev, rt_size_t size) { /* 从 CAN 读取一帧数据 */ rt_device_read(dev, 0, &rx_msg[rx.rxcnt], sizeof(rx_msg[rx.rxcnt])); rx.rxcnt++; if(rx.rxcnt >= rx_size) { rx.rxcnt = 0; } /* CAN 接收到数据后产生中断,调用此回调函数,然后发送接收信号量 */ rt_sem_release(sem); return RT_EOK; } /* 线程入口 */ static void can2_rx_thread_entry(void* parameter) { while(1) { rt_sem_take(sem,RT_WAITING_FOREVER); if(rx.delcnt!=rx.rxcnt) //有新数据 { tfmini_parse(rx_msg[rx.delcnt]); //协议解析 rx.delcnt++; //下一条 if(rx.delcnt>=rx_size) { rx.delcnt = 0; } } } } static void tfminix_param_init(TFMINI_TypeDef *tfmini) { tfmini->dist = 0; tfmini->stop_cnt = 0; tfmini->reduce_cnt = 0; tfmini->strength = 0; } static void tfmini_param_init(void) { tfminix_param_init(&tfmini_for); tfminix_param_init(&tfmini_right); tfminix_param_init(&tfmini_back); tfminix_param_init(&tfmini_left); } /**************************************** * can_config *函数功能 : 配置初始化 *参数描述 : 无 *返回值 : 无 ****************************************/ static void can2_config(void) { /* step1:查找CAN设备 */ dev = rt_device_find(DEV_NAME); //查找CAN口设备 if (dev) { LOG_I("find %s OK", DEV_NAME); } else { LOG_E("find %s failed!", DEV_NAME); } /* step2:打开CAN口设备。以中断接收及发送模式打开CAN设备 */ rt_device_open(dev, RT_DEVICE_FLAG_INT_TX | RT_DEVICE_FLAG_INT_RX); /*step3:设置 CAN 通信的波特率为 500kbit/s*/ rt_device_control(dev, RT_CAN_CMD_SET_BAUD, (void *)CAN500kBaud); /* step4:设置接收回调函数 */ rt_device_set_rx_indicate(dev, rx_callback); /* step5:设置硬件过滤表 */ #ifdef RT_CAN_USING_HDR struct rt_can_filter_item items[5] = { RT_CAN_FILTER_ITEM_INIT(0x100, 0, 0, 1, 0x700, RT_NULL, RT_NULL), /* std,match ID:0x100~0x1ff,hdr 为 - 1,设置默认过滤表 */ RT_CAN_FILTER_ITEM_INIT(0x300, 0, 0, 1, 0x700, RT_NULL, RT_NULL), /* std,match ID:0x300~0x3ff,hdr 为 - 1 */ RT_CAN_FILTER_ITEM_INIT(0x211, 0, 0, 1, 0x7ff, RT_NULL, RT_NULL), /* std,match ID:0x211,hdr 为 - 1 */ RT_CAN_FILTER_STD_INIT(0x486, RT_NULL, RT_NULL), /* std,match ID:0x486,hdr 为 - 1 */ {0x555, 0, 0, 1, 0x7ff, 7,} /* std,match ID:0x555,hdr 为 7,指定设置 7 号过滤表 */ }; struct rt_can_filter_config cfg = {5, 1, items}; /* 一共有 5 个过滤表 */ /* 设置硬件过滤表 */ rt_device_control(can_dev, RT_CAN_CMD_SET_FILTER, &cfg); #endif } /**************************************** * tfmini_init *函数功能 : 配置初始化 *参数描述 : 无 *返回值 : 无 ****************************************/ int tfmini_init(void) { tfmini_param_init(); can2_config();//配置初始化 //创建信号量 sem = rt_sem_create("sem",/* 计数信号量名字 */ 0, /* 信号量初始值,默认有一个信号量 */ RT_IPC_FLAG_FIFO); /* 信号量模式 FIFO(0x00)*/ can2_rx_thread = /* 线程控制块指针 */ rt_thread_create( "can2_rx_thread", /* 线程名字 */ can2_rx_thread_entry, /* 线程入口函数 */ RT_NULL, /* 线程入口函数参数 */ 2048, /* 线程栈大小 */ CAN2_RX_THREAD_PRIORITY, /* 线程的优先级 */ 20); /* 线程时间片 */ /* 启动线程,开启调度 */ if (can2_rx_thread != RT_NULL) { rt_thread_startup(can2_rx_thread); LOG_I("can2_rx_thread create."); } return RT_EOK; } INIT_APP_EXPORT(tfmini_init);