186 lines
4.5 KiB
XML
186 lines
4.5 KiB
XML
<?xml version="1.0" encoding="utf-8"?>
|
|
<TcPlcObject Version="1.1.0.1">
|
|
<POU Name="FB_ModbusMaster" Id="{90292ada-f5b3-4b38-a85e-d6dc52e68612}" SpecialFunc="None">
|
|
<Declaration><![CDATA[FUNCTION_BLOCK FB_ModbusMaster
|
|
VAR_INPUT
|
|
timGapTime : TIME := T#20MS;
|
|
END_VAR
|
|
VAR_OUTPUT
|
|
xBusy : BOOL;
|
|
END_VAR
|
|
VAR
|
|
// Modbus master comm fb
|
|
_fbMaster : ModbusRtuMasterV2_KL6x22B;
|
|
|
|
// Request queue
|
|
_astReqQueue : ARRAY[0..(GVL_ModbusMaster.QUEUE_SIZE - 1)] OF ST_Modbus_Req;
|
|
_iHead : INT := 0;
|
|
_iTail : INT := 0;
|
|
|
|
// Current request from queue
|
|
_stCurrReq : ST_Modbus_Req;
|
|
|
|
// Gap timer so that we dont send requests back to back
|
|
_tonGapTimer : TON;
|
|
|
|
// State machine state
|
|
_iState : INT := 0;
|
|
|
|
// Rerun flag for faster state machine
|
|
_xSMCycleDone : BOOL := TRUE;
|
|
|
|
_xMasterWasBusy : BOOL;
|
|
|
|
_xTest : BOOL;
|
|
END_VAR
|
|
]]></Declaration>
|
|
<Implementation>
|
|
<ST><![CDATA[//_fbMaster();
|
|
|
|
IF _xTest THEN
|
|
_xTest := FALSE;
|
|
_fbMaster.ReadInputRegs(Execute := FALSE);
|
|
END_IF
|
|
|
|
// Main state machine
|
|
REPEAT
|
|
// Safety reset
|
|
_xSMCycleDone := TRUE;
|
|
|
|
CASE _iState OF
|
|
// Wait for gap timer to be expired
|
|
0:
|
|
_tonGapTimer(IN := TRUE);
|
|
|
|
IF _tonGapTimer.Q THEN
|
|
_tonGapTimer(IN := FALSE);
|
|
_xSMCycleDone := FALSE;
|
|
_iState := 1;
|
|
END_IF
|
|
|
|
// Get next request if there is one in the queue
|
|
1:
|
|
// Get next request from queue if there is one
|
|
IF M_Dequeue() THEN
|
|
// Rerun statemachine this cycle
|
|
_xSMCycleDone := FALSE;
|
|
_iState := 10;
|
|
END_IF
|
|
|
|
// Execute dequeued command
|
|
10:
|
|
// Call modbus master corresponding function
|
|
CASE _stCurrReq.eCmd OF
|
|
E_Modbus_Cmd.READ_HOLDING:
|
|
_fbMaster.ReadRegs(
|
|
UnitID := _stCurrReq.bySlaveAddr,
|
|
Quantity := _stCurrReq.wQuantity,
|
|
MBAddr := _stCurrReq.wStartAddr,
|
|
cbLength := _stCurrReq.wDataSize,
|
|
pMemoryAddr := _stCurrReq.pvData,
|
|
Execute := TRUE,
|
|
Timeout := T#1S);
|
|
|
|
E_Modbus_Cmd.READ_INPUTS:
|
|
_fbMaster.ReadInputRegs(
|
|
UnitID := _stCurrReq.bySlaveAddr,
|
|
Quantity := _stCurrReq.wQuantity,
|
|
MBAddr := _stCurrReq.wStartAddr,
|
|
cbLength := _stCurrReq.wDataSize,
|
|
pMemoryAddr := _stCurrReq.pvData,
|
|
Execute := TRUE,
|
|
Timeout := T#1S);
|
|
|
|
E_Modbus_Cmd.WRITE_HOLDING:
|
|
_fbMaster.WriteRegs(
|
|
UnitID := _stCurrReq.bySlaveAddr,
|
|
Quantity := _stCurrReq.wQuantity,
|
|
MBAddr := _stCurrReq.wStartAddr,
|
|
cbLength := _stCurrReq.wDataSize,
|
|
pMemoryAddr := _stCurrReq.pvData,
|
|
Execute := TRUE,
|
|
Timeout := T#1S);
|
|
|
|
// Reject unknown commands
|
|
ELSE
|
|
_stCurrReq.pxError^ := TRUE;
|
|
_iState := 0;
|
|
END_CASE
|
|
|
|
IF _fbMaster.BUSY THEN
|
|
_xMasterWasBusy := TRUE;
|
|
END_IF
|
|
|
|
// Wait for fb to be done
|
|
IF (NOT _fbMaster.BUSY) THEN
|
|
_fbMaster.ReadRegs(Execute := FALSE);
|
|
_fbMaster.ReadInputRegs(Execute := FALSE);
|
|
_fbMaster.WriteRegs(Execute := FALSE);
|
|
_stCurrReq.pxDone^ := TRUE;
|
|
// Rerun statemachine this cycle
|
|
_xSMCycleDone := FALSE;
|
|
_iState := 0;
|
|
END_IF
|
|
|
|
IF _fbMaster.Error THEN
|
|
_fbMaster.ReadRegs(Execute := FALSE);
|
|
_fbMaster.ReadInputRegs(Execute := FALSE);
|
|
_fbMaster.WriteRegs(Execute := FALSE);
|
|
_stCurrReq.pxError^ := TRUE;
|
|
// Rerun statemachine this cycle
|
|
_xSMCycleDone := FALSE;
|
|
_iState := 0;
|
|
END_IF
|
|
END_CASE
|
|
|
|
// Check if we want to rerun the state machine
|
|
UNTIL _xSMCycleDone
|
|
END_REPEAT
|
|
]]></ST>
|
|
</Implementation>
|
|
<Method Name="M_Dequeue" Id="{083aa9a5-ac23-4366-b080-ab00cfa6b4ce}">
|
|
<Declaration><![CDATA[METHOD PRIVATE M_Dequeue : BOOL
|
|
VAR_INPUT
|
|
END_VAR
|
|
]]></Declaration>
|
|
<Implementation>
|
|
<ST><![CDATA[// No entries in queue
|
|
IF _iHead = _iTail THEN
|
|
M_Dequeue := FALSE;
|
|
ELSE
|
|
// Get next entry
|
|
_stCurrReq := _astReqQueue[_iHead];
|
|
_iHead := (_iHead + 1) MOD GVL_ModbusMaster.QUEUE_SIZE;
|
|
M_Dequeue := TRUE;
|
|
END_IF]]></ST>
|
|
</Implementation>
|
|
</Method>
|
|
<Method Name="M_Enqueue" Id="{37963eb3-9b4c-4945-ba35-cfefd78cb427}">
|
|
<Declaration><![CDATA[METHOD M_Enqueue : BOOL
|
|
VAR_INPUT
|
|
stReq : ST_Modbus_Req;
|
|
END_VAR
|
|
VAR
|
|
_iNextTail : INT;
|
|
END_VAR]]></Declaration>
|
|
<Implementation>
|
|
<ST><![CDATA[// Calculate next entry
|
|
_iNextTail := (_iTail + 1) MOD GVL_ModbusMaster.QUEUE_SIZE;
|
|
|
|
// If next tail euqals head, the buffer is full
|
|
// so disgard this request
|
|
IF _iNextTail = _iHead THEN
|
|
M_Enqueue := FALSE;
|
|
ELSE
|
|
_astReqQueue[_iTail] := stReq;
|
|
_iTail := _iNextTail;
|
|
M_Enqueue := TRUE;
|
|
END_IF
|
|
|
|
// Reset error and done
|
|
stReq.pxDone^ := FALSE;
|
|
stReq.pxError^ := FALSE;]]></ST>
|
|
</Implementation>
|
|
</Method>
|
|
</POU>
|
|
</TcPlcObject> |