forked from Archive/PX4-Autopilot
mavlink: implemented multicasting between mavlink instances (two options: forwarding: forward received messages from self to other mavlink instances, passing: send out messages received from other mavlink intances over serial
This commit is contained in:
parent
ed7b97c020
commit
f17c0b1335
|
@ -207,10 +207,15 @@ Mavlink::Mavlink() :
|
|||
_subscriptions(nullptr),
|
||||
_streams(nullptr),
|
||||
_mission_pub(-1),
|
||||
_verbose(false),
|
||||
_forwarding_on(false),
|
||||
_passing_on(false),
|
||||
_uart_fd(-1),
|
||||
_mavlink_param_queue_index(0),
|
||||
_subscribe_to_stream(nullptr),
|
||||
_subscribe_to_stream_rate(0.0f),
|
||||
_flow_control_enabled(true),
|
||||
_message_buffer({}),
|
||||
|
||||
/* performance counters */
|
||||
_loop_perf(perf_alloc(PC_ELAPSED, "mavlink"))
|
||||
|
@ -261,7 +266,6 @@ Mavlink::Mavlink() :
|
|||
errx(1, "instance ID is out of range");
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
Mavlink::~Mavlink()
|
||||
|
@ -394,6 +398,20 @@ Mavlink::instance_exists(const char *device_name, Mavlink *self)
|
|||
return false;
|
||||
}
|
||||
|
||||
void
|
||||
Mavlink::forward_message(mavlink_message_t *msg, Mavlink *self)
|
||||
{
|
||||
Mavlink *inst = ::_mavlink_instances;
|
||||
|
||||
while (inst != nullptr) {
|
||||
/* don't broadcast to itself */
|
||||
if (inst != self) {
|
||||
inst->pass_message(msg);
|
||||
}
|
||||
inst = inst->next;
|
||||
}
|
||||
}
|
||||
|
||||
int
|
||||
Mavlink::get_uart_fd(unsigned index)
|
||||
{
|
||||
|
@ -1616,6 +1634,125 @@ 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;
|
||||
}
|
||||
|
||||
void
|
||||
Mavlink::message_buffer_destroy()
|
||||
{
|
||||
_message_buffer.size = 0;
|
||||
_message_buffer.write_ptr = 0;
|
||||
_message_buffer.read_ptr = 0;
|
||||
free(_message_buffer.data);
|
||||
}
|
||||
|
||||
int
|
||||
Mavlink::message_buffer_count()
|
||||
{
|
||||
int n = _message_buffer.write_ptr - _message_buffer.read_ptr;
|
||||
|
||||
if (n < 0) {
|
||||
n += _message_buffer.size;
|
||||
}
|
||||
|
||||
return n;
|
||||
}
|
||||
|
||||
int
|
||||
Mavlink::message_buffer_is_empty()
|
||||
{
|
||||
return _message_buffer.read_ptr == _message_buffer.write_ptr;
|
||||
}
|
||||
|
||||
|
||||
bool
|
||||
Mavlink::message_buffer_write(void *ptr, int size)
|
||||
{
|
||||
// bytes available to write
|
||||
int available = _message_buffer.read_ptr - _message_buffer.write_ptr - 1;
|
||||
|
||||
if (available < 0) {
|
||||
available += _message_buffer.size;
|
||||
}
|
||||
|
||||
if (size > available) {
|
||||
// buffer overflow
|
||||
return false;
|
||||
}
|
||||
|
||||
char *c = (char *) ptr;
|
||||
int n = _message_buffer.size - _message_buffer.write_ptr; // bytes to end of the buffer
|
||||
|
||||
if (n < size) {
|
||||
// message goes over end of the buffer
|
||||
memcpy(&(_message_buffer.data[_message_buffer.write_ptr]), c, n);
|
||||
_message_buffer.write_ptr = 0;
|
||||
|
||||
} else {
|
||||
n = 0;
|
||||
}
|
||||
|
||||
// now: n = bytes already written
|
||||
int p = size - n; // number of bytes to write
|
||||
memcpy(&(_message_buffer.data[_message_buffer.write_ptr]), &(c[n]), p);
|
||||
_message_buffer.write_ptr = (_message_buffer.write_ptr + p) % _message_buffer.size;
|
||||
return true;
|
||||
}
|
||||
|
||||
int
|
||||
Mavlink::message_buffer_get_ptr(void **ptr, bool *is_part)
|
||||
{
|
||||
// bytes available to read
|
||||
int available = _message_buffer.write_ptr - _message_buffer.read_ptr;
|
||||
|
||||
if (available == 0) {
|
||||
return 0; // buffer is empty
|
||||
}
|
||||
|
||||
int n = 0;
|
||||
|
||||
if (available > 0) {
|
||||
// read pointer is before write pointer, all available bytes can be read
|
||||
n = available;
|
||||
*is_part = false;
|
||||
|
||||
} else {
|
||||
// read pointer is after write pointer, read bytes from read_ptr to end of the buffer
|
||||
n = _message_buffer.size - _message_buffer.read_ptr;
|
||||
*is_part = _message_buffer.write_ptr > 0;
|
||||
}
|
||||
|
||||
*ptr = &(_message_buffer.data[_message_buffer.read_ptr]);
|
||||
return n;
|
||||
}
|
||||
|
||||
void
|
||||
Mavlink::message_buffer_mark_read(int n)
|
||||
{
|
||||
_message_buffer.read_ptr = (_message_buffer.read_ptr + n) % _message_buffer.size;
|
||||
}
|
||||
|
||||
void
|
||||
Mavlink::pass_message(mavlink_message_t *msg)
|
||||
{
|
||||
if (_passing_on) {
|
||||
/* size is 8 bytes plus variable payload */
|
||||
int size = MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len;
|
||||
pthread_mutex_lock(&_message_buffer_mutex);
|
||||
message_buffer_write(msg, size);
|
||||
pthread_mutex_unlock(&_message_buffer_mutex);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
int
|
||||
Mavlink::task_main(int argc, char *argv[])
|
||||
{
|
||||
|
@ -1632,7 +1769,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:v")) != EOF) {
|
||||
while ((ch = getopt(argc, argv, "b:r:d:m:fpv")) != EOF) {
|
||||
switch (ch) {
|
||||
case 'b':
|
||||
_baudrate = strtoul(optarg, NULL, 10);
|
||||
|
@ -1672,6 +1809,14 @@ Mavlink::task_main(int argc, char *argv[])
|
|||
|
||||
break;
|
||||
|
||||
case 'f':
|
||||
_forwarding_on = true;
|
||||
break;
|
||||
|
||||
case 'p':
|
||||
_passing_on = true;
|
||||
break;
|
||||
|
||||
case 'v':
|
||||
_verbose = true;
|
||||
break;
|
||||
|
@ -1740,6 +1885,17 @@ Mavlink::task_main(int argc, char *argv[])
|
|||
/* initialize mavlink text message buffering */
|
||||
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(500)) {
|
||||
errx(1, "can't allocate message buffer, exiting");
|
||||
}
|
||||
|
||||
/* initialize message buffer mutex */
|
||||
pthread_mutex_init(&_message_buffer_mutex, NULL);
|
||||
}
|
||||
|
||||
/* create the device node that's used for sending text log messages, etc. */
|
||||
register_driver(MAVLINK_LOG_DEVICE, &fops, 0666, NULL);
|
||||
|
||||
|
@ -1884,6 +2040,37 @@ Mavlink::task_main(int argc, char *argv[])
|
|||
}
|
||||
}
|
||||
|
||||
/* pass messages from other UARTs */
|
||||
if (_passing_on) {
|
||||
|
||||
bool is_part;
|
||||
void *read_ptr;
|
||||
|
||||
/* guard get ptr by mutex */
|
||||
pthread_mutex_lock(&_message_buffer_mutex);
|
||||
int available = message_buffer_get_ptr(&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);
|
||||
|
||||
/* 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);
|
||||
message_buffer_mark_read(available);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
perf_end(_loop_perf);
|
||||
}
|
||||
|
||||
|
@ -1928,6 +2115,10 @@ Mavlink::task_main(int argc, char *argv[])
|
|||
/* close mavlink logging device */
|
||||
close(_mavlink_fd);
|
||||
|
||||
if (_passing_on) {
|
||||
message_buffer_destroy();
|
||||
pthread_mutex_destroy(&_message_buffer_mutex);
|
||||
}
|
||||
/* destroy log buffer */
|
||||
mavlink_logbuffer_destroy(&_logbuffer);
|
||||
|
||||
|
@ -2067,7 +2258,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] [-v]");
|
||||
warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate] [-r rate] [-m mode] [-s stream] [-f] [-p] [-v]");
|
||||
}
|
||||
|
||||
int mavlink_main(int argc, char *argv[])
|
||||
|
|
|
@ -138,6 +138,8 @@ public:
|
|||
|
||||
static bool instance_exists(const char *device_name, Mavlink *self);
|
||||
|
||||
static void forward_message(mavlink_message_t *msg, Mavlink *self);
|
||||
|
||||
static int get_uart_fd(unsigned index);
|
||||
|
||||
int get_uart_fd();
|
||||
|
@ -153,10 +155,12 @@ public:
|
|||
void set_mode(enum MAVLINK_MODE);
|
||||
enum MAVLINK_MODE get_mode() { return _mode; }
|
||||
|
||||
bool get_hil_enabled() { return _hil_enabled; };
|
||||
bool get_hil_enabled() { return _hil_enabled; }
|
||||
|
||||
bool get_flow_control_enabled() { return _flow_control_enabled; }
|
||||
|
||||
bool get_forwarding_on() { return _forwarding_on; }
|
||||
|
||||
/**
|
||||
* Handle waypoint related messages.
|
||||
*/
|
||||
|
@ -234,6 +238,8 @@ private:
|
|||
mavlink_wpm_storage *_wpm;
|
||||
|
||||
bool _verbose;
|
||||
bool _forwarding_on;
|
||||
bool _passing_on;
|
||||
int _uart_fd;
|
||||
int _baudrate;
|
||||
int _datarate;
|
||||
|
@ -252,6 +258,16 @@ private:
|
|||
|
||||
bool _flow_control_enabled;
|
||||
|
||||
struct mavlink_message_buffer {
|
||||
int write_ptr;
|
||||
int read_ptr;
|
||||
int size;
|
||||
char *data;
|
||||
};
|
||||
mavlink_message_buffer _message_buffer;
|
||||
|
||||
pthread_mutex_t _message_buffer_mutex;
|
||||
|
||||
/**
|
||||
* Send one parameter.
|
||||
*
|
||||
|
@ -315,6 +331,22 @@ private:
|
|||
int configure_stream(const char *stream_name, const float rate);
|
||||
void configure_stream_threadsafe(const char *stream_name, const float rate);
|
||||
|
||||
int message_buffer_init(int size);
|
||||
|
||||
void message_buffer_destroy();
|
||||
|
||||
int message_buffer_count();
|
||||
|
||||
int message_buffer_is_empty();
|
||||
|
||||
bool message_buffer_write(void *ptr, int size);
|
||||
|
||||
int message_buffer_get_ptr(void **ptr, bool *is_part);
|
||||
|
||||
void message_buffer_mark_read(int n);
|
||||
|
||||
void pass_message(mavlink_message_t *msg);
|
||||
|
||||
static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg);
|
||||
|
||||
/**
|
||||
|
|
|
@ -1265,11 +1265,13 @@ protected:
|
|||
|| status->arming_state == ARMING_STATE_ARMED_ERROR) {
|
||||
|
||||
/* send camera capture on */
|
||||
mavlink_msg_command_long_send(_channel, 42, 30, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0);
|
||||
/* XXX TODO: get param for system ID */
|
||||
mavlink_msg_command_long_send(_channel, 42, 250, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0);
|
||||
|
||||
} else {
|
||||
/* send camera capture off */
|
||||
mavlink_msg_command_long_send(_channel, 42, 30, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0);
|
||||
/* XXX TODO: get param for system ID */
|
||||
mavlink_msg_command_long_send(_channel, 42, 250, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
|
|
@ -913,6 +913,11 @@ MavlinkReceiver::receive_thread(void *arg)
|
|||
|
||||
/* handle packet with parameter component */
|
||||
_mavlink->mavlink_pm_message_handler(_mavlink->get_channel(), &msg);
|
||||
|
||||
if (_mavlink->get_forwarding_on()) {
|
||||
/* forward any messages to other mavlink instances */
|
||||
Mavlink::forward_message(&msg, _mavlink);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue