(modbus:0x08) Сделал предварительную реализацию в slave-echo и в mbrtutester

Utilities/MBTester/MBSlave.cc

@@ -45,6 +45,8 @@ MBSlave::MBSlave( ModbusRTU::ModbusAddr addr, const std::string dev, const std::
 	rscomm->connectSetDateTime( sigc::mem_fun(this, &MBSlave::setDateTime) );
 	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->setRecvTimeout(2000);
 //	rscomm->setAfterSendPause(afterSend);
@@ -390,3 +392,16 @@ ModbusRTU::mbErrCode MBSlave::fileTransfer( ModbusRTU::FileTransferMessage& quer
 
 }									
 // -------------------------------------------------------------------------
+ModbusRTU::mbErrCode MBSlave::diagnostics( ModbusRTU::DiagnosticMessage& query, 
+											ModbusRTU::DiagnosticRetMessage& reply )
+{
+	if( query.subf == ModbusRTU::subEcho )
+	{
+		reply = query;
+		return ModbusRTU::erNoError;
+	}
+	
+	return ModbusRTU::erOperationFailed; 
+}
+// -------------------------------------------------------------------------
+

Utilities/MBTester/MBSlave.h

@@ -80,6 +80,8 @@ class MBSlave
 		ModbusRTU::mbErrCode fileTransfer( ModbusRTU::FileTransferMessage& query, 
 															ModbusRTU::FileTransferRetMessage& reply );
 
+		ModbusRTU::mbErrCode diagnostics( ModbusRTU::DiagnosticMessage& query, 
+														ModbusRTU::DiagnosticRetMessage& reply );
 		
 		/*! интерфейс ModbusRTUSlave для обмена по RS */
 		ModbusRTUSlaveSlot* rscomm;

Utilities/MBTester/mbrtutester.cc

@@ -46,7 +46,7 @@ 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   	 - diagnostics request\n");
+	printf("[--diag08] slaveaddr subfunc [dat] - diagnostics request\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");
@@ -107,6 +107,7 @@ int main( int argc, char **argv )
 	ModbusRTU::ModbusAddr beg = 0;
 	ModbusRTU::ModbusAddr end = 255;
 	ModbusRTU::DiagnosticsSubFunction subfunc = ModbusRTU::subEcho;
+	ModbusRTU::ModbusData dat = 0;
 	int tout = 2000;
 	DebugStream dlog;
 	string tofile("");
@@ -147,7 +148,7 @@ int main( int argc, char **argv )
 						reg = ModbusRTU::str2mbData(argv[optind]);
 					
 					if( checkArg(optind+1,argc,argv) )
-						count = uni_atoi(argv[optind+1]);
+						dat = uni_atoi(argv[optind+1]);
 				break;
 
 				case 'o':
@@ -162,6 +163,9 @@ int main( int argc, char **argv )
 					}
 					else
 						subfunc = (ModbusRTU::DiagnosticsSubFunction)uni_atoi(argv[optind]);
+						
+					if( checkArg(optind+1,argc,argv) )
+						count = uni_atoi(argv[optind+1]);
 				break;
 
 				case 'f':
@@ -533,10 +537,11 @@ int main( int argc, char **argv )
 					{
 						cout << "diag08: slaveaddr=" << ModbusRTU::addr2str(slaveaddr)
 							 << " subfunc=" << ModbusRTU::dat2str(subfunc) << "(" << (int)subfunc << ")"
+							 << " dat=" << ModbusRTU::dat2str(dat) << "(" << (int)dat << ")"
 							 << endl;
 					}
 
-					ModbusRTU::DiagnosticRetMessage ret = mb.diag08(slaveaddr,subfunc);
+					ModbusRTU::DiagnosticRetMessage ret = mb.diag08(slaveaddr,subfunc,dat);
 					if( verb )
 						cout << "(reply): " << ret << endl;
 					cout << "(reply): count=" << ModbusRTU::dat2str(ret.count) << endl;

include/modbus/ModbusClient.h

