forked from Archive/PX4-Autopilot
Added UAVCAN Time Synchronization Master capabilities to FMU
This commit is contained in:
parent
f424cc6b18
commit
d69be4b554
|
@ -70,7 +70,7 @@ override EXTRADEFINES := $(EXTRADEFINES) \
|
|||
-DUAVCAN_NO_ASSERTIONS \
|
||||
-DUAVCAN_MEM_POOL_BLOCK_SIZE=48 \
|
||||
-DUAVCAN_MAX_NETWORK_SIZE_HINT=16 \
|
||||
-DUAVCAN_STM32_TIMER_NUMBER=2
|
||||
-DUAVCAN_STM32_TIMER_NUMBER=5
|
||||
|
||||
#
|
||||
# libuavcan drivers for STM32
|
||||
|
|
|
@ -77,7 +77,10 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys
|
|||
CDev("uavcan", UAVCAN_DEVICE_PATH),
|
||||
_node(can_driver, system_clock),
|
||||
_node_mutex(),
|
||||
_esc_controller(_node)
|
||||
_esc_controller(_node),
|
||||
_time_sync_master(_node),
|
||||
_time_sync_slave(_node),
|
||||
_master_timer(_node)
|
||||
{
|
||||
_task_should_exit = false;
|
||||
_fw_server_action = None;
|
||||
|
@ -459,6 +462,48 @@ int UavcanNode::add_poll_fd(int fd)
|
|||
}
|
||||
|
||||
|
||||
void UavcanNode::handle_time_sync(const uavcan::TimerEvent &)
|
||||
{
|
||||
|
||||
/*
|
||||
* Check whether there are higher priority masters in the network.
|
||||
* If there are, we need to activate the local slave in order to sync with them.
|
||||
*/
|
||||
if (_time_sync_slave.isActive()) { // "Active" means that the slave tracks at least one remote master in the network
|
||||
if (_node.getNodeID() < _time_sync_slave.getMasterNodeID()) {
|
||||
/*
|
||||
* We're the highest priority master in the network.
|
||||
* We need to suppress the slave now to prevent it from picking up unwanted sync messages from
|
||||
* lower priority masters.
|
||||
*/
|
||||
_time_sync_slave.suppress(true); // SUPPRESS
|
||||
|
||||
} else {
|
||||
/*
|
||||
* There is at least one higher priority master in the network.
|
||||
* We need to allow the slave to adjust our local clock in order to be in sync.
|
||||
*/
|
||||
_time_sync_slave.suppress(false); // UNSUPPRESS
|
||||
}
|
||||
|
||||
} else {
|
||||
/*
|
||||
* There are no other time sync masters in the network, so we're the only time source.
|
||||
* The slave must be suppressed anyway to prevent it from disrupting the local clock if a new
|
||||
* lower priority master suddenly appears in the network.
|
||||
*/
|
||||
_time_sync_slave.suppress(true);
|
||||
}
|
||||
|
||||
/*
|
||||
* Publish the sync message now, even if we're not a higher priority master.
|
||||
* Other nodes will be able to pick the right master anyway.
|
||||
*/
|
||||
_time_sync_master.publish();
|
||||
}
|
||||
|
||||
|
||||
|
||||
int UavcanNode::run()
|
||||
{
|
||||
(void)pthread_mutex_lock(&_node_mutex);
|
||||
|
@ -472,6 +517,27 @@ int UavcanNode::run()
|
|||
|
||||
memset(&_outputs, 0, sizeof(_outputs));
|
||||
|
||||
/*
|
||||
* 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;
|
||||
}
|
||||
|
||||
/* When we have a system wide notion of time update (i.e the transition from the initial
|
||||
* System RTC setting to the GPS) we would call uavcan_stm32::clock::setUtc() when that
|
||||
* happens, but for now we use adjustUtc with a correction of 0
|
||||
*/
|
||||
uavcan_stm32::clock::adjustUtc(uavcan::UtcDuration::fromUSec(0));
|
||||
_master_timer.setCallback(TimerCallback(this, &UavcanNode::handle_time_sync));
|
||||
_master_timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000));
|
||||
|
||||
|
||||
|
||||
const int busevent_fd = ::open(uavcan_stm32::BusEvent::DevName, 0);
|
||||
|
||||
if (busevent_fd < 0) {
|
||||
|
|
|
@ -36,6 +36,9 @@
|
|||
#include <px4_config.h>
|
||||
|
||||
#include <uavcan_stm32/uavcan_stm32.hpp>
|
||||
#include <uavcan/protocol/global_time_sync_master.hpp>
|
||||
#include <uavcan/protocol/global_time_sync_slave.hpp>
|
||||
|
||||
#include <drivers/device/device.h>
|
||||
#include <systemlib/perf_counter.h>
|
||||
|
||||
|
@ -48,7 +51,7 @@
|
|||
#include "actuators/esc.hpp"
|
||||
#include "sensors/sensor_bridge.hpp"
|
||||
|
||||
# include "uavcan_servers.hpp"
|
||||
#include "uavcan_servers.hpp"
|
||||
|
||||
/**
|
||||
* @file uavcan_main.hpp
|
||||
|
@ -121,6 +124,7 @@ public:
|
|||
void attachITxQueueInjector(ITxQueueInjector *injector) {_tx_injector = injector;}
|
||||
|
||||
private:
|
||||
|
||||
void fill_node_info();
|
||||
int init(uavcan::NodeID node_id);
|
||||
void node_spin_once();
|
||||
|
@ -150,6 +154,8 @@ private:
|
|||
pthread_mutex_t _node_mutex;
|
||||
sem_t _server_command_sem;
|
||||
UavcanEscController _esc_controller;
|
||||
uavcan::GlobalTimeSyncMaster _time_sync_master;
|
||||
uavcan::GlobalTimeSyncSlave _time_sync_slave;
|
||||
|
||||
List<IUavcanSensorBridge *> _sensor_bridges; ///< List of active sensor bridges
|
||||
|
||||
|
@ -175,4 +181,10 @@ private:
|
|||
perf_counter_t _perfcnt_node_spin_elapsed = perf_alloc(PC_ELAPSED, "uavcan_node_spin_elapsed");
|
||||
perf_counter_t _perfcnt_esc_mixer_output_elapsed = perf_alloc(PC_ELAPSED, "uavcan_esc_mixer_output_elapsed");
|
||||
perf_counter_t _perfcnt_esc_mixer_total_elapsed = perf_alloc(PC_ELAPSED, "uavcan_esc_mixer_total_elapsed");
|
||||
|
||||
void handle_time_sync(const uavcan::TimerEvent &);
|
||||
|
||||
typedef uavcan::MethodBinder<UavcanNode *, void (UavcanNode::*)(const uavcan::TimerEvent &)> TimerCallback;
|
||||
uavcan::TimerEventForwarder<TimerCallback> _master_timer;
|
||||
|
||||
};
|
||||
|
|
Loading…
Reference in New Issue