Bachelorthesis/Modbus-CAPL/include/CAPL/TestTheStack.can

/*@!Encoding:1252*/
includes
{
	#include "include/DeviceInformation.cin"
	#include "include/ModbusUdp.cin"
	#include "include/ModbusClient.cin"
}

variables
{
	word state = 0;
	byte s0i = 0;
	struct device s0dev;
	byte s20i = 1;
	msTimer timr;
	byte skipOutput = 1;
}

on preStart
{
	setStartdelay(100);
	OutputDebugLevel = Debug;
}

on start
{
	char ip[16];
	sysGetVariableString("Device::Config", "IP", ip, 16);

	DeviceInit(@sysvar::Device::Config::Vendor);
	ModbusInit(ip,  @sysvar::Config::Modbus::Port, @sysvar::Config::Modbus::RequestTimeout, 1/*retry*/);

	if (gSocketState < CONNECTING)	// We are not connecting and not connected
		return;

	// Start the Test
	s0dev.Vendor = (enum Vendor)@sysvar::Device::Config::Vendor;
	s0i = _DeviceGetInformation((enum Vendor)@sysvar::Device::Config::Vendor);
}

on preStop
{
	ModbusEnd();
}



void OnModbusReadBitsFailed(enum ModbusRequestError error, enum ModbusException ex, struct ModbusApHeader mbap)
{
	char reason[100];
	struct ModbusReqRead mbreq;

	switch (error)
	{
		case Timeout:
			break;
		case FinalTimeout:
			strncpy(reason, "Timeout", elCount(reason));
			break;
		case Exception:
			snprintf(reason, elCount(reason), "Exception: %s", ModbusExceptions[ex-1]);
			break;
		case NotSent:
			strncpy(reason, "Impossible to send", elCount(reason));
			break;
		default:
			writeDbg(MbError, "OnModbusReadBitsFailed: Unkown error: %d", error);
			OnModbusClientPanics(SwitchArgumentInvalid);
			return;
	}

	memcpy_n2h(mbreq, gQueueAck[mbap.TxID].Buffer);

	switch (state)
	{
		case 0:
			writeDbg(MbError, "State %d. Reading %d output bit from 0x%04X did not work! Reason: %s", state, mbreq.Count, mbreq.Address, reason);
			break;
		case 20:
			writeDbg(MbError, "State %d. Reading %d input bit from 0x%04X did not work! Reason: %s", state, mbreq.Count, mbreq.Address, reason);
			break;
		case 60:
		case 120:
		case 130:
			writeDbg(MbError, "State %d. Reading %d output bits from 0x%04X did not work! Reason: %s", state, mbreq.Count, mbreq.Address, reason);
			break;
		case 200:
			if (error == FinalTimeout)
			{
				writeDbg(MbError, "State %d. Packet timed out! Receive window size: %d", state, s20i-1);
				state = 210;
				stop();
				break;
			}
			else
				writeDbg(MbError, "State %d. Error while writing bit: %s", state, reason);
			break;
		default:
			writeDbg(MbError, "I did not expect state %d in OnModbusReadBitsFailed()!", state);
			break;
	}
	runError(1001, 0);
}
void OnModbusReadBitsSuccess(struct ModbusResReceiveBits mbres, byte bitStatus[], struct ModbusReqRead mbreq)
{
	byte i;
	word s6[1] = {0x5555};

	switch (state)
	{
		case 0:
			writeDbg(MbDebug, "State %d. Successfully read %d output bit from 0x%04X", state, mbreq.Count, mbreq.Address);
			ModbusReadOutRegisters(thisDev.Addr.Read.OutputRegisters, 1);
			state = 10;
			break;
		case 20:
			writeDbg(MbDebug, "State %d. Successfully read %d input bit from 0x%04X", state, mbreq.Count, mbreq.Address);
			ModbusReadInRegisters(thisDev.Addr.Read.InputRegisters, 1);
			state = 30;
			break;
		case 60:
			writeDbg(MbDebug, "State %d. Successfully read %d output bits from 0x%04X", state, mbreq.Count, mbreq.Address);
			for (i = 0; i < mbreq.Count; i++)
			{
				writeDbg(MbWarning, "i: %d, Status: %d, Expected: %d", i, bitStatus[i], (i != 2));
				if (bitStatus[i] != (i != 2))
				{
					writeDbg(MbError, "State of output bit %d was incorrect: %d (expected %d)", i, bitStatus[i], (i != 2));
					runError(1001, 0);
					return;
				}
			}
			// OK.
			writeDbg(MbDebug, "State %d. Status of these output bits was correct.", state);
			ModbusWriteRegisters(thisDev.Addr.Write.OutputRegisters, 1, s6);
			state = 70;
			break;
		case 120:
			state = 130;
			break;
		case 130:
			writeDbg(MbDebug, "State %d. Successfully received the Read-Bits telegrams (as expected)", state);
			state = 140;
			timr.Set(1);
			break;
		case 200:
			if (gQueueSent.Size() > 0 || gQueuePending.Size() > 0)
				break;
			++s20i;
			for (i = 0; i < s20i && i < 100; i++)
				ModbusReadBits(thisDev.Addr.Read.InputBits, 1);
			break;
		default:
			writeDbg(MbError, "I did not expect state %d in OnModbusReadBitsSuccess()!", state);
			runError(1001, 0);
			break;
	}
}





