CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutSign UpSign In
Ardupilot

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

GitHub Repository: Ardupilot/ardupilot
 Path: blob/master/libraries/AP_DDS/AP_DDS_UDP.cpp
Views: 1798
1
#include "AP_DDS_Client.h"

2


3
#if AP_DDS_UDP_ENABLED

4


5
#include <errno.h>

6


7
/*

8
  open connection on UDP

9
 */

10
bool AP_DDS_Client::udp_transport_open(uxrCustomTransport *t)

11
{

12
    AP_DDS_Client *dds = (AP_DDS_Client *)t->args;

13
    auto *sock = NEW_NOTHROW SocketAPM(true);

14
    if (sock == nullptr) {

15
        return false;

16
    }

17
    if (!sock->connect(dds->udp.ip.get_str(), dds->udp.port.get())) {

18
        return false;

19
    }

20
    dds->udp.socket = sock;

21
    return true;

22
}

23


24
/*

25
  close UDP connection

26
 */

27
bool AP_DDS_Client::udp_transport_close(uxrCustomTransport *t)

28
{

29
    AP_DDS_Client *dds = (AP_DDS_Client *)t->args;

30
    delete dds->udp.socket;

31
    dds->udp.socket = nullptr;

32
    return true;

33
}

34


35
/*

36
  write on UDP

37
 */

38
size_t AP_DDS_Client::udp_transport_write(uxrCustomTransport *t, const uint8_t* buf, size_t len, uint8_t* error)

39
{

40
    AP_DDS_Client *dds = (AP_DDS_Client *)t->args;

41
    if (dds->udp.socket == nullptr) {

42
        *error = EINVAL;

43
        return 0;

44
    }

45
    const ssize_t ret = dds->udp.socket->send(buf, len);

46
    if (ret <= 0) {

47
        *error = errno;

48
        return 0;

49
    }

50
    return ret;

51
}

52


53
/*

54
  read from UDP

55
 */

56
size_t AP_DDS_Client::udp_transport_read(uxrCustomTransport *t, uint8_t* buf, size_t len, int timeout_ms, uint8_t* error)

57
{

58
    AP_DDS_Client *dds = (AP_DDS_Client *)t->args;

59
    if (dds->udp.socket == nullptr) {

60
        *error = EINVAL;

61
        return 0;

62
    }

63
    const ssize_t ret = dds->udp.socket->recv(buf, len, timeout_ms);

64
    if (ret <= 0) {

65
        *error = errno;

66
        return 0;

67
    }

68
    return ret;

69
}

70


71
/*

72
  initialise UDP connection

73
 */

74
bool AP_DDS_Client::ddsUdpInit()

75
{

76
    // setup a non-framed transport for UDP

77
    uxr_set_custom_transport_callbacks(&udp.transport, false,

78
                                       udp_transport_open,

79
                                       udp_transport_close,

80
                                       udp_transport_write,

81
                                       udp_transport_read);

82


83
    if (!uxr_init_custom_transport(&udp.transport, (void*)this)) {

84
        return false;

85
    }

86
    comm = &udp.transport.comm;

87
    return true;

88
}

89
#endif // AP_DDS_UDP_ENABLED

90


91