/// <summary>
/// Send a SDO packet, with command and payload, should be only called from SDO state machine
/// </summary>
/// <param name="cmd">SDO command byte</param>
/// <param name="payload">Data payload to send</param>
private void sendpacket(byte cmd, byte[] payload)
{
canpacket p = new canpacket();
p.cob = (UInt16)(0x600 + node);
p.len = 8;
p.data = new byte[8];
p.data[0] = cmd;
p.data[1] = (byte)index;
p.data[2] = (byte)(index >> 8);
p.data[3] = subindex;
int sendlength = 4;
if (payload.Length < 4)
{
sendlength = payload.Length;
}
for (int x = 0; x < sendlength; x++)
{
p.data[4 + x] = payload[x];
}
if (dbglevel == debuglevel.DEBUG_ALL)
{
Console.WriteLine(String.Format("Sending a new SDO packet: {0}", p.ToString()));
}
if (can.isopen())
{
can.SendPacket(p);
}
}
public void NMT_stop(byte nodeid = 0)
{
canpacket p = new canpacket();
p.cob = 000;
p.len = 2;
p.data = new byte[2];
p.data[0] = 0x80;
p.data[1] = nodeid;
SendPacket(p);
}
/// <summary>
/// Send a Can packet on the bus
/// </summary>
/// <param name="p"></param>
public void SendPacket(canpacket p)
{
DriverInstance.Message msg = p.ToMsg();
driver.cansend(msg);
if (echo == true)
{
Driver_rxmessage(msg);
}
}
public void NMT_ReseCommunication(byte nodeid = 0)
{
canpacket p = new canpacket();
p.cob = 000;
p.len = 2;
p.data = new byte[2];
p.data[0] = 0x81;
p.data[1] = nodeid;
SendPacket(p);
}
public void writePDO(UInt16 cob, byte[] payload)
{
canpacket p = new canpacket();
p.cob = cob;
p.len = (byte)payload.Length;
p.data = new byte[p.len];
for (int x = 0; x < payload.Length; x++)
{
p.data[x] = payload[x];
}
SendPacket(p);
}
/// <summary>
/// Segmented transfer update function, should be only called from SDO state machine
/// </summary>
/// <param name="cmd">SDO command byte</param>
/// <param name="payload">Data payload</param>
private void sendpacketsegment(byte cmd, byte[] payload)
{
canpacket p = new canpacket();
p.cob = (UInt16)(0x600 + node);
p.len = 8;
p.data = new byte[8];
p.data[0] = cmd;
for (int x = 0; x < payload.Length; x++)
{
p.data[1 + x] = payload[x];
}
if (dbglevel == debuglevel.DEBUG_ALL)
{
Console.WriteLine(String.Format("Sending a new segmented SDO packet: {0}", p.ToString()));
}
can.SendPacket(p);
}
/// <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);
}
private static void Lco_nmtevent(libCanopenSimple.canpacket p)
{
Console.WriteLine("NMT :" + p.ToString());
}
private static void Lco_sdoevent(libCanopenSimple.canpacket p, DateTime dt)
{
Console.WriteLine("SDO :" + p.ToString());
}
