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_Serial.cpp
Views: 1798
1
#include "AP_DDS_Client.h"

2


3
#include <AP_SerialManager/AP_SerialManager.h>

4


5
#include <errno.h>

6


7
/*

8
  open connection on a serial port

9
 */

10
bool AP_DDS_Client::serial_transport_open(uxrCustomTransport *t)

11
{

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

13
    AP_SerialManager *serial_manager = AP_SerialManager::get_singleton();

14
    auto *dds_port = serial_manager->find_serial(AP_SerialManager::SerialProtocol_DDS_XRCE, 0);

15
    if (dds_port == nullptr) {

16
        return false;

17
    }

18
    // ensure we own the UART

19
    dds_port->begin(0);

20
    dds->serial.port = dds_port;

21
    return true;

22
}

23


24
/*

25
  close serial transport

26
 */

27
bool AP_DDS_Client::serial_transport_close(uxrCustomTransport *t)

28
{

29
    // we don't actually close the UART

30
    return true;

31
}

32


33
/*

34
  write on serial transport

35
 */

36
size_t AP_DDS_Client::serial_transport_write(uxrCustomTransport *t, const uint8_t* buf, size_t len, uint8_t* error)

37
{

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

39
    if (dds->serial.port == nullptr) {

40
        *error = EINVAL;

41
        return 0;

42
    }

43
    ssize_t bytes_written = dds->serial.port->write(buf, len);

44
    if (bytes_written <= 0) {

45
        *error = 1;

46
        return 0;

47
    }

48
    //! @todo populate the error code correctly

49
    *error = 0;

50
    return bytes_written;

51
}

52


53
/*

54
  read from a serial transport

55
 */

56
size_t AP_DDS_Client::serial_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->serial.port == nullptr) {

60
        *error = EINVAL;

61
        return 0;

62
    }

63
    const uint32_t tstart = AP_HAL::millis();

64
    while (AP_HAL::millis() - tstart < uint32_t(timeout_ms) &&

65
           dds->serial.port->available() < len) {

66
        hal.scheduler->delay_microseconds(100); // TODO select or poll this is limiting speed (100us)

67
    }

68
    ssize_t bytes_read = dds->serial.port->read(buf, len);

69
    if (bytes_read <= 0) {

70
        *error = 1;

71
        return 0;

72
    }

73
    //! @todo Add error reporting

74
    *error = 0;

75
    return bytes_read;

76
}

77


78
/*

79
  initialise serial connection

80
 */

81
bool AP_DDS_Client::ddsSerialInit()

82
{

83
    // setup a framed transport for serial

84
    uxr_set_custom_transport_callbacks(&serial.transport, true,

85
                                       serial_transport_open,

86
                                       serial_transport_close,

87
                                       serial_transport_write,

88
                                       serial_transport_read);

89


90
    if (!uxr_init_custom_transport(&serial.transport, (void*)this)) {

91
        return false;

92
    }

93
    comm = &serial.transport.comm;

94
    return true;

95
}

96


97