void OnModbusReadRegistersFailed(enum ModbusRequestError error, enum ModbusException ex, struct ModbusApHeader mbap)
{
	char reason[100];
	struct ModbusReqRead mbreq;

	switch (error)
	{
		case Timeout:
			break;
		case FinalTimeout:
			strncpy(reason, "Timeout", elCount(reason));
			break;
		case Exception:
			snprintf(reason, elCount(reason), "Exception: %s", ModbusExceptions[ex-1]);
			break;
		case NotSent:
			strncpy(reason, "Impossible to send", elCount(reason));
			break;
		default:
			writeDbg(MbError, "OnModbusReadRegistersFailed: Unkown error: %d", error);
			OnModbusClientPanics(SwitchArgumentInvalid);
			return;
	}

	memcpy_n2h(mbreq, gQueueAck[mbap.TxID].Buffer);

	switch (state)
	{
		case 0:
		case 10:
		case 110:
		case 75:
			writeDbg(MbError, "State %d. Reading %d output register from 0x%04X did not work! Reason: %s", state, mbreq.Count, mbreq.Address, reason);
			break;
		case 30:
			writeDbg(MbError, "State %d. Reading %d input register from 0x%04X did not work! Reason: %s", state, mbreq.Count, mbreq.Address, reason);
			break;
		case 90:
		case 140:
		case 145:
		case 150:
			writeDbg(MbError, "State %d. Reading %d output registers from 0x%04X did not work! Reason: %s", state, mbreq.Count, mbreq.Address, reason);
			break;
		default:
			writeDbg(MbError, "I did not expect state %d in OnModbusReadRegistersFailed()!", state);
			break;
	}
	runError(1001, 0);
}
void OnModbusReadRegistersSuccess(struct ModbusResReceiveRegisters mbres, struct ModbusReqRead mbreq)
{
	switch (state)
	{
		case 0:
			s0i--;
			_DeviceParseRegister(s0dev, mbreq.Address, mbres.Data, 0);

			if (s0i == 0)
			{
				if (s0dev.DeviceIOs.OutputBits / 8 + s0dev.DeviceIOs.OutputRegisters < 2)
				{
					writeDbg(MbError, "Please connect more output! %d bits and %d registers are not enough, we need at least 1 word (2 bytes). (Input: %d bits, %d regs)", s0dev.DeviceIOs.OutputBits, s0dev.DeviceIOs.OutputRegisters, s0dev.DeviceIOs.InputBits, s0dev.DeviceIOs.InputRegisters);
					skipOutput = 1;
					//runError(1001, 0);
					//return;
				}
				ModbusReadOutBits(thisDev.Addr.Read.OutputBits, 1);
			}
			break;
		case 10:
			writeDbg(MbDebug, "State %d. Successfully read %d output register from 0x%04X", state, mbreq.Count, mbreq.Address);
			ModbusReadInBits(thisDev.Addr.Read.InputBits, 1);
			state = 20;
			break;
		case 30:
			writeDbg(MbDebug, "State %d. Successfully read %d input register from 0x%04X", state, mbreq.Count, mbreq.Address);
			if (skipOutput)
				state = 140;
			else
				state = 40;
			timr.Set(1);
			break;
		case 75:
			writeDbg(MbDebug, "State %d. Successfully read %d input register from 0x%04X", state, mbreq.Count, mbreq.Address);
			if (mbres.Data[0] != 0x5555)
			{
				writeDbg(MbError, "State %d. Value of output register 0 was incorrect: 0x%04X (expected 0x5555). ModbusWriteRegisters failed!", state, mbres.Data[0]);
				runError(1001, 0);
				return;
			}
			ModbusWriteRegister(thisDev.Addr.Write.OutputRegisters, 0x9999);
			state = 80;
			break;
		case 90:
			writeDbg(MbDebug, "State %d. Successfully read %d output register from 0x%04X", state, mbreq.Count, mbreq.Address);
			if (mbres.Data[0] != 0x9999)
			{
				writeDbg(MbError, "State %d. Value of output register 0 was incorrect: 0x%04X (expected 0x9999). ModbusWriteRegister failed!", state, mbres.Data[0]);
				runError(1001, 0);
				return;
			}
			ModbusWriteMasks(thisDev.Addr.Write.OutputRegisters, 0x00F9, 0x0006);
			state = 100;
			break;
		case 110:
			writeDbg(MbDebug, "State %d. Successfully read %d output register from 0x%04X", state, mbreq.Count, mbreq.Address);
			if (mbres.Data[0] != 0x009F)
			{
				writeDbg(MbError, "State %d. Value of register at 0x%04X is incorrect: 0x%04X (expected 0x009F). ModbusWriteMasks failed!", state, mbreq.Address, mbres.Data[0]);
				runError(1001, 0);
				return;
			}
			writeDbg(MbDebug, "State %d. Successfully applied the masks at 0x%04X", state, mbreq.Address);
			ModbusReadInBits(thisDev.Addr.Read.InputBits, 3000);
			state = 120;
			if (s0dev.Vendor == Wago)		// This test does not work with Wago
				state = 130;
			break;
		case 140:
			state = 145;
			break;
		case 145:
			state = 150;
			break;
		case 150:
			writeDbg(MbDebug, "State %d. Successfully received three Read-Registers telegrams (as expected)", state);
			if (skipOutput)
				state = 200;
			else
			{
				state = 160;
				if (s0dev.Vendor == Wago)		// This test does not work with Wago
					state = 170;
			}
			timr.Set(1);
			break;
		default:
			writeDbg(MbError, "I did not expect state %d in OnModbusReadRegistersSuccess()!", state);
			runError(1001, 0);
			break;
	}
}






