skyetekm1.cc

机器人仿真软件

		// Write data to the RFID tag (NACK for now)
		Publish (device_addr, resp_queue, PLAYER_MSGTYPE_RESP_NACK, 
			 hdr->subtype);
	}
	
	else if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_REQ, 
			 PLAYER_RFID_REQ_LOCKTAG, device_addr))
	{
		// Lock a RFID tag (NACK for now)
		Publish (device_addr, resp_queue, PLAYER_MSGTYPE_RESP_NACK, 
			 hdr->subtype);
	}
	
	else
	{
		return -1;
	}
	
	return 0;
}

////////////////////////////////////////////////////////////////////////////////
// Main function for device thread
void SkyetekM1::Main () 
{
	// Zero data
	memset (&this->Data, 0, sizeof (player_rfid_data_t));
	
	timespec sleepTime = {0, 0};
	
	// The main loop; interact with the device here
	while (true)
	{
		// test if we are supposed to cancel
		pthread_testcancel ();
	
		// Process any pending messages
		ProcessMessages();
		
		// Interact with the device, and push out the resulting data.
		this->RefreshData ();
		
		nanosleep (&sleepTime, NULL);
	}
}

////////////////////////////////////////////////////////////////////////////////
// RefreshData function
void SkyetekM1::RefreshData ()
{
	memset (&this->Data, 0, sizeof (player_rfid_data_t));
	
	// Get the time at which we started reading
	// This will be a pretty good estimate of when the phenomena occured
	struct timeval time;
	GlobalTime->GetTime(&time);
	
	// Anticollision mode
	SelectTags ();
	Wake ();

	// Write the RFID data
	Publish (device_addr, NULL, PLAYER_MSGTYPE_DATA, PLAYER_RFID_DATA, 
			 &Data, sizeof (player_rfid_data_t), NULL);
	
	return;
}

////////////////////////////////////////////////////////////////////////////////
// WriteSerial function
void SkyetekM1::WriteSerial (unsigned char *mdmData, unsigned char commandLen)
{
	int wresult;
	wresult = write (this->fd, mdmData, commandLen);
	if (wresult < 0)
		PLAYER_WARN1 (">> Error while writing %d bytes !", commandLen);
}	

////////////////////////////////////////////////////////////////////////////////
// ReadSerial function
int SkyetekM1::ReadSerial (unsigned char *mdmData, unsigned char commandLen)
{
	int bytes_read = read (this->fd, mdmData, commandLen);
	return bytes_read;
}

////////////////////////////////////////////////////////////////////////////////
// SelectTags function
void SkyetekM1::SelectTags ()
{
	unsigned char command_buf[80], response_buf[80];
	unsigned char i = 1;
	unsigned int crc_check;

	// REQUEST for SELECT_TAG: MSGLEN|FLAGS|0x14|rid|TAGTYPE|tid|AFI|CRC
	// ---[ Build the request ]---
	command_buf[i++] = 0x22;             // SEL_TAG_INV (CRC_F, INV_F enabled)
	command_buf[i++] = 0x14;             // SELECT_TAG (set command (0x14))
	// set reader ID - empty (no RID)
	command_buf[i++] = 0x00;             // AUTODETECT (0x00)

	
	command_buf[0] = i + 1;              // set message length
	
	crc_check = CRC16 (command_buf, i);
	command_buf[i++] = crc_check >> 8;   // set CRC
	command_buf[i++] = crc_check & 0x00FF;

	// copy the command buffer and prepare to write - OK
	int j;
	unsigned char temp[80];
	temp[0] = 0x02;                  // Start of Transmission (STX = 0x02)
	for (j = 0; j < i+1; j++)
		temp[j+1] = command_buf[j];	
	WriteSerial (temp, i + 1);
	
	// poll for reader response
	while (response_buf[2] != 0x94)
	{
		ReadSerial (response_buf, 3);
		int len = 0;
		if (response_buf[0] == 0x02)
			len = response_buf[1];
		
		unsigned char TID[len];
		memset (&TID, 0, sizeof (TID)); // clear the struct for new port settings
		usleep (10000); // sleep for 10ms
		ReadSerial (TID, len);
		if (response_buf[2] == 0x94)
			break;
		
		if (response_buf[0] == 0x02)
		{
			this->Data.tags[this->Data.tags_count].type       = TID[0];
			this->Data.tags[this->Data.tags_count].guid_count = 8;
			int j;
			for (j = 0; j < 9; j++)
				this->Data.tags[this->Data.tags_count].guid[j] = TID[j+1];
			this->Data.tags_count++;
		}
	}
}

