master

分支 (1)

管理

管理

master

linuxcan
/
can_listener.c

#include "can_listener.h"

/*****************************************************************/
void f_msg_3E9(uint8_t* pbuf);
void f_msg_0C9(uint8_t* pbuf);
void f_msg_120(uint8_t* pbuf);
void f_msg_4D1(uint8_t* pbuf);
void f_msg_4C1(uint8_t* pbuf);
void f_msg_1F1(uint8_t* pbuf);
void f_msg_1F5(uint8_t* pbuf);
void f_msg_140(uint8_t* pbuf);
void f_msg_12A(uint8_t* pbuf);
void f_msg_135(uint8_t* pbuf);

struct msgitem can0_msgs[] =
{
    {0,0, 0, 0x3E9,	0x7FF,	100*1.2,	&f_msg_3E9,    },
    {0,0, 0, 0x0C9,	0x7FF,   10*1.2,	&f_msg_0C9,    },
    {0,0, 0, 0x120,	0x7FF, 5000*1.2,	&f_msg_120,    },
    {0,0, 0, 0x4D1,	0x7FF,  500*1.2,	&f_msg_4D1,    },
    {0,0, 0, 0x4C1,	0x7FF,  500*1.2,	&f_msg_4C1,    },
    {0,0, 0, 0x1F1,	0x7FF,  100*1.2,	&f_msg_1F1,    },
    {0,0, 0, 0x1F5,	0x7FF,   25*1.2,	&f_msg_1F5,    },
    {0,0, 0, 0x140,	0x7FF, 1000*1.2,	&f_msg_140,    },
    {0,0, 0, 0x12A,	0x7FF,  100*1.2,	&f_msg_12A,    },
    {0,0, 0, 0x135,	0x7FF,  100*1.2,	&f_msg_135,    },
};

int size_can0 = sizeof(can0_msgs)/sizeof(can0_msgs[0]);