void OnModbusWriteBitFailed(enum ModbusRequestError error, enum ModbusException ex, struct ModbusApHeader mbap)
{
	char reason[100];
	struct ModbusReqWriteSingle mbreq;

	switch (error)
	{
		case Timeout:
			break;
		case FinalTimeout:
			strncpy(reason, "Timeout", elCount(reason));
			break;
		case Exception:
			snprintf(reason, elCount(reason), "Exception: %s", ModbusExceptions[ex-1]);
			break;
		case NotSent:
			strncpy(reason, "Impossible to send", elCount(reason));
			break;
		default:
			writeDbg(MbError, "OnModbusWriteBitFailed: Unkown error: %d", error);
			OnModbusClientPanics(SwitchArgumentInvalid);
			return;
	}


	memcpy_n2h(mbreq, gQueueAck[mbap.TxID].Buffer);

	switch (state)
	{
		case 50:
			writeDbg(MbError, "State %d. Setting bit at 0x%04X did not work! Reason: %s", state, mbreq.Address, reason);
			break;
		default:
			writeDbg(MbError, "I did not expect state %d in OnModbusWriteBitFailed()!", state);
			break;
	}
	runError(1001, 0);
}
void OnModbusWriteBitSuccess(struct ModbusResConfirmSingle mbres)
{
	byte i;
	switch (state)
	{
		case 50:
			writeDbg(MbDebug, "State %d. Successfully set bit at 0x%04X", state, mbres.Address);
			timr.Set(1);
			break;
		default:
			writeDbg(MbError, "I did not expect state %d in OnModbusWriteBitSuccess()!", state);
			runError(1001, 0);
			break;
	}
}







