Fix sdo_writer not detecting emergency messages by skipping wrong arbitration ids

Author: tobiasjaster

.devcontainer/devcontainer.json

@@ -1,8 +1,8 @@
 {
-  "name": "CANOPENREST",
+  "name": "GoCanOpen",
   "image": "devcontainer-go-dev:latest",
   "runArgs": [
-    "--name=CANOPENREST_Devcontainer"
+    "--name=GoCanOpen_Devcontainer"
   ],
   "customizations": {
     // Configure properties specific to VS Code.

interfaces.go

@@ -0,0 +1,23 @@
+package canopen
+
+import (
+	"time"
+
+	"github.com/angelodlfrtr/go-can"
+)
+
+type ISDOClient interface {
+	FindName(name string) DicObject
+	Read(index uint16, subIndex uint8) ([]byte, error)
+	Send(req []byte, expectFunc networkFramesChanFilterFunc, timeout *time.Duration, retryCount *int) (*can.Frame, error)
+	SendRequest(req []byte) error
+	Write(index uint16, subIndex uint8, forceSegment bool, data []byte) error
+}
+
+type INode interface {
+	GetId() int
+	FindName(name string) DicObject
+	Send(arbID uint32, data []byte) error
+	AcquireFramesChanFromNetwork(filterFunc networkFramesChanFilterFunc) *NetworkFramesChan
+	ReleaseFramesChanFromNetwork(id string)
+}

node.go

@@ -1,5 +1,7 @@
 package canopen
 
+import "errors"
+
 // Node is a canopen node
 type Node struct {
 	// Each node has an id, which is ArbitrationID & 0x7F
@@ -23,6 +25,42 @@ func NewNode(id int, network *Network, objectDic *DicObjectDic) *Node {
 	return node
 }
 
+// GetId returns Node ID
+func (node *Node) GetId() int {
+	return node.ID
+}
+
+// FindName gets DicObject from ObjectDic
+func (node *Node) FindName(name string) DicObject {
+	if node.ObjectDic == nil {
+		return nil
+	}
+	return node.ObjectDic.FindName(name)
+}
+
+// Send sends Frame with arbitration ID by connected network
+func (node *Node) Send(arbID uint32, data []byte) error {
+	if node.Network == nil {
+		return errors.New("Network not defined")
+	}
+	return node.Network.Send(arbID, data)
+}
+
+// AcquireFramesChanFromNetwork gets new Channel for given FilterFunc
+func (node *Node) AcquireFramesChanFromNetwork(filterFunc networkFramesChanFilterFunc) *NetworkFramesChan {
+	if node.Network == nil {
+		return nil
+	}
+	return node.Network.AcquireFramesChan(filterFunc)
+}
+
+// ReleaseFramesChanFromNetwork free channel with given id from network
+func (node *Node) ReleaseFramesChanFromNetwork(id string) {
+	if node.Network != nil {
+		node.Network.ReleaseFramesChan(id)
+	}
+}
+
 // SetNetwork set node.Network to the desired network
 func (node *Node) SetNetwork(network *Network) {
 	node.Network = network

sdo.go

@@ -26,29 +26,29 @@ const (
 
 // SDOClient represent an SDO client
 type SDOClient struct {
-	Node      *Node
+	Node      INode
 	RXCobID   uint32
 	TXCobID   uint32
 	SendQueue []string
 }
 
-func NewSDOClient(node *Node) *SDOClient {
+func NewSDOClient(node INode) *SDOClient {
 	return &SDOClient{
 		Node:      node,
-		RXCobID:   uint32(0x600 + node.ID),
-		TXCobID:   uint32(0x580 + node.ID),
+		RXCobID:   uint32(0x600 + node.GetId()),
+		TXCobID:   uint32(0x580 + node.GetId()),
 		SendQueue: []string{},
 	}
 }
 
 // SendRequest to network bus
 func (sdoClient *SDOClient) SendRequest(req []byte) error {
-	return sdoClient.Node.Network.Send(sdoClient.RXCobID, req)
+	return sdoClient.Node.Send(sdoClient.RXCobID, req)
 }
 
 // FindName find an sdo object from object dictionary by name
 func (sdoClient *SDOClient) FindName(name string) DicObject {
-	if ob := sdoClient.Node.ObjectDic.FindName(name); ob != nil {
+	if ob := sdoClient.Node.FindName(name); ob != nil {
 		ob.SetSDO(sdoClient)
 		return ob
 	}
@@ -83,12 +83,22 @@ func (sdoClient *SDOClient) Send(
 		retryCount = &rtc
 	}
 
-	framesChan := sdoClient.Node.Network.AcquireFramesChan(expectFunc)
+	var expectSdoFunc networkFramesChanFilterFunc
+	if expectFunc != nil {
+		expectSdoFilterFunc := func(frm *can.Frame) bool {
+			arbitrationId := frm.ArbitrationID
+			if arbitrationId != sdoClient.TXCobID {
+				return false
+			}
+			return (*expectFunc)(frm)
+		}
+		expectSdoFunc = &expectSdoFilterFunc
+	}
+	framesChan := sdoClient.Node.AcquireFramesChanFromNetwork(expectSdoFunc)
+	defer sdoClient.Node.ReleaseFramesChanFromNetwork(framesChan.ID)
 
 	// Retry loop
 	remainingCount := *retryCount
-	var frm *can.Frame
-
 	for {
 		if remainingCount == 0 {
 			break
@@ -100,32 +110,27 @@ func (sdoClient *SDOClient) Send(
 
 		timer := time.NewTimer(*timeout)
 
-		select {
-		case <-timer.C:
-			// Double timeout for each retry
-			newTimeout := *timeout * 2
-			timeout = &newTimeout
-		case fr := <-framesChan.C:
-			frm = fr
+		loop := true
+		for {
+			if !loop {
+				break
+			}
+			select {
+			case <-timer.C:
+				// Double timeout for each retry
+				newTimeout := *timeout * 2
+				timeout = &newTimeout
+				loop = false
+			case fr := <-framesChan.C:
+				return fr, nil
+			}
 		}
 
 		timer.Stop()
 		remainingCount--
-
-		if frm != nil {
-			break
-		}
-	}
-
-	// Release data chan
-	sdoClient.Node.Network.ReleaseFramesChan(framesChan.ID)
-
-	// If no frm, timeout execeded
-	if frm == nil {
-		return nil, errors.New("timeout execeded")
 	}
 
-	return frm, nil
+	return nil, errors.New("timeout execeded")
 }
 
 // Read sdo