fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-12 20:17:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

msg ROS2 compatibility, microdds_client improvements (timesync, reduced code size, added topics, etc), fastrtps purge

Browse Source 
- update all msgs to be directly compatible with ROS2
 - microdds_client improvements
   - timesync
   - reduced code size
   - add to most default builds if we can afford it
   - lots of other little changes
 - purge fastrtps (I tried to save this multiple times, but kept hitting roadblocks)
This commit is contained in:
Daniel Agar
2022-10-19 19:36:47 -04:00
committed by GitHub
parent e211e0ca0e
commit cea185268e
318 changed files with 1948 additions and 8939 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/drv_hrt.h
+6 -6
View File
@@ -139,7 +139,7 @@ static inline hrt_abstime ts_to_abstime(const struct timespec *ts)
hrt_abstime result;
result = (hrt_abstime)(ts->tv_sec) * 1000000;
result += ts->tv_nsec / 1000;
result += (hrt_abstime)(ts->tv_nsec / 1000);
return result;
}
@@ -149,9 +149,9 @@ static inline hrt_abstime ts_to_abstime(const struct timespec *ts)
*/
static inline void abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
{
ts->tv_sec = abstime / 1000000;
abstime -= ts->tv_sec * 1000000;
ts->tv_nsec = abstime * 1000;
ts->tv_sec = (time_t)abstime / 1000000;
abstime -= (hrt_abstime)(ts->tv_sec * 1000000);
ts->tv_nsec = (typeof(ts->tv_nsec))(abstime * 1000);
}
/**
@@ -249,8 +249,8 @@ __EXPORT extern void px4_lockstep_wait_for_components(void);
#else
static inline int px4_lockstep_register_component(void) { return 0; }
static inline void px4_lockstep_unregister_component(int component) { }
static inline void px4_lockstep_progress(int component) { }
static inline void px4_lockstep_unregister_component(int component) { (void)component; }
static inline void px4_lockstep_progress(int component) {(void)component; }
static inline void px4_lockstep_wait_for_components(void) { }
#endif /* defined(ENABLE_LOCKSTEP_SCHEDULER) */
src/drivers/protocol_splitter/Kconfig
-5
View File
@@ -1,5 +0,0 @@
menuconfig DRIVERS_PROTOCOL_SPLITTER
bool "protocol_splitter"
default n
---help---
Enable support for protocol_splitter
src/drivers/protocol_splitter/protocol_splitter.cpp
-845
View File
@@ -1,845 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2016-2021 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 protocol_splitter.cpp
*
* NuttX Driver to multiplex MAVLink and RTPS on a single serial port.
* Makes sure the two protocols can be read & written simultaneously by two
* processes.
*
* It will create two devices:
* /dev/mavlink
* /dev/rtps
*/
#include <lib/cdev/CDev.hpp>
#include <px4_platform_common/sem.hpp>
#include <px4_platform_common/log.h>
#include <lib/perf/perf_counter.h>
#include <mathlib/mathlib.h>
#include <sys/ioctl.h>
#include <assert.h>
#include <unistd.h>
#include <termios.h>
#include <errno.h>
#include <cstdint>
#include <cstring>
class Mavlink2Dev;
class RtpsDev;
class ReadBuffer;
extern "C" __EXPORT int protocol_splitter_main(int argc, char *argv[]);
/*
MessageType is in MSB of header[1]
|
v
Mavlink 0000 0000b
Rtps 1000 0000b
*/
enum class MessageType : uint8_t {Mavlink = 0x00, Rtps = 0x01};
constexpr char Sp2HeaderMagic = 'S';
constexpr int Sp2HeaderSize = 4;
/*
Header Structure:
bits: 1 2 3 4 5 6 7 8
header[0] - | Magic |
header[1] - |T| LenH |
header[2] - | LenL |
header[3] - | Checksum |
*/
union __attribute__((packed)) Sp2Header {
uint8_t bytes[4];
struct {
char magic; // 'S'
uint8_t len_h: 7, // Length MSB
type: 1; // 0=MAVLINK, 1=RTPS
uint8_t len_l; // Length LSB
uint8_t checksum; // XOR of the three bytes above
} fields;
};
struct StaticData {
Mavlink2Dev *mavlink2;
RtpsDev *rtps;
sem_t r_lock;
sem_t w_lock;
char device_name[16];
ReadBuffer *read_buffer;
};
namespace
{
static StaticData *objects = nullptr;
}
// Perf counters
perf_counter_t bytes_received_count;
perf_counter_t header_bytes_received_count;
perf_counter_t bytes_lost_count;
perf_counter_t mavlink_messages_parsed_count;
perf_counter_t mavlink_bytes_parsed_count;
perf_counter_t rtps_messages_parsed_count;
perf_counter_t rtps_bytes_parsed_count;
perf_counter_t buffer_drops;
class ReadBuffer
{
public:
int read(int fd);
void copy(void *dest, size_t pos, size_t n);
void remove(size_t pos, size_t n);
void print_stats();
void update_lost_stats();
uint8_t buffer[1024] = {};
size_t buf_size = 0;
static const size_t BUFFER_THRESHOLD = sizeof(buffer) * 0.8;
// We keep track of the first Mavlink and Rtps packet in the buffer.
// If start and end are equal there is no packet.
size_t start_mavlink = 0;
size_t end_mavlink = 0;
size_t start_rtps = 0;
size_t end_rtps = 0;
// Just for stats.
size_t mavlink_parsed = 0;
size_t rtps_parsed = 0;
size_t bytes_received = 0;
size_t bytes_lost = 0;
size_t header_bytes_received = 0;
};
int ReadBuffer::read(int fd)
{
if (buf_size > BUFFER_THRESHOLD) {
// Drop the buffer if it's too full. This is not expected to happen, but it might, if one of the readers
// is too slow. In that case it's best to make space for new data, otherwise the faster reader might
// busy-loop w/o getting new data.
perf_count(buffer_drops);
PX4_DEBUG("Buffer full, dropping: %zu %zu %zu %zu", start_mavlink, end_mavlink, start_rtps, end_rtps);
// Drop only as much as we have to
size_t num_remove = math::max(start_mavlink, start_rtps);
if (num_remove == 0) {
num_remove = buf_size;
}
remove(0, num_remove);
start_mavlink -= math::min(num_remove, start_mavlink);
end_mavlink -= math::min(num_remove, end_mavlink);
start_rtps -= math::min(num_remove, start_rtps);
end_rtps -= math::min(num_remove, end_rtps);
}
int bytes_available = 0;
int err = ::ioctl(fd, FIONREAD, (unsigned long)&bytes_available);
ssize_t r = 0;
if (err != 0 || bytes_available > 0) {
r = ::read(fd, buffer + buf_size, sizeof(buffer) - buf_size);
}
if (r <= 0) {
return r;
}
buf_size += r;
bytes_received += r;
// Update the lost/unused bytes count
update_lost_stats();
perf_set_count(bytes_received_count, bytes_received);
return r;
}
void ReadBuffer::copy(void *dest, size_t pos, size_t n)
{
ASSERT(pos < buf_size);
ASSERT(pos + n <= buf_size);
if (dest) {
memcpy(dest, buffer + pos, n);
}
}
void ReadBuffer::remove(size_t pos, size_t n)
{
ASSERT(pos < buf_size);
ASSERT(pos + n <= buf_size);
memmove(buffer + pos, buffer + (pos + n), buf_size - pos - n);
buf_size -= n;
}
void ReadBuffer::update_lost_stats()
{
bytes_lost = bytes_received - mavlink_parsed - rtps_parsed - header_bytes_received;
if (end_mavlink > start_mavlink) {
bytes_lost -= end_mavlink - start_mavlink;
}
if (end_rtps > start_rtps) {
bytes_lost -= end_rtps - start_rtps;
}
perf_set_count(bytes_lost_count, bytes_lost);
}
void ReadBuffer::print_stats()
{
PX4_INFO_RAW("\tReceived:\n");
PX4_INFO_RAW("\tTotal: %9zu bytes\n",
bytes_received);
PX4_INFO_RAW("\tHeaders: %9zu bytes (%5.1f %%)\n",
header_bytes_received,
static_cast<double>(static_cast<float>(header_bytes_received)
/ static_cast<float>(bytes_received)
* 100.f));
PX4_INFO_RAW("\tMAVLink: %9zu bytes (%5.1f %%)\n",
mavlink_parsed,
static_cast<double>(static_cast<float>(mavlink_parsed)
/ static_cast<float>(bytes_received - header_bytes_received)
* 100.f));
PX4_INFO_RAW("\tRTPS: %9zu bytes (%5.1f %%)\n",
rtps_parsed,
static_cast<double>(static_cast<float>(rtps_parsed)
/ static_cast<float>(bytes_received - header_bytes_received)
* 100.f));
PX4_INFO_RAW("\tUnused: %9zu bytes (%5.1f %%)\n", bytes_lost,
static_cast<double>(static_cast<float>(bytes_lost)
/ static_cast<float>(bytes_received)
* 100.f));
}
class DevCommon : public cdev::CDev
{
public:
DevCommon(const char *device_path, ReadBuffer *read_buffer);
virtual ~DevCommon();
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
virtual int open(file *filp);
virtual int close(file *filp);
enum Operation {Read, Write};
protected:
virtual pollevent_t poll_state(struct file *filp);
int try_to_copy_data(char *buffer, size_t buflen, MessageType message_type);
void scan_for_packets();
void cleanup();
void lock(enum Operation op)
{
sem_t *this_lock = op == Read ? &objects->r_lock : &objects->w_lock;
while (sem_wait(this_lock) != 0) {
/* The only case that an error should occur here is if
* the wait was awakened by a signal.
*/
ASSERT(get_errno() == EINTR);
}
}
void unlock(enum Operation op)
{
sem_t *this_lock = op == Read ? &objects->r_lock : &objects->w_lock;
sem_post(this_lock);
}
int _fd = -1;
Sp2Header _header;
ReadBuffer *_read_buffer;
};
DevCommon::DevCommon(const char *device_path, ReadBuffer *read_buffer)
: CDev(device_path)
, _read_buffer(read_buffer)
{
}
DevCommon::~DevCommon()
{
if (_fd >= 0) {
// discard all pending data, as close() might block otherwise on NuttX with flow control enabled
tcflush(_fd, TCIOFLUSH);
::close(_fd);
}
}
int DevCommon::ioctl(struct file *filp, int cmd, unsigned long arg)
{
if (cmd == FIONSPACE) {
int ret = ::ioctl(_fd, FIONSPACE, arg);
if (ret == 0) {
int *buf_free = (int *)arg;
*buf_free -= sizeof(_header);
if (*buf_free < 0) {
*buf_free = 0;
}
}
return ret;
}
return ::ioctl(_fd, cmd, arg);
}
int DevCommon::open(file *filp)
{
_fd = ::open(objects->device_name, O_RDWR | O_NOCTTY);
if (_fd < 0) {
PX4_ERR("open failed: %s", strerror(errno));
return -1;
}
CDev::open(filp);
return 0;
}
int DevCommon::close(file *filp)
{
//int ret = ::close(_fd); // FIXME: calling this results in a dead-lock, because DevCommon::close()
// is called from within another close(), and NuttX seems to hold a semaphore at this point
_fd = -1;
CDev::close(filp);
return 0;
}
pollevent_t DevCommon::poll_state(struct file *filp)
{
pollfd fds[1];
fds[0].fd = _fd;
fds[0].events = POLLIN;
/* Here we should just check the poll state (which is called before an actual poll waiting).
* Instead we poll on the fd with some timeout, and then pretend that there is data.
* This will let the calling poll return immediately (there's still no busy loop since
* we do actually poll here).
* We do this because there is no simple way with the given interface to poll on
* the _fd in here or by overriding some other method.
*/
::poll(fds, sizeof(fds) / sizeof(fds[0]), 10);
return POLLIN;
}
int DevCommon::try_to_copy_data(char *buffer, size_t buflen, MessageType message_type)
{
if (buflen == 0) {
return 0;
}
switch (message_type) {
case MessageType::Mavlink:
if (_read_buffer->start_mavlink < _read_buffer->end_mavlink) {
// We have Mavlink data ready to send.
const size_t len_available = _read_buffer->end_mavlink - _read_buffer->start_mavlink;
// We can only send what fits in the callers buffer.
const size_t len_to_copy = math::min(len_available, buflen);
// Copy it to the callers buffer and remove it from our buffer.
_read_buffer->copy(buffer, _read_buffer->start_mavlink, len_to_copy);
// Shift the markers accordingly.
_read_buffer->start_mavlink += len_to_copy;
// Keep track for stats.
_read_buffer->mavlink_parsed += len_to_copy;
// Update the lost/unused bytes count
_read_buffer->update_lost_stats();
// Update the number of MAVLink bytes parsed
perf_set_count(mavlink_bytes_parsed_count, _read_buffer->mavlink_parsed);
// Update the number of MAVLink messages parsed
perf_count(mavlink_messages_parsed_count);
return len_to_copy;
} else {
return 0;
}
case MessageType::Rtps:
if (_read_buffer->start_rtps < _read_buffer->end_rtps) {
// We have Rtps data ready to send
const size_t len_available = _read_buffer->end_rtps - _read_buffer->start_rtps;
// We can only send what fits in the callers buffer.
const size_t len_to_copy = math::min(len_available, buflen);
// Copy it to the callers buffer and remove it from our buffer.
_read_buffer->copy(buffer, _read_buffer->start_rtps, len_to_copy);
// Shift the markers accordingly.
_read_buffer->start_rtps += len_to_copy;
// Keep track for stats.
_read_buffer->rtps_parsed += len_to_copy;
// Update the lost/unused bytes count
_read_buffer->update_lost_stats();
// Update the number of RTPS bytes parsed
perf_set_count(rtps_bytes_parsed_count, _read_buffer->rtps_parsed);
// Update the number of RTPS messages parsed
perf_count(rtps_messages_parsed_count);
return len_to_copy;
} else {
return 0;
}
break;
default:
return 0;
}
}
void DevCommon::scan_for_packets()
{
if (_read_buffer->buf_size < Sp2HeaderSize) {
// We have not even one header in the buffer, no need to scan yet.
return;
}
bool mavlink_available = (_read_buffer->start_mavlink < _read_buffer->end_mavlink);
bool rtps_available = (_read_buffer->start_rtps < _read_buffer->end_rtps);
if (mavlink_available && rtps_available) {
// We still have data for both, no need to scan yet.
return;
}
const size_t begin = math::min(_read_buffer->end_mavlink, _read_buffer->end_rtps);
for (size_t i = begin; i < _read_buffer->buf_size - Sp2HeaderSize; /* ++i */) {
const Sp2Header *header = reinterpret_cast<Sp2Header *>(&_read_buffer->buffer[i]);
if (header->fields.magic != Sp2HeaderMagic) {
// Not the magic byte that we're looking for.
++i;
continue;
}
const uint8_t checksum = (_read_buffer->buffer[i] ^ _read_buffer->buffer[i + 1] ^ _read_buffer->buffer[i + 2]);
if (header->fields.checksum != checksum) {
// Checksum failed.
++i;
continue;
}
if (header->fields.type != static_cast<uint8_t>(MessageType::Mavlink) &&
header->fields.type != static_cast<uint8_t>(MessageType::Rtps)) {
// Ignore unknown protocols
++i;
continue;
}
const size_t payload_len = ((uint16_t)header->fields.len_h << 8) | header->fields.len_l;
if (payload_len > sizeof(_read_buffer->buffer)) {
// This can happen if by accident data matches the header including checksum.
// Given we skip most data using the last payload_len, we should not see this too often,
// unless the link is very lossy and we often have to re-sync.
PX4_DEBUG("payload size %zu > buffer size %zu: %d, protocol: %s",
payload_len, sizeof(_read_buffer->buffer),
(header->fields.type == static_cast<uint8_t>(MessageType::Mavlink)) ? "Mavlink" : "Rtps");
++i;
continue;
}
if (i + Sp2HeaderSize + payload_len > _read_buffer->buf_size) {
// We don't have a enough data in the buffer yet, try again later.
break;
}
if (header->fields.type == static_cast<uint8_t>(MessageType::Mavlink) && !mavlink_available) {
_read_buffer->start_mavlink = i + Sp2HeaderSize;
_read_buffer->end_mavlink = _read_buffer->start_mavlink + payload_len;
mavlink_available = true;
// Overwrite header magic byte, so we don't parse them again.
_read_buffer->buffer[i] = 0;
_read_buffer->header_bytes_received += Sp2HeaderSize;
} else if (header->fields.type == static_cast<uint8_t>(MessageType::Rtps) && !rtps_available) {
_read_buffer->start_rtps = i + Sp2HeaderSize;
_read_buffer->end_rtps = _read_buffer->start_rtps + payload_len;
rtps_available = true;
// Overwrite header magic byte, so we don't parse them again.
_read_buffer->buffer[i] = 0;
_read_buffer->header_bytes_received += Sp2HeaderSize;
}
if (mavlink_available && rtps_available) {
// Both have at least one message ready, we can stop now.
break;
}
// Update the lost/unused bytes count
_read_buffer->update_lost_stats();
perf_set_count(header_bytes_received_count, _read_buffer->header_bytes_received);
i += Sp2HeaderSize + payload_len;
}
}
void DevCommon::cleanup()
{
const bool mavlink_available = (_read_buffer->start_mavlink < _read_buffer->end_mavlink);
const bool rtps_available = (_read_buffer->start_rtps < _read_buffer->end_rtps);
// Clean up garbage bytes and accumulated headers
size_t garbage_end = 0;
if (!mavlink_available && !rtps_available) {
garbage_end = math::max(_read_buffer->start_mavlink, _read_buffer->start_rtps);
} else {
garbage_end = math::min(_read_buffer->start_mavlink, _read_buffer->start_rtps);
}
if (garbage_end > 0) {
_read_buffer->remove(0, garbage_end);
_read_buffer->start_mavlink -= math::min(garbage_end, _read_buffer->start_mavlink);
_read_buffer->end_mavlink -= math::min(garbage_end, _read_buffer->end_mavlink);
_read_buffer->start_rtps -= math::min(garbage_end, _read_buffer->start_rtps);
_read_buffer->end_rtps -= math::min(garbage_end, _read_buffer->end_rtps);
}
}
class Mavlink2Dev : public DevCommon
{
public:
Mavlink2Dev(ReadBuffer *read_buffer);
virtual ~Mavlink2Dev() {}
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
virtual ssize_t write(struct file *filp, const char *buffer, size_t buflen);
};
Mavlink2Dev::Mavlink2Dev(ReadBuffer *read_buffer)
: DevCommon("/dev/mavlink", read_buffer)
{
_header.fields.magic = Sp2HeaderMagic;
_header.fields.len_h = 0;
_header.fields.len_l = 0;
_header.fields.checksum = 0;
_header.fields.type = static_cast<uint8_t>(MessageType::Mavlink);
}
ssize_t Mavlink2Dev::read(struct file *filp, char *buffer, size_t buflen)
{
lock(Read);
// The cleanup needs to be right after a scan, so we don't clean up
// something that we haven't found yet.
scan_for_packets();
cleanup();
// If we have already a packet ready in the current buffer, we don't have
// to read and can grab data straightaway.
int ret = try_to_copy_data(buffer, buflen, MessageType::Mavlink);
if (ret > 0) {
unlock(Read);
return ret;
}
// Otherwise, we have to do a read.
ret = _read_buffer->read(_fd);
if (ret <= 0) {
unlock(Read);
return ret;
}
// Now we need to check again if there is data available.
scan_for_packets();
// And try to copy it out.
ret = try_to_copy_data(buffer, buflen, MessageType::Mavlink);
unlock(Read);
return ret;
}
ssize_t Mavlink2Dev::write(struct file *filp, const char *buffer, size_t buflen)
{
_header.fields.len_h = (buflen >> 8) & 0x7f;
_header.fields.len_l = buflen & 0xff;
_header.fields.checksum = _header.bytes[0] ^ _header.bytes[1] ^ _header.bytes[2];
lock(Write);
int buf_free = 0;
::ioctl(_fd, FIONSPACE, (unsigned long)&buf_free);
ssize_t ret = -1;
if ((size_t)buf_free >= sizeof(_header) + buflen) {
ret = ::write(_fd, _header.bytes, sizeof(_header));
if (ret == sizeof(_header)) {
ret = ::write(_fd, buffer, buflen);
}
}
unlock(Write);
return ret;
}
class RtpsDev : public DevCommon
{
public:
RtpsDev(ReadBuffer *read_buffer);
virtual ~RtpsDev() {}
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
virtual ssize_t write(struct file *filp, const char *buffer, size_t buflen);
protected:
static const uint8_t HEADER_SIZE = 10;
};
RtpsDev::RtpsDev(ReadBuffer *read_buffer)
: DevCommon("/dev/rtps", read_buffer)
{
_header.fields.magic = Sp2HeaderMagic;
_header.fields.len_h = 0;
_header.fields.len_l = 0;
_header.fields.checksum = 0;
_header.fields.type = static_cast<uint8_t>(MessageType::Rtps);
}
ssize_t RtpsDev::read(struct file *filp, char *buffer, size_t buflen)
{
lock(Read);
scan_for_packets();
cleanup();
// If we have already a packet ready in the current buffer, we don't have to read.
int ret = try_to_copy_data(buffer, buflen, MessageType::Rtps);
if (ret > 0) {
unlock(Read);
return ret;
}
// Otherwise, we have to do a read.
ret = _read_buffer->read(_fd);
if (ret <= 0) {
unlock(Read);
return ret;
}
scan_for_packets();
// And check again.
ret = try_to_copy_data(buffer, buflen, MessageType::Rtps);
unlock(Read);
return ret;
}
ssize_t RtpsDev::write(struct file *filp, const char *buffer, size_t buflen)
{
_header.fields.len_h = (buflen >> 8) & 0x7f;
_header.fields.len_l = buflen & 0xff;
_header.fields.checksum = _header.bytes[0] ^ _header.bytes[1] ^ _header.bytes[2];
lock(Write);
int buf_free = 0;
ssize_t ret = -1;
::ioctl(_fd, FIONSPACE, (unsigned long)&buf_free);
if ((size_t)buf_free >= sizeof(_header) + buflen) {
ret = ::write(_fd, _header.bytes, sizeof(_header));
if (ret == sizeof(_header)) {
ret = ::write(_fd, buffer, buflen);
}
}
unlock(Write);
return ret;
}
int protocol_splitter_main(int argc, char *argv[])
{
if (argc < 2) {
goto out;
}
/*
* Start/load the driver.
*/
if (!strcmp(argv[1], "start")) {
if (objects) {
PX4_WARN("already running");
return 1;
}
if (argc != 3) {
goto out;
}
objects = new StaticData();
if (!objects) {
PX4_ERR("alloc failed");
return -1;
}
bytes_received_count = perf_alloc(PC_COUNT, "protocol_splitter: bytes received");
bytes_lost_count = perf_alloc(PC_COUNT, "protocol_splitter: bytes unused/lost");
header_bytes_received_count = perf_alloc(PC_COUNT, "protocol_splitter: header bytes received");
mavlink_messages_parsed_count = perf_alloc(PC_COUNT, "protocol_splitter: MAVLink msgs parsed");
mavlink_bytes_parsed_count = perf_alloc(PC_COUNT, "protocol_splitter: MAVLink msgs bytes parsed");
rtps_messages_parsed_count = perf_alloc(PC_COUNT, "protocol_splitter: RTPS msgs parsed");
rtps_bytes_parsed_count = perf_alloc(PC_COUNT, "protocol_splitter: RTPS msgs bytes parsed");
buffer_drops = perf_alloc(PC_COUNT, "protocol_splitter: buffer drops");
strncpy(objects->device_name, argv[2], sizeof(objects->device_name));
sem_init(&objects->r_lock, 1, 1);
sem_init(&objects->w_lock, 1, 1);
objects->read_buffer = new ReadBuffer();
objects->mavlink2 = new Mavlink2Dev(objects->read_buffer);
objects->rtps = new RtpsDev(objects->read_buffer);
if (!objects->mavlink2 || !objects->rtps) {
delete objects->mavlink2;
delete objects->rtps;
delete objects->read_buffer;
sem_destroy(&objects->r_lock);
sem_destroy(&objects->w_lock);
delete objects;
objects = nullptr;
PX4_ERR("alloc failed");
return -1;
} else {
objects->mavlink2->init();
objects->rtps->init();
}
}
if (!strcmp(argv[1], "stop")) {
if (objects) {
delete objects->mavlink2;
delete objects->rtps;
delete objects->read_buffer;
sem_destroy(&objects->r_lock);
sem_destroy(&objects->w_lock);
delete objects;
objects = nullptr;
perf_free(bytes_received_count);
perf_free(header_bytes_received_count);
perf_free(bytes_lost_count);
perf_free(mavlink_messages_parsed_count);
perf_free(mavlink_bytes_parsed_count);
perf_free(rtps_messages_parsed_count);
perf_free(rtps_bytes_parsed_count);
perf_free(buffer_drops);
}
}
/*
* Print driver status.
*/
if (!strcmp(argv[1], "status")) {
if (objects) {
PX4_INFO("running");
if (sem_wait(&objects->r_lock) != 0) {
return -1;
}
objects->read_buffer->print_stats();
sem_post(&objects->r_lock);
} else {
PX4_INFO("not running");
}
}
return 0;
out:
PX4_ERR("unrecognized command, try 'start <device>', 'stop', 'status'");
return 1;
}
src/lib/CMakeLists.txt
+1
View File
@@ -65,6 +65,7 @@ add_subdirectory(systemlib)
add_subdirectory(system_identification)
add_subdirectory(tecs)
add_subdirectory(terrain_estimation)
add_subdirectory(timesync)
add_subdirectory(tunes)
add_subdirectory(version)
add_subdirectory(weather_vane)
src/drivers/protocol_splitter/CMakeLists.txt → src/lib/timesync/CMakeLists.txt
+7 -8
View File
@@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2015 PX4 Development Team. All rights reserved.
# Copyright (c) 2022 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
@@ -30,10 +30,9 @@
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_module(
MODULE drivers__protocol_splitter
MAIN protocol_splitter
SRCS
protocol_splitter.cpp
DEPENDS
)
px4_add_library(timesync
Timesync.cpp
Timesync.hpp
)
target_link_libraries(timesync)
src/lib/timesync/Timesync.cpp
+168
View File
@@ -0,0 +1,168 @@
/****************************************************************************
*
* Copyright (c) 2018-2022 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 timesync.cpp
* timesync implementation.
*
* @author Mohammed Kabir <mhkabir98@gmail.com>
*/
#include "Timesync.hpp"
#include <stdlib.h>
void Timesync::update(const uint64_t now_us, const int64_t remote_timestamp_ns, const int64_t originate_timestamp_ns)
{
// Message originating from this system, compute time offset from it
if (remote_timestamp_ns > 0) {
// Calculate time offset between this system and the remote system, assuming RTT for
// the timesync packet is roughly equal both ways.
int64_t offset_us = (int64_t)((originate_timestamp_ns / 1000ULL) + now_us - (remote_timestamp_ns / 1000ULL) * 2) / 2 ;
// Calculate the round trip time (RTT) it took the timesync packet to bounce back to us from remote system
uint64_t rtt_us = now_us - (originate_timestamp_ns / 1000ULL);
// Calculate the difference of this sample from the current estimate
uint64_t deviation = llabs((int64_t)_time_offset - offset_us);
if (rtt_us < MAX_RTT_SAMPLE) { // Only use samples with low RTT
if (sync_converged() && (deviation > MAX_DEVIATION_SAMPLE)) {
// Increment the counter if we have a good estimate and are getting samples far from the estimate
_high_deviation_count++;
// We reset the filter if we received 5 consecutive samples which violate our present estimate.
// This is most likely due to a time jump on the offboard system.
if (_high_deviation_count > MAX_CONSECUTIVE_HIGH_DEVIATION) {
PX4_ERR("Time jump detected. Resetting time synchroniser.");
// Reset the filter
reset_filter();
}
} else {
// Filter gain scheduling
if (!sync_converged()) {
// Interpolate with a sigmoid function
double progress = (double)_sequence / (double)CONVERGENCE_WINDOW;
double p = 1.0 - exp(0.5 * (1.0 - 1.0 / (1.0 - progress)));
_filter_alpha = p * ALPHA_GAIN_FINAL + (1.0 - p) * ALPHA_GAIN_INITIAL;
_filter_beta = p * BETA_GAIN_FINAL + (1.0 - p) * BETA_GAIN_INITIAL;
} else {
_filter_alpha = ALPHA_GAIN_FINAL;
_filter_beta = BETA_GAIN_FINAL;
}
// Perform filter update
add_sample(offset_us);
// Increment sequence counter after filter update
_sequence++;
// Reset high deviation count after filter update
_high_deviation_count = 0;
// Reset high RTT count after filter update
_high_rtt_count = 0;
}
} else {
// Increment counter if round trip time is too high for accurate timesync
_high_rtt_count++;
if (_high_rtt_count > MAX_CONSECUTIVE_HIGH_RTT) {
PX4_WARN("RTT too high for timesync: %llu ms", rtt_us / 1000ULL);
// Reset counter to rate-limit warnings
_high_rtt_count = 0;
}
}
// Publish status message
timesync_status_s tsync_status{};
tsync_status.source_protocol = _source;
tsync_status.remote_timestamp = remote_timestamp_ns / 1000ULL;
tsync_status.observed_offset = offset_us;
tsync_status.estimated_offset = (int64_t)_time_offset;
tsync_status.round_trip_time = rtt_us;
tsync_status.timestamp = hrt_absolute_time();
_timesync_status_pub.publish(tsync_status);
}
}
uint64_t Timesync::sync_stamp(uint64_t usec)
{
// Only return synchronised stamp if we have converged to a good value
if (sync_converged()) {
return usec + (int64_t)_time_offset;
} else {
return hrt_absolute_time();
}
}
void Timesync::add_sample(int64_t offset_us)
{
// Online exponential smoothing filter. The derivative of the estimate is also
// estimated in order to produce an estimate without steady state lag:
// https://en.wikipedia.org/wiki/Exponential_smoothing#Double_exponential_smoothing
double time_offset_prev = _time_offset;
if (_sequence == 0) {
// First offset sample
_time_offset = offset_us;
} else {
// Update the clock offset estimate
_time_offset = _filter_alpha * offset_us + (1.0 - _filter_alpha) * (_time_offset + _time_skew);
// Update the clock skew estimate
_time_skew = _filter_beta * (_time_offset - time_offset_prev) + (1.0 - _filter_beta) * _time_skew;
}
}
void Timesync::reset_filter()
{
// Do a full reset of all statistics and parameters
_sequence = 0;
_time_offset = 0.0;
_time_skew = 0.0;
_filter_alpha = ALPHA_GAIN_INITIAL;
_filter_beta = BETA_GAIN_INITIAL;
_high_deviation_count = 0;
_high_rtt_count = 0;
}
src/lib/timesync/Timesync.hpp
+144
View File
@@ -0,0 +1,144 @@
/****************************************************************************
*
* Copyright (c) 2018-2022 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 Timesync.hpp
* time synchroniser definition.
*
* @author Mohammed Kabir <mhkabir98@gmail.com>
*/
#pragma once
#include <drivers/drv_hrt.h>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/timesync_status.h>
#include <math.h>
#include <float.h>
using namespace time_literals;
static constexpr time_t PX4_EPOCH_SECS = 1234567890ULL;
// Filter gains
//
// Alpha : Used to smooth the overall clock offset estimate. Smaller values will lead
// to a smoother estimate, but track time drift more slowly, introducing a bias
// in the estimate. Larger values will cause low-amplitude oscillations.
//
// Beta : Used to smooth the clock skew estimate. Smaller values will lead to a
// tighter estimation of the skew (derivative), but will negatively affect how fast the
// filter reacts to clock skewing (e.g cause by temperature changes to the oscillator).
// Larger values will cause large-amplitude oscillations.
static constexpr double ALPHA_GAIN_INITIAL = 0.05;
static constexpr double BETA_GAIN_INITIAL = 0.05;
static constexpr double ALPHA_GAIN_FINAL = 0.003;
static constexpr double BETA_GAIN_FINAL = 0.003;
// Filter gain scheduling
//
// The filter interpolates between the INITIAL and FINAL gains while the number of
// exhanged timesync packets is less than CONVERGENCE_WINDOW. A lower value will
// allow the timesync to converge faster, but with potentially less accurate initial
// offset and skew estimates.
static constexpr uint32_t CONVERGENCE_WINDOW = 500;
// Outlier rejection and filter reset
//
// Samples with round-trip time higher than MAX_RTT_SAMPLE are not used to update the filter.
// More than MAX_CONSECUTIVE_HIGH_RTT number of such events in a row will throw a warning
// but not reset the filter.
// Samples whose calculated clock offset is more than MAX_DEVIATION_SAMPLE off from the current
// estimate are not used to update the filter. More than MAX_CONSECUTIVE_HIGH_DEVIATION number
// of such events in a row will reset the filter. This usually happens only due to a time jump
// on the remote system.
// TODO : automatically determine these using ping statistics?
static constexpr uint64_t MAX_RTT_SAMPLE = 10_ms;
static constexpr uint64_t MAX_DEVIATION_SAMPLE = 100_ms;
static constexpr uint32_t MAX_CONSECUTIVE_HIGH_RTT = 5;
static constexpr uint32_t MAX_CONSECUTIVE_HIGH_DEVIATION = 5;
class Timesync
{
public:
Timesync(uint8_t source = timesync_status_s::SOURCE_PROTOCOL_UNKNOWN) : _source(source) {};
~Timesync() = default;
void update(const uint64_t now_us, const int64_t remote_timestamp_ns, int64_t originate_timestamp_ns);
/**
* Convert remote timestamp to local hrt time (usec)
* Use synchronised time if available, monotonic boot time otherwise
*/
uint64_t sync_stamp(uint64_t usec);
int64_t offset() const { return (int64_t)_time_offset; }
private:
/**
* Online exponential filter to smooth time offset
*/
void add_sample(int64_t offset_us);
/**
* Return true if the timesync algorithm converged to a good estimate,
* return false otherwise
*/
bool sync_converged() const { return _sequence >= CONVERGENCE_WINDOW; }
/**
* Reset the exponential filter and its states
*/
void reset_filter();
uORB::PublicationMulti<timesync_status_s> _timesync_status_pub{ORB_ID(timesync_status)};
uint32_t _sequence{0};
// Timesync statistics
double _time_offset{0};
double _time_skew{0};
// Filter parameters
double _filter_alpha{ALPHA_GAIN_INITIAL};
double _filter_beta{BETA_GAIN_INITIAL};
// Outlier rejection and filter reset
uint32_t _high_deviation_count{0};
uint32_t _high_rtt_count{0};
uint8_t _source{};
};
src/modules/airship_att_control/airship_att_control_main.cpp
+1 -1
View File
@@ -190,7 +190,7 @@ int AirshipAttitudeControl::print_status()
perf_print_counter(_loop_perf);
print_message(ORB_ID(actuator_controls), _actuator_controls);
print_message(ORB_ID(actuator_controls_0), _actuator_controls);
return 0;
}
src/modules/commander/failsafe/CMakeLists.txt
+1 -1
View File
@@ -32,7 +32,7 @@
############################################################################
# Extract information from failsafe msg file
set(failsafe_flags_msg_file ${PX4_SOURCE_DIR}/msg/failsafe_flags.msg)
set(failsafe_flags_msg_file ${PX4_SOURCE_DIR}/msg/FailsafeFlags.msg)
set(generated_uorb_struct_field_mapping_header ${PX4_BINARY_DIR}/generated_uorb_struct_field_mapping.h)
set(html_template_file ${CMAKE_CURRENT_SOURCE_DIR}/emscripten_template.html)
set(html_output_file ${PX4_BINARY_DIR}/failsafe_html_template.html)
src/modules/commander/failsafe/parse_flags_from_msg.py
+2 -2
View File
@@ -12,7 +12,7 @@ import re
if __name__ == "__main__":
parser = argparse.ArgumentParser(description='Extract fields from .msg files')
parser.add_argument('msg_file', help='failsafe_flags.msg file')
parser.add_argument('msg_file', help='FailsafeFlags.msg file')
parser.add_argument('header_file', help='generated_uorb_struct_field_mapping.h')
parser.add_argument('html_template', help='HTML template input file')
parser.add_argument('html_output', help='HTML output file')
@@ -81,7 +81,7 @@ if __name__ == "__main__":
macro_lines = ''
for group in groups:
for field_type, field_name, comment in group.fields:
macro_lines += ' .property("{0}", &{1}_s::{0}) \\\n'.format(field_name, msg_name)
macro_lines += ' .property("{0}", &px4::msg::{1}::{0}) \\\n'.format(field_name, msg_name)
cpp_emscription_macro = '#define UORB_STRUCT_FIELD_MAPPING \\\n{}\n'.format(macro_lines)
src/modules/ekf2/EKF/airspeed_fusion.cpp
+2 -2
View File
@@ -49,7 +49,7 @@
#include <mathlib/mathlib.h>
void Ekf::updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_source_1d_s &airspeed) const
void Ekf::updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_source1d_s &airspeed) const
{
// reset flags
resetEstimatorAidStatus(airspeed);
@@ -75,7 +75,7 @@ void Ekf::updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_so
setEstimatorAidStatusTestRatio(airspeed, innov_gate);
}
void Ekf::fuseAirspeed(estimator_aid_source_1d_s &airspeed)
void Ekf::fuseAirspeed(estimator_aid_source1d_s &airspeed)
{
if (airspeed.innovation_rejected) {
return;
src/modules/ekf2/EKF/ekf.h
+38 -38
View File
@@ -49,9 +49,9 @@
#include "bias_estimator.hpp"
#include "height_bias_estimator.hpp"
#include <uORB/topics/estimator_aid_source_1d.h>
#include <uORB/topics/estimator_aid_source_2d.h>
#include <uORB/topics/estimator_aid_source_3d.h>
#include <uORB/topics/estimator_aid_source1d.h>
#include <uORB/topics/estimator_aid_source2d.h>
#include <uORB/topics/estimator_aid_source3d.h>
enum class Likelihood { LOW, MEDIUM, HIGH };
@@ -551,30 +551,30 @@ private:
uint64_t _time_good_motion_us{0}; ///< last system time that on-ground motion was within limits (uSec)
Vector2f _flow_compensated_XY_rad{}; ///< measured delta angle of the image about the X and Y body axes after removal of body rotation (rad), RH rotation is positive
estimator_aid_source_1d_s _aid_src_baro_hgt{};
estimator_aid_source_1d_s _aid_src_rng_hgt{};
estimator_aid_source_1d_s _aid_src_airspeed{};
estimator_aid_source_1d_s _aid_src_sideslip{};
estimator_aid_source1d_s _aid_src_baro_hgt{};
estimator_aid_source1d_s _aid_src_rng_hgt{};
estimator_aid_source1d_s _aid_src_airspeed{};
estimator_aid_source1d_s _aid_src_sideslip{};
estimator_aid_source_2d_s _aid_src_fake_pos{};
estimator_aid_source_1d_s _aid_src_fake_hgt{};
estimator_aid_source2d_s _aid_src_fake_pos{};
estimator_aid_source1d_s _aid_src_fake_hgt{};
estimator_aid_source_1d_s _aid_src_ev_hgt{};
estimator_aid_source_2d_s _aid_src_ev_pos{};
estimator_aid_source_3d_s _aid_src_ev_vel{};
estimator_aid_source_1d_s _aid_src_ev_yaw{};
estimator_aid_source1d_s _aid_src_ev_hgt{};
estimator_aid_source2d_s _aid_src_ev_pos{};
estimator_aid_source3d_s _aid_src_ev_vel{};
estimator_aid_source1d_s _aid_src_ev_yaw{};
estimator_aid_source_1d_s _aid_src_gnss_hgt{};
estimator_aid_source_2d_s _aid_src_gnss_pos{};
estimator_aid_source_3d_s _aid_src_gnss_vel{};
estimator_aid_source_1d_s _aid_src_gnss_yaw{};
estimator_aid_source1d_s _aid_src_gnss_hgt{};
estimator_aid_source2d_s _aid_src_gnss_pos{};
estimator_aid_source3d_s _aid_src_gnss_vel{};
estimator_aid_source1d_s _aid_src_gnss_yaw{};
estimator_aid_source_1d_s _aid_src_mag_heading{};
estimator_aid_source_3d_s _aid_src_mag{};
estimator_aid_source1d_s _aid_src_mag_heading{};
estimator_aid_source3d_s _aid_src_mag{};
estimator_aid_source_2d_s _aid_src_aux_vel{};
estimator_aid_source2d_s _aid_src_aux_vel{};
estimator_aid_source_2d_s _aid_src_optical_flow{};
estimator_aid_source2d_s _aid_src_optical_flow{};
// output predictor states
Vector3f _delta_angle_corr{}; ///< delta angle correction vector (rad)
@@ -662,12 +662,12 @@ private:
void predictCovariance();
// ekf sequential fusion of magnetometer measurements
bool fuseMag(const Vector3f &mag, estimator_aid_source_3d_s &aid_src_mag, bool update_all_states = true);
bool fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bool update_all_states = true);
// update quaternion states and covariances using an innovation, observation variance and Jacobian vector
// innovation : prediction - measurement
// variance : observaton variance
bool fuseYaw(const float innovation, const float variance, estimator_aid_source_1d_s &aid_src_status);
bool fuseYaw(const float innovation, const float variance, estimator_aid_source1d_s &aid_src_status);
// fuse the yaw angle obtained from a dual antenna GPS unit
void fuseGpsYaw(const gpsSample &gps_sample);
@@ -683,12 +683,12 @@ private:
// apply sensible limits to the declination and length of the NE mag field states estimates
void limitDeclination();
void updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_source_1d_s &airspeed) const;
void fuseAirspeed(estimator_aid_source_1d_s &airspeed);
void updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_source1d_s &airspeed) const;
void fuseAirspeed(estimator_aid_source1d_s &airspeed);
// fuse synthetic zero sideslip measurement
void updateSideslip(estimator_aid_source_1d_s &_aid_src_sideslip) const;
void fuseSideslip(estimator_aid_source_1d_s &_aid_src_sideslip);
void updateSideslip(estimator_aid_source1d_s &_aid_src_sideslip) const;
void fuseSideslip(estimator_aid_source1d_s &_aid_src_sideslip);
// fuse body frame drag specific forces for multi-rotor wind estimation
void fuseDrag(const dragSample &drag_sample);
@@ -718,24 +718,24 @@ private:
void resetVerticalVelocityToZero();
// fuse optical flow line of sight rate measurements
void updateOptFlow(estimator_aid_source_2d_s &aid_src);
void updateOptFlow(estimator_aid_source2d_s &aid_src);
void fuseOptFlow();
// horizontal and vertical position aid source
void updateHorizontalPositionAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, const float innov_gate, estimator_aid_source_2d_s &aid_src) const;
void updateVerticalPositionAidSrcStatus(const uint64_t &time_us, const float obs, const float obs_var, const float innov_gate, estimator_aid_source_1d_s &aid_src) const;
void updateHorizontalPositionAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, const float innov_gate, estimator_aid_source2d_s &aid_src) const;
void updateVerticalPositionAidSrcStatus(const uint64_t &time_us, const float obs, const float obs_var, const float innov_gate, estimator_aid_source1d_s &aid_src) const;
// 2d & 3d velocity aid source
void updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, const float innov_gate, estimator_aid_source_2d_s &aid_src) const;
void updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector3f &obs, const Vector3f &obs_var, const float innov_gate, estimator_aid_source_3d_s &aid_src) const;
void updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, const float innov_gate, estimator_aid_source2d_s &aid_src) const;
void updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector3f &obs, const Vector3f &obs_var, const float innov_gate, estimator_aid_source3d_s &aid_src) const;
// horizontal and vertical position fusion
void fuseHorizontalPosition(estimator_aid_source_2d_s &pos_aid_src);
void fuseVerticalPosition(estimator_aid_source_1d_s &hgt_aid_src);
void fuseHorizontalPosition(estimator_aid_source2d_s &pos_aid_src);
void fuseVerticalPosition(estimator_aid_source1d_s &hgt_aid_src);
// 2d & 3d velocity fusion
void fuseVelocity(estimator_aid_source_2d_s &vel_aid_src);
void fuseVelocity(estimator_aid_source_3d_s &vel_aid_src);
void fuseVelocity(estimator_aid_source2d_s &vel_aid_src);
void fuseVelocity(estimator_aid_source3d_s &vel_aid_src);
void updateGpsYaw(const gpsSample &gps_sample);
@@ -1085,7 +1085,7 @@ private:
void resetGpsDriftCheckFilters();
void resetEstimatorAidStatus(estimator_aid_source_1d_s &status) const
void resetEstimatorAidStatus(estimator_aid_source1d_s &status) const
{
// only bother resetting if timestamp_sample is set
if (status.timestamp_sample != 0) {
@@ -1130,7 +1130,7 @@ private:
}
}
void setEstimatorAidStatusTestRatio(estimator_aid_source_1d_s &status, float innovation_gate) const
void setEstimatorAidStatusTestRatio(estimator_aid_source1d_s &status, float innovation_gate) const
{
if (PX4_ISFINITE(status.innovation) && PX4_ISFINITE(status.innovation_variance) && (status.innovation_variance > 0.f)) {
status.test_ratio = sq(status.innovation) / (sq(innovation_gate) * status.innovation_variance);
src/modules/ekf2/EKF/mag_fusion.cpp
+2 -2
View File
@@ -50,7 +50,7 @@
#include <mathlib/mathlib.h>
bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source_3d_s &aid_src_mag, bool update_all_states)
bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bool update_all_states)
{
// XYZ Measurement uncertainty. Need to consider timing errors for fast rotations
const float R_MAG = sq(fmaxf(_params.mag_noise, 0.0f));
@@ -223,7 +223,7 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source_3d_s &aid_src_mag, b
}
// update quaternion states and covariances using the yaw innovation and yaw observation variance
bool Ekf::fuseYaw(const float innovation, const float variance, estimator_aid_source_1d_s& aid_src_status)
bool Ekf::fuseYaw(const float innovation, const float variance, estimator_aid_source1d_s& aid_src_status)
{
aid_src_status.innovation = innovation;
src/modules/ekf2/EKF/optflow_fusion.cpp
+1 -1
View File
@@ -47,7 +47,7 @@
#include <float.h>
#include "utils.hpp"
void Ekf::updateOptFlow(estimator_aid_source_2d_s &aid_src)
void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src)
{
resetEstimatorAidStatus(aid_src);
aid_src.timestamp_sample = _flow_sample_delayed.time_us;
src/modules/ekf2/EKF/sideslip_fusion.cpp
+2 -2
View File
@@ -47,7 +47,7 @@
#include <mathlib/mathlib.h>
void Ekf::updateSideslip(estimator_aid_source_1d_s &sideslip) const
void Ekf::updateSideslip(estimator_aid_source1d_s &sideslip) const
{
// reset flags
resetEstimatorAidStatus(sideslip);
@@ -71,7 +71,7 @@ void Ekf::updateSideslip(estimator_aid_source_1d_s &sideslip) const
setEstimatorAidStatusTestRatio(sideslip, innov_gate);
}
void Ekf::fuseSideslip(estimator_aid_source_1d_s &sideslip)
void Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip)
{
if (sideslip.innovation_rejected) {
return;
src/modules/ekf2/EKF/vel_pos_fusion.cpp
+8 -8
View File
@@ -44,7 +44,7 @@
#include "ekf.h"
void Ekf::updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var,
const float innov_gate, estimator_aid_source_2d_s &aid_src) const
const float innov_gate, estimator_aid_source2d_s &aid_src) const
{
resetEstimatorAidStatus(aid_src);
@@ -62,7 +62,7 @@ void Ekf::updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector2f &ob
}
void Ekf::updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector3f &obs, const Vector3f &obs_var,
const float innov_gate, estimator_aid_source_3d_s &aid_src) const
const float innov_gate, estimator_aid_source3d_s &aid_src) const
{
resetEstimatorAidStatus(aid_src);
@@ -88,7 +88,7 @@ void Ekf::updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector3f &ob
}
void Ekf::updateVerticalPositionAidSrcStatus(const uint64_t &time_us, const float obs, const float obs_var,
const float innov_gate, estimator_aid_source_1d_s &aid_src) const
const float innov_gate, estimator_aid_source1d_s &aid_src) const
{
resetEstimatorAidStatus(aid_src);
@@ -112,7 +112,7 @@ void Ekf::updateVerticalPositionAidSrcStatus(const uint64_t &time_us, const floa
}
void Ekf::updateHorizontalPositionAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var,
const float innov_gate, estimator_aid_source_2d_s &aid_src) const
const float innov_gate, estimator_aid_source2d_s &aid_src) const
{
resetEstimatorAidStatus(aid_src);
@@ -129,7 +129,7 @@ void Ekf::updateHorizontalPositionAidSrcStatus(const uint64_t &time_us, const Ve
aid_src.timestamp_sample = time_us;
}
void Ekf::fuseVelocity(estimator_aid_source_2d_s &aid_src)
void Ekf::fuseVelocity(estimator_aid_source2d_s &aid_src)
{
if (aid_src.fusion_enabled && !aid_src.innovation_rejected) {
// vx, vy
@@ -145,7 +145,7 @@ void Ekf::fuseVelocity(estimator_aid_source_2d_s &aid_src)
}
}
void Ekf::fuseVelocity(estimator_aid_source_3d_s &aid_src)
void Ekf::fuseVelocity(estimator_aid_source3d_s &aid_src)
{
if (aid_src.fusion_enabled && !aid_src.innovation_rejected) {
// vx, vy, vz
@@ -162,7 +162,7 @@ void Ekf::fuseVelocity(estimator_aid_source_3d_s &aid_src)
}
}
void Ekf::fuseHorizontalPosition(estimator_aid_source_2d_s &aid_src)
void Ekf::fuseHorizontalPosition(estimator_aid_source2d_s &aid_src)
{
// x & y
if (aid_src.fusion_enabled && !aid_src.innovation_rejected) {
@@ -178,7 +178,7 @@ void Ekf::fuseHorizontalPosition(estimator_aid_source_2d_s &aid_src)
}
}
void Ekf::fuseVerticalPosition(estimator_aid_source_1d_s &aid_src)
void Ekf::fuseVerticalPosition(estimator_aid_source1d_s &aid_src)
{
// z
if (aid_src.fusion_enabled && !aid_src.innovation_rejected) {
src/modules/ekf2/EKF/zero_innovation_heading_update.cpp
+3 -3
View File
@@ -47,7 +47,7 @@ void Ekf::controlZeroInnovationHeadingUpdate()
// fuse zero heading innovation during the leveling fine alignment step to keep the yaw variance low
float innovation = 0.f;
float obs_var = _control_status.flags.vehicle_at_rest ? 0.001f : 0.1f;
estimator_aid_source_1d_s unused;
estimator_aid_source1d_s unused;
fuseYaw(innovation, obs_var, unused);
_last_static_yaw = NAN;
@@ -62,7 +62,7 @@ void Ekf::controlZeroInnovationHeadingUpdate()
if (!yaw_aiding && isTimedOut(_time_last_heading_fuse, (uint64_t)200'000)) {
float innovation = wrap_pi(euler_yaw - _last_static_yaw);
float obs_var = 0.01f;
estimator_aid_source_1d_s unused;
estimator_aid_source1d_s unused;
fuseYaw(innovation, obs_var, unused);
}
@@ -78,7 +78,7 @@ void Ekf::controlZeroInnovationHeadingUpdate()
if (!yaw_aiding && isTimedOut(_time_last_heading_fuse, (uint64_t)200'000)) {
float innovation = 0.f;
float obs_var = 0.01f;
estimator_aid_source_1d_s unused;
estimator_aid_source1d_s unused;
fuseYaw(innovation, obs_var, unused);
}
src/modules/ekf2/EKF2.hpp
+18 -18
View File
@@ -353,30 +353,30 @@ private:
uORB::PublicationMulti<vehicle_optical_flow_vel_s> _estimator_optical_flow_vel_pub{ORB_ID(estimator_optical_flow_vel)};
uORB::PublicationMulti<yaw_estimator_status_s> _yaw_est_pub{ORB_ID(yaw_estimator_status)};
uORB::PublicationMulti<estimator_aid_source_1d_s> _estimator_aid_src_airspeed_pub{ORB_ID(estimator_aid_src_airspeed)};
uORB::PublicationMulti<estimator_aid_source_1d_s> _estimator_aid_src_baro_hgt_pub{ORB_ID(estimator_aid_src_baro_hgt)};
uORB::PublicationMulti<estimator_aid_source_1d_s> _estimator_aid_src_rng_hgt_pub{ORB_ID(estimator_aid_src_rng_hgt)};
uORB::PublicationMulti<estimator_aid_source_1d_s> _estimator_aid_src_sideslip_pub{ORB_ID(estimator_aid_src_sideslip)};
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_airspeed_pub{ORB_ID(estimator_aid_src_airspeed)};
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_baro_hgt_pub{ORB_ID(estimator_aid_src_baro_hgt)};
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_rng_hgt_pub{ORB_ID(estimator_aid_src_rng_hgt)};
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_sideslip_pub{ORB_ID(estimator_aid_src_sideslip)};
uORB::PublicationMulti<estimator_aid_source_1d_s> _estimator_aid_src_fake_hgt_pub{ORB_ID(estimator_aid_src_fake_hgt)};
uORB::PublicationMulti<estimator_aid_source_2d_s> _estimator_aid_src_fake_pos_pub{ORB_ID(estimator_aid_src_fake_pos)};
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_fake_hgt_pub{ORB_ID(estimator_aid_src_fake_hgt)};
uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_fake_pos_pub{ORB_ID(estimator_aid_src_fake_pos)};
uORB::PublicationMulti<estimator_aid_source_1d_s> _estimator_aid_src_ev_hgt_pub{ORB_ID(estimator_aid_src_ev_hgt)};
uORB::PublicationMulti<estimator_aid_source_2d_s> _estimator_aid_src_ev_pos_pub{ORB_ID(estimator_aid_src_ev_pos)};
uORB::PublicationMulti<estimator_aid_source_3d_s> _estimator_aid_src_ev_vel_pub{ORB_ID(estimator_aid_src_ev_vel)};
uORB::PublicationMulti<estimator_aid_source_1d_s> _estimator_aid_src_ev_yaw_pub{ORB_ID(estimator_aid_src_ev_yaw)};
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_ev_hgt_pub{ORB_ID(estimator_aid_src_ev_hgt)};
uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_ev_pos_pub{ORB_ID(estimator_aid_src_ev_pos)};
uORB::PublicationMulti<estimator_aid_source3d_s> _estimator_aid_src_ev_vel_pub{ORB_ID(estimator_aid_src_ev_vel)};
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_ev_yaw_pub{ORB_ID(estimator_aid_src_ev_yaw)};
uORB::PublicationMulti<estimator_aid_source_1d_s> _estimator_aid_src_gnss_hgt_pub{ORB_ID(estimator_aid_src_gnss_hgt)};
uORB::PublicationMulti<estimator_aid_source_2d_s> _estimator_aid_src_gnss_pos_pub{ORB_ID(estimator_aid_src_gnss_pos)};
uORB::PublicationMulti<estimator_aid_source_3d_s> _estimator_aid_src_gnss_vel_pub{ORB_ID(estimator_aid_src_gnss_vel)};
uORB::PublicationMulti<estimator_aid_source_1d_s> _estimator_aid_src_gnss_yaw_pub{ORB_ID(estimator_aid_src_gnss_yaw)};
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_gnss_hgt_pub{ORB_ID(estimator_aid_src_gnss_hgt)};
uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_gnss_pos_pub{ORB_ID(estimator_aid_src_gnss_pos)};
uORB::PublicationMulti<estimator_aid_source3d_s> _estimator_aid_src_gnss_vel_pub{ORB_ID(estimator_aid_src_gnss_vel)};
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_gnss_yaw_pub{ORB_ID(estimator_aid_src_gnss_yaw)};
uORB::PublicationMulti<estimator_aid_source_1d_s> _estimator_aid_src_mag_heading_pub{ORB_ID(estimator_aid_src_mag_heading)};
uORB::PublicationMulti<estimator_aid_source_3d_s> _estimator_aid_src_mag_pub{ORB_ID(estimator_aid_src_mag)};
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_mag_heading_pub{ORB_ID(estimator_aid_src_mag_heading)};
uORB::PublicationMulti<estimator_aid_source3d_s> _estimator_aid_src_mag_pub{ORB_ID(estimator_aid_src_mag)};
uORB::PublicationMulti<estimator_aid_source_2d_s> _estimator_aid_src_aux_vel_pub{ORB_ID(estimator_aid_src_aux_vel)};
uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_aux_vel_pub{ORB_ID(estimator_aid_src_aux_vel)};
uORB::PublicationMulti<estimator_aid_source_2d_s> _estimator_aid_src_optical_flow_pub{ORB_ID(estimator_aid_src_optical_flow)};
uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_optical_flow_pub{ORB_ID(estimator_aid_src_optical_flow)};
// publications with topic dependent on multi-mode
uORB::PublicationMulti<vehicle_attitude_s> _attitude_pub;
src/modules/mavlink/CMakeLists.txt
+1
View File
@@ -100,6 +100,7 @@ px4_add_module(
sensor_calibration
geo
mavlink_c
timesync
tunes
version
UNITY_BUILD
src/modules/mavlink/mavlink_timesync.cpp
+1 -131
View File
@@ -72,82 +72,7 @@ MavlinkTimesync::handle_message(const mavlink_message_t *msg)
} else if (tsync.tc1 > 0) { // Message originating from this system, compute time offset from it
// Calculate time offset between this system and the remote system, assuming RTT for
// the timesync packet is roughly equal both ways.
int64_t offset_us = (int64_t)((tsync.ts1 / 1000ULL) + now - (tsync.tc1 / 1000ULL) * 2) / 2 ;
// Calculate the round trip time (RTT) it took the timesync packet to bounce back to us from remote system
uint64_t rtt_us = now - (tsync.ts1 / 1000ULL);
// Calculate the difference of this sample from the current estimate
uint64_t deviation = llabs((int64_t)_time_offset - offset_us);
if (rtt_us < MAX_RTT_SAMPLE) { // Only use samples with low RTT
if (sync_converged() && (deviation > MAX_DEVIATION_SAMPLE)) {
// Increment the counter if we have a good estimate and are getting samples far from the estimate
_high_deviation_count++;
// We reset the filter if we received 5 consecutive samples which violate our present estimate.
// This is most likely due to a time jump on the offboard system.
if (_high_deviation_count > MAX_CONSECUTIVE_HIGH_DEVIATION) {
PX4_ERR("[timesync] Time jump detected. Resetting time synchroniser.");
// Reset the filter
reset_filter();
}
} else {
// Filter gain scheduling
if (!sync_converged()) {
// Interpolate with a sigmoid function
double progress = (double)_sequence / (double)CONVERGENCE_WINDOW;
double p = 1.0 - exp(0.5 * (1.0 - 1.0 / (1.0 - progress)));
_filter_alpha = p * ALPHA_GAIN_FINAL + (1.0 - p) * ALPHA_GAIN_INITIAL;
_filter_beta = p * BETA_GAIN_FINAL + (1.0 - p) * BETA_GAIN_INITIAL;
} else {
_filter_alpha = ALPHA_GAIN_FINAL;
_filter_beta = BETA_GAIN_FINAL;
}
// Perform filter update
add_sample(offset_us);
// Increment sequence counter after filter update
_sequence++;
// Reset high deviation count after filter update
_high_deviation_count = 0;
// Reset high RTT count after filter update
_high_rtt_count = 0;
}
} else {
// Increment counter if round trip time is too high for accurate timesync
_high_rtt_count++;
if (_high_rtt_count > MAX_CONSECUTIVE_HIGH_RTT) {
PX4_WARN("[timesync] RTT too high for timesync: %llu ms (sender: %i)", rtt_us / 1000ULL, msg->compid);
// Reset counter to rate-limit warnings
_high_rtt_count = 0;
}
}
// Publish status message
timesync_status_s tsync_status{};
tsync_status.timestamp = hrt_absolute_time();
tsync_status.source_protocol = timesync_status_s::SOURCE_PROTOCOL_MAVLINK;
tsync_status.remote_timestamp = tsync.tc1 / 1000ULL;
tsync_status.observed_offset = offset_us;
tsync_status.estimated_offset = (int64_t)_time_offset;
tsync_status.round_trip_time = rtt_us;
_timesync_status_pub.publish(tsync_status);
_timesync.update(now, tsync.tc1, tsync.ts1);
}
break;
@@ -181,58 +106,3 @@ MavlinkTimesync::handle_message(const mavlink_message_t *msg)
break;
}
}
uint64_t
MavlinkTimesync::sync_stamp(uint64_t usec)
{
// Only return synchronised stamp if we have converged to a good value
if (sync_converged()) {
return usec + (int64_t)_time_offset;
} else {
return hrt_absolute_time();
}
}
bool
MavlinkTimesync::sync_converged()
{
return _sequence >= CONVERGENCE_WINDOW;
}
void
MavlinkTimesync::add_sample(int64_t offset_us)
{
/* Online exponential smoothing filter. The derivative of the estimate is also
* estimated in order to produce an estimate without steady state lag:
* https://en.wikipedia.org/wiki/Exponential_smoothing#Double_exponential_smoothing
*/
double time_offset_prev = _time_offset;
if (_sequence == 0) { // First offset sample
_time_offset = offset_us;
} else {
// Update the clock offset estimate
_time_offset = _filter_alpha * offset_us + (1.0 - _filter_alpha) * (_time_offset + _time_skew);
// Update the clock skew estimate
_time_skew = _filter_beta * (_time_offset - time_offset_prev) + (1.0 - _filter_beta) * _time_skew;
}
}
void
MavlinkTimesync::reset_filter()
{
// Do a full reset of all statistics and parameters
_sequence = 0;
_time_offset = 0.0;
_time_skew = 0.0;
_filter_alpha = ALPHA_GAIN_INITIAL;
_filter_beta = BETA_GAIN_INITIAL;
_high_deviation_count = 0;
_high_rtt_count = 0;
}
src/modules/mavlink/mavlink_timesync.h
+3 -89
View File
@@ -42,55 +42,7 @@
#include "mavlink_bridge_header.h"
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/timesync_status.h>
#include <drivers/drv_hrt.h>
#include <math.h>
#include <float.h>
using namespace time_literals;
static constexpr time_t PX4_EPOCH_SECS = 1234567890ULL;
// Filter gains
//
// Alpha : Used to smooth the overall clock offset estimate. Smaller values will lead
// to a smoother estimate, but track time drift more slowly, introducing a bias
// in the estimate. Larger values will cause low-amplitude oscillations.
//
// Beta : Used to smooth the clock skew estimate. Smaller values will lead to a
// tighter estimation of the skew (derivative), but will negatively affect how fast the
// filter reacts to clock skewing (e.g cause by temperature changes to the oscillator).
// Larger values will cause large-amplitude oscillations.
static constexpr double ALPHA_GAIN_INITIAL = 0.05;
static constexpr double BETA_GAIN_INITIAL = 0.05;
static constexpr double ALPHA_GAIN_FINAL = 0.003;
static constexpr double BETA_GAIN_FINAL = 0.003;
// Filter gain scheduling
//
// The filter interpolates between the INITIAL and FINAL gains while the number of
// exhanged timesync packets is less than CONVERGENCE_WINDOW. A lower value will
// allow the timesync to converge faster, but with potentially less accurate initial
// offset and skew estimates.
static constexpr uint32_t CONVERGENCE_WINDOW = 500;
// Outlier rejection and filter reset
//
// Samples with round-trip time higher than MAX_RTT_SAMPLE are not used to update the filter.
// More than MAX_CONSECUTIVE_HIGH_RTT number of such events in a row will throw a warning
// but not reset the filter.
// Samples whose calculated clock offset is more than MAX_DEVIATION_SAMPLE off from the current
// estimate are not used to update the filter. More than MAX_CONSECUTIVE_HIGH_DEVIATION number
// of such events in a row will reset the filter. This usually happens only due to a time jump
// on the remote system.
// TODO : automatically determine these using ping statistics?
static constexpr uint64_t MAX_RTT_SAMPLE = 10_ms;
static constexpr uint64_t MAX_DEVIATION_SAMPLE = 100_ms;
static constexpr uint32_t MAX_CONSECUTIVE_HIGH_RTT = 5;
static constexpr uint32_t MAX_CONSECUTIVE_HIGH_DEVIATION = 5;
#include <lib/timesync/Timesync.hpp>
class Mavlink;
@@ -106,47 +58,9 @@ public:
* Convert remote timestamp to local hrt time (usec)
* Use synchronised time if available, monotonic boot time otherwise
*/
uint64_t sync_stamp(uint64_t usec);
uint64_t sync_stamp(uint64_t usec) { return _timesync.sync_stamp(usec); }
private:
/* do not allow top copying this class */
MavlinkTimesync(MavlinkTimesync &);
MavlinkTimesync &operator = (const MavlinkTimesync &);
protected:
/**
* Online exponential filter to smooth time offset
*/
void add_sample(int64_t offset_us);
/**
* Return true if the timesync algorithm converged to a good estimate,
* return false otherwise
*/
bool sync_converged();
/**
* Reset the exponential filter and its states
*/
void reset_filter();
uORB::PublicationMulti<timesync_status_s> _timesync_status_pub{ORB_ID(timesync_status)};
uint32_t _sequence{0};
// Timesync statistics
double _time_offset{0};
double _time_skew{0};
// Filter parameters
double _filter_alpha{ALPHA_GAIN_INITIAL};
double _filter_beta{BETA_GAIN_INITIAL};
// Outlier rejection and filter reset
uint32_t _high_deviation_count{0};
uint32_t _high_rtt_count{0};
Mavlink *const _mavlink;
Timesync _timesync{};
};
src/modules/microdds_client/CMakeLists.txt
+113 -77
View File
@@ -31,89 +31,125 @@
#
############################################################################
px4_add_git_submodule(TARGET git_micro_xrce_dds_client PATH "${CMAKE_CURRENT_LIST_DIR}/Micro-XRCE-DDS-Client")
if(${CMAKE_VERSION} VERSION_LESS "3.11")
message(WARNING "skipping microdds_client, Micro-XRCE-DDS-Client needs to be fixed to work with CMAKE_VERSION ${CMAKE_VERSION}")
# If a toolchain file is given, explicitly define its path when building the Micro-XRCE-DDS-CLient
if(CMAKE_TOOLCHAIN_FILE MATCHES "cmake$")
set(microxrceddsclient_toolchain ${CMAKE_TOOLCHAIN_FILE})
elseif(CMAKE_TOOLCHAIN_FILE)
set(microxrceddsclient_toolchain ${PX4_SOURCE_DIR}/platforms/${PX4_PLATFORM}/cmake/${CMAKE_TOOLCHAIN_FILE}.cmake)
endif()
else()
px4_add_git_submodule(TARGET git_micro_xrce_dds_client PATH "${CMAKE_CURRENT_LIST_DIR}/Micro-XRCE-DDS-Client")
# Include NuttX headers
get_property(include_dirs DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} PROPERTY INCLUDE_DIRECTORIES)
set(c_flags_with_includes "${CMAKE_C_FLAGS}")
set(cxx_flags_with_includes "${CMAKE_CXX_FLAGS}")
foreach(dir ${include_dirs})
set(c_flags_with_includes "${c_flags_with_includes} -I${dir}")
set(cxx_flags_with_includes "${cxx_flags_with_includes} -I${dir}")
endforeach()
# If a toolchain file is given, explicitly define its path when building the Micro-XRCE-DDS-CLient
if(CMAKE_TOOLCHAIN_FILE MATCHES "cmake$")
set(microxrceddsclient_toolchain ${CMAKE_TOOLCHAIN_FILE})
elseif(CMAKE_TOOLCHAIN_FILE)
set(microxrceddsclient_toolchain ${PX4_SOURCE_DIR}/platforms/${PX4_PLATFORM}/cmake/${CMAKE_TOOLCHAIN_FILE}.cmake)
endif()
set(lib_dir ${CMAKE_INSTALL_LIBDIR})
if("${lib_dir}" STREQUAL "")
set(lib_dir "lib")
endif()
# Include NuttX headers
get_property(include_dirs DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} PROPERTY INCLUDE_DIRECTORIES)
set(c_flags_with_includes "${CMAKE_C_FLAGS}")
set(cxx_flags_with_includes "${CMAKE_CXX_FLAGS}")
foreach(dir ${include_dirs})
set(c_flags_with_includes "${c_flags_with_includes} -I${dir}")
set(cxx_flags_with_includes "${cxx_flags_with_includes} -I${dir}")
endforeach()
set(microxrceddsclient_src_dir ${CMAKE_CURRENT_SOURCE_DIR}/Micro-XRCE-DDS-Client)
set(microxrceddsclient_build_dir ${CMAKE_CURRENT_BINARY_DIR})
set(lib_dir ${CMAKE_INSTALL_LIBDIR})
if("${lib_dir}" STREQUAL "")
set(lib_dir "lib")
endif()
include(ExternalProject)
set(microxrceddsclient_src_dir ${CMAKE_CURRENT_SOURCE_DIR}/Micro-XRCE-DDS-Client)
set(microxrceddsclient_build_dir ${CMAKE_CURRENT_BINARY_DIR})
ExternalProject_Add(
libmicroxrceddsclient_project
PREFIX ${microxrceddsclient_build_dir}
SOURCE_DIR ${microxrceddsclient_src_dir}
INSTALL_DIR ${CMAKE_CURRENT_BINARY_DIR}
CMAKE_CACHE_ARGS
-DCMAKE_CXX_COMPILER:FILEPATH=${CMAKE_CXX_COMPILER}
-DCMAKE_C_COMPILER:FILEPATH=${CMAKE_C_COMPILER}
-DCMAKE_BUILD_TYPE:STRING=${CMAKE_BUILD_TYPE}
-DCMAKE_C_FLAGS:STRING=${c_flags_with_includes}
-DCMAKE_CXX_FLAGS:STRING=${cxx_flags_with_includes}
-DCMAKE_EXE_LINKER_FLAGS:STRING=${CMAKE_EXE_LINKER_FLAGS}
-DUCLIENT_PIC:BOOL=OFF
-DUCLIENT_PROFILE_TCP:BOOL=OFF
-DUCLIENT_PROFILE_UDP:BOOL=ON
-DUCLIENT_PROFILE_SERIAL:BOOL=ON
-DUCLIENT_PROFILE_DISCOVERY:BOOL=OFF
-DUCLIENT_PROFILE_CUSTOM_TRANSPORT:BOOL=ON
-DUCLIENT_PLATFORM_POSIX:BOOL=ON
-DUCLIENT_BUILD_MICROCDR:BOOL=ON
-DCMAKE_INSTALL_PREFIX:PATH=${CMAKE_CURRENT_BINARY_DIR}
-DCMAKE_PREFIX_PATH:PATH=${CMAKE_CURRENT_BINARY_DIR}
-DCMAKE_TOOLCHAIN_FILE:STRING=${microxrceddsclient_toolchain}
BUILD_BYPRODUCTS ${CMAKE_CURRENT_BINARY_DIR}/${lib_dir}/libmicroxrcedds_client.a
BUILD_BYPRODUCTS ${CMAKE_CURRENT_BINARY_DIR}/${lib_dir}/libmicrocdr.a
DEPENDS prebuild_targets git_micro_xrce_dds_client
)
include(ExternalProject)
add_library(libmicroxrceddsclient STATIC IMPORTED GLOBAL)
set_target_properties(libmicroxrceddsclient
PROPERTIES IMPORTED_LOCATION ${CMAKE_CURRENT_BINARY_DIR}/${lib_dir}/libmicroxrcedds_client.a
)
add_library(libmicrocdr STATIC IMPORTED GLOBAL)
set_target_properties(libmicrocdr
PROPERTIES IMPORTED_LOCATION ${CMAKE_CURRENT_BINARY_DIR}/${lib_dir}/libmicrocdr.a
)
add_library(microxrceddsclient INTERFACE)
add_dependencies(microxrceddsclient libmicroxrceddsclient)
add_dependencies(microxrceddsclient libmicrocdr)
add_dependencies(microxrceddsclient libmicroxrceddsclient_project)
target_include_directories(microxrceddsclient INTERFACE ${microxrceddsclient_build_dir}/include)
px4_add_module(
MODULE modules__microdds_client
MAIN microdds_client
STACK_MAIN 8000
SRCS
microdds_client.cpp
DEPENDS
microxrceddsclient
libmicroxrceddsclient
libmicrocdr
uorb_ucdr_headers
ExternalProject_Add(
libmicroxrceddsclient_project
PREFIX ${microxrceddsclient_build_dir}
SOURCE_DIR ${microxrceddsclient_src_dir}
INSTALL_DIR ${CMAKE_CURRENT_BINARY_DIR}
CMAKE_CACHE_ARGS
-DCMAKE_CXX_COMPILER:FILEPATH=${CMAKE_CXX_COMPILER}
-DCMAKE_C_COMPILER:FILEPATH=${CMAKE_C_COMPILER}
-DCMAKE_BUILD_TYPE:STRING=${CMAKE_BUILD_TYPE}
-DCMAKE_C_FLAGS:STRING=${c_flags_with_includes}
-DCMAKE_CXX_FLAGS:STRING=${cxx_flags_with_includes}
-DCMAKE_EXE_LINKER_FLAGS:STRING=${CMAKE_EXE_LINKER_FLAGS}
-DUCLIENT_PIC:BOOL=OFF
-DUCLIENT_PROFILE_TCP:BOOL=OFF
-DUCLIENT_PROFILE_UDP:BOOL=ON
-DUCLIENT_PROFILE_SERIAL:BOOL=ON
-DUCLIENT_PROFILE_DISCOVERY:BOOL=OFF
-DUCLIENT_PROFILE_CUSTOM_TRANSPORT:BOOL=OFF
-DUCLIENT_PROFILE_MULTITHREAD:BOOL=OFF
-DUCLIENT_PROFILE_SHARED_MEMORY:BOOL=OFF
-DUCLIENT_PLATFORM_POSIX:BOOL=ON
-DUCLIENT_BUILD_MICROCDR:BOOL=ON
-DCMAKE_INSTALL_PREFIX:PATH=${CMAKE_CURRENT_BINARY_DIR}
-DCMAKE_PREFIX_PATH:PATH=${CMAKE_CURRENT_BINARY_DIR}
-DCMAKE_TOOLCHAIN_FILE:STRING=${microxrceddsclient_toolchain}
BUILD_BYPRODUCTS ${CMAKE_CURRENT_BINARY_DIR}/${lib_dir}/libmicroxrcedds_client.a
BUILD_BYPRODUCTS ${CMAKE_CURRENT_BINARY_DIR}/${lib_dir}/libmicrocdr.a
DEPENDS prebuild_targets git_micro_xrce_dds_client
LOG_CONFIGURE true
LOG_BUILD true
LOG_INSTALL true
LOG_OUTPUT_ON_FAILURE true
)
add_dependencies(modules__microdds_client topic_bridge_files)
add_library(libmicroxrceddsclient STATIC IMPORTED GLOBAL)
set_target_properties(libmicroxrceddsclient
PROPERTIES IMPORTED_LOCATION ${CMAKE_CURRENT_BINARY_DIR}/${lib_dir}/libmicroxrcedds_client.a
)
add_library(libmicrocdr STATIC IMPORTED GLOBAL)
set_target_properties(libmicrocdr
PROPERTIES IMPORTED_LOCATION ${CMAKE_CURRENT_BINARY_DIR}/${lib_dir}/libmicrocdr.a
)
add_library(microxrceddsclient INTERFACE)
add_dependencies(microxrceddsclient libmicroxrceddsclient)
add_dependencies(microxrceddsclient libmicrocdr)
add_dependencies(microxrceddsclient libmicroxrceddsclient_project)
target_include_directories(microxrceddsclient INTERFACE ${microxrceddsclient_build_dir}/include)
add_custom_command(OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/dds_topics.h
COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/generate_dds_topics.py
--topic-msg-dir ${PX4_SOURCE_DIR}/msg
--client-outdir ${CMAKE_CURRENT_BINARY_DIR}
--dds-topics-file ${CMAKE_CURRENT_SOURCE_DIR}/dds_topics.yaml
--template_file ${CMAKE_CURRENT_SOURCE_DIR}/dds_topics.h.em
DEPENDS
${CMAKE_CURRENT_SOURCE_DIR}/generate_dds_topics.py
${CMAKE_CURRENT_SOURCE_DIR}/dds_topics.yaml
${CMAKE_CURRENT_SOURCE_DIR}/dds_topics.h.em
COMMENT "Generating XRCE-DDS topic bridge"
)
add_custom_target(topic_bridge_files DEPENDS ${CMAKE_CURRENT_BINARY_DIR}/dds_topics.h)
px4_add_module(
MODULE modules__microdds_client
MAIN microdds_client
STACK_MAIN 9000
INCLUDES
${CMAKE_CURRENT_SOURCE_DIR}
COMPILE_FLAGS
#-O0
${MAX_CUSTOM_OPT_LEVEL}
SRCS
${CMAKE_CURRENT_BINARY_DIR}/dds_topics.h
microdds_client.cpp
microdds_client.h
DEPENDS
microxrceddsclient
libmicroxrceddsclient
libmicrocdr
timesync
topic_bridge_files
uorb_ucdr_headers
MODULE_CONFIG
module.yaml
)
endif()
src/modules/microdds_client/Micro-XRCE-DDS-Client
+1 -1
Submodule src/modules/microdds_client/Micro-XRCE-DDS-Client updated: b5187a9f39...4248559f3b
src/modules/microdds_client/dds_topics.h.em
+127
View File
@@ -0,0 +1,127 @@
@###############################################
@#
@# EmPy template
@#
@###############################################
@# Generates publications and subscriptions for XRCE
@#
@# Context:
@# - msgs (List) list of all RTPS messages
@# - topics (List) list of all topic names
@# - spec (msggen.MsgSpec) Parsed specification of the .msg file
@###############################################
@{
import os
}@
#include <utilities.hpp>
#include <uxr/client/client.h>
#include <ucdr/microcdr.h>
#include <uORB/Subscription.hpp>
#include <uORB/Publication.hpp>
@[for include in type_includes]@
#include <uORB/ucdr/@(include).h>
@[end for]@
// Subscribers for messages to send
struct SendTopicsSubs {
@[ for pub in publications]@
uORB::Subscription @(pub['topic_simple'])_sub{ORB_ID(@(pub['topic_simple']))};
uxrObjectId @(pub['topic_simple'])_data_writer{};
@[ end for]@
uint32_t num_payload_sent{};
void update(uxrSession *session, uxrStreamId reliable_out_stream_id, uxrStreamId best_effort_stream_id, uxrObjectId participant_id, const char *client_namespace);
};
void SendTopicsSubs::update(uxrSession *session, uxrStreamId reliable_out_stream_id, uxrStreamId best_effort_stream_id, uxrObjectId participant_id, const char *client_namespace)
{
int64_t time_offset_us = session->time_offset / 1000; // ns -> us
@[ for idx, pub in enumerate(publications)]@
{
@(pub['simple_base_type'])_s data;
if (@(pub['topic_simple'])_sub.update(&data)) {
if (@(pub['topic_simple'])_data_writer.id == UXR_INVALID_ID) {
// data writer not created yet
create_data_writer(session, reliable_out_stream_id, participant_id, ORB_ID::@(pub['topic_simple']), client_namespace, "@(pub['topic_simple'])", "@(pub['dds_type'])", @(pub['topic_simple'])_data_writer);
}
if (@(pub['topic_simple'])_data_writer.id != UXR_INVALID_ID) {
ucdrBuffer ub;
uint32_t topic_size = ucdr_topic_size_@(pub['simple_base_type'])();
if (uxr_prepare_output_stream(session, best_effort_stream_id, @(pub['topic_simple'])_data_writer, &ub, topic_size) != UXR_INVALID_REQUEST_ID) {
ucdr_serialize_@(pub['simple_base_type'])(data, ub, time_offset_us);
// TODO: fill up the MTU and then flush, which reduces the packet overhead
uxr_flash_output_streams(session);
num_payload_sent += topic_size;
} else {
//PX4_ERR("Error uxr_prepare_output_stream UXR_INVALID_REQUEST_ID @(pub['topic_simple'])");
}
} else {
//PX4_ERR("Error UXR_INVALID_ID @(pub['topic_simple'])");
}
}
}
@[ end for]@
}
// Publishers for received messages
struct RcvTopicsPubs {
@[ for sub in subscriptions]@
uORB::Publication<@(sub['simple_base_type'])_s> @(sub['topic_simple'])_pub{ORB_ID(@(sub['topic_simple']))};
@[ end for]@
uint32_t num_payload_received{};
bool init(uxrSession *session, uxrStreamId reliable_out_stream_id, uxrStreamId reliable_in_stream_id, uxrStreamId best_effort_in_stream_id, uxrObjectId participant_id, const char *client_namespace);
};
static void on_topic_update(uxrSession *session, uxrObjectId object_id, uint16_t request_id, uxrStreamId stream_id,
struct ucdrBuffer *ub, uint16_t length, void *args)
{
RcvTopicsPubs *pubs = (RcvTopicsPubs *)args;
const int64_t time_offset_us = session->time_offset / 1000; // ns -> us
pubs->num_payload_received += length;
switch (object_id.id) {
@[ for idx, sub in enumerate(subscriptions)]@
case @(idx)+1000: {
@(sub['simple_base_type'])_s data;
if (ucdr_deserialize_@(sub['simple_base_type'])(*ub, data, time_offset_us)) {
//print_message(ORB_ID(@(sub['simple_base_type'])), data);
pubs->@(sub['topic_simple'])_pub.publish(data);
}
}
break;
@[ end for]@
default:
PX4_ERR("unknown object id: %i", object_id.id);
break;
}
}
bool RcvTopicsPubs::init(uxrSession *session, uxrStreamId reliable_out_stream_id, uxrStreamId reliable_in_stream_id, uxrStreamId best_effort_in_stream_id, uxrObjectId participant_id, const char *client_namespace)
{
@[ for idx, sub in enumerate(subscriptions)]@
create_data_reader(session, reliable_out_stream_id, best_effort_in_stream_id, participant_id, @(idx), client_namespace, "@(sub['topic_simple'])", "@(sub['dds_type'])");
@[ end for]@
uxr_set_topic_callback(session, on_topic_update, this);
return true;
}
src/modules/microdds_client/dds_topics.yaml
+86
View File
@@ -0,0 +1,86 @@
#####
#
# This file maps all the topics that are to be used on the micro DDS client.
#
#####
publications:
- topic: /fmu/out/collision_constraints
type: px4_msgs::msg::CollisionConstraints
- topic: /fmu/out/failsafe_flags
type: px4_msgs::msg::FailsafeFlags
- topic: /fmu/out/sensor_combined
type: px4_msgs::msg::SensorCombined
- topic: /fmu/out/timesync_status
type: px4_msgs::msg::TimesyncStatus
# - topic: /fmu/out/vehicle_angular_velocity
# type: px4_msgs::msg::VehicleAngularVelocity
- topic: /fmu/out/vehicle_attitude
type: px4_msgs::msg::VehicleAttitude
- topic: /fmu/out/vehicle_control_mode
type: px4_msgs::msg::VehicleControlMode
- topic: /fmu/out/vehicle_global_position
type: px4_msgs::msg::VehicleGlobalPosition
- topic: /fmu/out/vehicle_gps_position
type: px4_msgs::msg::SensorGps
- topic: /fmu/out/vehicle_local_position
type: px4_msgs::msg::VehicleLocalPosition
- topic: /fmu/out/vehicle_odometry
type: px4_msgs::msg::VehicleOdometry
- topic: /fmu/out/vehicle_status
type: px4_msgs::msg::VehicleStatus
- topic: /fmu/out/vehicle_trajectory_waypoint_desired
type: px4_msgs::msg::VehicleTrajectoryWaypoint
subscriptions:
- topic: /fmu/in/offboard_control_mode
type: px4_msgs::msg::OffboardControlMode
- topic: /fmu/in/onboard_computer_status
type: px4_msgs::msg::OnboardComputerStatus
- topic: /fmu/in/obstacle_distance
type: px4_msgs::msg::ObstacleDistance
- topic: /fmu/in/sensor_optical_flow
type: px4_msgs::msg::SensorOpticalFlow
- topic: /fmu/in/telemetry_status
type: px4_msgs::msg::TelemetryStatus
- topic: /fmu/in/trajectory_setpoint
type: px4_msgs::msg::TrajectorySetpoint
- topic: /fmu/in/vehicle_attitude_setpoint
type: px4_msgs::msg::VehicleAttitudeSetpoint
- topic: /fmu/in/vehicle_mocap_odometry
type: px4_msgs::msg::VehicleOdometry
- topic: /fmu/in/vehicle_rates_setpoint
type: px4_msgs::msg::VehicleRatesSetpoint
- topic: /fmu/in/vehicle_visual_odometry
type: px4_msgs::msg::VehicleOdometry
- topic: /fmu/in/vehicle_command
type: px4_msgs::msg::VehicleCommand
- topic: /fmu/in/vehicle_trajectory_bezier
type: px4_msgs::msg::VehicleTrajectoryBezier
- topic: /fmu/in/vehicle_trajectory_waypoint
type: px4_msgs::msg::VehicleTrajectoryWaypoint
src/modules/microdds_client/generate_dds_topics.py
+143
View File
@@ -0,0 +1,143 @@
#!/usr/bin/env python3
################################################################################
#
# Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima).
# Copyright (c) 2018-2021 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 of the copyright holder 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 HOLDER 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.
#
################################################################################
# This script can generate the client code based on a set of topics
# to sent and set to receive.
import sys
import os
import argparse
import re
import em
import yaml
parser = argparse.ArgumentParser()
parser.add_argument("-m", "--topic-msg-dir", dest='msgdir', type=str,
help="Topics message, by default using relative path 'msg/'", default="msg")
parser.add_argument("-y", "--dds-topics-file", dest='yaml_file', type=str,
help="Setup topics file path, by default using 'dds_topics.yaml'")
parser.add_argument("-t", "--template_file", dest='template_file', type=str,
help="DDS topics template file")
parser.add_argument("-u", "--client-outdir", dest='clientdir', type=str,
help="Client output dir, by default using relative path 'src/modules/microdds_client'", default=None)
if len(sys.argv) <= 1:
parser.print_usage()
exit(-1)
# Parse arguments
args = parser.parse_args()
# Client files output path
client_out_dir = os.path.abspath(args.clientdir)
template_file = os.path.join(args.template_file)
# Make sure output directory exists:
if not os.path.isdir(client_out_dir):
os.makedirs(client_out_dir)
output_file = os.path.join(client_out_dir, os.path.basename(template_file).replace(".em", ""))
folder_name = os.path.dirname(output_file)
if not os.path.exists(folder_name):
os.makedirs(folder_name)
# open yaml file to load dictionary of publications and subscriptions
with open(args.yaml_file, 'r') as file:
msg_map = yaml.safe_load(file)
merged_em_globals = {}
all_type_includes = []
for p in msg_map['publications']:
# eg TrajectoryWaypoint from px4_msgs::msg::TrajectoryWaypoint
simple_base_type = p['type'].split('::')[-1]
# eg TrajectoryWaypoint -> trajectory_waypoint
base_type_name_snake_case = re.sub(r'(?<!^)(?=[A-Z])', '_', simple_base_type).lower()
all_type_includes.append(base_type_name_snake_case)
# simple_base_type: eg vehicle_status
p['simple_base_type'] = base_type_name_snake_case
# dds_type: eg px4_msgs::msg::dds_::VehicleStatus_
p['dds_type'] = p['type'].replace("::msg::", "::msg::dds_::") + "_"
# topic_simple: eg vehicle_status
p['topic_simple'] = p['topic'].split('/')[-1]
merged_em_globals['publications'] = msg_map['publications']
for s in msg_map['subscriptions']:
# eg TrajectoryWaypoint from px4_msgs::msg::TrajectoryWaypoint
simple_base_type = s['type'].split('::')[-1]
# eg TrajectoryWaypoint -> trajectory_waypoint
base_type_name_snake_case = re.sub(r'(?<!^)(?=[A-Z])', '_', simple_base_type).lower()
all_type_includes.append(base_type_name_snake_case)
# simple_base_type: eg vehicle_status
s['simple_base_type'] = base_type_name_snake_case
# dds_type: eg px4_msgs::msg::dds_::VehicleStatus_
s['dds_type'] = s['type'].replace("::msg::", "::msg::dds_::") + "_"
# topic_simple: eg vehicle_status
s['topic_simple'] = s['topic'].split('/')[-1]
merged_em_globals['subscriptions'] = msg_map['subscriptions']
merged_em_globals['type_includes'] = sorted(set(all_type_includes))
# run interpreter
ofile = open(output_file, 'w')
interpreter = em.Interpreter(output=ofile, globals=merged_em_globals, options={em.RAW_OPT: True, em.BUFFERED_OPT: True})
try:
interpreter.file(open(template_file))
except OSError as e:
ofile.close()
os.remove(output_file)
raise
interpreter.shutdown()
ofile.close()
src/modules/microdds_client/microdds_client.cpp
+187 -81
View File
@@ -33,7 +33,6 @@
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/cli.h>
#include <uORB/topics/vehicle_imu.h>
#include "microdds_client.h"
@@ -46,27 +45,68 @@
#include <stdlib.h>
#include <unistd.h>
#if defined(CONFIG_NET) || defined(__PX4_POSIX)
# define MICRODDS_CLIENT_UDP 1
#endif
#define STREAM_HISTORY 4
#define BUFFER_SIZE (UXR_CONFIG_SERIAL_TRANSPORT_MTU * STREAM_HISTORY) // MTU==512 by default
using namespace time_literals;
void on_time(uxrSession *session, int64_t current_time, int64_t received_timestamp, int64_t transmit_timestamp,
int64_t originate_timestamp, void *args)
{
// latest round trip time (RTT)
int64_t rtt = current_time - originate_timestamp;
// HRT to AGENT
int64_t offset_1 = (received_timestamp - originate_timestamp) - (rtt / 2);
int64_t offset_2 = (transmit_timestamp - current_time) - (rtt / 2);
session->time_offset = (offset_1 + offset_2) / 2;
if (args) {
Timesync *timesync = static_cast<Timesync *>(args);
timesync->update(current_time / 1000, transmit_timestamp, originate_timestamp);
//fprintf(stderr, "time_offset: %ld, timesync: %ld, diff: %ld\n", session->time_offset/1000, timesync->offset(), session->time_offset/1000 + timesync->offset());
session->time_offset = -timesync->offset() * 1000; // us -> ns
}
}
MicroddsClient::MicroddsClient(Transport transport, const char *device, int baudrate, const char *host,
const char *port, bool localhost_only)
: _localhost_only(localhost_only)
const char *port, bool localhost_only, const char *client_namespace) :
ModuleParams(nullptr),
_localhost_only(localhost_only),
_client_namespace(client_namespace)
{
if (transport == Transport::Serial) {
int fd = open(device, O_RDWR | O_NOCTTY | O_NONBLOCK);
int fd = -1;
if (fd < 0) {
PX4_ERR("open %s failed (%i)", device, errno);
for (int attempt = 0; attempt < 3; attempt++) {
fd = open(device, O_RDWR | O_NOCTTY | O_NONBLOCK);
if (fd < 0) {
PX4_ERR("open %s failed (%i)", device, errno);
// sleep before trying again
usleep(1'000'000);
} else {
break;
}
}
_transport_serial = new uxrSerialTransport();
if (fd >= 0 && setBaudrate(fd, baudrate) == 0 && _transport_serial) {
if (uxr_init_serial_transport(_transport_serial, fd, 0, 1)) {
// TODO:
uint8_t remote_addr = 0; // Identifier of the Agent in the connection
uint8_t local_addr = 1; // Identifier of the Client in the serial connection
if (uxr_init_serial_transport(_transport_serial, fd, remote_addr, local_addr)) {
_comm = &_transport_serial->comm;
_fd = fd;
@@ -77,7 +117,7 @@ MicroddsClient::MicroddsClient(Transport transport, const char *device, int baud
} else if (transport == Transport::Udp) {
#if defined(CONFIG_NET) || defined(__PX4_POSIX)
#if defined(MICRODDS_CLIENT_UDP)
_transport_udp = new uxrUDPTransport();
if (_transport_udp) {
@@ -127,8 +167,6 @@ void MicroddsClient::run()
return;
}
int polling_topic_sub = orb_subscribe(ORB_ID(vehicle_imu));
while (!should_exit()) {
bool got_response = false;
@@ -142,29 +180,48 @@ void MicroddsClient::run()
}
// Session
// The key identifier of the Client. All Clients connected to an Agent must have a different key.
const uint32_t key = 0xAAAABBBB;
uxrSession session;
uxr_init_session(&session, _comm, 0xAAAABBBB);
uxr_init_session(&session, _comm, key);
// void uxr_create_session_retries(uxrSession* session, size_t retries);
if (!uxr_create_session(&session)) {
PX4_ERR("uxr_create_session failed");
return;
}
// TODO: uxr_set_status_callback
// Streams
// Reliable for setup, afterwards best-effort to send the data (important: need to create all 4 streams)
uint8_t output_reliable_stream_buffer[BUFFER_SIZE] {};
uxrStreamId reliable_out = uxr_create_output_reliable_stream(&session, output_reliable_stream_buffer, BUFFER_SIZE,
STREAM_HISTORY);
uint8_t output_data_stream_buffer[1024] {};
uxrStreamId data_out = uxr_create_output_best_effort_stream(&session, output_data_stream_buffer,
sizeof(output_data_stream_buffer));
uxrStreamId reliable_out = uxr_create_output_reliable_stream(&session, output_reliable_stream_buffer,
sizeof(output_reliable_stream_buffer), STREAM_HISTORY);
uint8_t output_data_stream_buffer[2048] {};
uxrStreamId best_effort_out = uxr_create_output_best_effort_stream(&session, output_data_stream_buffer,
sizeof(output_data_stream_buffer));
uint8_t input_reliable_stream_buffer[BUFFER_SIZE] {};
uxr_create_input_reliable_stream(&session, input_reliable_stream_buffer, BUFFER_SIZE, STREAM_HISTORY);
uxrStreamId input_stream = uxr_create_input_best_effort_stream(&session);
uxrStreamId reliable_in = uxr_create_input_reliable_stream(&session, input_reliable_stream_buffer,
sizeof(input_reliable_stream_buffer),
STREAM_HISTORY);
(void)reliable_in;
uxrStreamId best_effort_in = uxr_create_input_best_effort_stream(&session);
(void)best_effort_in;
// Create entities
uxrObjectId participant_id = uxr_object_id(0x01, UXR_PARTICIPANT_ID);
uint16_t domain_id = _param_xrce_dds_dom_id.get();
// const char *participant_name = "px4_micro_xrce_dds";
// uint16_t participant_req = uxr_buffer_create_participant_bin(&session, reliable_out, participant_id, domain_id,
// participant_name, UXR_REPLACE);
// TODO: configurable participant name with client namespace?
const char *participant_xml = _localhost_only ?
"<dds>"
"<profiles>"
@@ -178,7 +235,7 @@ void MicroddsClient::run()
"</profiles>"
"<participant>"
"<rtps>"
"<name>default_xrce_participant</name>"
"<name>px4_micro_xrce_dds</name>"
"<useBuiltinTransports>false</useBuiltinTransports>"
"<userTransports><transport_id>udp_localhost</transport_id></userTransports>"
"</rtps>"
@@ -188,11 +245,11 @@ void MicroddsClient::run()
"<dds>"
"<participant>"
"<rtps>"
"<name>default_xrce_participant</name>"
"<name>px4_micro_xrce_dds</name>"
"</rtps>"
"</participant>"
"</dds>" ;
uint16_t participant_req = uxr_buffer_create_participant_xml(&session, reliable_out, participant_id, 0,
uint16_t participant_req = uxr_buffer_create_participant_xml(&session, reliable_out, participant_id, domain_id,
participant_xml, UXR_REPLACE);
uint8_t request_status;
@@ -202,69 +259,60 @@ void MicroddsClient::run()
return;
}
if (!_subs->init(&session, reliable_out, participant_id)) {
PX4_ERR("subs init failed");
return;
}
if (!_pubs->init(&session, reliable_out, input_stream, participant_id)) {
if (!_pubs->init(&session, reliable_out, reliable_in, best_effort_in, participant_id, _client_namespace)) {
PX4_ERR("pubs init failed");
return;
}
_connected = true;
// Set time-callback.
uxr_set_time_callback(&session, on_time, &_timesync);
// Synchronize with the Agent
bool synchronized = false;
while (!synchronized) {
synchronized = uxr_sync_session(&session, 1000);
if (synchronized) {
PX4_INFO("synchronized with time offset %-5" PRId64 "us", session.time_offset / 1000);
//sleep(1);
} else {
usleep(10000);
}
}
hrt_abstime last_sync_session = 0;
hrt_abstime last_status_update = hrt_absolute_time();
hrt_abstime last_ping = hrt_absolute_time();
int num_pings_missed = 0;
bool had_ping_reply = false;
uint32_t last_num_payload_sent{};
uint32_t last_num_payload_received{};
bool error_printed = false;
hrt_abstime last_read = hrt_absolute_time();
while (!should_exit() && _connected) {
px4_pollfd_struct_t fds[1];
fds[0].fd = polling_topic_sub;
fds[0].events = POLLIN;
// we could poll on the uart/udp fd as well (on nuttx)
int pret = px4_poll(fds, 1, 20);
if (pret < 0) {
if (!error_printed) {
PX4_ERR("poll failed (%i)", pret);
error_printed = true;
}
_subs->update(&session, reliable_out, best_effort_out, participant_id, _client_namespace);
} else if (pret != 0) {
if (fds[0].revents & POLLIN) {
vehicle_imu_s data;
orb_copy(ORB_ID(vehicle_imu), polling_topic_sub, &data);
uxr_run_session_timeout(&session, 0);
// time sync session
if (hrt_elapsed_time(&last_sync_session) > 1_s) {
if (uxr_sync_session(&session, 100)) {
//PX4_INFO("synchronized with time offset %-5" PRId64 "ns", session.time_offset);
last_sync_session = hrt_absolute_time();
}
}
_subs->update(data_out);
hrt_abstime read_start = hrt_absolute_time();
if (read_start - last_read > 5_ms) {
last_read = read_start;
// Read as long as there's data or until a timeout
pollfd fd_read;
fd_read.fd = _fd;
fd_read.events = POLLIN;
do {
uxr_run_session_timeout(&session, 0);
if (session.on_pong_flag == 1 /* PONG_IN_SESSION_STATUS */) { // Check for a ping response
had_ping_reply = true;
}
} while (poll(&fd_read, 1, 0) > 0 && hrt_absolute_time() - read_start < 2_ms);
// Check for a ping response
/* PONG_IN_SESSION_STATUS */
if (session.on_pong_flag == 1) {
had_ping_reply = true;
}
hrt_abstime now = hrt_absolute_time();
const hrt_abstime now = hrt_absolute_time();
if (now - last_status_update > 1_s) {
float dt = (now - last_status_update) / 1e6f;
@@ -287,6 +335,7 @@ void MicroddsClient::run()
}
uxr_ping_agent_session(&session, 0, 1);
had_ping_reply = false;
}
@@ -294,14 +343,14 @@ void MicroddsClient::run()
PX4_INFO("No ping response, disconnecting");
_connected = false;
}
px4_usleep(1000);
}
uxr_delete_session_retries(&session, _connected ? 1 : 0);
_last_payload_tx_rate = 0;
_last_payload_tx_rate = 0;
}
orb_unsubscribe(polling_topic_sub);
}
int MicroddsClient::setBaudrate(int fd, unsigned baud)
@@ -333,6 +382,48 @@ int MicroddsClient::setBaudrate(int fd, unsigned baud)
case 921600: speed = B921600; break;
#ifndef B1000000
#define B1000000 1000000
#endif
case 1000000: speed = B1000000; break;
#ifndef B1500000
#define B1500000 1500000
#endif
case 1500000: speed = B1500000; break;
#ifndef B2000000
#define B2000000 2000000
#endif
case 2000000: speed = B2000000; break;
#ifndef B2500000
#define B2500000 2500000
#endif
case 2500000: speed = B2500000; break;
#ifndef B3000000
#define B3000000 3000000
#endif
case 3000000: speed = B3000000; break;
#ifndef B3500000
#define B3500000 3500000
#endif
case 3500000: speed = B3500000; break;
#ifndef B4000000
#define B4000000 4000000
#endif
case 4000000: speed = B4000000; break;
default:
PX4_ERR("ERR: unknown baudrate: %d", baud);
return -EINVAL;
@@ -399,12 +490,6 @@ int MicroddsClient::setBaudrate(int fd, unsigned baud)
return 0;
}
int microdds_client_main(int argc, char *argv[])
{
return MicroddsClient::main(argc, argv);
}
int MicroddsClient::custom_command(int argc, char *argv[])
{
return print_usage("unknown command");
@@ -414,8 +499,8 @@ int MicroddsClient::task_spawn(int argc, char *argv[])
{
_task_id = px4_task_spawn_cmd("microdds_client",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT - 4,
PX4_STACK_ADJUSTED(8000),
SCHED_PRIORITY_DEFAULT,
PX4_STACK_ADJUSTED(10000),
(px4_main_t)&run_trampoline,
(char *const *)argv);
@@ -442,14 +527,22 @@ MicroddsClient *MicroddsClient::instantiate(int argc, char *argv[])
int ch;
const char *myoptarg = nullptr;
#if defined(MICRODDS_CLIENT_UDP)
Transport transport = Transport::Udp;
const char *device = nullptr;
const char *ip = "127.0.0.1";
int baudrate = 921600;
const char *port = "15555";
bool localhost_only = false;
while ((ch = px4_getopt(argc, argv, "t:d:b:h:p:l", &myoptind, &myoptarg)) != EOF) {
#else
Transport transport = Transport::Serial;
#endif
const char *device = nullptr;
int baudrate = 921600;
const char *port = "8888";
bool localhost_only = false;
const char *ip = "127.0.0.1";
const char *client_namespace = nullptr;//"px4";
while ((ch = px4_getopt(argc, argv, "t:d:b:h:p:l:n:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 't':
if (!strcmp(myoptarg, "serial")) {
@@ -477,6 +570,8 @@ MicroddsClient *MicroddsClient::instantiate(int argc, char *argv[])
break;
#if defined(MICRODDS_CLIENT_UDP)
case 'h':
ip = myoptarg;
break;
@@ -488,6 +583,11 @@ MicroddsClient *MicroddsClient::instantiate(int argc, char *argv[])
case 'l':
localhost_only = true;
break;
#endif // MICRODDS_CLIENT_UDP
case 'n':
client_namespace = myoptarg;
break;
case '?':
error_flag = true;
@@ -511,7 +611,7 @@ MicroddsClient *MicroddsClient::instantiate(int argc, char *argv[])
}
}
return new MicroddsClient(transport, device, baudrate, ip, port, localhost_only);
return new MicroddsClient(transport, device, baudrate, ip, port, localhost_only, client_namespace);
}
int MicroddsClient::print_usage(const char *reason)
@@ -536,9 +636,15 @@ $ microdds_client start -t udp -h 127.0.0.1 -p 15555
PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, "<file:dev>", "serial device", true);
PRINT_MODULE_USAGE_PARAM_INT('b', 0, 0, 3000000, "Baudrate (can also be p:<param_name>)", true);
PRINT_MODULE_USAGE_PARAM_STRING('h', "127.0.0.1", "<IP>", "Host IP", true);
PRINT_MODULE_USAGE_PARAM_INT('p', 15555, 0, 3000000, "Remote Port", true);
PRINT_MODULE_USAGE_PARAM_INT('p', 8888, 0, 65535, "Remote Port", true);
PRINT_MODULE_USAGE_PARAM_FLAG('l', "Restrict to localhost (use in combination with ROS_LOCALHOST_ONLY=1)", true);
PRINT_MODULE_USAGE_PARAM_STRING('n', nullptr, nullptr, "Client DDS namespace", true);
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0;
}
extern "C" __EXPORT int microdds_client_main(int argc, char *argv[])
{
return MicroddsClient::main(argc, argv);
}
src/modules/microdds_client/microdds_client.h
+12 -5
View File
@@ -34,13 +34,13 @@
#pragma once
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <src/modules/micrortps_bridge/micrortps_client/dds_topics.h>
#include <src/modules/microdds_client/dds_topics.h>
extern "C" __EXPORT int microdds_client_main(int argc, char *argv[]);
#include <lib/timesync/Timesync.hpp>
class MicroddsClient : public ModuleBase<MicroddsClient>
class MicroddsClient : public ModuleBase<MicroddsClient>, public ModuleParams
{
public:
enum class Transport {
@@ -49,7 +49,7 @@ public:
};
MicroddsClient(Transport transport, const char *device, int baudrate, const char *host, const char *port,
bool localhost_only);
bool localhost_only, const char *client_namespace);
~MicroddsClient();
@@ -75,6 +75,7 @@ private:
int setBaudrate(int fd, unsigned baud);
const bool _localhost_only;
const char *_client_namespace;
SendTopicsSubs *_subs{nullptr};
RcvTopicsPubs *_pubs{nullptr};
@@ -87,5 +88,11 @@ private:
int _last_payload_tx_rate{}; ///< in B/s
int _last_payload_rx_rate{}; ///< in B/s
bool _connected{false};
Timesync _timesync{};
DEFINE_PARAMETERS(
(ParamInt<px4::params::XRCE_DDS_DOM_ID>) _param_xrce_dds_dom_id
)
};
src/modules/microdds_client/module.yaml
+56
View File
@@ -0,0 +1,56 @@
# parameters to auto start
# mode (normal, minimal)
# UDP port
# max rate
# DDS DOMAIN ID
#
# multiple instances?
module_name: Micro XRCE-DDS
serial_config:
- command: |
if [ $SERIAL_DEV != ethernet ]; then
set XRCE_DDS_ARGS "-t serial -d ${SERIAL_DEV} -b p:${BAUD_PARAM}"
else
set XRCE_DDS_ARGS "-t udp"
fi
microdds_client start ${XRCE_DDS_ARGS}
port_config_param:
name: XRCE_DDS_${i}_CFG
group: Micro XRCE-DDS
# MAVLink instances:
# 0: Telem1 Port (Telemetry Link)
# 1: Telem2 Port (Companion Link). Disabled by default to reduce RAM usage
# 2: Board-specific / no fixed function or port
#default: [TEL1, "", ""]
supports_networking: true
parameters:
- group: Micro XRCE-DDS
definitions:
XRCE_DDS_DOM_ID:
description:
short: XRCE DDS domain ID
long: XRCE DDS domain ID
category: System
type: int32
reboot_required: true
default: 0
XRCE_DDS_UDP_PRT:
description:
short: Micro DDS UDP Port
long: |
If ethernet enabled and selected as configuration for micro DDS,
selected udp port will be set and used.
type: int32
reboot_required: true
default: 8888
requires_ethernet: true
src/modules/microdds_client/utilities.hpp
+138
View File
@@ -0,0 +1,138 @@
#pragma once
#include <uxr/client/client.h>
#include <ucdr/microcdr.h>
#include <uORB/topics/uORBTopics.hpp>
#define TOPIC_NAME_SIZE 128
uxrObjectId topic_id_from_orb(ORB_ID orb_id, uint8_t instance = 0)
{
if (orb_id != ORB_ID::INVALID) {
uint16_t id = static_cast<uint8_t>(orb_id) + (instance * UINT8_MAX);
uxrObjectId topic_id = uxr_object_id(id, UXR_TOPIC_ID);
return topic_id;
}
return uxrObjectId{};
}
static bool generate_topic_name(char *topic, const char *client_namespace, const char *direction, const char *name)
{
if (client_namespace != nullptr) {
int ret = snprintf(topic, TOPIC_NAME_SIZE, "rt/%s/fmu/%s/%s", client_namespace, direction, name);
return (ret > 0 && ret < TOPIC_NAME_SIZE);
}
int ret = snprintf(topic, TOPIC_NAME_SIZE, "rt/fmu/%s/%s", direction, name);
return (ret > 0 && ret < TOPIC_NAME_SIZE);
}
static bool create_data_writer(uxrSession *session, uxrStreamId reliable_out_stream_id, uxrObjectId participant_id,
ORB_ID orb_id, const char *client_namespace, const char *topic_name_simple, const char *type_name,
uxrObjectId &datawriter_id)
{
// topic
char topic_name[TOPIC_NAME_SIZE];
if (!generate_topic_name(topic_name, client_namespace, "out", topic_name_simple)) {
PX4_ERR("topic path too long");
return false;
}
uxrObjectId topic_id = topic_id_from_orb(orb_id);
uint16_t topic_req = uxr_buffer_create_topic_bin(session, reliable_out_stream_id, topic_id, participant_id, topic_name,
type_name, UXR_REPLACE);
// publisher
uxrObjectId publisher_id = uxr_object_id(topic_id.id, UXR_PUBLISHER_ID);
uint16_t publisher_req = uxr_buffer_create_publisher_bin(session, reliable_out_stream_id, publisher_id, participant_id,
UXR_REPLACE);
// data writer
datawriter_id = uxr_object_id(topic_id.id, UXR_DATAWRITER_ID);
uxrQoS_t qos = {
.durability = UXR_DURABILITY_TRANSIENT_LOCAL,
.reliability = UXR_RELIABILITY_BEST_EFFORT,
.history = UXR_HISTORY_KEEP_LAST,
.depth = 0,
};
uint16_t datawriter_req = uxr_buffer_create_datawriter_bin(session, reliable_out_stream_id, datawriter_id, publisher_id,
topic_id, qos, UXR_REPLACE);
// Send create entities message and wait its status
uint16_t requests[3] {topic_req, publisher_req, datawriter_req};
uint8_t status[3];
if (!uxr_run_session_until_all_status(session, 1000, requests, status, 3)) {
PX4_ERR("create entities failed: %s, topic: %i publisher: %i datawriter: %i",
topic_name, status[0], status[1], status[2]);
return false;
} else {
PX4_INFO("successfully created %s data writer, topic id: %d", topic_name, topic_id.id);
}
return true;
}
static bool create_data_reader(uxrSession *session, uxrStreamId reliable_out_stream_id, uxrStreamId input_stream_id,
uxrObjectId participant_id, uint16_t index, const char *client_namespace, const char *topic_name_simple,
const char *type_name)
{
// topic
char topic_name[TOPIC_NAME_SIZE];
if (!generate_topic_name(topic_name, client_namespace, "in", topic_name_simple)) {
PX4_ERR("topic path too long");
return false;
}
uint16_t id = index + 1000;
uxrObjectId topic_id = uxr_object_id(id, UXR_TOPIC_ID);
uint16_t topic_req = uxr_buffer_create_topic_bin(session, reliable_out_stream_id, topic_id, participant_id, topic_name,
type_name, UXR_REPLACE);
// subscriber
uxrObjectId subscriber_id = uxr_object_id(id, UXR_SUBSCRIBER_ID);
uint16_t subscriber_req = uxr_buffer_create_subscriber_bin(session, reliable_out_stream_id, subscriber_id,
participant_id, UXR_REPLACE);
// data reader
uxrObjectId datareader_id = uxr_object_id(id, UXR_DATAREADER_ID);
uxrQoS_t qos = {
.durability = UXR_DURABILITY_TRANSIENT_LOCAL,
.reliability = UXR_RELIABILITY_BEST_EFFORT,
.history = UXR_HISTORY_KEEP_LAST,
.depth = 0,
};
uint16_t datareader_req = uxr_buffer_create_datareader_bin(session, reliable_out_stream_id, datareader_id,
subscriber_id, topic_id, qos, UXR_REPLACE);
uint16_t requests[3] {topic_req, subscriber_req, datareader_req};
uint8_t status[3];
if (!uxr_run_session_until_all_status(session, 1000, requests, status, 3)) {
PX4_ERR("create entities failed: %s %i %i %i", topic_name,
status[0], status[1], status[2]);
return false;
}
uxrDeliveryControl delivery_control{};
delivery_control.max_samples = UXR_MAX_SAMPLES_UNLIMITED;
uxr_buffer_request_data(session, reliable_out_stream_id, datareader_id, input_stream_id, &delivery_control);
return true;
}
src/modules/micrortps_bridge/CMakeLists.txt
-193
View File
@@ -1,193 +0,0 @@
############################################################################
#
# Copyright (c) 2017 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.
#
############################################################################
#=============================================================================
# RTPS and micro-cdr
#
find_program(FASTRTPSGEN fastrtpsgen PATHS $ENV{FASTRTPSGEN_DIR})
if(NOT FASTRTPSGEN)
message(FATAL_ERROR "Unable to find fastrtpsgen")
else()
execute_process(
COMMAND $ENV{FASTRTPSGEN_DIR}fastrtpsgen -version
OUTPUT_VARIABLE FASTRTPSGEN_VERSION
OUTPUT_STRIP_TRAILING_WHITESPACE
ERROR_QUIET
)
message(STATUS "${FASTRTPSGEN_VERSION}")
endif()
set_property(DIRECTORY APPEND PROPERTY CMAKE_CONFIGURE_DEPENDS
${PX4_SOURCE_DIR}/msg/tools/urtps_bridge_topics.yaml
)
if (EXISTS "${PX4_SOURCE_DIR}/msg/tools/urtps_bridge_topics.yaml")
set(config_rtps_send_topics)
execute_process(
COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/msg/tools/uorb_rtps_classifier.py -sa
OUTPUT_VARIABLE config_rtps_send_topics
)
set(config_rtps_send_alias_topics "")
string(FIND ${config_rtps_send_topics} "alias" found_send_alias)
if (NOT ${found_send_alias} EQUAL "-1")
STRING(REGEX REPLACE ".*alias " "" config_rtps_send_alias_topics "${config_rtps_send_topics}")
STRING(REPLACE ", " ";" config_rtps_send_alias_topics "${config_rtps_send_alias_topics}")
STRING(REPLACE "\n" "" config_rtps_send_alias_topics "${config_rtps_send_alias_topics}")
STRING(REGEX REPLACE " alias.*" "" config_rtps_send_topics "${config_rtps_send_topics}")
endif()
STRING(REGEX REPLACE ", " ";" config_rtps_send_topics "${config_rtps_send_topics}")
STRING(REGEX REPLACE "\n" "" config_rtps_send_topics "${config_rtps_send_topics}")
set(config_rtps_receive_topics)
execute_process(
COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/msg/tools/uorb_rtps_classifier.py -ra
OUTPUT_VARIABLE config_rtps_receive_topics
)
set(config_rtps_receive_alias_topics "")
string(FIND ${config_rtps_receive_topics} "alias" found_receive_alias)
if (NOT ${found_receive_alias} EQUAL "-1")
STRING(REGEX REPLACE ".*alias " "" config_rtps_receive_alias_topics "${config_rtps_receive_topics}")
STRING(REPLACE ", " ";" config_rtps_receive_alias_topics "${config_rtps_receive_alias_topics}")
STRING(REPLACE "\n" "" config_rtps_receive_alias_topics "${config_rtps_receive_alias_topics}")
STRING(REGEX REPLACE " alias.*" "" config_rtps_receive_topics "${config_rtps_receive_topics}")
endif()
STRING(REPLACE ", " ";" config_rtps_receive_topics "${config_rtps_receive_topics}")
STRING(REPLACE "\n" "" config_rtps_receive_topics "${config_rtps_receive_topics}")
endif()
if (FASTRTPSGEN AND (config_rtps_send_topics OR config_rtps_receive_topics))
option(GENERATE_RTPS_BRIDGE "enable RTPS and microCDR" ON)
endif()
if (GENERATE_RTPS_BRIDGE)
add_subdirectory(micrortps_client)
###############################################################################
# micro-cdr serialization
###############################################################################
include(px4_git)
px4_add_git_submodule(TARGET git_micro_cdr PATH micro-CDR)
set(UCDR_SUPERBUILD CACHE BOOL "Disable micro-CDR superbuild compilation.")
add_subdirectory(micro-CDR)
set(msg_out_path_microcdr ${PX4_BINARY_DIR}/uORB_microcdr/topics)
set(msg_source_out_path_microcdr ${CMAKE_CURRENT_BINARY_DIR}/topics_microcdr_sources)
set(uorb_headers_microcdr)
set(uorb_sources_microcdr)
# send topic files
STRING(REGEX REPLACE ";" ", " send_list "${config_rtps_send_topics};${config_rtps_send_alias_topics}")
message(STATUS "microRTPS bridge:")
message(STATUS " Publish to the bridge from: ${send_list}")
set(send_topic_files)
foreach(topic ${config_rtps_send_topics})
list(APPEND send_topic_files ${PX4_SOURCE_DIR}/msg/${topic}.msg)
list(APPEND uorb_headers_microcdr ${msg_out_path_microcdr}/${topic}.h)
list(APPEND uorb_sources_microcdr ${msg_source_out_path_microcdr}/${topic}.cpp)
endforeach()
# receive topic files
STRING(REGEX REPLACE ";" ", " rcv_list "${config_rtps_receive_topics};${config_rtps_receive_alias_topics}")
message(STATUS " Subscribe from the bridge to: ${rcv_list}")
set(receive_topic_files)
foreach(topic ${config_rtps_receive_topics})
list(APPEND receive_topic_files ${PX4_SOURCE_DIR}/msg/${topic}.msg)
list(APPEND uorb_headers_microcdr ${msg_out_path_microcdr}/${topic}.h)
list(APPEND uorb_sources_microcdr ${msg_source_out_path_microcdr}/${topic}.cpp)
endforeach()
list(REMOVE_DUPLICATES uorb_headers_microcdr)
list(REMOVE_DUPLICATES uorb_sources_microcdr)
# Generate uORB serialization headers
add_custom_command(OUTPUT ${uorb_headers_microcdr}
COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/msg/tools/px_generate_uorb_topic_files.py
--headers
-f ${send_topic_files} ${receive_topic_files}
-i ${PX4_SOURCE_DIR}/msg/
-o ${msg_out_path_microcdr}
-e ${PX4_SOURCE_DIR}/msg/templates/uorb_microcdr
-t ${CMAKE_CURRENT_BINARY_DIR}/tmp/headers_microcdr
-q
DEPENDS
${receive_topic_files}
${send_topic_files}
${PX4_SOURCE_DIR}/msg/tools/px_generate_uorb_topic_files.py
${PX4_SOURCE_DIR}/msg/tools/urtps_bridge_topics.yaml
COMMENT "Generating uORB microcdr topic headers"
VERBATIM
)
add_custom_target(uorb_headers_microcdr_gen DEPENDS ${uorb_headers_microcdr})
# Generate uORB serialization sources
add_custom_command(OUTPUT ${uorb_sources_microcdr}
COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/msg/tools/px_generate_uorb_topic_files.py
--sources
-f ${send_topic_files} ${receive_topic_files}
-i ${PX4_SOURCE_DIR}/msg/
-o ${msg_source_out_path_microcdr}
-e ${PX4_SOURCE_DIR}/msg/templates/uorb_microcdr
-t ${CMAKE_CURRENT_BINARY_DIR}/tmp/sources_microcdr
-q
DEPENDS
${receive_topic_files}
${send_topic_files}
${PX4_SOURCE_DIR}/msg/tools/px_generate_uorb_topic_files.py
${PX4_SOURCE_DIR}/msg/tools/urtps_bridge_topics.yaml
COMMENT "Generating uORB microcdr topic sources"
VERBATIM
)
add_custom_target(uorb_sources_microcdr_gen DEPENDS ${uorb_sources_microcdr})
px4_add_library(uorb_msgs_microcdr ${uorb_sources_microcdr} ${uorb_headers_microcdr})
add_dependencies(uorb_msgs_microcdr
uorb_headers_microcdr_gen
uorb_sources_microcdr_gen
git_micro_cdr
microcdr
)
add_dependencies(microcdr prebuild_targets)
# microCDR
target_include_directories(uorb_msgs_microcdr
PUBLIC
${CMAKE_CURRENT_SOURCE_DIR}/micro-CDR/include
${CMAKE_CURRENT_BINARY_DIR}/micro-CDR/include
${CMAKE_CURRENT_BINARY_DIR}/micro-CDR/include/microcdr
)
target_link_libraries(uorb_msgs_microcdr PRIVATE microcdr)
endif()
src/modules/micrortps_bridge/Kconfig
-12
View File
@@ -1,12 +0,0 @@
menuconfig MODULES_MICRORTPS_BRIDGE
bool "micrortps_bridge"
default n
---help---
Enable support for micrortps_bridge
menuconfig USER_MICRORTPS_BRIDGE
bool "micrortps_bridge running as userspace module"
default y
depends on BOARD_PROTECTED && MODULES_MICRORTPS_BRIDGE
---help---
Put micrortps_bridge in userspace memory
src/modules/micrortps_bridge/README.md
-1
View File
@@ -1 +0,0 @@
For see a complete documentation, please follow this [link](https://docs.px4.io/main/en/middleware/micrortps.html)
src/modules/micrortps_bridge/micro-CDR
-1
Submodule src/modules/micrortps_bridge/micro-CDR deleted from 21d3cfe3ae
src/modules/micrortps_bridge/micrortps_client/CMakeLists.txt
-120
View File
@@ -1,120 +0,0 @@
############################################################################
#
# Copyright (c) 2015 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.
#
############################################################################
set(msg_out_path ${CMAKE_CURRENT_BINARY_DIR})
get_filename_component(micrortps_bridge_path ${msg_out_path} PATH)
option(BUILD_MICRORTPS_AGENT "enable building the micrortps_agent after its generation" OFF)
if (NOT "${config_rtps_send_topics}" STREQUAL "" OR NOT "${config_rtps_receive_topics}" STREQUAL "")
set(send_topic_files)
foreach(topic ${config_rtps_send_topics})
list(APPEND send_topic_files ${PX4_SOURCE_DIR}/msg/${topic}.msg)
endforeach()
set(receive_topic_files)
foreach(topic ${config_rtps_receive_topics})
list(APPEND receive_topic_files ${PX4_SOURCE_DIR}/msg/${topic}.msg)
endforeach()
if (NOT "${config_rtps_send_alias_topics}" STREQUAL "")
set(config_rtps_send_topics "${config_rtps_send_topics};${config_rtps_send_alias_topics}")
endif()
if (NOT "${config_rtps_receive_alias_topics}" STREQUAL "")
set(config_rtps_receive_topics "${config_rtps_receive_topics};${config_rtps_receive_alias_topics}")
endif()
foreach(topic ${config_rtps_send_topics})
list(APPEND topic_bridge_files_out ${micrortps_bridge_path}/micrortps_agent/src/${topic}_Publisher.cpp)
list(APPEND topic_bridge_files_out ${micrortps_bridge_path}/micrortps_agent/src/${topic}_Publisher.h)
endforeach()
foreach(topic ${config_rtps_receive_topics})
list(APPEND topic_bridge_files_out ${micrortps_bridge_path}/micrortps_agent/src/${topic}_Subscriber.cpp)
list(APPEND topic_bridge_files_out ${micrortps_bridge_path}/micrortps_agent/src/${topic}_Subscriber.h)
endforeach()
list(APPEND topic_bridge_files_out
${micrortps_bridge_path}/micrortps_client/microRTPS_client.cpp
${micrortps_bridge_path}/micrortps_client/microRTPS_transport.cpp
${micrortps_bridge_path}/micrortps_client/microRTPS_transport.h
${micrortps_bridge_path}/micrortps_client/dds_topics.h
)
add_custom_command(OUTPUT ${topic_bridge_files_out}
COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/msg/tools/generate_microRTPS_bridge.py
--fastrtpsgen-dir $ENV{FASTRTPSGEN_DIR}
--generate-idl
--mkdir-build
--generate-cmakelists
--topic-msg-dir ${PX4_SOURCE_DIR}/msg
--uorb-templates-dir templates/uorb_microcdr
--urtps-templates-dir templates/urtps
--agent-outdir ${micrortps_bridge_path}/micrortps_agent/src
--client-outdir ${micrortps_bridge_path}/micrortps_client
--idl-dir ${micrortps_bridge_path}/micrortps_agent/idl
>micrortps_bridge.log 2>&1 || cat micrortps_bridge.log
DEPENDS ${send_topic_files} ${receive_topic_files} ${PX4_SOURCE_DIR}/msg/tools/urtps_bridge_topics.yaml
${PX4_SOURCE_DIR}/msg/templates/uorb_microcdr/dds_topics.h.em
COMMENT "Generating RTPS topic bridge"
)
add_custom_target(topic_bridge_files DEPENDS ${topic_bridge_files_out})
px4_add_module(
MODULE modules__micrortps_bridge__micrortps_client
MAIN micrortps_client
STACK_MAIN 4096
INCLUDES
${CMAKE_CURRENT_SOURCE_DIR}
${micrortps_bridge_path}/micrortps_client
SRCS
microRTPS_client_main.cpp
${micrortps_bridge_path}/micrortps_client/microRTPS_client.cpp
${micrortps_bridge_path}/micrortps_client/microRTPS_transport.cpp
MODULE_CONFIG
module.yaml
DEPENDS
topic_bridge_files
)
target_link_libraries(modules__micrortps_bridge__micrortps_client PRIVATE uorb_msgs_microcdr)
if (BUILD_MICRORTPS_AGENT)
add_custom_command(TARGET modules__micrortps_bridge__micrortps_client POST_BUILD
COMMAND ${PX4_SOURCE_DIR}/Tools/build_micrortps_agent.sh
WORKING_DIRECTORY ${PX4_SOURCE_DIR}
COMMENT "Building micrortps_agent..."
)
# add_subdirectory(${micrortps_bridge_path}/micrortps_agent)
endif()
endif()
src/modules/micrortps_bridge/micrortps_client/microRTPS_client.h
-99
View File
@@ -1,99 +0,0 @@
/****************************************************************************
*
* Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima).
* Copyright (c) 2019-2021 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 of the copyright holder 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 HOLDER 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.
*
****************************************************************************/
#pragma once
#include <microRTPS_transport.h>
#include <inttypes.h>
#include <cstdio>
#include <ctime>
#include <pthread.h>
#include <termios.h>
#include <ucdr/microcdr.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/tasks.h>
#include <px4_platform_common/time.h>
#include <uORB/uORB.h>
#define LOOPS -1
#define SLEEP_US 1000
#define MAX_SLEEP_US 1000000
#define BAUDRATE 460800
#define MAX_DATA_RATE 10000000
#define DEVICE "/dev/ttyACM0"
#define POLL_MS 1
#define MAX_POLL_MS 1000
#define DEFAULT_IP "127.0.0.1"
#define DEFAULT_RECV_PORT 2019
#define DEFAULT_SEND_PORT 2020
#define MIN_TX_INTERVAL_US 1000.f
#define MAX_TX_INTERVAL_US 1000000.f
void *send(void *args);
void micrortps_start_topics(const uint32_t &datarate, struct timespec &begin, uint64_t &total_rcvd,
uint64_t &total_sent, uint64_t &sent_last_sec,
uint64_t &rcvd_last_sec, uint64_t &received, uint64_t &sent, int &rcvd_loop, int &sent_loop);
struct baudtype {
speed_t code;
uint32_t val;
};
struct options {
enum class eTransports {
UART,
UDP
};
eTransports transport = options::eTransports::UART;
char device[64] = DEVICE;
char ip[16] = DEFAULT_IP;
uint16_t recv_port = DEFAULT_RECV_PORT;
uint16_t send_port = DEFAULT_SEND_PORT;
uint32_t sleep_us = SLEEP_US;
uint32_t baudrate = BAUDRATE;
uint32_t datarate = 0;
uint32_t poll_ms = POLL_MS;
int loops = LOOPS;
bool sw_flow_control = false;
bool hw_flow_control = false;
bool verbose_debug = false;
};
extern struct options _options;
extern bool _should_exit_task;
extern Transport_node *transport_node;
src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp
-330
View File
@@ -1,330 +0,0 @@
/****************************************************************************
*
* Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima).
* Copyright (c) 2019-2021 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 of the copyright holder 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 HOLDER 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.
*
****************************************************************************/
#include <microRTPS_transport.h>
#include <microRTPS_client.h>
#include <inttypes.h>
#include <cstdio>
#include <cstdlib>
#include <ctime>
#include <termios.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/cli.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/tasks.h>
#include <px4_platform_common/time.h>
extern "C" __EXPORT int micrortps_client_main(int argc, char *argv[]);
static int _rtps_task = -1;
bool _should_exit_task = false;
Transport_node *transport_node = nullptr;
struct options _options;
struct timespec begin;
struct timespec end;
uint64_t total_rcvd{0};
uint64_t total_sent{0};
uint64_t rcvd_last_sec{0};
uint64_t sent_last_sec{0};
uint64_t received{0};
uint64_t sent{0};
int rcv_loop{0};
int send_loop{0};
static void usage(const char *name)
{
PRINT_MODULE_USAGE_NAME("micrortps_client", "communication");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_PARAM_STRING('t', "UART", "UART|UDP", "Transport protocol", true);
PRINT_MODULE_USAGE_PARAM_STRING('d', "/dev/ttyACM0", "<file:dev>", "Select Serial Device", true);
PRINT_MODULE_USAGE_PARAM_INT('b', 460800, 9600, 3000000, "Baudrate (can also be p:<param_name>)", true);
PRINT_MODULE_USAGE_PARAM_INT('m', 0, 0, MAX_DATA_RATE, "Maximum sending data rate in B/s (0=not limited)", true);
PRINT_MODULE_USAGE_PARAM_INT('p', 1, 1, MAX_POLL_MS, "Poll timeout for UART in milliseconds", true);
PRINT_MODULE_USAGE_PARAM_INT('l', -1, -1, INT32_MAX, "Limit number of iterations until the program exits (-1=infinite)",
true);
PRINT_MODULE_USAGE_PARAM_INT('w', 1000, 0, MAX_SLEEP_US,
"Iteration time for data publishing to the uORB side, in microseconds", true);
PRINT_MODULE_USAGE_PARAM_INT('r', 2019, 0, 65536, "Select UDP Network Port for receiving (local)", true);
PRINT_MODULE_USAGE_PARAM_INT('s', 2020, 0, 65536, "Select UDP Network Port for sending (remote)", true);
PRINT_MODULE_USAGE_PARAM_STRING('i', "127.0.0.1", "<x.x.x.x>", "Select IP address (remote)", true);
PRINT_MODULE_USAGE_PARAM_FLAG('f', "Activate UART link SW flow control", true);
PRINT_MODULE_USAGE_PARAM_FLAG('h', "Activate UART link HW flow control", true);
PRINT_MODULE_USAGE_PARAM_FLAG('v', "Add more verbosity", true);
PRINT_MODULE_USAGE_COMMAND("stop");
PRINT_MODULE_USAGE_COMMAND("status");
}
static int parse_options(int argc, char *argv[])
{
int ch;
int myoptind = 1;
const char *myoptarg = nullptr;
while ((ch = px4_getopt(argc, argv, "t:d:l:w:b:m:p:r:s:i:fhv", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 't': _options.transport = strcmp(myoptarg, "UDP") == 0 ?
options::eTransports::UDP
: options::eTransports::UART; break;
case 'd': if (nullptr != myoptarg) strcpy(_options.device, myoptarg); break;
case 'l': _options.loops = strtol(myoptarg, nullptr, 10); break;
case 'w': _options.sleep_us = strtoul(myoptarg, nullptr, 10); break;
case 'b': {
int baudrate = 0;
if (px4_get_parameter_value(myoptarg, baudrate) != 0) {
PX4_ERR("baudrate parsing failed");
}
_options.baudrate = baudrate;
break;
}
case 'm': {
int datarate = 0;
if (px4_get_parameter_value(myoptarg, datarate) != 0) {
PX4_ERR("datarate parsing failed");
}
_options.datarate = datarate;
break;
}
case 'p': _options.poll_ms = strtoul(myoptarg, nullptr, 10); break;
case 'r': _options.recv_port = strtoul(myoptarg, nullptr, 10); break;
case 's': _options.send_port = strtoul(myoptarg, nullptr, 10); break;
case 'i': if (nullptr != myoptarg) strcpy(_options.ip, myoptarg); break;
case 'f': _options.sw_flow_control = true; break;
case 'h': _options.hw_flow_control = true; break;
case 'v': _options.verbose_debug = true; break;
default:
usage(argv[1]);
return -1;
}
}
if (_options.datarate > MAX_DATA_RATE) {
_options.datarate = MAX_DATA_RATE;
PX4_WARN("Data rate too high. Using max datarate of %d B/s instead", MAX_DATA_RATE);
}
if (_options.poll_ms < POLL_MS) {
_options.poll_ms = POLL_MS;
PX4_WARN("Poll timeout too low. Using %d ms instead", POLL_MS);
} else if (_options.poll_ms > MAX_POLL_MS) {
_options.poll_ms = MAX_POLL_MS;
PX4_WARN("Poll timeout too high. Using %d ms instead", MAX_POLL_MS);
}
if (_options.sleep_us > MAX_SLEEP_US) {
_options.sleep_us = MAX_SLEEP_US;
PX4_WARN("Publishing iteration cycle too slow. Using %d us instead", MAX_SLEEP_US);
}
if (_options.hw_flow_control && _options.sw_flow_control) {
PX4_ERR("HW and SW flow control set. Please set only one or another");
return -1;
}
return 0;
}
static int micrortps_start(int argc, char *argv[])
{
if (0 > parse_options(argc, argv)) {
PX4_INFO("EXITING...");
_rtps_task = -1;
return -1;
}
// Set the system ID to FMU, in order to identify the client side
const uint8_t sys_id = static_cast<uint8_t>(MicroRtps::System::FMU);
switch (_options.transport) {
case options::eTransports::UART: {
transport_node = new UART_node(_options.device, _options.baudrate, _options.poll_ms,
_options.sw_flow_control, _options.hw_flow_control, sys_id,
_options.verbose_debug);
PX4_INFO("UART transport: device: %s; baudrate: %" PRIu32 "; poll: %" PRIu32 "ms; flow_control: %s",
_options.device, _options.baudrate, _options.poll_ms,
_options.sw_flow_control ? "SW enabled" : (_options.hw_flow_control ? "HW enabled" : "No"));
}
break;
case options::eTransports::UDP: {
transport_node = new UDP_node(_options.ip, _options.recv_port, _options.send_port,
sys_id, _options.verbose_debug);
PX4_INFO("UDP transport: ip address: %s; recv port: %" PRIu16 "; send port: %" PRIu16,
_options.ip, _options.recv_port, _options.send_port);
}
break;
default:
_rtps_task = -1;
PX4_INFO("EXITING...");
return -1;
}
if (0 > transport_node->init()) {
PX4_INFO("EXITING...");
_rtps_task = -1;
return -1;
}
micrortps_start_topics(_options.datarate, begin, total_rcvd, total_sent, sent_last_sec, rcvd_last_sec, received, sent,
rcv_loop,
send_loop);
px4_clock_gettime(CLOCK_REALTIME, &end);
const double elapsed_secs = static_cast<double>(end.tv_sec - begin.tv_sec + (end.tv_nsec - begin.tv_nsec) / 1e9);
PX4_INFO("RECEIVED: %" PRIu64 " messages in %d LOOPS, %" PRIu64 " bytes in %.03f seconds - avg %.02fKB/s",
received, rcv_loop, total_rcvd, elapsed_secs, static_cast<double>(total_rcvd / (1e3 * elapsed_secs)));
PX4_INFO("SENT: %" PRIu64 " messages in %d LOOPS, %" PRIu64 " bytes in %.03f seconds - avg %.02fKB/s",
sent, send_loop, total_sent, elapsed_secs, total_sent / (1e3 * elapsed_secs));
delete transport_node;
transport_node = nullptr;
PX4_INFO("Stopped!");
fflush(stdout);
_rtps_task = -1;
return 0;
}
int micrortps_client_main(int argc, char *argv[])
{
if (argc < 2) {
usage(argv[0]);
return -1;
}
if (!strcmp(argv[1], "start")) {
if (_rtps_task != -1) {
PX4_INFO("Already running");
return -1;
}
_rtps_task = px4_task_spawn_cmd("micrortps_client",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
PX4_STACK_ADJUSTED(2900),
(px4_main_t) micrortps_start,
(char *const *)argv);
if (_rtps_task < 0) {
PX4_WARN("Could not start task");
_rtps_task = -1;
return -1;
}
return 0;
}
if (!strcmp(argv[1], "status")) {
if (_rtps_task == -1) {
PX4_INFO("Not running");
} else {
px4_clock_gettime(CLOCK_REALTIME, &end);
const double elapsed_secs = static_cast<double>(end.tv_sec - begin.tv_sec + (end.tv_nsec - begin.tv_nsec) / 1e9);
printf("\tup and running for %.03f seconds\n", elapsed_secs);
printf("\tnr. of messages received: %" PRIu64 "\n", received);
printf("\tnr. of messages sent: %" PRIu64 "\n", sent);
printf("\ttotal data read: %" PRIu64 " bytes\n", total_rcvd);
printf("\ttotal data sent: %" PRIu64 " bytes\n", total_sent);
printf("\trates:\n");
printf("\t rx: %.3f kB/s\n", rcvd_last_sec / 1e3);
printf("\t tx: %.3f kB/s\n", sent_last_sec / 1e3);
printf("\t avg rx: %.3f kB/s\n", total_rcvd / (1e3 * elapsed_secs));
printf("\t avg tx: %.3f kB/s\n", total_sent / (1e3 * elapsed_secs));
printf("\t tx rate max:");
if (_options.datarate != 0) {
printf(" %.1f kB/s\n", _options.datarate / 1e3);
} else {
printf(" Unlimited\n");
}
}
return 0;
}
if (!strcmp(argv[1], "stop")) {
if (_rtps_task == -1) {
PX4_INFO("Not running");
return -1;
}
_should_exit_task = true;
if (nullptr != transport_node) { transport_node->close(); }
_rtps_task = -1;
return 0;
}
usage(argv[0]);
return -1;
}
src/modules/micrortps_bridge/micrortps_client/module.yaml
-38
View File
@@ -1,38 +0,0 @@
module_name: RTPS
serial_config:
- command: |
protocol_splitter start ${SERIAL_DEV}
mavlink start -d /dev/mavlink -b p:${BAUD_PARAM} -m onboard -r 5000 -x
micrortps_client start -d /dev/rtps -b p:${BAUD_PARAM} -m p:RTPS_RATE -l -1
port_config_param:
name: RTPS_MAV_CONFIG
group: RTPS
label: MAVLink + FastRTPS
- command: |
micrortps_client start -d ${SERIAL_DEV} -b p:${BAUD_PARAM} -m p:RTPS_RATE -l -1
port_config_param:
name: RTPS_CONFIG
group: RTPS
label: FastRTPS
parameters:
- group: RTPS
definitions:
RTPS_RATE:
description:
short: Maximum RTPS data rate
long: |
Configure the maximum sending rate for the RTPS streams in Bytes/sec.
If the configured streams exceed the maximum rate, the sending rate of
each stream is automatically decreased.
0 is unlimited. Note this can cause reliability issues
if enough RTPS topics are selected that exceed the
serial bus limit.
type: int32
min: 0
unit: B/s
reboot_required: true
default: 0
src/modules/micrortps_bridge/res/basic_example_flow.png
BIN
View File
Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 178 KiB

src/modules/simulation/simulator_mavlink/sitl_targets_gazebo.cmake
+2 -2
View File
@@ -29,8 +29,8 @@ if(parallel_jobs LESS 1)
set(parallel_jobs 1)
endif()
message(DEBUG "${NUMBER_OF_LOGICAL_CORES} logical cores detected and ${AVAILABLE_PHYSICAL_MEMORY} megabytes of memory available.
Limiting sitl_gazebo concurrent jobs to ${parallel_jobs}")
# message(DEBUG "${NUMBER_OF_LOGICAL_CORES} logical cores detected and ${AVAILABLE_PHYSICAL_MEMORY} megabytes of memory available.
# Limiting sitl_gazebo concurrent jobs to ${parallel_jobs}")
# project to build sitl_gazebo if necessary
px4_add_git_submodule(TARGET git_gazebo PATH "${PX4_SOURCE_DIR}/Tools/simulation/gazebo/sitl_gazebo")