-
-
Notifications
You must be signed in to change notification settings - Fork 1k
/
mavlink_driver_test.go
89 lines (74 loc) · 2.09 KB
/
mavlink_driver_test.go
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
//nolint:forcetypeassert,nilnil // ok here
package mavlink
import (
"io"
"strings"
"testing"
"time"
"github.com/stretchr/testify/assert"
"github.com/stretchr/testify/require"
"gobot.io/x/gobot/v2"
common "gobot.io/x/gobot/v2/platforms/mavlink/common"
)
var _ gobot.Driver = (*Driver)(nil)
func initTestMavlinkDriver() *Driver {
m := NewAdaptor("/dev/null")
m.connect = func(port string) (io.ReadWriteCloser, error) { return nil, nil }
m.sp = nullReadWriteCloser{}
return NewDriver(m)
}
func TestMavlinkDriver(t *testing.T) {
m := NewAdaptor("/dev/null")
m.sp = nullReadWriteCloser{}
m.connect = func(port string) (io.ReadWriteCloser, error) { return nil, nil }
d := NewDriver(m)
assert.NotNil(t, d.Connection())
assert.Equal(t, 10*time.Millisecond, d.interval)
d = NewDriver(m, 100*time.Millisecond)
assert.Equal(t, 100*time.Millisecond, d.interval)
}
func TestMavlinkDriverName(t *testing.T) {
d := initTestMavlinkDriver()
assert.True(t, strings.HasPrefix(d.Name(), "Mavlink"))
d.SetName("NewName")
assert.Equal(t, "NewName", d.Name())
}
func TestMavlinkDriverStart(t *testing.T) {
d := initTestMavlinkDriver()
err := make(chan error)
packet := make(chan *common.MAVLinkPacket)
message := make(chan common.MAVLinkMessage)
_ = d.On(PacketEvent, func(data interface{}) {
packet <- data.(*common.MAVLinkPacket)
})
_ = d.On(MessageEvent, func(data interface{}) {
message <- data.(common.MAVLinkMessage)
})
_ = d.On(ErrorIOEvent, func(data interface{}) {
err <- data.(error)
})
_ = d.On(ErrorMAVLinkEvent, func(data interface{}) {
err <- data.(error)
})
require.NoError(t, d.Start())
select {
case p := <-packet:
require.NoError(t, d.SendPacket(p))
case <-time.After(100 * time.Millisecond):
require.Fail(t, "packet was not emitted")
}
select {
case <-message:
case <-time.After(100 * time.Millisecond):
require.Fail(t, "message was not emitted")
}
select {
case <-err:
case <-time.After(100 * time.Millisecond):
require.Fail(t, "error was not emitted")
}
}
func TestMavlinkDriverHalt(t *testing.T) {
d := initTestMavlinkDriver()
require.NoError(t, d.Halt())
}