int
Mavlink::get_uart_fd(unsigned index)
{
Mavlink *inst = get_instance(index);
if (inst) {
return inst->get_uart_fd();
}
return -1;
}
/*
* Internal function to send the bytes through the right serial port
*/
void
mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length)
{
Mavlink *instance;
switch (channel) {
case MAVLINK_COMM_0:
instance = Mavlink::get_instance(0);
break;
case MAVLINK_COMM_1:
instance = Mavlink::get_instance(1);
break;
case MAVLINK_COMM_2:
instance = Mavlink::get_instance(2);
break;
case MAVLINK_COMM_3:
instance = Mavlink::get_instance(3);
break;
#ifdef MAVLINK_COMM_4
case MAVLINK_COMM_4:
instance = Mavlink::get_instance(4);
break;
#endif
#ifdef MAVLINK_COMM_5
case MAVLINK_COMM_5:
instance = Mavlink::get_instance(5);
break;
#endif
#ifdef MAVLINK_COMM_6
case MAVLINK_COMM_6:
instance = Mavlink::get_instance(6);
break;
#endif
default:
return;
}
int uart = instance->get_uart_fd();
ssize_t desired = (sizeof(uint8_t) * length);
/*
* Check if the OS buffer is full and disable HW
* flow control if it continues to be full
*/
int buf_free = 0;
if (instance->get_flow_control_enabled()
&& ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) {
/* Disable hardware flow control:
* if no successful write since a defined time
* and if the last try was not the last successful write
*/
if (last_write_try_times[(unsigned)channel] != 0 &&
hrt_elapsed_time(&last_write_success_times[(unsigned)channel]) > 500 * 1000UL &&
last_write_success_times[(unsigned)channel] !=
last_write_try_times[(unsigned)channel]) {
warnx("DISABLING HARDWARE FLOW CONTROL");
instance->enable_flow_control(false);
}
}
/* If the wait until transmit flag is on, only transmit after we've received messages.
Otherwise, transmit all the time. */
if (instance->should_transmit()) {
last_write_try_times[(unsigned)channel] = hrt_absolute_time();
/* check if there is space in the buffer, let it overflow else */
if (!ioctl(uart, FIONWRITE, (unsigned long)&buf_free)) {
if (buf_free < desired) {
/* we don't want to send anything just in half, so return */
instance->count_txerr();
instance->count_txerrbytes(desired);
return;
}
}
ssize_t ret = write(uart, ch, desired);
if (ret != desired) {
instance->count_txerr();
instance->count_txerrbytes(desired);
} else {
last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel];
instance->count_txbytes(desired);
}
}
}