aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--ROMFS/px4fmu_common/init.d/rc.usb6
-rw-r--r--src/modules/mavlink/mavlink_bridge_header.h7
-rw-r--r--src/modules/mavlink/mavlink_commands.cpp73
-rw-r--r--src/modules/mavlink/mavlink_ftp.h1
-rw-r--r--src/modules/mavlink/mavlink_main.cpp622
-rw-r--r--src/modules/mavlink/mavlink_main.h89
-rw-r--r--src/modules/mavlink/mavlink_messages.cpp1406
-rw-r--r--src/modules/mavlink/mavlink_messages.h4
-rw-r--r--src/modules/mavlink/mavlink_mission.cpp51
-rw-r--r--src/modules/mavlink/mavlink_mission.h119
-rw-r--r--src/modules/mavlink/mavlink_parameters.cpp197
-rw-r--r--src/modules/mavlink/mavlink_parameters.h (renamed from src/modules/mavlink/mavlink_commands.h)92
-rw-r--r--src/modules/mavlink/mavlink_rate_limiter.cpp2
-rw-r--r--src/modules/mavlink/mavlink_stream.cpp27
-rw-r--r--src/modules/mavlink/mavlink_stream.h33
-rw-r--r--src/modules/mavlink/module.mk2
16 files changed, 1539 insertions, 1192 deletions
diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb
index 1f8d8b862..eb6039b4c 100644
--- a/ROMFS/px4fmu_common/init.d/rc.usb
+++ b/ROMFS/px4fmu_common/init.d/rc.usb
@@ -5,6 +5,10 @@
mavlink start -r 10000 -d /dev/ttyACM0 -x
# Enable a number of interesting streams we want via USB
+mavlink stream -d /dev/ttyACM0 -s PARAM_VALUE -r 200
+usleep 100000
+mavlink stream -d /dev/ttyACM0 -s MISSION_ITEM -r 50
+usleep 100000
mavlink stream -d /dev/ttyACM0 -s NAMED_VALUE_FLOAT -r 10
usleep 100000
mavlink stream -d /dev/ttyACM0 -s OPTICAL_FLOW -r 10
@@ -15,6 +19,8 @@ mavlink stream -d /dev/ttyACM0 -s ATTITUDE -r 20
usleep 100000
mavlink stream -d /dev/ttyACM0 -s ATTITUDE_CONTROLS -r 30
usleep 100000
+mavlink stream -d /dev/ttyACM0 -s RC_CHANNELS_RAW -r 5
+usleep 100000
mavlink stream -d /dev/ttyACM0 -s SERVO_OUTPUT_RAW_0 -r 20
usleep 100000
mavlink stream -d /dev/ttyACM0 -s GLOBAL_POSITION_SETPOINT_INT -r 20
diff --git a/src/modules/mavlink/mavlink_bridge_header.h b/src/modules/mavlink/mavlink_bridge_header.h
index 374d1511c..0cd08769e 100644
--- a/src/modules/mavlink/mavlink_bridge_header.h
+++ b/src/modules/mavlink/mavlink_bridge_header.h
@@ -44,7 +44,12 @@
__BEGIN_DECLS
-#define MAVLINK_USE_CONVENIENCE_FUNCTIONS
+/*
+ * We are NOT using convenience functions,
+ * but instead send messages with a custom function.
+ * So we do NOT do this:
+ * #define MAVLINK_USE_CONVENIENCE_FUNCTIONS
+ */
/* use efficient approach, see mavlink_helpers.h */
#define MAVLINK_SEND_UART_BYTES mavlink_send_uart_bytes
diff --git a/src/modules/mavlink/mavlink_commands.cpp b/src/modules/mavlink/mavlink_commands.cpp
deleted file mode 100644
index b502c3c86..000000000
--- a/src/modules/mavlink/mavlink_commands.cpp
+++ /dev/null
@@ -1,73 +0,0 @@
-/****************************************************************************
- *
- * Copyright (c) 2014 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- * notice, this list of conditions and the following disclaimer in
- * the documentation and/or other materials provided with the
- * distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- * used to endorse or promote products derived from this software
- * without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file mavlink_commands.cpp
- * Mavlink commands stream implementation.
- *
- * @author Anton Babushkin <anton.babushkin@me.com>
- */
-
-#include "mavlink_commands.h"
-
-MavlinkCommandsStream::MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel) :
- _cmd_sub(mavlink->add_orb_subscription(ORB_ID(vehicle_command))),
- _cmd{},
- _channel(channel),
- _cmd_time(0)
-{
-}
-
-void
-MavlinkCommandsStream::update(const hrt_abstime t)
-{
- struct vehicle_command_s cmd;
-
- if (_cmd_sub->update(&_cmd_time, &cmd)) {
- /* only send commands for other systems/components */
- if (cmd.target_system != mavlink_system.sysid || cmd.target_component != mavlink_system.compid) {
- mavlink_msg_command_long_send(_channel,
- cmd.target_system,
- cmd.target_component,
- cmd.command,
- cmd.confirmation,
- cmd.param1,
- cmd.param2,
- cmd.param3,
- cmd.param4,
- cmd.param5,
- cmd.param6,
- cmd.param7);
- }
- }
-}
diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h
index 59237a4c4..1bd1158fb 100644
--- a/src/modules/mavlink/mavlink_ftp.h
+++ b/src/modules/mavlink/mavlink_ftp.h
@@ -55,6 +55,7 @@
#include <systemlib/err.h>
#include "mavlink_messages.h"
+#include "mavlink_main.h"
class MavlinkFTP
{
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 861c91d47..5589738c1 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -78,7 +78,6 @@
#include "mavlink_messages.h"
#include "mavlink_receiver.h"
#include "mavlink_rate_limiter.h"
-#include "mavlink_commands.h"
#ifndef MAVLINK_CRC_EXTRA
#error MAVLINK_CRC_EXTRA has to be defined on PX4 systems
@@ -94,11 +93,16 @@ static const int ERROR = -1;
#define MAX_DATA_RATE 10000 // max data rate in bytes/s
#define MAIN_LOOP_DELAY 10000 // 100 Hz @ 1000 bytes/s data rate
+#define TX_BUFFER_GAP MAVLINK_MAX_PACKET_LEN
+
static Mavlink *_mavlink_instances = nullptr;
/* TODO: if this is a class member it crashes */
static struct file_operations fops;
+static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS;
+static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS;
+
/**
* mavlink app start / stop handling function
*
@@ -108,113 +112,6 @@ extern "C" __EXPORT int mavlink_main(int argc, char *argv[]);
extern mavlink_system_t mavlink_system;
-static uint64_t last_write_success_times[6] = {0};
-static uint64_t last_write_try_times[6] = {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);
- }
- }
-}
-
static void usage(void);
Mavlink::Mavlink() :
@@ -233,8 +130,7 @@ Mavlink::Mavlink() :
_subscriptions(nullptr),
_streams(nullptr),
_mission_manager(nullptr),
- _mission_pub(-1),
- _mission_result_sub(-1),
+ _parameters_manager(nullptr),
_mode(MAVLINK_MODE_NORMAL),
_channel(MAVLINK_COMM_0),
_logbuffer {},
@@ -246,12 +142,16 @@ Mavlink::Mavlink() :
_ftp_on(false),
_uart_fd(-1),
_baudrate(57600),
- _datarate(10000),
+ _datarate(1000),
+ _datarate_events(500),
+ _rate_mult(1.0f),
_mavlink_param_queue_index(0),
mavlink_link_termination_allowed(false),
_subscribe_to_stream(nullptr),
_subscribe_to_stream_rate(0.0f),
_flow_control_enabled(true),
+ _last_write_success_time(0),
+ _last_write_try_time(0),
_bytes_tx(0),
_bytes_txerr(0),
_bytes_rx(0),
@@ -262,6 +162,7 @@ Mavlink::Mavlink() :
_rstatus {},
_message_buffer {},
_message_buffer_mutex {},
+ _send_mutex {},
_param_initialized(false),
_param_system_id(0),
_param_component_id(0),
@@ -486,7 +387,7 @@ Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *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)) {
+ if (!(self->_mode != MAVLINK_MODE_NORMAL && msg->sysid != mavlink_system.sysid)) {
inst->pass_message(msg);
}
}
@@ -536,10 +437,26 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg)
case (int)MAVLINK_IOC_SEND_TEXT_EMERGENCY: {
const char *txt = (const char *)arg;
-// printf("logmsg: %s\n", txt);
struct mavlink_logmessage msg;
strncpy(msg.text, txt, sizeof(msg.text));
- msg.severity = (unsigned char)cmd;
+
+ switch (cmd) {
+ case MAVLINK_IOC_SEND_TEXT_INFO:
+ msg.severity = MAV_SEVERITY_INFO;
+ break;
+
+ case MAVLINK_IOC_SEND_TEXT_CRITICAL:
+ msg.severity = MAV_SEVERITY_CRITICAL;
+ break;
+
+ case MAVLINK_IOC_SEND_TEXT_EMERGENCY:
+ msg.severity = MAV_SEVERITY_EMERGENCY;
+ break;
+
+ default:
+ msg.severity = MAV_SEVERITY_INFO;
+ break;
+ }
Mavlink *inst;
LL_FOREACH(_mavlink_instances, inst) {
@@ -733,6 +650,9 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
if (enable_flow_control(true)) {
warnx("hardware flow control not supported");
}
+
+ } else {
+ _flow_control_enabled = false;
}
return _uart_fd;
@@ -774,8 +694,7 @@ Mavlink::set_hil_enabled(bool hil_enabled)
/* enable HIL */
if (hil_enabled && !_hil_enabled) {
_hil_enabled = true;
- float rate_mult = _datarate / 1000.0f;
- configure_stream("HIL_CONTROLS", 15.0f * rate_mult);
+ configure_stream("HIL_CONTROLS", 150.0f);
}
/* disable HIL */
@@ -790,248 +709,188 @@ Mavlink::set_hil_enabled(bool hil_enabled)
return ret;
}
-void
-Mavlink::send_message(const mavlink_message_t *msg)
+unsigned
+Mavlink::get_free_tx_buf()
{
- uint8_t buf[MAVLINK_MAX_PACKET_LEN];
+ /*
+ * Check if the OS buffer is full and disable HW
+ * flow control if it continues to be full
+ */
+ int buf_free = 0;
+ (void) ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free);
+
+ if (get_flow_control_enabled() && buf_free < TX_BUFFER_GAP) {
+ /* 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_time != 0 &&
+ hrt_elapsed_time(&_last_write_success_time) > 500 * 1000UL &&
+ _last_write_success_time != _last_write_try_time) {
+ warnx("DISABLING HARDWARE FLOW CONTROL");
+ enable_flow_control(false);
+ }
+ }
- uint16_t len = mavlink_msg_to_send_buffer(buf, msg);
- mavlink_send_uart_bytes(_channel, buf, len);
+ return buf_free;
}
void
-Mavlink::handle_message(const mavlink_message_t *msg)
+Mavlink::send_message(const uint8_t msgid, const void *msg)
{
- /* handle packet with mission manager */
- _mission_manager->handle_message(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;
+ }
- /* handle packet with parameter component */
- mavlink_pm_message_handler(_channel, msg);
+ pthread_mutex_lock(&_send_mutex);
- if (get_forwarding_on()) {
- /* forward any messages to other mavlink instances */
- Mavlink::forward_message(msg, this);
+ int 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 < TX_BUFFER_GAP) {
+ /* no enough space in buffer to send */
+ count_txerr();
+ count_txerrbytes(packet_len);
+ pthread_mutex_unlock(&_send_mutex);
+ return;
}
-}
-int
-Mavlink::mavlink_pm_queued_send()
-{
- if (_mavlink_param_queue_index < param_count()) {
- mavlink_pm_send_param(param_for_index(_mavlink_param_queue_index));
- _mavlink_param_queue_index++;
- return 0;
+ 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] = mavlink_system.compid;
+ 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 {
- return 1;
+ _last_write_success_time = _last_write_try_time;
+ count_txbytes(packet_len);
}
-}
-void Mavlink::mavlink_pm_start_queued_send()
-{
- _mavlink_param_queue_index = 0;
+ pthread_mutex_unlock(&_send_mutex);
}
-int Mavlink::mavlink_pm_send_param_for_index(uint16_t index)
+void
+Mavlink::resend_message(mavlink_message_t *msg)
{
- return mavlink_pm_send_param(param_for_index(index));
-}
+ /* If the wait until transmit flag is on, only transmit after we've received messages.
+ Otherwise, transmit all the time. */
+ if (!should_transmit()) {
+ return;
+ }
-int Mavlink::mavlink_pm_send_param_for_name(const char *name)
-{
- return mavlink_pm_send_param(param_find(name));
-}
+ pthread_mutex_lock(&_send_mutex);
-int Mavlink::mavlink_pm_send_param(param_t param)
-{
- if (param == PARAM_INVALID) { return 1; }
+ int buf_free = get_free_tx_buf();
- /* buffers for param transmission */
- char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN];
- float val_buf;
- mavlink_message_t tx_msg;
+ _last_write_try_time = hrt_absolute_time();
- /* query parameter type */
- param_type_t type = param_type(param);
- /* copy parameter name */
- strncpy((char *)name_buf, param_name(param), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
+ unsigned packet_len = msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
- /*
- * Map onboard parameter type to MAVLink type,
- * endianess matches (both little endian)
- */
- uint8_t mavlink_type;
+ /* check if there is space in the buffer, let it overflow else */
+ if (buf_free < TX_BUFFER_GAP) {
+ /* no enough space in buffer to send */
+ count_txerr();
+ count_txerrbytes(packet_len);
+ pthread_mutex_unlock(&_send_mutex);
+ return;
+ }
- if (type == PARAM_TYPE_INT32) {
- mavlink_type = MAVLINK_TYPE_INT32_T;
+ uint8_t buf[MAVLINK_MAX_PACKET_LEN];
- } else if (type == PARAM_TYPE_FLOAT) {
- mavlink_type = MAVLINK_TYPE_FLOAT;
+ /* header and payload */
+ memcpy(&buf[0], &msg->magic, MAVLINK_NUM_HEADER_BYTES + msg->len);
- } else {
- mavlink_type = MAVLINK_TYPE_FLOAT;
- }
+ /* 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);
- /*
- * get param value, since MAVLink encodes float and int params in the same
- * space during transmission, copy param onto float val_buf
- */
+ /* send message to UART */
+ ssize_t ret = write(_uart_fd, buf, packet_len);
- int ret;
+ if (ret != (int) packet_len) {
+ count_txerr();
+ count_txerrbytes(packet_len);
- if ((ret = param_get(param, &val_buf)) != OK) {
- return ret;
+ } else {
+ _last_write_success_time = _last_write_try_time;
+ count_txbytes(packet_len);
}
- mavlink_msg_param_value_pack_chan(mavlink_system.sysid,
- mavlink_system.compid,
- _channel,
- &tx_msg,
- name_buf,
- val_buf,
- mavlink_type,
- param_count(),
- param_get_index(param));
- send_message(&tx_msg);
- return OK;
+ pthread_mutex_unlock(&_send_mutex);
}
-void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg)
+void
+Mavlink::handle_message(const mavlink_message_t *msg)
{
- switch (msg->msgid) {
- case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
- mavlink_param_request_list_t req;
- mavlink_msg_param_request_list_decode(msg, &req);
-
- if (req.target_system == mavlink_system.sysid &&
- (req.target_component == mavlink_system.compid || req.target_component == MAV_COMP_ID_ALL)) {
- /* Start sending parameters */
- mavlink_pm_start_queued_send();
- send_statustext_info("[pm] sending list");
- }
- } break;
-
- case MAVLINK_MSG_ID_PARAM_SET: {
-
- /* Handle parameter setting */
-
- if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET) {
- mavlink_param_set_t mavlink_param_set;
- mavlink_msg_param_set_decode(msg, &mavlink_param_set);
-
- if (mavlink_param_set.target_system == mavlink_system.sysid
- && ((mavlink_param_set.target_component == mavlink_system.compid)
- || (mavlink_param_set.target_component == MAV_COMP_ID_ALL))) {
- /* local name buffer to enforce null-terminated string */
- char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
- strncpy(name, mavlink_param_set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
- /* enforce null termination */
- name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
- /* attempt to find parameter, set and send it */
- param_t param = param_find(name);
-
- if (param == PARAM_INVALID) {
- char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
- sprintf(buf, "[pm] unknown: %s", name);
- send_statustext_info(buf);
-
- } else {
- /* set and send parameter */
- param_set(param, &(mavlink_param_set.param_value));
- mavlink_pm_send_param(param);
- }
- }
- }
- } break;
-
- case MAVLINK_MSG_ID_PARAM_REQUEST_READ: {
- mavlink_param_request_read_t mavlink_param_request_read;
- mavlink_msg_param_request_read_decode(msg, &mavlink_param_request_read);
-
- if (mavlink_param_request_read.target_system == mavlink_system.sysid
- && ((mavlink_param_request_read.target_component == mavlink_system.compid)
- || (mavlink_param_request_read.target_component == MAV_COMP_ID_ALL))) {
- /* when no index is given, loop through string ids and compare them */
- if (mavlink_param_request_read.param_index == -1) {
- /* local name buffer to enforce null-terminated string */
- char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
- strncpy(name, mavlink_param_request_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
- /* enforce null termination */
- name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
- /* attempt to find parameter and send it */
- mavlink_pm_send_param_for_name(name);
+ /* handle packet with mission manager */
+ _mission_manager->handle_message(msg);
- } else {
- /* when index is >= 0, send this parameter again */
- mavlink_pm_send_param_for_index(mavlink_param_request_read.param_index);
- }
- }
+ /* handle packet with parameter component */
+ _parameters_manager->handle_message(msg);
- } break;
+ if (get_forwarding_on()) {
+ /* forward any messages to other mavlink instances */
+ Mavlink::forward_message(msg, this);
}
}
-int
+void
Mavlink::send_statustext_info(const char *string)
{
- return send_statustext(MAVLINK_IOC_SEND_TEXT_INFO, string);
+ send_statustext(MAV_SEVERITY_INFO, string);
}
-int
+void
Mavlink::send_statustext_critical(const char *string)
{
- return send_statustext(MAVLINK_IOC_SEND_TEXT_CRITICAL, string);
+ send_statustext(MAV_SEVERITY_CRITICAL, string);
}
-int
+void
Mavlink::send_statustext_emergency(const char *string)
{
- return send_statustext(MAVLINK_IOC_SEND_TEXT_EMERGENCY, string);
+ send_statustext(MAV_SEVERITY_EMERGENCY, string);
}
-int
-Mavlink::send_statustext(unsigned severity, const char *string)
+void
+Mavlink::send_statustext(unsigned char severity, const char *string)
{
- const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN;
- mavlink_statustext_t statustext;
-
- int i = 0;
+ struct mavlink_logmessage logmsg;
+ strncpy(logmsg.text, string, sizeof(logmsg.text));
+ logmsg.severity = severity;
- while (i < len - 1) {
- statustext.text[i] = string[i];
-
- if (string[i] == '\0') {
- break;
- }
-
- i++;
- }
-
- if (i > 1) {
- /* Enforce null termination */
- statustext.text[i] = '\0';
-
- /* Map severity */
- switch (severity) {
- case MAVLINK_IOC_SEND_TEXT_INFO:
- statustext.severity = MAV_SEVERITY_INFO;
- break;
-
- case MAVLINK_IOC_SEND_TEXT_CRITICAL:
- statustext.severity = MAV_SEVERITY_CRITICAL;
- break;
-
- case MAVLINK_IOC_SEND_TEXT_EMERGENCY:
- statustext.severity = MAV_SEVERITY_EMERGENCY;
- break;
- }
-
- mavlink_msg_statustext_send(_channel, statustext.severity, statustext.text);
- return OK;
-
- } else {
- return ERROR;
- }
+ mavlink_logbuffer_write(&_logbuffer, &logmsg);
}
MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic)
@@ -1054,11 +913,17 @@ MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic)
return sub_new;
}
+unsigned int
+Mavlink::interval_from_rate(float rate)
+{
+ return (rate > 0.0f) ? (1000000.0f / rate) : 0;
+}
+
int
Mavlink::configure_stream(const char *stream_name, const float rate)
{
/* calculate interval in us, 0 means disabled stream */
- unsigned int interval = (rate > 0.0f) ? (1000000.0f / rate) : 0;
+ unsigned int interval = interval_from_rate(rate);
/* search if stream exists */
MavlinkStream *stream;
@@ -1089,10 +954,8 @@ Mavlink::configure_stream(const char *stream_name, const float rate)
if (strcmp(stream_name, streams_list[i]->get_name()) == 0) {
/* create new instance */
- stream = streams_list[i]->new_instance();
- stream->set_channel(get_channel());
+ stream = streams_list[i]->new_instance(this);
stream->set_interval(interval);
- stream->subscribe(this);
LL_APPEND(_streams, stream);
return OK;
@@ -1106,6 +969,31 @@ Mavlink::configure_stream(const char *stream_name, const float rate)
}
void
+Mavlink::adjust_stream_rates(const float multiplier)
+{
+ /* do not allow to push us to zero */
+ if (multiplier < 0.01f) {
+ return;
+ }
+
+ /* search if stream exists */
+ MavlinkStream *stream;
+ LL_FOREACH(_streams, stream) {
+ /* set new interval */
+ unsigned interval = stream->get_interval();
+ interval /= multiplier;
+
+ /* allow max ~600 Hz */
+ if (interval < 1600) {
+ interval = 1600;
+ }
+
+ /* set new interval */
+ stream->set_interval(interval * multiplier);
+ }
+}
+
+void
Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate)
{
/* orb subscription must be done from the main thread,
@@ -1265,7 +1153,27 @@ Mavlink::pass_message(const mavlink_message_t *msg)
float
Mavlink::get_rate_mult()
{
- return _datarate / 1000.0f;
+ return _rate_mult;
+}
+
+void
+Mavlink::update_rate_mult()
+{
+ float const_rate = 0.0f;
+ float rate = 0.0f;
+
+ MavlinkStream *stream;
+ LL_FOREACH(_streams, stream) {
+ if (stream->const_rate()) {
+ const_rate += stream->get_size() * 1000000.0f / stream->get_interval();
+
+ } else {
+ rate += stream->get_size() * 1000000.0f / stream->get_interval();
+ }
+ }
+
+ /* don't scale up rates, only scale down if needed */
+ _rate_mult = fminf(1.0f, ((float)_datarate - const_rate) / rate);
}
int
@@ -1403,6 +1311,9 @@ Mavlink::task_main(int argc, char *argv[])
return ERROR;
}
+ /* initialize send mutex */
+ pthread_mutex_init(&_send_mutex, NULL);
+
/* initialize mavlink text message buffering */
mavlink_logbuffer_init(&_logbuffer, 5);
@@ -1412,7 +1323,7 @@ Mavlink::task_main(int argc, char *argv[])
* make space for two messages plus off-by-one space as we use the empty element
* marker ring buffer approach.
*/
- if (OK != message_buffer_init(2 * MAVLINK_MAX_PACKET_LEN + 2)) {
+ if (OK != message_buffer_init(2 * sizeof(mavlink_message_t) + 1)) {
errx(1, "can't allocate message buffer, exiting");
}
@@ -1432,12 +1343,6 @@ Mavlink::task_main(int argc, char *argv[])
/* start the MAVLink receiver */
_receive_thread = MavlinkReceiver::receive_start(this);
- _mission_result_sub = orb_subscribe(ORB_ID(mission_result));
-
- /* create mission manager */
- _mission_manager = new MavlinkMissionManager(this);
- _mission_manager->set_verbose(_verbose);
-
_task_running = true;
MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update));
@@ -1448,34 +1353,50 @@ Mavlink::task_main(int argc, char *argv[])
struct vehicle_status_s status;
status_sub->update(&status_time, &status);
- MavlinkCommandsStream commands_stream(this, _channel);
-
- /* add default streams depending on mode and intervals depending on datarate */
- float rate_mult = get_rate_mult();
+ /* add default streams depending on mode */
+ /* HEARTBEAT is constant rate stream, rate never adjusted */
configure_stream("HEARTBEAT", 1.0f);
+ /* STATUSTEXT stream is like normal stream but gets messages from logbuffer instead of uORB */
+ configure_stream("STATUSTEXT", 20.0f);
+
+ /* COMMAND_LONG stream: use high rate to avoid commands skipping */
+ configure_stream("COMMAND_LONG", 100.0f);
+
+ /* PARAM_VALUE stream */
+ _parameters_manager = (MavlinkParametersManager *) MavlinkParametersManager::new_instance(this);
+ _parameters_manager->set_interval(interval_from_rate(30.0f));
+ LL_APPEND(_streams, _parameters_manager);
+
+ /* MISSION_STREAM stream, actually sends all MISSION_XXX messages at some rate depending on
+ * remote requests rate. Rate specified here controls how much bandwidth we will reserve for
+ * mission messages. */
+ _mission_manager = (MavlinkMissionManager *) MavlinkMissionManager::new_instance(this);
+ _mission_manager->set_interval(interval_from_rate(10.0f));
+ _mission_manager->set_verbose(_verbose);
+ LL_APPEND(_streams, _mission_manager);
+
switch (_mode) {
case MAVLINK_MODE_NORMAL:
configure_stream("SYS_STATUS", 1.0f);
configure_stream("GPS_GLOBAL_ORIGIN", 0.5f);
- configure_stream("HIGHRES_IMU", 1.0f * rate_mult);
- configure_stream("ATTITUDE", 10.0f * rate_mult);
- configure_stream("VFR_HUD", 8.0f * rate_mult);
- configure_stream("GPS_RAW_INT", 1.0f * rate_mult);
- configure_stream("GLOBAL_POSITION_INT", 3.0f * rate_mult);
- configure_stream("LOCAL_POSITION_NED", 3.0f * rate_mult);
- configure_stream("RC_CHANNELS_RAW", 1.0f * rate_mult);
- configure_stream("NAMED_VALUE_FLOAT", 1.0f * rate_mult);
- configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f * rate_mult);
- configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f * rate_mult);
+ configure_stream("HIGHRES_IMU", 1.0f);
+ configure_stream("ATTITUDE", 10.0f);
+ configure_stream("VFR_HUD", 8.0f);
+ configure_stream("GPS_RAW_INT", 1.0f);
+ configure_stream("GLOBAL_POSITION_INT", 3.0f);
+ configure_stream("LOCAL_POSITION_NED", 3.0f);
+ configure_stream("RC_CHANNELS_RAW", 1.0f);
+ configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f);
+ configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f);
configure_stream("DISTANCE_SENSOR", 0.5f);
break;
case MAVLINK_MODE_CAMERA:
configure_stream("SYS_STATUS", 1.0f);
- configure_stream("ATTITUDE", 15.0f * rate_mult);
- configure_stream("GLOBAL_POSITION_INT", 15.0f * rate_mult);
+ configure_stream("ATTITUDE", 15.0f);
+ configure_stream("GLOBAL_POSITION_INT", 15.0f);
configure_stream("CAMERA_CAPTURE", 1.0f);
break;
@@ -1483,13 +1404,8 @@ Mavlink::task_main(int argc, char *argv[])
break;
}
- /* don't send parameters on startup without request */
- _mavlink_param_queue_index = param_count();
-
- MavlinkRateLimiter fast_rate_limiter(30000.0f / rate_mult);
-
/* set main loop delay depending on data rate to minimize CPU overhead */
- _main_loop_delay = MAIN_LOOP_DELAY / rate_mult;
+ _main_loop_delay = MAIN_LOOP_DELAY * 1000 / _datarate;
/* now the instance is fully initialized and we can bump the instance count */
LL_APPEND(_mavlink_instances, this);
@@ -1502,6 +1418,8 @@ Mavlink::task_main(int argc, char *argv[])
hrt_abstime t = hrt_absolute_time();
+ update_rate_mult();
+
if (param_sub->update(&param_time, nullptr)) {
/* parameters updated */
mavlink_update_system();
@@ -1512,9 +1430,6 @@ Mavlink::task_main(int argc, char *argv[])
set_hil_enabled(status.hil_state == HIL_STATE_ON);
}
- /* update commands stream */
- commands_stream.update(t);
-
/* check for requested subscriptions */
if (_subscribe_to_stream != nullptr) {
if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) {
@@ -1540,20 +1455,6 @@ Mavlink::task_main(int argc, char *argv[])
stream->update(t);
}
- if (fast_rate_limiter.check(t)) {
- mavlink_pm_queued_send();
- _mission_manager->eventloop();
-
- if (!mavlink_logbuffer_is_empty(&_logbuffer)) {
- struct mavlink_logmessage msg;
- int lb_ret = mavlink_logbuffer_read(&_logbuffer, &msg);
-
- if (lb_ret == OK) {
- send_statustext(msg.severity, msg.text);
- }
- }
- }
-
/* pass messages from other UARTs or FTP worker */
if (_passing_on || _ftp_on) {
@@ -1598,7 +1499,7 @@ Mavlink::task_main(int argc, char *argv[])
pthread_mutex_unlock(&_message_buffer_mutex);
- _mavlink_resend_uart(_channel, &msg);
+ resend_message(&msg);
}
}
@@ -1613,14 +1514,13 @@ Mavlink::task_main(int argc, char *argv[])
_bytes_txerr = 0;
_bytes_rx = 0;
}
+
_bytes_timestamp = t;
}
perf_end(_loop_perf);
}
- delete _mission_manager;
-
delete _subscribe_to_stream;
_subscribe_to_stream = nullptr;
@@ -1778,10 +1678,12 @@ Mavlink::display_status()
} else {
printf("\tno telem status.\n");
}
+
printf("\trates:\n");
printf("\ttx: %.3f kB/s\n", (double)_rate_tx);
printf("\ttxerr: %.3f kB/s\n", (double)_rate_txerr);
printf("\trx: %.3f kB/s\n", (double)_rate_rx);
+ printf("\trate mult: %.3f\n", (double)_rate_mult);
}
int
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 70d13acb0..1e2e94cef 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -58,7 +58,7 @@
#include "mavlink_stream.h"
#include "mavlink_messages.h"
#include "mavlink_mission.h"
-
+#include "mavlink_parameters.h"
class Mavlink
{
@@ -160,7 +160,12 @@ public:
*/
int set_hil_enabled(bool hil_enabled);
- void send_message(const mavlink_message_t *msg);
+ void send_message(const uint8_t msgid, const void *msg);
+
+ /**
+ * Resend message as is, don't change sequence number and CRC.
+ */
+ void resend_message(mavlink_message_t *msg);
void handle_message(const mavlink_message_t *msg);
@@ -188,29 +193,33 @@ public:
*
* @param string the message to send (will be capped by mavlink max string length)
*/
- int send_statustext_info(const char *string);
+ void send_statustext_info(const char *string);
/**
* Send a status text with loglevel CRITICAL
*
* @param string the message to send (will be capped by mavlink max string length)
*/
- int send_statustext_critical(const char *string);
+ void send_statustext_critical(const char *string);
/**
* Send a status text with loglevel EMERGENCY
*
* @param string the message to send (will be capped by mavlink max string length)
*/
- int send_statustext_emergency(const char *string);
+ void send_statustext_emergency(const char *string);
/**
- * Send a status text with loglevel
+ * Send a status text with loglevel, the difference from mavlink_log_xxx() is that message sent
+ * only on this mavlink connection. Useful for reporting communication specific, not system-wide info
+ * only to client interested in it. Message will be not sent immediately but queued in buffer as
+ * for mavlink_log_xxx().
*
* @param string the message to send (will be capped by mavlink max string length)
- * @param severity the log level, one of
+ * @param severity the log level
*/
- int send_statustext(unsigned severity, const char *string);
+ void send_statustext(unsigned char severity, const char *string);
+
MavlinkStream * get_streams() const { return _streams; }
float get_rate_mult();
@@ -252,6 +261,8 @@ public:
*/
struct telemetry_status_s& get_rx_status() { return _rstatus; }
+ struct mavlink_logbuffer *get_logbuffer() { return &_logbuffer; }
+
protected:
Mavlink *next;
@@ -274,9 +285,8 @@ private:
MavlinkStream *_streams;
MavlinkMissionManager *_mission_manager;
+ MavlinkParametersManager *_parameters_manager;
- orb_advert_t _mission_pub;
- int _mission_result_sub;
MAVLINK_MODE _mode;
mavlink_channel_t _channel;
@@ -292,7 +302,9 @@ private:
bool _ftp_on;
int _uart_fd;
int _baudrate;
- int _datarate;
+ int _datarate; ///< data rate for normal streams (attitude, position, etc.)
+ int _datarate_events; ///< data rate for params, waypoints, text messages
+ float _rate_mult;
/**
* If the queue index is not at 0, the queue sending
@@ -307,6 +319,8 @@ private:
float _subscribe_to_stream_rate;
bool _flow_control_enabled;
+ uint64_t _last_write_success_time;
+ uint64_t _last_write_try_time;
unsigned _bytes_tx;
unsigned _bytes_txerr;
@@ -328,6 +342,7 @@ private:
mavlink_message_buffer _message_buffer;
pthread_mutex_t _message_buffer_mutex;
+ pthread_mutex_t _send_mutex;
bool _param_initialized;
param_t _param_system_id;
@@ -338,52 +353,27 @@ private:
perf_counter_t _loop_perf; /**< loop performance counter */
perf_counter_t _txerr_perf; /**< TX error counter */
- /**
- * Send one parameter.
- *
- * @param param The parameter id to send.
- * @return zero on success, nonzero on failure.
- */
- int mavlink_pm_send_param(param_t param);
+ void mavlink_update_system();
- /**
- * Send one parameter identified by index.
- *
- * @param index The index of the parameter to send.
- * @return zero on success, nonzero else.
- */
- int mavlink_pm_send_param_for_index(uint16_t index);
+ int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb);
/**
- * Send one parameter identified by name.
+ * Get the free space in the transmit buffer
*
- * @param name The index of the parameter to send.
- * @return zero on success, nonzero else.
+ * @return free space in the UART TX buffer
*/
- int mavlink_pm_send_param_for_name(const char *name);
+ unsigned get_free_tx_buf();
- /**
- * Send a queue of parameters, one parameter per function call.
- *
- * @return zero on success, nonzero on failure
- */
- int mavlink_pm_queued_send(void);
+ static unsigned int interval_from_rate(float rate);
+
+ int configure_stream(const char *stream_name, const float rate);
/**
- * Start sending the parameter queue.
+ * Adjust the stream rates based on the current rate
*
- * This function will not directly send parameters, but instead
- * activate the sending of one parameter on each call of
- * mavlink_pm_queued_send().
- * @see mavlink_pm_queued_send()
+ * @param multiplier if greater than 1, the transmission rate will increase, if smaller than one decrease
*/
- void mavlink_pm_start_queued_send();
-
- void mavlink_update_system();
-
- int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb);
-
- int configure_stream(const char *stream_name, const float rate);
+ void adjust_stream_rates(const float multiplier);
int message_buffer_init(int size);
@@ -399,6 +389,11 @@ private:
void pass_message(const mavlink_message_t *msg);
+ /**
+ * Update rate mult so total bitrate will be equal to _datarate.
+ */
+ void update_rate_mult();
+
static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg);
/**
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 6885bebde..683f0f1e8 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -40,9 +40,9 @@
*/
#include <stdio.h>
+
#include <commander/px4_custom_mode.h>
#include <lib/geo/geo.h>
-
#include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
@@ -72,11 +72,11 @@
#include <drivers/drv_rc_input.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_range_finder.h>
-
#include <systemlib/err.h>
+#include <mavlink/mavlink_log.h>
#include "mavlink_messages.h"
-
+#include "mavlink_main.h"
static uint16_t cm_uint16_from_m_float(float m);
static void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet,
@@ -249,61 +249,184 @@ public:
return MAVLINK_MSG_ID_HEARTBEAT;
}
- static MavlinkStream *new_instance()
+ static MavlinkStream *new_instance(Mavlink *mavlink)
{
- return new MavlinkStreamHeartbeat();
+ return new MavlinkStreamHeartbeat(mavlink);
+ }
+
+ unsigned get_size()
+ {
+ return MAVLINK_MSG_ID_HEARTBEAT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+ }
+
+ bool const_rate() {
+ return true;
}
private:
- MavlinkOrbSubscription *status_sub;
- MavlinkOrbSubscription *pos_sp_triplet_sub;
+ MavlinkOrbSubscription *_status_sub;
+ MavlinkOrbSubscription *_pos_sp_triplet_sub;
/* do not allow top copying this class */
MavlinkStreamHeartbeat(MavlinkStreamHeartbeat &);
MavlinkStreamHeartbeat& operator = (const MavlinkStreamHeartbeat &);
protected:
- explicit MavlinkStreamHeartbeat() : MavlinkStream(),
- status_sub(nullptr),
- pos_sp_triplet_sub(nullptr)
+ explicit MavlinkStreamHeartbeat(Mavlink *mavlink) : MavlinkStream(mavlink),
+ _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))),
+ _pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)))
{}
- void subscribe(Mavlink *mavlink)
- {
- status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status));
- pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet));
- }
-
void send(const hrt_abstime t)
{
struct vehicle_status_s status;
struct position_setpoint_triplet_s pos_sp_triplet;
/* always send the heartbeat, independent of the update status of the topics */
- if (!status_sub->update(&status)) {
+ if (!_status_sub->update(&status)) {
/* if topic update failed fill it with defaults */
memset(&status, 0, sizeof(status));
}
- if (!pos_sp_triplet_sub->update(&pos_sp_triplet)) {
+ if (!_pos_sp_triplet_sub->update(&pos_sp_triplet)) {
/* if topic update failed fill it with defaults */
memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet));
}
- uint8_t mavlink_state = 0;
- uint8_t mavlink_base_mode = 0;
- uint32_t mavlink_custom_mode = 0;
- get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
+ mavlink_heartbeat_t msg;
+
+ msg.base_mode = 0;
+ msg.custom_mode = 0;
+ get_mavlink_mode_state(&status, &pos_sp_triplet, &msg.system_status, &msg.base_mode, &msg.custom_mode);
+ msg.type = mavlink_system.type;
+ msg.autopilot = MAV_AUTOPILOT_PX4;
+ msg.mavlink_version = 3;
+
+ _mavlink->send_message(MAVLINK_MSG_ID_HEARTBEAT, &msg);
+ }
+};
+
+class MavlinkStreamStatustext : public MavlinkStream
+{
+public:
+ const char *get_name() const
+ {
+ return MavlinkStreamStatustext::get_name_static();
+ }
+
+ static const char *get_name_static()
+ {
+ return "STATUSTEXT";
+ }
+
+ uint8_t get_id()
+ {
+ return MAVLINK_MSG_ID_STATUSTEXT;
+ }
+
+ static MavlinkStream *new_instance(Mavlink *mavlink)
+ {
+ return new MavlinkStreamStatustext(mavlink);
+ }
+
+ unsigned get_size() {
+ return mavlink_logbuffer_is_empty(_mavlink->get_logbuffer()) ? 0 : (MAVLINK_MSG_ID_STATUSTEXT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES);
+ }
+
+private:
+ /* do not allow top copying this class */
+ MavlinkStreamStatustext(MavlinkStreamStatustext &);
+ MavlinkStreamStatustext& operator = (const MavlinkStreamStatustext &);
- mavlink_msg_heartbeat_send(_channel,
- mavlink_system.type,
- MAV_AUTOPILOT_PX4,
- mavlink_base_mode,
- mavlink_custom_mode,
- mavlink_state);
+protected:
+ explicit MavlinkStreamStatustext(Mavlink *mavlink) : MavlinkStream(mavlink)
+ {}
+
+ void send(const hrt_abstime t)
+ {
+ if (!mavlink_logbuffer_is_empty(_mavlink->get_logbuffer())) {
+ struct mavlink_logmessage logmsg;
+ int lb_ret = mavlink_logbuffer_read(_mavlink->get_logbuffer(), &logmsg);
+
+ if (lb_ret == OK) {
+ mavlink_statustext_t msg;
+
+ msg.severity = logmsg.severity;
+ strncpy(msg.text, logmsg.text, sizeof(msg.text));
+
+ _mavlink->send_message(MAVLINK_MSG_ID_STATUSTEXT, &msg);
+ }
+ }
}
};
+class MavlinkStreamCommandLong : public MavlinkStream
+{
+public:
+ const char *get_name() const
+ {
+ return MavlinkStreamCommandLong::get_name_static();
+ }
+
+ static const char *get_name_static()
+ {
+ return "COMMAND_LONG";
+ }
+
+ uint8_t get_id()
+ {
+ return MAVLINK_MSG_ID_COMMAND_LONG;
+ }
+
+ static MavlinkStream *new_instance(Mavlink *mavlink)
+ {
+ return new MavlinkStreamCommandLong(mavlink);
+ }
+
+ unsigned get_size() {
+ return 0; // commands stream is not regular and not predictable
+ }
+
+private:
+ MavlinkOrbSubscription *_cmd_sub;
+ uint64_t _cmd_time;
+
+ /* do not allow top copying this class */
+ MavlinkStreamCommandLong(MavlinkStreamCommandLong &);
+ MavlinkStreamCommandLong& operator = (const MavlinkStreamCommandLong &);
+
+protected:
+ explicit MavlinkStreamCommandLong(Mavlink *mavlink) : MavlinkStream(mavlink),
+ _cmd_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_command))),
+ _cmd_time(0)
+ {}
+
+ void send(const hrt_abstime t)
+ {
+ struct vehicle_command_s cmd;
+
+ if (_cmd_sub->update(&_cmd_time, &cmd)) {
+ /* only send commands for other systems/components */
+ if (cmd.target_system != mavlink_system.sysid || cmd.target_component != mavlink_system.compid) {
+ mavlink_command_long_t msg;
+
+ msg.target_system = cmd.target_system;
+ msg.target_component = cmd.target_component;
+ msg.command = cmd.command;
+ msg.confirmation = cmd.confirmation;
+ msg.param1 = cmd.param1;
+ msg.param2 = cmd.param2;
+ msg.param3 = cmd.param3;
+ msg.param4 = cmd.param4;
+ msg.param5 = cmd.param5;
+ msg.param6 = cmd.param6;
+ msg.param7 = cmd.param7;
+
+ _mavlink->send_message(MAVLINK_MSG_ID_COMMAND_LONG, &msg);
+ }
+ }
+ }
+};
class MavlinkStreamSysStatus : public MavlinkStream
{
@@ -313,7 +436,7 @@ public:
return MavlinkStreamSysStatus::get_name_static();
}
- static const char *get_name_static ()
+ static const char *get_name_static()
{
return "SYS_STATUS";
}
@@ -323,47 +446,50 @@ public:
return MAVLINK_MSG_ID_SYS_STATUS;
}
- static MavlinkStream *new_instance()
+ static MavlinkStream *new_instance(Mavlink *mavlink)
{
- return new MavlinkStreamSysStatus();
+ return new MavlinkStreamSysStatus(mavlink);
+ }
+
+ unsigned get_size()
+ {
+ return MAVLINK_MSG_ID_SYS_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
}
private:
- MavlinkOrbSubscription *status_sub;
+ MavlinkOrbSubscription *_status_sub;
/* do not allow top copying this class */
MavlinkStreamSysStatus(MavlinkStreamSysStatus &);
MavlinkStreamSysStatus& operator = (const MavlinkStreamSysStatus &);
protected:
- explicit MavlinkStreamSysStatus() : MavlinkStream(),
- status_sub(nullptr)
+ explicit MavlinkStreamSysStatus(Mavlink *mavlink) : MavlinkStream(mavlink),
+ _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status)))
{}
- void subscribe(Mavlink *mavlink)
- {
- status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status));
- }
-
void send(const hrt_abstime t)
{
struct vehicle_status_s status;
- if (status_sub->update(&status)) {
- mavlink_msg_sys_status_send(_channel,
- status.onboard_control_sensors_present,
- status.onboard_control_sensors_enabled,
- status.onboard_control_sensors_health,
- status.load * 1000.0f,
- status.battery_voltage * 1000.0f,
- status.battery_current * 100.0f,
- status.battery_remaining * 100.0f,
- status.drop_rate_comm,
- status.errors_comm,
- status.errors_count1,
- status.errors_count2,
- status.errors_count3,
- status.errors_count4);
+ if (_status_sub->update(&status)) {
+ mavlink_sys_status_t msg;
+
+ msg.onboard_control_sensors_present = status.onboard_control_sensors_present;
+ msg.onboard_control_sensors_enabled = status.onboard_control_sensors_enabled;
+ msg.onboard_control_sensors_health = status.onboard_control_sensors_health;
+ msg.load = status.load * 1000.0f;
+ msg.voltage_battery = status.battery_voltage * 1000.0f;
+ msg.current_battery = status.battery_current * 100.0f;
+ msg.drop_rate_comm = status.drop_rate_comm;
+ msg.errors_comm = status.errors_comm;
+ msg.errors_count1 = status.errors_count1;
+ msg.errors_count2 = status.errors_count2;
+ msg.errors_count3 = status.errors_count3;
+ msg.errors_count4 = status.errors_count4;
+ msg.battery_remaining = status.battery_remaining * 100.0f;
+
+ _mavlink->send_message(MAVLINK_MSG_ID_SYS_STATUS, &msg);
}
}
};
@@ -387,78 +513,89 @@ public:
return MAVLINK_MSG_ID_HIGHRES_IMU;
}
- static MavlinkStream *new_instance()
+ static MavlinkStream *new_instance(Mavlink *mavlink)
{
- return new MavlinkStreamHighresIMU();
+ return new MavlinkStreamHighresIMU(mavlink);
+ }
+
+ unsigned get_size()
+ {
+ return MAVLINK_MSG_ID_HIGHRES_IMU_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
}
private:
- MavlinkOrbSubscription *sensor_sub;
- uint64_t sensor_time;
+ MavlinkOrbSubscription *_sensor_sub;
+ uint64_t _sensor_time;
- uint64_t accel_timestamp;
- uint64_t gyro_timestamp;
- uint64_t mag_timestamp;
- uint64_t baro_timestamp;
+ uint64_t _accel_timestamp;
+ uint64_t _gyro_timestamp;
+ uint64_t _mag_timestamp;
+ uint64_t _baro_timestamp;
/* do not allow top copying this class */
MavlinkStreamHighresIMU(MavlinkStreamHighresIMU &);
MavlinkStreamHighresIMU& operator = (const MavlinkStreamHighresIMU &);
protected:
- explicit MavlinkStreamHighresIMU() : MavlinkStream(),
- sensor_sub(nullptr),
- sensor_time(0),
- accel_timestamp(0),
- gyro_timestamp(0),
- mag_timestamp(0),
- baro_timestamp(0)
+ explicit MavlinkStreamHighresIMU(Mavlink *mavlink) : MavlinkStream(mavlink),
+ _sensor_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined))),
+ _sensor_time(0),
+ _accel_timestamp(0),
+ _gyro_timestamp(0),
+ _mag_timestamp(0),
+ _baro_timestamp(0)
{}
- void subscribe(Mavlink *mavlink)
- {
- sensor_sub = mavlink->add_orb_subscription(ORB_ID(sensor_combined));
- }
-
void send(const hrt_abstime t)
{
struct sensor_combined_s sensor;
- if (sensor_sub->update(&sensor_time, &sensor)) {
+ if (_sensor_sub->update(&_sensor_time, &sensor)) {
uint16_t fields_updated = 0;
- if (accel_timestamp != sensor.accelerometer_timestamp) {
+ if (_accel_timestamp != sensor.accelerometer_timestamp) {
/* mark first three dimensions as changed */
fields_updated |= (1 << 0) | (1 << 1) | (1 << 2);
- accel_timestamp = sensor.accelerometer_timestamp;
+ _accel_timestamp = sensor.accelerometer_timestamp;
}
- if (gyro_timestamp != sensor.timestamp) {
+ if (_gyro_timestamp != sensor.timestamp) {
/* mark second group dimensions as changed */
fields_updated |= (1 << 3) | (1 << 4) | (1 << 5);
- gyro_timestamp = sensor.timestamp;
+ _gyro_timestamp = sensor.timestamp;
}
- if (mag_timestamp != sensor.magnetometer_timestamp) {
+ if (_mag_timestamp != sensor.magnetometer_timestamp) {
/* mark third group dimensions as changed */
fields_updated |= (1 << 6) | (1 << 7) | (1 << 8);
- mag_timestamp = sensor.magnetometer_timestamp;
+ _mag_timestamp = sensor.magnetometer_timestamp;
}
- if (baro_timestamp != sensor.baro_timestamp) {
+ if (_baro_timestamp != sensor.baro_timestamp) {
/* mark last group dimensions as changed */
fields_updated |= (1 << 9) | (1 << 11) | (1 << 12);
- baro_timestamp = sensor.baro_timestamp;
+ _baro_timestamp = sensor.baro_timestamp;
}
- mavlink_msg_highres_imu_send(_channel,
- sensor.timestamp,
- sensor.accelerometer_m_s2[0], sensor.accelerometer_m_s2[1], sensor.accelerometer_m_s2[2],
- sensor.gyro_rad_s[0], sensor.gyro_rad_s[1], sensor.gyro_rad_s[2],
- sensor.magnetometer_ga[0], sensor.magnetometer_ga[1], sensor.magnetometer_ga[2],
- sensor.baro_pres_mbar, sensor.differential_pressure_pa,
- sensor.baro_alt_meter, sensor.baro_temp_celcius,
- fields_updated);
+ mavlink_highres_imu_t msg;
+
+ msg.time_usec = sensor.timestamp;
+ msg.xacc = sensor.accelerometer_m_s2[0];
+ msg.yacc = sensor.accelerometer_m_s2[1];
+ msg.zacc = sensor.accelerometer_m_s2[2];
+ msg.xgyro = sensor.gyro_rad_s[0];
+ msg.ygyro = sensor.gyro_rad_s[1];
+ msg.zgyro = sensor.gyro_rad_s[2];
+ msg.xmag = sensor.magnetometer_ga[0];
+ msg.ymag = sensor.magnetometer_ga[1];
+ msg.zmag = sensor.magnetometer_ga[2];
+ msg.abs_pressure = sensor.baro_pres_mbar;
+ msg.diff_pressure = sensor.differential_pressure_pa;
+ msg.pressure_alt = sensor.baro_alt_meter;
+ msg.temperature = sensor.baro_temp_celcius;
+ msg.fields_updated = fields_updated;
+
+ _mavlink->send_message(MAVLINK_MSG_ID_HIGHRES_IMU, &msg);
}
}
};
@@ -482,14 +619,19 @@ public:
return MAVLINK_MSG_ID_ATTITUDE;
}
- static MavlinkStream *new_instance()
+ static MavlinkStream *new_instance(Mavlink *mavlink)
+ {
+ return new MavlinkStreamAttitude(mavlink);
+ }
+
+ unsigned get_size()
{
- return new MavlinkStreamAttitude();
+ return MAVLINK_MSG_ID_ATTITUDE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
}
private:
- MavlinkOrbSubscription *att_sub;
- uint64_t att_time;
+ MavlinkOrbSubscription *_att_sub;
+ uint64_t _att_time;
/* do not allow top copying this class */
MavlinkStreamAttitude(MavlinkStreamAttitude &);
@@ -497,25 +639,27 @@ private:
protected:
- explicit MavlinkStreamAttitude() : MavlinkStream(),
- att_sub(nullptr),
- att_time(0)
+ explicit MavlinkStreamAttitude(Mavlink *mavlink) : MavlinkStream(mavlink),
+ _att_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude))),
+ _att_time(0)
{}
- void subscribe(Mavlink *mavlink)
- {
- att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude));
- }
-
void send(const hrt_abstime t)
{
struct vehicle_attitude_s att;
- if (att_sub->update(&att_time, &att)) {
- mavlink_msg_attitude_send(_channel,
- att.timestamp / 1000,
- att.roll, att.pitch, att.yaw,
- att.rollspeed, att.pitchspeed, att.yawspeed);
+ if (_att_sub->update(&_att_time, &att)) {
+ mavlink_attitude_t msg;
+
+ msg.time_boot_ms = att.timestamp / 1000;
+ msg.roll = att.roll;
+ msg.pitch = att.pitch;
+ msg.yaw = att.yaw;
+ msg.rollspeed = att.rollspeed;
+ msg.pitchspeed = att.pitchspeed;
+ msg.yawspeed = att.yawspeed;
+
+ _mavlink->send_message(MAVLINK_MSG_ID_ATTITUDE, &msg);
}
}
};
@@ -539,44 +683,47 @@ public:
return MAVLINK_MSG_ID_ATTITUDE_QUATERNION;
}
- static MavlinkStream *new_instance()
+ static MavlinkStream *new_instance(Mavlink *mavlink)
+ {
+ return new MavlinkStreamAttitudeQuaternion(mavlink);
+ }
+
+ unsigned get_size()
{
- return new MavlinkStreamAttitudeQuaternion();
+ return MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
}
private:
- MavlinkOrbSubscription *att_sub;
- uint64_t att_time;
+ MavlinkOrbSubscription *_att_sub;
+ uint64_t _att_time;
/* do not allow top copying this class */
MavlinkStreamAttitudeQuaternion(MavlinkStreamAttitudeQuaternion &);
MavlinkStreamAttitudeQuaternion& operator = (const MavlinkStreamAttitudeQuaternion &);
protected:
- explicit MavlinkStreamAttitudeQuaternion() : MavlinkStream(),
- att_sub(nullptr),
- att_time(0)
+ explicit MavlinkStreamAttitudeQuaternion(Mavlink *mavlink) : MavlinkStream(mavlink),
+ _att_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude))),
+ _att_time(0)
{}
- void subscribe(Mavlink *mavlink)
- {
- att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude));
- }
-
void send(const hrt_abstime t)
{
struct vehicle_attitude_s att;
- if (att_sub->update(&att_time, &att)) {
- mavlink_msg_attitude_quaternion_send(_channel,
- att.timestamp / 1000,
- att.q[0],
- att.q[1],
- att.q[2],
- att.q[3],
- att.rollspeed,
- att.pitchspeed,
- att.yawspeed);
+ if (_att_sub->update(&_att_time, &att)) {
+ mavlink_attitude_quaternion_t msg;
+
+ msg.time_boot_ms = att.timestamp / 1000;
+ msg.q1 = att.q[0];
+ msg.q2 = att.q[1];
+ msg.q3 = att.q[2];
+ msg.q4 = att.q[3];
+ msg.rollspeed = att.rollspeed;
+ msg.pitchspeed = att.pitchspeed;
+ msg.yawspeed = att.yawspeed;
+
+ _mavlink->send_message(MAVLINK_MSG_ID_ATTITUDE_QUATERNION, &msg);
}
}
};
@@ -601,54 +748,50 @@ public:
return MAVLINK_MSG_ID_VFR_HUD;
}
- static MavlinkStream *new_instance()
+ static MavlinkStream *new_instance(Mavlink *mavlink)
+ {
+ return new MavlinkStreamVFRHUD(mavlink);
+ }
+
+ unsigned get_size()
{
- return new MavlinkStreamVFRHUD();
+ return MAVLINK_MSG_ID_VFR_HUD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
}
private:
- MavlinkOrbSubscription *att_sub;
- uint64_t att_time;
+ MavlinkOrbSubscription *_att_sub;
+ uint64_t _att_time;
- MavlinkOrbSubscription *pos_sub;
- uint64_t pos_time;
+ MavlinkOrbSubscription *_pos_sub;
+ uint64_t _pos_time;
- MavlinkOrbSubscription *armed_sub;
- uint64_t armed_time;
+ MavlinkOrbSubscription *_armed_sub;
+ uint64_t _armed_time;
- MavlinkOrbSubscription *act_sub;
- uint64_t act_time;
+ MavlinkOrbSubscription *_act_sub;
+ uint64_t _act_time;
- MavlinkOrbSubscription *airspeed_sub;
- uint64_t airspeed_time;
+ MavlinkOrbSubscription *_airspeed_sub;
+ uint64_t _airspeed_time;
/* do not allow top copying this class */
MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &);
MavlinkStreamVFRHUD& operator = (const MavlinkStreamVFRHUD &);
protected:
- explicit MavlinkStreamVFRHUD() : MavlinkStream(),
- att_sub(nullptr),
- att_time(0),
- pos_sub(nullptr),
- pos_time(0),
- armed_sub(nullptr),
- armed_time(0),
- act_sub(nullptr),
- act_time(0),
- airspeed_sub(nullptr),
- airspeed_time(0)
+ explicit MavlinkStreamVFRHUD(Mavlink *mavlink) : MavlinkStream(mavlink),
+ _att_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude))),
+ _att_time(0),
+ _pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position))),
+ _pos_time(0),
+ _armed_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_armed))),
+ _armed_time(0),
+ _act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_0))),
+ _act_time(0),
+ _airspeed_sub(_mavlink->add_orb_subscription(ORB_ID(airspeed))),
+ _airspeed_time(0)
{}
- void subscribe(Mavlink *mavlink)
- {
- att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude));
- pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position));
- armed_sub = mavlink->add_orb_subscription(ORB_ID(actuator_armed));
- act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_controls_0));
- airspeed_sub = mavlink->add_orb_subscription(ORB_ID(airspeed));
- }
-
void send(const hrt_abstime t)
{
struct vehicle_attitude_s att;
@@ -657,24 +800,23 @@ protected:
struct actuator_controls_s act;
struct airspeed_s airspeed;
- bool updated = att_sub->update(&att_time, &att);
- updated |= pos_sub->update(&pos_time, &pos);
- updated |= armed_sub->update(&armed_time, &armed);
- updated |= act_sub->update(&act_time, &act);
- updated |= airspeed_sub->update(&airspeed_time, &airspeed);
+ bool updated = _att_sub->update(&_att_time, &att);
+ updated |= _pos_sub->update(&_pos_time, &pos);
+ updated |= _armed_sub->update(&_armed_time, &armed);
+ updated |= _act_sub->update(&_act_time, &act);
+ updated |= _airspeed_sub->update(&_airspeed_time, &airspeed);
if (updated) {
- float groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e);
- uint16_t heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F;
- float throttle = armed.armed ? act.control[3] * 100.0f : 0.0f;
-
- mavlink_msg_vfr_hud_send(_channel,
- airspeed.true_airspeed_m_s,
- groundspeed,
- heading,
- throttle,
- pos.alt,
- -pos.vel_d);
+ mavlink_vfr_hud_t msg;
+
+ msg.airspeed = airspeed.true_airspeed_m_s;
+ msg.groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e);
+ msg.heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F;
+ msg.throttle = armed.armed ? act.control[3] * 100.0f : 0.0f;
+ msg.alt = pos.alt;
+ msg.climb = -pos.vel_d;
+
+ _mavlink->send_message(MAVLINK_MSG_ID_VFR_HUD, &msg);
}
}
};
@@ -698,46 +840,49 @@ public:
return MAVLINK_MSG_ID_GPS_RAW_INT;
}
- static MavlinkStream *new_instance()
+ static MavlinkStream *new_instance(Mavlink *mavlink)
+ {
+ return new MavlinkStreamGPSRawInt(mavlink);
+ }
+
+ unsigned get_size()
{
- return new MavlinkStreamGPSRawInt();
+ return MAVLINK_MSG_ID_GPS_RAW_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
}
private:
- MavlinkOrbSubscription *gps_sub;
- uint64_t gps_time;
+ MavlinkOrbSubscription *_gps_sub;
+ uint64_t _gps_time;
/* do not allow top copying this class */
MavlinkStreamGPSRawInt(MavlinkStreamGPSRawInt &);
MavlinkStreamGPSRawInt& operator = (const MavlinkStreamGPSRawInt &);
protected:
- explicit MavlinkStreamGPSRawInt() : MavlinkStream(),
- gps_sub(nullptr),
- gps_time(0)
+ explicit MavlinkStreamGPSRawInt(Mavlink *mavlink) : MavlinkStream(mavlink),
+ _gps_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position))),
+ _gps_time(0)
{}
- void subscribe(Mavlink *mavlink)
- {
- gps_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position));
- }
-
void send(const hrt_abstime t)
{
struct vehicle_gps_position_s gps;
- if (gps_sub->update(&gps_time, &gps)) {
- mavlink_msg_gps_raw_int_send(_channel,
- gps.timestamp_position,
- gps.fix_type,
- gps.lat,
- gps.lon,
- gps.alt,
- cm_uint16_from_m_float(gps.eph),
- cm_uint16_from_m_float(gps.epv),
- gps.vel_m_s * 100.0f,
- _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f,
- gps.satellites_used);
+ if (_gps_sub->update(&_gps_time, &gps)) {
+ mavlink_gps_raw_int_t msg;
+
+ msg.time_usec = gps.timestamp_position;
+ msg.fix_type = gps.fix_type;
+ msg.lat = gps.lat;
+ msg.lon = gps.lon;
+ msg.alt = gps.alt;
+ msg.eph = cm_uint16_from_m_float(gps.eph);
+ msg.epv = cm_uint16_from_m_float(gps.epv);
+ msg.vel = cm_uint16_from_m_float(gps.vel_m_s),
+ msg.cog = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f,
+ msg.satellites_visible = gps.satellites_used;
+
+ _mavlink->send_message(MAVLINK_MSG_ID_GPS_RAW_INT, &msg);
}
}
};
@@ -761,55 +906,57 @@ public:
return MAVLINK_MSG_ID_GLOBAL_POSITION_INT;
}
- static MavlinkStream *new_instance()
+ static MavlinkStream *new_instance(Mavlink *mavlink)
+ {
+ return new MavlinkStreamGlobalPositionInt(mavlink);
+ }
+
+ unsigned get_size()
{
- return new MavlinkStreamGlobalPositionInt();
+ return MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
}
private:
- MavlinkOrbSubscription *pos_sub;
- uint64_t pos_time;
+ MavlinkOrbSubscription *_pos_sub;
+ uint64_t _pos_time;
- MavlinkOrbSubscription *home_sub;
- uint64_t home_time;
+ MavlinkOrbSubscription *_home_sub;
+ uint64_t _home_time;
/* do not allow top copying this class */
MavlinkStreamGlobalPositionInt(MavlinkStreamGlobalPositionInt &);
MavlinkStreamGlobalPositionInt& operator = (const MavlinkStreamGlobalPositionInt &);
protected:
- explicit MavlinkStreamGlobalPositionInt() : MavlinkStream(),
- pos_sub(nullptr),
- pos_time(0),
- home_sub(nullptr),
- home_time(0)
+ explicit MavlinkStreamGlobalPositionInt(Mavlink *mavlink) : MavlinkStream(mavlink),
+ _pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position))),
+ _pos_time(0),
+ _home_sub(_mavlink->add_orb_subscription(ORB_ID(home_position))),
+ _home_time(0)
{}
- void subscribe(Mavlink *mavlink)
- {
- pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position));
- home_sub = mavlink->add_orb_subscription(ORB_ID(home_position));
- }
-
void send(const hrt_abstime t)
{
struct vehicle_global_position_s pos;
struct home_position_s home;
- bool updated = pos_sub->update(&pos_time, &pos);
- updated |= home_sub->update(&home_time, &home);
+ bool updated = _pos_sub->update(&_pos_time, &pos);
+ updated |= _home_sub->update(&_home_time, &home);
if (updated) {
- mavlink_msg_global_position_int_send(_channel,
- pos.timestamp / 1000,
- pos.lat * 1e7,
- pos.lon * 1e7,
- pos.alt * 1000.0f,
- (pos.alt - home.alt) * 1000.0f,
- pos.vel_n * 100.0f,
- pos.vel_e * 100.0f,
- pos.vel_d * 100.0f,
- _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f);
+ mavlink_global_position_int_t msg;
+
+ msg.time_boot_ms = pos.timestamp / 1000;
+ msg.lat = pos.lat * 1e7;
+ msg.lon = pos.lon * 1e7;
+ msg.alt = pos.alt * 1000.0f;
+ msg.relative_alt = (pos.alt - home.alt) * 1000.0f;
+ msg.vx = pos.vel_n * 100.0f;
+ msg.vy = pos.vel_e * 100.0f;
+ msg.vz = pos.vel_d * 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);
}
}
};
@@ -833,49 +980,51 @@ public:
return MAVLINK_MSG_ID_LOCAL_POSITION_NED;
}
- static MavlinkStream *new_instance()
+ static MavlinkStream *new_instance(Mavlink *mavlink)
+ {
+ return new MavlinkStreamLocalPositionNED(mavlink);
+ }
+
+ unsigned get_size()
{
- return new MavlinkStreamLocalPositionNED();
+ return MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
}
private:
- MavlinkOrbSubscription *pos_sub;
- uint64_t pos_time;
+ MavlinkOrbSubscription *_pos_sub;
+ uint64_t _pos_time;
/* do not allow top copying this class */
MavlinkStreamLocalPositionNED(MavlinkStreamLocalPositionNED &);
MavlinkStreamLocalPositionNED& operator = (const MavlinkStreamLocalPositionNED &);
protected:
- explicit MavlinkStreamLocalPositionNED() : MavlinkStream(),
- pos_sub(nullptr),
- pos_time(0)
+ explicit MavlinkStreamLocalPositionNED(Mavlink *mavlink) : MavlinkStream(mavlink),
+ _pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position))),
+ _pos_time(0)
{}
- void subscribe(Mavlink *mavlink)
- {
- pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position));
- }
-
void send(const hrt_abstime t)
{
struct vehicle_local_position_s pos;
- if (pos_sub->update(&pos_time, &pos)) {
- mavlink_msg_local_position_ned_send(_channel,
- pos.timestamp / 1000,
- pos.x,
- pos.y,
- pos.z,
- pos.vx,
- pos.vy,
- pos.vz);
+ if (_pos_sub->update(&_pos_time, &pos)) {
+ mavlink_local_position_ned_t msg;
+
+ msg.time_boot_ms = pos.timestamp / 1000;
+ msg.x = pos.x;
+ msg.y = pos.y;
+ msg.z = pos.z;
+ msg.vx = pos.vx;
+ msg.vy = pos.vy;
+ msg.vz = pos.vz;
+
+ _mavlink->send_message(MAVLINK_MSG_ID_LOCAL_POSITION_NED, &msg);
}
}
};
-
class MavlinkStreamViconPositionEstimate : public MavlinkStream
{
public:
@@ -894,43 +1043,46 @@ public:
return MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE;
}
- static MavlinkStream *new_instance()
+ static MavlinkStream *new_instance(Mavlink *mavlink)
+ {
+ return new MavlinkStreamViconPositionEstimate(mavlink);
+ }
+
+ unsigned get_size()
{
- return new MavlinkStreamViconPositionEstimate();
+ return MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
}
private:
- MavlinkOrbSubscription *pos_sub;
- uint64_t pos_time;
+ MavlinkOrbSubscription *_pos_sub;
+ uint64_t _pos_time;
/* do not allow top copying this class */
MavlinkStreamViconPositionEstimate(MavlinkStreamViconPositionEstimate &);
MavlinkStreamViconPositionEstimate& operator = (const MavlinkStreamViconPositionEstimate &);
protected:
- explicit MavlinkStreamViconPositionEstimate() : MavlinkStream(),
- pos_sub(nullptr),
- pos_time(0)
+ explicit MavlinkStreamViconPositionEstimate(Mavlink *mavlink) : MavlinkStream(mavlink),
+ _pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position))),
+ _pos_time(0)
{}
- void subscribe(Mavlink *mavlink)
- {
- pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position));
- }
-
void send(const hrt_abstime t)
{
struct vehicle_vicon_position_s pos;
- if (pos_sub->update(&pos_time, &pos)) {
- mavlink_msg_vicon_position_estimate_send(_channel,
- pos.timestamp / 1000,
- pos.x,
- pos.y,
- pos.z,
- pos.roll,
- pos.pitch,
- pos.yaw);
+ if (_pos_sub->update(&_pos_time, &pos)) {
+ mavlink_vicon_position_estimate_t msg;
+
+ msg.usec = pos.timestamp;
+ msg.x = pos.x;
+ msg.y = pos.y;
+ msg.z = pos.z;
+ msg.roll = pos.roll;
+ msg.pitch = pos.pitch;
+ msg.yaw = pos.yaw;
+
+ _mavlink->send_message(MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, &msg);
}
}
};
@@ -954,45 +1106,49 @@ public:
return MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN;
}
- static MavlinkStream *new_instance()
+ static MavlinkStream *new_instance(Mavlink *mavlink)
+ {
+ return new MavlinkStreamGPSGlobalOrigin(mavlink);
+ }
+
+ unsigned get_size()
{
- return new MavlinkStreamGPSGlobalOrigin();
+ return _home_sub->is_published() ? (MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
}
private:
- MavlinkOrbSubscription *home_sub;
+ MavlinkOrbSubscription *_home_sub;
/* do not allow top copying this class */
MavlinkStreamGPSGlobalOrigin(MavlinkStreamGPSGlobalOrigin &);
MavlinkStreamGPSGlobalOrigin& operator = (const MavlinkStreamGPSGlobalOrigin &);
protected:
- explicit MavlinkStreamGPSGlobalOrigin() : MavlinkStream(),
- home_sub(nullptr)
+ explicit MavlinkStreamGPSGlobalOrigin(Mavlink *mavlink) : MavlinkStream(mavlink),
+ _home_sub(_mavlink->add_orb_subscription(ORB_ID(home_position)))
{}
- void subscribe(Mavlink *mavlink)
- {
- home_sub = mavlink->add_orb_subscription(ORB_ID(home_position));
- }
-
void send(const hrt_abstime t)
{
/* we're sending the GPS home periodically to ensure the
* the GCS does pick it up at one point */
- if (home_sub->is_published()) {
+ if (_home_sub->is_published()) {
struct home_position_s home;
- if (home_sub->update(&home)) {
- mavlink_msg_gps_global_origin_send(_channel,
- (int32_t)(home.lat * 1e7),
- (int32_t)(home.lon * 1e7),
- (int32_t)(home.alt) * 1000.0f);
+ if (_home_sub->update(&home)) {
+ mavlink_gps_global_origin_t msg;
+
+ msg.latitude = home.lat * 1e7;
+ msg.longitude = home.lon * 1e7;
+ msg.altitude = home.alt * 1e3f;
+
+ _mavlink->send_message(MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, &msg);
}
}
}
};
+
template <int N>
class MavlinkStreamServoOutputRaw : public MavlinkStream
{
@@ -1024,26 +1180,28 @@ public:
}
}
- static MavlinkStream *new_instance()
+ static MavlinkStream *new_instance(Mavlink *mavlink)
+ {
+ return new MavlinkStreamServoOutputRaw<N>(mavlink);
+ }
+
+ unsigned get_size()
{
- return new MavlinkStreamServoOutputRaw<N>();
+ return MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
}
private:
- MavlinkOrbSubscription *act_sub;
- uint64_t act_time;
+ MavlinkOrbSubscription *_act_sub;
+ uint64_t _act_time;
/* do not allow top copying this class */
MavlinkStreamServoOutputRaw(MavlinkStreamServoOutputRaw &);
MavlinkStreamServoOutputRaw& operator = (const MavlinkStreamServoOutputRaw &);
protected:
- explicit MavlinkStreamServoOutputRaw() : MavlinkStream(),
- act_sub(nullptr),
- act_time(0)
- {}
-
- void subscribe(Mavlink *mavlink)
+ explicit MavlinkStreamServoOutputRaw(Mavlink *mavlink) : MavlinkStream(mavlink),
+ _act_sub(nullptr),
+ _act_time(0)
{
orb_id_t act_topics[] = {
ORB_ID(actuator_outputs_0),
@@ -1052,25 +1210,28 @@ protected:
ORB_ID(actuator_outputs_3)
};
- act_sub = mavlink->add_orb_subscription(act_topics[N]);
+ _act_sub = _mavlink->add_orb_subscription(act_topics[N]);
}
void send(const hrt_abstime t)
{
struct actuator_outputs_s act;
- if (act_sub->update(&act_time, &act)) {
- mavlink_msg_servo_output_raw_send(_channel,
- act.timestamp / 1000,
- N,
- act.output[0],
- act.output[1],
- act.output[2],
- act.output[3],
- act.output[4],
- act.output[5],
- act.output[6],
- act.output[7]);
+ if (_act_sub->update(&_act_time, &act)) {
+ mavlink_servo_output_raw_t msg;
+
+ msg.time_usec = act.timestamp;
+ msg.port = N;
+ msg.servo1_raw = act.output[0];
+ msg.servo2_raw = act.output[1];
+ msg.servo3_raw = act.output[2];
+ msg.servo4_raw = act.output[3];
+ msg.servo5_raw = act.output[4];
+ msg.servo6_raw = act.output[5];
+ msg.servo7_raw = act.output[6];
+ msg.servo8_raw = act.output[7];
+
+ _mavlink->send_message(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, &msg);
}
}
};
@@ -1094,51 +1255,49 @@ public:
return MAVLINK_MSG_ID_HIL_CONTROLS;
}
- static MavlinkStream *new_instance()
+ static MavlinkStream *new_instance(Mavlink *mavlink)
+ {
+ return new MavlinkStreamHILControls(mavlink);
+ }
+
+ unsigned get_size()
{
- return new MavlinkStreamHILControls();
+ return MAVLINK_MSG_ID_HIL_CONTROLS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
}
private:
- MavlinkOrbSubscription *status_sub;
- uint64_t status_time;
+ MavlinkOrbSubscription *_status_sub;
+ uint64_t _status_time;
- MavlinkOrbSubscription *pos_sp_triplet_sub;
- uint64_t pos_sp_triplet_time;
+ MavlinkOrbSubscription *_pos_sp_triplet_sub;
+ uint64_t _pos_sp_triplet_time;
- MavlinkOrbSubscription *act_sub;
- uint64_t act_time;
+ MavlinkOrbSubscription *_act_sub;
+ uint64_t _act_time;
/* do not allow top copying this class */
MavlinkStreamHILControls(MavlinkStreamHILControls &);
MavlinkStreamHILControls& operator = (const MavlinkStreamHILControls &);
protected:
- explicit MavlinkStreamHILControls() : MavlinkStream(),
- status_sub(nullptr),
- status_time(0),
- pos_sp_triplet_sub(nullptr),
- pos_sp_triplet_time(0),
- act_sub(nullptr),
- act_time(0)
+ explicit MavlinkStreamHILControls(Mavlink *mavlink) : MavlinkStream(mavlink),
+ _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))),
+ _status_time(0),
+ _pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet))),
+ _pos_sp_triplet_time(0),
+ _act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0))),
+ _act_time(0)
{}
- void subscribe(Mavlink *mavlink)
- {
- status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status));
- pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet));
- act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0));
- }
-
void send(const hrt_abstime t)
{
struct vehicle_status_s status;
struct position_setpoint_triplet_s pos_sp_triplet;
struct actuator_outputs_s act;
- bool updated = act_sub->update(&act_time, &act);
- updated |= pos_sp_triplet_sub->update(&pos_sp_triplet_time, &pos_sp_triplet);
- updated |= status_sub->update(&status_time, &status);
+ bool updated = _act_sub->update(&_act_time, &act);
+ updated |= _pos_sp_triplet_sub->update(&_pos_sp_triplet_time, &pos_sp_triplet);
+ updated |= _status_sub->update(&_status_time, &status);
if (updated && (status.arming_state == ARMING_STATE_ARMED)) {
/* translate the current syste state to mavlink state and mode */
@@ -1206,11 +1365,21 @@ protected:
}
}
- mavlink_msg_hil_controls_send(_channel,
- hrt_absolute_time(),
- out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7],
- mavlink_base_mode,
- 0);
+ mavlink_hil_controls_t msg;
+
+ msg.time_usec = hrt_absolute_time();
+ msg.roll_ailerons = out[0];
+ msg.pitch_elevator = out[1];
+ msg.yaw_rudder = out[2];
+ msg.throttle = out[3];
+ msg.aux1 = out[4];
+ msg.aux2 = out[5];
+ msg.aux3 = out[6];
+ msg.aux4 = out[7];
+ msg.mode = mavlink_base_mode;
+ msg.nav_mode = 0;
+
+ _mavlink->send_message(MAVLINK_MSG_ID_HIL_CONTROLS, &msg);
}
}
};
@@ -1234,39 +1403,42 @@ public:
return MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT;
}
- static MavlinkStream *new_instance()
+ static MavlinkStream *new_instance(Mavlink *mavlink)
{
- return new MavlinkStreamGlobalPositionSetpointInt();
+ return new MavlinkStreamGlobalPositionSetpointInt(mavlink);
+ }
+
+ unsigned get_size()
+ {
+ return MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
}
private:
- MavlinkOrbSubscription *pos_sp_triplet_sub;
+ MavlinkOrbSubscription *_pos_sp_triplet_sub;
/* do not allow top copying this class */
MavlinkStreamGlobalPositionSetpointInt(MavlinkStreamGlobalPositionSetpointInt &);
MavlinkStreamGlobalPositionSetpointInt& operator = (const MavlinkStreamGlobalPositionSetpointInt &);
protected:
- explicit MavlinkStreamGlobalPositionSetpointInt() : MavlinkStream(),
- pos_sp_triplet_sub(nullptr)
+ explicit MavlinkStreamGlobalPositionSetpointInt(Mavlink *mavlink) : MavlinkStream(mavlink),
+ _pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)))
{}
- void subscribe(Mavlink *mavlink)
- {
- pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet));
- }
-
void send(const hrt_abstime t)
{
struct position_setpoint_triplet_s pos_sp_triplet;
- if (pos_sp_triplet_sub->update(&pos_sp_triplet)) {
- mavlink_msg_global_position_setpoint_int_send(_channel,
- MAV_FRAME_GLOBAL,
- (int32_t)(pos_sp_triplet.current.lat * 1e7),
- (int32_t)(pos_sp_triplet.current.lon * 1e7),
- (int32_t)(pos_sp_triplet.current.alt * 1000),
- (int16_t)(pos_sp_triplet.current.yaw * M_RAD_TO_DEG_F * 100.0f));
+ if (_pos_sp_triplet_sub->update(&pos_sp_triplet)) {
+ mavlink_global_position_setpoint_int_t msg;
+
+ msg.coordinate_frame = MAV_FRAME_GLOBAL;
+ msg.latitude = pos_sp_triplet.current.lat * 1e7;
+ msg.longitude = pos_sp_triplet.current.lon * 1e7;
+ msg.altitude = pos_sp_triplet.current.alt * 1000;
+ msg.yaw = pos_sp_triplet.current.yaw * M_RAD_TO_DEG_F * 100.0f;
+
+ _mavlink->send_message(MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT, &msg);
}
}
};
@@ -1290,41 +1462,44 @@ public:
return MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT;
}
- static MavlinkStream *new_instance()
+ static MavlinkStream *new_instance(Mavlink *mavlink)
{
- return new MavlinkStreamLocalPositionSetpoint();
+ return new MavlinkStreamLocalPositionSetpoint(mavlink);
+ }
+
+ unsigned get_size()
+ {
+ return MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
}
private:
- MavlinkOrbSubscription *pos_sp_sub;
- uint64_t pos_sp_time;
+ MavlinkOrbSubscription *_pos_sp_sub;
+ uint64_t _pos_sp_time;
/* do not allow top copying this class */
MavlinkStreamLocalPositionSetpoint(MavlinkStreamLocalPositionSetpoint &);
MavlinkStreamLocalPositionSetpoint& operator = (const MavlinkStreamLocalPositionSetpoint &);
protected:
- explicit MavlinkStreamLocalPositionSetpoint() : MavlinkStream(),
- pos_sp_sub(nullptr),
- pos_sp_time(0)
+ explicit MavlinkStreamLocalPositionSetpoint(Mavlink *mavlink) : MavlinkStream(mavlink),
+ _pos_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint))),
+ _pos_sp_time(0)
{}
- void subscribe(Mavlink *mavlink)
- {
- pos_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint));
- }
-
void send(const hrt_abstime t)
{
struct vehicle_local_position_setpoint_s pos_sp;
- if (pos_sp_sub->update(&pos_sp_time, &pos_sp)) {
- mavlink_msg_local_position_setpoint_send(_channel,
- MAV_FRAME_LOCAL_NED,
- pos_sp.x,
- pos_sp.y,
- pos_sp.z,
- pos_sp.yaw);
+ if (_pos_sp_sub->update(&_pos_sp_time, &pos_sp)) {
+ mavlink_local_position_setpoint_t msg;
+
+ msg.coordinate_frame = MAV_FRAME_LOCAL_NED;
+ msg.x = pos_sp.x;
+ msg.y = pos_sp.y;
+ msg.z = pos_sp.z;
+ msg.yaw = pos_sp.yaw;
+
+ _mavlink->send_message(MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT, &msg);
}
}
};
@@ -1348,9 +1523,14 @@ public:
return MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT;
}
- static MavlinkStream *new_instance()
+ static MavlinkStream *new_instance(Mavlink *mavlink)
{
- return new MavlinkStreamRollPitchYawThrustSetpoint();
+ return new MavlinkStreamRollPitchYawThrustSetpoint(mavlink);
+ }
+
+ unsigned get_size()
+ {
+ return MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
}
private:
@@ -1362,27 +1542,25 @@ private:
MavlinkStreamRollPitchYawThrustSetpoint& operator = (const MavlinkStreamRollPitchYawThrustSetpoint &);
protected:
- explicit MavlinkStreamRollPitchYawThrustSetpoint() : MavlinkStream(),
- att_sp_sub(nullptr),
+ explicit MavlinkStreamRollPitchYawThrustSetpoint(Mavlink *mavlink) : MavlinkStream(mavlink),
+ att_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint))),
att_sp_time(0)
{}
- void subscribe(Mavlink *mavlink)
- {
- att_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint));
- }
-
void send(const hrt_abstime t)
{
struct vehicle_attitude_setpoint_s att_sp;
if (att_sp_sub->update(&att_sp_time, &att_sp)) {
- mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel,
- att_sp.timestamp / 1000,
- att_sp.roll_body,
- att_sp.pitch_body,
- att_sp.yaw_body,
- att_sp.thrust);
+ mavlink_roll_pitch_yaw_thrust_setpoint_t msg;
+
+ msg.time_boot_ms = att_sp.timestamp / 1000;
+ msg.roll = att_sp.roll_body;
+ msg.pitch = att_sp.pitch_body;
+ msg.yaw = att_sp.yaw_body;
+ msg.thrust = att_sp.thrust;
+
+ _mavlink->send_message(MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT, &msg);
}
}
};
@@ -1406,41 +1584,44 @@ public:
return MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT;
}
- static MavlinkStream *new_instance()
+ static MavlinkStream *new_instance(Mavlink *mavlink)
{
- return new MavlinkStreamRollPitchYawRatesThrustSetpoint();
+ return new MavlinkStreamRollPitchYawRatesThrustSetpoint(mavlink);
+ }
+
+ unsigned get_size()
+ {
+ return MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
}
private:
- MavlinkOrbSubscription *att_rates_sp_sub;
- uint64_t att_rates_sp_time;
+ MavlinkOrbSubscription *_att_rates_sp_sub;
+ uint64_t _att_rates_sp_time;
/* do not allow top copying this class */
MavlinkStreamRollPitchYawRatesThrustSetpoint(MavlinkStreamRollPitchYawRatesThrustSetpoint &);
MavlinkStreamRollPitchYawRatesThrustSetpoint& operator = (const MavlinkStreamRollPitchYawRatesThrustSetpoint &);
protected:
- explicit MavlinkStreamRollPitchYawRatesThrustSetpoint() : MavlinkStream(),
- att_rates_sp_sub(nullptr),
- att_rates_sp_time(0)
+ explicit MavlinkStreamRollPitchYawRatesThrustSetpoint(Mavlink *mavlink) : MavlinkStream(mavlink),
+ _att_rates_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint))),
+ _att_rates_sp_time(0)
{}
- void subscribe(Mavlink *mavlink)
- {
- att_rates_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint));
- }
-
void send(const hrt_abstime t)
{
struct vehicle_rates_setpoint_s att_rates_sp;
- if (att_rates_sp_sub->update(&att_rates_sp_time, &att_rates_sp)) {
- mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel,
- att_rates_sp.timestamp / 1000,
- att_rates_sp.roll,
- att_rates_sp.pitch,
- att_rates_sp.yaw,
- att_rates_sp.thrust);
+ if (_att_rates_sp_sub->update(&_att_rates_sp_time, &att_rates_sp)) {
+ mavlink_roll_pitch_yaw_rates_thrust_setpoint_t msg;
+
+ msg.time_boot_ms = att_rates_sp.timestamp / 1000;
+ msg.roll_rate = att_rates_sp.roll;
+ msg.pitch_rate = att_rates_sp.pitch;
+ msg.yaw_rate = att_rates_sp.yaw;
+ msg.thrust = att_rates_sp.thrust;
+
+ _mavlink->send_message(MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, &msg);
}
}
};
@@ -1464,77 +1645,84 @@ public:
return MAVLINK_MSG_ID_RC_CHANNELS_RAW;
}
- static MavlinkStream *new_instance()
+ static MavlinkStream *new_instance(Mavlink *mavlink)
{
- return new MavlinkStreamRCChannelsRaw();
+ return new MavlinkStreamRCChannelsRaw(mavlink);
+ }
+
+ unsigned get_size()
+ {
+ return _rc_sub->is_published() ? ((RC_INPUT_MAX_CHANNELS / 8) * (MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) +
+ MAVLINK_MSG_ID_RC_CHANNELS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
}
private:
- MavlinkOrbSubscription *rc_sub;
- uint64_t rc_time;
+ MavlinkOrbSubscription *_rc_sub;
+ uint64_t _rc_time;
/* do not allow top copying this class */
MavlinkStreamRCChannelsRaw(MavlinkStreamRCChannelsRaw &);
MavlinkStreamRCChannelsRaw& operator = (const MavlinkStreamRCChannelsRaw &);
protected:
- explicit MavlinkStreamRCChannelsRaw() : MavlinkStream(),
- rc_sub(nullptr),
- rc_time(0)
+ explicit MavlinkStreamRCChannelsRaw(Mavlink *mavlink) : MavlinkStream(mavlink),
+ _rc_sub(_mavlink->add_orb_subscription(ORB_ID(input_rc))),
+ _rc_time(0)
{}
- void subscribe(Mavlink *mavlink)
- {
- rc_sub = mavlink->add_orb_subscription(ORB_ID(input_rc));
- }
-
void send(const hrt_abstime t)
{
struct rc_input_values rc;
- if (rc_sub->update(&rc_time, &rc)) {
+ if (_rc_sub->update(&_rc_time, &rc)) {
const unsigned port_width = 8;
- // Deprecated message (but still needed for compatibility!)
+ /* deprecated message (but still needed for compatibility!) */
for (unsigned i = 0; (i * port_width) < rc.channel_count; i++) {
- /* Channels are sent in MAVLink main loop at a fixed interval */
- mavlink_msg_rc_channels_raw_send(_channel,
- rc.timestamp_publication / 1000,
- i,
- (rc.channel_count > (i * port_width) + 0) ? rc.values[(i * port_width) + 0] : UINT16_MAX,
- (rc.channel_count > (i * port_width) + 1) ? rc.values[(i * port_width) + 1] : UINT16_MAX,
- (rc.channel_count > (i * port_width) + 2) ? rc.values[(i * port_width) + 2] : UINT16_MAX,
- (rc.channel_count > (i * port_width) + 3) ? rc.values[(i * port_width) + 3] : UINT16_MAX,
- (rc.channel_count > (i * port_width) + 4) ? rc.values[(i * port_width) + 4] : UINT16_MAX,
- (rc.channel_count > (i * port_width) + 5) ? rc.values[(i * port_width) + 5] : UINT16_MAX,
- (rc.channel_count > (i * port_width) + 6) ? rc.values[(i * port_width) + 6] : UINT16_MAX,
- (rc.channel_count > (i * port_width) + 7) ? rc.values[(i * port_width) + 7] : UINT16_MAX,
- rc.rssi);
+ /* channels are sent in MAVLink main loop at a fixed interval */
+ mavlink_rc_channels_raw_t msg;
+
+ msg.time_boot_ms = rc.timestamp_publication / 1000;
+ msg.port = i;
+ msg.chan1_raw = (rc.channel_count > (i * port_width) + 0) ? rc.values[(i * port_width) + 0] : UINT16_MAX;
+ msg.chan2_raw = (rc.channel_count > (i * port_width) + 1) ? rc.values[(i * port_width) + 1] : UINT16_MAX;
+ msg.chan3_raw = (rc.channel_count > (i * port_width) + 2) ? rc.values[(i * port_width) + 2] : UINT16_MAX;
+ msg.chan4_raw = (rc.channel_count > (i * port_width) + 3) ? rc.values[(i * port_width) + 3] : UINT16_MAX;
+ msg.chan4_raw = (rc.channel_count > (i * port_width) + 4) ? rc.values[(i * port_width) + 4] : UINT16_MAX;
+ msg.chan4_raw = (rc.channel_count > (i * port_width) + 5) ? rc.values[(i * port_width) + 5] : UINT16_MAX;
+ msg.chan4_raw = (rc.channel_count > (i * port_width) + 6) ? rc.values[(i * port_width) + 6] : UINT16_MAX;
+ msg.chan4_raw = (rc.channel_count > (i * port_width) + 7) ? rc.values[(i * port_width) + 7] : UINT16_MAX;
+ msg.rssi = rc.rssi;
+
+ _mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS_RAW, &msg);
}
- // New message
- mavlink_msg_rc_channels_send(_channel,
- rc.timestamp_publication / 1000,
- rc.channel_count,
- ((rc.channel_count > 0) ? rc.values[0] : UINT16_MAX),
- ((rc.channel_count > 1) ? rc.values[1] : UINT16_MAX),
- ((rc.channel_count > 2) ? rc.values[2] : UINT16_MAX),
- ((rc.channel_count > 3) ? rc.values[3] : UINT16_MAX),
- ((rc.channel_count > 4) ? rc.values[4] : UINT16_MAX),
- ((rc.channel_count > 5) ? rc.values[5] : UINT16_MAX),
- ((rc.channel_count > 6) ? rc.values[6] : UINT16_MAX),
- ((rc.channel_count > 7) ? rc.values[7] : UINT16_MAX),
- ((rc.channel_count > 8) ? rc.values[8] : UINT16_MAX),
- ((rc.channel_count > 9) ? rc.values[9] : UINT16_MAX),
- ((rc.channel_count > 10) ? rc.values[10] : UINT16_MAX),
- ((rc.channel_count > 11) ? rc.values[11] : UINT16_MAX),
- ((rc.channel_count > 12) ? rc.values[12] : UINT16_MAX),
- ((rc.channel_count > 13) ? rc.values[13] : UINT16_MAX),
- ((rc.channel_count > 14) ? rc.values[14] : UINT16_MAX),
- ((rc.channel_count > 15) ? rc.values[15] : UINT16_MAX),
- ((rc.channel_count > 16) ? rc.values[16] : UINT16_MAX),
- ((rc.channel_count > 17) ? rc.values[17] : UINT16_MAX),
- rc.rssi);
+ /* new message */
+ mavlink_rc_channels_t msg;
+
+ msg.time_boot_ms = rc.timestamp_publication / 1000;
+ msg.chancount = rc.channel_count;
+ msg.chan1_raw = (rc.channel_count > 0) ? rc.values[0] : UINT16_MAX;
+ msg.chan2_raw = (rc.channel_count > 1) ? rc.values[1] : UINT16_MAX;
+ msg.chan3_raw = (rc.channel_count > 2) ? rc.values[2] : UINT16_MAX;
+ msg.chan4_raw = (rc.channel_count > 3) ? rc.values[3] : UINT16_MAX;
+ msg.chan5_raw = (rc.channel_count > 4) ? rc.values[4] : UINT16_MAX;
+ msg.chan6_raw = (rc.channel_count > 5) ? rc.values[5] : UINT16_MAX;
+ msg.chan7_raw = (rc.channel_count > 6) ? rc.values[6] : UINT16_MAX;
+ msg.chan8_raw = (rc.channel_count > 7) ? rc.values[7] : UINT16_MAX;
+ msg.chan9_raw = (rc.channel_count > 8) ? rc.values[8] : UINT16_MAX;
+ msg.chan10_raw = (rc.channel_count > 9) ? rc.values[9] : UINT16_MAX;
+ msg.chan11_raw = (rc.channel_count > 10) ? rc.values[10] : UINT16_MAX;
+ msg.chan12_raw = (rc.channel_count > 11) ? rc.values[11] : UINT16_MAX;
+ msg.chan13_raw = (rc.channel_count > 12) ? rc.values[12] : UINT16_MAX;
+ msg.chan14_raw = (rc.channel_count > 13) ? rc.values[13] : UINT16_MAX;
+ msg.chan15_raw = (rc.channel_count > 14) ? rc.values[14] : UINT16_MAX;
+ msg.chan16_raw = (rc.channel_count > 15) ? rc.values[15] : UINT16_MAX;
+ msg.chan17_raw = (rc.channel_count > 16) ? rc.values[16] : UINT16_MAX;
+ msg.chan18_raw = (rc.channel_count > 17) ? rc.values[17] : UINT16_MAX;
+ msg.rssi = rc.rssi;
+
+ _mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS, &msg);
}
}
};
@@ -1558,42 +1746,45 @@ public:
return MAVLINK_MSG_ID_MANUAL_CONTROL;
}
- static MavlinkStream *new_instance()
+ static MavlinkStream *new_instance(Mavlink *mavlink)
+ {
+ return new MavlinkStreamManualControl(mavlink);
+ }
+
+ unsigned get_size()
{
- return new MavlinkStreamManualControl();
+ return _manual_sub->is_published() ? (MAVLINK_MSG_ID_MANUAL_CONTROL_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
}
private:
- MavlinkOrbSubscription *manual_sub;
- uint64_t manual_time;
+ MavlinkOrbSubscription *_manual_sub;
+ uint64_t _manual_time;
/* do not allow top copying this class */
MavlinkStreamManualControl(MavlinkStreamManualControl &);
MavlinkStreamManualControl& operator = (const MavlinkStreamManualControl &);
protected:
- explicit MavlinkStreamManualControl() : MavlinkStream(),
- manual_sub(nullptr),
- manual_time(0)
+ explicit MavlinkStreamManualControl(Mavlink *mavlink) : MavlinkStream(mavlink),
+ _manual_sub(_mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint))),
+ _manual_time(0)
{}
- void subscribe(Mavlink *mavlink)
- {
- manual_sub = mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint));
- }
-
void send(const hrt_abstime t)
{
struct manual_control_setpoint_s manual;
- if (manual_sub->update(&manual_time, &manual)) {
- mavlink_msg_manual_control_send(_channel,
- mavlink_system.sysid,
- manual.x * 1000,
- manual.y * 1000,
- manual.z * 1000,
- manual.r * 1000,
- 0);
+ if (_manual_sub->update(&_manual_time, &manual)) {
+ mavlink_manual_control_t msg;
+
+ msg.target = mavlink_system.sysid;
+ msg.x = manual.x * 1000;
+ msg.y = manual.y * 1000;
+ msg.z = manual.z * 1000;
+ msg.r = manual.r * 1000;
+ msg.buttons = 0;
+
+ _mavlink->send_message(MAVLINK_MSG_ID_MANUAL_CONTROL, &msg);
}
}
};
@@ -1617,42 +1808,47 @@ public:
return MAVLINK_MSG_ID_OPTICAL_FLOW;
}
- static MavlinkStream *new_instance()
+ static MavlinkStream *new_instance(Mavlink *mavlink)
+ {
+ return new MavlinkStreamOpticalFlow(mavlink);
+ }
+
+ unsigned get_size()
{
- return new MavlinkStreamOpticalFlow();
+ return _flow_sub->is_published() ? (MAVLINK_MSG_ID_OPTICAL_FLOW_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
}
private:
- MavlinkOrbSubscription *flow_sub;
- uint64_t flow_time;
+ MavlinkOrbSubscription *_flow_sub;
+ uint64_t _flow_time;
/* do not allow top copying this class */
MavlinkStreamOpticalFlow(MavlinkStreamOpticalFlow &);
MavlinkStreamOpticalFlow& operator = (const MavlinkStreamOpticalFlow &);
protected:
- explicit MavlinkStreamOpticalFlow() : MavlinkStream(),
- flow_sub(nullptr),
- flow_time(0)
+ explicit MavlinkStreamOpticalFlow(Mavlink *mavlink) : MavlinkStream(mavlink),
+ _flow_sub(_mavlink->add_orb_subscription(ORB_ID(optical_flow))),
+ _flow_time(0)
{}
- void subscribe(Mavlink *mavlink)
- {
- flow_sub = mavlink->add_orb_subscription(ORB_ID(optical_flow));
- }
-
void send(const hrt_abstime t)
{
struct optical_flow_s flow;
- if (flow_sub->update(&flow_time, &flow)) {
- mavlink_msg_optical_flow_send(_channel,
- flow.timestamp,
- flow.sensor_id,
- flow.flow_raw_x, flow.flow_raw_y,
- flow.flow_comp_x_m, flow.flow_comp_y_m,
- flow.quality,
- flow.ground_distance_m);
+ if (_flow_sub->update(&_flow_time, &flow)) {
+ mavlink_optical_flow_t msg;
+
+ msg.time_usec = flow.timestamp;
+ msg.sensor_id = flow.sensor_id;
+ msg.flow_x = flow.flow_raw_x;
+ msg.flow_y = flow.flow_raw_y;
+ msg.flow_comp_m_x = flow.flow_comp_x_m;
+ msg.flow_comp_m_y = flow.flow_comp_y_m;
+ msg.quality = flow.quality;
+ msg.ground_distance = flow.ground_distance_m;
+
+ _mavlink->send_message(MAVLINK_MSG_ID_OPTICAL_FLOW, &msg);
}
}
};
@@ -1675,56 +1871,64 @@ public:
return 0;
}
- static MavlinkStream *new_instance()
+ static MavlinkStream *new_instance(Mavlink *mavlink)
+ {
+ return new MavlinkStreamAttitudeControls(mavlink);
+ }
+
+ unsigned get_size()
{
- return new MavlinkStreamAttitudeControls();
+ return 4 * (MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES);
}
private:
- MavlinkOrbSubscription *att_ctrl_sub;
- uint64_t att_ctrl_time;
+ MavlinkOrbSubscription *_att_ctrl_sub;
+ uint64_t _att_ctrl_time;
/* do not allow top copying this class */
MavlinkStreamAttitudeControls(MavlinkStreamAttitudeControls &);
MavlinkStreamAttitudeControls& operator = (const MavlinkStreamAttitudeControls &);
protected:
- explicit MavlinkStreamAttitudeControls() : MavlinkStream(),
- att_ctrl_sub(nullptr),
- att_ctrl_time(0)
+ explicit MavlinkStreamAttitudeControls(Mavlink *mavlink) : MavlinkStream(mavlink),
+ _att_ctrl_sub(_mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS)),
+ _att_ctrl_time(0)
{}
- void subscribe(Mavlink *mavlink)
- {
- att_ctrl_sub = mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
- }
-
void send(const hrt_abstime t)
{
struct actuator_controls_s att_ctrl;
- if (att_ctrl_sub->update(&att_ctrl_time, &att_ctrl)) {
+ if (_att_ctrl_sub->update(&_att_ctrl_time, &att_ctrl)) {
/* send, add spaces so that string buffer is at least 10 chars long */
- mavlink_msg_named_value_float_send(_channel,
- att_ctrl.timestamp / 1000,
- "rll ctrl ",
- att_ctrl.control[0]);
- mavlink_msg_named_value_float_send(_channel,
- att_ctrl.timestamp / 1000,
- "ptch ctrl ",
- att_ctrl.control[1]);
- mavlink_msg_named_value_float_send(_channel,
- att_ctrl.timestamp / 1000,
- "yaw ctrl ",
- att_ctrl.control[2]);
- mavlink_msg_named_value_float_send(_channel,
- att_ctrl.timestamp / 1000,
- "thr ctrl ",
- att_ctrl.control[3]);
+ mavlink_named_value_float_t msg;
+
+ msg.time_boot_ms = att_ctrl.timestamp / 1000;
+
+ snprintf(msg.name, sizeof(msg.name), "rll ctrl");
+ msg.value = att_ctrl.control[0];
+
+ _mavlink->send_message(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, &msg);
+
+ snprintf(msg.name, sizeof(msg.name), "ptch ctrl");
+ msg.value = att_ctrl.control[1];
+
+ _mavlink->send_message(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, &msg);
+
+ snprintf(msg.name, sizeof(msg.name), "yaw ctrl");
+ msg.value = att_ctrl.control[2];
+
+ _mavlink->send_message(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, &msg);
+
+ snprintf(msg.name, sizeof(msg.name), "thr ctrl");
+ msg.value = att_ctrl.control[3];
+
+ _mavlink->send_message(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, &msg);
}
}
};
+
class MavlinkStreamNamedValueFloat : public MavlinkStream
{
public:
@@ -1743,46 +1947,49 @@ public:
return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT;
}
- static MavlinkStream *new_instance()
+ static MavlinkStream *new_instance(Mavlink *mavlink)
+ {
+ return new MavlinkStreamNamedValueFloat(mavlink);
+ }
+
+ unsigned get_size()
{
- return new MavlinkStreamNamedValueFloat();
+ return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
}
private:
- MavlinkOrbSubscription *debug_sub;
- uint64_t debug_time;
+ MavlinkOrbSubscription *_debug_sub;
+ uint64_t _debug_time;
/* do not allow top copying this class */
MavlinkStreamNamedValueFloat(MavlinkStreamNamedValueFloat &);
MavlinkStreamNamedValueFloat& operator = (const MavlinkStreamNamedValueFloat &);
protected:
- explicit MavlinkStreamNamedValueFloat() : MavlinkStream(),
- debug_sub(nullptr),
- debug_time(0)
+ explicit MavlinkStreamNamedValueFloat(Mavlink *mavlink) : MavlinkStream(mavlink),
+ _debug_sub(_mavlink->add_orb_subscription(ORB_ID(debug_key_value))),
+ _debug_time(0)
{}
- void subscribe(Mavlink *mavlink)
- {
- debug_sub = mavlink->add_orb_subscription(ORB_ID(debug_key_value));
- }
-
void send(const hrt_abstime t)
{
struct debug_key_value_s debug;
- if (debug_sub->update(&debug_time, &debug)) {
+ if (_debug_sub->update(&_debug_time, &debug)) {
+ mavlink_named_value_float_t msg;
+
+ msg.time_boot_ms = debug.timestamp_ms;
+ memcpy(msg.name, debug.key, sizeof(msg.name));
/* enforce null termination */
- debug.key[sizeof(debug.key) - 1] = '\0';
+ msg.name[sizeof(msg.name) - 1] = '\0';
+ msg.value = debug.value;
- mavlink_msg_named_value_float_send(_channel,
- debug.timestamp_ms,
- debug.key,
- debug.value);
+ _mavlink->send_message(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, &msg);
}
}
};
+
class MavlinkStreamCameraCapture : public MavlinkStream
{
public:
@@ -1801,46 +2008,53 @@ public:
return 0;
}
- static MavlinkStream *new_instance()
+ static MavlinkStream *new_instance(Mavlink *mavlink)
+ {
+ return new MavlinkStreamCameraCapture(mavlink);
+ }
+
+ unsigned get_size()
{
- return new MavlinkStreamCameraCapture();
+ return MAVLINK_MSG_ID_COMMAND_LONG_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
}
private:
- MavlinkOrbSubscription *status_sub;
+ MavlinkOrbSubscription *_status_sub;
/* do not allow top copying this class */
MavlinkStreamCameraCapture(MavlinkStreamCameraCapture &);
MavlinkStreamCameraCapture& operator = (const MavlinkStreamCameraCapture &);
protected:
- explicit MavlinkStreamCameraCapture() : MavlinkStream(),
- status_sub(nullptr)
+ explicit MavlinkStreamCameraCapture(Mavlink *mavlink) : MavlinkStream(mavlink),
+ _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status)))
{}
- void subscribe(Mavlink *mavlink)
- {
- status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status));
- }
-
void send(const hrt_abstime t)
{
struct vehicle_status_s status;
- (void)status_sub->update(&status);
+ (void)_status_sub->update(&status);
- if (status.arming_state == ARMING_STATE_ARMED
- || status.arming_state == ARMING_STATE_ARMED_ERROR) {
+ mavlink_command_long_t msg;
- /* send camera capture on */
- mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0);
+ msg.target_system = mavlink_system.sysid;
+ msg.target_component = MAV_COMP_ID_ALL;
+ msg.command = MAV_CMD_DO_CONTROL_VIDEO;
+ msg.confirmation = 0;
+ msg.param1 = 0;
+ msg.param2 = 0;
+ msg.param3 = 0;
+ /* set camera capture ON/OFF depending on arming state */
+ msg.param4 = (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) ? 1 : 0;
+ msg.param5 = 0;
+ msg.param6 = 0;
+ msg.param7 = 0;
- } else {
- /* send camera capture off */
- mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0);
- }
+ _mavlink->send_message(MAVLINK_MSG_ID_COMMAND_LONG, &msg);
}
};
+
class MavlinkStreamDistanceSensor : public MavlinkStream
{
public:
@@ -1859,50 +2073,58 @@ public:
return MAVLINK_MSG_ID_DISTANCE_SENSOR;
}
- static MavlinkStream *new_instance()
+ static MavlinkStream *new_instance(Mavlink *mavlink)
+ {
+ return new MavlinkStreamDistanceSensor(mavlink);
+ }
+
+ unsigned get_size()
{
- return new MavlinkStreamDistanceSensor();
+ return _range_sub->is_published() ? (MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
}
private:
- MavlinkOrbSubscription *range_sub;
- uint64_t range_time;
+ MavlinkOrbSubscription *_range_sub;
+ uint64_t _range_time;
/* do not allow top copying this class */
MavlinkStreamDistanceSensor(MavlinkStreamDistanceSensor &);
MavlinkStreamDistanceSensor& operator = (const MavlinkStreamDistanceSensor &);
protected:
- explicit MavlinkStreamDistanceSensor() : MavlinkStream(),
- range_sub(nullptr),
- range_time(0)
+ explicit MavlinkStreamDistanceSensor(Mavlink *mavlink) : MavlinkStream(mavlink),
+ _range_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_range_finder))),
+ _range_time(0)
{}
- void subscribe(Mavlink *mavlink)
- {
- range_sub = mavlink->add_orb_subscription(ORB_ID(sensor_range_finder));
- }
-
void send(const hrt_abstime t)
{
struct range_finder_report range;
- if (range_sub->update(&range_time, &range)) {
+ if (_range_sub->update(&_range_time, &range)) {
+
+ mavlink_distance_sensor_t msg;
- uint8_t type;
+ msg.time_boot_ms = range.timestamp / 1000;
switch (range.type) {
- case RANGE_FINDER_TYPE_LASER:
- type = MAV_DISTANCE_SENSOR_LASER;
+ case RANGE_FINDER_TYPE_LASER:
+ msg.type = MAV_DISTANCE_SENSOR_LASER;
+ break;
+
+ default:
+ msg.type = MAV_DISTANCE_SENSOR_LASER;
break;
}
- uint8_t id = 0;
- uint8_t orientation = 0;
- uint8_t covariance = 20;
+ msg.id = 0;
+ msg.orientation = 0;
+ msg.min_distance = range.minimum_distance * 100;
+ msg.max_distance = range.minimum_distance * 100;
+ msg.current_distance = range.distance * 100;
+ msg.covariance = 20;
- mavlink_msg_distance_sensor_send(_channel, range.timestamp / 1000, type, id, orientation,
- range.minimum_distance*100, range.maximum_distance*100, range.distance*100, covariance);
+ _mavlink->send_message(MAVLINK_MSG_ID_DISTANCE_SENSOR, &msg);
}
}
};
@@ -1910,6 +2132,8 @@ protected:
StreamListItem *streams_list[] = {
new StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static),
+ new StreamListItem(&MavlinkStreamStatustext::new_instance, &MavlinkStreamStatustext::get_name_static),
+ new StreamListItem(&MavlinkStreamCommandLong::new_instance, &MavlinkStreamCommandLong::get_name_static),
new StreamListItem(&MavlinkStreamSysStatus::new_instance, &MavlinkStreamSysStatus::get_name_static),
new StreamListItem(&MavlinkStreamHighresIMU::new_instance, &MavlinkStreamHighresIMU::get_name_static),
new StreamListItem(&MavlinkStreamAttitude::new_instance, &MavlinkStreamAttitude::get_name_static),
@@ -1918,6 +2142,7 @@ StreamListItem *streams_list[] = {
new StreamListItem(&MavlinkStreamGPSRawInt::new_instance, &MavlinkStreamGPSRawInt::get_name_static),
new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static),
new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static),
+ new StreamListItem(&MavlinkStreamViconPositionEstimate::new_instance, &MavlinkStreamViconPositionEstimate::get_name_static),
new StreamListItem(&MavlinkStreamGPSGlobalOrigin::new_instance, &MavlinkStreamGPSGlobalOrigin::get_name_static),
new StreamListItem(&MavlinkStreamServoOutputRaw<0>::new_instance, &MavlinkStreamServoOutputRaw<0>::get_name_static),
new StreamListItem(&MavlinkStreamServoOutputRaw<1>::new_instance, &MavlinkStreamServoOutputRaw<1>::get_name_static),
@@ -1935,6 +2160,5 @@ StreamListItem *streams_list[] = {
new StreamListItem(&MavlinkStreamNamedValueFloat::new_instance, &MavlinkStreamNamedValueFloat::get_name_static),
new StreamListItem(&MavlinkStreamCameraCapture::new_instance, &MavlinkStreamCameraCapture::get_name_static),
new StreamListItem(&MavlinkStreamDistanceSensor::new_instance, &MavlinkStreamDistanceSensor::get_name_static),
- new StreamListItem(&MavlinkStreamViconPositionEstimate::new_instance, &MavlinkStreamViconPositionEstimate::get_name_static),
nullptr
};
diff --git a/src/modules/mavlink/mavlink_messages.h b/src/modules/mavlink/mavlink_messages.h
index ee64d0e42..7e4416609 100644
--- a/src/modules/mavlink/mavlink_messages.h
+++ b/src/modules/mavlink/mavlink_messages.h
@@ -46,10 +46,10 @@
class StreamListItem {
public:
- MavlinkStream* (*new_instance)();
+ MavlinkStream* (*new_instance)(Mavlink *mavlink);
const char* (*get_name)();
- StreamListItem(MavlinkStream* (*inst)(), const char* (*name)()) :
+ StreamListItem(MavlinkStream* (*inst)(Mavlink *mavlink), const char* (*name)()) :
new_instance(inst),
get_name(name) {};
diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp
index 068774c47..7a761588a 100644
--- a/src/modules/mavlink/mavlink_mission.cpp
+++ b/src/modules/mavlink/mavlink_mission.cpp
@@ -59,8 +59,7 @@
static const int ERROR = -1;
-MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) :
- _mavlink(mavlink),
+MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(mavlink),
_state(MAVLINK_WPM_STATE_IDLE),
_time_last_recv(0),
_time_last_sent(0),
@@ -79,9 +78,8 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) :
_offboard_mission_sub(-1),
_mission_result_sub(-1),
_offboard_mission_pub(-1),
- _slow_rate_limiter(2000000.0f / mavlink->get_rate_mult()),
+ _slow_rate_limiter(_interval / 10.0f),
_verbose(false),
- _channel(mavlink->get_channel()),
_comp_id(MAV_COMP_ID_MISSIONPLANNER)
{
_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
@@ -96,6 +94,20 @@ MavlinkMissionManager::~MavlinkMissionManager()
close(_mission_result_sub);
}
+unsigned
+MavlinkMissionManager::get_size()
+{
+ if (_state == MAVLINK_WPM_STATE_SENDLIST) {
+ return MAVLINK_MSG_ID_MISSION_ITEM_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+
+ } else if (_state == MAVLINK_WPM_STATE_GETLIST) {
+ return MAVLINK_MSG_ID_MISSION_REQUEST + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+
+ } else {
+ return 0;
+ }
+}
+
void
MavlinkMissionManager::init_offboard_mission()
{
@@ -157,15 +169,13 @@ MavlinkMissionManager::update_active_mission(int dataman_id, unsigned count, int
void
MavlinkMissionManager::send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t type)
{
- mavlink_message_t msg;
mavlink_mission_ack_t wpa;
wpa.target_system = sysid;
wpa.target_component = compid;
wpa.type = type;
- mavlink_msg_mission_ack_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpa);
- _mavlink->send_message(&msg);
+ _mavlink->send_message(MAVLINK_MSG_ID_MISSION_ACK, &wpa);
if (_verbose) { warnx("WPM: Send MISSION_ACK type %u to ID %u", wpa.type, wpa.target_system); }
}
@@ -175,13 +185,11 @@ void
MavlinkMissionManager::send_mission_current(uint16_t seq)
{
if (seq < _count) {
- mavlink_message_t msg;
mavlink_mission_current_t wpc;
wpc.seq = seq;
- mavlink_msg_mission_current_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpc);
- _mavlink->send_message(&msg);
+ _mavlink->send_message(MAVLINK_MSG_ID_MISSION_CURRENT, &wpc);
} else if (seq == 0 && _count == 0) {
/* don't broadcast if no WPs */
@@ -199,15 +207,13 @@ MavlinkMissionManager::send_mission_count(uint8_t sysid, uint8_t compid, uint16_
{
_time_last_sent = hrt_absolute_time();
- mavlink_message_t msg;
mavlink_mission_count_t wpc;
wpc.target_system = sysid;
wpc.target_component = compid;
wpc.count = _count;
- mavlink_msg_mission_count_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpc);
- _mavlink->send_message(&msg);
+ _mavlink->send_message(MAVLINK_MSG_ID_MISSION_COUNT, &wpc);
if (_verbose) { warnx("WPM: Send MISSION_COUNT %u to ID %u", wpc.count, wpc.target_system); }
}
@@ -226,13 +232,12 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t
mavlink_mission_item_t wp;
format_mavlink_mission_item(&mission_item, &wp);
- mavlink_message_t msg;
wp.target_system = sysid;
wp.target_component = compid;
wp.seq = seq;
wp.current = (_current_seq == seq) ? 1 : 0;
- mavlink_msg_mission_item_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wp);
- _mavlink->send_message(&msg);
+
+ _mavlink->send_message(MAVLINK_MSG_ID_MISSION_ITEM, &wp);
if (_verbose) { warnx("WPM: Send MISSION_ITEM seq %u to ID %u", wp.seq, wp.target_system); }
@@ -251,13 +256,12 @@ MavlinkMissionManager::send_mission_request(uint8_t sysid, uint8_t compid, uint1
if (seq < _max_count) {
_time_last_sent = hrt_absolute_time();
- mavlink_message_t msg;
mavlink_mission_request_t wpr;
wpr.target_system = sysid;
wpr.target_component = compid;
wpr.seq = seq;
- mavlink_msg_mission_request_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpr);
- _mavlink->send_message(&msg);
+
+ _mavlink->send_message(MAVLINK_MSG_ID_MISSION_REQUEST, &wpr);
if (_verbose) { warnx("WPM: Send MISSION_REQUEST seq %u to ID %u", wpr.seq, wpr.target_system); }
@@ -272,22 +276,21 @@ MavlinkMissionManager::send_mission_request(uint8_t sysid, uint8_t compid, uint1
void
MavlinkMissionManager::send_mission_item_reached(uint16_t seq)
{
- mavlink_message_t msg;
mavlink_mission_item_reached_t wp_reached;
wp_reached.seq = seq;
- mavlink_msg_mission_item_reached_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wp_reached);
- _mavlink->send_message(&msg);
+ _mavlink->send_message(MAVLINK_MSG_ID_MISSION_ITEM_REACHED, &wp_reached);
if (_verbose) { warnx("WPM: Send MISSION_ITEM_REACHED reached_seq %u", wp_reached.seq); }
}
void
-MavlinkMissionManager::eventloop()
+MavlinkMissionManager::send(const hrt_abstime now)
{
- hrt_abstime now = hrt_absolute_time();
+ /* update interval for slow rate limiter */
+ _slow_rate_limiter.set_interval(_interval * 10 / _mavlink->get_rate_mult());
bool updated = false;
orb_check(_mission_result_sub, &updated);
diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h
index b792b9aaf..8ff27f87d 100644
--- a/src/modules/mavlink/mavlink_mission.h
+++ b/src/modules/mavlink/mavlink_mission.h
@@ -42,11 +42,12 @@
#pragma once
+#include <uORB/uORB.h>
+
#include "mavlink_bridge_header.h"
#include "mavlink_rate_limiter.h"
-#include <uORB/uORB.h>
+#include "mavlink_stream.h"
-// FIXME XXX - TO BE MOVED TO XML
enum MAVLINK_WPM_STATES {
MAVLINK_WPM_STATE_IDLE = 0,
MAVLINK_WPM_STATE_SENDLIST,
@@ -66,20 +67,75 @@ enum MAVLINK_WPM_CODES {
#define MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication action timeout in useconds
#define MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT 500000 ///< Protocol communication retry timeout in useconds
-class Mavlink;
-
-class MavlinkMissionManager {
+class MavlinkMissionManager : public MavlinkStream {
public:
- MavlinkMissionManager(Mavlink *parent);
-
~MavlinkMissionManager();
+ const char *get_name() const
+ {
+ return MavlinkMissionManager::get_name_static();
+ }
+
+ static const char *get_name_static()
+ {
+ return "MISSION_ITEM";
+ }
+
+ uint8_t get_id()
+ {
+ return MAVLINK_MSG_ID_MISSION_ITEM;
+ }
+
+ static MavlinkStream *new_instance(Mavlink *mavlink)
+ {
+ return new MavlinkMissionManager(mavlink);
+ }
+
+ unsigned get_size();
+
+ void handle_message(const mavlink_message_t *msg);
+
+ void set_verbose(bool v) { _verbose = v; }
+
+private:
+ enum MAVLINK_WPM_STATES _state; ///< Current state
+
+ uint64_t _time_last_recv;
+ uint64_t _time_last_sent;
+
+ uint32_t _action_timeout;
+ uint32_t _retry_timeout;
+ unsigned _max_count; ///< Maximum number of mission items
+
+ int _dataman_id; ///< Dataman storage ID for active mission
+ unsigned _count; ///< Count of items in active mission
+ int _current_seq; ///< Current item sequence in active mission
+
+ int _transfer_dataman_id; ///< Dataman storage ID for current transmission
+ unsigned _transfer_count; ///< Items count in current transmission
+ unsigned _transfer_seq; ///< Item sequence in current transmission
+ unsigned _transfer_current_seq; ///< Current item ID for current transmission (-1 means not initialized)
+ unsigned _transfer_partner_sysid; ///< Partner system ID for current transmission
+ unsigned _transfer_partner_compid; ///< Partner component ID for current transmission
+
+ int _offboard_mission_sub;
+ int _mission_result_sub;
+ orb_advert_t _offboard_mission_pub;
+
+ MavlinkRateLimiter _slow_rate_limiter;
+
+ bool _verbose;
+
+ uint8_t _comp_id;
+
+ /* do not allow top copying this class */
+ MavlinkMissionManager(MavlinkMissionManager &);
+ MavlinkMissionManager& operator = (const MavlinkMissionManager &);
+
void init_offboard_mission();
int update_active_mission(int dataman_id, unsigned count, int seq);
- void send_message(mavlink_message_t *msg);
-
/**
* @brief Sends an waypoint ack message
*/
@@ -111,10 +167,6 @@ public:
*/
void send_mission_item_reached(uint16_t seq);
- void eventloop();
-
- void handle_message(const mavlink_message_t *msg);
-
void handle_mission_ack(const mavlink_message_t *msg);
void handle_mission_set_current(const mavlink_message_t *msg);
@@ -139,43 +191,8 @@ public:
*/
int format_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item);
- void set_verbose(bool v) { _verbose = v; }
-
-private:
- Mavlink* _mavlink;
-
- enum MAVLINK_WPM_STATES _state; ///< Current state
-
- uint64_t _time_last_recv;
- uint64_t _time_last_sent;
-
- uint32_t _action_timeout;
- uint32_t _retry_timeout;
- unsigned _max_count; ///< Maximum number of mission items
-
- int _dataman_id; ///< Dataman storage ID for active mission
- unsigned _count; ///< Count of items in active mission
- int _current_seq; ///< Current item sequence in active mission
-
- int _transfer_dataman_id; ///< Dataman storage ID for current transmission
- unsigned _transfer_count; ///< Items count in current transmission
- unsigned _transfer_seq; ///< Item sequence in current transmission
- unsigned _transfer_current_seq; ///< Current item ID for current transmission (-1 means not initialized)
- unsigned _transfer_partner_sysid; ///< Partner system ID for current transmission
- unsigned _transfer_partner_compid; ///< Partner component ID for current transmission
-
- int _offboard_mission_sub;
- int _mission_result_sub;
- orb_advert_t _offboard_mission_pub;
-
- MavlinkRateLimiter _slow_rate_limiter;
-
- bool _verbose;
-
- mavlink_channel_t _channel;
- uint8_t _comp_id;
+protected:
+ explicit MavlinkMissionManager(Mavlink *mavlink);
- /* do not allow copying this class */
- MavlinkMissionManager(const MavlinkMissionManager&);
- MavlinkMissionManager operator=(const MavlinkMissionManager&);
+ void send(const hrt_abstime t);
};
diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp
new file mode 100644
index 000000000..cd5f53d88
--- /dev/null
+++ b/src/modules/mavlink/mavlink_parameters.cpp
@@ -0,0 +1,197 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mavlink_parameters.cpp
+ * Mavlink parameters manager implementation.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#include <stdio.h>
+
+#include "mavlink_parameters.h"
+#include "mavlink_main.h"
+
+MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : MavlinkStream(mavlink),
+ _send_all_index(-1)
+{
+}
+
+unsigned
+MavlinkParametersManager::get_size()
+{
+ if (_send_all_index >= 0) {
+ return MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+
+ } else {
+ return 0;
+ }
+}
+
+void
+MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
+{
+ switch (msg->msgid) {
+ case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
+ /* request all parameters */
+ mavlink_param_request_list_t req_list;
+ mavlink_msg_param_request_list_decode(msg, &req_list);
+
+ if (req_list.target_system == mavlink_system.sysid &&
+ (req_list.target_component == mavlink_system.compid || req_list.target_component == MAV_COMP_ID_ALL)) {
+
+ _send_all_index = 0;
+ _mavlink->send_statustext_info("[pm] sending list");
+ }
+ break;
+ }
+
+ case MAVLINK_MSG_ID_PARAM_SET: {
+ /* set parameter */
+ if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET) {
+ mavlink_param_set_t set;
+ mavlink_msg_param_set_decode(msg, &set);
+
+ if (set.target_system == mavlink_system.sysid &&
+ (set.target_component == mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) {
+
+ /* local name buffer to enforce null-terminated string */
+ char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
+ strncpy(name, set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
+ /* enforce null termination */
+ name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
+ /* attempt to find parameter, set and send it */
+ param_t param = param_find(name);
+
+ if (param == PARAM_INVALID) {
+ char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
+ sprintf(buf, "[pm] unknown param: %s", name);
+ _mavlink->send_statustext_info(buf);
+
+ } else {
+ /* set and send parameter */
+ param_set(param, &(set.param_value));
+ send_param(param);
+ }
+ }
+ }
+ break;
+ }
+
+ case MAVLINK_MSG_ID_PARAM_REQUEST_READ: {
+ /* request one parameter */
+ mavlink_param_request_read_t req_read;
+ mavlink_msg_param_request_read_decode(msg, &req_read);
+
+ if (req_read.target_system == mavlink_system.sysid &&
+ (req_read.target_component == mavlink_system.compid || req_read.target_component == MAV_COMP_ID_ALL)) {
+
+ /* when no index is given, loop through string ids and compare them */
+ if (req_read.param_index < 0) {
+ /* local name buffer to enforce null-terminated string */
+ char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
+ strncpy(name, req_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
+ /* enforce null termination */
+ name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
+ /* attempt to find parameter and send it */
+ send_param(param_find(name));
+
+ } else {
+ /* when index is >= 0, send this parameter again */
+ send_param(param_for_index(req_read.param_index));
+ }
+ }
+ break;
+ }
+
+ default:
+ break;
+ }
+}
+
+void
+MavlinkParametersManager::send(const hrt_abstime t)
+{
+ /* send all parameters if requested */
+ if (_send_all_index >= 0) {
+ send_param(param_for_index(_send_all_index));
+ _send_all_index++;
+ if (_send_all_index >= (int) param_count()) {
+ _send_all_index = -1;
+ }
+ }
+}
+
+void
+MavlinkParametersManager::send_param(param_t param)
+{
+ if (param == PARAM_INVALID) {
+ return;
+ }
+
+ mavlink_param_value_t msg;
+
+ /*
+ * get param value, since MAVLink encodes float and int params in the same
+ * space during transmission, copy param onto float val_buf
+ */
+ if (param_get(param, &msg.param_value) != OK) {
+ return;
+ }
+
+ msg.param_count = param_count();
+ msg.param_index = param_get_index(param);
+
+ /* copy parameter name */
+ strncpy(msg.param_id, param_name(param), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
+
+ /* query parameter type */
+ param_type_t type = param_type(param);
+
+ /*
+ * Map onboard parameter type to MAVLink type,
+ * endianess matches (both little endian)
+ */
+ if (type == PARAM_TYPE_INT32) {
+ msg.param_type = MAVLINK_TYPE_INT32_T;
+
+ } else if (type == PARAM_TYPE_FLOAT) {
+ msg.param_type = MAVLINK_TYPE_FLOAT;
+
+ } else {
+ msg.param_type = MAVLINK_TYPE_FLOAT;
+ }
+
+ _mavlink->send_message(MAVLINK_MSG_ID_PARAM_VALUE, &msg);
+}
diff --git a/src/modules/mavlink/mavlink_commands.h b/src/modules/mavlink/mavlink_parameters.h
index abdba34b9..5576e6b84 100644
--- a/src/modules/mavlink/mavlink_commands.h
+++ b/src/modules/mavlink/mavlink_parameters.h
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -32,34 +32,84 @@
****************************************************************************/
/**
- * @file mavlink_commands.h
- * Mavlink commands stream definition.
+ * @file mavlink_parameters.h
+ * Mavlink parameters manager definition.
*
* @author Anton Babushkin <anton.babushkin@me.com>
*/
-#ifndef MAVLINK_COMMANDS_H_
-#define MAVLINK_COMMANDS_H_
+#pragma once
-#include <uORB/uORB.h>
-#include <uORB/topics/vehicle_command.h>
+#include <systemlib/param/param.h>
-class Mavlink;
-class MavlinkCommansStream;
+#include "mavlink_bridge_header.h"
+#include "mavlink_stream.h"
-#include "mavlink_main.h"
-
-class MavlinkCommandsStream
+class MavlinkParametersManager : public MavlinkStream
{
+public:
+ const char *get_name() const
+ {
+ return MavlinkParametersManager::get_name_static();
+ }
+
+ static const char *get_name_static()
+ {
+ return "PARAM_VALUE";
+ }
+
+ uint8_t get_id()
+ {
+ return MAVLINK_MSG_ID_PARAM_VALUE;
+ }
+
+ static MavlinkStream *new_instance(Mavlink *mavlink)
+ {
+ return new MavlinkParametersManager(mavlink);
+ }
+
+ unsigned get_size();
+
+ void handle_message(const mavlink_message_t *msg);
+
+ /**
+ * Send one parameter identified by index.
+ *
+ * @param index The index of the parameter to send.
+ * @return zero on success, nonzero else.
+ */
+ void start_send_one(int index);
+
+
+ /**
+ * Send one parameter identified by name.
+ *
+ * @param name The index of the parameter to send.
+ * @return zero on success, nonzero else.
+ */
+ int start_send_for_name(const char *name);
+
+ /**
+ * Start sending the parameter queue.
+ *
+ * This function will not directly send parameters, but instead
+ * activate the sending of one parameter on each call of
+ * mavlink_pm_queued_send().
+ * @see mavlink_pm_queued_send()
+ */
+ void start_send_all();
+
private:
- MavlinkOrbSubscription *_cmd_sub;
- struct vehicle_command_s *_cmd;
- mavlink_channel_t _channel;
- uint64_t _cmd_time;
+ int _send_all_index;
-public:
- MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel);
- void update(const hrt_abstime t);
-};
+ /* do not allow top copying this class */
+ MavlinkParametersManager(MavlinkParametersManager &);
+ MavlinkParametersManager& operator = (const MavlinkParametersManager &);
+
+protected:
+ explicit MavlinkParametersManager(Mavlink *mavlink);
-#endif /* MAVLINK_COMMANDS_H_ */
+ void send(const hrt_abstime t);
+
+ void send_param(param_t param);
+};
diff --git a/src/modules/mavlink/mavlink_rate_limiter.cpp b/src/modules/mavlink/mavlink_rate_limiter.cpp
index 9cdda8b17..d3b3e1cde 100644
--- a/src/modules/mavlink/mavlink_rate_limiter.cpp
+++ b/src/modules/mavlink/mavlink_rate_limiter.cpp
@@ -64,7 +64,7 @@ MavlinkRateLimiter::check(hrt_abstime t)
uint64_t dt = t - _last_sent;
if (dt > 0 && dt >= _interval) {
- _last_sent = (t / _interval) * _interval;
+ _last_sent = t;
return true;
}
diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp
index 7279206db..5b9e45d3e 100644
--- a/src/modules/mavlink/mavlink_stream.cpp
+++ b/src/modules/mavlink/mavlink_stream.cpp
@@ -43,9 +43,9 @@
#include "mavlink_stream.h"
#include "mavlink_main.h"
-MavlinkStream::MavlinkStream() :
+MavlinkStream::MavlinkStream(Mavlink *mavlink) :
next(nullptr),
- _channel(MAVLINK_COMM_0),
+ _mavlink(mavlink),
_interval(1000000),
_last_sent(0)
{
@@ -65,26 +65,27 @@ MavlinkStream::set_interval(const unsigned int interval)
}
/**
- * Set mavlink channel
- */
-void
-MavlinkStream::set_channel(mavlink_channel_t channel)
-{
- _channel = channel;
-}
-
-/**
* Update subscriptions and send message if necessary
*/
int
MavlinkStream::update(const hrt_abstime t)
{
uint64_t dt = t - _last_sent;
+ unsigned int interval = _interval;
+
+ if (!const_rate()) {
+ interval /= _mavlink->get_rate_mult();
+ }
- if (dt > 0 && dt >= _interval) {
+ if (dt > 0 && dt >= interval) {
/* interval expired, send message */
send(t);
- _last_sent = (t / _interval) * _interval;
+ if (const_rate()) {
+ _last_sent = (t / _interval) * _interval;
+
+ } else {
+ _last_sent = t;
+ }
return 0;
}
diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h
index 20e1c7c44..ef22dc443 100644
--- a/src/modules/mavlink/mavlink_stream.h
+++ b/src/modules/mavlink/mavlink_stream.h
@@ -46,31 +46,50 @@
class Mavlink;
class MavlinkStream;
-#include "mavlink_main.h"
-
class MavlinkStream
{
public:
MavlinkStream *next;
- MavlinkStream();
+ MavlinkStream(Mavlink *mavlink);
virtual ~MavlinkStream();
+
+ /**
+ * Get the interval
+ *
+ * @param interval the inveral in microseconds (us) between messages
+ */
void set_interval(const unsigned int interval);
- void set_channel(mavlink_channel_t channel);
+
+ /**
+ * Get the interval
+ *
+ * @return the inveral in microseconds (us) between messages
+ */
+ unsigned get_interval() { return _interval; }
/**
* @return 0 if updated / sent, -1 if unchanged
*/
int update(const hrt_abstime t);
- static MavlinkStream *new_instance();
+ static MavlinkStream *new_instance(const Mavlink *mavlink);
static const char *get_name_static();
- virtual void subscribe(Mavlink *mavlink) = 0;
virtual const char *get_name() const = 0;
virtual uint8_t get_id() = 0;
+ /**
+ * @return true if steam rate shouldn't be adjusted
+ */
+ virtual bool const_rate() { return false; }
+
+ /**
+ * Get maximal total messages size on update
+ */
+ virtual unsigned get_size() = 0;
+
protected:
- mavlink_channel_t _channel;
+ Mavlink * _mavlink;
unsigned int _interval;
virtual void send(const hrt_abstime t) = 0;
diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk
index 1986ae3c8..91fdd6154 100644
--- a/src/modules/mavlink/module.mk
+++ b/src/modules/mavlink/module.mk
@@ -40,11 +40,11 @@ SRCS += mavlink_main.cpp \
mavlink.c \
mavlink_receiver.cpp \
mavlink_mission.cpp \
+ mavlink_parameters.cpp \
mavlink_orb_subscription.cpp \
mavlink_messages.cpp \
mavlink_stream.cpp \
mavlink_rate_limiter.cpp \
- mavlink_commands.cpp \
mavlink_ftp.cpp
INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-15 06:56:15 +0000