rgv-kinco/applications/tfmini.c

/*
 * @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 <rtdbg.h>

// 操作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);