void OnModbusWriteRegisterFailed(enum ModbusRequestError error, enum ModbusException ex, struct ModbusApHeader mbap)
{
	char reason[100];
	struct ModbusReqWriteSingle mbreq;

	switch (error)
	{
		case Timeout:
			break;
		case FinalTimeout:
			strncpy(reason, "Timeout", elCount(reason));
			break;
		case Exception:
			snprintf(reason, elCount(reason), "Exception: %s", ModbusExceptions[ex-1]);
			break;
		case NotSent:
			strncpy(reason, "Impossible to send", elCount(reason));
			break;
		default:
			writeDbg(MbError, "OnModbusWriteRegisterFailed: Unkown error: %d", error);
			OnModbusClientPanics(SwitchArgumentInvalid);
			return;
	}

	memcpy_n2h(mbreq, gQueueAck[mbap.TxID].Buffer);

	switch (state)
	{
		case 80:
			writeDbg(MbError, "State %d. Writing output register at 0x%04X did not work! Reason: %s", state, mbreq.Address, reason);
			break;
		default:
			writeDbg(MbError, "I did not expect state %d in OnModbusWriteRegisterFailed()!", state);
			break;
	}
	runError(1001, 0);
}
void OnModbusWriteRegisterSuccess(struct ModbusResConfirmSingle mbres)
{
	switch (state)
	{
		case 80:
			writeDbg(MbDebug, "State %d. Successfully set output register at 0x%04X to 0x%04X", state, mbres.Address, mbres.Value);
			timr.Set(1);
			break;
		default:
			writeDbg(MbError, "I did not expect state %d in OnModbusWriteRegisterSuccess()!", state);
			runError(1001, 0);
			break;
	}
}







void OnModbusWriteMasksFailed(enum ModbusRequestError error, enum ModbusException ex, struct ModbusApHeader mbap)
{
	char reason[100];
	struct ModbusReqWriteMasks mbreq;

	switch (error)
	{
		case Timeout:
			break;
		case FinalTimeout:
			strncpy(reason, "Timeout", elCount(reason));
			break;
		case Exception:
			snprintf(reason, elCount(reason), "Exception: %s", ModbusExceptions[ex-1]);
			break;
		case NotSent:
			strncpy(reason, "Impossible to send", elCount(reason));
			break;
		default:
			writeDbg(MbError, "OnModbusWriteMasksFailed: Unkown error: %d", error);
			OnModbusClientPanics(SwitchArgumentInvalid);
			return;
	}

	memcpy_n2h(mbreq, gQueueAck[mbap.TxID].Buffer);

	switch (state)
	{
		case 100:
			writeDbg(MbError, "State %d. Applying masks &0x%04X |0x%04X at 0x%04X did not work! Reason: %s", state, mbreq.And, mbreq.Or, mbreq.Address);
			break;
		default:
			writeDbg(MbError, "I did not expect state %d in OnModbusWriteMasksFailed()!", state);
			break;
	}
	runError(1001, 0);
}
void OnModbusWriteMasksSuccess(struct ModbusResConfirmMasks mbres)
{
	switch (state)
	{
		case 100:
			writeDbg(MbDebug, "State %d. Successfully applied masks &0x%04X |0x%04X at 0x%04X", state, mbres.And, mbres.Or, mbres.Address);
			timr.Set(1);
			break;
		default:
			writeDbg(MbError, "I did not expect state %d in OnModbusWriteMasksSuccess()!", state);
			runError(1001, 0);
			break;
	}
}







