dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge pull request #2545 from PX4/beta_uavacan 

Merged master_uavcan_modular src/modules/uavcan/
This commit is contained in:
Lorenz Meier 2015-08-14 14:16:40 +02:00
parent dd2f7987b6 e278419e84
commit e9ce0789c3
14 changed files with 1264 additions and 214 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
Tools/check_submodules.sh
View File

@ -25,8 +25,7 @@ if [ -d NuttX/nuttx ];
exit 1
fi
else
git submodule init;
git submodule update;
git submodule update --init --recursive
fi
@ -47,8 +46,7 @@ if [ -d mavlink/include/mavlink/v1.0 ];
exit 1
fi
else
git submodule init;
git submodule update;
git submodule update --init --recursive
fi
@ -70,8 +68,7 @@ then
exit 1
fi
else
git submodule init;
git submodule update;
git submodule update --init --recursive
fi
if [ -d src/lib/eigen ]
@ -92,8 +89,7 @@ then
exit 1
fi
else
git submodule init;
git submodule update;
git submodule update --init --recursive
fi
if [ -d Tools/gencpp ]
@ -114,8 +110,7 @@ then
exit 1
fi
else
git submodule init;
git submodule update;
git submodule update --init --recursive
fi
if [ -d Tools/genmsg ]
@ -136,8 +131,7 @@ then
exit 1
fi
else
git submodule init;
git submodule update;
git submodule update --init --recursive
fi
exit 0

2
src/lib/uavcan

@ -1 +1 @@
Subproject commit 1f1679c75d989420350e69339d48b53203c5e874
Subproject commit 46793c5e0699d494cb9ec10ca70b6d833acbec46

34
src/modules/uavcan/actuators/esc.cpp
View File

