usb2 @ dcc9934b

-Subproject commit bbf88d71ce04b87c7a3a2aa98996f91a5079bb47
+Subproject commit dcc9934b81c440a5737c26448a08527e88bd8907

src/E621.cpp

@@ -261,8 +261,7 @@ void E621::init_device()
 		command("SSN? A\n", rep);
 		strncpy(attr_SerialNumber_read[0], rep.c_str(), MAX_DEVSTRING_LENGTH);
 
-		// TODO
-		// check_state();
+		check_state();
 	} catch (Tango::DevFailed &e) {
 		init_error = "Initialization failed: " + string(e.errors[0].desc);
 	} catch(...) { 
@@ -306,62 +305,19 @@ void E621::always_executed_hook()
 		assert(false);
 		return;
 	}
-///////////////////////////////////////////////////////////////////////////////////
-	// TODO Da rivedere!
-	set_state(Tango::ON);
-	set_status("The device is in ON state");
 
 	string error;
 	try {
-		string s;
-		command("I2C?\n", s); // TODO
-		/* bit 0 (LSB):CHK_SEN0 timeout
-bit 1:CHK_PEN0 timeout
-bit 2:CHK_RSEN0 timeout
-bit 3:CHK_RW0 timeout
-bit 4:CHK_BF0 timeout
-bit 5:CHK_BF1 timeout
-bit 6:CHK_ACK0 timeout
-bit 7: (MSB)SLAVE_BUSY timeout*/
-
-#if 0
-		for (size_t i = 0; i < sai_output.size(); ++i) {
-			stringstream cmd;
-			cmd << "OVF? " << sai_output[i] << "\n";
-			string rep;
-			command(cmd.str(), rep);
-			int code;
-			convert(rep, code);
-			if (code != 0) {
-				set_state(Tango::FAULT);
-				set_status("The device is in FAULT state due overflow!");
-				return;
-			}
-		}
-		for (size_t i = 0; i < sai_output.size(); ++i) {
-			stringstream cmd;
-			cmd << "ONT? " << sai_output[i] << "\n";
-			string rep;
-			command(cmd.str(), rep);
-			int code;
-			convert(rep, code);
-			if (code != 1) {
-				set_state(Tango::MOVING);
-				set_status("The device is in MOVING state");
-				return;
-			}
-		}
-#endif
-///////////////////////////////////////////////////////////////////////////////////
+		check_state();
 	} catch (Tango::DevFailed &e) {
 		error = string(e.errors[0].desc);
 	} catch(...) { 
-		error = "unknown";
+		error = "Unknown exception";
 	}
 	
 	if (! error.empty()) {
 		ERROR_STREAM << error << endl;
-		set_state(Tango::FAULT);
+		set_state(Tango::UNKNOWN);
 		set_status(error);
 		assert(false);
 	}
@@ -646,6 +602,34 @@ void E621::check_init()
 			"E621::check_init()");
 }
 
+void E621::check_state()
+{
+	set_state(Tango::ON);
+	set_status("The device is in ON state");
+
+	/* I2C? and OVF? doesn't seem useful so use ONT? only */
+	for (size_t i = 0; i < sai_output.size(); ++i) {
+		stringstream cmd;
+		cmd << "ONT? " << sai_output[i] << "\n";
+		string rep;
+		command(cmd.str(), rep);
+		/* Sometimes ONT? returns three characters which
+		 * the first one is BELL that should be removed */
+//		if (rep[0] == 0x7)
+//			rep.erase(rep.begin());
+		/* ONT? reply is always two characters */
+		assert(rep.size() == 2);
+
+		int code;
+		convert(rep, code);
+		if (code != 1) {
+			set_state(Tango::MOVING);
+			set_status("The device is in MOVING state");
+			return;
+		}
+	}
+}
+
 void E621::_command(const string &cmd)
 {
 	/* More than one command mnemonic per line is not allowed. (pag 34) */
@@ -655,6 +639,7 @@ void E621::_command(const string &cmd)
 	assert(cmd.size() < 26);
 
 	write(cmd);
+
 	/* When controlling the E-816, timing problems can occur if several commands
 	 * are run in rapid sequence, resulting in lost commands. To prevent such 
 	 * communication errors, it is recommended that you include a certain wait time

src/E621.h

@@ -239,6 +239,7 @@ public:
 
 //	Additional Method prototypes
 	void check_init();
+	void check_state();
 	void verify_command_execution();
 	void _command(const string &cmd);
 	void command(const string &cmd);

src/E621StateMachine.cpp

@@ -42,7 +42,6 @@
 //  States   |  Description
 //================================================================
 //  ON       |  
-//  FAULT    |  
 //  UNKNOWN  |  
 //  ALARM    |  
 //  MOVING   |