(Modbus): Исправил ошибку (сдвиг не единицу) в обработке количества возвращаемых…

Utilities/MBTester/MBSlave.cc

@@ -3,27 +3,20 @@
 //#include <string.h>
 //#include <errno.h>
 #include <sstream>
-#include "UniSetTypes.h"
+#include <UniSetTypes.h>
 #include "MBSlave.h"
-#include "uniset-config.h"
-// -------------------------------------------------------------------------
-#ifndef PACKAGE_URL
-#define PACKAGE_URL "http://git.etersoft.ru/projects/?p=asu/uniset.git;a=summary"
-#endif
 // -------------------------------------------------------------------------
 using namespace std;
 using namespace UniSetTypes;
 using namespace ModbusRTU;
 // -------------------------------------------------------------------------
-MBSlave::MBSlave( ModbusRTU::ModbusAddr addr, const std::string& dev, const std::string& speed, bool use485 ):
+MBSlave::MBSlave( ModbusRTU::ModbusAddr addr, const std::string dev, const std::string speed, bool use485 ):
 	rscomm(NULL),
 	addr(addr),
 //	prev(ModbusRTU::erNoError),
 //	askCount(0),
 	verbose(false),
-	replyVal(-1),
-	replyVal2(-1),
-	replyVal3(-1)
+	replyVal(-1)
 {
 //	int replyTimeout = uni_atoi( conf->getArgParam("--reply-timeout",it.getProp("reply_timeout")).c_str() );
 //	if( replyTimeout <= 0 )
@@ -53,7 +46,6 @@ MBSlave::MBSlave( ModbusRTU::ModbusAddr addr, const std::string& dev, const std:
 	rscomm->connectRemoteService( sigc::mem_fun(this, &MBSlave::remoteService) );
 	rscomm->connectFileTransfer( sigc::mem_fun(this, &MBSlave::fileTransfer) );
 	rscomm->connectDiagnostics( sigc::mem_fun(this, &MBSlave::diagnostics) );
-	rscomm->connectMEIRDI( sigc::mem_fun(this, &MBSlave::read4314) );
 
 
 	rscomm->setRecvTimeout(2000);
@@ -161,16 +153,9 @@ ModbusRTU::mbErrCode MBSlave::readInputStatus( ReadInputStatusMessage& query,
 			bcnt++;
 
 		for( int i=0; i<bcnt; i++ )
-		{
-			if( i == 1 )
-				reply.addData(replyVal2);
-			else if( i == 2 )
-				reply.addData(replyVal3);
-			else
-				reply.addData(replyVal);
-		}
+			reply.addData(replyVal);
 	}
-	
+
 	return ModbusRTU::erNoError;
 }
 // -------------------------------------------------------------------------
@@ -195,14 +180,7 @@ mbErrCode MBSlave::readInputRegisters( ReadInputMessage& query,
 	for( ; num<query.count; num++, reg++ )
 	{
 		if( replyVal != -1 )
-		{
-			if( num == 1 && replyVal2 != -1  )
-				reply.addData(replyVal2);
-			else if( num == 2 && replyVal3 != -1 )
-				reply.addData(replyVal3);
-			else
-				reply.addData(replyVal);
-		}
+			reply.addData(replyVal);
 		else
 			reply.addData(reg);
 	}
@@ -239,14 +217,7 @@ ModbusRTU::mbErrCode MBSlave::readOutputRegisters(
 	for( ; num<query.count; num++, reg++ )
 	{
 		if( replyVal != -1 )
-		{
-			if( num == 1 && replyVal2 != -1  )
-				reply.addData(replyVal2);
-			else if( num == 2 && replyVal3 != -1 )
-				reply.addData(replyVal3);
-			else
-				reply.addData(replyVal);
-		}
+			reply.addData(replyVal);
 		else
 			reply.addData(reg);
 	}
@@ -431,12 +402,12 @@ ModbusRTU::mbErrCode MBSlave::fileTransfer( ModbusRTU::FileTransferMessage& quer
 
 }									
 // -------------------------------------------------------------------------
-ModbusRTU::mbErrCode MBSlave::diagnostics( ModbusRTU::DiagnosticMessage& query, 
+ModbusRTU::mbErrCode MBSlave::diagnostics( ModbusRTU::DiagnosticMessage& query,
 											ModbusRTU::DiagnosticRetMessage& reply )
 {
 	if( verbose )
 		cout << "(diagnostics): " << query << endl;
-	
+
 	if( query.subf == ModbusRTU::subEcho )
 	{
 		reply = query;
@@ -449,7 +420,7 @@ ModbusRTU::mbErrCode MBSlave::diagnostics( ModbusRTU::DiagnosticMessage& query,
 		reply.data[0] = 10;
 		return ModbusRTU::erNoError;
 	}
-	
+
 	if( query.subf == ModbusRTU::dgMsgSlaveCount || query.subf == ModbusRTU::dgBusMsgCount )
 	{
 		reply = query;
@@ -468,73 +439,8 @@ ModbusRTU::mbErrCode MBSlave::diagnostics( ModbusRTU::DiagnosticMessage& query,
 	{
 		reply = query;
 		return ModbusRTU::erNoError;
-	}	
-	
-	return ModbusRTU::erOperationFailed; 
-}
-// -------------------------------------------------------------------------
-ModbusRTU::mbErrCode MBSlave::read4314( ModbusRTU::MEIMessageRDI& query, 
-								ModbusRTU::MEIMessageRetRDI& reply )
-{
-	if( verbose )
-		cout << "(read4314): " << query << endl;
-
-	if( query.devID <= rdevMinNum || query.devID >= rdevMaxNum )
-		return erOperationFailed;
-
-	if( query.objID == rdiVendorName )
-	{
-		reply.mf = 0xFF;
-		reply.conformity = rdevBasicDevice;
-		reply.addData(query.objID,"etersoft");
-//		reply.addData(rdiProductCode, PACKAGE_NAME);
-//		reply.addData(rdiMajorMinorRevision,PACKAGE_VERSION);
-		return erNoError;
-	}
-	else if( query.objID == rdiProductCode )
-	{
-		reply.mf = 0xFF;
-		reply.conformity = rdevBasicDevice;
-		reply.addData(query.objID,PACKAGE_NAME);
-		return erNoError;
-	}
-	else if( query.objID == rdiMajorMinorRevision )
-	{
-		reply.mf = 0xFF;
-		reply.conformity = rdevBasicDevice;
-		reply.addData(query.objID,PACKAGE_VERSION);
-		return erNoError;
-	}
-	else if( query.objID == rdiVendorURL )
-	{
-		reply.mf = 0xFF;
-		reply.conformity = rdevRegularDevice;
-		reply.addData(query.objID,PACKAGE_URL);
-		return erNoError;
 	}
-	else if( query.objID == rdiProductName )
-	{
-		reply.mf = 0xFF;
-		reply.conformity = rdevRegularDevice;
-		reply.addData(query.objID,PACKAGE_NAME);
-		return erNoError;
-	}
-	else if( query.objID == rdiModelName )
-	{
-		reply.mf = 0xFF;
-		reply.conformity = rdevRegularDevice;
-		reply.addData(query.objID,"MBSlaveEcho");
-		return erNoError;
-	}
-	else if( query.objID == rdiUserApplicationName )
-	{
-		reply.mf = 0xFF;
-		reply.conformity = rdevRegularDevice;
-		reply.addData(query.objID,"uniset-mbrtuslave-echo");
-		return erNoError;
-	}
-	return ModbusRTU::erBadDataAddress; 
+
+	return ModbusRTU::erOperationFailed;
 }
 // -------------------------------------------------------------------------
-
-

Utilities/MBTester/mbrtutester.cc

@@ -16,11 +16,10 @@ static struct option longopts[] = {
 	{ "read03", required_argument, 0, 'r' },
 	{ "read04", required_argument, 0, 'x' },
 	{ "write05", required_argument, 0, 'f' }, 
-	{ "write06", required_argument, 0, 'z' }, 
+	{ "write06", required_argument, 0, 'z' },
 	{ "write0F", required_argument, 0, 'm' },
 	{ "write10", required_argument, 0, 'w' },
- 	{ "diag08", required_argument, 0, 'o' },
-	{ "read4314", required_argument, 0, 'e' },
+	{ "diag08", required_argument, 0, 'o' },
 //	{ "readfile14", required_argument, 0, 'g' },
 //	{ "writefile15", required_argument, 0, 'p' },
 	{ "filetransfer66", required_argument, 0, 'u' },
@@ -32,7 +31,7 @@ static struct option longopts[] = {
 	{ "myaddr", required_argument, 0, 'a' },
 	{ "speed", required_argument, 0, 's' },
 	{ "use485F", no_argument, 0, 'y' },
-	{ "num-cycles", required_argument, 0, 'l' },
+	{ "num-cycles", required_argument, 0, 'q' },
 	{ NULL, 0, 0, 0 }
 };
 // --------------------------------------------------------------------------
@@ -48,7 +47,6 @@ static void print_help()
 	printf("[--read03] slaveaddr reg count   - read from reg (from slaveaddr). Default: count=1\n");
 	printf("[--read04] slaveaddr reg count   - read from reg (from slaveaddr). Default: count=1\n");
 	printf("[--diag08] slaveaddr subfunc [dat] - diagnostics request\n");
-	printf("[--read4314] slaveaddr devID objID - (0x2B/0x0E): read device identification (devID=[1...4], objID=[0..255])\n");
 //	printf("[--readfile14] slaveaddr fileID  - read file from slaveaddr).\n");
 //	printf("[--writefile15] slaveaddr id filename  - write file to slaveaddr).\n");
 	printf("[--filetransfer66] slaveaddr fileID [filename] - get file from slaveaddr. Default save to 'fileID.transfer'\n");
@@ -58,7 +56,7 @@ static void print_help()
 	printf("          fn - function of test [0x01,0x02,0x03,0x04]. Default: 0x04\n");
 	printf("[--autodetect-slave] [beg end reg fn]  - find slave\n");
 	printf("          beg - start addres Default: 0\n");
-	printf("          end - end addres Default: 254\n");
+	printf("          end - end addres Default: 255\n");
 	printf("          reg - register of test. Default: 0\n");
 	printf("          fn - function of test [0x01,0x02,0x03,0x04]. Default: 0x04\n");
 	printf("[-y|--use485F]                    - use RS485 Fastwel.\n");
@@ -77,8 +75,6 @@ enum Command
 	cmdRead02,
 	cmdRead03,
 	cmdRead04,
-	cmdRead43_13,
-	cmdRead43_14,
 	cmdWrite05,
 	cmdWrite06,
 	cmdWrite0F,
@@ -109,7 +105,7 @@ int main( int argc, char **argv )
 	ModbusRTU::ModbusAddr slaveaddr = 0x00;
 	ModbusRTU::SlaveFunctionCode fn = ModbusRTU::fnReadInputRegisters;
 	ModbusRTU::ModbusAddr beg = 0;
-	ModbusRTU::ModbusAddr end = 254;
+	ModbusRTU::ModbusAddr end = 255;
 	ModbusRTU::DiagnosticsSubFunction subfunc = ModbusRTU::subEcho;
 	ModbusRTU::ModbusData dat = 0;
 	int tout = 2000;
@@ -117,14 +113,12 @@ int main( int argc, char **argv )
 	string tofile("");
 	int use485 = 0;
 	int ncycles = -1;
-	ModbusRTU::ModbusByte devID = 0;
-	ModbusRTU::ModbusByte objID = 0;
 
 	try
 	{
-		while( (opt = getopt_long(argc, argv, "hva:w:z:m:r:x:c:b:d:s:t:qn:u:yl:t:o:e:",longopts,&optindex)) != -1 ) 
+		while( (opt = getopt_long(argc, argv, "hva:w:z:m:r:x:c:b:d:s:t:qn:u:yl:t:o:",longopts,&optindex)) != -1 )
 		{
-			switch (opt) 
+			switch (opt)
 			{
 				case 'h':
 					print_help();	
@@ -154,14 +148,14 @@ int main( int argc, char **argv )
 						reg = ModbusRTU::str2mbData(argv[optind]);
 					
 					if( checkArg(optind+1,argc,argv) )
-						count = ModbusRTU::str2mbData(argv[optind+1]);
+						dat = uni_atoi(argv[optind+1]);
 				break;
 
 				case 'o':
 
 					cmd = cmdDiag;
 					slaveaddr = ModbusRTU::str2mbAddr( optarg );
-					
+
 					if( !checkArg(optind,argc,argv) )
 					{
 						cerr << "diagnostic command error: bad or no arguments..." << endl;
@@ -169,30 +163,11 @@ int main( int argc, char **argv )
 					}
 					else
 						subfunc = (ModbusRTU::DiagnosticsSubFunction)uni_atoi(argv[optind]);
-						
+
 					if( checkArg(optind+1,argc,argv) )
 						dat = uni_atoi(argv[optind+1]);
 				break;
-				case 'e':
-                {
-					if( cmd == cmdNOP )
-						cmd = cmdRead43_14;
-					
-					slaveaddr = ModbusRTU::str2mbAddr( optarg );
-					
-					if( optind > argc )
-					{
-						cerr << "read command error: bad or no arguments..." << endl;
-						return 1;
-					}
 
-					if( checkArg(optind,argc,argv) )
-						devID = ModbusRTU::str2mbData(argv[optind]);
-					
-					if( checkArg(optind+1,argc,argv) )
-						objID = uni_atoi(argv[optind+1]);
-				}
-				break;
 				case 'f':
 					cmd = cmdWrite05;
 				case 'z':
@@ -346,7 +321,7 @@ int main( int argc, char **argv )
 					fn = (ModbusRTU::SlaveFunctionCode)UniSetTypes::uni_atoi(argv[optind+1]);
 				}
 				break;
-				
+
 				case 'l':
 					ncycles = uni_atoi(optarg);
 				break;
@@ -404,7 +379,7 @@ int main( int argc, char **argv )
 					{
 						ModbusRTU::DataBits b(ret.data[i]);
 					
-						cout << i <<": (" << ModbusRTU::dat2str( reg + 8*i ) << ") = (" 
+						cout << i <<": (" << ModbusRTU::dat2str( reg + 8*i ) << ") = ("
 							<< ModbusRTU::b2str(ret.data[i]) << ") " << b << endl;
 					}
 				}
@@ -428,7 +403,7 @@ int main( int argc, char **argv )
 					{
 						ModbusRTU::DataBits b(ret.data[i]);
 					
-						cout << i <<": (" << ModbusRTU::dat2str( reg + 8*i ) << ") = (" 
+						cout << i <<": (" << ModbusRTU::dat2str( reg + 8*i ) << ") = ("
 							<< ModbusRTU::b2str(ret.data[i]) << ") " << b << endl;
 					}
 				}
@@ -443,7 +418,7 @@ int main( int argc, char **argv )
 							 << " count=" << ModbusRTU::dat2str(count) 
 							 << endl;
 					}
-	
+
 					ModbusRTU::ReadOutputRetMessage ret = mb.read03(slaveaddr,reg,count);
 					if( verb )
 						cout << "(reply): " << ret << endl;
@@ -484,24 +459,6 @@ int main( int argc, char **argv )
 				}
 				break;
 
-				case cmdRead43_14:
-				{
-					if( verb )
-					{
-						cout << "read4314: slaveaddr=" << ModbusRTU::addr2str(slaveaddr)
-							 << " devID=" << ModbusRTU::dat2str(devID) 
-							 << " objID=" << ModbusRTU::dat2str(objID) 
-							 << endl;
-					}
-
-					ModbusRTU::MEIMessageRetRDI ret = mb.read4314(slaveaddr,devID,objID);
-					if( verb )
-						cout << "(reply): " << ret << endl;
-					else	
-						cout << "(reply): devID='" << (int)ret.devID << "' objNum='" << (int)ret.objNum << "'" << endl << ret.dlist << endl;
-				}
-				break;
-
 				case cmdWrite05:
 				{
 					if( verb )
@@ -573,7 +530,7 @@ int main( int argc, char **argv )
 						cout << "(reply): " << ret << endl;
 				}
 				break;
-	
+
 				case cmdDiag:
 				{
 					if( verb )
@@ -590,15 +547,15 @@ int main( int argc, char **argv )
 					cout << "(reply): count=" << ModbusRTU::dat2str(ret.count) << endl;
 					for( int i=0; i<ret.count; i++ )
 					{
-						cout << i <<": (" << ModbusRTU::dat2str( reg + i ) << ") = " << (int)(ret.data[i]) 
-							<< " (" 
-							<< ModbusRTU::dat2str(ret.data[i]) 
-							<< ")" 
+						cout << i <<": (" << ModbusRTU::dat2str( reg + i ) << ") = " << (int)(ret.data[i])
+							<< " ("
+							<< ModbusRTU::dat2str(ret.data[i])
+							<< ")"
 							<< endl;
 					}
 				}
 				break;
-				
+
 				case cmdDetectSlave:
 				{
 					if( verb )
@@ -676,18 +633,18 @@ int main( int argc, char **argv )
             catch( ModbusRTU::mbException& ex )
 			{
 				if( ex.err != ModbusRTU::erTimeOut )
-            		throw;
+			throw ex;
 
-            	cout << "timeout..." << endl;
+		cout << "timeout..." << endl;
             }
-			
+
 			if( ncycles > 0 )
 			{
 				nc--;
 				if( nc <=0 )
 					break;
-			}	
-			
+			}
+
 			msleep(200);
 		}
 	}

Utilities/MBTester/mbtcptester.cc

@@ -14,8 +14,6 @@ static struct option longopts[] = {
 	{ "read02", required_argument, 0, 'b' },
 	{ "read03", required_argument, 0, 'r' },
 	{ "read04", required_argument, 0, 'x' },
-//	{ "read4313", required_argument, 0, 'u' },
-	{ "read4314", required_argument, 0, 'e' },
 	{ "write05", required_argument, 0, 'f' }, 
 	{ "write06", required_argument, 0, 'z' }, 
 	{ "write0F", required_argument, 0, 'm' },
@@ -42,16 +40,14 @@ static void print_help()
 	printf("[--read02] slaveaddr reg count   - read from reg (from slaveaddr). Default: count=1\n");
 	printf("[--read03] slaveaddr reg count   - read from reg (from slaveaddr). Default: count=1\n");
 	printf("[--read04] slaveaddr reg count   - read from reg (from slaveaddr). Default: count=1\n");
-	printf("[--diag08] slaveaddr subfunc [dat]  - diagnostics request\n");
-	printf("[--read4314] slaveaddr devID objID - (0x2B/0x0E): read device identification (devID=[1...4], objID=[0..254])\n");
-//	printf("[--read43-13] slaveaddr ...     - (0x2B/0x0D):  CANopen General Reference Request and Response PDU \n");
+	printf("[--diag08] slaveaddr subfunc [dat] - diagnostics request\n");
 	printf("[-i|--iaddr] ip                 - Modbus server ip. Default: 127.0.0.1\n");
-	printf("[-a|--myaddr] addr              - Modbus address for master. Default: 0x01.\n");
-	printf("[-p|--port] port                - Modbus server port. Default: 502.\n");
-	printf("[-t|--timeout] msec             - Timeout. Default: 2000.\n");
-	printf("[-o|--persistent-connection]    - Use persistent-connection.\n");
-	printf("[-l|--num-cycles] num           - Number of cycles of exchange. Default: -1 - infinitely.\n");
-	printf("[-v|--verbose]                  - Print all messages to stdout\n");
+	printf("[-a|--myaddr] addr                - Modbus address for master. Default: 0x01.\n");
+	printf("[-p|--port] port                  - Modbus server port. Default: 502.\n");
+	printf("[-t|--timeout] msec               - Timeout. Default: 2000.\n");
+	printf("[-o|--persistent-connection]      - Use persistent-connection.\n");
+	printf("[-l|--num-cycles] num             - Number of cycles of exchange. Default: -1 - infinitely.\n");
+	printf("[-v|--verbose]                    - Print all messages to stdout\n");
 }
 // --------------------------------------------------------------------------
 enum Command
@@ -61,8 +57,6 @@ enum Command
 	cmdRead02,
 	cmdRead03,
 	cmdRead04,
-	cmdRead43_13,
-	cmdRead43_14,
 	cmdWrite05,
 	cmdWrite06,
 	cmdWrite0F,
@@ -92,14 +86,12 @@ int main( int argc, char **argv )
 	int tout = 2000;
 	DebugStream dlog;
 	int ncycles = -1;
-	ModbusRTU::ModbusByte devID = 0;
-	ModbusRTU::ModbusByte objID = 0;
 
 	try
 	{
-		while( (opt = getopt_long(argc, argv, "hva:w:z:r:x:c:b:d:s:t:p:i:ol:d:e:u:",longopts,&optindex)) != -1 ) 
+		while( (opt = getopt_long(argc, argv, "hva:w:z:r:x:c:b:d:s:t:p:i:ol:d:",longopts,&optindex)) != -1 )
 		{
-			switch (opt) 
+			switch (opt)
 			{
 				case 'h':
 					print_help();	
@@ -115,7 +107,7 @@ int main( int argc, char **argv )
 					if( cmd == cmdNOP )
 						cmd = cmdRead03;
 				case 'x':
-                {
+
 					if( cmd == cmdNOP )
 						cmd = cmdRead04;
 					slaveaddr = ModbusRTU::str2mbAddr( optarg );
@@ -130,29 +122,8 @@ int main( int argc, char **argv )
 					
 					if( checkArg(optind+1,argc,argv) )
 						count = uni_atoi(argv[optind+1]);
-				}
 				break;
 
-				case 'e':
-                {
-					if( cmd == cmdNOP )
-						cmd = cmdRead43_14;
-					
-					slaveaddr = ModbusRTU::str2mbAddr( optarg );
-					
-					if( optind > argc )
-					{
-						cerr << "read command error: bad or no arguments..." << endl;
-						return 1;
-					}
-
-					if( checkArg(optind,argc,argv) )
-						devID = ModbusRTU::str2mbData(argv[optind]);
-					
-					if( checkArg(optind+1,argc,argv) )
-						objID = uni_atoi(argv[optind+1]);
-				}
-				break;
 				case 'f':
 					cmd = cmdWrite05;
 				case 'z':
@@ -228,7 +199,7 @@ int main( int argc, char **argv )
 
 					cmd = cmdDiag;
 					slaveaddr = ModbusRTU::str2mbAddr( optarg );
-					
+
 					if( !checkArg(optind,argc,argv) )
 					{
 						cerr << "diagnostic command error: bad or no arguments..." << endl;
@@ -236,7 +207,7 @@ int main( int argc, char **argv )
 					}
 					else
 						subfunc = (ModbusRTU::DiagnosticsSubFunction)uni_atoi(argv[optind]);
-						
+
 					if( checkArg(optind+1,argc,argv) )
 						dat = uni_atoi(argv[optind+1]);
 				break;
@@ -251,7 +222,7 @@ int main( int argc, char **argv )
 		if( verb )
 		{
 			cout << "(init): ip=" << iaddr << ":" << port
-					<< " mbaddr=" << ModbusRTU::addr2str(myaddr)
+					<< ModbusRTU::addr2str(myaddr)
 					<< " timeout=" << tout << " msec "
 					<< endl;
 
@@ -303,7 +274,7 @@ int main( int argc, char **argv )
 					{
 						ModbusRTU::DataBits b(ret.data[i]);
 					
-						cout << i <<": (" << ModbusRTU::dat2str( reg + 8*i ) << ") = (" 
+						cout << i <<": (" << ModbusRTU::dat2str( reg + 8*i ) << ") = ("
 							<< ModbusRTU::b2str(ret.data[i]) << ") " << b << endl;
 					}
 				}
@@ -327,7 +298,7 @@ int main( int argc, char **argv )
 					{
 						ModbusRTU::DataBits b(ret.data[i]);
 					
-						cout << i <<": (" << ModbusRTU::dat2str( reg + 8*i ) << ") = (" 
+						cout << i <<": (" << ModbusRTU::dat2str( reg + 8*i ) << ") = ("
 							<< ModbusRTU::b2str(ret.data[i]) << ") " << b << endl;
 					}
 				}
@@ -383,24 +354,6 @@ int main( int argc, char **argv )
 				}
 				break;
 
-				case cmdRead43_14:
-				{
-					if( verb )
-					{
-						cout << "read4314: slaveaddr=" << ModbusRTU::addr2str(slaveaddr)
-							 << " devID=" << ModbusRTU::dat2str(devID) 
-							 << " objID=" << ModbusRTU::dat2str(objID) 
-							 << endl;
-					}
-
-					ModbusRTU::MEIMessageRetRDI ret = mb.read4314(slaveaddr,devID,objID);
-					if( verb )
-						cout << "(reply): " << ret << endl;
-					else	
-						cout << "(reply): devID='" << (int)ret.devID << "' objNum='" << (int)ret.objNum << "'" << endl << ret.dlist << endl;
-				}
-				break;
-
 				case cmdWrite05:
 				{
 					if( verb )
@@ -489,10 +442,10 @@ int main( int argc, char **argv )
 					cout << "(reply): count=" << ModbusRTU::dat2str(ret.count) << endl;
 					for( int i=0; i<ret.count; i++ )
 					{
-						cout << i <<": (" << ModbusRTU::dat2str( reg + i ) << ") = " << (int)(ret.data[i]) 
-							<< " (" 
-							<< ModbusRTU::dat2str(ret.data[i]) 
-							<< ")" 
+						cout << i <<": (" << ModbusRTU::dat2str( reg + i ) << ") = " << (int)(ret.data[i])
+							<< " ("
+							<< ModbusRTU::dat2str(ret.data[i])
+							<< ")"
 							<< endl;
 					}
 				}
@@ -507,7 +460,7 @@ int main( int argc, char **argv )
             catch( ModbusRTU::mbException& ex )
             {
             	if( ex.err != ModbusRTU::erTimeOut )
-            		throw;
+            		throw ex;
 
             	cout << "timeout..." << endl;
             }
@@ -520,7 +473,7 @@ int main( int argc, char **argv )
 			}
 
 			msleep(200);
-			
+
 		} // end of while
 
 		mb.disconnect();

conf/libuniset.spec

@@ -3,7 +3,7 @@
 
 Name: libuniset
 Version: 1.0
-Release: alt55
+Release: alt56
 Summary: UniSet - library for building distributed industrial control systems
 License: GPL
 Group: Development/C++
@@ -212,6 +212,9 @@ rm -f %buildroot%_libdir/*.la
 
 
 %changelog
+* Sat Nov 26 2011 Pavel Vainerman <pv@altlinux.ru> 1.0-alt56
+- (modbus): fixed bug (again) in ModbusSlave::readInputStatus(0x02)
+
 * Sat Nov 26 2011 Pavel Vainerman <pv@altlinux.ru> 1.0-alt55
 - (modbus): fixed bug in ModbusSlave::readInputStatus(0x02)
 

extensions/ModbusSlave/MBSlave.cc

@@ -11,7 +11,7 @@ using namespace UniSetTypes;
 using namespace UniSetExtensions;
 using namespace ModbusRTU;
 // -----------------------------------------------------------------------------
-MBSlave::MBSlave( UniSetTypes::ObjectId objId, UniSetTypes::ObjectId shmId,
+MBSlave::MBSlave( UniSetTypes::ObjectId objId, UniSetTypes::ObjectId shmId, 
 					SharedMemory* ic, string prefix ):
 UniSetObject_LT(objId),
 mbslot(0),
@@ -65,7 +65,7 @@ prefix(prefix)
 	respond_invert = conf->getArgInt("--" + prefix + "-respond-invert",it.getProp("respond_invert"));
 
 	string stype = conf->getArgParam("--" + prefix + "-type",it.getProp("type"));
-
+	
 	if( stype == "RTU" )
 	{
 		// ---------- init RS ----------
@@ -90,7 +90,7 @@ prefix(prefix)
 		mbslot = rs;
 		thr = new ThreadCreator<MBSlave>(this,&MBSlave::execute_rtu);
 
-		dlog[Debug::INFO] << myname << "(init): type=RTU myaddr=" << ModbusRTU::addr2str(addr)
+		dlog[Debug::INFO] << myname << "(init): type=RTU myaddr=" << ModbusRTU::addr2str(addr) 
 			<< " dev=" << dev << " speed=" << speed << endl;
 	}
 	else if( stype == "TCP" )
@@ -98,12 +98,12 @@ prefix(prefix)
 		string iaddr = conf->getArgParam("--" + prefix + "-inet-addr",it.getProp("iaddr"));
 		if( iaddr.empty() )
 			throw UniSetTypes::SystemError(myname+"(MBSlave): Unknown TCP server address. Use: --prefix-inet-addr [ XXX.XXX.XXX.XXX| hostname ]");
-
+		
 		int port = conf->getArgPInt("--" + prefix + "-inet-port",it.getProp("iport"), 502);
 
-		dlog[Debug::INFO] << myname << "(init): type=TCP myaddr=" << ModbusRTU::addr2str(addr)
+		dlog[Debug::INFO] << myname << "(init): type=TCP myaddr=" << ModbusRTU::addr2str(addr) 
 			<< " inet=" << iaddr << " port=" << port << endl;
-
+	
 		ost::InetAddress ia(iaddr.c_str());
 		mbslot	= new ModbusTCPServerSlot(ia,port);
 		thr = new ThreadCreator<MBSlave>(this,&MBSlave::execute_tcp);
@@ -125,7 +125,9 @@ prefix(prefix)
 	if( findArgParam("--" + prefix + "-allow-setdatetime",conf->getArgc(),conf->getArgv())!=-1 )
 		mbslot->connectSetDateTime( sigc::mem_fun(this, &MBSlave::setDateTime) );
 
-	mbslot->connectFileTransfer( sigc::mem_fun(this, &MBSlave::fileTransfer) );
+	mbslot->connectDiagnostics( sigc::mem_fun(this, &MBSlave::diagnostics) );
+	mbslot->connectFileTransfer( sigc::mem_fun(this, &MBSlave::fileTransfer) );	
+
 //	mbslot->connectJournalCommand( sigc::mem_fun(this, &MBSlave::journalCommand) );
 //	mbslot->connectRemoteService( sigc::mem_fun(this, &MBSlave::remoteService) );
 	// -------------------------------
@@ -212,7 +214,7 @@ prefix(prefix)
 					dlog[Debug::WARN] << myname << "(build file list): FAILED ID for " << nm << "... ignore..." << endl;
 					continue;
 				}
-
+			
 				std::string dir = fit.getProp("directory");
 				if( !dir.empty() )
 				{
@@ -226,7 +228,7 @@ prefix(prefix)
 
 				dlog[Debug::INFO] << myname << "(init):       add to filelist: "
 						<< "id=" << id
-						<< " file=" << nm
+						<< " file=" << nm 
 						<< endl;
 
 				flist[id] = nm;
@@ -268,7 +270,7 @@ void MBSlave::waitSMReady()
 void MBSlave::execute_rtu()
 {
 	ModbusRTUSlaveSlot* rscomm = dynamic_cast<ModbusRTUSlaveSlot*>(mbslot);
-
+	
 	while(1)
 	{
 		try
@@ -277,7 +279,7 @@ void MBSlave::execute_rtu()
 
 			if( res!=ModbusRTU::erTimeOut )
 				ptTimeout.reset();
-
+	
 			// собираем статистику обмена
 			if( prev!=ModbusRTU::erTimeOut )
 			{
@@ -285,10 +287,10 @@ void MBSlave::execute_rtu()
 				askCount = askCount>=numeric_limits<long>::max() ? 0 : askCount+1;
 				if( res!=ModbusRTU::erNoError )
 					errmap[res]++;
-
+	
 				prev = res;
 			}
-
+			
 			if( res!=ModbusRTU::erNoError && res!=ModbusRTU::erTimeOut )
 				dlog[Debug::WARN] << myname << "(execute_rtu): " << ModbusRTU::mbErr2Str(res) << endl;
 
@@ -338,7 +340,7 @@ void MBSlave::execute_rtu()
 						<< "(execute_rtu): (askCount) " << ex << std::endl;
 				}
 			}
-
+		
 			for( IOMap::iterator it=iomap.begin(); it!=iomap.end(); ++it )
 				IOBase::processingThreshold(&it->second,shm,force);
 		}
@@ -358,7 +360,7 @@ void MBSlave::execute_tcp()
 
 			if( res!=ModbusRTU::erTimeOut )
 				ptTimeout.reset();
-
+	
 			// собираем статистику обмена
 			if( prev!=ModbusRTU::erTimeOut )
 			{
@@ -366,10 +368,10 @@ void MBSlave::execute_tcp()
 				askCount = askCount>=numeric_limits<long>::max() ? 0 : askCount+1;
 				if( res!=ModbusRTU::erNoError )
 					errmap[res]++;
-
+	
 				prev = res;
 			}
-
+			
 			if( res!=ModbusRTU::erNoError && res!=ModbusRTU::erTimeOut )
 				dlog[Debug::WARN] << myname << "(execute_tcp): " << ModbusRTU::mbErr2Str(res) << endl;
 
@@ -477,9 +479,9 @@ void MBSlave::sysCommand(UniSetTypes::SystemMessage *sm)
 			{
 				dlog[Debug::CRIT] << myname << "(sysCommand): iomap EMPTY! terminated..." << endl;
 				raise(SIGTERM);
-				return;
+				return; 
 			}
-
+		
 			waitSMReady();
 
 			// подождать пока пройдёт инициализация датчиков
@@ -487,16 +489,16 @@ void MBSlave::sysCommand(UniSetTypes::SystemMessage *sm)
 			msleep(initPause);
 			PassiveTimer ptAct(activateTimeout);
 			while( !activated && !ptAct.checkTime() )
-			{
+			{	
 				cout << myname << "(sysCommand): wait activate..." << endl;
 				msleep(300);
 				if( activated )
 					break;
 			}
-
+			
 			if( !activated )
 				dlog[Debug::CRIT] << myname << "(sysCommand): ************* don`t activate?! ************" << endl;
-			else
+			else 
 			{
 				UniSetTypes::uniset_mutex_lock l(mutex_start, 10000);
 				askSensors(UniversalIO::UIONotify);
@@ -509,11 +511,11 @@ void MBSlave::sysCommand(UniSetTypes::SystemMessage *sm)
 		case SystemMessage::Finish:
 			askSensors(UniversalIO::UIODontNotify);
 			break;
-
+		
 		case SystemMessage::WatchDog:
 		{
 			// ОПТИМИЗАЦИЯ (защита от двойного перезаказа при старте)
-			// Если идёт локальная работа
+			// Если идёт локальная работа 
 			// (т.е. MBSlave  запущен в одном процессе с SharedMemory2)
 			// то обрабатывать WatchDog не надо, т.к. мы и так ждём готовности SM
 			// при заказе датчиков, а если SM вылетит, то вместе с этим процессом(MBSlave)
@@ -554,10 +556,10 @@ void MBSlave::askSensors( UniversalIO::UIOCommand cmd )
 	if( !shm->waitSMworking(test_id,activateTimeout,50) )
 	{
 		ostringstream err;
-		err << myname
-			<< "(askSensors): Не дождались готовности(work) SharedMemory к работе в течение "
+		err << myname 
+			<< "(askSensors): Не дождались готовности(work) SharedMemory к работе в течение " 
 			<< activateTimeout << " мсек";
-
+	
 		dlog[Debug::CRIT] << err.str() << endl;
 		kill(SIGTERM,getpid());	// прерываем (перезапускаем) процесс...
 		throw SystemError(err.str());
@@ -570,7 +572,7 @@ void MBSlave::askSensors( UniversalIO::UIOCommand cmd )
 	for( ; it!=iomap.end(); ++it )
 	{
 		IOProperty* p(&it->second);
-
+		
 //		if( p->stype != UniversalIO::DigitalOutput && p->stype != UniversalIO::AnalogOutput )
 //			continue;
 
@@ -608,14 +610,38 @@ void MBSlave::sensorInfo( UniSetTypes::SensorMessage* sm )
 				uniset_spin_lock lock(p->val_lock);
 				p->value = sm->value;
 			}
-			break;
+
+			int sz = VTypes::wsize(p->vtype);
+			if( sz < 1 )
+				return;
+
+			// если размер больше одного слова
+			// то надо обновить значение "везде"
+			// они если "всё верно инициализировано" идут подряд
+			int i=0;
+			for( ;i<sz && it!=iomap.end(); i++,it++ )
+			{
+				p  = &it->second;
+				if( p->si.id == sm->id )
+					p->value = sm->value;
+			}
+
+			if( dlog.debugging(Debug::CRIT) )
+			{
+				// вообще этого не может случиться
+				// потому-что корректность проверяется при загрузке
+				if( i != sz )
+					dlog[Debug::CRIT] << myname << "(sensorInfo): update failed for sid=" << sm->id
+						<< " (i=" << i << " sz=" << sz << ")" << endl;
+			}
+			return;
 		}
 	}
 }
 // ------------------------------------------------------------------------------------------
 bool MBSlave::activateObject()
 {
-	// блокирование обработки Starsp
+	// блокирование обработки Starsp 
 	// пока не пройдёт инициализация датчиков
 	// см. sysCommand()
 	{
@@ -639,7 +665,7 @@ void MBSlave::sigterm( int signo )
 			mbslot->sigterm(signo);
 	}
 	catch(...){}
-
+	
 	UniSetObject_LT::sigterm(signo);
 }
 // ------------------------------------------------------------------------------------------
@@ -666,7 +692,7 @@ void MBSlave::readConfiguration()
 		if( UniSetTypes::check_filter(it,s_field,s_fvalue) )
 			initItem(it);
 	}
-
+	
 //	readconf_ok = true;
 }
 // ------------------------------------------------------------------------------------------
@@ -695,10 +721,10 @@ bool MBSlave::initItem( UniXML_iterator& it )
 			dlog[Debug::CRIT] << myname << "(initItem): Unknown 'mbreg' for " << it.getProp("name") << endl;
 			return false;
 		}
-
+		
 		p.mbreg = ModbusRTU::str2mbData(r);
 	}
-
+	
 	string stype( it.getProp("mb_iotype") );
 	if( stype.empty() )
 		stype = it.getProp("iotype");
@@ -732,8 +758,8 @@ bool MBSlave::initItem( UniXML_iterator& it )
 		VTypes::VType v(VTypes::str2type(vt));
 		if( v == VTypes::vtUnknown )
 		{
-			dlog[Debug::CRIT] << myname << "(initItem): Unknown rtuVType=" << vt << " for "
-					<< it.getProp("name")
+			dlog[Debug::CRIT] << myname << "(initItem): Unknown rtuVType=" << vt << " for " 
+					<< it.getProp("name") 
 					<< endl;
 
 			return false;
@@ -749,7 +775,7 @@ bool MBSlave::initItem( UniXML_iterator& it )
 				dlog[Debug::INFO] << myname << "(initItem): add " << p << endl;
 		}
 	}
-
+	
 	return true;
 }
 // ------------------------------------------------------------------------------------------
@@ -772,7 +798,7 @@ void MBSlave::help_print( int argc, const char* const* argv )
 	cout << "Default: prefix='mbs'" << endl;
 	cout << "--prefix-heartbeat-id		- Данный процесс связан с указанным аналоговым heartbeat-дачиком." << endl;
 	cout << "--prefix-heartbeat-max  	- Максимальное значение heartbeat-счётчика для данного процесса. По умолчанию 10." << endl;
-	cout << "--prefix-ready-timeout	- Время ожидания готовности SM к работе, мсек. (-1 - ждать 'вечно')" << endl;
+	cout << "--prefix-ready-timeout	- Время ожидания готовности SM к работе, мсек. (-1 - ждать 'вечно')" << endl;    
 	cout << "--prefix-initPause		- Задержка перед инициализацией (время на активизация процесса)" << endl;
 	cout << "--prefix-respond-id - respond sensor id" << endl;
 	cout << "--prefix-respond-invert [0|1] - invert respond logic" << endl;
@@ -804,7 +830,7 @@ MBSlave* MBSlave::init_mbslave( int argc, const char* const* argv, UniSetTypes::
 	ObjectId ID = conf->getObjectID(name);
 	if( ID == UniSetTypes::DefaultObjectId )
 	{
-		cerr << "(mbslave): идентификатор '" << name
+		cerr << "(mbslave): идентификатор '" << name 
 			<< "' не найден в конф. файле!"
 			<< " в секции " << conf->getObjectsSection() << endl;
 		return 0;
@@ -827,12 +853,12 @@ std::ostream& operator<<( std::ostream& os, MBSlave::IOProperty& p )
 	{
 		os 	<< p.cal
 			<< " cdiagram=" << ( p.cdiagram ? "yes" : "no" );
-	}
-
+	}		
+	
 	return os;
 }
 // -----------------------------------------------------------------------------
-ModbusRTU::mbErrCode MBSlave::readOutputRegisters( ModbusRTU::ReadOutputMessage& query,
+ModbusRTU::mbErrCode MBSlave::readOutputRegisters( ModbusRTU::ReadOutputMessage& query, 
 													ModbusRTU::ReadOutputRetMessage& reply )
 {
 	if( dlog.debugging(Debug::INFO) )
@@ -856,7 +882,7 @@ ModbusRTU::mbErrCode MBSlave::readOutputRegisters( ModbusRTU::ReadOutputMessage&
 		for( int i=0; i<query.count; i++ )
 			reply.addData( buf[i] );
 	}
-
+	
 	return ret;
 }
 
@@ -870,7 +896,7 @@ ModbusRTU::mbErrCode MBSlave::writeOutputRegisters( ModbusRTU::WriteOutputMessag
 	// Формирование ответа:
 	ModbusRTU::mbErrCode ret = much_real_write(query.start,query.data,query.quant);
 	if( ret == ModbusRTU::erNoError )
-		reply.set(query.start,query.quant);
+        	reply.set(query.start,query.quant); 
 	return ret;
 }
 // -------------------------------------------------------------------------
@@ -887,9 +913,14 @@ ModbusRTU::mbErrCode MBSlave::writeOutputSingleRegister( ModbusRTU::WriteSingleO
 	return ret;
 }
 // -------------------------------------------------------------------------
-ModbusRTU::mbErrCode MBSlave::much_real_write( ModbusRTU::ModbusData reg, ModbusRTU::ModbusData* dat,
+ModbusRTU::mbErrCode MBSlave::much_real_write( ModbusRTU::ModbusData reg, ModbusRTU::ModbusData* dat, 
 						int count )
-{
+{	if( dlog.debugging(Debug::INFO) )
+	{
+		dlog[Debug::INFO] << myname << "(much_real_write): read mbID="
+			<< ModbusRTU::dat2str(reg) << " count=" << count << endl;
+	}
+
 	int i=0;
 	IOMap::iterator it = iomap.end();
 	for( ; i<count; i++ )
@@ -913,7 +944,7 @@ ModbusRTU::mbErrCode MBSlave::much_real_write( ModbusRTU::ModbusData reg, Modbus
 			it++;
 		}
 	}
-
+	
 	return ModbusRTU::erNoError;
 }
 // -------------------------------------------------------------------------
@@ -921,8 +952,8 @@ ModbusRTU::mbErrCode MBSlave::real_write( ModbusRTU::ModbusData reg, ModbusRTU::
 {
 	if( dlog.debugging(Debug::INFO) )
 	{
-		dlog[Debug::INFO] << myname << "(write): save mbID="
-			<< ModbusRTU::dat2str(reg)
+		dlog[Debug::INFO] << myname << "(write): save mbID=" 
+			<< ModbusRTU::dat2str(reg) 
 			<< " data=" << ModbusRTU::dat2str(mbval)
 			<< "(" << (int)mbval << ")" << endl;
 	}
@@ -967,7 +998,7 @@ ModbusRTU::mbErrCode MBSlave::real_write_it( IOMap::iterator& it, ModbusRTU::Mod
 			long val = (signed short)(mbval);
 			IOBase::processingAsAI( p, val, shm, force );
 		}
-/*
+/*		
 		else if( p->vtype == VTypes::vtByte )
 		{
 			VTypes::Byte b(r->mbval);
@@ -980,10 +1011,10 @@ ModbusRTU::mbErrCode MBSlave::real_write_it( IOMap::iterator& it, ModbusRTU::Mod
 			ModbusRTU::ModbusData* data = new ModbusRTU::ModbusData[VTypes::F2::wsize()];
 				for( int k=0; k<VTypes::F2::wsize(); k++, i++ )
 					data[k] = i->second->mbval;
-
+				
 				VTypes::F2 f(data,VTypes::F2::wsize());
 				delete[] data;
-
+			
 				IOBase::processingFasAI( p, (float)f, shm, force );
 			}
 			else if( p->vtype == VTypes::vtF4 )
@@ -993,10 +1024,10 @@ ModbusRTU::mbErrCode MBSlave::real_write_it( IOMap::iterator& it, ModbusRTU::Mod
 				ModbusRTU::ModbusData* data = new ModbusRTU::ModbusData[VTypes::F4::wsize()];
 				for( int k=0; k<VTypes::F4::wsize(); k++, i++ )
 					data[k] = i->second->mbval;
-
+				
 				VTypes::F4 f(data,VTypes::F4::wsize());
 				delete[] data;
-
+				
 				IOBase::processingFasAI( p, (float)f, shm, force );
 			}
 */
@@ -1043,20 +1074,20 @@ ModbusRTU::mbErrCode MBSlave::real_write_it( IOMap::iterator& it, ModbusRTU::Mod
 		if( pingOK )
 			dlog[Debug::CRIT] << myname << "(write) catch ..." << endl;
 	}
-
+	
 	pingOK = false;
 	return ModbusRTU::erTimeOut;
 }
 // -------------------------------------------------------------------------
-ModbusRTU::mbErrCode MBSlave::much_real_read( ModbusRTU::ModbusData reg, ModbusRTU::ModbusData* dat,
+ModbusRTU::mbErrCode MBSlave::much_real_read( ModbusRTU::ModbusData reg, ModbusRTU::ModbusData* dat, 
 						int count )
 {
 	if( dlog.debugging(Debug::INFO) )
 	{
-		dlog[Debug::INFO] << myname << "(mush_real_read): read mbID="
+		dlog[Debug::INFO] << myname << "(much_real_read): read mbID="
 			<< ModbusRTU::dat2str(reg) << " count=" << count << endl;
 	}
-
+	
 	IOMap::iterator it = iomap.end();
 	int i=0;
 	for( ; i<count; i++ )
@@ -1073,7 +1104,7 @@ ModbusRTU::mbErrCode MBSlave::much_real_read( ModbusRTU::ModbusData reg, ModbusR
 
 	if( it == iomap.end() )
 		return ModbusRTU::erBadDataAddress;
-
+	
 	ModbusRTU::ModbusData val=0;
 	for( ; (it!=iomap.end()) && (i<count); i++,reg++ )
 	{
@@ -1093,7 +1124,7 @@ ModbusRTU::mbErrCode MBSlave::much_real_read( ModbusRTU::ModbusData reg, ModbusR
 	{
 		for( ; i<count; i++ )
 			dat[i] = 0;
-	}
+	}	
 
 	return ModbusRTU::erNoError;
 }
@@ -1102,7 +1133,7 @@ ModbusRTU::mbErrCode MBSlave::real_read( ModbusRTU::ModbusData reg, ModbusRTU::M
 {
 	if( dlog.debugging(Debug::INFO) )
 	{
-		dlog[Debug::INFO] << myname << "(real_read): read mbID="
+		dlog[Debug::INFO] << myname << "(real_read): read mbID=" 
 			<< ModbusRTU::dat2str(reg) << endl;
 	}
 
@@ -1122,14 +1153,14 @@ ModbusRTU::mbErrCode MBSlave::real_read_it( IOMap::iterator& it, ModbusRTU::Modb
 			dlog[Debug::INFO] << myname << "(real_read_it): read mbID="
 				<< ModbusRTU::dat2str(it->first) << endl;
 		}
-
+				
 		IOProperty* p(&it->second);
 		val = 0;
 
 		if( p->amode == MBSlave::amWO )
 			return ModbusRTU::erBadDataAddress;
-
-		if( p->stype == UniversalIO::DigitalInput ||
+		
+		if( p->stype == UniversalIO::DigitalInput || 
 			p->stype == UniversalIO::DigitalOutput )
 		{
 			val = IOBase::processingAsDO(p,shm,force) ? 1 : 0;
@@ -1140,7 +1171,7 @@ ModbusRTU::mbErrCode MBSlave::real_read_it( IOMap::iterator& it, ModbusRTU::Modb
 			if( p->vtype == VTypes::vtUnknown )
 			{
 				val = IOBase::processingAsAO(p,shm,force);
-			}
+			}		
 			if( p->vtype == VTypes::vtF2 )
 			{
 				float f = IOBase::processingFasAO(p,shm,force);
@@ -1216,7 +1247,7 @@ ModbusRTU::mbErrCode MBSlave::real_read_it( IOMap::iterator& it, ModbusRTU::Modb
 		if( pingOK )
 			dlog[Debug::CRIT] << myname << "(real_read_it) catch ..." << endl;
 	}
-
+	
 	pingOK = false;
 	return ModbusRTU::erTimeOut;
 }
@@ -1235,7 +1266,7 @@ mbErrCode MBSlave::readInputRegisters( ReadInputMessage& query, ReadInputRetMess
 			reply.addData(d);
 		else
 			reply.addData(0);
-
+		
 		return ret;
 	}
 
@@ -1246,24 +1277,24 @@ mbErrCode MBSlave::readInputRegisters( ReadInputMessage& query, ReadInputRetMess
 		for( int i=0; i<query.count; i++ )
 			reply.addData( buf[i] );
 	}
-
+	
 	return ret;
 }
 // -------------------------------------------------------------------------
-ModbusRTU::mbErrCode MBSlave::setDateTime( ModbusRTU::SetDateTimeMessage& query,
+ModbusRTU::mbErrCode MBSlave::setDateTime( ModbusRTU::SetDateTimeMessage& query, 
 									ModbusRTU::SetDateTimeRetMessage& reply )
 {
 	return ModbusServer::replySetDateTime(query,reply,&dlog);
 }
 // -------------------------------------------------------------------------
-ModbusRTU::mbErrCode MBSlave::remoteService( ModbusRTU::RemoteServiceMessage& query,
+ModbusRTU::mbErrCode MBSlave::remoteService( ModbusRTU::RemoteServiceMessage& query, 
 									ModbusRTU::RemoteServiceRetMessage& reply )
 {
 //	cerr << "(remoteService): " << query << endl;
 	return ModbusRTU::erOperationFailed;
-}
+}									
 // -------------------------------------------------------------------------
-ModbusRTU::mbErrCode MBSlave::fileTransfer( ModbusRTU::FileTransferMessage& query,
+ModbusRTU::mbErrCode MBSlave::fileTransfer( ModbusRTU::FileTransferMessage& query, 
 									ModbusRTU::FileTransferRetMessage& reply )
 {
 	if( dlog.debugging(Debug::INFO) )
@@ -1275,16 +1306,16 @@ ModbusRTU::mbErrCode MBSlave::fileTransfer( ModbusRTU::FileTransferMessage& quer
 
 	std::string fname(it->second);
 	return ModbusServer::replyFileTransfer( fname,query,reply,&dlog );
-}
+}									
 // -------------------------------------------------------------------------
-ModbusRTU::mbErrCode MBSlave::readCoilStatus( ReadCoilMessage& query,
+ModbusRTU::mbErrCode MBSlave::readCoilStatus( ReadCoilMessage& query, 
 												ReadCoilRetMessage& reply )
 {
 //	cout << "(readInputStatus): " << query << endl;
 	return ModbusRTU::erOperationFailed;
 }
 // -------------------------------------------------------------------------
-ModbusRTU::mbErrCode MBSlave::readInputStatus( ReadInputStatusMessage& query,
+ModbusRTU::mbErrCode MBSlave::readInputStatus( ReadInputStatusMessage& query, 
 												ReadInputStatusRetMessage& reply )
 {
 	if( dlog.debugging(Debug::INFO) )
@@ -1297,40 +1328,25 @@ ModbusRTU::mbErrCode MBSlave::readInputStatus( ReadInputStatusMessage& query,
 			ModbusRTU::ModbusData d = 0;
 			ModbusRTU::mbErrCode ret = real_read(query.start,d);
 			if( ret == ModbusRTU::erNoError )
-				reply.addData(d);
+				reply.setBit(0,0,d);
 			else
-				reply.addData(0);
-
-
+				reply.setBit(0,0,0);
+			
 			pingOK = true;
 			return ret;
 		}
 
 		// Фомирование ответа:
 		much_real_read(query.start,buf,query.count);
-		for( int i=0; i<query.count; i++ )
-			reply.addData( buf[i] );
-/*
-		int num=0; // добавленное количество данных
-		ModbusRTU::ModbusData d = 0;
-		ModbusRTU::ModbusData reg = query.start;
-		for( ; num<query.count; num++, reg++ )
+		int bnum = 0;
+		int i=0;
+		while( i<query.count )
 		{
-			ModbusRTU::mbErrCode ret = real_read(reg,d);
-			if( ret == ModbusRTU::erNoError )
-				reply.addData(d);
-			else
-				reply.addData(0);
+			reply.addData(0);
+			for( int nbit=0; nbit<BitsPerByte && i<query.count; nbit++,i++ )
+				reply.setBit(bnum,nbit,buf[i]);
+			bnum++;
 		}
-*/
-		// Если мы в начале проверили, что запрос входит в разрешёный диапазон
-		// то теоретически этой ситуации возникнуть не может...
-//		if( reply.bcnt < query.count )
-//		{
-//			dlog[Debug::WARN] << myname
-//				<< "(readInputStatus): query.count=" << query.count
-//					<< " > reply.count=" << reply.count << endl;
-//		}
 
 		pingOK = true;
 		return ModbusRTU::erNoError;
@@ -1356,17 +1372,17 @@ ModbusRTU::mbErrCode MBSlave::readInputStatus( ReadInputStatusMessage& query,
 		if( pingOK )
 			dlog[Debug::CRIT] << myname << "(readInputStatus): catch ..." << endl;
 	}
-
+	
 	pingOK = false;
 	return ModbusRTU::erTimeOut;
 }
 // -------------------------------------------------------------------------
-ModbusRTU::mbErrCode MBSlave::forceMultipleCoils( ModbusRTU::ForceCoilsMessage& query,
+ModbusRTU::mbErrCode MBSlave::forceMultipleCoils( ModbusRTU::ForceCoilsMessage& query, 
 													ModbusRTU::ForceCoilsRetMessage& reply )
 {
 //	cout << "(forceMultipleCoils): " << query << endl;
 	return ModbusRTU::erOperationFailed;
-}
+}													
 // -------------------------------------------------------------------------
 ModbusRTU::mbErrCode MBSlave::forceSingleCoil( ModbusRTU::ForceSingleCoilMessage& query,
 											ModbusRTU::ForceSingleCoilRetMessage& reply )
@@ -1375,3 +1391,46 @@ ModbusRTU::mbErrCode MBSlave::forceSingleCoil( ModbusRTU::ForceSingleCoilMessage
 	return ModbusRTU::erOperationFailed;
 }
 // -------------------------------------------------------------------------
+ModbusRTU::mbErrCode MBSlave::diagnostics( ModbusRTU::DiagnosticMessage& query,
+											ModbusRTU::DiagnosticRetMessage& reply )
+{
+	if( query.subf == ModbusRTU::subEcho )
+	{
+		reply = query;
+		return ModbusRTU::erNoError;
+	}
+
+	if( query.subf == ModbusRTU::dgBusErrCount )
+	{
+		reply = query;
+		reply.data[0] = errmap[ModbusRTU::erBadCheckSum];
+		return ModbusRTU::erNoError;
+	}
+
+	if( query.subf == ModbusRTU::dgMsgSlaveCount || query.subf == ModbusRTU::dgBusMsgCount )
+	{
+		reply = query;
+		reply.data[0] = askCount;
+		return ModbusRTU::erNoError;
+	}
+
+	if( query.subf == ModbusRTU::dgSlaveNAKCount )
+	{
+		reply = query;
+		reply.data[0] = errmap[erOperationFailed];
+		return ModbusRTU::erNoError;
+	}
+
+	if( query.subf == ModbusRTU::dgClearCounters )
+	{
+		askCount = 0;
+		errmap[erOperationFailed] = 0;
+		errmap[ModbusRTU::erBadCheckSum] = 0;
+		// другие счётчики пока не сбрасываем..
+		reply = query;
+		return ModbusRTU::erNoError;
+	}
+
+	return ModbusRTU::erOperationFailed;
+}
+// -------------------------------------------------------------------------