insidem300.cc

机器人仿真软件

/*
 *  Player - One Hell of a Robot Server
 *  Copyright (C) 2006 Radu Bogdan Rusu ([e-mail])
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */
/*
 Desc: Driver for the INSIDE M-300/R-300 2G RFID reader
 Author: Radu Bogdan Rusu
 Date: 25 Jan 2006
 CVS: $Id: insideM300.cc,v 1.3 2006/04/06 16:14:26 veedee Exp $
*/

/** @ingroup drivers */
/** @{ */
/** @defgroup driver_insideM300 insideM300
 * @brief Inside M300 RFID reader

The insideM300 driver controls the Inside Contactless M300/R300 2G RFID reader 
(13.56Mhz). Currently, only ISO 15693 tags are supported.

@par Compile-time dependencies

- none

@par Provides

- @ref interface_rfid

@par Requires

- none

@par Configuration requests

- PLAYER_RFID_REQ_POWER
- PLAYER_RFID_REQ_READTAG
- PLAYER_RFID_REQ_WRITETAG
- PLAYER_RFID_REQ_LOCKTAG

@par Configuration file options

- port (string)
  - Default: "/dev/ttyS0"
  - Serial port to which the Inside M300/R300 2G reader is attached.  If you are
    using a USB/232 or USB/422 converter, this will be "/dev/ttyUSBx".

- rate (integer)
  - Default: 9600
  - Baud rate. Valid values are 9600, 19200, 38400, 57600 and 115200.

@par Example 

@verbatim
driver
(
  name "insideM300"
  provides ["rfid:0"]
  port "/dev/ttyS0"
  rate "57600"
)
@endverbatim

@author Radu Bogdan Rusu

*/
/** @} */

#include <assert.h>
#include <errno.h>
#include <fcntl.h>
#include <stdio.h>
#include <string.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <termios.h>
#include <unistd.h>
#include <sys/ioctl.h>

// Includes needed for player
#include <libplayercore/playercore.h>

#define DEFAULT_RFID_PORT "/dev/ttyS0"
#define DEFAULT_RFID_RATE B9600

// ISO 7816-4 command set format
#define CMD_CLASS 0
#define CMD_INSTR 1
#define CMD_P1_P  2
#define CMD_P2_P  3
#define CMD_P3_P  4

// T=0 protocol direction
#define ISO_NONE   0x00
#define ISO_IN     0x01
#define ISO_OUT    0x02
#define ISO_IN_OUT 0x03

// STATUS DEFINITION
#define STATUS_OK                  0x9000
#define RESET_EEPROM_SUCCESFUL     0x3B00
#define P3_INCORRECT               0x6700
#define P1_P2_INCORRECT            0x6B00
#define CLASS_NOT_RECOGNIZED       0x6E00
#define INSTRUCTION_NOT_RECOGNIZED 0x6D00
#define CARD_NOT_IDENTIFIED        0x6982
#define COMMAND_FLOW_INCORRECT     0x9835
#define CARD_NOT_FOUND             0x6A82
#define EEPROM_RERROR              0x6200

// The InsideM300 device class.
class InsideM300 : public Driver
{
	public:
		// Constructor
		InsideM300 (ConfigFile* cf, int section);

		// Destructor
		~InsideM300 ();

		// Implementations of virtual functions
		int Setup ();
		int Shutdown ();

		// This method will be invoked on each incoming message
		virtual int ProcessMessage (MessageQueue* resp_queue, 
					    player_msghdr * hdr,
					    void * data);
	private:

		// Main function for device thread.
		virtual void Main  ();
		void RefreshData ();
		
		// Port file descriptor
		int                fd;
		// Initial serial port attributes
		struct termios     initial_options;

		// RFID interface
		player_rfid_data_t Data;
		player_rfid_cmd_t  Cmd;
		
		const char*        portName;
		int                portSpeed;
		
