Skip to content

Commit b659b93

Browse files
tobiasjastertobiasjaster
committed
Fix sdo_writer not detecting emergency messages by skipping wrong arbitration ids
1 parent e308e4a commit b659b93

File tree

3 files changed

+76
-42
lines changed

3 files changed

+76
-42
lines changed

‎sdo.go‎

Lines changed: 28 additions & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -41,6 +41,16 @@ func NewSDOClient(node *Node) *SDOClient {
4141
}
4242
}
4343

44+
// GetRxCobId returns RxCobId
45+
func (sdoClient *SDOClient) GetRxCobId() uint32 {
46+
return sdoClient.RXCobID
47+
}
48+
49+
// GetTxCobId returns RxCobId
50+
func (sdoClient *SDOClient) GetTxCobId() uint32 {
51+
return sdoClient.TXCobID
52+
}
53+
4454
// SendRequest to network bus
4555
func (sdoClient *SDOClient) SendRequest(req []byte) error {
4656
return sdoClient.Node.Network.Send(sdoClient.RXCobID, req)
@@ -84,11 +94,10 @@ func (sdoClient *SDOClient) Send(
8494
}
8595

8696
framesChan := sdoClient.Node.Network.AcquireFramesChan(expectFunc)
97+
defer sdoClient.Node.Network.ReleaseFramesChan(framesChan.ID)
8798

8899
// Retry loop
89100
remainingCount := *retryCount
90-
var frm *can.Frame
91-
92101
for {
93102
if remainingCount == 0 {
94103
break
@@ -100,32 +109,29 @@ func (sdoClient *SDOClient) Send(
100109

101110
timer := time.NewTimer(*timeout)
102111

103-
select {
104-
case <-timer.C:
105-
// Double timeout for each retry
106-
newTimeout := *timeout * 2
107-
timeout = &newTimeout
108-
case fr := <-framesChan.C:
109-
frm = fr
112+
loop := true
113+
for {
114+
if !loop {
115+
break
116+
}
117+
select {
118+
case <-timer.C:
119+
// Double timeout for each retry
120+
newTimeout := *timeout * 2
121+
timeout = &newTimeout
122+
loop = false
123+
case fr := <-framesChan.C:
124+
if fr.ArbitrationID == sdoClient.TXCobID {
125+
return fr, nil
126+
}
127+
}
110128
}
111129

112130
timer.Stop()
113131
remainingCount--
114-
115-
if frm != nil {
116-
break
117-
}
118-
}
119-
120-
// Release data chan
121-
sdoClient.Node.Network.ReleaseFramesChan(framesChan.ID)
122-
123-
// If no frm, timeout execeded
124-
if frm == nil {
125-
return nil, errors.New("timeout execeded")
126132
}
127133

128-
return frm, nil
134+
return nil, errors.New("timeout execeded")
129135
}
130136

131137
// Read sdo

‎sdo_writer.go‎

Lines changed: 16 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -8,6 +8,8 @@ import (
88
)
99

1010
type ISDOClient interface {
11+
GetRxCobId() uint32
12+
GetTxCobId() uint32
1113
FindName(name string) DicObject
1214
Read(index uint16, subIndex uint8) ([]byte, error)
1315
Send(req []byte, expectFunc networkFramesChanFilterFunc, timeout *time.Duration, retryCount *int) (*can.Frame, error)
@@ -89,6 +91,11 @@ func (writer *SDOWriter) RequestDownload(data []byte) error {
8991

9092
func (writer *SDOWriter) writeBufferExpedited(cmd []byte) error {
9193
expectFunc := func(frm *can.Frame) bool {
94+
arbitrationId := frm.ArbitrationID
95+
if arbitrationId != writer.SDOClient.GetTxCobId() {
96+
return false
97+
}
98+
9299
resCommand := frm.Data[0]
93100
resIndex := binary.LittleEndian.Uint16(frm.Data[1:])
94101
resSubindex := frm.Data[3]
@@ -115,6 +122,11 @@ func (writer *SDOWriter) writeBufferExpedited(cmd []byte) error {
115122

116123
func (writer *SDOWriter) writeBufferSegmented(cmd []byte, data []byte) error {
117124
expectFunc := func(frm *can.Frame) bool {
125+
arbitrationId := frm.ArbitrationID
126+
if arbitrationId != writer.SDOClient.GetTxCobId() {
127+
return false
128+
}
129+
118130
resCommand := frm.Data[0]
119131
resIndex := binary.LittleEndian.Uint16(frm.Data[1:])
120132
resSubindex := frm.Data[3]
@@ -155,6 +167,10 @@ func (writer *SDOWriter) writeBufferSegmented(cmd []byte, data []byte) error {
155167
copy(buf[1:frameSize+1], data[writer.Pos:writer.Pos+frameSize])
156168

157169
expectFunc := func(frm *can.Frame) bool {
170+
arbitrationId := frm.ArbitrationID
171+
if arbitrationId != writer.SDOClient.GetTxCobId() {
172+
return false
173+
}
158174
resCommand := frm.Data[0]
159175
// Check response validity
160176
if (resCommand & 0xE0) != SDOResponseSegmentDownload {

‎sdo_writer_test.go‎

Lines changed: 32 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -19,24 +19,24 @@ var (
1919
)
2020

2121
func getSDOClientMockExpeditedSuccess() *SDOClientMock {
22-
client := &SDOClientMock{}
23-
frame1 := can.Frame{Data: [8]byte{0x60, 0xE8, 0x03, 0x02, 0x00, 0x00, 0x00, 0x00}}
22+
client := &SDOClientMock{RXCobID: 0x600, TXCobID: 0x580}
23+
frame1 := can.Frame{ArbitrationID: 0x580, Data: [8]byte{0x60, 0xE8, 0x03, 0x02, 0x00, 0x00, 0x00, 0x00}}
2424
client.On("Send", []byte{0x23, 0xE8, 0x03, 0x02, 0x4C, 0x69, 0x6E, 0x65}).Return(&frame1, nil)
2525
return client
2626
}
2727

2828
func getSDOClientMockExpeditedFailed() *SDOClientMock {
29-
client := &SDOClientMock{}
29+
client := &SDOClientMock{RXCobID: 0x600, TXCobID: 0x580}
3030
client.On("Send", []byte{0x23, 0xE8, 0x03, 0x02, 0x4C, 0x69, 0x6E, 0x65}).Return(nil, errors.New("Failed to send frame"))
3131
return client
3232
}
3333

3434
func getSDOClientMockSegmentedSuccess() *SDOClientMock {
35-
client := &SDOClientMock{}
36-
frame1 := can.Frame{Data: [8]byte{0x60, 0xE8, 0x03, 0x02, 0x00, 0x00, 0x00, 0x00}}
37-
frame2 := can.Frame{Data: [8]byte{0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}}
38-
frame3 := can.Frame{Data: [8]byte{0x30, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}}
39-
frame4 := can.Frame{Data: [8]byte{0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}}
35+
client := &SDOClientMock{RXCobID: 0x600, TXCobID: 0x580}
36+
frame1 := can.Frame{ArbitrationID: 0x580, Data: [8]byte{0x60, 0xE8, 0x03, 0x02, 0x00, 0x00, 0x00, 0x00}}
37+
frame2 := can.Frame{ArbitrationID: 0x580, Data: [8]byte{0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}}
38+
frame3 := can.Frame{ArbitrationID: 0x580, Data: [8]byte{0x30, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}}
39+
frame4 := can.Frame{ArbitrationID: 0x580, Data: [8]byte{0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}}
4040
client.On("Send", []byte{0x21, 0xE8, 0x03, 0x02, 0x13, 0x00, 0x00, 0x00}).Return(&frame1, nil)
4141
client.On("Send", []byte{0x00, 0x53, 0x69, 0x7A, 0x65, 0x4C, 0x6F, 0x6E}).Return(&frame2, nil)
4242
client.On("Send", []byte{0x10, 0x67, 0x65, 0x72, 0x41, 0x73, 0x4F, 0x6E}).Return(&frame3, nil)
@@ -45,46 +45,58 @@ func getSDOClientMockSegmentedSuccess() *SDOClientMock {
4545
}
4646

4747
func getSDOClientMockSegmentedFailed1() *SDOClientMock {
48-
client := &SDOClientMock{}
49-
frame1 := can.Frame{Data: [8]byte{0x00, 0xE8, 0x03, 0x02, 0x00, 0x00, 0x00, 0x00}}
48+
client := &SDOClientMock{RXCobID: 0x600, TXCobID: 0x580}
49+
frame1 := can.Frame{ArbitrationID: 0x580, Data: [8]byte{0x00, 0xE8, 0x03, 0x02, 0x00, 0x00, 0x00, 0x00}}
5050
client.On("Send", []byte{0x21, 0xE8, 0x03, 0x02, 0x13, 0x00, 0x00, 0x00}).Return(&frame1, nil)
5151
return client
5252
}
5353

5454
func getSDOClientMockSegmentedFailed2() *SDOClientMock {
55-
client := &SDOClientMock{}
56-
frame1 := can.Frame{Data: [8]byte{0x60, 0xE8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}}
55+
client := &SDOClientMock{RXCobID: 0x600, TXCobID: 0x580}
56+
frame1 := can.Frame{ArbitrationID: 0x580, Data: [8]byte{0x60, 0xE8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}}
5757
client.On("Send", []byte{0x21, 0xE8, 0x03, 0x02, 0x13, 0x00, 0x00, 0x00}).Return(&frame1, nil)
5858
return client
5959
}
6060

6161
func getSDOClientMockSegmentedFailed3() *SDOClientMock {
62-
client := &SDOClientMock{}
63-
frame1 := can.Frame{Data: [8]byte{0x60, 0xE8, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00}}
62+
client := &SDOClientMock{RXCobID: 0x600, TXCobID: 0x580}
63+
frame1 := can.Frame{ArbitrationID: 0x580, Data: [8]byte{0x60, 0xE8, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00}}
6464
client.On("Send", []byte{0x21, 0xE8, 0x03, 0x02, 0x13, 0x00, 0x00, 0x00}).Return(&frame1, nil)
6565
return client
6666
}
6767

6868
func getSDOClientMockSegmentedFailed4() *SDOClientMock {
69-
client := &SDOClientMock{}
70-
frame1 := can.Frame{Data: [8]byte{0x60, 0xE8, 0x03, 0x02, 0x00, 0x00, 0x00, 0x00}}
71-
frame2 := can.Frame{Data: [8]byte{0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}}
69+
client := &SDOClientMock{RXCobID: 0x600, TXCobID: 0x580}
70+
frame1 := can.Frame{ArbitrationID: 0x580, Data: [8]byte{0x60, 0xE8, 0x03, 0x02, 0x00, 0x00, 0x00, 0x00}}
71+
frame2 := can.Frame{ArbitrationID: 0x580, Data: [8]byte{0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}}
7272
client.On("Send", []byte{0x21, 0xE8, 0x03, 0x02, 0x13, 0x00, 0x00, 0x00}).Return(&frame1, nil)
7373
client.On("Send", []byte{0x00, 0x53, 0x69, 0x7A, 0x65, 0x4C, 0x6F, 0x6E}).Return(&frame2, nil)
7474
return client
7575
}
7676

7777
func getSDOClientMockSegmentedFailed5() *SDOClientMock {
78-
client := &SDOClientMock{}
79-
frame1 := can.Frame{Data: [8]byte{0x60, 0xE8, 0x03, 0x02, 0x00, 0x00, 0x00, 0x00}}
80-
frame2 := can.Frame{Data: [8]byte{0x30, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}}
78+
client := &SDOClientMock{RXCobID: 0x600, TXCobID: 0x580}
79+
frame1 := can.Frame{ArbitrationID: 0x580, Data: [8]byte{0x60, 0xE8, 0x03, 0x02, 0x00, 0x00, 0x00, 0x00}}
80+
frame2 := can.Frame{ArbitrationID: 0x580, Data: [8]byte{0x30, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}}
8181
client.On("Send", []byte{0x21, 0xE8, 0x03, 0x02, 0x13, 0x00, 0x00, 0x00}).Return(&frame1, nil)
8282
client.On("Send", []byte{0x00, 0x53, 0x69, 0x7A, 0x65, 0x4C, 0x6F, 0x6E}).Return(&frame2, nil)
8383
return client
8484
}
8585

8686
type SDOClientMock struct {
8787
mock.Mock
88+
RXCobID uint32
89+
TXCobID uint32
90+
}
91+
92+
// GetRxCobId returns RxCobId
93+
func (s *SDOClientMock) GetRxCobId() uint32 {
94+
return s.RXCobID
95+
}
96+
97+
// GetTxCobId returns RxCobId
98+
func (s *SDOClientMock) GetTxCobId() uint32 {
99+
return s.TXCobID
88100
}
89101

90102
func (s *SDOClientMock) FindName(name string) DicObject {

0 commit comments

Comments
 (0)