static void
comms_init(void)
{
/* initialise the FMU interface */
fmu_fd = open("/dev/ttyS1", O_RDWR);
stream = hx_stream_init(fmu_fd, comms_handle_frame, NULL);
comms_rx_errors = perf_alloc(PC_COUNT, "rx_err");
hx_stream_set_counters(stream, 0, 0, comms_rx_errors);
/* default state in the report to FMU */
report.i2f_magic = I2F_MAGIC;
struct termios t;
/* 115200bps, no parity, one stop bit */
tcgetattr(fmu_fd, &t);
cfsetspeed(&t, 115200);
t.c_cflag &= ~(CSTOPB | PARENB);
tcsetattr(fmu_fd, TCSANOW, &t);
/* init the ADC */
adc_init();
}
void
PX4IO::task_main()
{
ASSERT(_fd == -1);
log("ready");
/* open the serial port */
_fd = ::open("/dev/ttyS2", O_RDWR | O_NONBLOCK);
if (_fd < 0) {
debug("failed to open serial port for IO: %d", errno);
_task = -1;
_exit(errno);
}
/* protocol stream */
_io_stream = hx_stream_init(_fd, &PX4IO::rx_callback_trampoline, this);
perf_counter_t pc_tx_bytes = perf_alloc(PC_COUNT, "PX4IO frames transmitted");
perf_counter_t pc_rx_bytes = perf_alloc(PC_COUNT, "PX4IO frames received");
perf_counter_t pc_rx_errors = perf_alloc(PC_COUNT, "PX4IO receive errors");
hx_stream_set_counters(_io_stream, pc_tx_bytes, pc_rx_bytes, pc_rx_errors);
/* poll descriptor(s) */
struct pollfd fds[1];
fds[0].fd = _fd;
fds[0].events = POLLIN;
/* loop handling received serial bytes */
while (!_task_should_exit) {
/* sleep waiting for data, but no more than 100ms */
int ret = ::poll(&fds[0], 1, 100);
/* this would be bad... */
if (ret < 0) {
log("poll error %d", errno);
usleep(1000000);
continue;
}
/* if we timed out waiting, we should send an update */
if (ret == 0)
_send_needed = true;
/* if we have new data from IO, go handle it */
if ((ret > 0) && (fds[0].revents & POLLIN))
io_recv();
/* send an update to IO if required */
if (_send_needed) {
_send_needed = false;
io_send();
}
}
if (_io_stream != nullptr)
hx_stream_free(_io_stream);
::close(_fd);
/* tell the dtor that we are exiting */
_task = -1;
_exit(0);
}
