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

     1  package mavlink
     2  
     3  import (
     4  	"net"
     5  
     6  	common "gobot.io/x/gobot/platforms/mavlink/common"
     7  )
     8  
     9  type UDPConnection interface {
    10  	Close() error
    11  	ReadFromUDP([]byte) (int, *net.UDPAddr, error)
    12  	WriteTo([]byte, net.Addr) (int, error)
    13  }
    14  
    15  type UDPAdaptor struct {
    16  	name string
    17  	port string
    18  	sock UDPConnection
    19  }
    20  
    21  var _ BaseAdaptor = (*UDPAdaptor)(nil)
    22  
    23  // NewAdaptor creates a new Mavlink-over-UDP adaptor with specified
    24  // port.
    25  func NewUDPAdaptor(port string) *UDPAdaptor {
    26  	return &UDPAdaptor{
    27  		name: "Mavlink",
    28  		port: port,
    29  	}
    30  }
    31  
    32  func (m *UDPAdaptor) Name() string     { return m.name }
    33  func (m *UDPAdaptor) SetName(n string) { m.name = n }
    34  func (m *UDPAdaptor) Port() string     { return m.port }
    35  
    36  // Connect returns true if connection to device is successful
    37  func (m *UDPAdaptor) Connect() error {
    38  	m.close()
    39  
    40  	addr, err := net.ResolveUDPAddr("udp", m.Port())
    41  	if err != nil {
    42  		return err
    43  	}
    44  
    45  	m.sock, err = net.ListenUDP("udp", addr)
    46  	if err != nil {
    47  		return err
    48  	}
    49  	return nil
    50  }
    51  
    52  func (m *UDPAdaptor) close() error {
    53  	sock := m.sock
    54  	m.sock = nil
    55  
    56  	if sock != nil {
    57  		return sock.Close()
    58  	} else {
    59  		return nil
    60  	}
    61  }
    62  
    63  // Finalize returns true if connection to devices is closed successfully
    64  func (m *UDPAdaptor) Finalize() (err error) {
    65  	return m.close()
    66  }
    67  
    68  func (m *UDPAdaptor) ReadMAVLinkPacket() (*common.MAVLinkPacket, error) {
    69  	buf := make([]byte, 4096)
    70  
    71  	for {
    72  		got, _, err := m.sock.ReadFromUDP(buf)
    73  		if err != nil {
    74  			return nil, err
    75  		}
    76  		if got < 2 {
    77  			continue
    78  		}
    79  		sof := buf[0]
    80  		length := buf[1]
    81  
    82  		if sof != common.MAVLINK_10_STX {
    83  			continue
    84  		}
    85  		if length > 250 {
    86  			continue
    87  		}
    88  		m := &common.MAVLinkPacket{}
    89  		m.Decode(buf)
    90  		return m, nil
    91  	}
    92  }
    93  
    94  func (m *UDPAdaptor) Write(b []byte) (int, error) {
    95  	addr, err := net.ResolveUDPAddr("udp", m.Port())
    96  	if err != nil {
    97  		return 0, err
    98  	}
    99  
   100  	return m.sock.WriteTo(b, addr)
   101  }