librod2.c

rod4 扫描仪协议分析代码

#include <sys/types.h>
#include <sys/socket.h>
#include <string.h>
#include <stdlib.h>
#include <unistd.h>
#include <math.h>
#include "librod2.h"
#include "type.h"
#include "debug.h"
#include "libsk.h"



int librod2_init(char *dev)
{
	int sk/*, i*/;
	sk = libsk_connect_to(dev, 9008);
	if(sk < 0)
	{
		DEBUG("sick init error");
		return -1;
	}
	libsk_set_noblock(sk);
	return sk;
}

int librod2_cmd_send(int fd, unsigned char cmd , char *buf , unsigned short size)
{
	DEBUG("not complite");

	return -1;
}

static int data_para(char *buf, struct _data_type *data)
{
	float angle;
	int i = 0, data_num = 0, start, end;
	
	memset(data, 0x00, sizeof(struct _data_type) * 1024);
	
	i = 14;
	angle = buf[i]*0.36;

	
	i++;
	if(buf[i] == 0x00 && buf[i+1] == 0x00 && buf[i+2] == 0xff)
	{
		start = 0;
		i += 3;
	}
	else
	{
		short *tmp = (short *)&buf[i];
		start = *tmp;
		i+=2;
	}
	
	if(buf[i] == 0x00 && buf[i+1] == 0x00 && buf[i+2] == 0xff)
	{
		DEBUG("error end");
		end = 0;
		i += 3;
	}
	else
	{
		short *tmp = (short *)&buf[i];
		end = *tmp;
		i+=2;
	}

	while(1)
	{
		if(start > end)
		{
			break;
		}
		
		if(buf[i] == 0x00 && buf[i+1] == 0x00 && buf[i+2] == 0xff)
		{
			data[data_num].x = 1;
			data[data_num].y = 1;
			i += 3;
		}
		else if(buf[i] == 0x00 && buf[i+1] == 0x00 && buf[i + 2] == 0x00)
		{
			break;
		}
		else 
		{
			unsigned short tmp = *(unsigned short *)&buf[i];
		
			if(buf[i - 1] == 0x00 && buf[i] == 0x00 && buf[i+1] == 0xff)
			{
				tmp = buf[i+2];
				i+=3;
			}
			else
			{
				i += 2;
			}
			
			tmp = tmp >> 1;
			tmp = tmp << 1;
			/*printf("no.%d %x %x tmp %d\n", data_num, buf[i], buf[i+1] ,tmp);	*/
			data[data_num].x = tmp * cos(M_PI*((-5.04)+ angle*start)*2/360);
			data[data_num].y = tmp * sin(M_PI*((-5.04)+ angle*start)*2/360);
		}
		
		data_num++;
		start+=buf[14];
	}
	
        /*for(i = 0; i < 1024 && (data[i].x != 0 || data[i].y != 0); i++)*/
	/*{*/
	/*dprintf("No.%d x=%d \t y=%d\n", i, data[i].x , data[i].y);*/
	/*}*/
	/*printf("data_num %d \n", data_num);	*/
	return data_num * sizeof(struct _data_type);
}

#define BUF_SIZE 8192

int librod2_get_data(int fd, void *buf)
{
	static char data[BUF_SIZE];
	static int offset = 0;
	int n, i, ret = 0;

	//n = read(fd, &(data[offset]),  BUF_SIZE- offset);
	//n = recv(fd, &(data[offset]), 1081, MSG_WAITALL);
	n = read(fd, &(data[offset]),  BUF_SIZE- offset);
	if(n < 0)
	{
		DEBUG("read error");
		return -1;
	}
	if(n == 0)
	{
		DEBUG("get 0 bufsize %d\n", BUF_SIZE- offset);
		return 0;
	}



	offset += n;

	if(offset < 21)
		return 0;

	if(data[0] != 0x00 && data[1] != 0x00)
	{
		int i;
		DEBUG("data error ,do search now");
		for(i = 0; i < offset; i++)
		{
			if(data[i] == 0x00 && data[i+1] == 0x00 && (~(data[i+2] ==0x00 || data[i+2] == 0xff)))
			{
				memmove(data, &data[i], BUF_SIZE - i);
				offset -= i;
			}
		}
	}

	if(offset < 21)
		return 0;
	
	if(data[0] == 0x00 && data[1] == 0x00 && (~(data[2] ==0x00 || data[2] == 0xff)))
	{
		char check_byte = data[0];

		for(i = 1; i < offset - 4; i++)
		{
			check_byte ^= data[i];
			if( (!data[i+2]) && (!data[i+3]) && (!data[i + 4]))
			{
				if(check_byte == data[i+1])
				{
				//	DEBUG("check xor succeed");
				}
				else
				{
					DEBUG("check xor failed");
				}
				
				ret = data_para(data, (struct _data_type *)buf);
				memmove(data, &data[i+5], BUF_SIZE - i - 5);
				offset -= (i + 5);
				break;
			}
		}
	}

	return ret;
}

int librod2_parse_cmd(int fd, void *buf, int *size)
{
	int ret;
	ret = librod2_get_data(fd, buf);

	if(ret > 0)
	{
		*size = ret;
		return LASER_DATA_READY;
	}
	return 0;
}

void librod2_close(int fd)
{
	libsk_close(fd);
}