2020-12-16 09:29:40 +01:00
using System ;
using System.Collections.Generic ;
2020-12-21 23:02:06 +01:00
using System.Diagnostics ;
2020-12-20 02:31:00 +01:00
using System.IO ;
2020-12-16 09:29:40 +01:00
using System.Linq ;
using System.Threading ;
2020-12-21 23:02:06 +01:00
using ln.ethercat.controller ;
using ln.ethercat.controller.drives ;
2020-12-16 09:29:40 +01:00
using ln.logging ;
using ln.type ;
namespace ln.ethercat
{
public enum ECMasterState {
INITIALIZED ,
STARTING ,
STOPPING ,
RUNNING ,
}
2020-12-21 23:02:06 +01:00
public struct PDOMappingRequest
{
public ushort Slave ;
public ushort Index ;
public byte SubIndex ;
public bool RxPDO ;
public PDOMappingRequest ( ushort slave , ushort index , byte subIndex , bool rxpdo )
{
Slave = slave ;
Index = index ;
SubIndex = subIndex ;
RxPDO = rxpdo ;
}
}
2020-12-16 09:29:40 +01:00
public delegate void ECStateChange ( ECMaster sender , ECSlaveState newState ) ;
2021-04-14 10:02:35 +02:00
public delegate void ProcessDataExchanged ( ECMaster ecMaster ) ;
2020-12-16 09:29:40 +01:00
public class ECMaster
{
public int TIMEOUT_PREOP = 3000 ;
public int TIMEOUT_SAFEOP = 10000 ;
public int TIMEOUT_BACKTO_SAFEOP = 200 ;
2020-12-20 02:31:00 +01:00
public int INTERVALL_PROCESSDATA = 1 ;
public int INTERVALL_WATCHDOG = 250 ;
2020-12-16 09:29:40 +01:00
public event ECStateChange OnStateChange ;
2021-04-14 10:02:35 +02:00
public event ProcessDataExchanged OnProcessDataExchanged ;
2020-12-16 09:29:40 +01:00
public string InterfaceName { get ; }
public int ExpectedWorkCounter { get ; private set ; }
public int CountSlaves { get ; private set ; }
public IntPtr IOMapPtr { get ; private set ; }
public int IOMapSize { get ; private set ; }
public ECMasterState MasterState { get ; private set ; } = ECMasterState . INITIALIZED ;
ECSlaveState ethercatState = ECSlaveState . NONE ;
public ECSlaveState EthercatState {
2020-12-21 23:02:06 +01:00
get = > ethercatState ;
private set {
2020-12-16 09:29:40 +01:00
if ( value ! = ethercatState )
{
OnStateChange ? . Invoke ( this , value ) ;
ethercatState = value ;
Logging . Log ( LogLevel . DEBUG , "ECMaster: EthercatState is now {0}" , ethercatState ) ;
}
}
}
2020-12-20 02:31:00 +01:00
List < PDO > pdoMap = new List < PDO > ( ) ;
public PDO [ ] GetPDOMap ( ) = > pdoMap . ToArray ( ) ;
Dictionary < UInt16 , ECSlave > slaves = new Dictionary < ushort , ECSlave > ( ) ;
2020-12-16 09:29:40 +01:00
public ECMaster ( string interfaceName )
{
InterfaceName = interfaceName ;
int result = ECMBind . ecmbind_initialize ( interfaceName ) ;
Logging . Log ( LogLevel . INFO , "ecmbind_initialize({0}) = {1}" , interfaceName , result ) ;
if ( result < = 0 )
throw new Exception ( "ecmbind_initialize failed" ) ;
2020-12-21 23:02:06 +01:00
Controller = new Controller ( this ) ;
2020-12-16 09:29:40 +01:00
}
2020-12-21 23:02:06 +01:00
bool stopWatchdog ;
bool stopProcessData ;
2020-12-16 09:29:40 +01:00
Thread threadProcessData ;
2020-12-20 02:31:00 +01:00
Thread threadWatchdog ;
2020-12-16 09:29:40 +01:00
2020-12-21 23:02:06 +01:00
public Controller Controller { get ; }
List < DriveController > driveControllers = new List < DriveController > ( ) ;
public DriveController [ ] DriveControllers = > driveControllers . ToArray ( ) ;
public void Start ( )
2020-12-16 09:29:40 +01:00
{
2020-12-21 23:02:06 +01:00
lock ( this )
{
if ( threadWatchdog ? . IsAlive ? ? false )
throw new Exception ( "already started" ) ;
ethercatState = ECSlaveState . NONE ;
ExpectedWorkCounter = 0 ;
2020-12-20 02:31:00 +01:00
2020-12-21 23:02:06 +01:00
stopWatchdog = false ;
2020-12-16 09:29:40 +01:00
2020-12-21 23:02:06 +01:00
threadWatchdog = new Thread ( Watchdog ) ;
threadWatchdog . Start ( ) ;
}
}
public void Stop ( )
{
2020-12-16 09:29:40 +01:00
lock ( this )
{
2020-12-21 23:02:06 +01:00
if ( threadWatchdog ? . IsAlive ? ? false )
{
stopWatchdog = true ;
lock ( this )
Monitor . PulseAll ( this ) ;
threadWatchdog . Join ( ) ;
}
}
}
void Watchdog ( )
{
while ( ! stopWatchdog )
{
IOMapSize = 0 ;
if ( threadProcessData ? . IsAlive ? ? false )
{
stopProcessData = true ;
while ( threadProcessData . IsAlive )
threadProcessData . Join ( 250 ) ;
}
stopProcessData = false ;
EthercatState = ECSlaveState . INIT ;
2020-12-16 09:29:40 +01:00
CountSlaves = ECMBind . ecmbind_config_init ( ) ;
if ( CountSlaves < = 0 )
{
2020-12-21 23:02:06 +01:00
Logging . Log ( LogLevel . DEBUG , "ECMaster: no slaves connected, scheduling restart..." ) ;
Thread . Sleep ( 2500 ) ;
continue ;
2020-12-16 09:29:40 +01:00
}
2020-12-21 23:02:06 +01:00
if ( ! WaitForState ( ECSlaveState . PRE_OP , TIMEOUT_PREOP ) )
{
Logging . Log ( LogLevel . DEBUG , "ECMaster: slaves did not reach PRE_OP. restart..." ) ;
Thread . Sleep ( 2500 ) ;
continue ;
}
/* Bus State PRE_OP */
ScanControllers ( ) ;
2020-12-16 09:29:40 +01:00
2020-12-21 23:02:06 +01:00
EthercatState = ECSlaveState . PRE_OP ;
ConfigureProcessDataMappings ( ) ;
SetupPDOMapping ( ) ;
2020-12-16 09:29:40 +01:00
UpdatePDOMap ( ) ;
2020-12-21 23:02:06 +01:00
threadProcessData = new Thread ( ProcessData ) ;
2020-12-16 09:29:40 +01:00
threadProcessData . Start ( ) ;
2020-12-20 02:31:00 +01:00
Thread . Sleep ( 20 ) ;
2020-12-21 23:02:06 +01:00
RequestSlaveState ( 0 , ECSlaveState . SAFE_OP ) ;
2020-12-20 02:31:00 +01:00
2020-12-21 23:02:06 +01:00
if ( ! WaitForState ( ECSlaveState . SAFE_OP , TIMEOUT_SAFEOP ) )
2020-12-16 09:29:40 +01:00
{
2020-12-21 23:02:06 +01:00
Logging . Log ( LogLevel . DEBUG , "ECMaster: slaves did not reach SAFE_OP. restart..." ) ;
Thread . Sleep ( 2500 ) ;
continue ;
2020-12-16 09:29:40 +01:00
}
2020-12-21 23:02:06 +01:00
EthercatState = ECSlaveState . SAFE_OP ;
2020-12-16 09:29:40 +01:00
2020-12-21 23:02:06 +01:00
Thread . Sleep ( 20 ) ;
RequestSlaveState ( 0 , ECSlaveState . OPERATIONAL ) ;
Thread . Sleep ( 250 ) ;
if ( ! WaitForState ( ECSlaveState . OPERATIONAL , TIMEOUT_SAFEOP ) )
{
Logging . Log ( LogLevel . DEBUG , "ECMaster: slaves did not reach OPERATIONAL. restart..." ) ;
Thread . Sleep ( 2500 ) ;
continue ;
}
2020-12-16 09:29:40 +01:00
2020-12-21 23:02:06 +01:00
EthercatState = ECSlaveState . OPERATIONAL ;
2020-12-16 09:29:40 +01:00
2020-12-21 23:02:06 +01:00
/***** we handle the controller cycle from here on *****/
2020-12-20 02:31:00 +01:00
2020-12-21 23:02:06 +01:00
Controller . Initialize ( ) ;
DateTime nextControllerRun = DateTime . Now ;
while ( ! stopWatchdog )
2020-12-20 02:31:00 +01:00
{
2020-12-21 23:02:06 +01:00
EthercatState = IOLocked ( ( ) = > ECMBind . ecmbind_read_state ( ) ) ;
if ( EthercatState ! = ECSlaveState . OPERATIONAL )
break ;
DateTime currentTime = DateTime . Now ;
nextControllerRun + = TimeSpan . FromSeconds ( Controller . ControllerLoopInterval ) ;
if ( currentTime > nextControllerRun )
{
Logging . Log ( LogLevel . WARNING , "missed controller cycle" ) ;
while ( currentTime > nextControllerRun )
nextControllerRun + = TimeSpan . FromSeconds ( Controller . ControllerLoopInterval ) ;
}
TimeSpan timeToNextCycle = nextControllerRun - currentTime ;
try
{
Thread . Sleep ( timeToNextCycle ) ;
Controller . Cycle ( ) ;
} catch ( Exception e )
{
Logging . Log ( e ) ;
}
2020-12-20 02:31:00 +01:00
}
2020-12-21 23:02:06 +01:00
2020-12-16 09:29:40 +01:00
}
2020-12-21 23:02:06 +01:00
}
2020-12-20 02:31:00 +01:00
2020-12-21 23:02:06 +01:00
void ProcessData ( )
2020-12-20 02:31:00 +01:00
{
2020-12-21 23:02:06 +01:00
int wkc ;
lock ( lockIOMap )
ExpectedWorkCounter = ECMBind . ecmbind_get_expected_wkc_size ( ) ;
2020-12-20 02:31:00 +01:00
2020-12-21 23:02:06 +01:00
Logging . Log ( LogLevel . INFO , "ECMaster: ProcessData(): start" ) ;
while ( ! stopProcessData )
{
lock ( lockIOMap )
2020-12-20 02:31:00 +01:00
{
2020-12-21 23:02:06 +01:00
wkc = ECMBind . ecmbind_processdata2 ( iomap , iomap . Length ) ;
if ( wkc ! = ExpectedWorkCounter )
{
int success = ECMBind . ecmbind_recover ( ) ;
if ( success > 0 )
{
Logging . Log ( LogLevel . ERROR , "ECMaster: bus recovery successfull..." ) ;
ExpectedWorkCounter = success ;
//SetupPDOMapping();
} else
{
RequestSlaveState ( 0 , ECSlaveState . INIT ) ;
Logging . Log ( LogLevel . ERROR , "ECMaster: bus recovery failed, scheduling restart..." ) ;
ScheduleRestart ( ) ;
stopProcessData = true ;
}
2021-04-14 10:02:35 +02:00
} else {
OnProcessDataExchanged ? . Invoke ( this ) ;
2020-12-21 23:02:06 +01:00
}
2020-12-20 02:31:00 +01:00
}
2020-12-21 23:02:06 +01:00
Thread . Sleep ( INTERVALL_PROCESSDATA ) ;
}
Logging . Log ( LogLevel . INFO , "ECMaster: ProcessData(): finished" ) ;
}
public void ScanControllers ( )
{
driveControllers . Clear ( ) ;
2020-12-20 02:31:00 +01:00
2020-12-21 23:02:06 +01:00
for ( UInt16 slave_id = 1 ; slave_id < = CountSlaves ; slave_id + + )
{
if ( ReadSDO ( slave_id , 0x1000 , 0 , out byte [ ] bDeviceType ) )
2020-12-20 02:31:00 +01:00
{
2020-12-21 23:02:06 +01:00
ushort profilecode = BitConverter . ToUInt16 ( bDeviceType ) ;
Logging . Log ( LogLevel . INFO , "ECMaster: ScanControllers: Slave {0} has DeviceType {1} [{2}]" , slave_id , profilecode , bDeviceType . ToHexString ( ) ) ;
switch ( profilecode )
{
case 402 :
driveControllers . Add ( new CIA402Controller ( this , slave_id ) ) ;
break ;
}
2020-12-20 02:31:00 +01:00
}
2020-12-21 23:02:06 +01:00
}
2020-12-20 02:31:00 +01:00
}
2020-12-16 09:29:40 +01:00
2020-12-21 23:02:06 +01:00
bool WaitForState ( ECSlaveState expectedState , int timeout )
2020-12-16 09:29:40 +01:00
{
2020-12-21 23:02:06 +01:00
int waitIncrement = 100 ;
2020-12-16 09:29:40 +01:00
2020-12-21 23:02:06 +01:00
ECSlaveState currentState = IOLocked ( ( ) = > ECMBind . ecmbind_read_state ( ) ) ;
while ( ( timeout > 0 ) & & ( currentState ! = expectedState ) )
2020-12-16 09:29:40 +01:00
{
2020-12-21 23:02:06 +01:00
Thread . Sleep ( waitIncrement ) ;
timeout - = waitIncrement ;
currentState = IOLocked ( ( ) = > ECMBind . ecmbind_read_state ( ) ) ;
}
return currentState = = expectedState ;
}
2020-12-16 09:29:40 +01:00
2020-12-21 23:02:06 +01:00
object lockIOMap = new object ( ) ;
byte [ ] iomap = new byte [ 8192 ] ;
void SetupPDOMapping ( )
{
IOMapSize = ECMBind . ecmbind_config_map ( ) ;
Logging . Log ( LogLevel . DEBUG , "ECMaster: IOMapSize={0}" , IOMapSize ) ;
//UpdatePDOMap();
}
HashSet < PDOMappingRequest > mappingRequests = new HashSet < PDOMappingRequest > ( ) ;
public void ConfigureProcessDataMappings ( )
{
Logging . Log ( LogLevel . INFO , "configure PDO mappings" ) ;
for ( UInt16 slave = 1 ; slave < = CountSlaves ; slave + + )
{
List < int > rxPDOs = new List < int > ( ) ;
List < int > txPDOs = new List < int > ( ) ;
foreach ( PDOMappingRequest mappingRequest in mappingRequests )
2020-12-16 09:29:40 +01:00
{
2020-12-21 23:02:06 +01:00
if ( mappingRequest . Slave = = slave )
2020-12-20 02:31:00 +01:00
{
2020-12-21 23:02:06 +01:00
if ( GetSDOValue ( mappingRequest . Slave , mappingRequest . Index , mappingRequest . SubIndex , out SDOValue sdoValue ) )
2020-12-20 02:31:00 +01:00
{
2020-12-21 23:02:06 +01:00
int mappingEntry = ( mappingRequest . Index < < 16 ) | ( mappingRequest . SubIndex < < 8 ) | sdoValue . BitLength ;
Logging . Log ( LogLevel . DEBUG , "PDO mapping: {0:X8}" , mappingEntry ) ;
if ( mappingRequest . RxPDO )
rxPDOs . Add ( mappingEntry ) ;
else
txPDOs . Add ( mappingEntry ) ;
} else {
Logging . Log ( LogLevel . WARNING , "ECMaster: SetupProcessDataMappings(): could not retrieve SDOValue {0}:{1:X4}.{2}" , mappingRequest . Slave , mappingRequest . Index , mappingRequest . SubIndex ) ;
2020-12-20 02:31:00 +01:00
}
}
2020-12-16 09:29:40 +01:00
}
2020-12-21 23:02:06 +01:00
MemoryStream rxStream = new MemoryStream ( ) ;
rxStream . WriteByte ( ( byte ) rxPDOs . Count ) ;
rxStream . WriteByte ( 0 ) ;
foreach ( int pdoMapping in rxPDOs )
rxStream . WriteBytes ( pdoMapping . GetBytes ( ) ) ;
Logging . Log ( LogLevel . DEBUG , "RxPDO mappings for slave {0} => {1}" , slave , rxPDOs . Count ) ;
if ( ! WriteSDOCA ( slave , 0x1600 , 0 , rxStream . ToArray ( ) ) )
2020-12-16 09:29:40 +01:00
{
2020-12-21 23:02:06 +01:00
Logging . Log ( LogLevel . WARNING , "ECMaster: SetupProcessDataMappings(): could not write RxPDO mappings for slave {0}" , slave ) ;
}
MemoryStream txStream = new MemoryStream ( ) ;
txStream . WriteByte ( ( byte ) txPDOs . Count ) ;
txStream . WriteByte ( 0 ) ;
foreach ( int pdoMapping in txPDOs )
txStream . WriteBytes ( pdoMapping . GetBytes ( ) ) ;
Logging . Log ( LogLevel . DEBUG , "TxPDO mappings for slave {0} => {1}" , slave , txPDOs . Count ) ;
if ( ! WriteSDOCA ( slave , 0x1A00 , 0 , txStream . ToArray ( ) ) )
{
Logging . Log ( LogLevel . WARNING , "ECMaster: SetupProcessDataMappings(): could not write TxPDO mappings for slave {0}" , slave ) ;
2020-12-16 09:29:40 +01:00
}
2020-12-21 23:02:06 +01:00
if ( ! WriteSDOCA ( slave , 0x1c12 , 0 , new byte [ ] { 0x01 , 0x00 , 0x00 , 0x16 } ) )
{
Logging . Log ( LogLevel . WARNING , "ECMaster: SetupProcessDataMappings(): could not configure syncmanager 2 PDO assignment for slave {0}" , slave ) ;
}
if ( ! WriteSDOCA ( slave , 0x1c13 , 0 , new byte [ ] { 0x01 , 0x00 , 0x00 , 0x1A } ) )
{
Logging . Log ( LogLevel . WARNING , "ECMaster: SetupProcessDataMappings(): could not configure syncmanager 3 PDO assignment for slave {0}" , slave ) ;
}
2020-12-16 09:29:40 +01:00
}
}
2020-12-21 23:02:06 +01:00
public void RequestPDOMapping ( UInt16 slave , UInt16 index , byte subIndex , bool RxPDO ) = > RequestPDOMapping ( new PDOMappingRequest ( slave , index , subIndex , RxPDO ) ) ;
public void RequestPDOMapping ( IEnumerable < PDOMappingRequest > mappingRequests ) = > RequestPDOMapping ( mappingRequests . ToArray ( ) ) ;
public void RequestPDOMapping ( params PDOMappingRequest [ ] mappingRequests )
{
foreach ( PDOMappingRequest mappingRequest in mappingRequests )
this . mappingRequests . Add ( mappingRequest ) ;
}
void ScheduleRestart ( )
{
ThreadPool . QueueUserWorkItem ( ( o ) = > {
Stop ( ) ;
Thread . Sleep ( 500 ) ;
Start ( ) ;
} ) ;
}
2020-12-16 09:29:40 +01:00
2020-12-20 02:31:00 +01:00
public bool ReadSDO ( UInt16 slave , UInt16 index , byte subIndex , out byte [ ] rawValue )
2020-12-16 09:29:40 +01:00
{
2020-12-20 02:31:00 +01:00
rawValue =
new byte [ 128 ] ;
int dataSize ;
2020-12-16 09:29:40 +01:00
lock ( lockIOMap )
{
2020-12-20 02:31:00 +01:00
dataSize =
ECMBind . ecmbind_sdo_read ( slave , index , subIndex , rawValue , rawValue . Length ) ;
if ( dataSize < = 0 )
{
2020-12-21 23:02:06 +01:00
Logging . Log ( LogLevel . ERROR , "ECMBind.ecmbind_sdo_read({0},{1},{2},..,{3}) [{5}] == {4}" , slave , index , subIndex , rawValue . Length , rawValue . ToHexString ( ) , dataSize ) ;
2020-12-20 02:31:00 +01:00
throw new IOException ( ) ;
}
2020-12-16 09:29:40 +01:00
}
2020-12-20 02:31:00 +01:00
rawValue = rawValue . Slice ( 0 , dataSize ) ;
2020-12-16 09:29:40 +01:00
return true ;
}
2020-12-20 02:31:00 +01:00
2020-12-21 23:02:06 +01:00
public bool ReadSDOCA ( UInt16 slave , UInt16 index , byte subIndex , out byte [ ] rawValue )
2020-12-16 09:29:40 +01:00
{
2020-12-21 23:02:06 +01:00
rawValue =
new byte [ 128 ] ;
int dataSize ;
2020-12-16 09:29:40 +01:00
2020-12-21 23:02:06 +01:00
lock ( lockIOMap )
2020-12-16 09:29:40 +01:00
{
2020-12-21 23:02:06 +01:00
dataSize =
ECMBind . ecmbind_sdo_read_ca ( slave , index , subIndex , rawValue , rawValue . Length ) ;
if ( dataSize < = 0 )
2020-12-16 09:29:40 +01:00
{
2020-12-21 23:02:06 +01:00
Logging . Log ( LogLevel . ERROR , "ECMBind.ecmbind_sdo_read_ca({0},{1},{2},..,{3}) == {4}" , slave , index , subIndex , rawValue . Length , rawValue . ToHexString ( ) ) ;
throw new IOException ( ) ;
2020-12-16 09:29:40 +01:00
}
}
2020-12-21 23:02:06 +01:00
rawValue = rawValue . Slice ( 0 , dataSize ) ;
return true ;
}
public bool WriteSDO ( UInt16 slave , UInt16 index , byte subIndex , byte [ ] rawValue )
{
int result ;
lock ( lockIOMap )
result = ECMBind . ecmbind_sdo_write ( slave , index , subIndex , rawValue , rawValue . Length ) ;
//Logging.Log(LogLevel.DEBUG, "ECMaster: WriteSDO({0},{1},{2},{3}) => {4}", slave, index, subIndex, rawValue.ToHexString(), result);
return result > 0 ;
2020-12-16 09:29:40 +01:00
}
2020-12-21 23:02:06 +01:00
public bool WriteSDOCA ( UInt16 slave , UInt16 index , byte subIndex , byte [ ] rawValue )
2020-12-16 09:29:40 +01:00
{
2020-12-21 23:02:06 +01:00
int result ;
lock ( lockIOMap )
result = ECMBind . ecmbind_sdo_write_ca ( slave , index , subIndex , rawValue , rawValue . Length ) ;
//Logging.Log(LogLevel.DEBUG, "ECMaster: WriteSDOCA({0},{1},{2},{3}) => {4}", slave, index, subIndex, rawValue.ToHexString(), result);
return result > 0 ;
2020-12-16 09:29:40 +01:00
}
2020-12-21 23:02:06 +01:00
public ECSlaveState ReadSlaveState ( int slave ) = > IOLocked ( ( ) = > ECMBind . ecmbind_get_slave_state ( slave ) ) ;
int RequestSlaveState ( int slave , ECSlaveState slaveState ) = > IOLocked ( ( ) = > ECMBind . ecmbind_write_slave_state ( slave , slaveState ) ) ;
2020-12-20 02:31:00 +01:00
public bool GetSlave ( UInt16 slave_id , out ECSlave slave )
2020-12-16 09:29:40 +01:00
{
2020-12-20 02:31:00 +01:00
if ( ! slaves . TryGetValue ( slave_id , out slave ) )
{
if ( slave_id > CountSlaves )
return false ;
slave = new ECSlave ( this , slave_id ) ;
slaves . Add ( slave_id , slave ) ;
}
2020-12-16 09:29:40 +01:00
return true ;
}
2020-12-20 02:31:00 +01:00
public bool GetSDOValue ( SDOAddr address , out SDOValue sdoValue ) = > GetSDOValue ( address . Slave , address . Index , address . SubIndex , out sdoValue ) ;
public bool GetSDOValue ( UInt16 slave_id , UInt16 index , byte subIndex , out SDOValue sdoValue )
2020-12-16 09:29:40 +01:00
{
2020-12-20 02:31:00 +01:00
sdoValue = null ;
return GetSlave ( slave_id , out ECSlave slave ) & & slave . GetDescriptor ( index , out SDODescriptor descriptor ) & & descriptor . GetValue ( subIndex , out sdoValue ) ;
}
2020-12-16 09:29:40 +01:00
2020-12-20 02:31:00 +01:00
public bool GetIOmapData ( int offset , int length , out byte [ ] rawData )
{
lock ( lockIOMap )
if ( ( offset > = 0 ) & & ( length > 0 ) & & ( offset + length < iomap . Length ) )
2020-12-16 09:29:40 +01:00
{
2020-12-20 02:31:00 +01:00
rawData = iomap . Slice ( offset , length ) ;
2020-12-21 23:02:06 +01:00
//Logging.Log(LogLevel.ERROR, "GetIOmapData({0},{1})", offset, length);
2020-12-20 02:31:00 +01:00
return true ;
2020-12-16 09:29:40 +01:00
}
2020-12-20 02:31:00 +01:00
Logging . Log ( LogLevel . ERROR , "GetIOmapData({0},{1}) failed" , offset , length ) ;
rawData = null ;
return false ;
2020-12-16 09:29:40 +01:00
}
2020-12-20 02:31:00 +01:00
public bool SetIOmapData ( int offset , int length , byte [ ] rawData )
2020-12-16 09:29:40 +01:00
{
2020-12-20 02:31:00 +01:00
lock ( lockIOMap )
if ( ( offset > = 0 ) & & ( length > 0 ) & & ( offset + length < iomap . Length ) )
2020-12-16 09:29:40 +01:00
{
2020-12-21 23:02:06 +01:00
//Logging.Log(LogLevel.DEBUG, "ECMaster: SetIOmapData({0},{1},{2})", offset, length, rawData.ToHexString());
2020-12-20 02:31:00 +01:00
Buffer . BlockCopy ( rawData , 0 , iomap , offset , length ) ;
return true ;
2020-12-16 09:29:40 +01:00
}
2020-12-20 02:31:00 +01:00
Logging . Log ( LogLevel . ERROR , "SetIOmapData({0},{1}) failed" , offset , length ) ;
rawData = null ;
return false ;
2020-12-16 09:29:40 +01:00
}
2020-12-20 02:31:00 +01:00
public bool GetPDOItem ( SDOValue sdoValue , out PDO pdo )
2020-12-16 09:29:40 +01:00
{
2020-12-20 02:31:00 +01:00
foreach ( PDO _pdo in pdoMap )
2020-12-16 09:29:40 +01:00
{
2020-12-20 02:31:00 +01:00
if ( _pdo . SDOValue . Equals ( sdoValue ) )
2020-12-16 09:29:40 +01:00
{
2020-12-20 02:31:00 +01:00
pdo = _pdo ;
return true ;
2020-12-16 09:29:40 +01:00
}
}
2020-12-20 02:31:00 +01:00
pdo = null ;
return false ;
2020-12-16 09:29:40 +01:00
}
public void UpdatePDOMap ( )
{
List < PDO > pdoList = new List < PDO > ( ) ;
2020-12-21 23:02:06 +01:00
lock ( lockIOMap )
ECMBind . ecmbind_pdo_enumerate ( ( UInt16 slave , UInt16 index , byte subindex , int addr_offset , int addr_bit , int bitlength ) = > {
if ( addr_bit ! = 0 )
Logging . Log ( LogLevel . WARNING , "currently only PDO mappings on byte boundaries are supported" ) ;
else
pdoList . Add ( new PDO ( this , slave , index , subindex ) {
AddressOffset = addr_offset ,
AddressBit = addr_bit ,
BitLength = bitlength
} ) ;
2020-12-16 09:29:40 +01:00
} ) ;
2020-12-21 23:02:06 +01:00
lock ( lockIOMap )
2020-12-16 09:29:40 +01:00
{
IOMapPtr = ECMBind . ecmbind_get_iomap ( ) ;
2020-12-20 02:31:00 +01:00
pdoMap = pdoList ;
2020-12-16 09:29:40 +01:00
}
}
T IOLocked < T > ( Func < T > f ) {
lock ( lockIOMap )
return f ( ) ;
}
}
}