@@ -84,7 +84,8 @@ class ModbusClient
 
 		/*! Диагностика (0x08) */
 		ModbusRTU::DiagnosticRetMessage diag08( ModbusRTU::ModbusAddr addr,
-												ModbusRTU::DiagnosticsSubFunction subfunc )
+												ModbusRTU::DiagnosticsSubFunction subfunc,
+												ModbusRTU::ModbusData dat=0 )
 														throw(ModbusRTU::mbException);
 
 		/*! Установить системное время (0x50)

include/modbus/ModbusTypes.h

@@ -966,7 +966,7 @@ namespace ModbusRTU
 		ModbusCRC crc;
 		
 		// ------- to master -------
-		DiagnosticMessage( ModbusAddr _from, DiagnosticsSubFunction subf );
+		DiagnosticMessage( ModbusAddr _from, DiagnosticsSubFunction subf, ModbusData d=0 );
 
 		/*! добавление данных.
 		 * \return TRUE - если удалось
@@ -1005,6 +1005,7 @@ namespace ModbusRTU
 	{
 		DiagnosticRetMessage( ModbusMessage& m );
 		DiagnosticRetMessage( DiagnosticMessage& m );
+		DiagnosticRetMessage( ModbusAddr a, DiagnosticsSubFunction subf, ModbusData d=0 );
 	};
 
 	std::ostream& operator<<(std::ostream& os, DiagnosticRetMessage& m );

src/Communications/Modbus/ModbusClient.cc

@@ -149,10 +149,11 @@ WriteOutputRetMessage ModbusClient::write10( WriteOutputMessage& msg )
 }
 // --------------------------------------------------------------------------------
 DiagnosticRetMessage ModbusClient::diag08( ModbusAddr addr,
-											DiagnosticsSubFunction subfunc )
+											DiagnosticsSubFunction subfunc,
+											ModbusRTU::ModbusData dat )
 												throw(ModbusRTU::mbException)
 {
-	DiagnosticMessage msg(addr,subfunc);
+	DiagnosticMessage msg(addr,subfunc,dat);
 	qbuf = msg.transport_msg();
 	mbErrCode res = query(msg.addr,qbuf,reply,replyTimeOut_ms);
 
@@ -459,6 +460,12 @@ mbErrCode ModbusClient::recv_pdu( ModbusByte qfunc, ModbusMessage& rbuf, timeout
 					rbuf.len -= szCRC;
 			break;
 
+			case fnDiagnostics:
+				rbuf.len = DiagnosticRetMessage::szHead();
+				if( crcNoCheckit )
+					rbuf.len -= szCRC;
+			break;
+
 			case fnSetDateTime:
 				rbuf.len = SetDateTimeRetMessage::szData();
 				if( crcNoCheckit )
@@ -830,6 +837,61 @@ mbErrCode ModbusClient::recv_pdu( ModbusByte qfunc, ModbusMessage& rbuf, timeout
 			}
 			return erNoError;
 		}
+		else if( rbuf.func == fnDiagnostics )
+		{
+			int szDataLen = DiagnosticRetMessage::getDataLen(rbuf)+szCRC;
+
+			if( crcNoCheckit )
+				szDataLen -= szCRC;
+			
+			// Мы получили только предварительный загловок
+			// Теперь необходимо дополучить данные 
+			// (c позиции rlen, т.к. часть уже получили)
+			int rlen1 = getNextData((unsigned char*)(&(rbuf.data[rlen])),szDataLen);
+			if( rlen1 < szDataLen )
+			{
+				rbuf.len = bcnt + rlen1 - szModbusHeader;
+				if( dlog.debugging(Debug::WARN) )
+				{
+					dlog[Debug::WARN] << "(0x08): buf: " << rbuf << endl;
+					dlog[Debug::WARN] << "(0x08)(" 
+						<< (int)rbuf.func << "):(fnDiagnostics) "
+						<< "Получили данных меньше чем ждали...(" 
+						<< rlen1 << " < " << szDataLen << ")" << endl;
+				}
+
+				cleanupChannel();
+				return erInvalidFormat;
+			}
+			
+			bcnt += rlen1;
+			rbuf.len = bcnt - szModbusHeader;
+
+			DiagnosticRetMessage mDiag(rbuf);
+
+			if( dlog.debugging(Debug::INFO) )
+				dlog[Debug::INFO] << "(recv)(fnDiagnostics): recv buf: " << rbuf << endl;
+
+			if( crcNoCheckit )
+				return erNoError;
+
+			// Проверяем контрольную сумму
+			// от начала(включая заголовок) 
+			// и до конца (исключив последний элемент содержащий CRC)
+			// int mlen = szModbusHeader + mWrite.szHead()+ mWrite.bcnt;
+			ModbusData tcrc = checkCRC((ModbusByte*)(&rbuf),bcnt-szCRC);
+			if( tcrc != mDiag.crc )
+			{
+				ostringstream err;
+				err << "(recv:fnDiagnostics): bad crc. calc.crc=" << dat2str(tcrc)
+					<< " msg.crc=" << dat2str(mDiag.crc);
+				if( dlog.debugging(Debug::WARN) )
+					dlog[Debug::WARN] << err.str() << endl;
+				return erBadCheckSum;
+			}
+
+			return erNoError;
+		}
 		else if( rbuf.func == fnSetDateTime )
 		{
 			SetDateTimeRetMessage mSet(rbuf);

src/Communications/Modbus/ModbusServer.cc

@@ -256,6 +256,36 @@ mbErrCode ModbusServer::processing( ModbusMessage& buf )
 		// --------------------------------
 		return res;
 	}
+	else if( buf.func == fnDiagnostics )
+	{
+		DiagnosticMessage mDiag(buf);
+		DiagnosticRetMessage reply(buf.addr, (DiagnosticsSubFunction)mDiag.subf );
+		mbErrCode res = diagnostics( mDiag, reply );
+		// в случае ошибок ответа не посылаем
+		if( res != erNoError )
+		{
+			dlog[Debug::WARN] << "(0x08): reply: " << mbErr2Str(res) << endl;
+
+			// Если ошибка подразумевает посылку ответа с сообщением об ошибке
+			// то посылаем
+			if( res < erInternalErrorCode )
+			{
+				ErrorRetMessage em( mDiag.addr, mDiag.func, res );
+				buf = em.transport_msg();
+				return send(buf);
+			}
+		
+			return res;
+		}
+
+		// отвечаем (используя тотже буфер, который будет очищен!!!)...
+		buf = reply.transport_msg();
+		// -----------------------------------
+		res = send(buf);
+		printProcessingTime();
+		// --------------------------------
+		return res;
+	}
 	else if( buf.func == fnForceSingleCoil )
 	{
 		ForceSingleCoilMessage mWrite(buf);
@@ -587,6 +617,10 @@ mbErrCode ModbusServer::recv_pdu( ModbusMessage& rbuf, timeout_t timeout )
 				rbuf.len = WriteSingleOutputMessage::szHead(); 
 			break;
 	
+			case fnDiagnostics:
+				rbuf.len = DiagnosticMessage::szHead();
+			break;
+			
 			case fnJournalCommand:
 				rbuf.len = JournalCommandMessage::szData();
 				if( crcNoCheckit )
@@ -1007,6 +1041,72 @@ mbErrCode ModbusServer::recv_pdu( ModbusMessage& rbuf, timeout_t timeout )
 
 			return erNoError;			
 		}
+		else if( rbuf.func == fnDiagnostics ) 
+		{
+			int szDataLen = DiagnosticMessage::getDataLen(rbuf)+szCRC;
+
+			if( crcNoCheckit )
+				szDataLen -= szCRC;
+			
+			// Мы получили только предварительный загловок
+			// Теперь необходимо дополучить данные 
+			// (c позиции rlen, т.к. часть уже получили)
+			int rlen1 = getNextData((unsigned char*)(&(rbuf.data[rlen])),szDataLen);
+			if( rlen1 < szDataLen )
+			{
+				if( dlog.debugging(Debug::WARN) )
+				{
+					rbuf.len = bcnt + rlen1 - szModbusHeader;
+					dlog[Debug::WARN] << "(0x08): buf: " << rbuf << endl;
+					dlog[Debug::WARN] << "(0x08)(" 
+						<< rbuf.func << "):(fnDiagnostics) "
+						<< "Получили данных меньше чем ждали...(" 
+						<< rlen1 << " < " << szDataLen << ")" << endl;
+				}
+				cleanupChannel();
+				return erInvalidFormat;
+			}
+			
+			bcnt += rlen1;
+			rbuf.len = bcnt - szModbusHeader;
+
+			DiagnosticMessage mDiag(rbuf);
+
+			if( dlog.debugging(Debug::INFO) )
+				dlog[Debug::INFO] << "(0x08): recv buf: " << rbuf << endl;
+
+			if( !crcNoCheckit )
+			{
+				// Проверяем контрольную сумму
+				// от начала(включая заголовок) 
+				// и до конца (исключив последний элемент содержащий CRC)
+				// int mlen = szModbusHeader + mWrite.szHead() + mWrite.bcnt;
+				ModbusData tcrc = checkCRC((ModbusByte*)(&rbuf),bcnt-szCRC);
+				if( tcrc != mDiag.crc )
+				{
+					if( dlog.debugging(Debug::WARN) )
+					{
+						ostringstream err;
+						err << "(0x08): bad crc. calc.crc=" << dat2str(tcrc)
+							<< " msg.crc=" << dat2str(mDiag.crc);
+						dlog[Debug::WARN] << err.str() << endl;
+					}
+					cleanupChannel();
+					return erBadCheckSum;
+				}
+			}
+/*			
+			if( !mDiag.checkFormat() )
+			{
+				dlog[Debug::WARN] << "(0x08): (" << rbuf.func 
+					<< ")(fnDiagnostics): "
+					<< ": некорректный формат сообщения..." << endl; 
+				cleanupChannel();
+				return erInvalidFormat;
+			}
+*/
+			return erNoError;
+		}	
 		else if( rbuf.func == fnJournalCommand )
 		{
 			JournalCommandMessage mRead(rbuf);

src/Communications/Modbus/ModbusTypes.cc

@@ -2168,7 +2168,7 @@ void DiagnosticMessage::init( ModbusMessage& m )
 	int last = sizeof(subf);
 	
 	subf = 	SWAPSHORT(subf);
-	int count = szRequestDiagnosticData((DiagnosticsSubFunction)subf );
+	count = szRequestDiagnosticData((DiagnosticsSubFunction)subf );
 	if( count > MAXDATALEN )
 		throw mbException(erPacketTooLong);
 
@@ -2190,25 +2190,26 @@ int DiagnosticMessage::getDataLen( ModbusMessage& m )
 	if( m.len < 0 )
 		return 0;
 
-	ModbusData d;
-	memcpy( &d,m.data,sizeof(d) );
-	d = SWAPSHORT(d);
-
-	int sz = szRequestDiagnosticData((DiagnosticsSubFunction)d );
+	// data[0] = subfunction
+	ModbusData sf;
+	memcpy( &sf,&(m.data[0]),sizeof(sf) );
+	sf = SWAPSHORT(sf);
 
+	int sz = szRequestDiagnosticData( (DiagnosticsSubFunction)sf );
 	if( sz >= 0 )
-		return sz + sizeof(d); // subf + sz
+		return sz*sizeof(ModbusData);
 
 	return	0;
 }
 // -------------------------------------------------------------------------
