dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

MAVLink: Update to version 2 compaat API

This commit is contained in:
Lorenz Meier 2016-05-14 14:17:30 +02:00
parent c6aa1b1efb
commit 0a597d0d62
9 changed files with 144 additions and 133 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/modules/mavlink/mavlink_bridge_header.h
View File

@ -49,6 +49,8 @@ __BEGIN_DECLS
/* use efficient approach, see mavlink_helpers.h */ /* use efficient approach, see mavlink_helpers.h */
#define MAVLINK_SEND_UART_BYTES mavlink_send_uart_bytes #define MAVLINK_SEND_UART_BYTES mavlink_send_uart_bytes
#define MAVLINK_END_UART_SEND mavlink_end_uart_send
#define MAVLINK_GET_CHANNEL_BUFFER mavlink_get_channel_buffer #define MAVLINK_GET_CHANNEL_BUFFER mavlink_get_channel_buffer
#define MAVLINK_GET_CHANNEL_STATUS mavlink_get_channel_status #define MAVLINK_GET_CHANNEL_STATUS mavlink_get_channel_status
@ -76,6 +78,8 @@ extern mavlink_system_t mavlink_system;
*/ */
void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int length); void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int length);
void mavlink_end_uart_send(mavlink_channel_t chan, int length);
extern mavlink_status_t *mavlink_get_channel_status(uint8_t chan); extern mavlink_status_t *mavlink_get_channel_status(uint8_t chan);
extern mavlink_message_t *mavlink_get_channel_buffer(uint8_t chan); extern mavlink_message_t *mavlink_get_channel_buffer(uint8_t chan);

4
src/modules/mavlink/mavlink_ftp.cpp
View File

@ -1,6 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved. * Copyright (c) 2014-2016 PX4 Development Team. All rights reserved.
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@ -292,7 +292,7 @@ MavlinkFTP::_reply(mavlink_file_transfer_protocol_t* ftp_req)
// Unit test hook is set, call that instead // Unit test hook is set, call that instead
_utRcvMsgFunc(ftp_req, _worker_data); _utRcvMsgFunc(ftp_req, _worker_data);
#else #else
_mavlink->send_message(MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL, ftp_req); mavlink_msg_file_transfer_protocol_send_struct(_mavlink->get_channel(), ftp_req);
#endif #endif
} }

6
src/modules/mavlink/mavlink_log_handler.cpp
View File