void OnModbusReadWriteRegistersFailed(enum ModbusRequestError error, enum ModbusException ex, struct ModbusApHeader mbap)
{
	char reason[100];
	struct ModbusReqReadWriteRegisters mbreq;

	switch (error)
	{
		case Timeout:
			break;
		case FinalTimeout:
			strncpy(reason, "Timeout", elCount(reason));
			break;
		case Exception:
			snprintf(reason, elCount(reason), "Exception: %s", ModbusExceptions[ex-1]);
			break;
		case NotSent:
			strncpy(reason, "Impossible to send", elCount(reason));
			break;
		default:
			writeDbg(MbError, "OnModbusReadWriteRegistersFailed: Unkown error: %d", error);
			OnModbusClientPanics(SwitchArgumentInvalid);
			return;
	}

	memcpy_n2h(mbreq, gQueueAck[mbap.TxID].Buffer);

	switch (state)
	{
		default:
			writeDbg(MbError, "I did not expect state %d in OnModbusReadWriteRegistersFailed()!", state);
			break;
	}
	runError(1001, 0);
}







void OnModbusWriteBitsFailed(enum ModbusRequestError error, enum ModbusException ex, struct ModbusApHeader mbap)
{
	char reason[100];
	struct ModbusReqWriteBits mbreq;

	switch (error)
	{
		case Timeout:
			break;
		case FinalTimeout:
			strncpy(reason, "Timeout", elCount(reason));
			break;
		case Exception:
			snprintf(reason, elCount(reason), "Exception: %s", ModbusExceptions[ex-1]);
			break;
		case NotSent:
			strncpy(reason, "Impossible to send", elCount(reason));
			break;
		default:
			writeDbg(MbError, "OnModbusWriteBitsFailed: Unkown error: %d", error);
			OnModbusClientPanics(SwitchArgumentInvalid);
			return;
	}

	memcpy_n2h(mbreq, gQueueAck[mbap.TxID].Buffer);

	switch (state)
	{
		case 40:
		case 160:
		case 170:
			writeDbg(MbError, "State %d. Writing %d bits at 0x%04X did not work! Reason: %s", state, mbreq.Count, mbreq.Address, reason);
			break;
		default:
			writeDbg(MbError, "I did not expect state %d in OnModbusWriteBitsFailed()!", state);
			break;
	}
	runError(1001, 0);
}
void OnModbusWriteBitsSuccess(struct ModbusResConfirmMultiple mbres)
{
	word s17[200];

	switch (state)
	{
		case 40:
			writeDbg(MbDebug, "State %d. Successfully set %d bits at 0x%04X", state, mbres.Count, mbres.Address);
			ModbusWriteBit(thisDev.Addr.Write.OutputBits+2, 0);
			state = 50;
			break;
		case 160:
			state = 170;
			break;
		case 170:
			writeDbg(MbDebug, "State %d. Successfully received two Write-Bits telegrams (as expected)", state);
			ModbusWriteRegisters(thisDev.Addr.Write.OutputRegisters, 200, s17);
			state = 180;
			break;
		default:
			writeDbg(MbError, "I did not expect state %d in OnModbusWriteBitsSuccess()!", state);
			runError(1001, 0);
			break;
	}
}










