bool operator==(const Transfer& rhs) const
{
return
(ts_monotonic == rhs.ts_monotonic) &&
((!ts_utc.isZero() && !rhs.ts_utc.isZero()) ? (ts_utc == rhs.ts_utc) : true) &&
(transfer_type == rhs.transfer_type) &&
(transfer_id == rhs.transfer_id) &&
(src_node_id == rhs.src_node_id) &&
((dst_node_id.isValid() && rhs.dst_node_id.isValid()) ? (dst_node_id == rhs.dst_node_id) : true) &&
(data_type == rhs.data_type) &&
(payload == rhs.payload);
}
int main()
{
app::init();
lowsyslog("Starting the UAVCAN thread\n");
app::uavcan_node_thread.start(LOWPRIO);
while (true)
{
for (int i = 0; i < 200; i++)
{
board::setLed(app::can.driver.hadActivity());
::usleep(25000);
}
const uavcan::UtcTime utc = uavcan_stm32::clock::getUtc();
lowsyslog("UTC %lu sec Rate corr: %fPPM Jumps: %lu Locked: %i\n",
static_cast<unsigned long>(utc.toMSec() / 1000),
uavcan_stm32::clock::getUtcRateCorrectionPPM(),
uavcan_stm32::clock::getUtcJumpCount(),
int(uavcan_stm32::clock::isUtcLocked()));
}
}
void setUtc(uavcan::UtcTime time)
{
MutexLocker mlocker(mutex);
UAVCAN_ASSERT(initialized);
{
CriticalSectionLocker locker;
time_utc = time.toUSec();
}
utc_set = true;
utc_locked = false;
utc_jump_cnt++;
utc_prev_adj = 0;
utc_rel_rate_ppm = 0;
}
virtual uavcan::int16_t receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic,
uavcan::UtcTime& out_ts_utc, uavcan::CanIOFlags& out_flags)
{
assert(this);
if (loopback.empty())
{
EXPECT_TRUE(rx.size()); // Shall never be called when not readable
if (rx_failure)
{
return -1;
}
if (rx.empty())
{
return 0;
}
const FrameWithTime frame = rx.front();
rx.pop();
out_frame = frame.frame;
out_ts_monotonic = frame.time;
out_ts_utc = frame.time_utc;
out_flags = frame.flags;
}
else
{
out_flags |= uavcan::CanIOFlagLoopback;
const FrameWithTime frame = loopback.front();
loopback.pop();
out_frame = frame.frame;
out_ts_monotonic = frame.time;
out_ts_utc = frame.time_utc;
}
// Let's just all pretend that this code is autogenerated, instead of being carefully designed by a human.
if (out_ts_utc.isZero())
{
out_ts_utc = enable_utc_timestamping ? iclock.getUtc() : uavcan::UtcTime();
}
return 1;
}
int UavcanNode::run()
{
get_node().setRestartRequestHandler(&restart_request_handler);
while (init_indication_controller(get_node()) < 0) {
::syslog(LOG_INFO, "UAVCAN: Indication controller init failed\n");
::sleep(1);
}
while (init_sim_controller(get_node()) < 0) {
::syslog(LOG_INFO, "UAVCAN: sim controller init failed\n");
::sleep(1);
}
(void)pthread_mutex_lock(&_node_mutex);
/*
* Set up the time synchronization
*/
const int slave_init_res = _time_sync_slave.start();
if (slave_init_res < 0) {
warnx("Failed to start time_sync_slave");
_task_should_exit = true;
}
const unsigned PollTimeoutMs = 50;
const int busevent_fd = ::open(uavcan_stm32::BusEvent::DevName, 0);
if (busevent_fd < 0) {
warnx("Failed to open %s", uavcan_stm32::BusEvent::DevName);
_task_should_exit = true;
}
/* If we had an RTC we would call uavcan_stm32::clock::setUtc()
* but for now we use adjustUtc with a correction of 0
*/
// uavcan_stm32::clock::adjustUtc(uavcan::UtcDuration::fromUSec(0));
_node.setModeOperational();
/*
* This event is needed to wake up the thread on CAN bus activity (RX/TX/Error).
* Please note that with such multiplexing it is no longer possible to rely only on
* the value returned from poll() to detect whether actuator control has timed out or not.
* Instead, all ORB events need to be checked individually (see below).
*/
add_poll_fd(busevent_fd);
uint32_t start_tick = clock_systimer();
while (!_task_should_exit) {
// Mutex is unlocked while the thread is blocked on IO multiplexing
(void)pthread_mutex_unlock(&_node_mutex);
const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs);
(void)pthread_mutex_lock(&_node_mutex);
node_spin_once(); // Non-blocking
// this would be bad...
if (poll_ret < 0) {
PX4_ERR("poll error %d", errno);
continue;
} else {
// Do Something
}
if (clock_systimer() - start_tick > TICK_PER_SEC) {
start_tick = clock_systimer();
resources("Udate:");
/*
* Printing the slave status information once a second
*/
const bool active = _time_sync_slave.isActive(); // Whether it can sync with a remote master
const int master_node_id = _time_sync_slave.getMasterNodeID().get(); // Returns an invalid Node ID if (active == false)
const long msec_since_last_adjustment = (_node.getMonotonicTime() - _time_sync_slave.getLastAdjustmentTime()).toMSec();
const uavcan::UtcTime utc = uavcan_stm32::clock::getUtc();
syslog(LOG_INFO, "Time:%lld\n"
" Time sync slave status:\n"
" Active: %d\n"
" Master Node ID: %d\n"
" Last clock adjustment was %ld ms ago\n",
utc .toUSec(), int(active), master_node_id, msec_since_last_adjustment);
syslog(LOG_INFO, "UTC %lu sec Rate corr: %fPPM Jumps: %lu Locked: %i\n\n",
static_cast<unsigned long>(utc.toMSec() / 1000),
static_cast<double>(uavcan_stm32::clock::getUtcRateCorrectionPPM()),
uavcan_stm32::clock::getUtcJumpCount(),
int(uavcan_stm32::clock::isUtcLocked()));
}
}
teardown();
warnx("exiting.");
exit(0);
}