		// 0=select just a single tag (no anticollision)
		// 1=select all tags in the RFID field (anticollision)
		int                selectTagMultiple;

		// Inside functions
		int           CouplerSynchronize ();
		void          WriteByte          (unsigned char byte);
		unsigned char ReadByte           ();
		unsigned int  ReadStatusWord     ();
		int           SelectTags ();
		unsigned int  SendISOCommand     (unsigned char typeISO, int dataOutLen,
						  unsigned char commandType, 
						  unsigned char p1,
						  unsigned char p2, unsigned char p3,
						  unsigned char *dataIn, 
						  unsigned char *dataOut);
		unsigned int  IsoExchange (unsigned char typeISO, 
	    				   unsigned char *ISOCommand,
					   int dataInLen, unsigned char *dataIn,
					   int dataOutLen, unsigned char *dataOut);
		void InsideInventory (int maskLength, int chipMask, 
				  unsigned char *globalChipAnswer);
		unsigned int ResetField ();
};

////////////////////////////////////////////////////////////////////////////////
//Factory creation function. This functions is given as an argument when
// the driver is added to the driver table
Driver* InsideM300_Init (ConfigFile* cf, int section)
{
	// Create and return a new instance of this driver
	return ((Driver*)(new InsideM300 (cf, section)));
}

////////////////////////////////////////////////////////////////////////////////
//Registers the driver in the driver table. Called from the 
// player_driver_init function that the loader looks for
void InsideM300_Register (DriverTable* table)
{
	table->AddDriver ("insideM300", InsideM300_Init);
}

////////////////////////////////////////////////////////////////////////////////
// Constructor.  Retrieve options from the configuration file and do any
// pre-Setup() setup.
InsideM300::InsideM300 (ConfigFile* cf, int section)
	: Driver (cf, section, false, PLAYER_MSGQUEUE_DEFAULT_MAXLEN, 
			  PLAYER_RFID_CODE)
{
	this->portName  = cf->ReadString (section, "port", DEFAULT_RFID_PORT);
	this->portSpeed = cf->ReadInt (section, "speed", DEFAULT_RFID_RATE);
	
	// Enable the anticollision mode
	this->selectTagMultiple = 1;
	
	return;
}


////////////////////////////////////////////////////////////////////////////////
// Destructor.
InsideM300::~InsideM300()
{
}