void OnModbusWriteRegistersFailed(enum ModbusRequestError error, enum ModbusException ex, struct ModbusApHeader mbap)
{
	char reason[100];
	struct ModbusReqWriteRegisters mbreq;

	switch (error)
	{
		case Timeout:
			break;
		case FinalTimeout:
			strncpy(reason, "Timeout", elCount(reason));
			break;
		case Exception:
			snprintf(reason, elCount(reason), "Exception: %s", ModbusExceptions[ex-1]);
			break;
		case NotSent:
			strncpy(reason, "Impossible to send", elCount(reason));
			break;
		default:
			writeDbg(MbError, "OnModbusWriteRegistersFailed: Unkown error: %d", error);
			OnModbusClientPanics(SwitchArgumentInvalid);
			return;
	}

	memcpy_n2h(mbreq, gQueueAck[mbap.TxID].Buffer);

	switch (state)
	{
		case 70:
		case 180:
		case 190:
			writeDbg(MbError, "State %d. Writing %d registers at 0x%04X did not work! Reason: %s", state, mbreq.Count, mbreq.Address, reason);
			break;
		default:
			writeDbg(MbError, "I did not expect state %d in OnModbusWriteRegistersFailed()!", state);
			break;
	}
	runError(1001, 0);
}
void OnModbusWriteRegistersSuccess(struct ModbusResConfirmMultiple mbres)
{
	switch (state)
	{
		case 70:
			writeDbg(MbDebug, "State %d. Successfully set %d output registers at 0x%04X", state, mbres.Count, mbres.Address);
			timr.Set(1);
			break;
		case 180:
			state = 190;
			break;
		case 190:
			writeDbg(MbDebug, "State %d. Successfully received two Write-Registers telegrams (as expected)", state);
			state = 200;
			timr.Set(1);
			break;
		default:
			writeDbg(MbError, "I did not expect state %d in OnModbusWriteRegistersSuccess()!", state);
			runError(1001, 0);
			break;
	}
}





void OnModbusClientPanics(enum FatalErrors reason)
{
	switch (reason)
	{
		case ParsingBuffer:
			writeDbg(MbError, "State %d. FATAL ERROR while parsing received buffer", state);
			break;
		case ModbusPackageWasSplit:
			writeDbg(MbError, "State %d. FATAL ERROR: Modbus package was split", state);
			break;
		case DeviceCodeUnknown:
			writeDbg(MbError, "State %d. FATAL ERROR: Device code unknown", state);
			break;
		case VendorIdUnknown:
			writeDbg(MbError, "State %d. FATAL ERROR: Vendor Id unknown", state);
			break;
		case ConnectionError:
			writeDbg(MbError, "State %d. FATAL ERROR: Connection Error", state);
			break;
		case FuncCodeIncorrect:
			writeDbg(MbError, "State %d. FATAL ERROR: FuncCode Incorrect", state);
			break;
		case AddressFailure:
			writeDbg(MbError, "State %d. FATAL ERROR: Some Modbus Address Failure", state);
			break;
		case SwitchArgumentInvalid:
			writeDbg(MbError, "State %d. FATAL ERROR: A argument of a switch statement is incorrect");
			break;
	}
	
	stop();
	runError(1001, 0);
}


on timer timr
{
	byte s3[10] = {1, 1, 1, 1, 1, 1, 1, 1, 1, 1};
	byte s15[3000];

	switch(state)
	{
		case 40:
			ModbusWriteBitsB(thisDev.Addr.Write.OutputBits, 10, s3);
			break;
		case 50:
			ModbusReadOutBits(thisDev.Addr.Read.OutputBits, 10);
			state = 60;
			break;
		case 70:
			ModbusReadOutRegisters(thisDev.Addr.Read.OutputRegisters, 1);
			state = 75;
			break;
		case 80:
			ModbusReadOutRegisters(thisDev.Addr.Read.OutputRegisters, 1);
			state = 90;
			break;
		case 100:
			ModbusReadOutRegisters(thisDev.Addr.Read.OutputRegisters, 1);
			state = 110;
			break;
		case 140:
			ModbusReadRegisters(thisDev.Addr.Read.InputRegisters, 300);
			break;
		case 160:
		case 170:
			ModbusWriteBitsB(thisDev.Addr.Write.OutputBits, 3000, s15);
			break;
		case 200:
			ModbusReadBits(thisDev.Addr.Read.InputBits, 1);
			thisDev.ReceiveWindow = 0xFF;		// Set receive window to maximum to test the limit
			break;
		default:
			writeDbg(MbError, "I did not expect state %d in timer timr!", state);
			break;			
	}
}