@ -1,6 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved. * Copyright (c) 2014-2016 PX4 Development Team. All rights reserved.
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@ -257,7 +257,7 @@ MavlinkLogHandler::_log_send_listing()
response.id = _pLogHandlerHelper->next_entry; response.id = _pLogHandlerHelper->next_entry;
response.num_logs = _pLogHandlerHelper->log_count; response.num_logs = _pLogHandlerHelper->log_count;
response.last_log_num = _pLogHandlerHelper->last_entry; response.last_log_num = _pLogHandlerHelper->last_entry;
_mavlink->send_message(MAVLINK_MSG_ID_LOG_ENTRY, &response); mavlink_msg_log_entry_send_struct(_mavlink->get_channel(), &response);
//-- If we're done listing, flag it. //-- If we're done listing, flag it.
if (_pLogHandlerHelper->next_entry == _pLogHandlerHelper->last_entry) { if (_pLogHandlerHelper->next_entry == _pLogHandlerHelper->last_entry) {
_pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_IDLE; _pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_IDLE;
@ -289,7 +289,7 @@ MavlinkLogHandler::_log_send_data()
response.ofs = _pLogHandlerHelper->current_log_data_offset; response.ofs = _pLogHandlerHelper->current_log_data_offset;
response.id = _pLogHandlerHelper->current_log_index; response.id = _pLogHandlerHelper->current_log_index;
response.count = read_size; response.count = read_size;
_mavlink->send_message(MAVLINK_MSG_ID_LOG_DATA, &response); mavlink_msg_log_data_send_struct(_mavlink->get_channel(), &response);
_pLogHandlerHelper->current_log_data_offset += read_size; _pLogHandlerHelper->current_log_data_offset += read_size;
_pLogHandlerHelper->current_log_data_remaining -= read_size; _pLogHandlerHelper->current_log_data_remaining -= read_size;
if (read_size < sizeof(response.data) || _pLogHandlerHelper->current_log_data_remaining == 0) { if (read_size < sizeof(response.data) || _pLogHandlerHelper->current_log_data_remaining == 0) {

144
src/modules/mavlink/mavlink_main.cpp
View File

@ -120,12 +120,17 @@ extern mavlink_system_t mavlink_system;
void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int length) void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int length)
{ {
Mavlink* m = Mavlink::get_instance((unsigned)chan); Mavlink* m = Mavlink::get_instance((unsigned)chan);
if (m != nullptr) {
if (m->get_free_tx_buf() < length) { m->send_bytes(ch, length);
return;
} }
}
m->send_bytes(ch, length); void mavlink_end_uart_send(mavlink_channel_t chan, int length)
{
Mavlink* m = Mavlink::get_instance((unsigned)chan);
if (m != nullptr) {
m->send_packet();
}
} }
static void usage(void); static void usage(void);
@ -191,6 +196,8 @@ Mavlink::Mavlink() :
_bcast_addr{}, _bcast_addr{},
_src_addr_initialized(false), _src_addr_initialized(false),
_broadcast_address_found(false), _broadcast_address_found(false),
_network_buf{},
_network_buf_len(0),
#endif #endif
_socket_fd(-1), _socket_fd(-1),
_protocol(SERIAL), _protocol(SERIAL),
@ -824,10 +831,61 @@ Mavlink::get_free_tx_buf()
} }
void void
Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID) Mavlink::send_packet()
{ {
uint8_t payload_len = mavlink_message_meta[msgid].msg_len; #ifdef __PX4_POSIX
unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
int ret = -1;
if (get_protocol() == UDP) {
ret = sendto(_socket_fd, _network_buf, _network_buf_len, 0, (struct sockaddr *)&_src_addr, sizeof(_src_addr));
struct telemetry_status_s &tstatus = get_rx_status();
warnx("UDP count: %d", ret);
/* resend message via broadcast if no valid connection exists */
if ((_mode != MAVLINK_MODE_ONBOARD) &&
(!get_client_source_initialized()
|| (hrt_elapsed_time(&tstatus.heartbeat_time) > 3 * 1000 * 1000))) {
if (!_broadcast_address_found) {
find_broadcast_address();
}
if (_broadcast_address_found) {
int bret = sendto(_socket_fd, _network_buf, _network_buf_len, 0, (struct sockaddr *)&_bcast_addr, sizeof(_bcast_addr));
if (bret <= 0) {
PX4_WARN("sending broadcast failed, errno: %d: %s", errno, strerror(errno));
} else {
warnx("BROADCAST count: %d", bret);
}
}
}
} else if (get_protocol() == TCP) {
/* not implemented, but possible to do so */
warnx("TCP transport pending implementation");
}
_network_buf_len = 0;
#endif
}
void
Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len)
{
warnx("send bytes");
/* If the wait until transmit flag is on, only transmit after we've received messages.
Otherwise, transmit all the time. */
if (!should_transmit()) {
warnx("not transmitting");
return;
}
pthread_mutex_lock(&_send_mutex);
_last_write_try_time = hrt_absolute_time(); _last_write_try_time = hrt_absolute_time();
@ -847,45 +905,6 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID
} }
} }
mavlink_get_channel_status(_channel)->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
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_meta[msgid].crc_extra, &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_bytes(&buf[0], packet_len);
}
void
Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len)
{
/* 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);
size_t ret = -1; size_t ret = -1;
/* send message to UART */ /* send message to UART */
@ -894,33 +913,14 @@ Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len)
} }
#ifdef __PX4_POSIX #ifdef __PX4_POSIX
if (get_protocol() == UDP) {
ret = sendto(_socket_fd, buf, packet_len, 0, (struct sockaddr *)&_src_addr, sizeof(_src_addr));
struct telemetry_status_s &tstatus = get_rx_status(); else {
if (_network_buf_len + packet_len < sizeof(_network_buf) / sizeof(_network_buf[0])) {
memcpy(&_network_buf[_network_buf_len], buf, packet_len);
_network_buf_len += packet_len;
/* resend message via broadcast if no valid connection exists */ ret = packet_len;
if ((_mode != MAVLINK_MODE_ONBOARD) &&
(!get_client_source_initialized()
|| (hrt_elapsed_time(&tstatus.heartbeat_time) > 3 * 1000 * 1000))) {
if (!_broadcast_address_found) {
find_broadcast_address();
}
if (_broadcast_address_found) {
int bret = sendto(_socket_fd, buf, packet_len, 0, (struct sockaddr *)&_bcast_addr, sizeof(_bcast_addr));
if (bret <= 0) {
PX4_WARN("sending broadcast failed, errno: %d: %s", errno, strerror(errno));
}
}
} }
} else if (get_protocol() == TCP) {
/* not implemented, but possible to do so */
warnx("TCP transport pending implementation");
} }
#endif #endif
@ -1231,7 +1231,7 @@ void Mavlink::send_autopilot_capabilites()
mcu_unique_id(uid); mcu_unique_id(uid);
msg.uid = (((uint64_t)uid[1]) << 32) | uid[2]; msg.uid = (((uint64_t)uid[1]) << 32) | uid[2];
this->send_message(MAVLINK_MSG_ID_AUTOPILOT_VERSION, &msg); mavlink_msg_autopilot_version_send_struct(get_channel(), &msg);
} }
} }
@ -2044,7 +2044,7 @@ Mavlink::task_main(int argc, char *argv[])
msg.result = command_ack.result; msg.result = command_ack.result;
msg.command = command_ack.command; msg.command = command_ack.command;
send_message(MAVLINK_MSG_ID_COMMAND_ACK, &msg); mavlink_msg_command_ack_send_struct(get_channel(), &msg);
} }
struct mavlink_log_s mavlink_log; struct mavlink_log_s mavlink_log;

