microRTPS_transport.cpp

microRTPS_transport.cpp

ssize_t Transport_node::write(const uint8_t topic_ID, char buffer[], size_t length)
{
        if (!fds_OK()) {
                return -1;
        }

        static struct Header header = {
                .marker = {'>', '>', '>'},
                .topic_ID = 0u,
                .seq = 0u,
                .payload_len_h = 0u,
                .payload_len_l = 0u,
                .crc_h = 0u,
                .crc_l = 0u

        };

        static uint8_t seq = 0;

        // [>,>,>,topic_ID,seq,payload_length,CRCHigh,CRCLow,payload_start, ... ,payload_end]

        uint16_t crc = crc16((uint8_t *)buffer, length);

        header.topic_ID = topic_ID;
        header.seq = seq++;
        header.payload_len_h = (length >> 8) & 0xff;
        header.payload_len_l = length & 0xff;
        header.crc_h = (crc >> 8) & 0xff;
        header.crc_l = crc & 0xff;

        ssize_t len = node_write(&header, sizeof(header));

        if (len != sizeof(header)) {
                goto err;
        }

        len = node_write(buffer, length);

        if (len != ssize_t(length)) {
                goto err;
        }

        return len + sizeof(header);

err:
        //int errsv = errno;
        //if (len == -1 ) printf("                               => Writing error '%d'\n", errsv);
        //else            printf("                               => Wrote '%ld' != length(%lu) error '%d'\n", (long)len, (unsigned long)length, errsv);

        return len;
}