aboutsummaryrefslogtreecommitdiff
path: root/src/modules/mavlink/mavlink_main.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/mavlink/mavlink_main.cpp')
-rw-r--r--src/modules/mavlink/mavlink_main.cpp135
1 files changed, 95 insertions, 40 deletions
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index f8a31a4ad..3c0460668 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -80,6 +80,10 @@
#include "mavlink_rate_limiter.h"
#include "mavlink_commands.h"
+#ifndef MAVLINK_CRC_EXTRA
+ #error MAVLINK_CRC_EXTRA has to be defined on PX4 systems
+#endif
+
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
@@ -113,6 +117,7 @@ static uint64_t last_write_try_times[6] = {0};
void
mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length)
{
+
Mavlink *instance;
switch (channel) {
@@ -197,7 +202,7 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
ssize_t ret = write(uart, ch, desired);
if (ret != desired) {
- warnx("TX FAIL");
+ // XXX overflow perf
} else {
last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel];
}
@@ -227,6 +232,7 @@ Mavlink::Mavlink() :
_verbose(false),
_forwarding_on(false),
_passing_on(false),
+ _ftp_on(false),
_uart_fd(-1),
_mavlink_param_queue_index(0),
_subscribe_to_stream(nullptr),
@@ -455,7 +461,7 @@ Mavlink::get_instance_id()
return _instance_id;
}
-mavlink_channel_t
+const mavlink_channel_t
Mavlink::get_channel()
{
return _channel;
@@ -532,6 +538,16 @@ void Mavlink::mavlink_update_system(void)
_use_hil_gps = (bool)use_hil_gps;
}
+int Mavlink::get_system_id()
+{
+ return mavlink_system.sysid;
+}
+
+int Mavlink::get_component_id()
+{
+ return mavlink_system.compid;
+}
+
int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb)
{
/* process baud rate */
@@ -950,32 +966,36 @@ Mavlink::configure_stream(const char *stream_name, const float rate)
/* delete stream */
LL_DELETE(_streams, stream);
delete stream;
+ warnx("deleted stream %s", stream->get_name());
}
return OK;
}
}
- if (interval > 0) {
- /* search for stream with specified name in supported streams list */
- for (unsigned int i = 0; streams_list[i] != nullptr; i++) {
+ if (interval == 0) {
+ /* stream was not active and is requested to be disabled, do nothing */
+ return OK;
+ }
- 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->set_interval(interval);
- stream->subscribe(this);
- LL_APPEND(_streams, stream);
- return OK;
- }
- }
+ /* search for stream with specified name in supported streams list */
+ for (unsigned int i = 0; streams_list[i] != nullptr; i++) {
- } else {
- /* stream not found, nothing to disable */
- return OK;
+ 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->set_interval(interval);
+ stream->subscribe(this);
+ LL_APPEND(_streams, stream);
+
+ return OK;
+ }
}
+ /* if we reach here, the stream list does not contain the stream */
+ warnx("stream %s not found", stream_name);
+
return ERROR;
}
@@ -1010,11 +1030,21 @@ Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate)
int
Mavlink::message_buffer_init(int size)
{
+
_message_buffer.size = size;
_message_buffer.write_ptr = 0;
_message_buffer.read_ptr = 0;
_message_buffer.data = (char*)malloc(_message_buffer.size);
- return (_message_buffer.data == 0) ? ERROR : OK;
+
+ int ret;
+ if (_message_buffer.data == 0) {
+ ret = ERROR;
+ _message_buffer.size = 0;
+ } else {
+ ret = OK;
+ }
+
+ return ret;
}
void
@@ -1146,7 +1176,7 @@ Mavlink::task_main(int argc, char *argv[])
* set error flag instead */
bool err_flag = false;
- while ((ch = getopt(argc, argv, "b:r:d:m:fpvw")) != EOF) {
+ while ((ch = getopt(argc, argv, "b:r:d:m:fpvwx")) != EOF) {
switch (ch) {
case 'b':
_baudrate = strtoul(optarg, NULL, 10);
@@ -1202,6 +1232,10 @@ Mavlink::task_main(int argc, char *argv[])
_wait_to_transmit = true;
break;
+ case 'x':
+ _ftp_on = true;
+ break;
+
default:
err_flag = true;
break;
@@ -1265,9 +1299,12 @@ Mavlink::task_main(int argc, char *argv[])
mavlink_logbuffer_init(&_logbuffer, 5);
/* if we are passing on mavlink messages, we need to prepare a buffer for this instance */
- if (_passing_on) {
- /* initialize message buffer if multiplexing is on */
- if (OK != message_buffer_init(300)) {
+ if (_passing_on || _ftp_on) {
+ /* initialize message buffer if multiplexing is on or its needed for FTP.
+ * 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)) {
errx(1, "can't allocate message buffer, exiting");
}
@@ -1408,32 +1445,50 @@ Mavlink::task_main(int argc, char *argv[])
}
}
- /* pass messages from other UARTs */
- if (_passing_on) {
+ /* pass messages from other UARTs or FTP worker */
+ if (_passing_on || _ftp_on) {
bool is_part;
- void *read_ptr;
+ uint8_t *read_ptr;
+ uint8_t *write_ptr;
- /* guard get ptr by mutex */
pthread_mutex_lock(&_message_buffer_mutex);
- int available = message_buffer_get_ptr(&read_ptr, &is_part);
+ int available = message_buffer_get_ptr((void**)&read_ptr, &is_part);
pthread_mutex_unlock(&_message_buffer_mutex);
if (available > 0) {
- /* write first part of buffer */
- _mavlink_resend_uart(_channel, (const mavlink_message_t*)read_ptr);
- message_buffer_mark_read(available);
+ // Reconstruct message from buffer
+
+ mavlink_message_t msg;
+ write_ptr = (uint8_t*)&msg;
+
+ // Pull a single message from the buffer
+ int read_count = available;
+ if (read_count > sizeof(mavlink_message_t)) {
+ read_count = sizeof(mavlink_message_t);
+ }
+
+ memcpy(write_ptr, read_ptr, read_count);
+
+ // We hold the mutex until after we complete the second part of the buffer. If we don't
+ // we may end up breaking the empty slot overflow detection semantics when we mark the
+ // possibly partial read below.
+ pthread_mutex_lock(&_message_buffer_mutex);
+
+ message_buffer_mark_read(read_count);
/* write second part of buffer if there is some */
- if (is_part) {
- /* guard get ptr by mutex */
- pthread_mutex_lock(&_message_buffer_mutex);
- available = message_buffer_get_ptr(&read_ptr, &is_part);
- pthread_mutex_unlock(&_message_buffer_mutex);
-
- _mavlink_resend_uart(_channel, (const mavlink_message_t*)read_ptr);
+ if (is_part && read_count < sizeof(mavlink_message_t)) {
+ write_ptr += read_count;
+ available = message_buffer_get_ptr((void**)&read_ptr, &is_part);
+ read_count = sizeof(mavlink_message_t) - read_count;
+ memcpy(write_ptr, read_ptr, read_count);
message_buffer_mark_read(available);
}
+
+ pthread_mutex_unlock(&_message_buffer_mutex);
+
+ _mavlink_resend_uart(_channel, &msg);
}
}
@@ -1483,7 +1538,7 @@ Mavlink::task_main(int argc, char *argv[])
/* close mavlink logging device */
close(_mavlink_fd);
- if (_passing_on) {
+ if (_passing_on || _ftp_on) {
message_buffer_destroy();
pthread_mutex_destroy(&_message_buffer_mutex);
}
@@ -1625,7 +1680,7 @@ Mavlink::stream(int argc, char *argv[])
static void usage()
{
- warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate] [-r rate] [-m mode] [-s stream] [-f] [-p] [-v] [-w]");
+ warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate]\n\t[-r rate][-m mode] [-s stream] [-f] [-p] [-v] [-w] [-x]");
}
int mavlink_main(int argc, char *argv[])
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-19 20:34:56 +0000