telem_serial.cpp

#include "telem_serial.h"

#ifdef _WIN32
#else

#include "la-log.h"

#include <errno.h>
#include <fcntl.h>
#include <stdlib.h>
#include <string.h>
#include <termios.h>
#include <unistd.h>

#define UNUSED __attribute__ ((unused))

//MACOSX doesn't define anything higher than 38400
#ifndef B115200
#define B115200 115200
#endif

#ifndef B500000
#define B500000 500000
#endif

#ifndef B921600
#define B921600 921600
#endif

#ifndef B1500000
#define B1500000 1500000
#endif

void Telem_Serial::pack_select_fds(fd_set &fds_read,
                                   fd_set &fds_write UNUSED,
                                   fd_set &fds_err,
                                   uint8_t &nfds)
{
    FD_SET(fd, &fds_read);
    FD_SET(fd, &fds_err);

    if (fd >= nfds) {
        nfds = fd + 1;
    }
}


void Telem_Serial::handle_select_fds(fd_set &fds_read,
                                     fd_set &fds_write UNUSED,
                                     fd_set &fds_err,
                                     uint8_t &nfds UNUSED)
{
    /* check for packets from telem_forwarder */
    if (FD_ISSET(fd, &fds_err)) {
        FD_CLR(fd, &fds_err);
        la_log(LOG_ERR, "select(%d): %s", fd, strerror(errno));
    }

    if (FD_ISSET(fd, &fds_read)) {
        FD_CLR(fd, &fds_read);
        _recv_buflen_content = handle_read();
        // ::fprintf(stderr, "received %u bytes\n", _recv_buflen_content);
    }
}


// from serial_setup in telem_forward
void Telem_Serial::open_serial_port()
{
    fd = open(serialPortName.c_str(), O_RDWR | O_NOCTTY | O_NONBLOCK);

    if(fd < 0) {
        la_log(LOG_ERR, "unable to open serial port %s", serialPortName.c_str());
        abort();
    }


    //Configure port for 8N1 transmission
    struct termios options;

    tcgetattr(fd, &options);      //Gets the current options for the port
    //Set the output baud rate
    switch(serialBaud) {
        case 1200: cfsetspeed(&options, B1200); break;
        case 2400: cfsetspeed(&options, B2400); break;
        case 4800: cfsetspeed(&options, B4800); break;
        case 9600: cfsetspeed(&options, B9600); break;
        case 19200: cfsetspeed(&options, B19200); break;
        case 38400: cfsetspeed(&options, B38400); break;
        case 57600: cfsetspeed(&options, B57600); break;
        case 115200: cfsetspeed(&options, B115200); break;
        case 500000: cfsetspeed(&options, B500000); break;
        case 921600: cfsetspeed(&options, B921600); break;
        case 1500000: cfsetspeed(&options, B1500000); break;
        default:
            syslog(LOG_ERR, "unsupported baud rate %d", serialBaud);
            return;
    }
    options.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | INLCR | PARMRK | INPCK | ISTRIP | IXON);
    options.c_oflag &= ~(OCRNL | ONLCR | ONLRET | ONOCR | OFILL | OPOST);
    options.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
    options.c_cflag &= ~(CSIZE | PARENB);
    options.c_cflag |= (CS8 | CLOCAL);

    if (serialFlow) {
        options.c_cflag |= CRTSCTS;     // hardware flow control
    } else {
        options.c_cflag &= ~(CRTSCTS);  // no hardware flow control
    }

    // At 115k (87 us per char), reading 1 char at a time results in increased
    // CPU usage, since we actually can keep up with getting a small number of
    // characters per loop. At 921k (11 us per char), we get more characters
    // each time through the loop, so there is less advantage to setting VMIN
    // to more than 1.
    //
    //          CPU Usage at
    // VMIN     115k    921k
    //    1     7.0%    1.8%
    //   10     2.7%    1.6%
    //  100     1.2%    1.2%
    //
    // The problem with asking for more than 1 character per read is that each
    // message will usually not be received until some bytes in the following
    // message are available. That is often not a problem, but there are
    // sometimes gaps of several 10s of milliseconds in the telemetry stream,
    // and it is preferable to process messages as soon as they are available.
    if (serialBaud <= 115200) {
        options.c_cc[VMIN]  = 10;
    } else {
        options.c_cc[VMIN]  = 1;
    }
    options.c_cc[VTIME] = 0;

    tcsetattr(fd, TCSANOW, &options);    //Set the new options for the port "NOW"

    la_log(LOG_INFO, "opened serial port %s", serialPortName.c_str());

    return;
}


uint32_t Telem_Serial::handle_read()
{
    ssize_t res = read(fd, _recv_buf, _recv_buflen );
    if (res == -1) {
        la_log(LOG_INFO, "Failed read: %s", strerror(errno));
    }

    return res;
}

void Telem_Serial::init()
{
    open_serial_port();
}

void Telem_Serial::configure(INIReader *config)
{
    set_serial(config->Get("solo","telemDev","/dev/ttymxc1"),
               config->GetInteger("solo", "telemBaud", 57600),
               config->GetBoolean("solo", "telemFlow", true));
}

void Telem_Serial::set_serial(std::string port, uint32_t baud, bool flow)
{
    serialPortName = port;
    serialBaud = baud;
    serialFlow = flow;
}

void Telem_Serial::do_writer_sends()
{
    while (_send_buf_start != _send_buf_stop) { // FIXME: use file descriptors!
        bool tail_first = false;
        if (_send_buf_stop < _send_buf_start) {
            tail_first = true;
        }
        uint32_t bytes_to_send = tail_first ? (send_buf_size() - _send_buf_start) : (_send_buf_stop - _send_buf_start);

        int32_t sent = ::write(fd, (const char *)&_send_buf[_send_buf_start], bytes_to_send);
        if (sent < 0) {
            // cry
            break;
        } else if (sent == 0) {
            break;
        } else {
            _send_buf_start += sent;
            if (_send_buf_start == send_buf_size()) {
                _send_buf_start = 0;
            }
        }
    }
}



bool Telem_Serial::send_message(const mavlink_message_t &msg)
{
    char sendbuf[1024]; // large enough...

    uint16_t messageLen = mavlink_msg_to_send_buffer((uint8_t*)sendbuf,&msg);
    if (send_buffer_space_free() < messageLen) {
        // dropped_packets++;
        return false;
    }
    if (_send_buf_stop >= _send_buf_start) {
        uint16_t to_copy = send_buf_size() - _send_buf_stop;
        if (to_copy > messageLen) {
            to_copy = messageLen;
        }
        memcpy(&_send_buf[_send_buf_stop], sendbuf, to_copy);
        _send_buf_stop += to_copy;
        if (_send_buf_stop >= send_buf_size()) {
            _send_buf_stop = 0;
        }
        to_copy = messageLen - to_copy;
        if (to_copy) {
            memcpy(&_send_buf[_send_buf_stop], &sendbuf[messageLen-to_copy], to_copy);
            _send_buf_stop += to_copy;
            if (_send_buf_stop >= send_buf_size()) {
                _send_buf_stop = 0;
            }
        }
    } else {
        memcpy(&_send_buf[_send_buf_stop], &sendbuf[0], messageLen);
        _send_buf_stop += messageLen;
        if (_send_buf_stop >= send_buf_size()) {
            _send_buf_stop = 0;
        }
    }
    return true;
}

#endif // _WIN32