aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--src/modules/mavlink/mavlink_ftp.cpp4
-rw-r--r--src/modules/mavlink/mavlink_ftp.h33
-rw-r--r--src/modules/mavlink/mavlink_main.cpp62
-rw-r--r--src/modules/mavlink/mavlink_receiver.cpp2
4 files changed, 80 insertions, 21 deletions
diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp
index d4d659d91..8c29043e0 100644
--- a/src/modules/mavlink/mavlink_ftp.cpp
+++ b/src/modules/mavlink/mavlink_ftp.cpp
@@ -64,7 +64,7 @@ MavlinkFTP::MavlinkFTP()
}
void
-MavlinkFTP::handle_message(mavlink_message_t *msg, mavlink_channel_t channel)
+MavlinkFTP::handle_message(Mavlink* mavlink, mavlink_message_t *msg)
{
// get a free request
auto req = _dqFree();
@@ -73,7 +73,7 @@ MavlinkFTP::handle_message(mavlink_message_t *msg, mavlink_channel_t channel)
if (req != nullptr) {
// decode the request
- if (req->decode(msg, channel)) {
+ if (req->decode(mavlink, msg)) {
// and queue it for the worker
work_queue(LPWORK, &req->work, &MavlinkFTP::_workerTrampoline, req, 0);
diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h
index f68dab98d..0869a5fdb 100644
--- a/src/modules/mavlink/mavlink_ftp.h
+++ b/src/modules/mavlink/mavlink_ftp.h
@@ -64,8 +64,8 @@ public:
static MavlinkFTP *getServer();
// static interface
- void handle_message(mavlink_message_t *msg,
- mavlink_channel_t channel);
+ void handle_message(Mavlink* mavlink,
+ mavlink_message_t *msg);
private:
@@ -145,9 +145,9 @@ private:
work_s work;
};
- bool decode(mavlink_message_t *fromMessage, mavlink_channel_t fromChannel) {
+ bool decode(Mavlink *mavlink, mavlink_message_t *fromMessage) {
if (fromMessage->msgid == MAVLINK_MSG_ID_ENCAPSULATED_DATA) {
- _channel = fromChannel;
+ _mavlink = mavlink;
mavlink_msg_encapsulated_data_decode(fromMessage, &_message);
return true;
}
@@ -155,7 +155,26 @@ private:
}
void reply() {
- mavlink_msg_encapsulated_data_send(_channel, sequence(), rawData());
+
+ // XXX the proper way would be an IOCTL / uORB call, rather than exploiting the
+ // flat memory architecture, as we're operating between threads here.
+ mavlink_message_t msg;
+ msg.checksum = 0;
+ unsigned len = mavlink_msg_encapsulated_data_pack_chan(_mavlink->get_system_id(), _mavlink->get_component_id(),
+ _mavlink->get_channel(), &msg, sequence(), rawData());
+ // unsigned len = mavlink_msg_system_time_pack_chan(_mavlink->get_system_id(), _mavlink->get_component_id(),
+ // _mavlink->get_channel(), &msg, 255, 255);
+
+ if (!_mavlink->message_buffer_write(&msg, len+2)) {
+ warnx("FTP TX ERR");
+ } else {
+ warnx("wrote: sys: %d, comp: %d, chan: %d, len: %d, checksum: %d",
+ _mavlink->get_system_id(),
+ _mavlink->get_component_id(),
+ _mavlink->get_channel(),
+ len,
+ msg.checksum);
+ }
}
uint8_t *rawData() { return &_message.data[0]; }
@@ -163,12 +182,12 @@ private:
uint8_t *requestData() { return &(header()->data[0]); }
unsigned dataSize() { return header()->size + sizeof(RequestHeader); }
uint16_t sequence() const { return _message.seqnr; }
- mavlink_channel_t &channel() { return _channel; }
+ mavlink_channel_t channel() { return _mavlink->get_channel(); }
char *dataAsCString();
private:
- mavlink_channel_t _channel;
+ Mavlink *_mavlink;
mavlink_encapsulated_data_t _message;
};
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index e300be074..066d25bf6 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -83,6 +83,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
@@ -114,6 +118,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) {
@@ -198,7 +203,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];
}
@@ -230,6 +235,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),
@@ -453,7 +459,7 @@ Mavlink::get_instance_id()
return _instance_id;
}
-mavlink_channel_t
+const mavlink_channel_t
Mavlink::get_channel()
{
return _channel;
@@ -536,6 +542,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 */
@@ -1649,11 +1665,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
@@ -1781,7 +1807,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);
@@ -1837,6 +1863,10 @@ Mavlink::task_main(int argc, char *argv[])
_wait_to_transmit = true;
break;
+ case 'x':
+ _ftp_on = true;
+ break;
+
default:
err_flag = true;
break;
@@ -1902,9 +1932,9 @@ 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) {
+ if (_passing_on || _ftp_on) {
/* initialize message buffer if multiplexing is on */
- if (OK != message_buffer_init(500)) {
+ if (OK != message_buffer_init(2 * MAVLINK_MAX_PACKET_LEN)) {
errx(1, "can't allocate message buffer, exiting");
}
@@ -2064,8 +2094,8 @@ 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;
@@ -2076,11 +2106,21 @@ Mavlink::task_main(int argc, char *argv[])
pthread_mutex_unlock(&_message_buffer_mutex);
if (available > 0) {
+
+ // int oldseq = mavlink_get_channel_status(get_channel())->current_tx_seq;
+
+ const mavlink_message_t* msg = (const mavlink_message_t*)read_ptr;
/* write first part of buffer */
- _mavlink_resend_uart(_channel, (const mavlink_message_t*)read_ptr);
+ _mavlink_resend_uart(_channel, msg);
+
+ // mavlink_get_channel_status(get_channel())->current_tx_seq = oldseq;
+ // mavlink_msg_system_time_send(get_channel(), 255, 255);
+
message_buffer_mark_read(available);
+
/* write second part of buffer if there is some */
+ // XXX this doesn't quite work, as the resend UART call assumes a continous block
if (is_part) {
/* guard get ptr by mutex */
pthread_mutex_lock(&_message_buffer_mutex);
@@ -2139,7 +2179,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);
}
@@ -2281,7 +2321,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[])
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 4f2c4ca85..4a244815a 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -154,7 +154,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
break;
case MAVLINK_MSG_ID_ENCAPSULATED_DATA:
- MavlinkFTP::getServer()->handle_message(msg, _mavlink->get_channel());
+ MavlinkFTP::getServer()->handle_message(_mavlink, msg);
break;
default:
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-22 10:31:51 +0000