-DiagnosticMessage::DiagnosticMessage( ModbusAddr _addr, DiagnosticsSubFunction sf ):
+DiagnosticMessage::DiagnosticMessage( ModbusAddr _addr, DiagnosticsSubFunction sf, ModbusData d ):
 	count(0)
 {
 	addr = _addr;
 	func = fnDiagnostics;
 	subf = sf;
 	memset(data,0,sizeof(data));
+	addData(d);
 }
 // -------------------------------------------------------------------------
 bool DiagnosticMessage::addData( ModbusData d )
@@ -2260,8 +2261,7 @@ ModbusMessage DiagnosticMessage::transport_msg()
 	ind+=szCRC;
 
 	// длина сообщения...
-	mm.len = ind; 
-
+	mm.len = szData(); 
 	return mm;
 }
 // -------------------------------------------------------------------------
@@ -2282,6 +2282,11 @@ DiagnosticMessage(m)
 
 }
 // -------------------------------------------------------------------------
+DiagnosticRetMessage::DiagnosticRetMessage( ModbusAddr a, DiagnosticsSubFunction subf, ModbusData d ):
+	DiagnosticMessage(a,subf,d)
+{
+}
+// -------------------------------------------------------------------------
 std::ostream& ModbusRTU::operator<<(std::ostream& os, DiagnosticMessage& m )
 {
 //	return mbPrintMessage(os,(ModbusByte*)(&m), szModbusHeader + m.szData() );
@@ -2296,7 +2301,17 @@ std::ostream& ModbusRTU::operator<<(std::ostream& os, DiagnosticMessage* m )
 // -------------------------------------------------------------------------
 std::ostream& ModbusRTU::operator<<(std::ostream& os, DiagnosticRetMessage& m )
 {
-	return mbPrintMessage(os,(ModbusByte*)(&m),sizeof(m));
+	//return mbPrintMessage(os,(ModbusByte*)(&m),sizeof(m));
+	os << "addr=" << addr2str(m.addr)
+				<< " subf=" << dat2str(m.subf)
+				<< " data[" << m.count << "]={";
+
+	for( int i=0; i<m.count; i++ )
+		os << dat2str(m.data[i]) << "  ";
+	
+	os << "}";
+	
+	return os;
 }
 
 std::ostream& ModbusRTU::operator<<(std::ostream& os, DiagnosticRetMessage* m )