/// <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); } } } }