示例#1
0
void
Mavlink::resend_message(mavlink_message_t *msg)
{
	/* If the wait until transmit flag is on, only transmit after we've received messages.
	   Otherwise, transmit all the time. */
	if (!should_transmit()) {
		return;
	}

	pthread_mutex_lock(&_send_mutex);

	unsigned buf_free = get_free_tx_buf();

	_last_write_try_time = hrt_absolute_time();

	unsigned packet_len = msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES;

	/* check if there is space in the buffer, let it overflow else */
	if (buf_free < packet_len) {
		/* no enough space in buffer to send */
		count_txerr();
		count_txerrbytes(packet_len);
		pthread_mutex_unlock(&_send_mutex);
		return;
	}

	uint8_t buf[MAVLINK_MAX_PACKET_LEN];

	/* header and payload */
	memcpy(&buf[0], &msg->magic, MAVLINK_NUM_HEADER_BYTES + msg->len);

	/* checksum */
	buf[MAVLINK_NUM_HEADER_BYTES + msg->len] = (uint8_t)(msg->checksum & 0xFF);
	buf[MAVLINK_NUM_HEADER_BYTES + msg->len + 1] = (uint8_t)(msg->checksum >> 8);

#ifndef __PX4_POSIX
	/* send message to UART */
	ssize_t ret = ::write(_uart_fd, buf, packet_len);

	if (ret != (int) packet_len) {
		count_txerr();
		count_txerrbytes(packet_len);

	} else {
		_last_write_success_time = _last_write_try_time;
		count_txbytes(packet_len);
	}

#endif

	pthread_mutex_unlock(&_send_mutex);
}
示例#2
0
void
Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID)
{
	/* If the wait until transmit flag is on, only transmit after we've received messages.
	   Otherwise, transmit all the time. */
	if (!should_transmit()) {
		return;
	}

	pthread_mutex_lock(&_send_mutex);

	unsigned buf_free = get_free_tx_buf();

	uint8_t payload_len = mavlink_message_lengths[msgid];
	unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES;

	_last_write_try_time = hrt_absolute_time();

	/* check if there is space in the buffer, let it overflow else */
	if (buf_free < packet_len) {
		/* no enough space in buffer to send */
		count_txerr();
		count_txerrbytes(packet_len);
		pthread_mutex_unlock(&_send_mutex);
		return;
	}

	uint8_t buf[MAVLINK_MAX_PACKET_LEN];

	/* header */
	buf[0] = MAVLINK_STX;
	buf[1] = payload_len;
	/* use mavlink's internal counter for the TX seq */
	buf[2] = mavlink_get_channel_status(_channel)->current_tx_seq++;
	buf[3] = mavlink_system.sysid;
	buf[4] = (component_ID == 0) ? mavlink_system.compid : component_ID;
	buf[5] = msgid;

	/* payload */
	memcpy(&buf[MAVLINK_NUM_HEADER_BYTES], msg, payload_len);

	/* checksum */
	uint16_t checksum;
	crc_init(&checksum);
	crc_accumulate_buffer(&checksum, (const char *) &buf[1], MAVLINK_CORE_HEADER_LEN + payload_len);
	crc_accumulate(mavlink_message_crcs[msgid], &checksum);

	buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF);
	buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8);

	/* send message to UART */
	ssize_t ret = write(_uart_fd, buf, packet_len);

	if (ret != (int) packet_len) {
		count_txerr();
		count_txerrbytes(packet_len);

	} else {
		_last_write_success_time = _last_write_try_time;
		count_txbytes(packet_len);
	}

	pthread_mutex_unlock(&_send_mutex);
}