/// <summary>
/// Construct a new SDO object
/// </summary>
/// <param name="can">a libCanopenSimple object that will give access to the hardware</param>
/// <param name="node">The note to talk to (UInt16)</param>
/// <param name="index">The index in the object dictionary to access</param>
/// <param name="subindex">The subindex in the object dictionary to access</param>
/// <param name="dir">Direction of transfer</param>
/// <param name="completedcallback">Optional, completed callback (or null if not required)</param>
/// <param name="databuffer">A byte array of data to be transfered to or from if more than 4 bytes</param>
public SDO(libCanopenSimple can, byte node, UInt16 index, byte subindex, direction dir, Action <SDO> completedcallback, byte[] databuffer)
{
this.can = can;
this.index = index;
this.subindex = subindex;
this.node = node;
this.dir = dir;
this.completedcallback = completedcallback;
this.databuffer = databuffer;
finishedevent = new ManualResetEvent(false);
state = SDO_STATE.SDO_INIT;
dbglevel = can.dbglevel;
}
/// <summary>
/// SDO Instance processor, process current SDO reply and decide what to do next
/// </summary>
/// <param name="cp">SDO Canpacket to process</param>
/// <returns></returns>
public bool SDOProcess(canpacket cp)
{
int SCS = cp.data[0] >> 5; //7-5
int n = (0x03 & (cp.data[0] >> 2)); //3-2 data size for normal packets
returnlen = 8 * (4 - n);
int e = (0x01 & (cp.data[0] >> 1)); // expidited flag
int s = (cp.data[0] & 0x01); // data size set flag
int sn = (0x07 & (cp.data[0] >> 1)); //3-1 data size for segment packets
int t = (0x01 & (cp.data[0] >> 4)); //toggle flag
int c = 0x01 & cp.data[0]; //More segments to upload?
//ERROR abort
if (SCS == 0x04)
{
expitideddata = (UInt32)(cp.data[4] + (cp.data[5] << 8) + (cp.data[6] << 16) + (cp.data[7] << 24));
databuffer = BitConverter.GetBytes(expitideddata);
state = SDO_STATE.SDO_ERROR;
Console.WriteLine("SDO Error on {0:x4}/{1:x2} {2:x8}", this.index, this.subindex, expitideddata);
if (completedcallback != null)
{
completedcallback(this);
}
return(true);
}
//Write complete
if (SCS == 0x03)
{
UInt16 index = (UInt16)(cp.data[1] + (cp.data[2] << 8));
byte sub = cp.data[3];
int node = cp.cob - 0x580;
foreach (SDO sdo in activeSDO)
{
if (sdo.node == node)
{
if ((index == sdo.index && sub == sdo.subindex)) //if segments break its here
{
if (expitided == false)
{
state = SDO_STATE.SDO_HANDSHAKE;
requestNextSegment(false);
return(false);
}
}
}
}
state = SDO_STATE.SDO_FINISHED;
if (completedcallback != null)
{
completedcallback(this);
}
return(true);
}
//Write segment complete
if (SCS == 0x01)
{
if (totaldata < expitideddata)
{
lasttoggle = !lasttoggle;
requestNextSegment(lasttoggle);
}
else
{
state = SDO_STATE.SDO_FINISHED;
if (completedcallback != null)
{
completedcallback(this);
}
}
}
//if expedited just handle the data
if (SCS == 0x02 && e == 1)
{
//Expidited and length are set so its a regular short transfer
expitideddata = (UInt32)(cp.data[4] + (cp.data[5] << 8) + (cp.data[6] << 16) + (cp.data[7] << 24));
databuffer = BitConverter.GetBytes(expitideddata);
state = SDO_STATE.SDO_FINISHED;
if (completedcallback != null)
{
try
{
completedcallback(this);
}
catch { }
}
return(true);
}
if (SCS == 0x02)
{
UInt32 count = (UInt32)(cp.data[4] + (cp.data[5] << 8) + (cp.data[6] << 16) + (cp.data[7] << 24));
Console.WriteLine("RX Segmented transfer start length is {0}", count);
expitideddata = count;
databuffer = new byte[expitideddata];
totaldata = 0;
//Request next segment
requestNextSegment(false); //toggle off on first request
return(false);
}
//segmented transfer
UInt32 scount = (UInt32)(7 - sn);
//Segments toggle on
if (SCS == 0x00)
{
Console.WriteLine("RX Segmented transfer update length is {0} -- {1}", scount, totaldata);
for (int x = 0; x < scount; x++)
{
if ((totaldata + x) < databuffer.Length)
{
databuffer[totaldata + x] = cp.data[1 + x];
}
}
totaldata += 7;
if ((totaldata < expitideddata) && c == 0)
{
lasttoggle = !lasttoggle;
requestNextSegment(lasttoggle);
}
else
{
state = SDO_STATE.SDO_FINISHED;
if (completedcallback != null)
{
completedcallback(this);
}
}
}
return(false);
}
/// <summary>
/// State machine for a specific SDO instance
/// </summary>
private void kick_SDOp()
{
if (state != SDO_STATE.SDO_INIT && DateTime.Now > timeout)
{
state = SDO_STATE.SDO_ERROR;
Console.WriteLine("SDO Timeout Error on {0:x4}/{1:x2} {2:x8}", this.index, this.subindex, expitideddata);
if (completedcallback != null)
{
completedcallback(this);
}
return;
}
if (state == SDO_STATE.SDO_INIT)
{
timeout = DateTime.Now + new TimeSpan(0, 0, 5);
state = SDO_STATE.SDO_SENT;
if (dir == direction.SDO_READ)
{
byte cmd = 0x40;
byte[] payload = new byte[4];
sendpacket(cmd, payload);
}
if (dir == direction.SDO_WRITE)
{
bool wpsent = false;
byte cmd = 0;
expitided = true;
switch (databuffer.Length)
{
case 1:
cmd = 0x2f;
break;
case 2:
cmd = 0x2b;
break;
case 3:
cmd = 0x27;
break;
case 4:
cmd = 0x23;
break;
default:
//Bigger than 4 bytes we use segmented transfer
cmd = 0x21;
expitided = false;
byte[] payload = new byte[4];
payload[0] = (byte)databuffer.Length;
payload[1] = (byte)(databuffer.Length >> 8);
payload[2] = (byte)(databuffer.Length >> 16);
payload[3] = (byte)(databuffer.Length >> 24);
expitideddata = (UInt32)databuffer.Length;
totaldata = 0;
wpsent = true;
sendpacket(cmd, payload);
break;
}
if (wpsent == false)
{
sendpacket(cmd, databuffer);
}
}
}
}
