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()));
}
}
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);
}
