fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-19 00:23:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #1217 from PX4/mavlinkrates2

Browse Source 
Mavlink: don't use mavlink helpers
This commit is contained in:
Lorenz Meier
2014-07-31 11:29:10 +02:00
parent 3a32ffa90b 2b71067618
commit 68415137b9
16 changed files with 1565 additions and 1218 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/init.d/rc.usb
+6
View File
@@ -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
src/modules/mavlink/mavlink_bridge_header.h
+6 -1
View File
@@ -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
src/modules/mavlink/mavlink_commands.cpp
-73
View File
@@ -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);
}
}
}
src/modules/mavlink/mavlink_ftp.h
+1
View File
@@ -55,6 +55,7 @@
#include <systemlib/err.h>
#include "mavlink_messages.h"
#include "mavlink_main.h"
class MavlinkFTP
{
src/modules/mavlink/mavlink_main.cpp
+281 -379
View File
@@ -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,13 +709,145 @@ Mavlink::set_hil_enabled(bool hil_enabled)
return ret;
}
void
Mavlink::send_message(const mavlink_message_t *msg)
unsigned
Mavlink::get_free_tx_buf()
{
/*
* 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);
}
}
return buf_free;
}
void
Mavlink::send_message(const uint8_t msgid, const void *msg)
{
/* If the wait until transmit flag is on, only transmit after we've received messages.
Otherwise, transmit all the time. */
if (!should_transmit()) {
return;
}
pthread_mutex_lock(&_send_mutex);
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;
}
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
uint16_t len = mavlink_msg_to_send_buffer(buf, msg);
mavlink_send_uart_bytes(_channel, buf, 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 {
_last_write_success_time = _last_write_try_time;
count_txbytes(packet_len);
}
pthread_mutex_unlock(&_send_mutex);
}
void
Mavlink::resend_message(mavlink_message_t *msg)
{
/* If the wait until transmit flag is on, only transmit after we've received messages.
Otherwise, transmit all the time. */
if (!should_transmit()) {
return;
}
pthread_mutex_lock(&_send_mutex);
int buf_free = get_free_tx_buf();
_last_write_try_time = hrt_absolute_time();
unsigned packet_len = msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
/* check if there is space in the buffer, let it overflow else */
if (buf_free < TX_BUFFER_GAP) {
/* no enough space in buffer to send */
count_txerr();
count_txerrbytes(packet_len);
pthread_mutex_unlock(&_send_mutex);
return;
}
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
/* header and payload */
memcpy(&buf[0], &msg->magic, MAVLINK_NUM_HEADER_BYTES + msg->len);
/* checksum */
buf[MAVLINK_NUM_HEADER_BYTES + msg->len] = (uint8_t)(msg->checksum & 0xFF);
buf[MAVLINK_NUM_HEADER_BYTES + msg->len + 1] = (uint8_t)(msg->checksum >> 8);
/* send message to UART */
ssize_t ret = write(_uart_fd, buf, packet_len);
if (ret != (int) packet_len) {
count_txerr();
count_txerrbytes(packet_len);
} else {
_last_write_success_time = _last_write_try_time;
count_txbytes(packet_len);
}
pthread_mutex_unlock(&_send_mutex);
}
void
@@ -806,7 +857,7 @@ Mavlink::handle_message(const mavlink_message_t *msg)
_mission_manager->handle_message(msg);
/* handle packet with parameter component */
mavlink_pm_message_handler(_channel, msg);
_parameters_manager->handle_message(msg);
if (get_forwarding_on()) {
/* forward any messages to other mavlink instances */
@@ -814,224 +865,32 @@ Mavlink::handle_message(const mavlink_message_t *msg)
}
}
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;
} else {
return 1;
}
}
void Mavlink::mavlink_pm_start_queued_send()
{
_mavlink_param_queue_index = 0;
}
int Mavlink::mavlink_pm_send_param_for_index(uint16_t index)
{
return mavlink_pm_send_param(param_for_index(index));
}
int Mavlink::mavlink_pm_send_param_for_name(const char *name)
{
return mavlink_pm_send_param(param_find(name));
}
int Mavlink::mavlink_pm_send_param(param_t param)
{
if (param == PARAM_INVALID) { return 1; }
/* buffers for param transmission */
char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN];
float val_buf;
mavlink_message_t tx_msg;
/* 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);
/*
* Map onboard parameter type to MAVLink type,
* endianess matches (both little endian)
*/
uint8_t mavlink_type;
if (type == PARAM_TYPE_INT32) {
mavlink_type = MAVLINK_TYPE_INT32_T;
} else if (type == PARAM_TYPE_FLOAT) {
mavlink_type = MAVLINK_TYPE_FLOAT;
} else {
mavlink_type = MAVLINK_TYPE_FLOAT;
}
/*
* get param value, since MAVLink encodes float and int params in the same
* space during transmission, copy param onto float val_buf
*/
int ret;
if ((ret = param_get(param, &val_buf)) != OK) {
return ret;
}
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;
}
void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, 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);
} else {
/* when index is >= 0, send this parameter again */
mavlink_pm_send_param_for_index(mavlink_param_request_read.param_index);
}
}
} break;
}
}
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;
struct mavlink_logmessage logmsg;
strncpy(logmsg.text, string, sizeof(logmsg.text));
logmsg.severity = severity;
int i = 0;
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;
@@ -1105,6 +968,31 @@ Mavlink::configure_stream(const char *stream_name, const float rate)
return ERROR;
}
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)
{
@@ -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
src/modules/mavlink/mavlink_main.h
+47 -52
View File
@@ -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,53 +353,28 @@ 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);
/**
* 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);
/**
* Send one parameter identified by name.
*
* @param name The index of the parameter to send.
* @return zero on success, nonzero else.
*/
int mavlink_pm_send_param_for_name(const char *name);
/**
* Send a queue of parameters, one parameter per function call.
*
* @return zero on success, nonzero on failure
*/
int mavlink_pm_queued_send(void);
/**
* 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 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);
/**
* Get the free space in the transmit buffer
*
* @return free space in the UART TX buffer
*/
unsigned get_free_tx_buf();
static unsigned int interval_from_rate(float rate);
int configure_stream(const char *stream_name, const float rate);
/**
* Adjust the stream rates based on the current rate
*
* @param multiplier if greater than 1, the transmission rate will increase, if smaller than one decrease
*/
void adjust_stream_rates(const float multiplier);
int message_buffer_init(int size);
void message_buffer_destroy();
@@ -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);
/**
src/modules/mavlink/mavlink_messages.cpp
+815 -591
View File
File diff suppressed because it is too large Load Diff
src/modules/mavlink/mavlink_messages.h
+2 -2
View File
@@ -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) {};
src/modules/mavlink/mavlink_mission.cpp
+27 -24
View File
@@ -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);
src/modules/mavlink/mavlink_mission.h
+69 -52
View File
@@ -42,11 +42,12 @@
#pragma once
#include "mavlink_bridge_header.h"
#include "mavlink_rate_limiter.h"
#include <uORB/uORB.h>
// FIXME XXX - TO BE MOVED TO XML
#include "mavlink_bridge_header.h"
#include "mavlink_rate_limiter.h"
#include "mavlink_stream.h"
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; }
protected:
explicit MavlinkMissionManager(Mavlink *mavlink);
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;
/* do not allow copying this class */
MavlinkMissionManager(const MavlinkMissionManager&);
MavlinkMissionManager operator=(const MavlinkMissionManager&);
void send(const hrt_abstime t);
};
src/modules/mavlink/mavlink_parameters.cpp
+197
View File
@@ -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);
}
src/modules/mavlink/mavlink_commands.h → src/modules/mavlink/mavlink_parameters.h
+72 -22
View File
@@ -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
{
private:
MavlinkOrbSubscription *_cmd_sub;
struct vehicle_command_s *_cmd;
mavlink_channel_t _channel;
uint64_t _cmd_time;
public:
MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel);
void update(const hrt_abstime t);
};
const char *get_name() const
{
return MavlinkParametersManager::get_name_static();
}
#endif /* MAVLINK_COMMANDS_H_ */
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:
int _send_all_index;
/* do not allow top copying this class */
MavlinkParametersManager(MavlinkParametersManager &);
MavlinkParametersManager& operator = (const MavlinkParametersManager &);
protected:
explicit MavlinkParametersManager(Mavlink *mavlink);
void send(const hrt_abstime t);
void send_param(param_t param);
};
src/modules/mavlink/mavlink_rate_limiter.cpp
+1 -1
View File
@@ -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;
}
src/modules/mavlink/mavlink_stream.cpp
+14 -13
View File
@@ -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)
{
@@ -64,15 +64,6 @@ MavlinkStream::set_interval(const unsigned int interval)
_interval = interval;
}
/**
* Set mavlink channel
*/
void
MavlinkStream::set_channel(mavlink_channel_t channel)
{
_channel = channel;
}
/**
* Update subscriptions and send message if necessary
*/
@@ -80,11 +71,21 @@ int
MavlinkStream::update(const hrt_abstime t)
{
uint64_t dt = t - _last_sent;
unsigned int interval = _interval;
if (dt > 0 && dt >= _interval) {
if (!const_rate()) {
interval /= _mavlink->get_rate_mult();
}
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;
}
src/modules/mavlink/mavlink_stream.h
+26 -7
View File
@@ -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;
src/modules/mavlink/module.mk
+1 -1
View File
@@ -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