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

UAVCAN: Refactored and generalized sensor bridge support

This commit is contained in:
Pavel Kirienko 2014-08-22 01:52:23 +04:00
parent f87860bc96
commit 0d9f6b6e2e
9 changed files with 265 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/modules/uavcan/esc_controller.cpp → src/modules/uavcan/actuators/esc.cpp
View File

@ -32,12 +32,12 @@
****************************************************************************/
/**
* @file esc_controller.cpp
* @file esc.cpp
*
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include "esc_controller.hpp"
#include "esc.hpp"
#include <systemlib/err.h>
UavcanEscController::UavcanEscController(uavcan::INode &node) :

2
src/modules/uavcan/esc_controller.hpp → src/modules/uavcan/actuators/esc.hpp
View File

@ -32,7 +32,7 @@
****************************************************************************/
/**
* @file esc_controller.hpp
* @file esc.hpp
*
* UAVCAN <--> ORB bridge for ESC messages:
* uavcan.equipment.esc.RawCommand

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

@ -40,10 +40,16 @@ MODULE_COMMAND = uavcan
MAXOPTIMIZATION = -Os
SRCS += uavcan_main.cpp \
uavcan_clock.cpp \
esc_controller.cpp \
gnss_receiver.cpp
# Main
SRCS += uavcan_main.cpp \
uavcan_clock.cpp
# Actuators
SRCS += actuators/esc.cpp
# Sensors
SRCS += sensors/sensor_bridge.cpp \
sensors/gnss.cpp
#
# libuavcan

15
src/modules/uavcan/gnss_receiver.cpp → src/modules/uavcan/sensors/gnss.cpp
View File

@ -32,32 +32,34 @@
****************************************************************************/
/**
* @file gnss_receiver.cpp
* @file gnss.cpp
*
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
* @author Andrew Chambers <achamber@gmail.com>
*
*/
#include "gnss_receiver.hpp"
#include "gnss.hpp"
#include <systemlib/err.h>
#include <mathlib/mathlib.h>
#define MM_PER_CM 10 // Millimeters per centimeter
UavcanGnssReceiver::UavcanGnssReceiver(uavcan::INode &node) :
UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) :
_node(node),
_uavcan_sub_status(node),
_report_pub(-1)
{
}
int UavcanGnssReceiver::init()
const char *UavcanGnssBridge::get_name() const { return "gnss"; }
int UavcanGnssBridge::init()
{
int res = -1;
// GNSS fix subscription
res = _uavcan_sub_status.start(FixCbBinder(this, &UavcanGnssReceiver::gnss_fix_sub_cb));
res = _uavcan_sub_status.start(FixCbBinder(this, &UavcanGnssBridge::gnss_fix_sub_cb));
if (res < 0)
{
warnx("GNSS fix sub failed %i", res);
@ -67,10 +69,11 @@ int UavcanGnssReceiver::init()
// Clear the uORB GPS report
memset(&_report, 0, sizeof(_report));
warnx("gnss sensor bridge init ok");
return res;
}
void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg)
void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg)
{
_report.timestamp_position = hrt_absolute_time();
_report.lat = msg.lat_1e7;

18
src/modules/uavcan/gnss_receiver.hpp → src/modules/uavcan/sensors/gnss.hpp
View File

@ -32,7 +32,7 @@
****************************************************************************/
/**
* @file gnss_receiver.hpp
* @file gnss.hpp
*
* UAVCAN --> ORB bridge for GNSS messages:
* uavcan.equipment.gnss.Fix
@ -51,12 +51,16 @@
#include <uavcan/uavcan.hpp>
#include <uavcan/equipment/gnss/Fix.hpp>
class UavcanGnssReceiver
#include "sensor_bridge.hpp"
class UavcanGnssBridge : public IUavcanSensorBridge
{
public:
UavcanGnssReceiver(uavcan::INode& node);
UavcanGnssBridge(uavcan::INode& node);
int init();
const char *get_name() const override;
int init() override;
private:
/**
@ -65,14 +69,14 @@ private:
void gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg);
typedef uavcan::MethodBinder<UavcanGnssReceiver*,
void (UavcanGnssReceiver::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix>&)>
typedef uavcan::MethodBinder<UavcanGnssBridge*,
void (UavcanGnssBridge::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix>&)>
FixCbBinder;
/*
* libuavcan related things
*/
uavcan::INode &_node;
uavcan::INode &_node;
uavcan::Subscriber<uavcan::equipment::gnss::Fix, FixCbBinder> _uavcan_sub_status;
/*

48
src/modules/uavcan/sensors/sensor_bridge.cpp Normal file
View File

@ -0,0 +1,48 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/**
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include "sensor_bridge.hpp"
#include "gnss.hpp"
IUavcanSensorBridge* IUavcanSensorBridge::make(uavcan::INode &node, const char *bridge_name)
{
if (!std::strncmp("gnss", bridge_name, MaxNameLen)) {
return new UavcanGnssBridge(node);
} else {
return nullptr;
}
}

70
src/modules/uavcan/sensors/sensor_bridge.hpp Normal file
View File

@ -0,0 +1,70 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/**
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#pragma once
#include <containers/List.hpp>
#include <uavcan/uavcan.hpp>
/**
* A sensor bridge class must implement this interface.
*/
class IUavcanSensorBridge : uavcan::Noncopyable, public ListNode<IUavcanSensorBridge*>
{
public:
static constexpr unsigned MaxNameLen = 20;
virtual ~IUavcanSensorBridge() { }
/**
* Returns ASCII name of the bridge.
*/
virtual const char *get_name() const = 0;
/**
* Starts the bridge.
* @return Non-negative value on success, negative on error.
*/
virtual int init() = 0;
/**
* Sensor bridge factory.
* 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);
};

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

@ -65,16 +65,18 @@ UavcanNode *UavcanNode::_instance;
UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) :
CDev("uavcan", UAVCAN_DEVICE_PATH),
_node(can_driver, system_clock),
_esc_controller(_node),
_gnss_receiver(_node)
_node_mutex(),
_esc_controller(_node)
{
_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);
// memset(_controls, 0, sizeof(_controls));
// memset(_poll_fds, 0, sizeof(_poll_fds));
const int res = pthread_mutex_init(&_node_mutex, nullptr);
if (res < 0) {
std::abort();
}
}
UavcanNode::~UavcanNode()
@ -99,7 +101,7 @@ UavcanNode::~UavcanNode()
}
/* clean up the alternate device node */
// unregister_driver(PWM_OUTPUT_DEVICE_PATH);
// unregister_driver(PWM_OUTPUT_DEVICE_PATH);
::close(_armed_sub);
@ -231,10 +233,6 @@ int UavcanNode::init(uavcan::NodeID node_id)
if (ret < 0)
return ret;
ret = _gnss_receiver.init();
if (ret < 0)
return ret;
return _node.start();
}
@ -248,6 +246,8 @@ void UavcanNode::node_spin_once()
int UavcanNode::run()
{
(void)pthread_mutex_lock(&_node_mutex);
const unsigned PollTimeoutMs = 50;
// XXX figure out the output count
@ -291,8 +291,13 @@ int UavcanNode::run()
_groups_subscribed = _groups_required;
}
// 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...
@ -352,7 +357,6 @@ int UavcanNode::run()
// Output to the bus
_esc_controller.update_outputs(outputs.output, outputs.noutputs);
}
}
// Check arming state
@ -376,10 +380,7 @@ int UavcanNode::run()
}
int
UavcanNode::control_callback(uintptr_t handle,
uint8_t control_group,
uint8_t control_index,
float &input)
UavcanNode::control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input)
{
const actuator_controls_s *controls = (actuator_controls_s *)handle;
@ -524,12 +525,69 @@ UavcanNode::print_info()
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::MaxNameLen)) {
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::MaxNameLen));
retval = bridge->init();
if (retval >= 0) {
_sensor_bridges.add(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);
}
/*
* App entry point
*/
static void print_usage()
{
warnx("usage: uavcan start <node_id> [can_bitrate]");
warnx("usage: \n"
"\tuavcan start <node_id> [can_bitrate]\n"
"\tuavcan sensor enable <sensor name>\n"
"\tuavcan sensor list");
}
extern "C" __EXPORT int uavcan_main(int argc, char *argv[]);
@ -585,7 +643,7 @@ int uavcan_main(int argc, char *argv[])
}
/* commands below require the app to be started */
UavcanNode *inst = UavcanNode::instance();
UavcanNode *const inst = UavcanNode::instance();
if (!inst) {
errx(1, "application not running");
@ -594,14 +652,37 @@ int uavcan_main(int argc, char *argv[])
if (!std::strcmp(argv[1], "status") || !std::strcmp(argv[1], "info")) {
inst->print_info();
return OK;
::exit(0);
}
if (!std::strcmp(argv[1], "stop")) {
delete inst;
inst = nullptr;
return OK;
::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();

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

@ -42,8 +42,8 @@
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_armed.h>
#include "esc_controller.hpp"
#include "gnss_receiver.hpp"
#include "actuators/esc.hpp"
#include "sensors/sensor_bridge.hpp"
/**
* @file uavcan_main.hpp
@ -77,12 +77,10 @@ public:
static int start(uavcan::NodeID node_id, uint32_t bitrate);
Node& getNode() { return _node; }
Node& get_node() { return _node; }
static int control_callback(uintptr_t handle,
uint8_t control_group,
uint8_t control_index,
float &input);
// TODO: move the actuator mixing stuff into the ESC controller class
static int control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input);
void subscribe();
@ -91,6 +89,10 @@ public:
void print_info();
int sensor_enable(const char *bridge_name);
void sensor_print_enabled();
static UavcanNode* instance() { return _instance; }
private:
@ -109,8 +111,11 @@ private:
static UavcanNode *_instance; ///< singleton pointer
Node _node; ///< library instance
pthread_mutex_t _node_mutex;
UavcanEscController _esc_controller;
UavcanGnssReceiver _gnss_receiver;
List<IUavcanSensorBridge*> _sensor_bridges; ///< Append-only list of active sensor bridges
MixerGroup *_mixers = nullptr;