Esempio n. 1
0
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);
}