struct VehicleBasicInfo* pCarData= NULL;
int can_sem_id = -1;
extern int can_msg_id;
/***********************************************************************************************/
uint64_t gettimestamp(void)
{
	uint64_t ret;
	struct timeval tv;

	ret = 0;
	memset(&tv,0,sizeof(struct timeval));
	if(0 == gettimeofday(&tv,NULL))
	{
		ret = ((uint64_t)(tv.tv_sec))*1000 + ((uint64_t)(tv.tv_usec))/1000;
	}

	return ret;
}
uint8_t can_data_init(void)
{
	uint8_t ret;

	ret = app_sem_init(CAN_SEMKEY_PATH,CAN_SEMKEY_ID);
	if(ret == RET_OK)
	{
		ret = app_sem_key(&can_sem_id,CAN_SEMKEY_PATH,CAN_SEMKEY_ID);
	}
	if(ret == RET_OK)
	{
		ret = app_shm_init(sizeof(struct VehicleBasicInfo),(void**)(&pCarData),CAN_SHMKEY_PATH,CAN_SHMKEY_ID);
	}
	if(ret == RET_OK)
	{
		ret = app_sem_lock(can_sem_id);
	}
	if(ret == RET_OK)
	{
		pCarData->type = 0;
		pCarData->width = 0.0;
		pCarData->length = 0.0;
		pCarData->velocity = 589.725;
		pCarData->angle = 360.000;
		pCarData->rotationspeed = 0;
		pCarData->gaspedalpos = 0;
		pCarData->brakestate = 0;
		pCarData->totalmileage = 0;
		pCarData->leftoil = 0;
		pCarData->coolanttemp = 0;
		pCarData->keystate = 0;
		pCarData->handbrakestate = 0;
		pCarData->gear = 0;
		pCarData->lightstate = 0;
		pCarData->doorstate = 0;
		pCarData->safetybelt = 0;
		pCarData->time_ms = 0;
		app_sem_unlock(can_sem_id);
	}

	return ret;
}
uint8_t can_data_get(struct VehicleBasicInfo* pdata)
{
	uint8_t ret;

	ret = app_sem_lock(can_sem_id);
	if(ret == RET_OK)
	{
		if((pdata != NULL)&&(pCarData != NULL))
		{
			memcpy(pdata,pCarData,sizeof(struct VehicleBasicInfo));
		}
		else
		{
			ret = RET_ERROR;
		}
	}

	ret = app_sem_unlock(can_sem_id);

	return ret;
}
uint8_t can_data_deinit()
{
	uint8_t ret;
	int can_shm_id = -1;

	ret = RET_OK;
	if(can_sem_id == -1)
	{
		ret = app_sem_key(&can_sem_id,CAN_SEMKEY_PATH,CAN_SEMKEY_ID);
	}
	if(ret == RET_OK)
	{
		ret = app_sem_deinit(can_sem_id);
	}
	if(can_sem_id == -1)
	{
		ret = app_shm_key(&can_shm_id,(void**)(&pCarData),CAN_SHMKEY_PATH,CAN_SHMKEY_ID);
	}
	if(ret == RET_OK)
	{
		ret = app_shm_deinit(can_shm_id,(void*)pCarData);
	}
	return ret;
}
void f_msg_3E9(uint8_t* pbuf)
{
	uint8_t ret;

	ret = app_sem_lock(can_sem_id);
	if(ret == RET_OK)
	{
		if(pbuf != NULL)
		{
			pCarData->velocity = (pbuf[0]*0x100+pbuf[1])*0.017;
			can_log(CAN_DEBUG_INFO,("velocity[%.03f]\r\n",pCarData->velocity));
			pCarData->time_ms = gettimestamp();
		}
		else
		{
			pCarData->velocity = 589.725;
		}

		app_sem_unlock(can_sem_id);
	}

}
void f_msg_0C9(uint8_t* pbuf)
{
	static uint8_t brake_state = 0;
	static uint8_t report_state = 1;  // 1: can report  0: can not report
	uint8_t ret;
	char msg[MSGBUF_SIZE];

	ret = app_sem_lock(can_sem_id);
	if(ret == RET_OK)
	{
		if(pbuf != NULL)
		{
			pCarData->rotationspeed = (pbuf[1]*0x100+pbuf[2])*0.25;
			can_log(CAN_DEBUG_INFO,("rotationspeed[%d]\r\n",pCarData->rotationspeed));

			pCarData->gaspedalpos = (pbuf[4])*100/255.0;
			can_log(CAN_DEBUG_INFO,("gaspedalpos[%d]\r\n",pCarData->gaspedalpos));

			pCarData->brakestate = ((pbuf[5]>>0)&0X01);
			can_log(CAN_DEBUG_INFO,("brakestate[%d]\r\n",pCarData->brakestate));
			pCarData->time_ms = gettimestamp();
		}
		else
		{
			pCarData->rotationspeed = 0;
			pCarData->gaspedalpos = 0;
			pCarData->brakestate = 0;
		}
		if(brake_state != pCarData->brakestate)
		{
			brake_state = pCarData->brakestate;
			if(report_state == 1)
			{
				if(brake_state == 0)
				{
					ret = app_msg_snd(can_msg_id,MSGTYPE_REPORT,STR_UNCHECK,strlen(STR_UNCHECK));
				}
				else
				{
					ret = app_msg_snd(can_msg_id,MSGTYPE_REPORT,STR_CHECK,strlen(STR_CHECK));
				}
				if(ret != RET_OK)
				{
					return;
				}
			}
			memset(msg,0,sizeof(msg));
			ret = app_msg_rcv(can_msg_id,MSGTYPE_REPORTACK,msg,sizeof(msg),9);
			if(ret == RET_OK)
			{
				report_state = 1;
			}
			else
			{
				report_state = 0;
			}
		}
		app_sem_unlock(can_sem_id);
	}
}
void f_msg_120(uint8_t* pbuf)
{
	uint8_t ret;

	ret = app_sem_lock(can_sem_id);
	if(ret == RET_OK)
	{
		if(pbuf != NULL)
		{
			pCarData->totalmileage = (pbuf[0]*0x1000000+pbuf[1]*0x10000+pbuf[2]*0x100+pbuf[3])*0.015625;
			can_log(CAN_DEBUG_INFO,("totalmileage[%d]\r\n",pCarData->totalmileage));
			pCarData->time_ms = gettimestamp();
		}
		else
		{
			pCarData->totalmileage = 0;
		}
		app_sem_unlock(can_sem_id);
	}
}
void f_msg_4D1(uint8_t* pbuf)
{
	uint8_t ret;

	ret = app_sem_lock(can_sem_id);
	if(ret == RET_OK)
	{
		if(pbuf != NULL)
		{
			pCarData->leftoil = (pbuf[5])*100/255.0;
			can_log(CAN_DEBUG_INFO,("leftoil[%d]\r\n",pCarData->leftoil));
			pCarData->time_ms = gettimestamp();
		}
		else
		{
			pCarData->leftoil = 0;
		}
		app_sem_unlock(can_sem_id);
	}
}
void f_msg_4C1(uint8_t* pbuf)
{
	uint8_t ret;

	ret = app_sem_lock(can_sem_id);
	if(ret == RET_OK)
	{
		if(pbuf != NULL)
		{
			pCarData->coolanttemp = pbuf[2]-40;
			can_log(CAN_DEBUG_INFO,("coolanttemp[%d]\r\n",pCarData->coolanttemp));
			pCarData->time_ms = gettimestamp();
		}
		else
		{
			pCarData->coolanttemp = 0;
		}
		app_sem_unlock(can_sem_id);
	}
}
void f_msg_1F1(uint8_t* pbuf)
{
	uint8_t ret;

	ret = app_sem_lock(can_sem_id);
	if(ret == RET_OK)
	{
		if(pbuf != NULL)
		{
			if(pbuf[0] == 0x80)
			{
				pCarData->keystate = 1; //lock
			}
			else if(pbuf[0] == 0x85)
			{
				pCarData->keystate = 2;//acc
			}
			else if(pbuf[0] == 0xAE)
			{
				pCarData->keystate = 3;//on
			}
			else
			{
				pCarData->keystate = 0;//unknown
			}
			can_log(CAN_DEBUG_INFO,("keystate[%d]\r\n",pCarData->keystate));
			pCarData->handbrakestate = (pbuf[4]>>4)&0x01;
			can_log(CAN_DEBUG_INFO,("handbrakestate[%d]\r\n",pCarData->handbrakestate));
			pCarData->time_ms = gettimestamp();
		}
		else
		{
			pCarData->keystate = 0;
			pCarData->handbrakestate = 0;
		}
		app_sem_unlock(can_sem_id);
	}
}
void f_msg_1F5(uint8_t* pbuf)
{
	uint8_t ret;

	ret = app_sem_lock(can_sem_id);
	if(ret == RET_OK)
	{
		if(pbuf != NULL)
		{
			if(pbuf[3]==0x01)
			{
				pCarData->gear = 1;//P
			}
			else if(pbuf[3]==0x02)
			{
				pCarData->gear =2;//R
			}
			else if(pbuf[3]==0x03)
			{
				pCarData->gear = 3;//N
			}
			else if(pbuf[3]==0x04)
			{
				pCarData->gear = 4;//D
			}
			else
			{
				pCarData->gear = 0;//unknown
			}
			can_log(CAN_DEBUG_INFO,("gear[%d]\r\n",pCarData->gear));
			pCarData->time_ms = gettimestamp();
		}
		else
		{
			pCarData->gear = 0;
		}
		app_sem_unlock(can_sem_id);
	}
}
void f_msg_140(uint8_t* pbuf)
{
	uint8_t ret;

	ret = app_sem_lock(can_sem_id);
	if(ret == RET_OK)
	{
		if(pbuf != NULL)
		{
			if((pbuf[0]>>1)&0x01)
			{
				pCarData->lightstate |= 0x01;//near light
			}else{
				pCarData->lightstate &= 0xFFFFFFFE;//near light
			}
			if((pbuf[0]>>7)&0x01)
			{
				pCarData->lightstate |= 0x02;//rear light
			}else{
				pCarData->lightstate &= 0xFFFFFFFD;//rear light
			}
			if((pbuf[2]>>2)&0x01)
			{
				pCarData->lightstate |= 0x04;//left light
			}else{
				pCarData->lightstate &= 0xFFFFFFFB;//left light
			}
			if((pbuf[2]>>3)&0x01)
			{
				pCarData->lightstate |= 0x08;//right light
			}else{
				pCarData->lightstate &= 0xFFFFFFF7;//right light
			}
			can_log(CAN_DEBUG_INFO,("lightstate[%08X]\r\n",pCarData->lightstate));
			pCarData->time_ms = gettimestamp();
		}
		else
		{
			pCarData->lightstate = 0;
		}
		app_sem_unlock(can_sem_id);
	}
}
void f_msg_12A(uint8_t* pbuf)
{
	uint8_t ret;

	ret = app_sem_lock(can_sem_id);
	if(ret == RET_OK)
	{
		if(pbuf != NULL)
		{
			if((pbuf[1]>>2)&0x01)
			{
				pCarData->doorstate |= 0x01;//left front door
			}else{
				pCarData->doorstate &= 0xFFFFFFFE;//left front door
			}
			if((pbuf[1]>>0)&0x01)
			{
				pCarData->doorstate |= 0x02;//left rear door
			}else{
				pCarData->doorstate &= 0xFFFFFFFD;//left rear door
			}
			if((pbuf[1]>>1)&0x01)
			{
				pCarData->doorstate |= 0x04;//right front door
			}else{
				pCarData->doorstate &= 0xFFFFFFFB;//right front door^M
			}
			if((pbuf[2]>>7)&0x01)
			{
				pCarData->doorstate |= 0x08;//right rear door
			}else{
				pCarData->doorstate &= 0xFFFFFFF7;//right rear door
			}
			if((pbuf[1]>>4)&0x01)
			{
				pCarData->safetybelt |= 0x01;//master safe belt
			}else{
				pCarData->safetybelt &= 0xFFFFFFFE;//master safe belt
			}
			if((pbuf[6]>>5)&0x01)
			{
				pCarData->safetybelt |= 0x02;//slave safe belt
			}else{
				pCarData->safetybelt &= 0xFFFFFFFD;//slave safe belt
			}
			can_log(CAN_DEBUG_INFO,("doorstate[%08X]\r\n",pCarData->doorstate));
			can_log(CAN_DEBUG_INFO,("safetybelt[%08X]\r\n",pCarData->safetybelt));
			pCarData->time_ms = gettimestamp();
		}
		else
		{
			pCarData->doorstate &= 0xFFFFFFF0;
			pCarData->safetybelt = 0;
		}
		app_sem_unlock(can_sem_id);
	}
}
void f_msg_135(uint8_t* pbuf)
{
	uint8_t ret;

	ret = app_sem_lock(can_sem_id);
	if(ret == RET_OK)
	{
		if(pbuf != NULL)
		{
			if((pbuf[1]>>4)&0x01)
			{
				pCarData->doorstate |= 0x10;//behind door
			}
			can_log(CAN_DEBUG_INFO,("doorstate[%08X]\r\n",pCarData->doorstate));
			pCarData->time_ms = gettimestamp();
		}
		else
		{
			pCarData->doorstate &= 0xFFFFFEFF;
		}
		app_sem_unlock(can_sem_id);
	}
}
void* can_lisener(void *arg)
{
	struct msgitem* item;
	int ret;
	int s;

	ret = 0;

	item = (struct msgitem*)arg;
	/* init socket */
	s = can_get_socket();
	if(s == -1)
	{
		can_log(CAN_DEBUG_ERROR,("socket create error\r\n"));
		ret = -1;
	}
	/* bind socket */
	if(ret == 0)
	{
		if(can_bind_socket(s,0) != 0)
		{
			can_log(CAN_DEBUG_ERROR,("socket bind error\r\n"));
			ret = -1;
		}
	}
	/* set can filter */
	if(ret == 0)
	{
		if(can_set_socketfilter(s,item->id,item->mask)!=0)
		{
			can_log(CAN_DEBUG_ERROR,("socket set filter error\r\n"));
			ret = -1;
		}
	}
	/* set read timeout */
	if(ret == 0)
	{
		if(can_set_readtimeout(s,item->timeout)!=0)
		{
			can_log(CAN_DEBUG_ERROR,("socket set read timeout error\r\n"));
			ret = -1;
		}
	}
	item->ctl = 1;
	item->state = 0;
	can_log(CAN_DEBUG_INFO,("can thread init %X\r\n",item->id));
	/* update data */
	while(ret == 0)
	{
		struct can_frame frame;
		ssize_t nbytes;

		memset((void*)(&frame),0,sizeof(struct can_frame));
		if(item->ctl == 0)
		{
			break;
		}
		nbytes = can_read_msg(s, &frame);
		if (nbytes == -1)
		{
			item->state = 0;
			if(item->func != NULL)
			{
				(*(item->func))(NULL);
			}
		}
		else
		{
			if(0)
			{
				int i;

				if((frame.can_id & CAN_EFF_FLAG) == CAN_EFF_FLAG )
				{
					printf("%08X|%02X|",frame.can_id&CAN_EFF_MASK,frame.can_dlc);
				}
				else
				{
					printf("%04X|%02X|",frame.can_id&CAN_SFF_MASK,frame.can_dlc);
				}
				for(i=0;i<frame.can_dlc;i++)
				{
					printf("%02X ",frame.data[i]);
				}
				printf("\r\n");
			}
			item->state = 1;
			if(item->func != NULL)
			{
				(*(item->func))(frame.data);
			}
		}

	}
	can_log(CAN_DEBUG_INFO,("can thread end %X\r\n",item->id));
	close(s);
	return ((void*)0);
}

