ConexAgp.cpp

/*----- PROTECTED REGION ID(ConexAgp.cpp) ENABLED START -----*/
static const char *RcsId = "$Id: ConexAgp.cpp,v 1.1 2017-12-21 11:26:12 graziano Exp $";
//=============================================================================
//
// file :        ConexAgp.cpp
//
// description : C++ source for the ConexAgp class and its commands.
//               The class is derived from Device. It represents the
//               CORBA servant object which will be accessed from the
//               network. All commands which can be executed on the
//               ConexAgp are implemented in this file.
//
// project :     Newport ConexAgp Agilis-D controller
//
// This file is part of Tango device class.
// 
// Tango 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 3 of the License, or
// (at your option) any later version.
// 
// Tango 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 Tango.  If not, see <http://www.gnu.org/licenses/>.
// 
// $Author: graziano $
//
// $Revision: 1.1 $
// $Date: 2017-12-21 11:26:12 $
//
// $HeadURL:  $
//
//=============================================================================
//                This file is generated by POGO
//        (Program Obviously used to Generate tango Object)
//=============================================================================


#include <ConexAgp.h>
#include <ConexAgpClass.h>

//from Serial.h for legacy serial-srv
#define SL_RAW        0     /* raw read/write mode */
#define SL_NCHAR      1     /* character read/write mode */
#define SL_LINE       2     /* line read mode */
#define SL_RETRY      3     /* retry read mode */

#ifndef MODULE_BUILDTIME
#define MODULE_BUILDTIME   " " __DATE__ " "  __TIME__
#endif

const char version_string[] = "$Build: " MODULE_BUILDTIME " $";
const char release_string[] = "$Name:  $";
static const char __FILE__rev[] = __FILE__ " $Revision: 1.1 $";

static const char *ver = (const char *)&version_string;

/*----- PROTECTED REGION END -----*/	//	ConexAgp.cpp

/**
 *  ConexAgp class description:
 *    Newport ConexAgp Agilis-D controller
 */

//================================================================
//  The following table gives the correspondence
//  between command and method names.
//
//  Command name  |  Method name
//================================================================
//  State         |  Inherited (no method)
//  Status        |  Inherited (no method)
//  Homing        |  homing
//  Stop          |  stop
//  GetVersion    |  get_version
//  Reset         |  reset
//  Backward      |  backward
//  Forward       |  forward
//  Enable        |  enable
//  Disable       |  disable
//================================================================

//================================================================
//  Attributes managed are:
//================================================================
//  Position        |  Tango::DevDouble	Scalar
//  TargetPosition  |  Tango::DevDouble	Scalar
//================================================================

