fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fixed Linux driver: SocketCan driver adds time offset from the provided clock instance, which fixes time synchronization in PerDriverPrivate clock adjustment mode

Browse Source
This commit is contained in:
Pavel Kirienko 2014-04-17 16:14:39 +04:00
parent bd27ab02ac
commit c17a2bbd5b
3 changed files with 42 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libuavcan_drivers/linux/include/uavcan_linux/helpers.hpp
View File

@ -31,12 +31,12 @@ class DefaultLogSink : public uavcan::ILogSink
*/
struct DriverPack
{
SocketCanDriver can;
SystemClock clock;
SocketCanDriver can;
DriverPack(ClockAdjustmentMode clock_adjustment_mode)
: can()
, clock(clock_adjustment_mode)
: clock(clock_adjustment_mode)
, can(clock)
{ }
};

17
libuavcan_drivers/linux/include/uavcan_linux/socketcan.hpp
View File

@ -116,7 +116,7 @@ class SocketCanIface : public uavcan::ICanIface
{ }
};
const SystemClock clock_;
const SystemClock& clock_;
const int fd_;
const unsigned max_frames_in_socket_tx_queue_;
@ -280,6 +280,7 @@ class SocketCanIface : public uavcan::ICanIface
}
if (accept)
{
rx.ts_utc += clock_.getPrivateAdjustment();
rx_queue_.push(rx);
}
}
@ -298,8 +299,9 @@ public:
/**
* Takes ownership of socket's file descriptor.
*/
explicit SocketCanIface(int socket_fd, int max_frames_in_socket_tx_queue = 3)
: fd_(socket_fd)
SocketCanIface(const SystemClock& clock, int socket_fd, int max_frames_in_socket_tx_queue = 3)
: clock_(clock)
, fd_(socket_fd)
, max_frames_in_socket_tx_queue_(max_frames_in_socket_tx_queue)
, frames_in_socket_tx_queue_(0)
{
@ -487,14 +489,15 @@ public:
static constexpr unsigned MaxIfaces = uavcan::MaxCanIfaces;
private:
const SystemClock clock_;
const SystemClock& clock_;
uavcan::LazyConstructor<SocketCanIface> ifaces_[MaxIfaces];
::pollfd pollfds_[MaxIfaces];
std::uint8_t num_ifaces_;
public:
SocketCanDriver()
: num_ifaces_(0)
explicit SocketCanDriver(const SystemClock& clock)
: clock_(clock)
, num_ifaces_(0)
{
for (auto& p : pollfds_)
{
@ -601,7 +604,7 @@ public:
// Construct the iface - upon successful construction the iface will take ownership of the fd.
try
{
ifaces_[num_ifaces_].construct<int>(fd);
ifaces_[num_ifaces_].construct<const SystemClock&, int>(clock_, fd);
}
catch (...)
{

37
libuavcan_drivers/linux/test/test_socket.cpp
View File

@ -32,8 +32,16 @@ static void testSocketRxTx(const std::string& iface_name)
const int sock2 = uavcan_linux::SocketCanIface::openSocket(iface_name);
ENFORCE(sock1 >= 0 && sock2 >= 0);
uavcan_linux::SocketCanIface if1(sock1);
uavcan_linux::SocketCanIface if2(sock2);
/*
* Clocks will have some offset from the true system time
* SocketCAN driver must handle this correctly
*/
uavcan_linux::SystemClock clock_impl(uavcan_linux::ClockAdjustmentMode::PerDriverPrivate);
clock_impl.adjustUtc(uavcan::UtcDuration::fromMSec(100000));
const uavcan_linux::SystemClock& clock = clock_impl;
uavcan_linux::SocketCanIface if1(clock, sock1);
uavcan_linux::SocketCanIface if2(clock, sock2);
/*
* Sending two frames, one of which must be returned back
@ -69,8 +77,6 @@ static void testSocketRxTx(const std::string& iface_name)
uavcan::UtcTime ts_utc;
uavcan::CanIOFlags flags = 0;
const uavcan_linux::SystemClock clock(uavcan_linux::ClockAdjustmentMode::PerDriverPrivate);
/*
* Read first
*/
@ -132,8 +138,16 @@ static void testSocketFilters(const std::string& iface_name)
const int sock2 = uavcan_linux::SocketCanIface::openSocket(iface_name);
ENFORCE(sock1 >= 0 && sock2 >= 0);
uavcan_linux::SocketCanIface if1(sock1);
uavcan_linux::SocketCanIface if2(sock2);
/*
* Clocks will have some offset from the true system time
* SocketCAN driver must handle this correctly
*/
uavcan_linux::SystemClock clock_impl(uavcan_linux::ClockAdjustmentMode::PerDriverPrivate);
clock_impl.adjustUtc(uavcan::UtcDuration::fromMSec(-1000));
const uavcan_linux::SystemClock& clock = clock_impl;
uavcan_linux::SocketCanIface if1(clock, sock1);
uavcan_linux::SocketCanIface if2(clock, sock2);
/*
* Configuring filters
@ -199,7 +213,15 @@ static void testSocketFilters(const std::string& iface_name)
static void testDriver(const std::vector<std::string>& iface_names)
{
uavcan_linux::SocketCanDriver driver;
/*
* Clocks will have some offset from the true system time
* SocketCAN driver must handle this correctly
*/
uavcan_linux::SystemClock clock_impl(uavcan_linux::ClockAdjustmentMode::PerDriverPrivate);
clock_impl.adjustUtc(uavcan::UtcDuration::fromMSec(9000000));
const uavcan_linux::SystemClock& clock = clock_impl;
uavcan_linux::SocketCanDriver driver(clock);
for (auto ifn : iface_names)
{
std::cout << "Adding iface " << ifn << std::endl;
@ -254,7 +276,6 @@ static void testDriver(const std::vector<std::string>& iface_names)
/*
* Receive loopback
*/
const uavcan_linux::SystemClock clock(uavcan_linux::ClockAdjustmentMode::PerDriverPrivate);
for (int i = 0; i < driver.getNumIfaces(); i++)
{
uavcan::CanFrame frame;