15
src/modules/mavlink/mavlink_main.h
View File

@ -224,10 +224,18 @@ public:
*/ */
bool get_manual_input_mode_generation() { return _generate_rc; } bool get_manual_input_mode_generation() { return _generate_rc; }
void send_message(const uint8_t msgid, const void *msg, uint8_t component_ID = 0); /**
* Send bytes out on the link.
*
* On a network port these might actually get buffered to form a packet.
*/
void send_bytes(const uint8_t *buf, unsigned packet_len); void send_bytes(const uint8_t *buf, unsigned packet_len);
/**
* Flush the transmit buffer and send one MAVLink packet
*/
void send_packet();
/** /**
* Resend message as is, don't change sequence number and CRC. * Resend message as is, don't change sequence number and CRC.
*/ */
@ -451,7 +459,8 @@ private:
struct sockaddr_in _bcast_addr; struct sockaddr_in _bcast_addr;
bool _src_addr_initialized; bool _src_addr_initialized;
bool _broadcast_address_found; bool _broadcast_address_found;
uint8_t _network_buf[MAVLINK_MAX_PACKET_LEN];
unsigned _network_buf_len;
#endif #endif
int _socket_fd; int _socket_fd;
Protocol _protocol; Protocol _protocol;

76
src/modules/mavlink/mavlink_messages.cpp
View File