namespace ConexAgp_ns
{
/*----- PROTECTED REGION ID(ConexAgp::namespace_starting) ENABLED START -----*/

//	static initializations

/*----- PROTECTED REGION END -----*/	//	ConexAgp::namespace_starting

//--------------------------------------------------------
/**
 *	Method      : ConexAgp::ConexAgp()
 *	Description : Constructors for a Tango device
 *                implementing the classConexAgp
 */
//--------------------------------------------------------
ConexAgp::ConexAgp(Tango::DeviceClass *cl, string &s)
 : TANGO_BASE_CLASS(cl, s.c_str())
{
	/*----- PROTECTED REGION ID(ConexAgp::constructor_1) ENABLED START -----*/
	init_device();
	
	/*----- PROTECTED REGION END -----*/	//	ConexAgp::constructor_1
}
//--------------------------------------------------------
ConexAgp::ConexAgp(Tango::DeviceClass *cl, const char *s)
 : TANGO_BASE_CLASS(cl, s)
{
	/*----- PROTECTED REGION ID(ConexAgp::constructor_2) ENABLED START -----*/
	init_device();
	
	/*----- PROTECTED REGION END -----*/	//	ConexAgp::constructor_2
}
//--------------------------------------------------------
ConexAgp::ConexAgp(Tango::DeviceClass *cl, const char *s, const char *d)
 : TANGO_BASE_CLASS(cl, s, d)
{
	/*----- PROTECTED REGION ID(ConexAgp::constructor_3) ENABLED START -----*/
	init_device();
	
	/*----- PROTECTED REGION END -----*/	//	ConexAgp::constructor_3
}

//--------------------------------------------------------
/**
 *	Method      : ConexAgp::delete_device()
 *	Description : will be called at device destruction or at init command
 */
//--------------------------------------------------------
void ConexAgp::delete_device()
{
	DEBUG_STREAM << "ConexAgp::delete_device() " << device_name << endl;
	/*----- PROTECTED REGION ID(ConexAgp::delete_device) ENABLED START -----*/
	
	//	Delete device allocated objects
	loop->abortflag = true;
	sleep(1);
	delete mutex;
	if (device_proxy)
	{
		delete device_proxy;
		device_proxy = NULL;
	}
	/*----- PROTECTED REGION END -----*/	//	ConexAgp::delete_device
	delete[] attr_Position_read;
	delete[] attr_TargetPosition_read;
}

//--------------------------------------------------------
/**
 *	Method      : ConexAgp::init_device()
 *	Description : will be called at device initialization.
 */
//--------------------------------------------------------
void ConexAgp::init_device()
{
	DEBUG_STREAM << "ConexAgp::init_device() create device " << device_name << endl;
	/*----- PROTECTED REGION ID(ConexAgp::init_device_before) ENABLED START -----*/
	DEBUG_STREAM << __func__<< "  entering... ver: " << version_string << " " << release_string << " file: " << __FILE__rev << endl;
	
	//	Initialization before get_device_property() call
	controller_states.insert(make_pair(0x0a, make_state_desc(Tango::STANDBY,string("Not Referenced from Reset"))));
	controller_states.insert(make_pair(0x0b, make_state_desc(Tango::STANDBY,string("Not Referenced from Homing"))));
	controller_states.insert(make_pair(0x0c, make_state_desc(Tango::STANDBY,string("Not Referenced from Configuration"))));
	controller_states.insert(make_pair(0x0d, make_state_desc(Tango::STANDBY,string("Not Referenced from Disable"))));
	controller_states.insert(make_pair(0x0e, make_state_desc(Tango::STANDBY,string("Not Referenced from Ready"))));
	controller_states.insert(make_pair(0x0f, make_state_desc(Tango::STANDBY,string("Not Referenced from Moving"))));
	controller_states.insert(make_pair(0x10, make_state_desc(Tango::STANDBY,string("Not Referenced no parameters"))));
	controller_states.insert(make_pair(0x14, make_state_desc(Tango::ALARM,string("Configuration"))));
	controller_states.insert(make_pair(0x1e, make_state_desc(Tango::MOVING,string("Homing"))));
	controller_states.insert(make_pair(0x28, make_state_desc(Tango::MOVING,string("Moving"))));
	controller_states.insert(make_pair(0x32, make_state_desc(Tango::ON,string("Ready from Homing"))));
	controller_states.insert(make_pair(0x33, make_state_desc(Tango::ON,string("Ready from Moving"))));
	controller_states.insert(make_pair(0x34, make_state_desc(Tango::ON,string("Ready from Disable"))));
	controller_states.insert(make_pair(0x3C, make_state_desc(Tango::DISABLE,string("Disable from Ready"))));
	controller_states.insert(make_pair(0x3D, make_state_desc(Tango::DISABLE,string("Disable from Moving"))));

	device_proxy = NULL;
	
	/*----- PROTECTED REGION END -----*/	//	ConexAgp::init_device_before
	

	//	Get the device properties from database
	get_device_property();
	
	attr_Position_read = new Tango::DevDouble[1];
	attr_TargetPosition_read = new Tango::DevDouble[1];
	/*----- PROTECTED REGION ID(ConexAgp::init_device) ENABLED START -----*/
	
	//	Initialize device
	device_proxy = new Tango::DeviceProxy(deviceName);
	try
	{
		device_proxy->ping();
	}
	catch(Tango::DevFailed &e)
	{
		DEBUG_STREAM << __func__<<": error pinging, err="<<e.errors[0].desc;
		set_state(Tango::FAULT);
		set_status(string(e.errors[0].desc));
		return;
	}
	//empty buffers
	try
	{
		if(!legacySerial)
		{
			Tango::DeviceAttribute dattrin1("Timeout",(Tango::DevLong)5);
			device_proxy->write_attribute(dattrin1);
			Tango::DevLong len =  500;

			Tango::DeviceData dintmp, douttmp;
			dintmp<<len;
			douttmp = device_proxy->command_inout("Read", dintmp);
		}
		else
		{
			Tango::DeviceData Din;
			Tango::DevLong flush = 2;  //flush input and output
			Din << flush;
			device_proxy->command_inout("DevSerFlush",Din);
		}
	}
	catch(Tango::DevFailed &e)
	{
	}
	catch(...)
	{
	}

	if(!legacySerial)
	{
		try
		{
			Tango::DeviceAttribute dattrin2("Timeout",(Tango::DevLong)1000);
			device_proxy->write_attribute(dattrin2);
		}
		catch(Tango::DevFailed &e)
		{
			DEBUG_STREAM << __func__<<": error setting timeout, err="<<e.errors[0].desc;
			set_state(Tango::FAULT);
			set_status(string(e.errors[0].desc));
		}
		catch(...)
		{
		}
	}

	set_change_event("State", true, true);
	set_archive_event("State", true, true);

	mutex = new omni_mutex();
	
	loop = new readthread(this);
	loop->start();

	/*----- PROTECTED REGION END -----*/	//	ConexAgp::init_device
}

//--------------------------------------------------------
/**
 *	Method      : ConexAgp::get_device_property()
 *	Description : Read database to initialize property data members.
 */
//--------------------------------------------------------
void ConexAgp::get_device_property()
{
	/*----- PROTECTED REGION ID(ConexAgp::get_device_property_before) ENABLED START -----*/
	
	//	Initialize property data members
	
	/*----- PROTECTED REGION END -----*/	//	ConexAgp::get_device_property_before


	//	Read device properties from database.
	Tango::DbData	dev_prop;
	dev_prop.push_back(Tango::DbDatum("DeviceName"));
	dev_prop.push_back(Tango::DbDatum("ControllerAddr"));
	dev_prop.push_back(Tango::DbDatum("LegacySerial"));

	//	is there at least one property to be read ?
	if (dev_prop.size()>0)
	{
		//	Call database and extract values
		if (Tango::Util::instance()->_UseDb==true)
			get_db_device()->get_property(dev_prop);
	
		//	get instance on ConexAgpClass to get class property
		Tango::DbDatum	def_prop, cl_prop;
		ConexAgpClass	*ds_class =
			(static_cast<ConexAgpClass *>(get_device_class()));
		int	i = -1;

		//	Try to initialize DeviceName from class property
		cl_prop = ds_class->get_class_property(dev_prop[++i].name);
		if (cl_prop.is_empty()==false)	cl_prop  >>  deviceName;
		else {
			//	Try to initialize DeviceName from default device value
			def_prop = ds_class->get_default_device_property(dev_prop[i].name);
			if (def_prop.is_empty()==false)	def_prop  >>  deviceName;
		}
		//	And try to extract DeviceName value from database
		if (dev_prop[i].is_empty()==false)	dev_prop[i]  >>  deviceName;

		//	Try to initialize ControllerAddr from class property
		cl_prop = ds_class->get_class_property(dev_prop[++i].name);
		if (cl_prop.is_empty()==false)	cl_prop  >>  controllerAddr;
		else {
			//	Try to initialize ControllerAddr from default device value
			def_prop = ds_class->get_default_device_property(dev_prop[i].name);
			if (def_prop.is_empty()==false)	def_prop  >>  controllerAddr;
		}
		//	And try to extract ControllerAddr value from database
		if (dev_prop[i].is_empty()==false)	dev_prop[i]  >>  controllerAddr;

		//	Try to initialize LegacySerial from class property
		cl_prop = ds_class->get_class_property(dev_prop[++i].name);
		if (cl_prop.is_empty()==false)	cl_prop  >>  legacySerial;
		else {
			//	Try to initialize LegacySerial from default device value
			def_prop = ds_class->get_default_device_property(dev_prop[i].name);
			if (def_prop.is_empty()==false)	def_prop  >>  legacySerial;
		}
		//	And try to extract LegacySerial value from database
		if (dev_prop[i].is_empty()==false)	dev_prop[i]  >>  legacySerial;

	}

	/*----- PROTECTED REGION ID(ConexAgp::get_device_property_after) ENABLED START -----*/
	
	//	Check device property data members init
	
	/*----- PROTECTED REGION END -----*/	//	ConexAgp::get_device_property_after
}

//--------------------------------------------------------
/**
 *	Method      : ConexAgp::always_executed_hook()
 *	Description : method always executed before any command is executed
 */
//--------------------------------------------------------
void ConexAgp::always_executed_hook()
{
	DEBUG_STREAM << "ConexAgp::always_executed_hook()  " << device_name << endl;
	/*----- PROTECTED REGION ID(ConexAgp::always_executed_hook) ENABLED START -----*/
	
	//	code always executed before all requests
	
	/*----- PROTECTED REGION END -----*/	//	ConexAgp::always_executed_hook
}

//--------------------------------------------------------
/**
 *	Method      : ConexAgp::read_attr_hardware()
 *	Description : Hardware acquisition for attributes
 */
//--------------------------------------------------------
void ConexAgp::read_attr_hardware(TANGO_UNUSED(vector<long> &attr_list))
{
	DEBUG_STREAM << "ConexAgp::read_attr_hardware(vector<long> &attr_list) entering... " << endl;
	/*----- PROTECTED REGION ID(ConexAgp::read_attr_hardware) ENABLED START -----*/
	
	//	Add your own code
	
	/*----- PROTECTED REGION END -----*/	//	ConexAgp::read_attr_hardware
}
//--------------------------------------------------------
/**
 *	Method      : ConexAgp::write_attr_hardware()
 *	Description : Hardware writing for attributes
 */
//--------------------------------------------------------
void ConexAgp::write_attr_hardware(TANGO_UNUSED(vector<long> &attr_list))
{
	DEBUG_STREAM << "ConexAgp::write_attr_hardware(vector<long> &attr_list) entering... " << endl;
	/*----- PROTECTED REGION ID(ConexAgp::write_attr_hardware) ENABLED START -----*/
	
	//	Add your own code
	
	/*----- PROTECTED REGION END -----*/	//	ConexAgp::write_attr_hardware
}

//--------------------------------------------------------
/**
 *	Read attribute Position related method
 *	Description: 
 *
 *	Data type:	Tango::DevDouble
 *	Attr type:	Scalar
 */
//--------------------------------------------------------
void ConexAgp::read_Position(Tango::Attribute &attr)
{
	DEBUG_STREAM << "ConexAgp::read_Position(Tango::Attribute &attr) entering... " << endl;
	/*----- PROTECTED REGION ID(ConexAgp::read_Position) ENABLED START -----*/
	//	Set the attribute value
	attr.set_value(attr_Position_read);
	
	/*----- PROTECTED REGION END -----*/	//	ConexAgp::read_Position
}
//--------------------------------------------------------
/**
 *	Write attribute Position related method
 *	Description: 
 *
 *	Data type:	Tango::DevDouble
 *	Attr type:	Scalar
 */
//--------------------------------------------------------
void ConexAgp::write_Position(Tango::WAttribute &attr)
{
	DEBUG_STREAM << "ConexAgp::write_Position(Tango::WAttribute &attr) entering... " << endl;
	//	Retrieve write value
	Tango::DevDouble	w_val;
	attr.get_write_value(w_val);
	/*----- PROTECTED REGION ID(ConexAgp::write_Position) ENABLED START -----*/
	string resp("NO");
	stringstream cmd;
	cmd << MOVE_ABSOLUTE << w_val;
	SendReceive(cmd.str(), resp);
	
	/*----- PROTECTED REGION END -----*/	//	ConexAgp::write_Position
}
//--------------------------------------------------------
/**
 *	Read attribute TargetPosition related method
 *	Description: 
 *
 *	Data type:	Tango::DevDouble
 *	Attr type:	Scalar
 */
//--------------------------------------------------------
void ConexAgp::read_TargetPosition(Tango::Attribute &attr)
{
	DEBUG_STREAM << "ConexAgp::read_TargetPosition(Tango::Attribute &attr) entering... " << endl;
	/*----- PROTECTED REGION ID(ConexAgp::read_TargetPosition) ENABLED START -----*/
	//	Set the attribute value
	attr.set_value(attr_TargetPosition_read);
	
	/*----- PROTECTED REGION END -----*/	//	ConexAgp::read_TargetPosition
}

//--------------------------------------------------------
/**
 *	Method      : ConexAgp::add_dynamic_attributes()
 *	Description : Create the dynamic attributes if any
 *                for specified device.
 */
//--------------------------------------------------------
void ConexAgp::add_dynamic_attributes()
{
	/*----- PROTECTED REGION ID(ConexAgp::add_dynamic_attributes) ENABLED START -----*/
	
	//	Add your own code to create and add dynamic attributes if any
	
	/*----- PROTECTED REGION END -----*/	//	ConexAgp::add_dynamic_attributes
}

//--------------------------------------------------------
/**
 *	Command Homing related method
 *	Description: 
 *
 */
//--------------------------------------------------------
void ConexAgp::homing()
{
	DEBUG_STREAM << "ConexAgp::Homing()  - " << device_name << endl;
	/*----- PROTECTED REGION ID(ConexAgp::homing) ENABLED START -----*/
	
	//	Add your own code
	string resp("NO");
	SendReceive(HOMING, resp);
	
	/*----- PROTECTED REGION END -----*/	//	ConexAgp::homing
}
//--------------------------------------------------------
/**
 *	Command Stop related method
 *	Description: 
 *
 */
//--------------------------------------------------------
void ConexAgp::stop()
{
	DEBUG_STREAM << "ConexAgp::Stop()  - " << device_name << endl;
	/*----- PROTECTED REGION ID(ConexAgp::stop) ENABLED START -----*/
	
	//	Add your own code
	string resp("NO");
	SendReceive(STOP_MOTION, resp);
	
	/*----- PROTECTED REGION END -----*/	//	ConexAgp::stop
}
//--------------------------------------------------------
/**
 *	Command GetVersion related method
 *	Description: 
 *
 *	@returns 
 */
//--------------------------------------------------------
Tango::DevString ConexAgp::get_version()
{
	Tango::DevString argout;
	DEBUG_STREAM << "ConexAgp::GetVersion()  - " << device_name << endl;
	/*----- PROTECTED REGION ID(ConexAgp::get_version) ENABLED START -----*/
	
	//	Add your own code
	string resp;
	SendReceive(GET_VERSION, resp);
	argout = Tango::string_dup(resp.c_str());
	
	/*----- PROTECTED REGION END -----*/	//	ConexAgp::get_version
	return argout;
}
//--------------------------------------------------------
/**
 *	Command Reset related method
 *	Description: Reset controller
 *
 */
//--------------------------------------------------------
void ConexAgp::reset()
{
	DEBUG_STREAM << "ConexAgp::Reset()  - " << device_name << endl;
	/*----- PROTECTED REGION ID(ConexAgp::reset) ENABLED START -----*/
	
	//	Add your own code
	string resp("NO");
	SendReceive(RESET_CONTROLLER, resp);
	
	/*----- PROTECTED REGION END -----*/	//	ConexAgp::reset
}
//--------------------------------------------------------
/**
 *	Command Backward related method
 *	Description: 
 *
 *	@param argin 
 */
//--------------------------------------------------------
void ConexAgp::backward(Tango::DevDouble argin)
{
	DEBUG_STREAM << "ConexAgp::Backward()  - " << device_name << endl;
	/*----- PROTECTED REGION ID(ConexAgp::backward) ENABLED START -----*/
	
	//	Add your own code
	string resp("NO");
	stringstream cmd;
	cmd << MOVE_RELATIVE << -argin;
	SendReceive(cmd.str(), resp);
	
	/*----- PROTECTED REGION END -----*/	//	ConexAgp::backward
}
//--------------------------------------------------------
/**
 *	Command Forward related method
 *	Description: 
 *
 *	@param argin 
 */
//--------------------------------------------------------
void ConexAgp::forward(Tango::DevDouble argin)
{
	DEBUG_STREAM << "ConexAgp::Forward()  - " << device_name << endl;
	/*----- PROTECTED REGION ID(ConexAgp::forward) ENABLED START -----*/
	
	//	Add your own code
	string resp("NO");
	stringstream cmd;
	cmd << MOVE_RELATIVE << argin;
	SendReceive(cmd.str(), resp);
	
	/*----- PROTECTED REGION END -----*/	//	ConexAgp::forward
}
//--------------------------------------------------------
/**
 *	Command Enable related method
 *	Description: 
 *
 */
//--------------------------------------------------------
void ConexAgp::enable()
{
	DEBUG_STREAM << "ConexAgp::Enable()  - " << device_name << endl;
	/*----- PROTECTED REGION ID(ConexAgp::enable) ENABLED START -----*/
	
	//	Add your own code
	string resp("NO");
	stringstream cmd;
	cmd << ENABLE << "1";
	SendReceive(cmd.str(), resp);
	
	/*----- PROTECTED REGION END -----*/	//	ConexAgp::enable
}
//--------------------------------------------------------
/**
 *	Command Disable related method
 *	Description: 
 *
 */
//--------------------------------------------------------
void ConexAgp::disable()
{
	DEBUG_STREAM << "ConexAgp::Disable()  - " << device_name << endl;
	/*----- PROTECTED REGION ID(ConexAgp::disable) ENABLED START -----*/
	
	//	Add your own code
	string resp("NO");
	stringstream cmd;
	cmd << ENABLE << "0";
	SendReceive(cmd.str(), resp);
	
	/*----- PROTECTED REGION END -----*/	//	ConexAgp::disable
}
//--------------------------------------------------------
/**
 *	Method      : ConexAgp::add_dynamic_commands()
 *	Description : Create the dynamic commands if any
 *                for specified device.
 */
//--------------------------------------------------------
void ConexAgp::add_dynamic_commands()
{
	/*----- PROTECTED REGION ID(ConexAgp::add_dynamic_commands) ENABLED START -----*/
	
	//	Add your own code to create and add dynamic commands if any
	
	/*----- PROTECTED REGION END -----*/	//	ConexAgp::add_dynamic_commands
}

/*----- PROTECTED REGION ID(ConexAgp::namespace_ending) ENABLED START -----*/

//	Additional Methods
void ConexAgp::SendReceive(const string & command, string & response)
{
	char cmd[64];
	memset(cmd,0,64);
	sprintf(cmd, "%d%s",controllerAddr,command.c_str());
	mutex->lock();
#if 0
	//emptying buffer
	try
	{
		Tango::DeviceAttribute dattrin("Timeout",(Tango::DevLong)1);
		device_proxy->write_attribute(dattrin);
		Tango::DevLong len = 100;

		Tango::DeviceData din2, dout;
		din2 << len;

		const Tango::DevVarCharArray *dtr;
		dout = device_proxy->command_inout("Read", din2);
		dout >> dtr;
	}
	catch(Tango::DevFailed &e)
	{
	}
	catch(...)
	{
	}
#endif
	try
	{
		if(!legacySerial)
		{
			Tango::DeviceAttribute dattrin("Timeout",(Tango::DevLong)5);
			device_proxy->write_attribute(dattrin);

			Tango::DevVarCharArray dvca;
			dvca.length(strlen(cmd)+2);
			for (unsigned int i=0; i<strlen(cmd); ++i)
				dvca[i] = cmd[i];
			dvca[strlen(cmd)] = '\r';
			dvca[strlen(cmd)+1] = '\n';

			Tango::DeviceData din;
			din << dvca;
			device_proxy->command_inout("Write", din);
		}
		else
		{
			char outbuf[256];
			long write_len=strlen(cmd)+2;
			strcpy(outbuf,cmd);
			strcat(outbuf,"\r\n");
			write_serial(outbuf,write_len);
		}
		DEBUG_STREAM << __func__<<": sent command: " << cmd;

		if(response != "NO")
		{
			Tango::DeviceData din2, dout;
			if(!legacySerial)
			{
				Tango::DeviceAttribute dattrin("Timeout",(Tango::DevLong)1);
				device_proxy->write_attribute(dattrin);
				Tango::DevVarCharArray *delim =  new Tango::DevVarCharArray();
				delim->length(1);
				//(*delim)[0] = '\n';
				(*delim)[0] = '\r';	//sometimes \n after \r is missing
				din2 << delim;
			}
			usleep(60000);	//60ms
			bool parsing_error = false;
			int count = 0;
			do
			{
			char cmd_parsing[64];
			strncpy(cmd_parsing,cmd, 64);
			count++;
			string resp;
			if(!legacySerial)
			{
				const Tango::DevVarCharArray *dtr;
				dout = device_proxy->command_inout("ReadUntil", din2);
				dout >> dtr;

				for (unsigned int i=0; i<dtr->length(); ++i)
					resp += (*dtr)[i];
			}
			else
			{
				long read_len;
				char inbuf[256];
				memset(inbuf,0,256);
				read_serial(inbuf,read_len);

				for (unsigned int i=0; i<read_len; ++i)
					resp += inbuf[i];
			}
			DEBUG_STREAM << __func__<<": read response='"<<resp << "'";
			char parsed_resp[256];
			//strcat(cmd, "%[^\r]\n");
			if(cmd_parsing[strlen(cmd_parsing)-1] == '?')
				cmd_parsing[strlen(cmd_parsing)-1] = 0;

			size_t pos = resp.find(cmd_parsing);
			if(pos != string::npos && pos > 0)
			{
				resp = resp.substr(pos);
				DEBUG_STREAM << __func__<<": 2! pos="<<pos<<" read response='"<<resp << "'";
			}

			strcat(cmd_parsing, "%[^\r]");
			int ret = sscanf (resp.c_str(), cmd_parsing, parsed_resp);
			if(ret == 1)
			{
				response = string(parsed_resp);
				parsing_error = false;
				break;
			}
			else
			{
				INFO_STREAM << __func__ << ": ERROR parsing response";
	                        parsing_error = true;
			}
			} while(parsing_error && count < 2);
		}
		else
		{
			response = string("");
		}
	}
	catch(Tango::DevFailed &e)
	{
		stringstream err;
		err << "Error sending command '" << command << "': '" << e.errors[0].desc << "'";
		INFO_STREAM << __func__ << ": " << err.str();
		response = string("");
		set_state(Tango::FAULT);
		set_status(err.str());
		usleep(100000);
	}
	catch(...)
	{
		INFO_STREAM << __func__<<": NON Tango Exception sending command";
	}
	mutex->unlock();
}

void ConexAgp::SendReceive(const string & command, int & val)
{
	string response;
	SendReceive(command, response);
	stringstream tmp;
	tmp << response;
	tmp >> val;
}

void ConexAgp::SendReceive(const string & command, double & val)
{
	string response;
	SendReceive(command, response);
	stringstream tmp;
	tmp << response;
	tmp >> val;
}

state_desc ConexAgp::make_state_desc(Tango::DevState _state, string _status)
{
	state_desc sd = {_state, _status};
	return sd;
}

//+------------------------------------------------------------------
//
//	method:			ConexAgp::write_serial
//
//	description:	method to execute "write_serial"
//					Write a string to serial interface
//
//-------------------------------------------------------------------
void ConexAgp::write_serial(char *write_str, long size)
//void Chiller::write_serial(string write_str)
{
	DEBUG_STREAM << "ConexAgp::write_serial(): entering... =" << string(write_str) <<"size="<<size<< endl;
	//	Add your own code to control device here
	Tango::DeviceData Din,Din2;
	int i;
	Tango::DevLong flush = 2;
	Tango::DevVarCharArray *input = new Tango::DevVarCharArray();
	input->length(size);
//	input->length(write_str.length());
	stringstream temp;

//	for (int i=0; i < write_str.length(); i++)
	for (long i=0; i < size; i++)
	{
	//DEBUG_STREAM << "ConexAgp::write_serial(): looping i=" << i<< endl;
		//(*input)[i] = (unsigned char) write_str.at(i);
		(*input)[i] = write_str[i];
		temp << showbase << hex << (int)(*input)[i] << " ";
	}
	//DEBUG_STREAM << "ConexAgp::write_serial(): writing=" << write_str << "=" << temp.str() << endl;
	DEBUG_STREAM << "ConexAgp::write_serial(): writing=" << temp.str() << endl;

	Din << input;
	device_proxy->command_inout("DevSerWriteChar",Din);
/*	Din2 << flush;
	device_proxy->command_inout("DevSerFlush",Din2);*/

}

//+------------------------------------------------------------------
//
//	method:			ConexAgp::read_serial
//
//	description:	method to execute "read_serial"
//					Read from serial interface
//
//-------------------------------------------------------------------
void ConexAgp::read_serial(char *read_str, long &read_len)
{
	DEBUG_STREAM << "ConexAgp::read_serial(): entering... !" << endl;
	//	Add your own code to control device here
	Tango::DevVarLongStringArray strin;
	string input, strout, strout_filt;
	Tango::DeviceData Din,Dout;
	const Tango::DevVarCharArray *out;
	//long attr_read_param = SL_RAW;
	long attr_read_param;
	//attr_read_param = SAALEN << 8;
	//attr_read_param = 1 << 8;   //provo a leggere solo un carattere
	//attr_read_param |= SL_NCHAR;
	attr_read_param = SL_LINE;

	//attr_read_param = 2 << 8;   //provo a leggere per 2 tentativi
	//attr_read_param |= SL_RETRY;
	long temp_len;
	int i;

	Din << attr_read_param;
	Dout = device_proxy->command_inout("DevSerReadChar",Din);

	if(!Dout.is_empty())
		Dout >> out;
	else
	{
		//set_state(Tango::FAULT);
		//set_status("Errors from serialdevice " + serialdevice);
		Tango::Except::throw_exception(
			(const char *) "Serialline timeout",
			(const char *) "No data from serialline",
			(const char *) "ConexAgp::read_serial()", Tango::ERR);
	}

	memset(read_str,0,256);
	temp_len = out->length();
	if(temp_len > 256)
	{
		WARN_STREAM << "ConexAgp::read_serial(): read buffer overflow, " <<
			temp_len << " > " <<  256 << endl;
		temp_len = 256;
	}
	read_len = temp_len;
	stringstream temp;
	for (int i = 0; i < temp_len; i++)
	{
		read_str[i] = (*out)[i];
		temp << showbase << hex << (int)(*out)[i] << " ";
	}
	DEBUG_STREAM << "ConexAgp::read_serial(): read=" << temp.str() << endl;
}
// //--------------------------------------------------------
// /**
//  *	Command Backward_Y related method
//  *	Description: 
//  *
//  *	@param argin 
//  */
// //--------------------------------------------------------
// void ConexAgp::backward__y(Tango::DevDouble argin)
// {
// 	DEBUG_STREAM << "ConexAgp::Backward_Y()  - " << device_name << endl;
// 	
// 	//	Add your own code
// 	string resp("NO");
// 	stringstream cmd;
// 	cmd << MOVE_RELATIVE << "V" << -argin;
// 	SendReceive(cmd.str(), resp);
// 	
// }

// //--------------------------------------------------------
// /**
//  *	Command Forward_Y related method
//  *	Description: 
//  *
//  *	@param argin 
//  */
// //--------------------------------------------------------
// void ConexAgp::forward__y(Tango::DevDouble argin)
// {
// 	DEBUG_STREAM << "ConexAgp::Forward_Y()  - " << device_name << endl;
// 	
// 	//	Add your own code
// 	string resp("NO");
// 	stringstream cmd;
// 	cmd << MOVE_RELATIVE << "V" << argin;
// 	SendReceive(cmd.str(), resp);
// 	
// }

// //--------------------------------------------------------
// /**
//  *	Command Move_X1 related method
//  *	Description: 
//  *
//  */
// //--------------------------------------------------------
// void ConexAgp::move__x1()
// {
// 	DEBUG_STREAM << "ConexAgp::Move_X1()  - " << device_name << endl;
// 	
// 	//	Add your own code
// 	string resp("NO");
// 	stringstream cmd;
// 	cmd << MOVE_ABSOLUTE << "U" << *attr_Position_X1_read;
// 	SendReceive(cmd.str(), resp);
// 	
// }

// //--------------------------------------------------------
// /**
//  *	Command Move_X2 related method
//  *	Description: 
//  *
//  */
// //--------------------------------------------------------
// void ConexAgp::move__x2()
// {
// 	DEBUG_STREAM << "ConexAgp::Move_X2()  - " << device_name << endl;
// 	
// 	//	Add your own code
// 	string resp("NO");
// 	stringstream cmd;
// 	cmd << MOVE_ABSOLUTE << "U" << *attr_Position_X2_read;
// 	SendReceive(cmd.str(), resp);
// 	
// }

// //--------------------------------------------------------
// /**
//  *	Command Move_XY_home related method
//  *	Description: 
//  *