@ -68,8 +68,8 @@ int UavcanEscController::init()
{
// ESC status subscription
int res = _uavcan_sub_status.start(StatusCbBinder(this, &UavcanEscController::esc_status_sub_cb));
if (res < 0)
{
if (res < 0) {
warnx("ESC status sub failed %i", res);
return res;
}
@ -83,9 +83,9 @@ int UavcanEscController::init()
void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
{
if ((outputs == nullptr) ||
(num_outputs > uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize) ||
(num_outputs > esc_status_s::CONNECTED_ESC_MAX)) {
if ((outputs == nullptr) ||
(num_outputs > uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize) ||
(num_outputs > esc_status_s::CONNECTED_ESC_MAX)) {
perf_count(_perfcnt_invalid_input);
return;
}
@ -94,9 +94,11 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
* Rate limiting - we don't want to congest the bus
*/
const auto timestamp = _node.getMonotonicTime();
if ((timestamp - _prev_cmd_pub).toUSec() < (1000000 / MAX_RATE_HZ)) {
return;
}
_prev_cmd_pub = timestamp;
/*
@ -110,15 +112,17 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
for (unsigned i = 0; i < num_outputs; i++) {
if (_armed_mask & MOTOR_BIT(i)) {
float scaled = (outputs[i] + 1.0F) * 0.5F * cmd_max;
// trim negative values back to 0. Previously
// we set this to 0.1, which meant motors kept
// spinning when armed, but that should be a
// policy decision for a specific vehicle
// type, as it is not appropriate for all
// types of vehicles (eg. fixed wing).
// trim negative values back to 0. Previously
// we set this to 0.1, which meant motors kept
// spinning when armed, but that should be a
// policy decision for a specific vehicle
// type, as it is not appropriate for all
// types of vehicles (eg. fixed wing).
if (scaled < 0.0F) {
scaled = 0.0F;
}
}
if (scaled > cmd_max) {
scaled = cmd_max;
perf_count(_perfcnt_scaling_error);
@ -127,6 +131,7 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
msg.cmd.push_back(static_cast<int>(scaled));
_esc_status.esc[i].esc_setpoint_raw = abs(static_cast<int>(scaled));
} else {
msg.cmd.push_back(static_cast<unsigned>(0));
}
@ -143,6 +148,7 @@ void UavcanEscController::arm_all_escs(bool arm)
{
if (arm) {
_armed_mask = -1;
} else {
_armed_mask = 0;
}
@ -152,6 +158,7 @@ void UavcanEscController::arm_single_esc(int num, bool arm)
{
if (arm) {
_armed_mask = MOTOR_BIT(num);
} else {
_armed_mask = 0;
}
@ -176,13 +183,14 @@ void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<
}
}
void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent&)
void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent &)
{
_esc_status.counter += 1;
_esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_CAN;
if (_esc_status_pub > 0) {
(void)orb_publish(ORB_ID(esc_status), _esc_status_pub, &_esc_status);
} else {
_esc_status_pub = orb_advertise(ORB_ID(esc_status), &_esc_status);
}

8
src/modules/uavcan/actuators/esc.hpp
View File

@ -54,7 +54,7 @@
class UavcanEscController
{
public:
UavcanEscController(uavcan::INode& node);
UavcanEscController(uavcan::INode &node);
~UavcanEscController();
int init();
@ -79,12 +79,12 @@ private:
static constexpr unsigned MAX_RATE_HZ = 200; ///< XXX make this configurable
static constexpr unsigned ESC_STATUS_UPDATE_RATE_HZ = 10;
typedef uavcan::MethodBinder<UavcanEscController*,
typedef uavcan::MethodBinder<UavcanEscController *,
void (UavcanEscController::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status>&)>
StatusCbBinder;
typedef uavcan::MethodBinder<UavcanEscController*, void (UavcanEscController::*)(const uavcan::TimerEvent&)>
TimerCbBinder;
typedef uavcan::MethodBinder<UavcanEscController *, void (UavcanEscController::*)(const uavcan::TimerEvent &)>
TimerCbBinder;
bool _armed = false;
esc_status_s _esc_status = {};

7
src/modules/uavcan/module.mk
View File

@ -44,6 +44,7 @@ WFRAME_LARGER_THAN = 1400
# Main
SRCS += uavcan_main.cpp \
uavcan_servers.cpp \
uavcan_clock.cpp \
uavcan_params.c
@ -65,7 +66,11 @@ SRCS += $(subst $(PX4_MODULE_SRC),../../,$(LIBUAVCAN_SRC))
INCLUDE_DIRS += $(LIBUAVCAN_INC)
# Since actual compiler mode is C++11, the library will default to UAVCAN_CPP11, but it will fail to compile
# because this platform lacks most of the standard library and STL. Hence we need to force C++03 mode.
override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 -DUAVCAN_NO_ASSERTIONS
override EXTRADEFINES := $(EXTRADEFINES) \
-DUAVCAN_CPP_VERSION=UAVCAN_CPP03 \
-DUAVCAN_NO_ASSERTIONS \
-DUAVCAN_MEM_POOL_BLOCK_SIZE=48 \
-DUAVCAN_MAX_NETWORK_SIZE_HINT=16
#
# libuavcan drivers for STM32

28
src/modules/uavcan/sensors/gnss.cpp
View File

@ -46,20 +46,21 @@
const char *const UavcanGnssBridge::NAME = "gnss";
UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) :
_node(node),
_sub_fix(node),
_report_pub(-1)
_node(node),
_sub_fix(node),
_report_pub(-1)
{
}
int UavcanGnssBridge::init()
{
int res = _sub_fix.start(FixCbBinder(this, &UavcanGnssBridge::gnss_fix_sub_cb));
if (res < 0)
{
if (res < 0) {
warnx("GNSS fix sub failed %i", res);
return res;
}
return res;
}
@ -71,8 +72,10 @@ unsigned UavcanGnssBridge::get_num_redundant_channels() const
void UavcanGnssBridge::print_status() const
{
printf("RX errors: %d, receiver node id: ", _sub_fix.getFailureCount());
if (_receiver_node_id < 0) {
printf("N/A\n");
} else {
printf("%d\n", _receiver_node_id);
}
@ -84,6 +87,7 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavca
if (_receiver_node_id < 0) {
_receiver_node_id = msg.getSrcNodeID().get();
warnx("GNSS receiver node ID: %d", _receiver_node_id);
} else {
if (_receiver_node_id != msg.getSrcNodeID().get()) {
return; // This GNSS receiver is the redundant one, ignore it.
@ -114,14 +118,15 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavca
// Vertical position uncertainty
report.epv = (pos_cov[8] > 0) ? sqrtf(pos_cov[8]) : -1.0F;
} else {
report.eph = -1.0F;
report.epv = -1.0F;
}
if (valid_velocity_covariance) {
float vel_cov[9];
msg.velocity_covariance.unpackSquareMatrix(vel_cov);
float vel_cov[9];
msg.velocity_covariance.unpackSquareMatrix(vel_cov);
report.s_variance_m_s = math::max(math::max(vel_cov[0], vel_cov[4]), vel_cov[8]);
/* There is a nonlinear relationship between the velocity vector and the heading.
@ -139,9 +144,9 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavca
float vel_n_sq = vel_n * vel_n;
float vel_e_sq = vel_e * vel_e;
report.c_variance_rad =
(vel_e_sq * vel_cov[0] +
-2 * vel_n * vel_e * vel_cov[1] + // Covariance matrix is symmetric
vel_n_sq* vel_cov[4]) / ((vel_n_sq + vel_e_sq) * (vel_n_sq + vel_e_sq));
(vel_e_sq * vel_cov[0] +
-2 * vel_n * vel_e * vel_cov[1] + // Covariance matrix is symmetric
vel_n_sq * vel_cov[4]) / ((vel_n_sq + vel_e_sq) * (vel_n_sq + vel_e_sq));
} else {
report.s_variance_m_s = -1.0F;
@ -154,7 +159,8 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavca
report.vel_n_m_s = msg.ned_velocity[0];
report.vel_e_m_s = msg.ned_velocity[1];
report.vel_d_m_s = msg.ned_velocity[2];
report.vel_m_s = sqrtf(report.vel_n_m_s * report.vel_n_m_s + report.vel_e_m_s * report.vel_e_m_s + report.vel_d_m_s * report.vel_d_m_s);
report.vel_m_s = sqrtf(report.vel_n_m_s * report.vel_n_m_s + report.vel_e_m_s * report.vel_e_m_s + report.vel_d_m_s *
report.vel_d_m_s);
report.cog_rad = atan2f(report.vel_e_m_s, report.vel_n_m_s);
report.vel_ned_valid = true;

57
src/modules/uavcan/sensors/mag.cpp
View File

@ -41,9 +41,9 @@
const char *const UavcanMagnetometerBridge::NAME = "mag";
UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode& node) :
UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_BASE_DEVICE_PATH, ORB_ID(sensor_mag)),
_sub_mag(node)
UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode &node) :
UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_BASE_DEVICE_PATH, ORB_ID(sensor_mag)),
_sub_mag(node)
{
_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883;
@ -55,15 +55,18 @@ _sub_mag(node)
int UavcanMagnetometerBridge::init()
{
int res = device::CDev::init();
if (res < 0) {
return res;
}
res = _sub_mag.start(MagCbBinder(this, &UavcanMagnetometerBridge::mag_sub_cb));
if (res < 0) {
log("failed to start uavcan sub: %d", res);
return res;
}
return 0;
}
@ -74,6 +77,7 @@ ssize_t UavcanMagnetometerBridge::read(struct file *filp, char *buffer, size_t b
/* buffer must be large enough */
unsigned count = buflen / sizeof(struct mag_report);
if (count < 1) {
return -ENOSPC;
}
@ -85,6 +89,7 @@ ssize_t UavcanMagnetometerBridge::read(struct file *filp, char *buffer, size_t b
last_read = _report.timestamp;
unlock();
return sizeof(struct mag_report);
} else {
/* no new data available, warn caller */
return -EAGAIN;
@ -95,25 +100,31 @@ int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long ar
{
switch (cmd) {
case SENSORIOCSQUEUEDEPTH: {
return OK; // Pretend that this stuff is supported to keep APM happy
}
return OK; // Pretend that this stuff is supported to keep APM happy
}
case MAGIOCSSCALE: {
std::memcpy(&_scale, reinterpret_cast<const void*>(arg), sizeof(_scale));
return 0;
}
std::memcpy(&_scale, reinterpret_cast<const void *>(arg), sizeof(_scale));
return 0;
}
case MAGIOCGSCALE: {
std::memcpy(reinterpret_cast<void*>(arg), &_scale, sizeof(_scale));
return 0;
}
std::memcpy(reinterpret_cast<void *>(arg), &_scale, sizeof(_scale));
return 0;
}
case MAGIOCSELFTEST: {
return 0; // Nothing to do
}
return 0; // Nothing to do
}
case MAGIOCGEXTERNAL: {
return 1; // declare it external rise it's priority and to allow for correct orientation compensation
}
return 1; // declare it external rise it's priority and to allow for correct orientation compensation
}
case MAGIOCSSAMPLERATE: {
return 0; // Pretend that this stuff is supported to keep the sensor app happy
}
return 0; // Pretend that this stuff is supported to keep the sensor app happy
}
case MAGIOCCALIBRATE:
case MAGIOCGSAMPLERATE:
case MAGIOCSRANGE:
@ -121,15 +132,17 @@ int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long ar
case MAGIOCSLOWPASS:
case MAGIOCEXSTRAP:
case MAGIOCGLOWPASS: {
return -EINVAL;
}
return -EINVAL;
}
default: {
return CDev::ioctl(filp, cmd, arg);
}
return CDev::ioctl(filp, cmd, arg);
}
}
}
void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::Magnetometer> &msg)
void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::Magnetometer>
&msg)
{
lock();
_report.range_ga = 1.3F; // Arbitrary number, doesn't really mean anything

9
src/modules/uavcan/sensors/sensor_bridge.cpp
View File

@ -45,7 +45,7 @@
/*
* IUavcanSensorBridge
*/
void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge*> &list)
void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge *> &list)
{
list.add(new UavcanBarometerBridge(node));
list.add(new UavcanMagnetometerBridge(node));
@ -62,6 +62,7 @@ UavcanCDevSensorBridgeBase::~UavcanCDevSensorBridgeBase()
(void)unregister_class_devname(_class_devname, _channels[i].class_instance);
}
}
delete [] _channels;
}
@ -107,6 +108,7 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report)
// Ask the CDev helper which class instance we can take
const int class_instance = register_class_devname(_class_devname);
if (class_instance < 0 || class_instance >= int(_max_channels)) {
_out_of_channels = true;
log("out of class instances");
@ -119,6 +121,7 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report)
channel->class_instance = class_instance;
channel->orb_advert = orb_advertise_multi(_orb_topic, report, &channel->orb_instance, ORB_PRIO_HIGH);
if (channel->orb_advert < 0) {
log("ADVERTISE FAILED");
(void)unregister_class_devname(_class_devname, class_instance);
@ -128,6 +131,7 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report)
log("channel %d class instance %d ok", channel->node_id, channel->class_instance);
}
assert(channel != nullptr);
(void)orb_publish(_orb_topic, channel->orb_advert, report);
@ -136,11 +140,13 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report)
unsigned UavcanCDevSensorBridgeBase::get_num_redundant_channels() const
{
unsigned out = 0;
for (unsigned i = 0; i < _max_channels; i++) {
if (_channels[i].node_id >= 0) {
out += 1;
}
}
return out;
}
@ -152,6 +158,7 @@ void UavcanCDevSensorBridgeBase::print_status() const
if (_channels[i].node_id >= 0) {
printf("channel %d: node id %d --> class instance %d\n",
i, _channels[i].node_id, _channels[i].class_instance);
} else {
printf("channel %d: empty\n", i);
}