////////////////////////////////////////////////////////////////////////////////
// Set up the device.  Return 0 if things go well, and -1 otherwise.
int InsideM300::Setup ()
{
	unsigned int status;
	unsigned char chipAnswer[24];
	unsigned char ISOCommand[4];
	
	// Open serial port
	this->fd = open (this->portName, O_RDWR | O_NOCTTY);
	if (this->fd < 0)
	{
		PLAYER_ERROR2 ("> Connecting to Inside M/R300 on [%s]; [%s]...[failed!]"
				,(char*) this->portName, strerror (errno));
		return (-1);
	}
	PLAYER_MSG0 (1, "> Connecting to Inside M/R300... [done]");

	// Change port settings
	struct termios options;
	memset (&options, 0,sizeof (options));// clear the struct for new port settings
	
	// Get the current port settings
	if (tcgetattr (this->fd, &options) != 0) {
		PLAYER_ERROR (">> Unable to get serial port attributes !");
		return (-1);
	}
	tcgetattr (this->fd, &this->initial_options);
	
	// turn off break sig, cr->nl, parity off, 8 bit strip, flow control
	options.c_iflag &= ~(BRKINT | ICRNL | INPCK | ISTRIP | IXON);

	// turn off echo, canonical mode, extended processing, signals
	options.c_lflag &= ~(ECHO | ECHOE | ICANON | IEXTEN | ISIG);

	options.c_cflag |= (CSTOPB);    // use two stop bits
	options.c_cflag |= (PARENB);    // generate parity bits (even parity)
	options.c_cflag &= ~(CSIZE );   // clear size
	options.c_cflag |= (CS8);       // set bit size (default is 8)
	options.c_oflag &= ~(OPOST);    // turn output processing off

	// read satisfied if TIME is exceeded (t = TIME *0.1 s)
	options.c_cc[VTIME] = 1;
	options.c_cc[VMIN] = 0;

	switch (this->portSpeed) {
	    case 9600:{
		this->portSpeed = B9600;
		break;
	    }
	    case 19200:{
		this->portSpeed = B19200;
		break;
	    }
	    case 38400:{
		this->portSpeed = B38400;
		break;
	    }
	    case 57600:{
		this->portSpeed = B57600;
		break;
	    }
	    case 115200:{
		this->portSpeed = B115200;
		break;
	    }
	    default:{
		this->portSpeed = B9600;
		break;
	    }
	}
	// Set the baudrate to the given portSpeed
	cfsetispeed (&options, this->portSpeed);
	cfsetospeed (&options, this->portSpeed);
	
	// Activate the settings for the port
	if (tcsetattr (this->fd, TCSAFLUSH, &options) < 0)
	{
		PLAYER_ERROR (">> Unable to set serial port attributes !");
		return (-1);
	}

	// Make sure queues are empty before we begin
	tcflush (this->fd, TCIOFLUSH);

	// Synchronizing buffer
	int result = CouplerSynchronize ();
	if (result != 0) {
		PLAYER_ERROR (">> Unable to synchronize with the reader !");
		return (-1);
	}
	
	// Get the current port settings
	if (tcgetattr (this->fd, &options) != 0) {
		PLAYER_ERROR (">> Unable to get serial port attributes !");
		return (-1);
	}
	// read satisfied if one char is read, or TIME is exceeded (t = TIME *0.1 s)
	options.c_cc[VTIME] = 1;
	options.c_cc[VMIN] = 1;
	// Activate the settings for the port
	if (tcsetattr (this->fd, TCSANOW, &options) !=0 )
	{
		PLAYER_ERROR (">> Unable to set serial port attributes !");
		return (-1);
	}

	ISOCommand[0] = 0x21;
	// 0xF4 = SET_STATUS
	status = SendISOCommand (ISO_IN, 0, 0xF4, 0x03, 0x5E, 0x01, ISOCommand, 
				 chipAnswer);
	if (status != STATUS_OK)
		PLAYER_WARN1 (">> Error 0x%x while sending ISO command (F4035E0121) !", 
				  status);
	
	ISOCommand[0] = 0x31;
	// 0xF4 = SET_STATUS
	status = SendISOCommand (ISO_IN, 0, 0xF4, 0x03, 0x5F, 0x01, ISOCommand, 
				 chipAnswer);
	if (status != STATUS_OK)
		PLAYER_WARN1 (">> Error 0x%x while sending ISO command (F4035F0131) !", 
				  status);
	
	ISOCommand[0] = 0x3B;
	// 0xF4 = SET_STATUS
	status = SendISOCommand (ISO_IN, 0, 0xF4, 0x03, 0x6B, 0x01, ISOCommand, 
				 chipAnswer);
	if (status != STATUS_OK)
		PLAYER_WARN1 (">> Error 0x%x while sending ISO command (F4036B013B) !", 
				  status);

	status = ResetField ();
	if (status != STATUS_OK)
		PLAYER_WARN1 (">> Error 0x%x while resetting field !", status);
	
	// Start the device thread
	StartThread ();

	return (0);
}

////////////////////////////////////////////////////////////////////////////////
// Shutdown the device
int InsideM300::Shutdown ()
{
	// Stop the driver thread
	StopThread ();

	// Close the serial port
	tcsetattr (this->fd, TCSANOW, &this->initial_options);
	close (this->fd);
	
	PLAYER_MSG0 (1, "> InsideM300 driver shutting down... [done]");
	return (0);
}

////////////////////////////////////////////////////////////////////////////////
// Main function for device thread
void InsideM300::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)
	{