void GazeboMavlinkInterface::send_mavlink_message(const uint8_t msgid, const void *msg, uint8_t component_ID) {
component_ID = 0;
uint8_t payload_len = mavlink_message_lengths[msgid];
unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
/* header */
buf[0] = MAVLINK_STX;
buf[1] = payload_len;
/* no idea which numbers should be here*/
buf[2] = 100;
buf[3] = 0;
buf[4] = 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);
ssize_t len = sendto(_fd, buf, packet_len, 0, (struct sockaddr *)&_srcaddr, sizeof(_srcaddr));
if (len <= 0) {
printf("Failed sending mavlink message");
}
}
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);
}
// send actuator controls message to Pixhawk
void send_outputs_mavlink(const uint16_t *pwm, const unsigned num_pwm)
{
// Fill up to number of outputs.
mavlink_actuator_control_target_t controls_message;
for (unsigned i = 0; i < num_pwm && i < actuator_controls_s::NUM_ACTUATOR_CONTROLS; ++i) {
controls_message.controls[i] = pwm[i];
}
// And the rest with NAN.
for (unsigned i = _outputs.noutputs; (i < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS)
&& (i < actuator_controls_s::NUM_ACTUATOR_CONTROLS); ++i) {
controls_message.controls[i] = NAN;
}
controls_message.time_usec = _controls.timestamp;
const uint8_t msgid = MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET;
const uint8_t component_ID = 0;
const uint8_t payload_len = mavlink_message_lengths[msgid];
const unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
/* header */
buf[0] = MAVLINK_STX;
buf[1] = payload_len;
// TODO FIXME: no idea which numbers should be here.
buf[2] = 100;
buf[3] = 0;
buf[4] = component_ID;
buf[5] = msgid;
/* payload */
memcpy(&buf[MAVLINK_NUM_HEADER_BYTES], (const void *)&controls_message, 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);
int ret = ::write(_fd, &buf[0], packet_len);
//static unsigned counter = 0;
//if (counter++ % 250 == 0) {
// PX4_INFO("send motor controls %d bytes %.2f %.2f %.2f %.2f",
// ret,
// controls_message.controls[0],
// controls_message.controls[1],
// controls_message.controls[2],
// controls_message.controls[3]);
//}
if (ret < 1) {
PX4_WARN("Failed sending rc mavlink message, ret: %d, errno: %d", ret, errno);
}
}
void send_rc_mavlink()
{
mavlink_rc_channels_t rc_message;
rc_message.time_boot_ms = _rc.timestamp_publication / 1000;
rc_message.chancount = _rc.channel_count;
rc_message.chan1_raw = (_rc.channel_count > 0) ? _rc.values[0] : UINT16_MAX;
rc_message.chan2_raw = (_rc.channel_count > 1) ? _rc.values[1] : UINT16_MAX;
rc_message.chan3_raw = (_rc.channel_count > 2) ? _rc.values[2] : UINT16_MAX;
rc_message.chan4_raw = (_rc.channel_count > 3) ? _rc.values[3] : UINT16_MAX;
rc_message.chan5_raw = (_rc.channel_count > 4) ? _rc.values[4] : UINT16_MAX;
rc_message.chan6_raw = (_rc.channel_count > 5) ? _rc.values[5] : UINT16_MAX;
rc_message.chan7_raw = (_rc.channel_count > 6) ? _rc.values[6] : UINT16_MAX;
rc_message.chan8_raw = (_rc.channel_count > 7) ? _rc.values[7] : UINT16_MAX;
rc_message.chan9_raw = (_rc.channel_count > 8) ? _rc.values[8] : UINT16_MAX;
rc_message.chan10_raw = (_rc.channel_count > 9) ? _rc.values[9] : UINT16_MAX;
rc_message.chan11_raw = (_rc.channel_count > 10) ? _rc.values[10] : UINT16_MAX;
rc_message.chan12_raw = (_rc.channel_count > 11) ? _rc.values[11] : UINT16_MAX;
rc_message.chan13_raw = (_rc.channel_count > 12) ? _rc.values[12] : UINT16_MAX;
rc_message.chan14_raw = (_rc.channel_count > 13) ? _rc.values[13] : UINT16_MAX;
rc_message.chan15_raw = (_rc.channel_count > 14) ? _rc.values[14] : UINT16_MAX;
rc_message.chan16_raw = (_rc.channel_count > 15) ? _rc.values[15] : UINT16_MAX;
rc_message.chan17_raw = (_rc.channel_count > 16) ? _rc.values[16] : UINT16_MAX;
rc_message.chan18_raw = (_rc.channel_count > 17) ? _rc.values[17] : UINT16_MAX;
rc_message.rssi = _rc.rssi;
const uint8_t msgid = MAVLINK_MSG_ID_RC_CHANNELS;
uint8_t component_ID = 0;
uint8_t payload_len = mavlink_message_lengths[msgid];
unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
/* header */
buf[0] = MAVLINK_STX;
buf[1] = payload_len;
/* no idea which numbers should be here*/
buf[2] = 100;
buf[3] = 0;
buf[4] = component_ID;
buf[5] = msgid;
/* payload */
memcpy(&buf[MAVLINK_NUM_HEADER_BYTES], (const void *)&rc_message, 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);
int len = ::write(_uart_fd, &buf[0], packet_len);
//static unsigned counter = 0;
//if (counter++ % 100 == 0) {
// PX4_INFO("sent %d %d %d %d %d %d", len,
// rc_message.chan1_raw,
// rc_message.chan2_raw,
// rc_message.chan3_raw,
// rc_message.chan4_raw,
// rc_message.chan5_raw);
//}
if (len < 1) {
PX4_WARN("failed sending rc mavlink message");
}
}
