int
Mavlink::get_uart_fd(unsigned index)
{
Mavlink *inst = get_instance(index);
if (inst) {
return inst->get_uart_fd();
}
return -1;
}
void
Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self)
{
Mavlink *inst;
LL_FOREACH(_mavlink_instances, inst) {
if (inst != self) {
inst->pass_message(msg);
}
}
}
void
Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self)
{
Mavlink *inst;
LL_FOREACH(_mavlink_instances, inst) {
if (inst != self) {
/* if not in normal mode, we are an onboard link
* onboard links should only pass on messages from the same system ID */
if (!(self->_mode != MAVLINK_MODE_NORMAL && msg->sysid != mavlink_system.sysid)) {
inst->pass_message(msg);
}
}
}
}
/// @brief Sends the specified FTP reponse message out through mavlink
void
MavlinkFTP::_reply(Request *req)
{
PayloadHeader *payload = reinterpret_cast<PayloadHeader *>(&req->message.payload[0]);
payload->seqNumber = payload->seqNumber + 1;
mavlink_message_t msg;
msg.checksum = 0;
#ifndef MAVLINK_FTP_UNIT_TEST
uint16_t len =
#endif
mavlink_msg_file_transfer_protocol_pack_chan(req->serverSystemId, // Sender system id
req->serverComponentId, // Sender component id
req->serverChannel, // Channel to send on
&msg, // Message to pack payload into
0, // Target network
req->targetSystemId, // Target system id
0, // Target component id
(const uint8_t*)payload); // Payload to pack into message
bool success = true;
#ifdef MAVLINK_FTP_UNIT_TEST
// Unit test hook is set, call that instead
_utRcvMsgFunc(&msg, _ftp_test);
#else
Mavlink *mavlink = req->mavlink;
mavlink->lockMessageBufferMutex();
success = mavlink->message_buffer_write(&msg, len);
mavlink->unlockMessageBufferMutex();
#endif
if (!success) {
warnx("FTP TX ERR");
}
#ifdef MAVLINK_FTP_DEBUG
else {
warnx("wrote: sys: %d, comp: %d, chan: %d, checksum: %d",
req->serverSystemId,
req->serverComponentId,
req->serverChannel,
msg.checksum);
}
#endif
}
int
Mavlink::get_status_all_instances()
{
Mavlink *inst = ::_mavlink_instances;
unsigned iterations = 0;
while (inst != nullptr) {
printf("\ninstance #%u:\n", iterations);
inst->display_status();
/* move on */
inst = inst->next;
iterations++;
}
/* return an error if there are no instances */
return (iterations == 0);
}
/*
* 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);
}
}
}
