gobot.io/x/gobot@v1.16.0/platforms/mavlink/mavlink_udp_adaptor_test.go (about)

     1  package mavlink
     2  
     3  import (
     4  	"bytes"
     5  	"errors"
     6  	"net"
     7  	"strings"
     8  	"testing"
     9  
    10  	"gobot.io/x/gobot"
    11  	"gobot.io/x/gobot/gobottest"
    12  	mavlink "gobot.io/x/gobot/platforms/mavlink/common"
    13  )
    14  
    15  var _ gobot.Adaptor = (*UDPAdaptor)(nil)
    16  
    17  type MockUDPConnection struct {
    18  	TestClose       func() error
    19  	TestReadFromUDP func([]byte) (int, *net.UDPAddr, error)
    20  	TestWriteTo     func([]byte, net.Addr) (int, error)
    21  }
    22  
    23  func (m *MockUDPConnection) Close() error {
    24  	return m.TestClose()
    25  }
    26  
    27  func (m *MockUDPConnection) ReadFromUDP(b []byte) (int, *net.UDPAddr, error) {
    28  	return m.TestReadFromUDP(b)
    29  }
    30  
    31  func (m *MockUDPConnection) WriteTo(b []byte, a net.Addr) (int, error) {
    32  	return m.TestWriteTo(b, a)
    33  }
    34  
    35  func NewMockUDPConnection() *MockUDPConnection {
    36  	return &MockUDPConnection{
    37  		TestClose: func() error {
    38  			return nil
    39  		},
    40  		TestReadFromUDP: func([]byte) (int, *net.UDPAddr, error) {
    41  			return 0, nil, nil
    42  		},
    43  		TestWriteTo: func([]byte, net.Addr) (int, error) {
    44  			return 0, nil
    45  		},
    46  	}
    47  }
    48  
    49  func initTestMavlinkUDPAdaptor() *UDPAdaptor {
    50  	m := NewUDPAdaptor(":14550")
    51  	return m
    52  }
    53  
    54  func TestMavlinkUDPAdaptor(t *testing.T) {
    55  	a := initTestMavlinkUDPAdaptor()
    56  	gobottest.Assert(t, a.Port(), ":14550")
    57  }
    58  
    59  func TestMavlinkUDPAdaptorName(t *testing.T) {
    60  	a := initTestMavlinkUDPAdaptor()
    61  	gobottest.Assert(t, strings.HasPrefix(a.Name(), "Mavlink"), true)
    62  	a.SetName("NewName")
    63  	gobottest.Assert(t, a.Name(), "NewName")
    64  }
    65  
    66  func TestMavlinkUDPAdaptorConnectAndFinalize(t *testing.T) {
    67  	a := initTestMavlinkUDPAdaptor()
    68  	gobottest.Assert(t, a.Connect(), nil)
    69  	gobottest.Assert(t, a.Finalize(), nil)
    70  }
    71  
    72  func TestMavlinkUDPAdaptorWrite(t *testing.T) {
    73  	a := initTestMavlinkUDPAdaptor()
    74  	a.Connect()
    75  	defer a.Finalize()
    76  
    77  	m := NewMockUDPConnection()
    78  	m.TestWriteTo = func([]byte, net.Addr) (int, error) {
    79  		return 3, nil
    80  	}
    81  	a.sock = m
    82  
    83  	i, err := a.Write([]byte{0x01, 0x02, 0x03})
    84  	gobottest.Assert(t, i, 3)
    85  	gobottest.Assert(t, err, nil)
    86  }
    87  
    88  func TestMavlinkReadMAVLinkReadDefaultPacket(t *testing.T) {
    89  	a := initTestMavlinkUDPAdaptor()
    90  	a.Connect()
    91  	defer a.Finalize()
    92  
    93  	m := NewMockUDPConnection()
    94  
    95  	m.TestReadFromUDP = func(b []byte) (int, *net.UDPAddr, error) {
    96  		buf := new(bytes.Buffer)
    97  		buf.Write([]byte{mavlink.MAVLINK_10_STX, 0x02, 0x03})
    98  		copy(b, buf.Bytes())
    99  		return buf.Len(), nil, nil
   100  	}
   101  	a.sock = m
   102  
   103  	p, _ := a.ReadMAVLinkPacket()
   104  	gobottest.Assert(t, p.Protocol, uint8(254))
   105  }
   106  
   107  func TestMavlinkReadMAVLinkPacketReadError(t *testing.T) {
   108  	a := initTestMavlinkUDPAdaptor()
   109  	a.Connect()
   110  	defer a.Finalize()
   111  
   112  	m := NewMockUDPConnection()
   113  
   114  	i := 0
   115  	m.TestReadFromUDP = func(b []byte) (int, *net.UDPAddr, error) {
   116  		switch i {
   117  		case 0:
   118  			i = 1
   119  			return 1, nil, nil
   120  		case 1:
   121  			i = 2
   122  			buf := new(bytes.Buffer)
   123  			buf.Write([]byte{0x01, 0x02, 0x03})
   124  			copy(b, buf.Bytes())
   125  			return buf.Len(), nil, nil
   126  		case 2:
   127  			i = 3
   128  			buf := new(bytes.Buffer)
   129  			buf.Write([]byte{mavlink.MAVLINK_10_STX, 255})
   130  			copy(b, buf.Bytes())
   131  			return buf.Len(), nil, nil
   132  		}
   133  
   134  		return 0, nil, errors.New("read error")
   135  	}
   136  	a.sock = m
   137  
   138  	_, err := a.ReadMAVLinkPacket()
   139  	gobottest.Assert(t, err, errors.New("read error"))
   140  }