uint8_t can0_check_standby(void)
{
	int i;
	uint8_t ret;

	ret = 1;
	for(i=0;i<(sizeof(can0_msgs)/sizeof(can0_msgs[0]));i++)
	{
		if(can0_msgs[i].state == 1)
		{
			ret = 0;
			break;
		}
	}
	return ret;
}
void can0_datalist(void)
{
	if(pCarData != NULL)
	{
		can_log(CAN_DEBUG_INFO,("time[%d]\r\n",pCarData->time_ms));
		can_log(CAN_DEBUG_INFO,("type[%d]\r\n",pCarData->type));
		can_log(CAN_DEBUG_INFO,("width[%.03f]\r\n",pCarData->width));
		can_log(CAN_DEBUG_INFO,("length[%.03f]\r\n",pCarData->length));
		can_log(CAN_DEBUG_INFO,("angle[%.03f]\r\n",pCarData->angle));
		can_log(CAN_DEBUG_INFO,("velocity[%.03f]\r\n",pCarData->velocity));
		can_log(CAN_DEBUG_INFO,("rotationspeed[%d]\r\n",pCarData->rotationspeed));
		can_log(CAN_DEBUG_INFO,("gaspedalpos[%d]\r\n",pCarData->gaspedalpos));
		can_log(CAN_DEBUG_INFO,("brakestate[%d]\r\n",pCarData->brakestate));
		can_log(CAN_DEBUG_INFO,("totalmileage[%d]\r\n",pCarData->totalmileage));
		can_log(CAN_DEBUG_INFO,("leftoil[%d]\r\n",pCarData->leftoil));
		can_log(CAN_DEBUG_INFO,("coolanttemp[%d]\r\n",pCarData->coolanttemp));
		can_log(CAN_DEBUG_INFO,("keystate[%d]\r\n",pCarData->keystate));
		can_log(CAN_DEBUG_INFO,("handbrakestate[%d]\r\n",pCarData->handbrakestate));
		can_log(CAN_DEBUG_INFO,("gear[%d]\r\n",pCarData->gear));
		can_log(CAN_DEBUG_INFO,("lightstate[%08X]\r\n",pCarData->lightstate));
		can_log(CAN_DEBUG_INFO,("doorstate[%08X]\r\n",pCarData->doorstate));
		can_log(CAN_DEBUG_INFO,("safetybelt[%08X]\r\n",pCarData->safetybelt));
	}
	else
	{
		can_log(CAN_DEBUG_ERROR,("the memory for data is not ready!!\r\n"));
	}
}
/****************************************************************************/