@ -1,6 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * Copyright (c) 2012-2016 PX4 Development Team. All rights reserved.
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@ -409,7 +409,7 @@ protected:
strncpy(msg.text, (const char *)mavlink_log.text, sizeof(msg.text)); strncpy(msg.text, (const char *)mavlink_log.text, sizeof(msg.text));
msg.text[sizeof(msg.text) - 1] = '\0'; msg.text[sizeof(msg.text) - 1] = '\0';
_mavlink->send_message(MAVLINK_MSG_ID_STATUSTEXT, &msg); mavlink_msg_statustext_send_struct(_mavlink->get_channel(), &msg);
// TODO: the logging doesn't work on Snapdragon yet because of file paths. // TODO: the logging doesn't work on Snapdragon yet because of file paths.
#ifndef __PX4_POSIX_EAGLE #ifndef __PX4_POSIX_EAGLE
@ -537,7 +537,7 @@ protected:
msg.param6 = cmd.param6; msg.param6 = cmd.param6;
msg.param7 = cmd.param7; msg.param7 = cmd.param7;
_mavlink->send_message(MAVLINK_MSG_ID_COMMAND_LONG, &msg); mavlink_msg_command_long_send_struct(_mavlink->get_channel(), &msg);
} }
} }
} }
@ -623,7 +623,7 @@ protected:
msg.errors_count3 = 0; msg.errors_count3 = 0;
msg.errors_count4 = 0; msg.errors_count4 = 0;
_mavlink->send_message(MAVLINK_MSG_ID_SYS_STATUS, &msg); mavlink_msg_sys_status_send_struct(_mavlink->get_channel(), &msg);
/* battery status message with higher resolution */ /* battery status message with higher resolution */
mavlink_battery_status_t bat_msg = {}; mavlink_battery_status_t bat_msg = {};
@ -643,7 +643,7 @@ protected:
} }
} }
_mavlink->send_message(MAVLINK_MSG_ID_BATTERY_STATUS, &bat_msg); mavlink_msg_battery_status_send_struct(_mavlink->get_channel(), &bat_msg);
} }
} }
}; };
@ -749,7 +749,7 @@ protected:
msg.temperature = sensor.baro_temp_celcius[0]; msg.temperature = sensor.baro_temp_celcius[0];
msg.fields_updated = fields_updated; msg.fields_updated = fields_updated;
_mavlink->send_message(MAVLINK_MSG_ID_HIGHRES_IMU, &msg); mavlink_msg_highres_imu_send_struct(_mavlink->get_channel(), &msg);
} }
} }
}; };
@ -813,7 +813,7 @@ protected:
msg.pitchspeed = att.pitchspeed; msg.pitchspeed = att.pitchspeed;
msg.yawspeed = att.yawspeed; msg.yawspeed = att.yawspeed;
_mavlink->send_message(MAVLINK_MSG_ID_ATTITUDE, &msg); mavlink_msg_attitude_send_struct(_mavlink->get_channel(), &msg);
} }
} }
}; };
@ -877,7 +877,7 @@ protected:
msg.pitchspeed = att.pitchspeed; msg.pitchspeed = att.pitchspeed;
msg.yawspeed = att.yawspeed; msg.yawspeed = att.yawspeed;
_mavlink->send_message(MAVLINK_MSG_ID_ATTITUDE_QUATERNION, &msg); mavlink_msg_attitude_quaternion_send_struct(_mavlink->get_channel(), &msg);
} }
} }
}; };
@ -983,7 +983,7 @@ protected:
} }
msg.climb = -pos.vel_d; msg.climb = -pos.vel_d;
_mavlink->send_message(MAVLINK_MSG_ID_VFR_HUD, &msg); mavlink_msg_vfr_hud_send_struct(_mavlink->get_channel(), &msg);
} }
} }
}; };
@ -1049,7 +1049,7 @@ protected:
msg.cog = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f, msg.cog = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f,
msg.satellites_visible = gps.satellites_used; msg.satellites_visible = gps.satellites_used;
_mavlink->send_message(MAVLINK_MSG_ID_GPS_RAW_INT, &msg); mavlink_msg_gps_raw_int_send_struct(_mavlink->get_channel(), &msg);
} }
} }
}; };
@ -1095,7 +1095,7 @@ protected:
msg.time_boot_ms = hrt_absolute_time() / 1000; msg.time_boot_ms = hrt_absolute_time() / 1000;
msg.time_unix_usec = (uint64_t)tv.tv_sec * 1000000 + tv.tv_nsec / 1000; msg.time_unix_usec = (uint64_t)tv.tv_sec * 1000000 + tv.tv_nsec / 1000;
_mavlink->send_message(MAVLINK_MSG_ID_SYSTEM_TIME, &msg); mavlink_msg_system_time_send_struct(_mavlink->get_channel(), &msg);
} }
}; };
@ -1137,7 +1137,7 @@ protected:
msg.tc1 = 0; msg.tc1 = 0;
msg.ts1 = hrt_absolute_time() * 1000; // boot time in nanoseconds msg.ts1 = hrt_absolute_time() * 1000; // boot time in nanoseconds
_mavlink->send_message(MAVLINK_MSG_ID_TIMESYNC, &msg); mavlink_msg_timesync_send_struct(_mavlink->get_channel(), &msg);
} }
}; };
@ -1204,7 +1204,7 @@ protected:
msg.flags = pos.flags; msg.flags = pos.flags;
msg.squawk = pos.squawk; msg.squawk = pos.squawk;
_mavlink->send_message(MAVLINK_MSG_ID_ADSB_VEHICLE, &msg); mavlink_msg_adsb_vehicle_send_struct(_mavlink->get_channel(), &msg);
} }
} }
}; };
@ -1263,7 +1263,7 @@ protected:
/* ensure that only active trigger events are sent */ /* ensure that only active trigger events are sent */
if (trigger.timestamp > 0) { if (trigger.timestamp > 0) {
_mavlink->send_message(MAVLINK_MSG_ID_CAMERA_TRIGGER, &msg); mavlink_msg_camera_trigger_send_struct(_mavlink->get_channel(), &msg);
} }
} }
} }
@ -1337,7 +1337,7 @@ protected:
msg.vz = pos.vel_d * 100.0f; msg.vz = pos.vel_d * 100.0f;
msg.hdg = _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f; msg.hdg = _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f;
_mavlink->send_message(MAVLINK_MSG_ID_GLOBAL_POSITION_INT, &msg); mavlink_msg_global_position_int_send_struct(_mavlink->get_channel(), &msg);
} }
} }
}; };
@ -1401,7 +1401,7 @@ protected:
vmsg.pitch = rpy(1); vmsg.pitch = rpy(1);
vmsg.yaw = rpy(2); vmsg.yaw = rpy(2);
_mavlink->send_message(MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE, &vmsg); mavlink_msg_vision_position_estimate_send_struct(_mavlink->get_channel(), &vmsg);
} }
} }
}; };
@ -1463,7 +1463,7 @@ protected:
msg.vy = pos.vy; msg.vy = pos.vy;
msg.vz = pos.vz; msg.vz = pos.vz;
_mavlink->send_message(MAVLINK_MSG_ID_LOCAL_POSITION_NED, &msg); mavlink_msg_local_position_ned_send_struct(_mavlink->get_channel(), &msg);
} }
} }
}; };
@ -1536,7 +1536,7 @@ protected:
msg.covariance[11] = est.timeout_flags; msg.covariance[11] = est.timeout_flags;
memcpy(msg.covariance, est.covariances, sizeof(est.covariances)); memcpy(msg.covariance, est.covariances, sizeof(est.covariances));
_mavlink->send_message(MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV, &msg); mavlink_msg_local_position_ned_cov_send_struct(_mavlink->get_channel(), &msg);
} }
} }
}; };
@ -1591,7 +1591,7 @@ protected:
// To be added and filled // To be added and filled
// mavlink_estimator_status_t msg = {}; // mavlink_estimator_status_t msg = {};
//_mavlink->send_message(MAVLINK_MSG_ID_ESTIMATOR_STATUS, &msg); //mavlink_msg_statustext_send_struct(_mavlink->get_channel(), &msg);
mavlink_vibration_t msg = {}; mavlink_vibration_t msg = {};
@ -1599,7 +1599,7 @@ protected:
msg.vibration_y = est.vibe[1]; msg.vibration_y = est.vibe[1];
msg.vibration_z = est.vibe[2]; msg.vibration_z = est.vibe[2];
_mavlink->send_message(MAVLINK_MSG_ID_VIBRATION, &msg); mavlink_msg_vibration_send_struct(_mavlink->get_channel(), &msg);
} }
} }
}; };
@ -1662,7 +1662,7 @@ protected:
msg.y = mocap.y; msg.y = mocap.y;
msg.z = mocap.z; msg.z = mocap.z;
_mavlink->send_message(MAVLINK_MSG_ID_ATT_POS_MOCAP, &msg); mavlink_msg_att_pos_mocap_send_struct(_mavlink->get_channel(), &msg);
} }
} }
}; };
@ -1735,7 +1735,7 @@ protected:
msg.approach_y = 0.0f; msg.approach_y = 0.0f;
msg.approach_z = 0.0f; msg.approach_z = 0.0f;
_mavlink->send_message(MAVLINK_MSG_ID_HOME_POSITION, &msg); mavlink_msg_home_position_send_struct(_mavlink->get_channel(), &msg);
} }
} }
} }
@ -1817,7 +1817,7 @@ protected:
msg.servo7_raw = act.output[6]; msg.servo7_raw = act.output[6];
msg.servo8_raw = act.output[7]; msg.servo8_raw = act.output[7];
_mavlink->send_message(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, &msg); mavlink_msg_servo_output_raw_send_struct(_mavlink->get_channel(), &msg);
} }
} }
}; };
@ -1910,7 +1910,7 @@ protected:
msg.controls[i] = att_ctrl.control[i]; msg.controls[i] = att_ctrl.control[i];
} }
_mavlink->send_message(MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET, &msg); mavlink_msg_actuator_control_target_send_struct(_mavlink->get_channel(), &msg);
} }
} }
}; };
@ -2076,7 +2076,7 @@ protected:
msg.mode = mavlink_base_mode; msg.mode = mavlink_base_mode;
msg.nav_mode = 0; msg.nav_mode = 0;
_mavlink->send_message(MAVLINK_MSG_ID_HIL_CONTROLS, &msg); mavlink_msg_hil_controls_send_struct(_mavlink->get_channel(), &msg);
} }
} }
}; };
@ -2135,7 +2135,7 @@ protected:
msg.lon_int = pos_sp_triplet.current.lon * 1e7; msg.lon_int = pos_sp_triplet.current.lon * 1e7;
msg.alt = pos_sp_triplet.current.alt; msg.alt = pos_sp_triplet.current.alt;
_mavlink->send_message(MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT, &msg); mavlink_msg_position_target_global_int_send_struct(_mavlink->get_channel(), &msg);
} }
} }
}; };
@ -2203,7 +2203,7 @@ protected:
msg.afy = pos_sp.acc_y; msg.afy = pos_sp.acc_y;
msg.afz = pos_sp.acc_z; msg.afz = pos_sp.acc_z;
_mavlink->send_message(MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, &msg); mavlink_msg_position_target_local_ned_send_struct(_mavlink->get_channel(), &msg);
} }
} }
}; };
@ -2275,7 +2275,7 @@ protected:
msg.thrust = att_sp.thrust; msg.thrust = att_sp.thrust;
_mavlink->send_message(MAVLINK_MSG_ID_ATTITUDE_TARGET, &msg); mavlink_msg_attitude_target_send_struct(_mavlink->get_channel(), &msg);
} }
} }
}; };
@ -2355,7 +2355,7 @@ protected:
msg.rssi = (rc.channel_count > 0) ? rc.rssi : 0; msg.rssi = (rc.channel_count > 0) ? rc.rssi : 0;
_mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS, &msg); mavlink_msg_rc_channels_send_struct(_mavlink->get_channel(), &msg);
/* send override message - harmless if connected to GCS, allows to connect a board to a Linux system */ /* send override message - harmless if connected to GCS, allows to connect a board to a Linux system */
/* http://mavlink.org/messages/common#RC_CHANNELS_OVERRIDE */ /* http://mavlink.org/messages/common#RC_CHANNELS_OVERRIDE */
@ -2371,7 +2371,7 @@ protected:
over.chan7_raw = msg.chan7_raw; over.chan7_raw = msg.chan7_raw;
over.chan8_raw = msg.chan8_raw; over.chan8_raw = msg.chan8_raw;
_mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, &over); mavlink_msg_rc_channels_override_send_struct(_mavlink->get_channel(), &over);
} }
} }
}; };
@ -2440,7 +2440,7 @@ protected:
msg.buttons |= (manual.acro_switch << (shift * 4)); msg.buttons |= (manual.acro_switch << (shift * 4));
msg.buttons |= (manual.offboard_switch << (shift * 5)); msg.buttons |= (manual.offboard_switch << (shift * 5));
_mavlink->send_message(MAVLINK_MSG_ID_MANUAL_CONTROL, &msg); mavlink_msg_manual_control_send_struct(_mavlink->get_channel(), &msg);
} }
} }
}; };
@ -2508,7 +2508,7 @@ protected:
msg.time_delta_distance_us = flow.time_since_last_sonar_update; msg.time_delta_distance_us = flow.time_since_last_sonar_update;
msg.temperature = flow.gyro_temperature; msg.temperature = flow.gyro_temperature;
_mavlink->send_message(MAVLINK_MSG_ID_OPTICAL_FLOW_RAD, &msg); mavlink_msg_optical_flow_rad_send_struct(_mavlink->get_channel(), &msg);
} }
} }
}; };
@ -2568,7 +2568,7 @@ protected:
msg.name[sizeof(msg.name) - 1] = '\0'; msg.name[sizeof(msg.name) - 1] = '\0';
msg.value = debug.value; msg.value = debug.value;
_mavlink->send_message(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, &msg); mavlink_msg_named_value_float_send_struct(_mavlink->get_channel(), &msg);
} }
} }
}; };
@ -2635,7 +2635,7 @@ protected:
msg.alt_error = _tecs_status.altitude_filtered - _tecs_status.altitudeSp; msg.alt_error = _tecs_status.altitude_filtered - _tecs_status.altitudeSp;
msg.aspd_error = _tecs_status.airspeed_filtered - _tecs_status.airspeedSp; msg.aspd_error = _tecs_status.airspeed_filtered - _tecs_status.airspeedSp;
_mavlink->send_message(MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT, &msg); mavlink_msg_nav_controller_output_send_struct(_mavlink->get_channel(), &msg);
} }
} }
}; };
@ -2700,7 +2700,7 @@ protected:
msg.param6 = 0; msg.param6 = 0;
msg.param7 = 0; msg.param7 = 0;
_mavlink->send_message(MAVLINK_MSG_ID_COMMAND_LONG, &msg); mavlink_msg_command_long_send_struct(_mavlink->get_channel(), &msg);
} }
}; };
@ -2783,7 +2783,7 @@ protected:
msg.current_distance = dist_sensor.current_distance * 100.0f; /* m to cm */ msg.current_distance = dist_sensor.current_distance * 100.0f; /* m to cm */
msg.covariance = dist_sensor.covariance; msg.covariance = dist_sensor.covariance;
_mavlink->send_message(MAVLINK_MSG_ID_DISTANCE_SENSOR, &msg); mavlink_msg_distance_sensor_send_struct(_mavlink->get_channel(), &msg);
} }
} }
}; };
@ -2877,7 +2877,7 @@ protected:
} }
if (updated) { if (updated) {
_mavlink->send_message(MAVLINK_MSG_ID_EXTENDED_SYS_STATE, &_msg); mavlink_msg_extended_sys_state_send_struct(_mavlink->get_channel(), &_msg);
} }
} }
}; };
@ -2971,7 +2971,7 @@ protected:
msg.bottom_clearance = NAN; msg.bottom_clearance = NAN;
} }
_mavlink->send_message(MAVLINK_MSG_ID_ALTITUDE, &msg); mavlink_msg_altitude_send_struct(_mavlink->get_channel(), &msg);
} }
} }
}; };

