forked from Archive/PX4-Autopilot
mavlink: new message sending API, includes fixed
This commit is contained in:
parent
344a34bb72
commit
a5f2d1b066
|
@ -55,6 +55,7 @@
|
||||||
#include <systemlib/err.h>
|
#include <systemlib/err.h>
|
||||||
|
|
||||||
#include "mavlink_messages.h"
|
#include "mavlink_messages.h"
|
||||||
|
#include "mavlink_main.h"
|
||||||
|
|
||||||
class MavlinkFTP
|
class MavlinkFTP
|
||||||
{
|
{
|
||||||
|
|
|
@ -102,6 +102,7 @@ static Mavlink *_mavlink_instances = nullptr;
|
||||||
static struct file_operations fops;
|
static struct file_operations fops;
|
||||||
|
|
||||||
static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS;
|
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
|
* mavlink app start / stop handling function
|
||||||
|
@ -130,6 +131,7 @@ Mavlink::Mavlink() :
|
||||||
_subscriptions(nullptr),
|
_subscriptions(nullptr),
|
||||||
_streams(nullptr),
|
_streams(nullptr),
|
||||||
_mission_manager(nullptr),
|
_mission_manager(nullptr),
|
||||||
|
_parameters_manager(nullptr),
|
||||||
_mission_pub(-1),
|
_mission_pub(-1),
|
||||||
_mission_result_sub(-1),
|
_mission_result_sub(-1),
|
||||||
_mode(MAVLINK_MODE_NORMAL),
|
_mode(MAVLINK_MODE_NORMAL),
|
||||||
|
@ -145,6 +147,7 @@ Mavlink::Mavlink() :
|
||||||
_baudrate(57600),
|
_baudrate(57600),
|
||||||
_datarate(1000),
|
_datarate(1000),
|
||||||
_datarate_events(500),
|
_datarate_events(500),
|
||||||
|
_rate_mult(0.0f),
|
||||||
_mavlink_param_queue_index(0),
|
_mavlink_param_queue_index(0),
|
||||||
mavlink_link_termination_allowed(false),
|
mavlink_link_termination_allowed(false),
|
||||||
_subscribe_to_stream(nullptr),
|
_subscribe_to_stream(nullptr),
|
||||||
|
@ -687,18 +690,23 @@ Mavlink::set_hil_enabled(bool hil_enabled)
|
||||||
return ret;
|
return ret;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool
|
void
|
||||||
Mavlink::check_can_send(const int prio, const unsigned packet_len)
|
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;
|
||||||
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Check if the OS buffer is full and disable HW
|
* Check if the OS buffer is full and disable HW
|
||||||
* flow control if it continues to be full
|
* flow control if it continues to be full
|
||||||
*/
|
*/
|
||||||
int buf_free = 0;
|
int buf_free = 0;
|
||||||
|
(void) ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free);
|
||||||
|
|
||||||
if (get_flow_control_enabled()
|
if (get_flow_control_enabled() && buf_free == 0) {
|
||||||
&& ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free) == 0) {
|
|
||||||
|
|
||||||
/* Disable hardware flow control:
|
/* Disable hardware flow control:
|
||||||
* if no successful write since a defined time
|
* if no successful write since a defined time
|
||||||
* and if the last try was not the last successful write
|
* and if the last try was not the last successful write
|
||||||
|
@ -711,36 +719,22 @@ Mavlink::check_can_send(const int prio, const unsigned packet_len)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/* If the wait until transmit flag is on, only transmit after we've received messages.
|
|
||||||
Otherwise, transmit all the time. */
|
|
||||||
if (should_transmit()) {
|
|
||||||
_last_write_try_time = hrt_absolute_time();
|
|
||||||
|
|
||||||
/* check if there is space in the buffer, let it overflow else */
|
|
||||||
ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free);
|
|
||||||
|
|
||||||
if (buf_free >= (prio <= 0 ? packet_len : TX_BUFFER_GAP)) {
|
|
||||||
warnx("\t\t\tSKIP %i bytes prio %i", packet_len, (int)prio);
|
|
||||||
/* we don't want to send anything just in half, so return */
|
|
||||||
count_txerr();
|
|
||||||
count_txerrbytes(packet_len);
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
void
|
|
||||||
Mavlink::send_message(const uint8_t msgid, const void *msg, const int prio)
|
|
||||||
{
|
|
||||||
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
|
|
||||||
|
|
||||||
uint8_t payload_len = mavlink_message_lengths[msgid];
|
uint8_t payload_len = mavlink_message_lengths[msgid];
|
||||||
unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
|
unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
|
||||||
if (!check_can_send(prio, packet_len)) {
|
|
||||||
|
_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) {
|
||||||
|
warnx("SKIP msgid %i, %i bytes", msgid, packet_len);
|
||||||
|
/* no enough space in buffer to send */
|
||||||
|
count_txerr();
|
||||||
|
count_txerrbytes(packet_len);
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
|
||||||
|
|
||||||
/* header */
|
/* header */
|
||||||
buf[0] = MAVLINK_STX;
|
buf[0] = MAVLINK_STX;
|
||||||
buf[1] = payload_len;
|
buf[1] = payload_len;
|
||||||
|
@ -755,9 +749,9 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, const int prio)
|
||||||
/* checksum */
|
/* checksum */
|
||||||
uint16_t checksum;
|
uint16_t checksum;
|
||||||
crc_init(&checksum);
|
crc_init(&checksum);
|
||||||
crc_accumulate_buffer(&checksum, &buf[1], MAVLINK_CORE_HEADER_LEN + payload_len);
|
crc_accumulate_buffer(&checksum, (const char*) &buf[1], MAVLINK_CORE_HEADER_LEN + payload_len);
|
||||||
#if MAVLINK_CRC_EXTRA
|
#if MAVLINK_CRC_EXTRA
|
||||||
crc_accumulate(crc_extra, &checksum);
|
crc_accumulate(mavlink_message_crcs[msgid], &checksum);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF);
|
buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF);
|
||||||
|
@ -766,7 +760,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, const int prio)
|
||||||
/* send message to UART */
|
/* send message to UART */
|
||||||
ssize_t ret = write(_uart_fd, buf, packet_len);
|
ssize_t ret = write(_uart_fd, buf, packet_len);
|
||||||
|
|
||||||
if (ret != packet_len) {
|
if (ret != (int) packet_len) {
|
||||||
count_txerr();
|
count_txerr();
|
||||||
count_txerrbytes(packet_len);
|
count_txerrbytes(packet_len);
|
||||||
|
|
||||||
|
@ -783,7 +777,7 @@ Mavlink::handle_message(const mavlink_message_t *msg)
|
||||||
_mission_manager->handle_message(msg);
|
_mission_manager->handle_message(msg);
|
||||||
|
|
||||||
/* handle packet with parameter component */
|
/* handle packet with parameter component */
|
||||||
mavlink_pm_message_handler(_channel, msg);
|
_parameters_manager->handle_message(msg);
|
||||||
|
|
||||||
if (get_forwarding_on()) {
|
if (get_forwarding_on()) {
|
||||||
/* forward any messages to other mavlink instances */
|
/* forward any messages to other mavlink instances */
|
||||||
|
@ -791,163 +785,6 @@ 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
|
int
|
||||||
Mavlink::send_statustext_info(const char *string)
|
Mavlink::send_statustext_info(const char *string)
|
||||||
{
|
{
|
||||||
|
@ -1031,11 +868,17 @@ MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic)
|
||||||
return sub_new;
|
return sub_new;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
unsigned int
|
||||||
|
Mavlink::interval_from_rate(float rate)
|
||||||
|
{
|
||||||
|
return (rate > 0.0f) ? (1000000.0f / rate) : 0;
|
||||||
|
}
|
||||||
|
|
||||||
int
|
int
|
||||||
Mavlink::configure_stream(const char *stream_name, const float rate)
|
Mavlink::configure_stream(const char *stream_name, const float rate)
|
||||||
{
|
{
|
||||||
/* calculate interval in us, 0 means disabled stream */
|
/* 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 */
|
/* search if stream exists */
|
||||||
MavlinkStream *stream;
|
MavlinkStream *stream;
|
||||||
|
@ -1066,10 +909,9 @@ Mavlink::configure_stream(const char *stream_name, const float rate)
|
||||||
|
|
||||||
if (strcmp(stream_name, streams_list[i]->get_name()) == 0) {
|
if (strcmp(stream_name, streams_list[i]->get_name()) == 0) {
|
||||||
/* create new instance */
|
/* create new instance */
|
||||||
stream = streams_list[i]->new_instance();
|
stream = streams_list[i]->new_instance(this);
|
||||||
stream->set_channel(get_channel());
|
|
||||||
stream->set_interval(interval);
|
stream->set_interval(interval);
|
||||||
stream->subscribe(this);
|
stream->subscribe();
|
||||||
LL_APPEND(_streams, stream);
|
LL_APPEND(_streams, stream);
|
||||||
|
|
||||||
return OK;
|
return OK;
|
||||||
|
@ -1267,7 +1109,26 @@ Mavlink::pass_message(const mavlink_message_t *msg)
|
||||||
float
|
float
|
||||||
Mavlink::get_rate_mult()
|
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();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
_rate_mult = ((float)_datarate - const_rate) / (rate - const_rate);
|
||||||
}
|
}
|
||||||
|
|
||||||
int
|
int
|
||||||
|
@ -1390,6 +1251,8 @@ Mavlink::task_main(int argc, char *argv[])
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
update_rate_mult();
|
||||||
|
|
||||||
warnx("data rate: %d Bytes/s, port: %s, baud: %d", _datarate, _device_name, _baudrate);
|
warnx("data rate: %d Bytes/s, port: %s, baud: %d", _datarate, _device_name, _baudrate);
|
||||||
|
|
||||||
/* flush stdout in case MAVLink is about to take it over */
|
/* flush stdout in case MAVLink is about to take it over */
|
||||||
|
@ -1452,32 +1315,37 @@ Mavlink::task_main(int argc, char *argv[])
|
||||||
|
|
||||||
MavlinkCommandsStream commands_stream(this, _channel);
|
MavlinkCommandsStream commands_stream(this, _channel);
|
||||||
|
|
||||||
/* add default streams depending on mode and intervals depending on datarate */
|
/* add default streams depending on mode */
|
||||||
float rate_mult = get_rate_mult();
|
|
||||||
|
|
||||||
|
/* HEARTBEAT is constant rate stream, rate never adjusted */
|
||||||
configure_stream("HEARTBEAT", 1.0f);
|
configure_stream("HEARTBEAT", 1.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);
|
||||||
|
|
||||||
switch (_mode) {
|
switch (_mode) {
|
||||||
case MAVLINK_MODE_NORMAL:
|
case MAVLINK_MODE_NORMAL:
|
||||||
configure_stream("SYS_STATUS", 1.0f);
|
configure_stream("SYS_STATUS", 1.0f);
|
||||||
configure_stream("GPS_GLOBAL_ORIGIN", 0.5f);
|
configure_stream("GPS_GLOBAL_ORIGIN", 0.5f);
|
||||||
configure_stream("HIGHRES_IMU", 1.0f * rate_mult);
|
configure_stream("HIGHRES_IMU", 1.0f);
|
||||||
configure_stream("ATTITUDE", 10.0f * rate_mult);
|
configure_stream("ATTITUDE", 10.0f);
|
||||||
configure_stream("VFR_HUD", 8.0f * rate_mult);
|
configure_stream("VFR_HUD", 8.0f);
|
||||||
configure_stream("GPS_RAW_INT", 1.0f * rate_mult);
|
configure_stream("GPS_RAW_INT", 1.0f);
|
||||||
configure_stream("GLOBAL_POSITION_INT", 3.0f * rate_mult);
|
configure_stream("GLOBAL_POSITION_INT", 3.0f);
|
||||||
configure_stream("LOCAL_POSITION_NED", 3.0f * rate_mult);
|
configure_stream("LOCAL_POSITION_NED", 3.0f);
|
||||||
configure_stream("RC_CHANNELS_RAW", 1.0f * rate_mult);
|
configure_stream("RC_CHANNELS_RAW", 1.0f);
|
||||||
configure_stream("NAMED_VALUE_FLOAT", 1.0f * rate_mult);
|
configure_stream("NAMED_VALUE_FLOAT", 1.0f);
|
||||||
configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f * rate_mult);
|
configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f);
|
||||||
configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f * rate_mult);
|
configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f);
|
||||||
configure_stream("DISTANCE_SENSOR", 0.5f);
|
configure_stream("DISTANCE_SENSOR", 0.5f);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MAVLINK_MODE_CAMERA:
|
case MAVLINK_MODE_CAMERA:
|
||||||
configure_stream("SYS_STATUS", 1.0f);
|
configure_stream("SYS_STATUS", 1.0f);
|
||||||
configure_stream("ATTITUDE", 15.0f * rate_mult);
|
configure_stream("ATTITUDE", 15.0f);
|
||||||
configure_stream("GLOBAL_POSITION_INT", 15.0f * rate_mult);
|
configure_stream("GLOBAL_POSITION_INT", 15.0f);
|
||||||
configure_stream("CAMERA_CAPTURE", 1.0f);
|
configure_stream("CAMERA_CAPTURE", 1.0f);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
|
@ -1488,10 +1356,12 @@ Mavlink::task_main(int argc, char *argv[])
|
||||||
/* don't send parameters on startup without request */
|
/* don't send parameters on startup without request */
|
||||||
_mavlink_param_queue_index = param_count();
|
_mavlink_param_queue_index = param_count();
|
||||||
|
|
||||||
MavlinkRateLimiter fast_rate_limiter(30000.0f / rate_mult);
|
float base_rate_mult = _datarate / 1000.0f;
|
||||||
|
|
||||||
|
MavlinkRateLimiter fast_rate_limiter(30000.0f / base_rate_mult);
|
||||||
|
|
||||||
/* set main loop delay depending on data rate to minimize CPU overhead */
|
/* 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 / base_rate_mult;
|
||||||
|
|
||||||
/* now the instance is fully initialized and we can bump the instance count */
|
/* now the instance is fully initialized and we can bump the instance count */
|
||||||
LL_APPEND(_mavlink_instances, this);
|
LL_APPEND(_mavlink_instances, this);
|
||||||
|
@ -1543,18 +1413,9 @@ Mavlink::task_main(int argc, char *argv[])
|
||||||
}
|
}
|
||||||
|
|
||||||
if (fast_rate_limiter.check(t)) {
|
if (fast_rate_limiter.check(t)) {
|
||||||
|
_mission_manager->eventloop();
|
||||||
|
|
||||||
/* only send messages if they fit the buffer */
|
if (!mavlink_logbuffer_is_empty(&_logbuffer)) {
|
||||||
if (check_can_send(0, MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)) {
|
|
||||||
mavlink_pm_queued_send();
|
|
||||||
}
|
|
||||||
|
|
||||||
if (check_can_send(0, MAVLINK_MSG_ID_MISSION_ITEM_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)) {
|
|
||||||
_mission_manager->eventloop();
|
|
||||||
}
|
|
||||||
|
|
||||||
if (!mavlink_logbuffer_is_empty(&_logbuffer) &&
|
|
||||||
check_can_send(0, MAVLINK_MSG_ID_STATUSTEXT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)) {
|
|
||||||
struct mavlink_logmessage msg;
|
struct mavlink_logmessage msg;
|
||||||
int lb_ret = mavlink_logbuffer_read(&_logbuffer, &msg);
|
int lb_ret = mavlink_logbuffer_read(&_logbuffer, &msg);
|
||||||
|
|
||||||
|
@ -1630,6 +1491,7 @@ Mavlink::task_main(int argc, char *argv[])
|
||||||
}
|
}
|
||||||
|
|
||||||
delete _mission_manager;
|
delete _mission_manager;
|
||||||
|
delete _parameters_manager;
|
||||||
|
|
||||||
delete _subscribe_to_stream;
|
delete _subscribe_to_stream;
|
||||||
_subscribe_to_stream = nullptr;
|
_subscribe_to_stream = nullptr;
|
||||||
|
|
|
@ -58,7 +58,7 @@
|
||||||
#include "mavlink_stream.h"
|
#include "mavlink_stream.h"
|
||||||
#include "mavlink_messages.h"
|
#include "mavlink_messages.h"
|
||||||
#include "mavlink_mission.h"
|
#include "mavlink_mission.h"
|
||||||
|
#include "mavlink_parameters.h"
|
||||||
|
|
||||||
class Mavlink
|
class Mavlink
|
||||||
{
|
{
|
||||||
|
@ -160,11 +160,7 @@ public:
|
||||||
*/
|
*/
|
||||||
int set_hil_enabled(bool hil_enabled);
|
int set_hil_enabled(bool hil_enabled);
|
||||||
|
|
||||||
bool check_can_send(const int prio, const unsigned packet_len);
|
void send_message(const uint8_t msgid, const void *msg);
|
||||||
|
|
||||||
void send_message(const uint8_t msgid, const void *msg, const int prio);
|
|
||||||
|
|
||||||
void send_message(const mavlink_message_t *msg);
|
|
||||||
|
|
||||||
void handle_message(const mavlink_message_t *msg);
|
void handle_message(const mavlink_message_t *msg);
|
||||||
|
|
||||||
|
@ -285,6 +281,7 @@ private:
|
||||||
MavlinkStream *_streams;
|
MavlinkStream *_streams;
|
||||||
|
|
||||||
MavlinkMissionManager *_mission_manager;
|
MavlinkMissionManager *_mission_manager;
|
||||||
|
MavlinkParametersManager *_parameters_manager;
|
||||||
|
|
||||||
orb_advert_t _mission_pub;
|
orb_advert_t _mission_pub;
|
||||||
int _mission_result_sub;
|
int _mission_result_sub;
|
||||||
|
@ -305,6 +302,7 @@ private:
|
||||||
int _baudrate;
|
int _baudrate;
|
||||||
int _datarate; ///< data rate for normal streams (attitude, position, etc.)
|
int _datarate; ///< data rate for normal streams (attitude, position, etc.)
|
||||||
int _datarate_events; ///< data rate for params, waypoints, text messages
|
int _datarate_events; ///< data rate for params, waypoints, text messages
|
||||||
|
float _rate_mult;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* If the queue index is not at 0, the queue sending
|
* If the queue index is not at 0, the queue sending
|
||||||
|
@ -352,51 +350,12 @@ private:
|
||||||
perf_counter_t _loop_perf; /**< loop performance counter */
|
perf_counter_t _loop_perf; /**< loop performance counter */
|
||||||
perf_counter_t _txerr_perf; /**< TX error 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();
|
void mavlink_update_system();
|
||||||
|
|
||||||
int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb);
|
int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb);
|
||||||
|
|
||||||
|
static unsigned int interval_from_rate(float rate);
|
||||||
|
|
||||||
int configure_stream(const char *stream_name, const float rate);
|
int configure_stream(const char *stream_name, const float rate);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
@ -420,6 +379,11 @@ private:
|
||||||
|
|
||||||
void pass_message(const mavlink_message_t *msg);
|
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);
|
static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
|
File diff suppressed because it is too large
Load Diff
|
@ -46,10 +46,10 @@
|
||||||
class StreamListItem {
|
class StreamListItem {
|
||||||
|
|
||||||
public:
|
public:
|
||||||
MavlinkStream* (*new_instance)();
|
MavlinkStream* (*new_instance)(Mavlink *mavlink);
|
||||||
const char* (*get_name)();
|
const char* (*get_name)();
|
||||||
|
|
||||||
StreamListItem(MavlinkStream* (*inst)(), const char* (*name)()) :
|
StreamListItem(MavlinkStream* (*inst)(Mavlink *mavlink), const char* (*name)()) :
|
||||||
new_instance(inst),
|
new_instance(inst),
|
||||||
get_name(name) {};
|
get_name(name) {};
|
||||||
|
|
||||||
|
|
|
@ -157,15 +157,13 @@ MavlinkMissionManager::update_active_mission(int dataman_id, unsigned count, int
|
||||||
void
|
void
|
||||||
MavlinkMissionManager::send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t type)
|
MavlinkMissionManager::send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t type)
|
||||||
{
|
{
|
||||||
mavlink_message_t msg;
|
|
||||||
mavlink_mission_ack_t wpa;
|
mavlink_mission_ack_t wpa;
|
||||||
|
|
||||||
wpa.target_system = sysid;
|
wpa.target_system = sysid;
|
||||||
wpa.target_component = compid;
|
wpa.target_component = compid;
|
||||||
wpa.type = type;
|
wpa.type = type;
|
||||||
|
|
||||||
mavlink_msg_mission_ack_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpa);
|
_mavlink->send_message(MAVLINK_MSG_ID_MISSION_ACK, &wpa);
|
||||||
_mavlink->send_message(&msg);
|
|
||||||
|
|
||||||
if (_verbose) { warnx("WPM: Send MISSION_ACK type %u to ID %u", wpa.type, wpa.target_system); }
|
if (_verbose) { warnx("WPM: Send MISSION_ACK type %u to ID %u", wpa.type, wpa.target_system); }
|
||||||
}
|
}
|
||||||
|
@ -175,13 +173,11 @@ void
|
||||||
MavlinkMissionManager::send_mission_current(uint16_t seq)
|
MavlinkMissionManager::send_mission_current(uint16_t seq)
|
||||||
{
|
{
|
||||||
if (seq < _count) {
|
if (seq < _count) {
|
||||||
mavlink_message_t msg;
|
|
||||||
mavlink_mission_current_t wpc;
|
mavlink_mission_current_t wpc;
|
||||||
|
|
||||||
wpc.seq = seq;
|
wpc.seq = seq;
|
||||||
|
|
||||||
mavlink_msg_mission_current_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpc);
|
_mavlink->send_message(MAVLINK_MSG_ID_MISSION_CURRENT, &wpc);
|
||||||
_mavlink->send_message(&msg);
|
|
||||||
|
|
||||||
} else if (seq == 0 && _count == 0) {
|
} else if (seq == 0 && _count == 0) {
|
||||||
/* don't broadcast if no WPs */
|
/* don't broadcast if no WPs */
|
||||||
|
@ -199,15 +195,13 @@ MavlinkMissionManager::send_mission_count(uint8_t sysid, uint8_t compid, uint16_
|
||||||
{
|
{
|
||||||
_time_last_sent = hrt_absolute_time();
|
_time_last_sent = hrt_absolute_time();
|
||||||
|
|
||||||
mavlink_message_t msg;
|
|
||||||
mavlink_mission_count_t wpc;
|
mavlink_mission_count_t wpc;
|
||||||
|
|
||||||
wpc.target_system = sysid;
|
wpc.target_system = sysid;
|
||||||
wpc.target_component = compid;
|
wpc.target_component = compid;
|
||||||
wpc.count = _count;
|
wpc.count = _count;
|
||||||
|
|
||||||
mavlink_msg_mission_count_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpc);
|
_mavlink->send_message(MAVLINK_MSG_ID_MISSION_COUNT, &wpc);
|
||||||
_mavlink->send_message(&msg);
|
|
||||||
|
|
||||||
if (_verbose) { warnx("WPM: Send MISSION_COUNT %u to ID %u", wpc.count, wpc.target_system); }
|
if (_verbose) { warnx("WPM: Send MISSION_COUNT %u to ID %u", wpc.count, wpc.target_system); }
|
||||||
}
|
}
|
||||||
|
@ -226,13 +220,12 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t
|
||||||
mavlink_mission_item_t wp;
|
mavlink_mission_item_t wp;
|
||||||
format_mavlink_mission_item(&mission_item, &wp);
|
format_mavlink_mission_item(&mission_item, &wp);
|
||||||
|
|
||||||
mavlink_message_t msg;
|
|
||||||
wp.target_system = sysid;
|
wp.target_system = sysid;
|
||||||
wp.target_component = compid;
|
wp.target_component = compid;
|
||||||
wp.seq = seq;
|
wp.seq = seq;
|
||||||
wp.current = (_current_seq == seq) ? 1 : 0;
|
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); }
|
if (_verbose) { warnx("WPM: Send MISSION_ITEM seq %u to ID %u", wp.seq, wp.target_system); }
|
||||||
|
|
||||||
|
@ -251,13 +244,12 @@ MavlinkMissionManager::send_mission_request(uint8_t sysid, uint8_t compid, uint1
|
||||||
if (seq < _max_count) {
|
if (seq < _max_count) {
|
||||||
_time_last_sent = hrt_absolute_time();
|
_time_last_sent = hrt_absolute_time();
|
||||||
|
|
||||||
mavlink_message_t msg;
|
|
||||||
mavlink_mission_request_t wpr;
|
mavlink_mission_request_t wpr;
|
||||||
wpr.target_system = sysid;
|
wpr.target_system = sysid;
|
||||||
wpr.target_component = compid;
|
wpr.target_component = compid;
|
||||||
wpr.seq = seq;
|
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); }
|
if (_verbose) { warnx("WPM: Send MISSION_REQUEST seq %u to ID %u", wpr.seq, wpr.target_system); }
|
||||||
|
|
||||||
|
@ -272,13 +264,11 @@ MavlinkMissionManager::send_mission_request(uint8_t sysid, uint8_t compid, uint1
|
||||||
void
|
void
|
||||||
MavlinkMissionManager::send_mission_item_reached(uint16_t seq)
|
MavlinkMissionManager::send_mission_item_reached(uint16_t seq)
|
||||||
{
|
{
|
||||||
mavlink_message_t msg;
|
|
||||||
mavlink_mission_item_reached_t wp_reached;
|
mavlink_mission_item_reached_t wp_reached;
|
||||||
|
|
||||||
wp_reached.seq = seq;
|
wp_reached.seq = seq;
|
||||||
|
|
||||||
mavlink_msg_mission_item_reached_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wp_reached);
|
_mavlink->send_message(MAVLINK_MSG_ID_MISSION_ITEM_REACHED, &wp_reached);
|
||||||
_mavlink->send_message(&msg);
|
|
||||||
|
|
||||||
if (_verbose) { warnx("WPM: Send MISSION_ITEM_REACHED reached_seq %u", wp_reached.seq); }
|
if (_verbose) { warnx("WPM: Send MISSION_ITEM_REACHED reached_seq %u", wp_reached.seq); }
|
||||||
}
|
}
|
||||||
|
|
|
@ -38,11 +38,11 @@
|
||||||
* @author Anton Babushkin <anton.babushkin@me.com>
|
* @author Anton Babushkin <anton.babushkin@me.com>
|
||||||
*/
|
*/
|
||||||
|
|
||||||
//#include <stdlib.h>
|
#include <stdio.h>
|
||||||
|
|
||||||
#include "mavlink_parameters.h"
|
#include "mavlink_parameters.h"
|
||||||
|
#include "mavlink_main.h"
|
||||||
|
|
||||||
explicit
|
|
||||||
MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : MavlinkStream(mavlink),
|
MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : MavlinkStream(mavlink),
|
||||||
_send_all_index(-1)
|
_send_all_index(-1)
|
||||||
{
|
{
|
||||||
|
@ -51,7 +51,7 @@ MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : MavlinkSt
|
||||||
unsigned
|
unsigned
|
||||||
MavlinkParametersManager::get_size()
|
MavlinkParametersManager::get_size()
|
||||||
{
|
{
|
||||||
if (_send_all_index() >= 0) {
|
if (_send_all_index >= 0) {
|
||||||
return MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
|
return MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
|
|
|
@ -40,6 +40,9 @@
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
|
#include <systemlib/param/param.h>
|
||||||
|
|
||||||
|
#include "mavlink_bridge_header.h"
|
||||||
#include "mavlink_stream.h"
|
#include "mavlink_stream.h"
|
||||||
|
|
||||||
class MavlinkParametersManager : public MavlinkStream
|
class MavlinkParametersManager : public MavlinkStream
|
||||||
|
|
|
@ -40,6 +40,7 @@ SRCS += mavlink_main.cpp \
|
||||||
mavlink.c \
|
mavlink.c \
|
||||||
mavlink_receiver.cpp \
|
mavlink_receiver.cpp \
|
||||||
mavlink_mission.cpp \
|
mavlink_mission.cpp \
|
||||||
|
mavlink_parameters.cpp \
|
||||||
mavlink_orb_subscription.cpp \
|
mavlink_orb_subscription.cpp \
|
||||||
mavlink_messages.cpp \
|
mavlink_messages.cpp \
|
||||||
mavlink_stream.cpp \
|
mavlink_stream.cpp \
|
||||||
|
|
Loading…
Reference in New Issue