21
src/modules/uavcan/sensors/sensor_bridge.hpp
View File

@ -45,7 +45,7 @@
/**
* A sensor bridge class must implement this interface.
*/
class IUavcanSensorBridge : uavcan::Noncopyable, public ListNode<IUavcanSensorBridge*>
class IUavcanSensorBridge : uavcan::Noncopyable, public ListNode<IUavcanSensorBridge *>
{
public:
static constexpr unsigned MAX_NAME_LEN = 20;
@ -77,7 +77,7 @@ public:
* Sensor bridge factory.
* Creates all known sensor bridges and puts them in the linked list.
*/
static void make_all(uavcan::INode &node, List<IUavcanSensorBridge*> &list);
static void make_all(uavcan::INode &node, List<IUavcanSensorBridge *> &list);
};
/**
@ -86,8 +86,7 @@ public:
*/
class UavcanCDevSensorBridgeBase : public IUavcanSensorBridge, public device::CDev
{
struct Channel
{
struct Channel {
int node_id = -1;
orb_advert_t orb_advert = -1;
int class_instance = -1;
@ -104,13 +103,13 @@ protected:
static constexpr unsigned DEFAULT_MAX_CHANNELS = 5; // 640 KB ought to be enough for anybody
UavcanCDevSensorBridgeBase(const char *name, const char *devname, const char *class_devname,
const orb_id_t orb_topic_sensor,
const unsigned max_channels = DEFAULT_MAX_CHANNELS) :
device::CDev(name, devname),
_max_channels(max_channels),
_class_devname(class_devname),
_orb_topic(orb_topic_sensor),
_channels(new Channel[max_channels])
const orb_id_t orb_topic_sensor,
const unsigned max_channels = DEFAULT_MAX_CHANNELS) :
device::CDev(name, devname),
_max_channels(max_channels),
_class_devname(class_devname),
_orb_topic(orb_topic_sensor),
_channels(new Channel[max_channels])
{
_device_id.devid_s.bus_type = DeviceBusType_UAVCAN;
_device_id.devid_s.bus = 0;

324
src/modules/uavcan/uavcan_main.cpp
View File

@ -53,15 +53,12 @@
#include <drivers/drv_pwm_output.h>
#include "uavcan_main.hpp"
#include <uavcan/util/templates.hpp>
#if defined(USE_FW_NODE_SERVER)
# include <uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp>
# include <uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp>
# include <uavcan_posix/firmware_version_checker.hpp>
//todo:The Inclusion of file_server_backend is killing
// #include <sys/types.h> and leaving OK undefined
# define OK 0
#endif
/**
* @file uavcan_main.cpp
@ -76,33 +73,28 @@
* UavcanNode
*/
UavcanNode *UavcanNode::_instance;
#if defined(USE_FW_NODE_SERVER)
uavcan::dynamic_node_id_server::CentralizedServer *UavcanNode::_server_instance;
uavcan_posix::dynamic_node_id_server::FileEventTracer tracer;
uavcan_posix::dynamic_node_id_server::FileStorageBackend storage_backend;
uavcan_posix::FirmwareVersionChecker fw_version_checker;
#endif
UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) :
CDev("uavcan", UAVCAN_DEVICE_PATH),
_node(can_driver, system_clock),
_node_mutex(),
#if !defined(USE_FW_NODE_SERVER)
_esc_controller(_node)
#else
_esc_controller(_node),
_fileserver_backend(_node),
_node_info_retriever(_node),
_fw_upgrade_trigger(_node, fw_version_checker),
_fw_server(_node, _fileserver_backend)
#endif
{
_task_should_exit = false;
_fw_server_action = None;
_fw_server_status = -1;
_tx_injector = nullptr;
_control_topics[0] = ORB_ID(actuator_controls_0);
_control_topics[1] = ORB_ID(actuator_controls_1);
_control_topics[2] = ORB_ID(actuator_controls_2);
_control_topics[3] = ORB_ID(actuator_controls_3);
const int res = pthread_mutex_init(&_node_mutex, nullptr);
int res = pthread_mutex_init(&_node_mutex, nullptr);
if (res < 0) {
std::abort();
}
res = sem_init(&_server_command_sem, 0 , 0);
if (res < 0) {
std::abort();
@ -123,7 +115,11 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys
UavcanNode::~UavcanNode()
{
fw_server(Stop);
if (_task != -1) {
/* tell the task we want it to go away */
_task_should_exit = true;
@ -161,13 +157,127 @@ UavcanNode::~UavcanNode()
perf_free(_perfcnt_node_spin_elapsed);
perf_free(_perfcnt_esc_mixer_output_elapsed);
perf_free(_perfcnt_esc_mixer_total_elapsed);
#if defined(USE_FW_NODE_SERVER)
delete(_server_instance);
#endif
pthread_mutex_destroy(&_node_mutex);
sem_destroy(&_server_command_sem);
}
int UavcanNode::getHardwareVersion(uavcan::protocol::HardwareVersion &hwver)
{
int rv = -1;
if (UavcanNode::instance()) {
if (!std::strncmp(HW_ARCH, "PX4FMU_V1", 9)) {
hwver.major = 1;
} else if (!std::strncmp(HW_ARCH, "PX4FMU_V2", 9)) {
hwver.major = 2;
} else {
; // All other values of HW_ARCH resolve to zero
}
uint8_t udid[12] = {}; // Someone seems to love magic numbers
get_board_serial(udid);
uavcan::copy(udid, udid + sizeof(udid), hwver.unique_id.begin());
rv = 0;
}
return rv;
}
int UavcanNode::start_fw_server()
{
int rv = -1;
_fw_server_action = Busy;
UavcanServers *_servers = UavcanServers::instance();
if (_servers == nullptr) {
rv = UavcanServers::start(2, _node);
if (rv >= 0) {
/*
* Set our pointer to to the injector
* This is a work around as
* main_node.getDispatcher().installRxFrameListener(driver.get());
* would require a dynamic cast and rtti is not enabled.
*/
UavcanServers::instance()->attachITxQueueInjector(&_tx_injector);
}
}
_fw_server_action = None;
sem_post(&_server_command_sem);
return rv;
}
int UavcanNode::request_fw_check()
{
int rv = -1;
_fw_server_action = Busy;
UavcanServers *_servers = UavcanServers::instance();
if (_servers != nullptr) {
_servers->requestCheckAllNodesFirmwareAndUpdate();
rv = 0;
}
_fw_server_action = None;
sem_post(&_server_command_sem);
return rv;
}
int UavcanNode::stop_fw_server()
{
int rv = -1;
_fw_server_action = Busy;
UavcanServers *_servers = UavcanServers::instance();
if (_servers != nullptr) {
/*
* Set our pointer to to the injector
* This is a work around as
* main_node.getDispatcher().remeveRxFrameListener();
* would require a dynamic cast and rtti is not enabled.
*/
_tx_injector = nullptr;
rv = _servers->stop();
}
_fw_server_action = None;
sem_post(&_server_command_sem);
return rv;
}
int UavcanNode::fw_server(eServerAction action)
{
int rv = -EAGAIN;
switch (action) {
case Start:
case Stop:
case CheckFW:
if (_fw_server_action == None) {
_fw_server_action = action;
sem_wait(&_server_command_sem);
rv = _fw_server_status;
}
break;
default:
rv = -EINVAL;
break;
}
return rv;
}
int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
{
if (_instance != nullptr) {
@ -213,6 +323,11 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
return -1;
}
if (_instance == nullptr) {
warnx("Out of memory");
return -1;
}
const int node_init_res = _instance->init(node_id);
if (node_init_res < 0) {
@ -248,7 +363,7 @@ void UavcanNode::fill_node_info()
assert(fw_git_short[8] == '\0');
char *end = nullptr;
swver.vcs_commit = std::strtol(fw_git_short, &end, 16);
swver.optional_field_mask |= swver.OPTIONAL_FIELD_MASK_VCS_COMMIT;
swver.optional_field_flags |= swver.OPTIONAL_FIELD_FLAG_VCS_COMMIT;
warnx("SW version vcs_commit: 0x%08x", unsigned(swver.vcs_commit));
@ -256,21 +371,7 @@ void UavcanNode::fill_node_info()
/* hardware version */
uavcan::protocol::HardwareVersion hwver;
if (!std::strncmp(HW_ARCH, "PX4FMU_V1", 9)) {
hwver.major = 1;
} else if (!std::strncmp(HW_ARCH, "PX4FMU_V2", 9)) {
hwver.major = 2;
} else {
; // All other values of HW_ARCH resolve to zero
}
uint8_t udid[12] = {}; // Someone seems to love magic numbers
get_board_serial(udid);
uavcan::copy(udid, udid + sizeof(udid), hwver.unique_id.begin());
getHardwareVersion(hwver);
_node.setHardwareVersion(hwver);
}
@ -315,75 +416,7 @@ int UavcanNode::init(uavcan::NodeID node_id)
br = br->getSibling();
}
#if defined(USE_FW_NODE_SERVER)
/* Initialize the fw version checker.
* giving it it's path
*/
ret = fw_version_checker.createFwPaths(UAVCAN_FIRMWARE_PATH);
if (ret < 0) {
return ret;
}
/* Start fw file server back */
ret = _fw_server.start();
if (ret < 0) {
return ret;
}
/* Initialize storage back end for the node allocator using UAVCAN_NODE_DB_PATH directory */
ret = storage_backend.init(UAVCAN_NODE_DB_PATH);
if (ret < 0) {
return ret;
}
/* Initialize trace in the UAVCAN_NODE_DB_PATH directory */
ret = tracer.init(UAVCAN_LOG_FILE);
if (ret < 0) {
return ret;
}
/* Create dynamic node id server for the Firmware updates directory */
_server_instance = new uavcan::dynamic_node_id_server::CentralizedServer(_node, storage_backend, tracer);
if (_server_instance == 0) {
return -ENOMEM;
}
/* Initialize the dynamic node id server */
ret = _server_instance->init(_node.getNodeStatusProvider().getHardwareVersion().unique_id);
if (ret < 0) {
return ret;
}
/* Start node info retriever to fetch node info from new nodes */
ret = _node_info_retriever.start();
if (ret < 0) {
return ret;
}
/* Start the fw version checker */
ret = _fw_upgrade_trigger.start(_node_info_retriever, fw_version_checker.getFirmwarePath());
if (ret < 0) {
return ret;
}
#endif
/* Start the Node */
return _node.start();
@ -398,6 +431,11 @@ void UavcanNode::node_spin_once()
warnx("node spin error %i", spin_res);
}
if (_tx_injector != nullptr) {
_tx_injector->injectTxFramesInto(_node);
}
perf_end(_perfcnt_node_spin_elapsed);
}
@ -446,7 +484,7 @@ int UavcanNode::run()
* IO multiplexing shall be done here.
*/
_node.setStatusOk();
_node.setModeOperational();
/*
* This event is needed to wake up the thread on CAN bus activity (RX/TX/Error).
@ -466,6 +504,24 @@ int UavcanNode::run()
while (!_task_should_exit) {
switch (_fw_server_action) {
case Start:
_fw_server_status = start_fw_server();
break;
case Stop:
_fw_server_status = stop_fw_server();
break;
case CheckFW:
_fw_server_status = request_fw_check();
break;
case None:
default:
break;
}
// update actuator controls subscriptions if needed
if (_groups_subscribed != _groups_required) {
subscribe();
@ -622,6 +678,8 @@ UavcanNode::control_callback(uintptr_t handle, uint8_t control_group, uint8_t co
int
UavcanNode::teardown()
{
sem_post(&_server_command_sem);
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_control_subs[i] > 0) {
::close(_control_subs[i]);
@ -812,7 +870,7 @@ UavcanNode::print_info()
static void print_usage()
{
warnx("usage: \n"
"\tuavcan {start|status|stop|arm|disarm}");
"\tuavcan {start [fw]|status|stop [all|fw]|arm|disarm|update fw}");
}
extern "C" __EXPORT int uavcan_main(int argc, char *argv[]);
@ -824,8 +882,21 @@ int uavcan_main(int argc, char *argv[])
::exit(1);
}
bool fw = argc > 2 && !std::strcmp(argv[2], "fw");
if (!std::strcmp(argv[1], "start")) {
if (UavcanNode::instance()) {
if (fw && UavcanServers::instance() == nullptr) {
int rv = UavcanNode::instance()->fw_server(UavcanNode::Start);
if (rv < 0) {
warnx("Firmware Server Failed to Start %d", rv);
::exit(rv);
}
::exit(0);
}
// Already running, no error
warnx("already started");
::exit(0);
@ -856,6 +927,20 @@ int uavcan_main(int argc, char *argv[])
errx(1, "application not running");
}
if (fw && !std::strcmp(argv[1], "update")) {
if (UavcanServers::instance() == nullptr) {
errx(1, "firmware server is not running");
}
int rv = UavcanNode::instance()->fw_server(UavcanNode::CheckFW);
::exit(rv);
}
if (fw && (!std::strcmp(argv[1], "status") || !std::strcmp(argv[1], "info"))) {
printf("Firmware Server is %s\n", UavcanServers::instance() ? "Running" : "Stopped");
::exit(0);
}
if (!std::strcmp(argv[1], "status") || !std::strcmp(argv[1], "info")) {
inst->print_info();
::exit(0);
@ -872,8 +957,21 @@ int uavcan_main(int argc, char *argv[])
}
if (!std::strcmp(argv[1], "stop")) {
delete inst;
::exit(0);
if (fw) {
int rv = inst->fw_server(UavcanNode::Stop);
if (rv < 0) {
warnx("Firmware Server Failed to Stop %d", rv);
::exit(rv);
}
::exit(0);
} else {
delete inst;
::exit(0);
}
}
print_usage();

39
src/modules/uavcan/uavcan_main.hpp
View File

@ -48,13 +48,7 @@
#include "actuators/esc.hpp"
#include "sensors/sensor_bridge.hpp"
#if defined(USE_FW_NODE_SERVER)
# include <uavcan/protocol/dynamic_node_id_server/centralized.hpp>
# include <uavcan/protocol/node_info_retriever.hpp>
# include <uavcan_posix/basic_file_server_backend.hpp>
# include <uavcan/protocol/firmware_update_trigger.hpp>
# include <uavcan/protocol/file_server.hpp>
#endif
# include "uavcan_servers.hpp"
/**
* @file uavcan_main.hpp
@ -66,9 +60,6 @@
#define NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN 4
#define UAVCAN_DEVICE_PATH "/dev/uavcan/esc"
#define UAVCAN_NODE_DB_PATH "/fs/microsd/uavcan.db"
#define UAVCAN_FIRMWARE_PATH "/fs/microsd/fw"
#define UAVCAN_LOG_FILE UAVCAN_NODE_DB_PATH"/trace.log"
// we add two to allow for actuator_direct and busevent
#define UAVCAN_NUM_POLL_FDS (NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN+2)
@ -84,8 +75,7 @@ class UavcanNode : public device::CDev
static constexpr unsigned PollTimeoutMs = 10;
static constexpr unsigned MemPoolSize = 10752; ///< Refer to the libuavcan manual to learn why
static constexpr unsigned MemPoolSize = 64 * uavcan::MemPoolBlockSize;
/*
* This memory is reserved for uavcan to use for queuing CAN frames.
* At 1Mbit there is approximately one CAN frame every 145 uS.
@ -99,11 +89,12 @@ class UavcanNode : public device::CDev
*/
static constexpr unsigned RxQueueLenPerIface = FramePerMSecond * PollTimeoutMs; // At
static constexpr unsigned StackSize = 3400;
static constexpr unsigned StackSize = 1600;
public:
typedef uavcan::Node<MemPoolSize> Node;
typedef uavcan_stm32::CanInitHelper<RxQueueLenPerIface> CanInitHelper;
enum eServerAction {None, Start, Stop, CheckFW , Busy};
UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock);
@ -126,6 +117,9 @@ public:
void print_info();
static UavcanNode *instance() { return _instance; }
static int getHardwareVersion(uavcan::protocol::HardwareVersion &hwver);
int fw_server(eServerAction action);
void attachITxQueueInjector(ITxQueueInjector *injector) {_tx_injector = injector;}
private:
void fill_node_info();
@ -133,10 +127,14 @@ private:
void node_spin_once();
int run();
int add_poll_fd(int fd); ///< add a fd to poll list, returning index into _poll_fds[]
int start_fw_server();
int stop_fw_server();
int request_fw_check();
int _task = -1; ///< handle to the OS task
bool _task_should_exit = false; ///< flag to indicate to tear down the CAN driver
volatile eServerAction _fw_server_action;
int _fw_server_status;
int _armed_sub = -1; ///< uORB subscription of the arming status
actuator_armed_s _armed = {}; ///< the arming request of the system
bool _is_armed = false; ///< the arming status of the actuators on the bus
@ -151,22 +149,13 @@ private:
Node _node; ///< library instance
pthread_mutex_t _node_mutex;
sem_t _server_command_sem;
UavcanEscController _esc_controller;
#if defined(USE_FW_NODE_SERVER)
static uavcan::dynamic_node_id_server::CentralizedServer *_server_instance; ///< server singleton pointer
uavcan_posix::BasicFileSeverBackend _fileserver_backend;
uavcan::NodeInfoRetriever _node_info_retriever;
uavcan::FirmwareUpdateTrigger _fw_upgrade_trigger;
uavcan::BasicFileServer _fw_server;
#endif
List<IUavcanSensorBridge *> _sensor_bridges; ///< List of active sensor bridges
MixerGroup *_mixers = nullptr;
ITxQueueInjector *_tx_injector;
uint32_t _groups_required = 0;
uint32_t _groups_subscribed = 0;
int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {};

280
src/modules/uavcan/uavcan_servers.cpp Normal file
View File

@ -0,0 +1,280 @@
/****************************************************************************
*
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include <nuttx/config.h>
#include <cstdlib>
#include <cstring>
#include <fcntl.h>
#include <memory>
#include <pthread.h>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include <systemlib/mixer/mixer.h>
#include <systemlib/board_serial.h>
#include <systemlib/scheduling_priorities.h>
#include <systemlib/git_version.h>
#include <version/version.h>
#include <arch/board/board.h>
#include <arch/chip/chip.h>
#include "uavcan_main.hpp"
#include "uavcan_servers.hpp"
#include "uavcan_virtual_can_driver.hpp"
# include <uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp>
# include <uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp>
# include <uavcan_posix/firmware_version_checker.hpp>
//todo:The Inclusion of file_server_backend is killing
// #include <sys/types.h> and leaving OK undefined
# define OK 0
/**
* @file uavcan_servers.cpp
*
* Implements basic functionality of UAVCAN node.
*
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
* David Sidrane <david_s5@nscdg.com>
*/
/*
* UavcanNode
*/
UavcanServers *UavcanServers::_instance;
UavcanServers::UavcanServers(uavcan::INode &main_node) :
_subnode_thread(-1),
_vdriver(UAVCAN_STM32_NUM_IFACES, uavcan_stm32::SystemClock::instance()),
_subnode(_vdriver, uavcan_stm32::SystemClock::instance()),
_main_node(main_node),
_tracer(),
_storage_backend(),
_fw_version_checker(),
_server_instance(_subnode, _storage_backend, _tracer),
_fileserver_backend(_subnode),
_node_info_retriever(_subnode),
_fw_upgrade_trigger(_subnode, _fw_version_checker),
_fw_server(_subnode, _fileserver_backend),
_mutex_inited(false),
_check_fw(false)
{
}
UavcanServers::~UavcanServers()
{
if (_mutex_inited) {
(void)Lock::deinit(_subnode_mutex);
}
}
int UavcanServers::stop(void)
{
UavcanServers *server = instance();
if (server == nullptr) {
warnx("Already stopped");
return -1;
}
_instance = nullptr;
if (server->_subnode_thread != -1) {
pthread_cancel(server->_subnode_thread);
pthread_join(server->_subnode_thread, NULL);
}
server->_main_node.getDispatcher().removeRxFrameListener();
delete server;
return 0;
}
int UavcanServers::start(unsigned num_ifaces, uavcan::INode &main_node)
{
if (_instance != nullptr) {
warnx("Already started");
return -1;
}
/*
* Node init
*/
_instance = new UavcanServers(main_node);
if (_instance == nullptr) {
warnx("Out of memory");
return -2;
}
int rv = _instance->init(num_ifaces);
if (rv < 0) {
warnx("Node init failed %i", rv);
delete _instance;
return rv;
}
/*
* Start the thread. Normally it should never exit.
*/
pthread_attr_t tattr;
struct sched_param param;
pthread_attr_init(&tattr);
tattr.stacksize = StackSize;
param.sched_priority = Priority;
pthread_attr_setschedparam(&tattr, &param);
static auto run_trampoline = [](void *) {return UavcanServers::_instance->run(_instance);};
rv = pthread_create(&_instance->_subnode_thread, &tattr, static_cast<pthread_startroutine_t>(run_trampoline), NULL);
if (rv < 0) {
warnx("start failed: %d", errno);
rv = -errno;
delete _instance;
}
return rv;
}
int UavcanServers::init(unsigned num_ifaces)
{
/*
* Initialize the mutex.
* giving it its path
*/
int ret = Lock::init(_subnode_mutex);
if (ret < 0) {
return ret;
}
_mutex_inited = true;
_subnode.setNodeID(_main_node.getNodeID());
_main_node.getDispatcher().installRxFrameListener(&_vdriver);
/*
* Initialize the fw version checker.
* giving it its path
*/
ret = _fw_version_checker.createFwPaths(UAVCAN_FIRMWARE_PATH);
if (ret < 0) {
return ret;
}
/* Start fw file server back */
ret = _fw_server.start();
if (ret < 0) {
return ret;
}
/* Initialize storage back end for the node allocator using UAVCAN_NODE_DB_PATH directory */
ret = _storage_backend.init(UAVCAN_NODE_DB_PATH);
if (ret < 0) {
return ret;
}
/* Initialize trace in the UAVCAN_NODE_DB_PATH directory */
ret = _tracer.init(UAVCAN_LOG_FILE);
if (ret < 0) {
return ret;
}
/* hardware version */
uavcan::protocol::HardwareVersion hwver;
UavcanNode::getHardwareVersion(hwver);
/* Initialize the dynamic node id server */
ret = _server_instance.init(hwver.unique_id);
if (ret < 0) {
return ret;
}
/* Start node info retriever to fetch node info from new nodes */
ret = _node_info_retriever.start();
if (ret < 0) {
return ret;
}
/* Start the fw version checker */
ret = _fw_upgrade_trigger.start(_node_info_retriever, _fw_version_checker.getFirmwarePath());
if (ret < 0) {
return ret;
}
/* Start the Node */
return OK;
}
__attribute__((optimize("-O0")))
pthread_addr_t UavcanServers::run(pthread_addr_t)
{
Lock lock(_subnode_mutex);
while (1) {
if (_check_fw == true) {
_check_fw = false;
_node_info_retriever.invalidateAll();
}
const int spin_res = _subnode.spin(uavcan::MonotonicDuration::fromMSec(100));
if (spin_res < 0) {
warnx("node spin error %i", spin_res);
}
}
warnx("exiting.");
return (pthread_addr_t) 0;
}

145
src/modules/uavcan/uavcan_servers.hpp Normal file
View File

@ -0,0 +1,145 @@
/****************************************************************************
*
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include <nuttx/config.h>
#include <uavcan_stm32/uavcan_stm32.hpp>
#include <drivers/device/device.h>
#include <systemlib/perf_counter.h>
#include <uavcan/node/sub_node.hpp>
#include <uavcan/protocol/node_status_monitor.hpp>
# include <uavcan/protocol/dynamic_node_id_server/centralized.hpp>
# include <uavcan/protocol/node_info_retriever.hpp>
# include <uavcan_posix/basic_file_server_backend.hpp>
# include <uavcan/protocol/firmware_update_trigger.hpp>
# include <uavcan/protocol/file_server.hpp>
# include <uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp>
# include <uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp>
# include <uavcan_posix/firmware_version_checker.hpp>
# include "uavcan_virtual_can_driver.hpp"
/**
* @file uavcan_servers.hpp
*
* Defines basic functionality of UAVCAN node.
*
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
* @author David Sidrane <david_s5@nscdg.com>
*/
#define UAVCAN_DEVICE_PATH "/dev/uavcan/esc"
#define UAVCAN_NODE_DB_PATH "/fs/microsd/uavcan.db"
#define UAVCAN_FIRMWARE_PATH "/fs/microsd/fw"
#define UAVCAN_LOG_FILE UAVCAN_NODE_DB_PATH"/trace.log"
/**
* A UAVCAN Server Sub node.
*/
class UavcanServers
{
static constexpr unsigned MemPoolSize = 64 * uavcan::MemPoolBlockSize;
static constexpr unsigned MaxCanFramsPerTransfer = 63;
/**
* This number is based on the worst case max number of frames per interface. With
* MemPoolBlockSize set at 48 this is 6048 Bytes.
*
* The servers can be forced to use the primary interface only, this can be achieved simply by passing
* 1 instead of UAVCAN_STM32_NUM_IFACES into the constructor of the virtual CAN driver.
*/
static constexpr unsigned QueuePoolSize =
(UAVCAN_STM32_NUM_IFACES * uavcan::MemPoolBlockSize *MaxCanFramsPerTransfer);
static constexpr unsigned StackSize = 3500;
static constexpr unsigned Priority = 120;
typedef uavcan::SubNode<MemPoolSize> SubNode;
public:
UavcanServers(uavcan::INode &main_node);
virtual ~UavcanServers();
static int start(unsigned num_ifaces, uavcan::INode &main_node);
static int stop(void);
SubNode &get_node() { return _subnode; }
static UavcanServers *instance() { return _instance; }
/*
* Set the main node's pointer to to the injector
* This is a work around as main_node.getDispatcher().remeveRxFrameListener();
* would require a dynamic cast and rtti is not enabled.
*/
void attachITxQueueInjector(ITxQueueInjector **injector) {*injector = &_vdriver;}
void requestCheckAllNodesFirmwareAndUpdate() { _check_fw = true; }
private:
pthread_t _subnode_thread;
pthread_mutex_t _subnode_mutex;
int init(unsigned num_ifaces);
void deinit(void);
pthread_addr_t run(pthread_addr_t);
static UavcanServers *_instance; ///< singleton pointer
typedef VirtualCanDriver<QueuePoolSize> vCanDriver;
vCanDriver _vdriver;
uavcan::SubNode<MemPoolSize> _subnode; ///< library instance
uavcan::INode &_main_node; ///< library instance
uavcan_posix::dynamic_node_id_server::FileEventTracer _tracer;
uavcan_posix::dynamic_node_id_server::FileStorageBackend _storage_backend;
uavcan_posix::FirmwareVersionChecker _fw_version_checker;
uavcan::dynamic_node_id_server::CentralizedServer _server_instance; ///< server singleton pointer
uavcan_posix::BasicFileSeverBackend _fileserver_backend;
uavcan::NodeInfoRetriever _node_info_retriever;
uavcan::FirmwareUpdateTrigger _fw_upgrade_trigger;
uavcan::BasicFileServer _fw_server;
bool _mutex_inited;
volatile bool _check_fw;
};

506
src/modules/uavcan/uavcan_virtual_can_driver.hpp Normal file
View File

@ -0,0 +1,506 @@
/****************************************************************************
*
* Copyright (c) 2015 PX4 Development Team. All rights reserved.
* Pavel Kirienko <pavel.kirienko@zubax.com>
* David Sidrane <david_s5@nscdg.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include <nuttx/config.h>
#include <cstdlib>
#include <cstdint>
#include <cstring>
#include <cstdio>
#include <fcntl.h>
#include <pthread.h>
#include <semaphore.h>
#include <debug.h>
#include <uavcan_stm32/clock.hpp>
#include <uavcan/node/sub_node.hpp>
#include <uavcan/protocol/node_status_monitor.hpp>
/*
* General purpose wrapper around os's mutual exclusion
* mechanism.
*
* It supports the
*
* Note the naming of thier_mutex_ implies that the underlying
* mutex is owned by class using the Lock.
* and this wrapper provides service to initialize and de initialize
* the mutex.
*/
class Lock
{
pthread_mutex_t &thier_mutex_;
public:
Lock(pthread_mutex_t &m) :
thier_mutex_(m)
{
(void)pthread_mutex_lock(&m);
}
~Lock()
{
(void)pthread_mutex_unlock(&thier_mutex_);
}
static int init(pthread_mutex_t &thier_mutex_)
{
return pthread_mutex_init(&thier_mutex_, NULL);
}
static int deinit(pthread_mutex_t &thier_mutex_)
{
return pthread_mutex_destroy(&thier_mutex_);
}
};
/**
* Generic queue based on the linked list class defined in libuavcan.
* This class does not use heap memory.
*/
template <typename T>
class Queue
{
struct Item : public uavcan::LinkedListNode<Item> {
T payload;
template <typename... Args>
Item(Args... args) : payload(args...) { }
};
uavcan::LimitedPoolAllocator allocator_;
uavcan::LinkedListRoot<Item> list_;
public:
Queue(uavcan::IPoolAllocator &arg_allocator, std::size_t block_allocation_quota) :
allocator_(arg_allocator, block_allocation_quota)
{
uavcan::IsDynamicallyAllocatable<Item>::check();
}
bool isEmpty() const { return list_.isEmpty(); }
/**
* Creates one item in-place at the end of the list.
* Returns true if the item was appended successfully, false if there's not enough memory.
* Complexity is O(N) where N is queue length.
*/
template <typename... Args>
bool tryEmplace(Args... args)
{
// Allocating memory
void *const ptr = allocator_.allocate(sizeof(Item));
if (ptr == nullptr) {
return false;
}
// Constructing the new item
Item *const item = new(ptr) Item(args...);
assert(item != nullptr);
// Inserting the new item at the end of the list
Item *p = list_.get();
if (p == nullptr) {
list_.insert(item);
} else {
while (p->getNextListNode() != nullptr) {
p = p->getNextListNode();
}
assert(p->getNextListNode() == nullptr);
p->setNextListNode(item);
assert(p->getNextListNode()->getNextListNode() == nullptr);
}
return true;
}
/**
* Accesses the first element.
* Nullptr will be returned if the queue is empty.
* Complexity is O(1).
*/
T *peek() { return isEmpty() ? nullptr : &list_.get()->payload; }
const T *peek() const { return isEmpty() ? nullptr : &list_.get()->payload; }
/**
* Removes the first element.
* If the queue is empty, nothing will be done and assertion failure will be triggered.
* Complexity is O(1).
*/
void pop()
{
Item *const item = list_.get();
assert(item != nullptr);
if (item != nullptr) {
list_.remove(item);
item->~Item();
allocator_.deallocate(item);
}
}
};
/**
* Objects of this class are owned by the sub-node thread.
* This class does not use heap memory.
*/
class VirtualCanIface : public uavcan::ICanIface,
uavcan::Noncopyable
{
/**
* This class re-defines uavcan::RxCanFrame with flags.
* Simple inheritance or composition won't work here, because the 40 byte limit will be exceeded,
* rendering this class unusable with Queue<>.
*/
struct RxItem: public uavcan::CanFrame {
const uavcan::MonotonicTime ts_mono;
const uavcan::UtcTime ts_utc;
const uavcan::CanIOFlags flags;
const uint8_t iface_index;
RxItem(const uavcan::CanRxFrame &arg_frame, uavcan::CanIOFlags arg_flags) :
uavcan::CanFrame(arg_frame),
ts_mono(arg_frame.ts_mono),
ts_utc(arg_frame.ts_utc),
flags(arg_flags),
iface_index(arg_frame.iface_index)
{
// Making sure it will fit into a pool block with a pointer prefix
static_assert(sizeof(RxItem) <= (uavcan::MemPoolBlockSize - 8), "Bad coder, no coffee");
}
};
pthread_mutex_t &common_driver_mutex_;
uavcan::CanTxQueue prioritized_tx_queue_;
Queue<RxItem> rx_queue_;
int16_t send(const uavcan::CanFrame &frame, uavcan::MonotonicTime tx_deadline, uavcan::CanIOFlags flags) override
{
Lock lock(common_driver_mutex_);
prioritized_tx_queue_.push(frame, tx_deadline, uavcan::CanTxQueue::Volatile, flags);
return 1;
}
int16_t receive(uavcan::CanFrame &out_frame, uavcan::MonotonicTime &out_ts_monotonic,
uavcan::UtcTime &out_ts_utc, uavcan::CanIOFlags &out_flags) override
{
Lock lock(common_driver_mutex_);
if (rx_queue_.isEmpty()) {
return 0;
}
const auto item = *rx_queue_.peek();
rx_queue_.pop();
out_frame = item;
out_ts_monotonic = item.ts_mono;
out_ts_utc = item.ts_utc;
out_flags = item.flags;
return 1;
}
int16_t configureFilters(const uavcan::CanFilterConfig *, std::uint16_t) override { return -uavcan::ErrDriver; }
uint16_t getNumFilters() const override { return 0; }
uint64_t getErrorCount() const override { return 0; }
public:
VirtualCanIface(uavcan::IPoolAllocator &allocator, uavcan::ISystemClock &clock,
pthread_mutex_t &arg_mutex, unsigned quota_per_queue) :
common_driver_mutex_(arg_mutex),
prioritized_tx_queue_(allocator, clock, quota_per_queue),
rx_queue_(allocator, quota_per_queue)
{
}
~VirtualCanIface()
{
}
/**
* Note that RX queue overwrites oldest items when overflowed.
* Call this from the main thread only.
* No additional locking is required.
*/
void addRxFrame(const uavcan::CanRxFrame &frame, uavcan::CanIOFlags flags)
{
Lock lock(common_driver_mutex_);
if (!rx_queue_.tryEmplace(frame, flags) && !rx_queue_.isEmpty()) {
rx_queue_.pop();
(void)rx_queue_.tryEmplace(frame, flags);
}
}
/**
* Call this from the main thread only.
* No additional locking is required.
*/
void flushTxQueueTo(uavcan::INode &main_node, std::uint8_t iface_index)
{
Lock lock(common_driver_mutex_);
const std::uint8_t iface_mask = static_cast<std::uint8_t>(1U << iface_index);
while (auto e = prioritized_tx_queue_.peek()) {
UAVCAN_TRACE("VirtualCanIface", "TX injection [iface=0x%02x]: %s",
unsigned(iface_mask), e->toString().c_str());
const int res = main_node.injectTxFrame(e->frame, e->deadline, iface_mask,
uavcan::CanTxQueue::Qos(e->qos), e->flags);
prioritized_tx_queue_.remove(e);
if (res <= 0) {
break;
}
}
}
/**
* Call this from the sub-node thread only.
* No additional locking is required.
*/
bool hasDataInRxQueue()
{
Lock lock(common_driver_mutex_);
return !rx_queue_.isEmpty();
}
};
/**
* This interface defines one method that will be called by the main node thread periodically in order to
* transfer contents of TX queue of the sub-node into the TX queue of the main node.
*/
class ITxQueueInjector
{
public:
virtual ~ITxQueueInjector() { }
/**
* Flush contents of TX queues into the main node.
* @param main_node Reference to the main node.
*/
virtual void injectTxFramesInto(uavcan::INode &main_node) = 0;
};
/**
* Objects of this class are owned by the sub-node thread.
* This class does not use heap memory.
* @tparam SharedMemoryPoolSize Amount of memory, in bytes, that will be statically allocated for the
* memory pool that will be shared across all interfaces for RX/TX queues.
* Typically this value should be no less than 4K per interface.
*/
template <unsigned SharedMemoryPoolSize>
class VirtualCanDriver : public uavcan::ICanDriver,
public uavcan::IRxFrameListener,
public ITxQueueInjector,
uavcan::Noncopyable
{
class Event
{
FAR sem_t sem;
public:
int init()
{
return sem_init(&sem, 0, 0);
}
int deinit()
{
return sem_destroy(&sem);
}
Event()
{
}
~Event()
{
}
/**
*/
void waitFor(uavcan::MonotonicDuration duration)
{
static const unsigned NsPerSec = 1000000000;
if (duration.isPositive()) {
auto abstime = ::timespec();
if (clock_gettime(CLOCK_REALTIME, &abstime) >= 0) {
abstime.tv_nsec += duration.toUSec() * 1000;
if (abstime.tv_nsec >= NsPerSec) {
abstime.tv_sec++;
abstime.tv_nsec -= NsPerSec;
}
(void)sem_timedwait(&sem, &abstime);
}
}
}
void signal()
{
int count;
int rv = sem_getvalue(&sem, &count);
if (rv > 0 && count <= 0) {
sem_post(&sem);
}
}
};
Event event_; ///< Used to unblock the select() call when IO happens.
pthread_mutex_t driver_mutex_; ///< Shared across all ifaces
uavcan::PoolAllocator<SharedMemoryPoolSize, uavcan::MemPoolBlockSize> allocator_; ///< Shared across all ifaces
uavcan::LazyConstructor<VirtualCanIface> ifaces_[uavcan::MaxCanIfaces];
const unsigned num_ifaces_;
uavcan::ISystemClock &clock_;
uavcan::ICanIface *getIface(uint8_t iface_index) override
{
return (iface_index < num_ifaces_) ? ifaces_[iface_index].operator VirtualCanIface * () : nullptr;
}
uint8_t getNumIfaces() const override { return num_ifaces_; }
/**
* This and other methods of ICanDriver will be invoked by the sub-node thread.
*/
int16_t select(uavcan::CanSelectMasks &inout_masks,
const uavcan::CanFrame * (&)[uavcan::MaxCanIfaces],
uavcan::MonotonicTime blocking_deadline) override
{
bool need_block = (inout_masks.write == 0); // Write queue is infinite
for (unsigned i = 0; need_block && (i < num_ifaces_); i++) {
const bool need_read = inout_masks.read & (1U << i);
if (need_read && ifaces_[i]->hasDataInRxQueue()) {
need_block = false;
}
}
if (need_block) {
event_.waitFor(blocking_deadline - clock_.getMonotonic());
}
inout_masks = uavcan::CanSelectMasks();
for (unsigned i = 0; i < num_ifaces_; i++) {
const std::uint8_t iface_mask = 1U << i;
inout_masks.write |= iface_mask; // Always ready to write
if (ifaces_[i]->hasDataInRxQueue()) {
inout_masks.read |= iface_mask;
}
}
return num_ifaces_; // We're always ready to write, hence > 0.
}
/**
* This handler will be invoked by the main node thread.
*/
void handleRxFrame(const uavcan::CanRxFrame &frame, uavcan::CanIOFlags flags) override
{
UAVCAN_TRACE("VirtualCanDriver", "RX [flags=%u]: %s", unsigned(flags), frame.toString().c_str());
if (frame.iface_index < num_ifaces_) {
ifaces_[frame.iface_index]->addRxFrame(frame, flags);
event_.signal();
}
}
/**
* This method will be invoked by the main node thread.
*/
void injectTxFramesInto(uavcan::INode &main_node) override
{
for (unsigned i = 0; i < num_ifaces_; i++) {
ifaces_[i]->flushTxQueueTo(main_node, i);
}
event_.signal();
}
public:
VirtualCanDriver(unsigned arg_num_ifaces, uavcan::ISystemClock &system_clock) :
num_ifaces_(arg_num_ifaces),
clock_(system_clock)
{
Lock::init(driver_mutex_);
event_.init();
assert(num_ifaces_ > 0 && num_ifaces_ <= uavcan::MaxCanIfaces);
const unsigned quota_per_iface = allocator_.getNumBlocks() / num_ifaces_;
const unsigned quota_per_queue = quota_per_iface; // 2x overcommit
UAVCAN_TRACE("VirtualCanDriver", "Total blocks: %u, quota per queue: %u",
unsigned(allocator_.getNumBlocks()), unsigned(quota_per_queue));
for (unsigned i = 0; i < num_ifaces_; i++) {
ifaces_[i].template construct<uavcan::IPoolAllocator &, uavcan::ISystemClock &,
pthread_mutex_t &, unsigned>(allocator_, clock_, driver_mutex_, quota_per_queue);
}
}
~VirtualCanDriver()
{
Lock::deinit(driver_mutex_);
event_.deinit();
}
};