14
src/modules/mavlink/mavlink_mission.cpp
View File

@ -1,6 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * Copyright (c) 2012-2016 PX4 Development Team. All rights reserved.
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@ -188,7 +188,7 @@ MavlinkMissionManager::send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t t
wpa.target_component = compid; wpa.target_component = compid;
wpa.type = type; wpa.type = type;
_mavlink->send_message(MAVLINK_MSG_ID_MISSION_ACK, &wpa); mavlink_msg_mission_ack_send_struct(_mavlink->get_channel(), &wpa);
if (_verbose) { warnx("WPM: Send MISSION_ACK type %u to ID %u", wpa.type, wpa.target_system); } if (_verbose) { warnx("WPM: Send MISSION_ACK type %u to ID %u", wpa.type, wpa.target_system); }
} }
@ -202,7 +202,7 @@ MavlinkMissionManager::send_mission_current(uint16_t seq)
wpc.seq = seq; wpc.seq = seq;
_mavlink->send_message(MAVLINK_MSG_ID_MISSION_CURRENT, &wpc); mavlink_msg_mission_current_send_struct(_mavlink->get_channel(), &wpc);
} else if (seq == 0 && _count == 0) { } else if (seq == 0 && _count == 0) {
/* don't broadcast if no WPs */ /* don't broadcast if no WPs */
@ -226,7 +226,7 @@ MavlinkMissionManager::send_mission_count(uint8_t sysid, uint8_t compid, uint16_
wpc.target_component = compid; wpc.target_component = compid;
wpc.count = _count; wpc.count = _count;
_mavlink->send_message(MAVLINK_MSG_ID_MISSION_COUNT, &wpc); mavlink_msg_mission_count_send_struct(_mavlink->get_channel(), &wpc);
if (_verbose) { warnx("WPM: Send MISSION_COUNT %u to ID %u", wpc.count, wpc.target_system); } if (_verbose) { warnx("WPM: Send MISSION_COUNT %u to ID %u", wpc.count, wpc.target_system); }
} }
@ -250,7 +250,7 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t
wp.seq = seq; wp.seq = seq;
wp.current = (_current_seq == seq) ? 1 : 0; wp.current = (_current_seq == seq) ? 1 : 0;
_mavlink->send_message(MAVLINK_MSG_ID_MISSION_ITEM, &wp); mavlink_msg_mission_item_send_struct(_mavlink->get_channel(), &wp);
if (_verbose) { warnx("WPM: Send MISSION_ITEM seq %u to ID %u", wp.seq, wp.target_system); } if (_verbose) { warnx("WPM: Send MISSION_ITEM seq %u to ID %u", wp.seq, wp.target_system); }
@ -276,7 +276,7 @@ MavlinkMissionManager::send_mission_request(uint8_t sysid, uint8_t compid, uint1
wpr.target_component = compid; wpr.target_component = compid;
wpr.seq = seq; wpr.seq = seq;
_mavlink->send_message(MAVLINK_MSG_ID_MISSION_REQUEST, &wpr); mavlink_msg_mission_request_send_struct(_mavlink->get_channel(), &wpr);
if (_verbose) { warnx("WPM: Send MISSION_REQUEST seq %u to ID %u", wpr.seq, wpr.target_system); } if (_verbose) { warnx("WPM: Send MISSION_REQUEST seq %u to ID %u", wpr.seq, wpr.target_system); }
@ -295,7 +295,7 @@ MavlinkMissionManager::send_mission_item_reached(uint16_t seq)
wp_reached.seq = seq; wp_reached.seq = seq;
_mavlink->send_message(MAVLINK_MSG_ID_MISSION_ITEM_REACHED, &wp_reached); mavlink_msg_mission_item_reached_send_struct(_mavlink->get_channel(), &wp_reached);
if (_verbose) { warnx("WPM: Send MISSION_ITEM_REACHED reached_seq %u", wp_reached.seq); } if (_verbose) { warnx("WPM: Send MISSION_ITEM_REACHED reached_seq %u", wp_reached.seq); }
} }

