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

UAVCAN: initializing all bridges by default

This commit is contained in:
Pavel Kirienko 2014-08-24 00:06:47 +04:00
parent 0f124963d4
commit e9da830316
5 changed files with 29 additions and 126 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -56,17 +56,12 @@ _report_pub(-1)
int UavcanGnssBridge::init()
{
int res = -1;
// GNSS fix subscription
res = _sub_fix.start(FixCbBinder(this, &UavcanGnssBridge::gnss_fix_sub_cb));
int res = _sub_fix.start(FixCbBinder(this, &UavcanGnssBridge::gnss_fix_sub_cb));
if (res < 0)
{
warnx("GNSS fix sub failed %i", res);
return res;
}
warnx("gnss sensor bridge init ok");
return res;
}

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

@ -45,27 +45,11 @@
/*
* IUavcanSensorBridge
*/
IUavcanSensorBridge* IUavcanSensorBridge::make(uavcan::INode &node, const char *bridge_name)
void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge*> &list)
{
/*
* TODO: make a linked list of known implementations at startup?
*/
if (!std::strncmp(UavcanGnssBridge::NAME, bridge_name, MAX_NAME_LEN)) {
return new UavcanGnssBridge(node);
} else if (!std::strncmp(UavcanMagnetometerBridge::NAME, bridge_name, MAX_NAME_LEN)) {
return new UavcanMagnetometerBridge(node);
} else if (!std::strncmp(UavcanBarometerBridge::NAME, bridge_name, MAX_NAME_LEN)) {
return new UavcanBarometerBridge(node);
} else {
return nullptr;
}
}
void IUavcanSensorBridge::print_known_names(const char *prefix)
{
printf("%s%s\n", prefix, UavcanGnssBridge::NAME);
printf("%s%s\n", prefix, UavcanMagnetometerBridge::NAME);
printf("%s%s\n", prefix, UavcanBarometerBridge::NAME);
list.add(new UavcanBarometerBridge(node));
list.add(new UavcanMagnetometerBridge(node));
list.add(new UavcanGnssBridge(node));
}
/*

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

@ -73,12 +73,7 @@ public:
* Creates a bridge object by its ASCII name, e.g. "gnss", "mag".
* @return nullptr if such bridge can't be created.
*/
static IUavcanSensorBridge* make(uavcan::INode &node, const char *bridge_name);
/**
* Prints all valid bridge names into stdout via printf(), one name per line with prefix.
*/
static void print_known_names(const char *prefix);
static void make_all(uavcan::INode &node, List<IUavcanSensorBridge*> &list);
};
/**

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

@ -217,11 +217,12 @@ int UavcanNode::init(uavcan::NodeID node_id)
{
int ret = -1;
/* do regular cdev init */
// Do regular cdev init
ret = CDev::init();
if (ret != OK)
if (ret != OK) {
return ret;
}
_node.setName("org.pixhawk.pixhawk");
@ -229,10 +230,24 @@ int UavcanNode::init(uavcan::NodeID node_id)
fill_node_info();
/* initializing the bridges UAVCAN <--> uORB */
// Actuators
ret = _esc_controller.init();
if (ret < 0)
if (ret < 0) {
return ret;
}
// Sensor bridges
IUavcanSensorBridge::make_all(_node, _sensor_bridges);
auto br = _sensor_bridges.getHead();
while (br != nullptr) {
ret = br->init();
if (ret < 0) {
warnx("cannot init sensor bridge '%s' (%d)", br->get_name(), ret);
return ret;
}
warnx("sensor bridge '%s' init ok", br->get_name());
br = br->getSibling();
}
return _node.start();
}
@ -522,62 +537,10 @@ UavcanNode::print_info()
warnx("not running, start first");
}
(void)pthread_mutex_lock(&_node_mutex);
warnx("actuators control groups: sub: %u / req: %u / fds: %u", (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num);
warnx("mixer: %s", (_mixers == nullptr) ? "NONE" : "OK");
}
int UavcanNode::sensor_enable(const char *bridge_name)
{
int retval = -1;
(void)pthread_mutex_lock(&_node_mutex);
// Checking if such bridge already exists
{
auto pos = _sensor_bridges.getHead();
while (pos != nullptr) {
if (!std::strncmp(pos->get_name(), bridge_name, IUavcanSensorBridge::MAX_NAME_LEN)) {
warnx("sensor bridge '%s' already exists", bridge_name);
retval = -1;
goto leave;
}
pos = pos->getSibling();
}
}
// Creating and initing
{
auto bridge = IUavcanSensorBridge::make(get_node(), bridge_name);
if (bridge == nullptr) {
warnx("cannot create sensor bridge '%s'", bridge_name);
retval = -1;
goto leave;
}
assert(!std::strncmp(bridge->get_name(), bridge_name, IUavcanSensorBridge::MAX_NAME_LEN));
retval = bridge->init();
if (retval >= 0) {
_sensor_bridges.add(bridge);
} else {
delete bridge;
}
}
leave:
(void)pthread_mutex_unlock(&_node_mutex);
return retval;
}
void UavcanNode::sensor_print_enabled()
{
(void)pthread_mutex_lock(&_node_mutex);
auto pos = _sensor_bridges.getHead();
while (pos != nullptr) {
warnx("%s", pos->get_name());
pos = pos->getSibling();
}
(void)pthread_mutex_unlock(&_node_mutex);
}
@ -588,12 +551,7 @@ void UavcanNode::sensor_print_enabled()
static void print_usage()
{
warnx("usage: \n"
"\tuavcan start\n"
"\tuavcan sensor enable <sensor name>\n"
"\tuavcan sensor list");
warnx("known sensor bridges:");
IUavcanSensorBridge::print_known_names("\t");
"\tuavcan {start|status|stop}");
}
extern "C" __EXPORT int uavcan_main(int argc, char *argv[]);
@ -645,31 +603,6 @@ int uavcan_main(int argc, char *argv[])
::exit(0);
}
if (!std::strcmp(argv[1], "sensor")) {
if (argc < 3) {
print_usage();
::exit(1);
}
if (!std::strcmp(argv[2], "list")) {
inst->sensor_print_enabled();
::exit(0);
}
if (argc < 4) {
print_usage();
::exit(1);
}
if (!std::strcmp(argv[2], "enable")) {
const int res = inst->sensor_enable(argv[3]);
if (res < 0) {
warnx("failed to enable sensor '%s': error %d", argv[3], res);
}
::exit(0);
}
}
print_usage();
::exit(1);
}

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

@ -89,10 +89,6 @@ public:
void print_info();
int sensor_enable(const char *bridge_name);
void sensor_print_enabled();
static UavcanNode* instance() { return _instance; }
private:
@ -115,7 +111,7 @@ private:
UavcanEscController _esc_controller;
List<IUavcanSensorBridge*> _sensor_bridges; ///< Append-only list of active sensor bridges
List<IUavcanSensorBridge*> _sensor_bridges; ///< List of active sensor bridges
MixerGroup *_mixers = nullptr;