github.com/bluenviron/gomavlib/v2@v2.2.1-0.20240308101627-2c07e3da629c/endpoint_serial_test.go (about) 1 package gomavlib 2 3 import ( 4 "io" 5 "testing" 6 7 "github.com/stretchr/testify/require" 8 9 "github.com/bluenviron/gomavlib/v2/pkg/dialect" 10 "github.com/bluenviron/gomavlib/v2/pkg/frame" 11 ) 12 13 var _ endpointChannelProvider = (*endpointSerial)(nil) 14 15 func TestEndpointSerial(t *testing.T) { 16 done := make(chan struct{}) 17 n := 0 18 19 serialOpenFunc = func(name string, baud int) (io.ReadWriteCloser, error) { 20 remote, local := newDummyReadWriterPair() 21 22 n++ 23 switch n { 24 case 1: 25 return remote, nil 26 27 case 2: 28 29 default: 30 t.Errorf("should not happen") 31 } 32 33 go func() { 34 dialectRW, err := dialect.NewReadWriter(testDialect) 35 require.NoError(t, err) 36 37 rw, err := frame.NewReadWriter(frame.ReadWriterConf{ 38 ReadWriter: local, 39 DialectRW: dialectRW, 40 OutVersion: frame.V2, 41 OutSystemID: 11, 42 }) 43 require.NoError(t, err) 44 45 for i := 0; i < 3; i++ { 46 err = rw.WriteMessage(&MessageHeartbeat{ 47 Type: 1, 48 Autopilot: 2, 49 BaseMode: 3, 50 CustomMode: 6, 51 SystemStatus: 4, 52 MavlinkVersion: 5, 53 }) 54 require.NoError(t, err) 55 56 fr, err := rw.Read() 57 require.NoError(t, err) 58 require.Equal(t, &frame.V2Frame{ 59 SequenceID: byte(i), 60 SystemID: 10, 61 ComponentID: 1, 62 Message: &MessageHeartbeat{ 63 Type: 6, 64 Autopilot: 5, 65 BaseMode: 4, 66 CustomMode: 3, 67 SystemStatus: 2, 68 MavlinkVersion: 1, 69 }, 70 Checksum: fr.GetChecksum(), 71 }, fr) 72 } 73 74 close(done) 75 }() 76 77 return remote, nil 78 } 79 80 node, err := NewNode(NodeConf{ 81 Dialect: testDialect, 82 OutVersion: V2, 83 OutSystemID: 10, 84 Endpoints: []EndpointConf{EndpointSerial{ 85 Device: "/dev/ttyUSB0", 86 Baud: 57600, 87 }}, 88 HeartbeatDisable: true, 89 }) 90 require.NoError(t, err) 91 defer node.Close() 92 93 evt := <-node.Events() 94 require.Equal(t, &EventChannelOpen{ 95 Channel: evt.(*EventChannelOpen).Channel, 96 }, evt) 97 98 for i := 0; i < 3; i++ { 99 evt := <-node.Events() 100 require.Equal(t, &EventFrame{ 101 Frame: &frame.V2Frame{ 102 SequenceID: byte(i), 103 SystemID: 11, 104 ComponentID: 1, 105 Message: &MessageHeartbeat{ 106 Type: 1, 107 Autopilot: 2, 108 BaseMode: 3, 109 CustomMode: 6, 110 SystemStatus: 4, 111 MavlinkVersion: 5, 112 }, 113 Checksum: evt.(*EventFrame).Frame.GetChecksum(), 114 }, 115 Channel: evt.(*EventFrame).Channel, 116 }, evt) 117 118 err := node.WriteMessageAll(&MessageHeartbeat{ 119 Type: 6, 120 Autopilot: 5, 121 BaseMode: 4, 122 CustomMode: 3, 123 SystemStatus: 2, 124 MavlinkVersion: 1, 125 }) 126 require.NoError(t, err) 127 } 128 129 <-done 130 } 131 132 func TestEndpointSerialReconnect(t *testing.T) { 133 done := make(chan struct{}) 134 count := 0 135 136 serialOpenFunc = func(name string, baud int) (io.ReadWriteCloser, error) { 137 remote, local := newDummyReadWriterPair() 138 139 switch count { 140 case 0: // skip first call to serialOpenFunc() 141 142 case 1: 143 go func() { 144 dialectRW, err := dialect.NewReadWriter(testDialect) 145 require.NoError(t, err) 146 147 rw, err := frame.NewReadWriter(frame.ReadWriterConf{ 148 ReadWriter: local, 149 DialectRW: dialectRW, 150 OutVersion: frame.V2, 151 OutSystemID: 11, 152 }) 153 require.NoError(t, err) 154 155 err = rw.WriteMessage(&MessageHeartbeat{ 156 Type: 1, 157 Autopilot: 2, 158 BaseMode: 3, 159 CustomMode: 6, 160 SystemStatus: 4, 161 MavlinkVersion: 5, 162 }) 163 require.NoError(t, err) 164 165 fr, err := rw.Read() 166 require.NoError(t, err) 167 require.Equal(t, &frame.V2Frame{ 168 SequenceID: 0, 169 SystemID: 10, 170 ComponentID: 1, 171 Message: &MessageHeartbeat{ 172 Type: 6, 173 Autopilot: 5, 174 BaseMode: 4, 175 CustomMode: 3, 176 SystemStatus: 2, 177 MavlinkVersion: 1, 178 }, 179 Checksum: fr.GetChecksum(), 180 }, fr) 181 182 remote.simulateReadError() 183 }() 184 185 case 2: 186 go func() { 187 dialectRW, err := dialect.NewReadWriter(testDialect) 188 require.NoError(t, err) 189 190 rw, err := frame.NewReadWriter(frame.ReadWriterConf{ 191 ReadWriter: local, 192 DialectRW: dialectRW, 193 OutVersion: frame.V2, 194 OutSystemID: 11, 195 }) 196 require.NoError(t, err) 197 198 fr, err := rw.Read() 199 require.NoError(t, err) 200 require.Equal(t, &frame.V2Frame{ 201 SequenceID: 0, 202 SystemID: 10, 203 ComponentID: 1, 204 Message: &MessageHeartbeat{ 205 Type: 7, 206 Autopilot: 5, 207 BaseMode: 4, 208 CustomMode: 3, 209 SystemStatus: 2, 210 MavlinkVersion: 1, 211 }, 212 Checksum: fr.GetChecksum(), 213 }, fr) 214 215 close(done) 216 }() 217 } 218 219 count++ 220 return remote, nil 221 } 222 223 node, err := NewNode(NodeConf{ 224 Dialect: testDialect, 225 OutVersion: V2, 226 OutSystemID: 10, 227 Endpoints: []EndpointConf{EndpointSerial{ 228 Device: "/dev/ttyUSB0", 229 Baud: 57600, 230 }}, 231 HeartbeatDisable: true, 232 }) 233 require.NoError(t, err) 234 defer node.Close() 235 236 evt := <-node.Events() 237 require.Equal(t, &EventChannelOpen{ 238 Channel: evt.(*EventChannelOpen).Channel, 239 }, evt) 240 241 evt = <-node.Events() 242 require.Equal(t, &EventFrame{ 243 Frame: &frame.V2Frame{ 244 SequenceID: 0, 245 SystemID: 11, 246 ComponentID: 1, 247 Message: &MessageHeartbeat{ 248 Type: 1, 249 Autopilot: 2, 250 BaseMode: 3, 251 CustomMode: 6, 252 SystemStatus: 4, 253 MavlinkVersion: 5, 254 }, 255 Checksum: evt.(*EventFrame).Frame.GetChecksum(), 256 }, 257 Channel: evt.(*EventFrame).Channel, 258 }, evt) 259 260 err = node.WriteMessageAll(&MessageHeartbeat{ 261 Type: 6, 262 Autopilot: 5, 263 BaseMode: 4, 264 CustomMode: 3, 265 SystemStatus: 2, 266 MavlinkVersion: 1, 267 }) 268 require.NoError(t, err) 269 270 evt = <-node.Events() 271 require.Equal(t, &EventChannelClose{ 272 Channel: evt.(*EventChannelClose).Channel, 273 }, evt) 274 275 evt = <-node.Events() 276 require.Equal(t, &EventChannelOpen{ 277 Channel: evt.(*EventChannelOpen).Channel, 278 }, evt) 279 280 err = node.WriteMessageAll(&MessageHeartbeat{ 281 Type: 7, 282 Autopilot: 5, 283 BaseMode: 4, 284 CustomMode: 3, 285 SystemStatus: 2, 286 MavlinkVersion: 1, 287 }) 288 require.NoError(t, err) 289 290 <-done 291 }