8
src/modules/mavlink/mavlink_parameters.cpp
View File

@ -186,7 +186,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
strncpy(param_value.param_id, HASH_PARAM, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); strncpy(param_value.param_id, HASH_PARAM, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
param_value.param_type = MAV_PARAM_TYPE_UINT32; param_value.param_type = MAV_PARAM_TYPE_UINT32;
memcpy(&param_value.param_value, &hash, sizeof(hash)); memcpy(&param_value.param_value, &hash, sizeof(hash));
_mavlink->send_message(MAVLINK_MSG_ID_PARAM_VALUE, &param_value); mavlink_msg_param_value_send_struct(_mavlink->get_channel(), &param_value);
} else { } else {
/* local name buffer to enforce null-terminated string */ /* local name buffer to enforce null-terminated string */
char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
@ -302,7 +302,7 @@ MavlinkParametersManager::send(const hrt_abstime t)
memcpy(&msg.param_value, &val, sizeof(int32_t)); memcpy(&msg.param_value, &val, sizeof(int32_t));
msg.param_type = MAVLINK_TYPE_INT32_T; msg.param_type = MAVLINK_TYPE_INT32_T;
} }
_mavlink->send_message(MAVLINK_MSG_ID_PARAM_VALUE, &msg, value.node_id); mavlink_msg_param_value_send_struct(_mavlink->get_channel(), &msg);
} else if (_send_all_index >= 0 && _mavlink->boot_complete()) { } else if (_send_all_index >= 0 && _mavlink->boot_complete()) {
/* send all parameters if requested, but only after the system has booted */ /* send all parameters if requested, but only after the system has booted */
@ -325,7 +325,7 @@ MavlinkParametersManager::send(const hrt_abstime t)
strncpy(msg.param_id, HASH_PARAM, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); strncpy(msg.param_id, HASH_PARAM, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
msg.param_type = MAV_PARAM_TYPE_UINT32; msg.param_type = MAV_PARAM_TYPE_UINT32;
memcpy(&msg.param_value, &hash, sizeof(hash)); memcpy(&msg.param_value, &hash, sizeof(hash));
_mavlink->send_message(MAVLINK_MSG_ID_PARAM_VALUE, &msg); mavlink_msg_param_value_send_struct(_mavlink->get_channel(), &msg);
/* after this we should start sending all params */ /* after this we should start sending all params */
_send_all_index = 0; _send_all_index = 0;
@ -396,7 +396,7 @@ MavlinkParametersManager::send_param(param_t param)
msg.param_type = MAVLINK_TYPE_FLOAT; msg.param_type = MAVLINK_TYPE_FLOAT;
} }
_mavlink->send_message(MAVLINK_MSG_ID_PARAM_VALUE, &msg); mavlink_msg_param_value_send_struct(_mavlink->get_channel(), &msg);
return 0; return 0;
} }

6
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -1353,9 +1353,7 @@ MavlinkReceiver::handle_message_ping(mavlink_message_t *msg)
if ((mavlink_system.sysid == ping.target_system) && if ((mavlink_system.sysid == ping.target_system) &&
(mavlink_system.compid == ping.target_component)) { (mavlink_system.compid == ping.target_component)) {
mavlink_message_t msg_out; mavlink_msg_ping_send_struct(_mavlink->get_channel(), &ping);
mavlink_msg_ping_encode(_mavlink->get_system_id(), _mavlink->get_component_id(), &msg_out, &ping);
_mavlink->send_message(MAVLINK_MSG_ID_PING, &msg_out);
} }
} }
@ -1423,7 +1421,7 @@ MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg)
rsync.tc1 = now_ns; rsync.tc1 = now_ns;
rsync.ts1 = tsync.ts1; rsync.ts1 = tsync.ts1;
_mavlink->send_message(MAVLINK_MSG_ID_TIMESYNC, &rsync); mavlink_msg_timesync_send_struct(_mavlink->get_channel(), &rsync);
return; return;