////////////////////////////////////////////////////////////////////////////////
// S_ReadWrite function
int SkyetekM1::S_ReadWrite (unsigned char *mdmData, 
			unsigned char length, unsigned char *response)
{
	unsigned char temp[80], read_data[80];
	bool read_done = FALSE;
	int tmp_count = 0;
	int i;
	
	// copy the command buffer and prepare to write
	temp[0] = 0x02;                  // Start of Transmission (STX = 0x02)
	for (i = 0; i < length + 1; i++)
		temp[i+1] = mdmData[i];	
	
	WriteSerial (temp, length + 1);
	
	// sleep for 10ms
	usleep (10000);
	
	while (!read_done)
	{ 
		tmp_count++;
	
		ReadSerial (read_data, sizeof (read_data));
		
		int m = 0;

		while (m != sizeof (read_data))
		{
			response[m] = read_data[m];
			m++;
		}
				
		if (tmp_count > 10)
			read_done = TRUE;
		
		if ((read_data[0] = 0x02) && (read_data[1] <= 0x50))
			read_done = TRUE;
		
		usleep (10000);               // Sleep for 10ms
	}

	return 0;
}


////////////////////////////////////////////////////////////////////////////////
// CRC16 function
unsigned int SkyetekM1::CRC16 (unsigned char *dataP, unsigned char n)
{
	unsigned char i, j;     // byte counter, bit counter
	unsigned int  crc_16;   // calculation

	crc_16 = 0x0000;        // PRESET value

	for (i = 0; i < n; i++) // check each byte in the array
	{
		crc_16 ^= *dataP++;

		for (j = 0; j < 8; j++) 	  // test each bit in the byte
			if (crc_16 & 0x0001)
			{
				crc_16 >>= 1;
				crc_16 ^= 0x8408; // POLYNOMIAL x^16+x^12+x^5+1
			}
			else
				crc_16 >>= 1;
	}
	return (crc_16);	// returns calculated crc (16 bits)
}


////////////////////////////////////////////////////////////////////////////////
// VerifyCRC function
unsigned char SkyetekM1::VerifyCRC (unsigned char *resp)
{
	unsigned char i = 0;
	unsigned char ret_crc[80];
	unsigned int  crc_check;

	int k = 0;
	
	for (k = 0; k < (int)((resp[1])-1); k++)
	{
		ret_crc[i] = resp[k+1];
		i++;
	}	

	crc_check = CRC16 (ret_crc, i);
	
	if ((resp[i+1] == (crc_check >> 8)) && (resp[i+2] == (crc_check & 0x00FF)))
		return 0;
	else
		return 1;
}

////////////////////////////////////////////////////////////////////////////////
// Sleep function
unsigned char SkyetekM1::Sleep ()
{
	unsigned char command_buf[80], response_buf[80];
	unsigned char i = 0;
	unsigned int crc_check;
	int      s_rwStatus;
	
	// binary mode command format: msgLen|flags|command|startblock|numblocks|crc
	command_buf[i++] = 0x07;             // set msg length
	command_buf[i++] = 0x20;             // set flags (CRC_F)
	command_buf[i++] = 0x42;             // WRITE_SYS (set command)
	
	command_buf[i++] = 0x04;             // OP_MODE
	command_buf[i++] = 0x01;             // set number of blocks to write
	command_buf[i++] = 0x00;             // set data to write

	crc_check = CRC16 (command_buf, i);
	command_buf[i++] = crc_check >> 8;   // set CRC
	command_buf[i++] = crc_check & 0x00FF;
	
	// copy the command buffer and prepare to write
	s_rwStatus = S_ReadWrite (command_buf, i, response_buf);
	
	// verify return CRC
	if (VerifyCRC (response_buf))
		return 0x81;                     // BAD_CRC
	
	return response_buf[2];
}

////////////////////////////////////////////////////////////////////////////////
// Wake function
unsigned char SkyetekM1::Wake ()
{

	unsigned char command_buf[80], response_buf[80];
	unsigned char i = 0;
	unsigned int crc_check;
	int      s_rwStatus;
	
	// binary mode command format: msgLen|flags|command|startblock|numblocks|crc
	command_buf[i++] = 0x07;             // set msg length
	command_buf[i++] = 0x20;             // set flags (CRC_F)
	command_buf[i++] = 0x42;             // WRITE_SYS (set command)
	command_buf[i++] = 0x04;             // OP_MODE
	command_buf[i++] = 0x01;             // set number of blocks to write
	command_buf[i++] = 0xFF;             // set data to write
	command_buf[0]   = i + 1;            // set msg Length

	crc_check = CRC16 (command_buf, i);
	command_buf[i++] = crc_check >> 8;   // set CRC
	command_buf[i++] = crc_check & 0x00FF;
	
	// copy the command buffer and prepare to write
	s_rwStatus = S_ReadWrite (command_buf, i, response_buf);
	
	// verify return CRC
	if (VerifyCRC (response_buf))
		return 0x81;                     // BAD_CRC
	
	return response_buf[2];
}