From f87860bc96a61c538de50b5b74aba0452f9b5784 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Thu, 21 Aug 2014 23:23:35 +0400 Subject: [PATCH 01/34] UAVCAN update --- uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/uavcan b/uavcan index 6980ee8240..446577e4fb 160000 --- a/uavcan +++ b/uavcan @@ -1 +1 @@ -Subproject commit 6980ee824074aa2f7a62221bf6040ee411119520 +Subproject commit 446577e4fb979ee4c629081368233eaa5683d086 From 0d9f6b6e2ec86f7fa95bc2e7650256e1392544b2 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 22 Aug 2014 01:52:23 +0400 Subject: [PATCH 02/34] UAVCAN: Refactored and generalized sensor bridge support --- .../{esc_controller.cpp => actuators/esc.cpp} | 4 +- .../{esc_controller.hpp => actuators/esc.hpp} | 2 +- src/modules/uavcan/module.mk | 14 +- .../{gnss_receiver.cpp => sensors/gnss.cpp} | 15 ++- .../{gnss_receiver.hpp => sensors/gnss.hpp} | 18 ++- src/modules/uavcan/sensors/sensor_bridge.cpp | 48 +++++++ src/modules/uavcan/sensors/sensor_bridge.hpp | 70 ++++++++++ src/modules/uavcan/uavcan_main.cpp | 121 +++++++++++++++--- src/modules/uavcan/uavcan_main.hpp | 21 +-- 9 files changed, 265 insertions(+), 48 deletions(-) rename src/modules/uavcan/{esc_controller.cpp => actuators/esc.cpp} (98%) rename src/modules/uavcan/{esc_controller.hpp => actuators/esc.hpp} (99%) rename src/modules/uavcan/{gnss_receiver.cpp => sensors/gnss.cpp} (92%) rename src/modules/uavcan/{gnss_receiver.hpp => sensors/gnss.hpp} (87%) create mode 100644 src/modules/uavcan/sensors/sensor_bridge.cpp create mode 100644 src/modules/uavcan/sensors/sensor_bridge.hpp diff --git a/src/modules/uavcan/esc_controller.cpp b/src/modules/uavcan/actuators/esc.cpp similarity index 98% rename from src/modules/uavcan/esc_controller.cpp rename to src/modules/uavcan/actuators/esc.cpp index 406eba88c4..223d947312 100644 --- a/src/modules/uavcan/esc_controller.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -32,12 +32,12 @@ ****************************************************************************/ /** - * @file esc_controller.cpp + * @file esc.cpp * * @author Pavel Kirienko */ -#include "esc_controller.hpp" +#include "esc.hpp" #include UavcanEscController::UavcanEscController(uavcan::INode &node) : diff --git a/src/modules/uavcan/esc_controller.hpp b/src/modules/uavcan/actuators/esc.hpp similarity index 99% rename from src/modules/uavcan/esc_controller.hpp rename to src/modules/uavcan/actuators/esc.hpp index 559ede561e..cf09882104 100644 --- a/src/modules/uavcan/esc_controller.hpp +++ b/src/modules/uavcan/actuators/esc.hpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file esc_controller.hpp + * @file esc.hpp * * UAVCAN <--> ORB bridge for ESC messages: * uavcan.equipment.esc.RawCommand diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index 3865f24681..2b97290456 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -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 diff --git a/src/modules/uavcan/gnss_receiver.cpp b/src/modules/uavcan/sensors/gnss.cpp similarity index 92% rename from src/modules/uavcan/gnss_receiver.cpp rename to src/modules/uavcan/sensors/gnss.cpp index ba1fe5e499..4fc0743fff 100644 --- a/src/modules/uavcan/gnss_receiver.cpp +++ b/src/modules/uavcan/sensors/gnss.cpp @@ -32,32 +32,34 @@ ****************************************************************************/ /** - * @file gnss_receiver.cpp + * @file gnss.cpp * * @author Pavel Kirienko * @author Andrew Chambers * */ -#include "gnss_receiver.hpp" +#include "gnss.hpp" #include #include #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 &msg) +void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure &msg) { _report.timestamp_position = hrt_absolute_time(); _report.lat = msg.lat_1e7; diff --git a/src/modules/uavcan/gnss_receiver.hpp b/src/modules/uavcan/sensors/gnss.hpp similarity index 87% rename from src/modules/uavcan/gnss_receiver.hpp rename to src/modules/uavcan/sensors/gnss.hpp index 18df8da2f5..eec595ad10 100644 --- a/src/modules/uavcan/gnss_receiver.hpp +++ b/src/modules/uavcan/sensors/gnss.hpp @@ -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 #include -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 &msg); - typedef uavcan::MethodBinder&)> + typedef uavcan::MethodBinder&)> FixCbBinder; /* * libuavcan related things */ - uavcan::INode &_node; + uavcan::INode &_node; uavcan::Subscriber _uavcan_sub_status; /* diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp new file mode 100644 index 0000000000..ba27d6c71c --- /dev/null +++ b/src/modules/uavcan/sensors/sensor_bridge.cpp @@ -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 + */ + +#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; + } +} diff --git a/src/modules/uavcan/sensors/sensor_bridge.hpp b/src/modules/uavcan/sensors/sensor_bridge.hpp new file mode 100644 index 0000000000..1667c2b57d --- /dev/null +++ b/src/modules/uavcan/sensors/sensor_bridge.hpp @@ -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 + */ + +#pragma once + +#include +#include + +/** + * A sensor bridge class must implement this interface. + */ +class IUavcanSensorBridge : uavcan::Noncopyable, public ListNode +{ +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); +}; diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 4535b6d6ad..fc5521aa7e 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -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 [can_bitrate]"); + warnx("usage: \n" + "\tuavcan start [can_bitrate]\n" + "\tuavcan sensor enable \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(); diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 05b66fd7b2..bca1aa530d 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -42,8 +42,8 @@ #include #include -#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 _sensor_bridges; ///< Append-only list of active sensor bridges MixerGroup *_mixers = nullptr; From f820010a2b51d03f0099d3b4853e0620593721e6 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 22 Aug 2014 13:24:31 +0400 Subject: [PATCH 03/34] UAVCAN GNSS subscription name fix --- src/modules/uavcan/sensors/gnss.cpp | 4 ++-- src/modules/uavcan/sensors/gnss.hpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp index 4fc0743fff..fb89b8365f 100644 --- a/src/modules/uavcan/sensors/gnss.cpp +++ b/src/modules/uavcan/sensors/gnss.cpp @@ -47,7 +47,7 @@ UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) : _node(node), -_uavcan_sub_status(node), +_sub_fix(node), _report_pub(-1) { } @@ -59,7 +59,7 @@ int UavcanGnssBridge::init() int res = -1; // GNSS fix subscription - res = _uavcan_sub_status.start(FixCbBinder(this, &UavcanGnssBridge::gnss_fix_sub_cb)); + res = _sub_fix.start(FixCbBinder(this, &UavcanGnssBridge::gnss_fix_sub_cb)); if (res < 0) { warnx("GNSS fix sub failed %i", res); diff --git a/src/modules/uavcan/sensors/gnss.hpp b/src/modules/uavcan/sensors/gnss.hpp index eec595ad10..6bdae8de34 100644 --- a/src/modules/uavcan/sensors/gnss.hpp +++ b/src/modules/uavcan/sensors/gnss.hpp @@ -77,7 +77,7 @@ private: * libuavcan related things */ uavcan::INode &_node; - uavcan::Subscriber _uavcan_sub_status; + uavcan::Subscriber _sub_fix; /* * uORB From 54affaf633216c3aef65164c7e43674c8c26f178 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 22 Aug 2014 13:58:05 +0400 Subject: [PATCH 04/34] UAVCAN sensor enable command fix --- src/modules/uavcan/uavcan_main.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index fc5521aa7e..71302d9282 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -558,6 +558,8 @@ int UavcanNode::sensor_enable(const char *bridge_name) retval = bridge->init(); if (retval >= 0) { _sensor_bridges.add(bridge); + } else { + delete bridge; } } From 29dbe8aed582f833f2994daaaf7ab227f7a7cf45 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 22 Aug 2014 14:27:32 +0400 Subject: [PATCH 05/34] UAVCAN magnetometer driver --- src/modules/uavcan/module.mk | 3 +- src/modules/uavcan/sensors/mag.cpp | 155 +++++++++++++++++++ src/modules/uavcan/sensors/mag.hpp | 72 +++++++++ src/modules/uavcan/sensors/sensor_bridge.cpp | 3 + 4 files changed, 232 insertions(+), 1 deletion(-) create mode 100644 src/modules/uavcan/sensors/mag.cpp create mode 100644 src/modules/uavcan/sensors/mag.hpp diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index 2b97290456..de411e1e24 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -49,7 +49,8 @@ SRCS += actuators/esc.cpp # Sensors SRCS += sensors/sensor_bridge.cpp \ - sensors/gnss.cpp + sensors/gnss.cpp \ + sensors/mag.cpp # # libuavcan diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp new file mode 100644 index 0000000000..41a1a5270e --- /dev/null +++ b/src/modules/uavcan/sensors/mag.cpp @@ -0,0 +1,155 @@ +/**************************************************************************** + * + * 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 + */ + +#include "mag.hpp" + +UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode& node) : +device::CDev("uavcan_mag", "/dev/uavcan/mag"), +_sub_mag(node) +{ + _scale.x_scale = 1.0F; + _scale.y_scale = 1.0F; + _scale.z_scale = 1.0F; +} + +UavcanMagnetometerBridge::~UavcanMagnetometerBridge() +{ + if (_class_instance > 0) { + (void)unregister_class_devname(MAG_DEVICE_PATH, _class_instance); + } +} + +const char *UavcanMagnetometerBridge::get_name() const { return "mag"; } + +int UavcanMagnetometerBridge::init() +{ + // Init the libuavcan subscription + int res = _sub_mag.start(MagCbBinder(this, &UavcanMagnetometerBridge::mag_sub_cb)); + if (res < 0) { + log("failed to start uavcan sub: %d", res); + return res; + } + + // Detect our device class + _class_instance = register_class_devname(MAG_DEVICE_PATH); + switch (_class_instance) { + case CLASS_DEVICE_PRIMARY: { + _orb_id = ORB_ID(sensor_mag0); + break; + } + case CLASS_DEVICE_SECONDARY: { + _orb_id = ORB_ID(sensor_mag1); + break; + } + case CLASS_DEVICE_TERTIARY: { + _orb_id = ORB_ID(sensor_mag2); + break; + } + default: { + log("invalid class instance: %d", _class_instance); + (void)unregister_class_devname(MAG_DEVICE_PATH, _class_instance); + return -1; + } + } + + log("inited with class instance %d", _class_instance); + return 0; +} + +int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + case MAGIOCSSAMPLERATE: + case MAGIOCGSAMPLERATE: + case MAGIOCSRANGE: + case MAGIOCGRANGE: + case MAGIOCSLOWPASS: + case MAGIOCGLOWPASS: { + return -EINVAL; + } + case MAGIOCSSCALE: { + std::memcpy(&_scale, reinterpret_cast(arg), sizeof(_scale)); + log("new scale/offset: x: %f/%f y: %f/%f z: %f/%f", + double(_scale.x_scale), double(_scale.x_offset), + double(_scale.y_scale), double(_scale.y_offset), + double(_scale.z_scale), double(_scale.z_offset)); + return 0; + } + case MAGIOCGSCALE: { + std::memcpy(reinterpret_cast(arg), &_scale, sizeof(_scale)); + return 0; + } + case MAGIOCCALIBRATE: + case MAGIOCEXSTRAP: + case MAGIOCSELFTEST: { + return -EINVAL; + } + case MAGIOCGEXTERNAL: { + return 1; + } + default: { + return CDev::ioctl(filp, cmd, arg); + } + } +} + +void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure &msg) +{ + auto report = ::mag_report(); + + report.range_ga = 1.3F; // Arbitrary number, doesn't really mean anything + + report.timestamp = msg.getUtcTimestamp().toUSec(); + if (report.timestamp == 0) { + report.timestamp = msg.getMonotonicTimestamp().toUSec(); + } + + report.x = (msg.magnetic_field[0] - _scale.x_offset) * _scale.x_scale; + report.y = (msg.magnetic_field[1] - _scale.x_offset) * _scale.x_scale; + report.z = (msg.magnetic_field[2] - _scale.x_offset) * _scale.x_scale; + + if (_orb_advert >= 0) { + orb_publish(_orb_id, _orb_advert, &report); + } else { + _orb_advert = orb_advertise(_orb_id, &report); + if (_orb_advert < 0) { + log("ADVERT FAIL"); + } else { + log("advertised"); + } + } +} diff --git a/src/modules/uavcan/sensors/mag.hpp b/src/modules/uavcan/sensors/mag.hpp new file mode 100644 index 0000000000..7f23a0b8f9 --- /dev/null +++ b/src/modules/uavcan/sensors/mag.hpp @@ -0,0 +1,72 @@ +/**************************************************************************** + * + * 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 + */ + +#pragma once + +#include "sensor_bridge.hpp" +#include +#include + +#include + +class UavcanMagnetometerBridge : public IUavcanSensorBridge, public device::CDev +{ +public: + UavcanMagnetometerBridge(uavcan::INode& node); + ~UavcanMagnetometerBridge() override; + + const char *get_name() const override; + + int init() override; + +private: + int ioctl(struct file *filp, int cmd, unsigned long arg) override; + + void mag_sub_cb(const uavcan::ReceivedDataStructure &msg); + + typedef uavcan::MethodBinder&)> + MagCbBinder; + + + uavcan::Subscriber _sub_mag; + mag_scale _scale = {}; + orb_id_t _orb_id = nullptr; + orb_advert_t _orb_advert = -1; + int _class_instance = -1; +}; diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp index ba27d6c71c..08fca73c53 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.cpp +++ b/src/modules/uavcan/sensors/sensor_bridge.cpp @@ -37,11 +37,14 @@ #include "sensor_bridge.hpp" #include "gnss.hpp" +#include "mag.hpp" IUavcanSensorBridge* IUavcanSensorBridge::make(uavcan::INode &node, const char *bridge_name) { if (!std::strncmp("gnss", bridge_name, MaxNameLen)) { return new UavcanGnssBridge(node); + } else if (!std::strncmp("mag", bridge_name, MaxNameLen)) { + return new UavcanMagnetometerBridge(node); } else { return nullptr; } From e32ff6004be5d7bbd4b94b437b6deaa770618259 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 22 Aug 2014 15:31:08 +0400 Subject: [PATCH 06/34] UAVCAN mag driver fix --- src/modules/uavcan/sensors/mag.cpp | 30 ++++++++++++++---------------- 1 file changed, 14 insertions(+), 16 deletions(-) diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp index 41a1a5270e..ac43ea1e3b 100644 --- a/src/modules/uavcan/sensors/mag.cpp +++ b/src/modules/uavcan/sensors/mag.cpp @@ -93,33 +93,31 @@ int UavcanMagnetometerBridge::init() int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { - case MAGIOCSSAMPLERATE: - case MAGIOCGSAMPLERATE: - case MAGIOCSRANGE: - case MAGIOCGRANGE: - case MAGIOCSLOWPASS: - case MAGIOCGLOWPASS: { - return -EINVAL; - } case MAGIOCSSCALE: { std::memcpy(&_scale, reinterpret_cast(arg), sizeof(_scale)); - log("new scale/offset: x: %f/%f y: %f/%f z: %f/%f", - double(_scale.x_scale), double(_scale.x_offset), - double(_scale.y_scale), double(_scale.y_offset), - double(_scale.z_scale), double(_scale.z_offset)); return 0; } case MAGIOCGSCALE: { std::memcpy(reinterpret_cast(arg), &_scale, sizeof(_scale)); return 0; } - case MAGIOCCALIBRATE: - case MAGIOCEXSTRAP: case MAGIOCSELFTEST: { - return -EINVAL; + return 0; // Nothing to do } case MAGIOCGEXTERNAL: { - return 1; + return 0; // We don't want anyone to transform the coordinate frame, so we declare it onboard + } + case MAGIOCSSAMPLERATE: { + return 0; // Pretend that this stuff is supported to keep the sensor app happy + } + case MAGIOCCALIBRATE: + case MAGIOCGSAMPLERATE: + case MAGIOCSRANGE: + case MAGIOCGRANGE: + case MAGIOCSLOWPASS: + case MAGIOCEXSTRAP: + case MAGIOCGLOWPASS: { + return -EINVAL; } default: { return CDev::ioctl(filp, cmd, arg); From bdc2ecd9f6d0ae3e66feb8a8e94391b606ee451e Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 22 Aug 2014 15:41:21 +0400 Subject: [PATCH 07/34] Too much Ctrl+C Ctrl+V --- src/modules/uavcan/sensors/mag.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp index ac43ea1e3b..6be9e9bac8 100644 --- a/src/modules/uavcan/sensors/mag.cpp +++ b/src/modules/uavcan/sensors/mag.cpp @@ -137,8 +137,8 @@ void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure= 0) { orb_publish(_orb_id, _orb_advert, &report); From 6ebd59c633db0d610f63eeb06c5c867da34740e0 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 22 Aug 2014 15:52:35 +0400 Subject: [PATCH 08/34] UAVCAN: improved sensor bridge factory --- src/modules/uavcan/sensors/gnss.cpp | 4 ++-- src/modules/uavcan/sensors/gnss.hpp | 4 +++- src/modules/uavcan/sensors/mag.cpp | 4 ++-- src/modules/uavcan/sensors/mag.hpp | 7 ++++--- src/modules/uavcan/sensors/sensor_bridge.cpp | 4 ++-- src/modules/uavcan/sensors/sensor_bridge.hpp | 2 +- src/modules/uavcan/uavcan_main.cpp | 4 ++-- 7 files changed, 16 insertions(+), 13 deletions(-) diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp index fb89b8365f..6b69d163f3 100644 --- a/src/modules/uavcan/sensors/gnss.cpp +++ b/src/modules/uavcan/sensors/gnss.cpp @@ -45,6 +45,8 @@ #define MM_PER_CM 10 // Millimeters per centimeter +const char *const UavcanGnssBridge::NAME = "gnss"; + UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) : _node(node), _sub_fix(node), @@ -52,8 +54,6 @@ _report_pub(-1) { } -const char *UavcanGnssBridge::get_name() const { return "gnss"; } - int UavcanGnssBridge::init() { int res = -1; diff --git a/src/modules/uavcan/sensors/gnss.hpp b/src/modules/uavcan/sensors/gnss.hpp index 6bdae8de34..db3a515fa7 100644 --- a/src/modules/uavcan/sensors/gnss.hpp +++ b/src/modules/uavcan/sensors/gnss.hpp @@ -56,9 +56,11 @@ class UavcanGnssBridge : public IUavcanSensorBridge { public: + static const char *const NAME; + UavcanGnssBridge(uavcan::INode& node); - const char *get_name() const override; + const char *get_name() const override { return NAME; } int init() override; diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp index 6be9e9bac8..4f8a5e1046 100644 --- a/src/modules/uavcan/sensors/mag.cpp +++ b/src/modules/uavcan/sensors/mag.cpp @@ -37,6 +37,8 @@ #include "mag.hpp" +const char *const UavcanMagnetometerBridge::NAME = "mag"; + UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode& node) : device::CDev("uavcan_mag", "/dev/uavcan/mag"), _sub_mag(node) @@ -53,8 +55,6 @@ UavcanMagnetometerBridge::~UavcanMagnetometerBridge() } } -const char *UavcanMagnetometerBridge::get_name() const { return "mag"; } - int UavcanMagnetometerBridge::init() { // Init the libuavcan subscription diff --git a/src/modules/uavcan/sensors/mag.hpp b/src/modules/uavcan/sensors/mag.hpp index 7f23a0b8f9..4bc5129a2f 100644 --- a/src/modules/uavcan/sensors/mag.hpp +++ b/src/modules/uavcan/sensors/mag.hpp @@ -46,10 +46,12 @@ class UavcanMagnetometerBridge : public IUavcanSensorBridge, public device::CDev { public: + static const char *const NAME; + UavcanMagnetometerBridge(uavcan::INode& node); ~UavcanMagnetometerBridge() override; - const char *get_name() const override; + const char *get_name() const override { return NAME; } int init() override; @@ -63,8 +65,7 @@ private: (const uavcan::ReceivedDataStructure&)> MagCbBinder; - - uavcan::Subscriber _sub_mag; + uavcan::Subscriber _sub_mag; mag_scale _scale = {}; orb_id_t _orb_id = nullptr; orb_advert_t _orb_advert = -1; diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp index 08fca73c53..2bd662d5ce 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.cpp +++ b/src/modules/uavcan/sensors/sensor_bridge.cpp @@ -41,9 +41,9 @@ IUavcanSensorBridge* IUavcanSensorBridge::make(uavcan::INode &node, const char *bridge_name) { - if (!std::strncmp("gnss", bridge_name, MaxNameLen)) { + if (!std::strncmp(UavcanGnssBridge::NAME, bridge_name, MAX_NAME_LEN)) { return new UavcanGnssBridge(node); - } else if (!std::strncmp("mag", bridge_name, MaxNameLen)) { + } else if (!std::strncmp(UavcanMagnetometerBridge::NAME, bridge_name, MAX_NAME_LEN)) { return new UavcanMagnetometerBridge(node); } else { return nullptr; diff --git a/src/modules/uavcan/sensors/sensor_bridge.hpp b/src/modules/uavcan/sensors/sensor_bridge.hpp index 1667c2b57d..7bd8118133 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.hpp +++ b/src/modules/uavcan/sensors/sensor_bridge.hpp @@ -46,7 +46,7 @@ class IUavcanSensorBridge : uavcan::Noncopyable, public ListNode { public: - static constexpr unsigned MaxNameLen = 20; + static constexpr unsigned MAX_NAME_LEN = 20; virtual ~IUavcanSensorBridge() { } diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 71302d9282..aca4587fff 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -535,7 +535,7 @@ int UavcanNode::sensor_enable(const char *bridge_name) { auto pos = _sensor_bridges.getHead(); while (pos != nullptr) { - if (!std::strncmp(pos->get_name(), bridge_name, IUavcanSensorBridge::MaxNameLen)) { + if (!std::strncmp(pos->get_name(), bridge_name, IUavcanSensorBridge::MAX_NAME_LEN)) { warnx("sensor bridge '%s' already exists", bridge_name); retval = -1; goto leave; @@ -553,7 +553,7 @@ int UavcanNode::sensor_enable(const char *bridge_name) goto leave; } - assert(!std::strncmp(bridge->get_name(), bridge_name, IUavcanSensorBridge::MaxNameLen)); + assert(!std::strncmp(bridge->get_name(), bridge_name, IUavcanSensorBridge::MAX_NAME_LEN)); retval = bridge->init(); if (retval >= 0) { From 2a6ab537b2f8687fdb125d9e5d46338ff85220ad Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 22 Aug 2014 20:04:31 +0400 Subject: [PATCH 09/34] UAVCAN update --- uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/uavcan b/uavcan index 446577e4fb..6c070852d7 160000 --- a/uavcan +++ b/uavcan @@ -1 +1 @@ -Subproject commit 446577e4fb979ee4c629081368233eaa5683d086 +Subproject commit 6c070852d76b18c2d57612f66b5bd00e63118c94 From 6870cd4d3d68941945d303b707c4b05bd5d1e6e4 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 22 Aug 2014 20:15:04 +0400 Subject: [PATCH 10/34] UAVCAN baro driver --- src/modules/uavcan/module.mk | 3 +- src/modules/uavcan/sensors/baro.cpp | 147 +++++++++++++++++++ src/modules/uavcan/sensors/baro.hpp | 73 +++++++++ src/modules/uavcan/sensors/sensor_bridge.cpp | 3 + 4 files changed, 225 insertions(+), 1 deletion(-) create mode 100644 src/modules/uavcan/sensors/baro.cpp create mode 100644 src/modules/uavcan/sensors/baro.hpp diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index de411e1e24..26ff7102db 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -50,7 +50,8 @@ SRCS += actuators/esc.cpp # Sensors SRCS += sensors/sensor_bridge.cpp \ sensors/gnss.cpp \ - sensors/mag.cpp + sensors/mag.cpp \ + sensors/baro.cpp # # libuavcan diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp new file mode 100644 index 0000000000..3afcc3f1cc --- /dev/null +++ b/src/modules/uavcan/sensors/baro.cpp @@ -0,0 +1,147 @@ +/**************************************************************************** + * + * 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 + */ + +#include "baro.hpp" +#include + +const char *const UavcanBarometerBridge::NAME = "baro"; + +UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode& node) : +device::CDev("uavcan_baro", "/dev/uavcan/baro"), +_sub_air_data(node) +{ +} + +UavcanBarometerBridge::~UavcanBarometerBridge() +{ + if (_class_instance > 0) { + (void)unregister_class_devname(BARO_DEVICE_PATH, _class_instance); + } +} + +int UavcanBarometerBridge::init() +{ + // Init the libuavcan subscription + int res = _sub_air_data.start(AirDataCbBinder(this, &UavcanBarometerBridge::air_data_sub_cb)); + if (res < 0) { + log("failed to start uavcan sub: %d", res); + return res; + } + + // Detect our device class + _class_instance = register_class_devname(BARO_DEVICE_PATH); + switch (_class_instance) { + case CLASS_DEVICE_PRIMARY: { + _orb_id = ORB_ID(sensor_baro0); + break; + } + case CLASS_DEVICE_SECONDARY: { + _orb_id = ORB_ID(sensor_baro1); + break; + } + default: { + log("invalid class instance: %d", _class_instance); + (void)unregister_class_devname(BARO_DEVICE_PATH, _class_instance); + return -1; + } + } + + log("inited with class instance %d", _class_instance); + return 0; +} + +int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + case BAROIOCSMSLPRESSURE: { + if ((arg < 80000) || (arg > 120000)) { + return -EINVAL; + } else { + log("new msl pressure %u", _msl_pressure); + _msl_pressure = arg; + return OK; + } + } + case BAROIOCGMSLPRESSURE: { + return _msl_pressure; + } + default: { + return CDev::ioctl(filp, cmd, arg); + } + } +} + +void UavcanBarometerBridge::air_data_sub_cb(const uavcan::ReceivedDataStructure &msg) +{ + auto report = ::baro_report(); + + report.timestamp = msg.getUtcTimestamp().toUSec(); + if (report.timestamp == 0) { + report.timestamp = msg.getMonotonicTimestamp().toUSec(); + } + + report.temperature = msg.static_temperature; + report.pressure = msg.static_pressure / 100.0F; // Convert to millibar + + /* + * Altitude computation + * Refer to the MS5611 driver for details + */ + const double T1 = 15.0 + 273.15; // temperature at base height in Kelvin + const double a = -6.5 / 1000; // temperature gradient in degrees per metre + const double g = 9.80665; // gravity constant in m/s/s + const double R = 287.05; // ideal gas constant in J/kg/K + + const double p1 = _msl_pressure / 1000.0; // current pressure at MSL in kPa + const double p = double(msg.static_pressure) / 1000.0; // measured pressure in kPa + + report.altitude = (((std::pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; + + /* + * Publish + */ + if (_orb_advert >= 0) { + orb_publish(_orb_id, _orb_advert, &report); + } else { + _orb_advert = orb_advertise(_orb_id, &report); + if (_orb_advert < 0) { + log("ADVERT FAIL"); + } else { + log("advertised"); + } + } +} diff --git a/src/modules/uavcan/sensors/baro.hpp b/src/modules/uavcan/sensors/baro.hpp new file mode 100644 index 0000000000..f6aa01216a --- /dev/null +++ b/src/modules/uavcan/sensors/baro.hpp @@ -0,0 +1,73 @@ +/**************************************************************************** + * + * 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 + */ + +#pragma once + +#include "sensor_bridge.hpp" +#include +#include + +#include + +class UavcanBarometerBridge : public IUavcanSensorBridge, public device::CDev +{ +public: + static const char *const NAME; + + UavcanBarometerBridge(uavcan::INode& node); + ~UavcanBarometerBridge() override; + + const char *get_name() const override { return NAME; } + + int init() override; + +private: + int ioctl(struct file *filp, int cmd, unsigned long arg) override; + + void air_data_sub_cb(const uavcan::ReceivedDataStructure &msg); + + typedef uavcan::MethodBinder&)> + AirDataCbBinder; + + uavcan::Subscriber _sub_air_data; + unsigned _msl_pressure = 101325; + orb_id_t _orb_id = nullptr; + orb_advert_t _orb_advert = -1; + int _class_instance = -1; +}; diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp index 2bd662d5ce..f826c8fd24 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.cpp +++ b/src/modules/uavcan/sensors/sensor_bridge.cpp @@ -38,6 +38,7 @@ #include "sensor_bridge.hpp" #include "gnss.hpp" #include "mag.hpp" +#include "baro.hpp" IUavcanSensorBridge* IUavcanSensorBridge::make(uavcan::INode &node, const char *bridge_name) { @@ -45,6 +46,8 @@ IUavcanSensorBridge* IUavcanSensorBridge::make(uavcan::INode &node, const char * 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; } From 7132141cc478e1b9cdde41207c03f2c622f7831a Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 22 Aug 2014 20:33:35 +0400 Subject: [PATCH 11/34] UAVCAN: Printing all known sensor bridge names with usage info --- src/modules/uavcan/sensors/sensor_bridge.cpp | 12 ++++++++++++ src/modules/uavcan/sensors/sensor_bridge.hpp | 5 +++++ src/modules/uavcan/uavcan_main.cpp | 3 +++ 3 files changed, 20 insertions(+) diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp index f826c8fd24..5752d5524d 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.cpp +++ b/src/modules/uavcan/sensors/sensor_bridge.cpp @@ -36,12 +36,17 @@ */ #include "sensor_bridge.hpp" +#include + #include "gnss.hpp" #include "mag.hpp" #include "baro.hpp" IUavcanSensorBridge* IUavcanSensorBridge::make(uavcan::INode &node, const char *bridge_name) { + /* + * 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)) { @@ -52,3 +57,10 @@ IUavcanSensorBridge* IUavcanSensorBridge::make(uavcan::INode &node, const char * 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); +} diff --git a/src/modules/uavcan/sensors/sensor_bridge.hpp b/src/modules/uavcan/sensors/sensor_bridge.hpp index 7bd8118133..976d9a03d4 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.hpp +++ b/src/modules/uavcan/sensors/sensor_bridge.hpp @@ -67,4 +67,9 @@ public: * @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); }; diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index aca4587fff..8607af1456 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -590,6 +590,9 @@ static void print_usage() "\tuavcan start [can_bitrate]\n" "\tuavcan sensor enable \n" "\tuavcan sensor list"); + + warnx("known sensor bridges:"); + IUavcanSensorBridge::print_known_names("\t"); } extern "C" __EXPORT int uavcan_main(int argc, char *argv[]); From 6a8971e28f492073a951d96065df30034853bea7 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 23 Aug 2014 17:23:59 +0400 Subject: [PATCH 12/34] New UAVCAN initialization logic --- ROMFS/px4fmu_common/init.d/rc.uavcan | 16 ++++++ ROMFS/px4fmu_common/init.d/rcS | 12 +++-- src/modules/uavcan/module.mk | 3 +- src/modules/uavcan/uavcan_main.cpp | 50 ++++++------------- src/modules/uavcan/uavcan_params.c | 73 ++++++++++++++++++++++++++++ 5 files changed, 114 insertions(+), 40 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/rc.uavcan create mode 100644 src/modules/uavcan/uavcan_params.c diff --git a/ROMFS/px4fmu_common/init.d/rc.uavcan b/ROMFS/px4fmu_common/init.d/rc.uavcan new file mode 100644 index 0000000000..abb76b4001 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.uavcan @@ -0,0 +1,16 @@ +#!nsh +# +# UAVCAN initialization script. +# + +if param compare UAVCAN_ENABLE 1 +then + if uavcan start + then + sleep 1 # Sensor autodetection delay + echo "[init] UAVCAN started" + else + echo "[init] ERROR: Could not start UAVCAN" + tone_alarm $TUNE_OUT_ERROR + fi +fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index c9e6a27cac..1957719057 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -304,11 +304,10 @@ then then if [ $OUTPUT_MODE == uavcan_esc ] then - if uavcan start 1 + if param compare UAVCAN_ENABLE 0 then - echo "CAN UP" - else - echo "CAN ERR" + echo "[init] OVERRIDING UAVCAN_ENABLE = 1" + param set UAVCAN_ENABLE 1 fi fi @@ -447,6 +446,11 @@ then mavlink start $MAVLINK_FLAGS + # + # UAVCAN + # + sh /etc/init.d/rc.uavcan + # # Sensors, Logging, GPS # diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index 26ff7102db..93a1bf96fd 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -42,7 +42,8 @@ MAXOPTIMIZATION = -Os # Main SRCS += uavcan_main.cpp \ - uavcan_clock.cpp + uavcan_clock.cpp \ + uavcan_params.c # Actuators SRCS += actuators/esc.cpp diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 8607af1456..a15f836964 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -38,6 +38,7 @@ #include #include #include +#include #include #include #include @@ -587,7 +588,7 @@ void UavcanNode::sensor_print_enabled() static void print_usage() { warnx("usage: \n" - "\tuavcan start [can_bitrate]\n" + "\tuavcan start\n" "\tuavcan sensor enable \n" "\tuavcan sensor list"); @@ -599,52 +600,32 @@ extern "C" __EXPORT int uavcan_main(int argc, char *argv[]); int uavcan_main(int argc, char *argv[]) { - constexpr unsigned DEFAULT_CAN_BITRATE = 1000000; - if (argc < 2) { print_usage(); ::exit(1); } if (!std::strcmp(argv[1], "start")) { - if (argc < 3) { - print_usage(); - ::exit(1); + if (UavcanNode::instance()) { + errx(1, "already started"); } - /* - * Node ID - */ - const int node_id = atoi(argv[2]); + // Node ID + int32_t node_id = 0; + (void)param_get(param_find("UAVCAN_NODE_ID"), &node_id); if (node_id < 0 || node_id > uavcan::NodeID::Max || !uavcan::NodeID(node_id).isUnicast()) { warnx("Invalid Node ID %i", node_id); ::exit(1); } - /* - * CAN bitrate - */ - unsigned bitrate = 0; + // CAN bitrate + int32_t bitrate = 0; + (void)param_get(param_find("UAVCAN_BITRATE"), &bitrate); - if (argc > 3) { - bitrate = atol(argv[3]); - } - - if (bitrate <= 0) { - bitrate = DEFAULT_CAN_BITRATE; - } - - if (UavcanNode::instance()) { - errx(1, "already started"); - } - - /* - * Start - */ + // Start warnx("Node ID %u, bitrate %u", node_id, bitrate); return UavcanNode::start(node_id, bitrate); - } /* commands below require the app to be started */ @@ -655,14 +636,13 @@ int uavcan_main(int argc, char *argv[]) } if (!std::strcmp(argv[1], "status") || !std::strcmp(argv[1], "info")) { - - inst->print_info(); - ::exit(0); + inst->print_info(); + ::exit(0); } if (!std::strcmp(argv[1], "stop")) { - delete inst; - ::exit(0); + delete inst; + ::exit(0); } if (!std::strcmp(argv[1], "sensor")) { diff --git a/src/modules/uavcan/uavcan_params.c b/src/modules/uavcan/uavcan_params.c new file mode 100644 index 0000000000..e6ea8a8fb7 --- /dev/null +++ b/src/modules/uavcan/uavcan_params.c @@ -0,0 +1,73 @@ +/**************************************************************************** + * + * 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 + */ + +#include +#include + +/** + * Enable UAVCAN. + * + * Enables support for UAVCAN-interfaced actuators and sensors. + * + * @min 0 + * @max 1 + * @group UAVCAN + */ +PARAM_DEFINE_INT32(UAVCAN_ENABLE, 0); + +/** + * UAVCAN Node ID. + * + * Read the specs at http://uavcan.org to learn more about Node ID. + * + * @min 1 + * @max 125 + * @group UAVCAN + */ +PARAM_DEFINE_INT32(UAVCAN_NODE_ID, 1); + +/** + * UAVCAN CAN bus bitrate. + * + * @min 20000 + * @max 1000000 + * @group UAVCAN + */ +PARAM_DEFINE_INT32(UAVCAN_BITRATE, 1000000); + + + From 4e0d7c6b0e52d3eecba65f4415d4c7372dfd8a49 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 23 Aug 2014 23:14:59 +0400 Subject: [PATCH 13/34] UAVCAN: redundant sensors support --- src/modules/uavcan/module.mk | 6 +- src/modules/uavcan/sensors/baro.cpp | 49 ++--------- src/modules/uavcan/sensors/baro.hpp | 7 +- src/modules/uavcan/sensors/gnss.cpp | 15 ++++ src/modules/uavcan/sensors/gnss.hpp | 18 ++-- src/modules/uavcan/sensors/mag.cpp | 51 ++--------- src/modules/uavcan/sensors/mag.hpp | 7 +- src/modules/uavcan/sensors/sensor_bridge.cpp | 93 +++++++++++++++++++- src/modules/uavcan/sensors/sensor_bridge.hpp | 54 ++++++++++++ 9 files changed, 188 insertions(+), 112 deletions(-) diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index 93a1bf96fd..f92bc754f0 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -49,9 +49,9 @@ SRCS += uavcan_main.cpp \ SRCS += actuators/esc.cpp # Sensors -SRCS += sensors/sensor_bridge.cpp \ - sensors/gnss.cpp \ - sensors/mag.cpp \ +SRCS += sensors/sensor_bridge.cpp \ + sensors/gnss.cpp \ + sensors/mag.cpp \ sensors/baro.cpp # diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp index 3afcc3f1cc..ef4f0dbbab 100644 --- a/src/modules/uavcan/sensors/baro.cpp +++ b/src/modules/uavcan/sensors/baro.cpp @@ -38,49 +38,26 @@ #include "baro.hpp" #include +static const orb_id_t BARO_TOPICS[2] = { + ORB_ID(sensor_baro0), + ORB_ID(sensor_baro1) +}; + const char *const UavcanBarometerBridge::NAME = "baro"; UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode& node) : -device::CDev("uavcan_baro", "/dev/uavcan/baro"), +UavcanCDevSensorBridgeBase("uavcan_baro", "/dev/uavcan/baro", BARO_DEVICE_PATH, BARO_TOPICS), _sub_air_data(node) { } -UavcanBarometerBridge::~UavcanBarometerBridge() -{ - if (_class_instance > 0) { - (void)unregister_class_devname(BARO_DEVICE_PATH, _class_instance); - } -} - int UavcanBarometerBridge::init() { - // Init the libuavcan subscription int res = _sub_air_data.start(AirDataCbBinder(this, &UavcanBarometerBridge::air_data_sub_cb)); if (res < 0) { log("failed to start uavcan sub: %d", res); return res; } - - // Detect our device class - _class_instance = register_class_devname(BARO_DEVICE_PATH); - switch (_class_instance) { - case CLASS_DEVICE_PRIMARY: { - _orb_id = ORB_ID(sensor_baro0); - break; - } - case CLASS_DEVICE_SECONDARY: { - _orb_id = ORB_ID(sensor_baro1); - break; - } - default: { - log("invalid class instance: %d", _class_instance); - (void)unregister_class_devname(BARO_DEVICE_PATH, _class_instance); - return -1; - } - } - - log("inited with class instance %d", _class_instance); return 0; } @@ -131,17 +108,5 @@ void UavcanBarometerBridge::air_data_sub_cb(const uavcan::ReceivedDataStructure< report.altitude = (((std::pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; - /* - * Publish - */ - if (_orb_advert >= 0) { - orb_publish(_orb_id, _orb_advert, &report); - } else { - _orb_advert = orb_advertise(_orb_id, &report); - if (_orb_advert < 0) { - log("ADVERT FAIL"); - } else { - log("advertised"); - } - } + publish(msg.getSrcNodeID().get(), &report); } diff --git a/src/modules/uavcan/sensors/baro.hpp b/src/modules/uavcan/sensors/baro.hpp index f6aa01216a..9d470219ed 100644 --- a/src/modules/uavcan/sensors/baro.hpp +++ b/src/modules/uavcan/sensors/baro.hpp @@ -39,17 +39,15 @@ #include "sensor_bridge.hpp" #include -#include #include -class UavcanBarometerBridge : public IUavcanSensorBridge, public device::CDev +class UavcanBarometerBridge : public UavcanCDevSensorBridgeBase { public: static const char *const NAME; UavcanBarometerBridge(uavcan::INode& node); - ~UavcanBarometerBridge() override; const char *get_name() const override { return NAME; } @@ -67,7 +65,4 @@ private: uavcan::Subscriber _sub_air_data; unsigned _msl_pressure = 101325; - orb_id_t _orb_id = nullptr; - orb_advert_t _orb_advert = -1; - int _class_instance = -1; }; diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp index 6b69d163f3..f2bb280878 100644 --- a/src/modules/uavcan/sensors/gnss.cpp +++ b/src/modules/uavcan/sensors/gnss.cpp @@ -73,8 +73,23 @@ int UavcanGnssBridge::init() return res; } +unsigned UavcanGnssBridge::get_num_redundant_channels() const +{ + return (_receiver_node_id < 0) ? 0 : 1; +} + void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure &msg) { + // This bridge does not support redundant GNSS receivers yet. + 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. + } + } + _report.timestamp_position = hrt_absolute_time(); _report.lat = msg.lat_1e7; _report.lon = msg.lon_1e7; diff --git a/src/modules/uavcan/sensors/gnss.hpp b/src/modules/uavcan/sensors/gnss.hpp index db3a515fa7..9488c5fe52 100644 --- a/src/modules/uavcan/sensors/gnss.hpp +++ b/src/modules/uavcan/sensors/gnss.hpp @@ -64,27 +64,23 @@ public: int init() override; + unsigned get_num_redundant_channels() const override; + private: /** * GNSS fix message will be reported via this callback. */ void gnss_fix_sub_cb(const uavcan::ReceivedDataStructure &msg); - typedef uavcan::MethodBinder&)> FixCbBinder; - /* - * libuavcan related things - */ - uavcan::INode &_node; - uavcan::Subscriber _sub_fix; + uavcan::INode &_node; + uavcan::Subscriber _sub_fix; + int _receiver_node_id = -1; - /* - * uORB - */ - struct vehicle_gps_position_s _report; ///< uORB topic for gnss position - orb_advert_t _report_pub; ///< uORB pub for gnss position + struct vehicle_gps_position_s _report; ///< uORB topic for gnss position + orb_advert_t _report_pub; ///< uORB pub for gnss position }; diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp index 4f8a5e1046..aaa3a44632 100644 --- a/src/modules/uavcan/sensors/mag.cpp +++ b/src/modules/uavcan/sensors/mag.cpp @@ -37,10 +37,16 @@ #include "mag.hpp" +static const orb_id_t MAG_TOPICS[3] = { + ORB_ID(sensor_mag0), + ORB_ID(sensor_mag1), + ORB_ID(sensor_mag2) +}; + const char *const UavcanMagnetometerBridge::NAME = "mag"; UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode& node) : -device::CDev("uavcan_mag", "/dev/uavcan/mag"), +UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_DEVICE_PATH, MAG_TOPICS), _sub_mag(node) { _scale.x_scale = 1.0F; @@ -48,45 +54,13 @@ _sub_mag(node) _scale.z_scale = 1.0F; } -UavcanMagnetometerBridge::~UavcanMagnetometerBridge() -{ - if (_class_instance > 0) { - (void)unregister_class_devname(MAG_DEVICE_PATH, _class_instance); - } -} - int UavcanMagnetometerBridge::init() { - // Init the libuavcan subscription int res = _sub_mag.start(MagCbBinder(this, &UavcanMagnetometerBridge::mag_sub_cb)); if (res < 0) { log("failed to start uavcan sub: %d", res); return res; } - - // Detect our device class - _class_instance = register_class_devname(MAG_DEVICE_PATH); - switch (_class_instance) { - case CLASS_DEVICE_PRIMARY: { - _orb_id = ORB_ID(sensor_mag0); - break; - } - case CLASS_DEVICE_SECONDARY: { - _orb_id = ORB_ID(sensor_mag1); - break; - } - case CLASS_DEVICE_TERTIARY: { - _orb_id = ORB_ID(sensor_mag2); - break; - } - default: { - log("invalid class instance: %d", _class_instance); - (void)unregister_class_devname(MAG_DEVICE_PATH, _class_instance); - return -1; - } - } - - log("inited with class instance %d", _class_instance); return 0; } @@ -140,14 +114,5 @@ void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure= 0) { - orb_publish(_orb_id, _orb_advert, &report); - } else { - _orb_advert = orb_advertise(_orb_id, &report); - if (_orb_advert < 0) { - log("ADVERT FAIL"); - } else { - log("advertised"); - } - } + publish(msg.getSrcNodeID().get(), &report); } diff --git a/src/modules/uavcan/sensors/mag.hpp b/src/modules/uavcan/sensors/mag.hpp index 4bc5129a2f..6d413a8f71 100644 --- a/src/modules/uavcan/sensors/mag.hpp +++ b/src/modules/uavcan/sensors/mag.hpp @@ -38,18 +38,16 @@ #pragma once #include "sensor_bridge.hpp" -#include #include #include -class UavcanMagnetometerBridge : public IUavcanSensorBridge, public device::CDev +class UavcanMagnetometerBridge : public UavcanCDevSensorBridgeBase { public: static const char *const NAME; UavcanMagnetometerBridge(uavcan::INode& node); - ~UavcanMagnetometerBridge() override; const char *get_name() const override { return NAME; } @@ -67,7 +65,4 @@ private: uavcan::Subscriber _sub_mag; mag_scale _scale = {}; - orb_id_t _orb_id = nullptr; - orb_advert_t _orb_advert = -1; - int _class_instance = -1; }; diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp index 5752d5524d..05d589b580 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.cpp +++ b/src/modules/uavcan/sensors/sensor_bridge.cpp @@ -36,12 +36,15 @@ */ #include "sensor_bridge.hpp" -#include +#include #include "gnss.hpp" #include "mag.hpp" #include "baro.hpp" +/* + * IUavcanSensorBridge + */ IUavcanSensorBridge* IUavcanSensorBridge::make(uavcan::INode &node, const char *bridge_name) { /* @@ -64,3 +67,91 @@ void IUavcanSensorBridge::print_known_names(const char *prefix) printf("%s%s\n", prefix, UavcanMagnetometerBridge::NAME); printf("%s%s\n", prefix, UavcanBarometerBridge::NAME); } + +/* + * UavcanCDevSensorBridgeBase + */ +UavcanCDevSensorBridgeBase::~UavcanCDevSensorBridgeBase() +{ + for (unsigned i = 0; i < _max_channels; i++) { + if (_channels[i].redunancy_channel_id >= 0) { + (void)unregister_class_devname(_class_devname, _channels[i].class_instance); + } + } + delete [] _orb_topics; + delete [] _channels; +} + +void UavcanCDevSensorBridgeBase::publish(const int redundancy_channel_id, const void *report) +{ + Channel *channel = nullptr; + + // Checking if such channel already exists + for (unsigned i = 0; i < _max_channels; i++) { + if (_channels[i].redunancy_channel_id == redundancy_channel_id) { + channel = _channels + i; + break; + } + } + + // No such channel - try to create one + if (channel == nullptr) { + if (_out_of_channels) { + return; // Give up immediately - saves some CPU time + } + + log("adding channel %d...", redundancy_channel_id); + + // Search for the first free channel + for (unsigned i = 0; i < _max_channels; i++) { + if (_channels[i].redunancy_channel_id < 0) { + channel = _channels + i; + break; + } + } + + // No free channels left + if (channel == nullptr) { + _out_of_channels = true; + log("out of channels"); + return; + } + + // 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"); + (void)unregister_class_devname(_class_devname, class_instance); + return; + } + + // Publish to the appropriate topic, abort on failure + channel->orb_id = _orb_topics[class_instance]; + channel->redunancy_channel_id = redundancy_channel_id; + channel->class_instance = class_instance; + + channel->orb_advert = orb_advertise(channel->orb_id, report); + if (channel->orb_advert < 0) { + log("ADVERTISE FAILED"); + *channel = Channel(); + return; + } + + log("channel %d class instance %d ok", channel->redunancy_channel_id, channel->class_instance); + } + assert(channel != nullptr); + + (void)orb_publish(channel->orb_id, channel->orb_advert, report); +} + +unsigned UavcanCDevSensorBridgeBase::get_num_redundant_channels() const +{ + unsigned out = 0; + for (unsigned i = 0; i < _max_channels; i++) { + if (_channels[i].redunancy_channel_id >= 0) { + out += 1; + } + } + return out; +} diff --git a/src/modules/uavcan/sensors/sensor_bridge.hpp b/src/modules/uavcan/sensors/sensor_bridge.hpp index 976d9a03d4..d27ccb8a09 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.hpp +++ b/src/modules/uavcan/sensors/sensor_bridge.hpp @@ -39,6 +39,8 @@ #include #include +#include +#include /** * A sensor bridge class must implement this interface. @@ -61,6 +63,11 @@ public: */ virtual int init() = 0; + /** + * Returns number of active redundancy channels. + */ + virtual unsigned get_num_redundant_channels() const = 0; + /** * Sensor bridge factory. * Creates a bridge object by its ASCII name, e.g. "gnss", "mag". @@ -73,3 +80,50 @@ public: */ static void print_known_names(const char *prefix); }; + +/** + * This is the base class for redundant sensors with an independent ORB topic per each redundancy channel. + * For example, sensor_mag0, sensor_mag1, etc. + */ +class UavcanCDevSensorBridgeBase : public IUavcanSensorBridge, public device::CDev +{ + struct Channel + { + int redunancy_channel_id = -1; + orb_id_t orb_id = nullptr; + orb_advert_t orb_advert = -1; + int class_instance = -1; + }; + + const unsigned _max_channels; + const char *const _class_devname; + orb_id_t *const _orb_topics; + Channel *const _channels; + bool _out_of_channels = false; + +protected: + template + UavcanCDevSensorBridgeBase(const char *name, const char *devname, const char *class_devname, + const orb_id_t (&orb_topics)[MaxChannels]) : + device::CDev(name, devname), + _max_channels(MaxChannels), + _class_devname(class_devname), + _orb_topics(new orb_id_t[MaxChannels]), + _channels(new Channel[MaxChannels]) + { + memcpy(_orb_topics, orb_topics, sizeof(orb_id_t) * MaxChannels); + } + + /** + * Sends one measurement into appropriate ORB topic. + * New redundancy channels will be registered automatically. + * @param redundancy_channel_id Redundant sensor identifier (e.g. UAVCAN Node ID) + * @param report ORB message object + */ + void publish(const int redundancy_channel_id, const void *report); + +public: + virtual ~UavcanCDevSensorBridgeBase(); + + unsigned get_num_redundant_channels() const override; +}; From ce73be514e0add0356c120c19e43f6818af236ad Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 23 Aug 2014 23:30:49 +0400 Subject: [PATCH 14/34] UAVCAN: Proper CDev initialization from sensor bridges --- src/modules/uavcan/sensors/baro.cpp | 7 ++++++- src/modules/uavcan/sensors/mag.cpp | 7 ++++++- 2 files changed, 12 insertions(+), 2 deletions(-) diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp index ef4f0dbbab..80c5e3828e 100644 --- a/src/modules/uavcan/sensors/baro.cpp +++ b/src/modules/uavcan/sensors/baro.cpp @@ -53,7 +53,12 @@ _sub_air_data(node) int UavcanBarometerBridge::init() { - int res = _sub_air_data.start(AirDataCbBinder(this, &UavcanBarometerBridge::air_data_sub_cb)); + int res = device::CDev::init(); + if (res < 0) { + return res; + } + + res = _sub_air_data.start(AirDataCbBinder(this, &UavcanBarometerBridge::air_data_sub_cb)); if (res < 0) { log("failed to start uavcan sub: %d", res); return res; diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp index aaa3a44632..8e6a9a22f0 100644 --- a/src/modules/uavcan/sensors/mag.cpp +++ b/src/modules/uavcan/sensors/mag.cpp @@ -56,7 +56,12 @@ _sub_mag(node) int UavcanMagnetometerBridge::init() { - int res = _sub_mag.start(MagCbBinder(this, &UavcanMagnetometerBridge::mag_sub_cb)); + 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; From 0f124963d4f0c07afa94d96b779a0d28b0fbd66f Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 23 Aug 2014 23:43:01 +0400 Subject: [PATCH 15/34] UAVCAN: Minor improvement of the GNSS bridge --- src/modules/uavcan/sensors/gnss.cpp | 57 ++++++++++++++--------------- src/modules/uavcan/sensors/gnss.hpp | 1 - 2 files changed, 28 insertions(+), 30 deletions(-) diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp index f2bb280878..b3a9cb99b3 100644 --- a/src/modules/uavcan/sensors/gnss.cpp +++ b/src/modules/uavcan/sensors/gnss.cpp @@ -66,9 +66,6 @@ int UavcanGnssBridge::init() return res; } - // Clear the uORB GPS report - memset(&_report, 0, sizeof(_report)); - warnx("gnss sensor bridge init ok"); return res; } @@ -90,12 +87,14 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure 0) ? sqrtf(horizontal_pos_variance) : -1.0F; + report.eph = (horizontal_pos_variance > 0) ? sqrtf(horizontal_pos_variance) : -1.0F; // Vertical position uncertainty - _report.epv = (pos_cov[8] > 0) ? sqrtf(pos_cov[8]) : -1.0F; + report.epv = (pos_cov[8] > 0) ? sqrtf(pos_cov[8]) : -1.0F; } else { - _report.eph = -1.0F; - _report.epv = -1.0F; + report.eph = -1.0F; + report.epv = -1.0F; } if (valid_velocity_covariance) { 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]); + 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. * Use Jacobian to transform velocity covariance to heading covariance @@ -136,36 +135,36 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure 0) { - orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); + orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &report); } else { - _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); + _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &report); } } diff --git a/src/modules/uavcan/sensors/gnss.hpp b/src/modules/uavcan/sensors/gnss.hpp index 9488c5fe52..c2b6e41956 100644 --- a/src/modules/uavcan/sensors/gnss.hpp +++ b/src/modules/uavcan/sensors/gnss.hpp @@ -80,7 +80,6 @@ private: uavcan::Subscriber _sub_fix; int _receiver_node_id = -1; - struct vehicle_gps_position_s _report; ///< uORB topic for gnss position orb_advert_t _report_pub; ///< uORB pub for gnss position }; From e9da8303161a1e1f012b7c5d770249c3d606fcbf Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 Aug 2014 00:06:47 +0400 Subject: [PATCH 16/34] UAVCAN: initializing all bridges by default --- src/modules/uavcan/sensors/gnss.cpp | 7 +- src/modules/uavcan/sensors/sensor_bridge.cpp | 24 +--- src/modules/uavcan/sensors/sensor_bridge.hpp | 7 +- src/modules/uavcan/uavcan_main.cpp | 111 ++++--------------- src/modules/uavcan/uavcan_main.hpp | 6 +- 5 files changed, 29 insertions(+), 126 deletions(-) diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp index b3a9cb99b3..8548660fe1 100644 --- a/src/modules/uavcan/sensors/gnss.cpp +++ b/src/modules/uavcan/sensors/gnss.cpp @@ -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; } diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp index 05d589b580..a69514d77e 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.cpp +++ b/src/modules/uavcan/sensors/sensor_bridge.cpp @@ -45,27 +45,11 @@ /* * IUavcanSensorBridge */ -IUavcanSensorBridge* IUavcanSensorBridge::make(uavcan::INode &node, const char *bridge_name) +void IUavcanSensorBridge::make_all(uavcan::INode &node, List &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)); } /* diff --git a/src/modules/uavcan/sensors/sensor_bridge.hpp b/src/modules/uavcan/sensors/sensor_bridge.hpp index d27ccb8a09..a13d0ab35d 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.hpp +++ b/src/modules/uavcan/sensors/sensor_bridge.hpp @@ -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 &list); }; /** diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index a15f836964..19b54b9cf3 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -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 \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); } diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index bca1aa530d..be7db97411 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -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 _sensor_bridges; ///< Append-only list of active sensor bridges + List _sensor_bridges; ///< List of active sensor bridges MixerGroup *_mixers = nullptr; From 701bd803ce2b7b212b53704453ee9a493d473d2d Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 Aug 2014 00:20:57 +0400 Subject: [PATCH 17/34] UAVCAN status reporting and proper termination --- src/modules/uavcan/uavcan_main.cpp | 21 +++++++++++++++++++-- 1 file changed, 19 insertions(+), 2 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 19b54b9cf3..482fec1e0a 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -106,6 +106,14 @@ UavcanNode::~UavcanNode() ::close(_armed_sub); + // Removing the sensor bridges + auto br = _sensor_bridges.getHead(); + while (br != nullptr) { + auto next = br->getSibling(); + delete br; + br = next; + } + _instance = nullptr; } @@ -539,8 +547,17 @@ UavcanNode::print_info() (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"); + // ESC mixer status + warnx("ESC actuators control groups: sub: %u / req: %u / fds: %u", + (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num); + warnx("ESC mixer: %s", (_mixers == nullptr) ? "NONE" : "OK"); + + // Sensor bridges + auto br = _sensor_bridges.getHead(); + while (br != nullptr) { + warnx("Sensor '%s': channels: %u", br->get_name(), br->get_num_redundant_channels()); + br = br->getSibling(); + } (void)pthread_mutex_unlock(&_node_mutex); } From 1fa49aaea98c18a00ec5bc6e227b46ac19fe66a1 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 Aug 2014 01:41:54 +0400 Subject: [PATCH 18/34] UAVCAN: clarification --- ROMFS/px4fmu_common/init.d/rc.uavcan | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.uavcan b/ROMFS/px4fmu_common/init.d/rc.uavcan index abb76b4001..9a470a6b8d 100644 --- a/ROMFS/px4fmu_common/init.d/rc.uavcan +++ b/ROMFS/px4fmu_common/init.d/rc.uavcan @@ -7,7 +7,9 @@ if param compare UAVCAN_ENABLE 1 then if uavcan start then - sleep 1 # Sensor autodetection delay + # First sensor publisher to initialize takes lowest instance ID + # This delay ensures that UAVCAN-interfaced sensors would be allocated on lowest instance IDs + sleep 1 echo "[init] UAVCAN started" else echo "[init] ERROR: Could not start UAVCAN" From 3866b5a5fe1c6bc9d8b56ef2d603b8c88f1a295d Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 Aug 2014 03:02:52 +0400 Subject: [PATCH 19/34] Resource leak fix --- src/modules/uavcan/sensors/sensor_bridge.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp index a69514d77e..a98596f9c5 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.cpp +++ b/src/modules/uavcan/sensors/sensor_bridge.cpp @@ -118,6 +118,7 @@ void UavcanCDevSensorBridgeBase::publish(const int redundancy_channel_id, const channel->orb_advert = orb_advertise(channel->orb_id, report); if (channel->orb_advert < 0) { log("ADVERTISE FAILED"); + (void)unregister_class_devname(_class_devname, class_instance); *channel = Channel(); return; } From 2418969b8705a859e49480c338770fc476cd0c24 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 Aug 2014 15:35:02 +0400 Subject: [PATCH 20/34] UAVCAN update --- uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/uavcan b/uavcan index 6c070852d7..5bfa1999f4 160000 --- a/uavcan +++ b/uavcan @@ -1 +1 @@ -Subproject commit 6c070852d76b18c2d57612f66b5bd00e63118c94 +Subproject commit 5bfa1999f41e4983a947efa0029efd2de6873beb From 1a582e8be0993dadb3b1a70d8ccab8ed3ededfce Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 Aug 2014 16:36:35 +0400 Subject: [PATCH 21/34] UAVCAN update for #1306 --- uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/uavcan b/uavcan index 5bfa1999f4..75153eb643 160000 --- a/uavcan +++ b/uavcan @@ -1 +1 @@ -Subproject commit 5bfa1999f41e4983a947efa0029efd2de6873beb +Subproject commit 75153eb6436d0cc00679056ff8e916c6d99057ad From 04ccf5d8c955d1074bede184eeb35ebed897b55a Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 Aug 2014 19:44:54 +0400 Subject: [PATCH 22/34] UAVCAN update --- uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/uavcan b/uavcan index 75153eb643..c7872def16 160000 --- a/uavcan +++ b/uavcan @@ -1 +1 @@ -Subproject commit 75153eb6436d0cc00679056ff8e916c6d99057ad +Subproject commit c7872def16e82a8b318d571829fe9622e2d35ff0 From 2f5c0cbd133deb492102ea8515563f471531acce Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 24 Aug 2014 23:05:28 +0200 Subject: [PATCH 23/34] Deal with zero airspeed measurements --- .../fw_att_control/fw_att_control_main.cpp | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 0cea13cc4d..ad873203eb 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -145,6 +145,7 @@ private: perf_counter_t _nonfinite_output_perf; /**< performance counter for non finite output */ bool _setpoint_valid; /**< flag if the position control setpoint is valid */ + bool _debug; /**< if set to true, print debug output */ struct { float tconst; @@ -324,7 +325,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control nonfinite input")), _nonfinite_output_perf(perf_alloc(PC_COUNT, "fw att control nonfinite output")), /* states */ - _setpoint_valid(false) + _setpoint_valid(false), + _debug(false) { /* safely initialize structs */ _att = {}; @@ -700,7 +702,8 @@ FixedwingAttitudeControl::task_main() perf_count(_nonfinite_input_perf); } } else { - airspeed = _airspeed.true_airspeed_m_s; + /* prevent numerical drama by requiring 0.5 m/s minimal speed */ + airspeed = math::max(0.5f, _airspeed.true_airspeed_m_s); } /* @@ -785,7 +788,7 @@ FixedwingAttitudeControl::task_main() speed_body_v = _att.R[0][1] * _global_pos.vel_n + _att.R[1][1] * _global_pos.vel_e + _att.R[2][1] * _global_pos.vel_d; speed_body_w = _att.R[0][2] * _global_pos.vel_n + _att.R[1][2] * _global_pos.vel_e + _att.R[2][2] * _global_pos.vel_d; } else { - if (loop_counter % 10 == 0) { + if (_debug && loop_counter % 10 == 0) { warnx("Did not get a valid R\n"); } } @@ -808,7 +811,7 @@ FixedwingAttitudeControl::task_main() _roll_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); - if (loop_counter % 10 == 0) { + if (_debug && loop_counter % 10 == 0) { warnx("roll_u %.4f", (double)roll_u); } } @@ -821,7 +824,7 @@ FixedwingAttitudeControl::task_main() if (!isfinite(pitch_u)) { _pitch_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); - if (loop_counter % 10 == 0) { + if (_debug && loop_counter % 10 == 0) { warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f," " airspeed %.4f, airspeed_scaling %.4f," " roll_sp %.4f, pitch_sp %.4f," @@ -845,7 +848,7 @@ FixedwingAttitudeControl::task_main() if (!isfinite(yaw_u)) { _yaw_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); - if (loop_counter % 10 == 0) { + if (_debug && loop_counter % 10 == 0) { warnx("yaw_u %.4f", (double)yaw_u); } } @@ -853,13 +856,13 @@ FixedwingAttitudeControl::task_main() /* throttle passed through */ _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; if (!isfinite(throttle_sp)) { - if (loop_counter % 10 == 0) { + if (_debug && loop_counter % 10 == 0) { warnx("throttle_sp %.4f", (double)throttle_sp); } } } else { perf_count(_nonfinite_input_perf); - if (loop_counter % 10 == 0) { + if (_debug && loop_counter % 10 == 0) { warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp); } } From b150c3092c2bfe532618678b91c1eab42095c486 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 25 Aug 2014 09:33:39 +0200 Subject: [PATCH 24/34] mavlink_main: raise rates of onboard mode --- src/modules/mavlink/mavlink_main.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index ed7e879d3d..93f4fec92e 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1401,9 +1401,9 @@ Mavlink::task_main(int argc, char *argv[]) case MAVLINK_MODE_ONBOARD: configure_stream("SYS_STATUS", 1.0f); - configure_stream("ATTITUDE", 15.0f); - configure_stream("GLOBAL_POSITION_INT", 15.0f); - configure_stream("CAMERA_CAPTURE", 1.0f); + configure_stream("ATTITUDE", 50.0f); + configure_stream("GLOBAL_POSITION_INT", 50.0f); + configure_stream("CAMERA_CAPTURE", 2.0f); break; default: From 9825ed8f3cd037b8fd131a911ba350a29203ef0c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 25 Aug 2014 10:21:26 +0200 Subject: [PATCH 25/34] Attempt at fixing programming timeouts --- Tools/px_uploader.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index d8f4884bc0..b46db00b57 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -178,9 +178,9 @@ class uploader(object): MAVLINK_REBOOT_ID1 = bytearray(b'\xfe\x21\x72\xff\x00\x4c\x00\x00\x80\x3f\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf6\x00\x01\x00\x00\x48\xf0') MAVLINK_REBOOT_ID0 = bytearray(b'\xfe\x21\x45\xff\x00\x4c\x00\x00\x80\x3f\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf6\x00\x00\x00\x00\xd7\xac') - def __init__(self, portname, baudrate): + def __init__(self, portname, baudrate, interCharTimeout=0.001, timeout=0.5): # open the port, keep the default timeout short so we can poll quickly - self.port = serial.Serial(portname, baudrate, timeout=2.0) + self.port = serial.Serial(portname, baudrate) self.otp = b'' self.sn = b'' @@ -195,7 +195,7 @@ class uploader(object): def __recv(self, count=1): c = self.port.read(count) if len(c) < 1: - raise RuntimeError("timeout waiting for data") + raise RuntimeError("timeout waiting for data (%u bytes)", count) # print("recv " + binascii.hexlify(c)) return c From 73ecbbe13d1f231bf2a9c2ccaafe29065352d75c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 25 Aug 2014 11:12:01 +0200 Subject: [PATCH 26/34] config_px4fmu-v2_default: include px4flow driver by default --- makefiles/config_px4fmu-v2_default.mk | 1 + 1 file changed, 1 insertion(+) diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index d0a40445d6..5f146686c8 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -41,6 +41,7 @@ MODULES += drivers/frsky_telemetry MODULES += modules/sensors MODULES += drivers/mkblctrl MODULES += drivers/pca8574 +MODULES += drivers/px4flow # Needs to be burned to the ground and re-written; for now, From 60799e51558e6259a7a2d20d765a4f8d29b88cc5 Mon Sep 17 00:00:00 2001 From: Nuno Marques Date: Mon, 25 Aug 2014 11:07:30 +0100 Subject: [PATCH 27/34] sdlog2: add vision log struct --- src/modules/sdlog2/sdlog2_messages.h | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index fb7609435b..82d83b5c1e 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -391,6 +391,16 @@ struct log_TEL_s { uint64_t heartbeat_time; }; +/* --- VISN - VISION POSITION --- */ +#define LOG_VISN_MSG 38 +struct log_VISN_s { + float x; + float y; + float z; + float roll; + float pitch; + float yaw; +}; /********** SYSTEM MESSAGES, ID > 0x80 **********/ @@ -449,6 +459,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(EST1, "ffffffffffffffff", "s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27"), LOG_FORMAT(PWR, "fffBBBBB", "Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"), LOG_FORMAT(VICN, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"), + LOG_FORMAT(VISN, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"), LOG_FORMAT(GS0A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), From ec8438bdcab31c566f91869140505b506a4fcaf8 Mon Sep 17 00:00:00 2001 From: Nuno Marques Date: Mon, 25 Aug 2014 11:20:55 +0100 Subject: [PATCH 28/34] sdlog2: added vision estimate logging --- src/modules/sdlog2/sdlog2.c | 25 +++++++++++++++++-------- 1 file changed, 17 insertions(+), 8 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 6c4b494522..59765415a6 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -934,6 +934,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_global_position_s global_pos; struct position_setpoint_triplet_s triplet; struct vehicle_vicon_position_s vicon_pos; + struct vehicle_vision_position_s vision_pos; struct optical_flow_s flow; struct rc_channels_s rc; struct differential_pressure_s diff_pres; @@ -984,6 +985,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_EST1_s log_EST1; struct log_PWR_s log_PWR; struct log_VICN_s log_VICN; + struct log_VISN_s log_VISN; struct log_GS0A_s log_GS0A; struct log_GS0B_s log_GS0B; struct log_GS1A_s log_GS1A; @@ -1013,6 +1015,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int gps_pos_sub; int sat_info_sub; int vicon_pos_sub; + int vision_pos_sub; int flow_sub; int rc_sub; int airspeed_sub; @@ -1043,6 +1046,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); subs.triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); subs.vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position)); + subs.vision_pos_sub = orb_subscribe(ORB_ID(vehicle_vision_position)); subs.flow_sub = orb_subscribe(ORB_ID(optical_flow)); subs.rc_sub = orb_subscribe(ORB_ID(rc_channels)); subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed)); @@ -1459,6 +1463,18 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_VICN.yaw = buf.vicon_pos.yaw; LOGBUFFER_WRITE_AND_COUNT(VICN); } + + /* --- VISION POSITION --- */ + if (copy_if_updated(ORB_ID(vehicle_vision_position), subs.vision_pos_sub, &buf.vision_pos)) { + log_msg.msg_type = LOG_VISN_MSG; + log_msg.body.log_VISN.x = buf.vision_pos.x; + log_msg.body.log_VISN.y = buf.vision_pos.y; + log_msg.body.log_VISN.z = buf.vision_pos.z; + log_msg.body.log_VISN.pitch = buf.vision_pos.pitch; + log_msg.body.log_VISN.roll = buf.vision_pos.roll; + log_msg.body.log_VISN.yaw = buf.vision_pos.yaw; + LOGBUFFER_WRITE_AND_COUNT(VISN); + } /* --- FLOW --- */ if (copy_if_updated(ORB_ID(optical_flow), subs.flow_sub, &buf.flow)) { @@ -1565,14 +1581,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } } - /* --- BOTTOM DISTANCE --- */ - if (copy_if_updated(ORB_ID(sensor_range_finder), subs.range_finder_sub, &buf.range_finder)) { - log_msg.msg_type = LOG_DIST_MSG; - log_msg.body.log_DIST.bottom = buf.range_finder.distance; - log_msg.body.log_DIST.bottom_rate = 0.0f; - log_msg.body.log_DIST.flags = (buf.range_finder.valid ? 1 : 0); - LOGBUFFER_WRITE_AND_COUNT(DIST); - } + /* --- ESTIMATOR STATUS --- */ if (copy_if_updated(ORB_ID(estimator_status), subs.estimator_status_sub, &buf.estimator_status)) { From d0f5eca5be3d3257b1350337b58f020063b65eb9 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 25 Aug 2014 13:13:07 +0200 Subject: [PATCH 29/34] px4flow: removed flow report in driver, just use uORB topic --- src/drivers/drv_px4flow.h | 31 ------------- src/drivers/px4flow/px4flow.cpp | 68 ++++++++++++++-------------- src/modules/mavlink/mavlink_main.cpp | 2 +- 3 files changed, 35 insertions(+), 66 deletions(-) diff --git a/src/drivers/drv_px4flow.h b/src/drivers/drv_px4flow.h index 76ec55c3e8..ab640837bb 100644 --- a/src/drivers/drv_px4flow.h +++ b/src/drivers/drv_px4flow.h @@ -46,37 +46,6 @@ #define PX4FLOW_DEVICE_PATH "/dev/px4flow" -/** - * @addtogroup topics - * @{ - */ - -/** - * Optical flow in NED body frame in SI units. - * - * @see http://en.wikipedia.org/wiki/International_System_of_Units - * - * @warning If possible the usage of the raw flow and performing rotation-compensation - * using the autopilot angular rate estimate is recommended. - */ -struct px4flow_report { - - uint64_t timestamp; /**< in microseconds since system start */ - - int16_t flow_raw_x; /**< flow in pixels in X direction, not rotation-compensated */ - int16_t flow_raw_y; /**< flow in pixels in Y direction, not rotation-compensated */ - float flow_comp_x_m; /**< speed over ground in meters per second, rotation-compensated */ - float flow_comp_y_m; /**< speed over ground in meters per second, rotation-compensated */ - float ground_distance_m; /**< Altitude / distance to ground in meters */ - uint8_t quality; /**< Quality of the measurement, 0: bad quality, 255: maximum quality */ - uint8_t sensor_id; /**< id of the sensor emitting the flow value */ - -}; - -/** - * @} - */ - /* * ObjDev tag for px4flow data. */ diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index f214b5964f..60ad3c1aff 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -37,7 +37,7 @@ * * Driver for the PX4FLOW module connected via I2C. */ - + #include #include @@ -68,7 +68,7 @@ #include #include -//#include +#include #include @@ -80,7 +80,7 @@ /* PX4FLOW Registers addresses */ #define PX4FLOW_REG 0x00 /* Measure Register */ -#define PX4FLOW_CONVERSION_INTERVAL 8000 /* 8ms 125Hz +#define PX4FLOW_CONVERSION_INTERVAL 8000 /* 8ms 125Hz */ /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -115,17 +115,17 @@ class PX4FLOW : public device::I2C public: PX4FLOW(int bus = PX4FLOW_BUS, int address = I2C_FLOW_ADDRESS); virtual ~PX4FLOW(); - + virtual int init(); - + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - + /** * Diagnostics - print some basic information about the driver. */ void print_info(); - + protected: virtual int probe(); @@ -136,13 +136,13 @@ private: bool _sensor_ok; int _measure_ticks; bool _collect_phase; - + orb_advert_t _px4flow_topic; perf_counter_t _sample_perf; perf_counter_t _comms_errors; perf_counter_t _buffer_overflows; - + /** * Test whether the device supported by the driver is present at a * specific address. @@ -151,7 +151,7 @@ private: * @return True if the device is present. */ int probe_address(uint8_t address); - + /** * Initialise the automatic measurement state machine and start it. * @@ -159,12 +159,12 @@ private: * to make it more aggressive about resetting the bus in case of errors. */ void start(); - + /** * Stop the automatic measurement state machine. */ void stop(); - + /** * Perform a poll cycle; collect from the previous measurement * and start a new one. @@ -179,8 +179,8 @@ private: * @param arg Instance pointer for the driver that is polling. */ static void cycle_trampoline(void *arg); - - + + }; /* @@ -201,7 +201,7 @@ PX4FLOW::PX4FLOW(int bus, int address) : { // enable debug() calls _debug_enabled = true; - + // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); } @@ -226,13 +226,13 @@ PX4FLOW::init() goto out; /* allocate basic report buffers */ - _reports = new RingBuffer(2, sizeof(px4flow_report)); + _reports = new RingBuffer(2, sizeof(struct optical_flow_s)); if (_reports == nullptr) goto out; /* get a publish handle on the px4flow topic */ - struct px4flow_report zero_report; + struct optical_flow_s zero_report; memset(&zero_report, 0, sizeof(zero_report)); _px4flow_topic = orb_advertise(ORB_ID(optical_flow), &zero_report); @@ -323,24 +323,24 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg) /* lower bound is mandatory, upper bound is a sanity check */ if ((arg < 1) || (arg > 100)) return -EINVAL; - + irqstate_t flags = irqsave(); if (!_reports->resize(arg)) { irqrestore(flags); return -ENOMEM; } irqrestore(flags); - + return OK; } case SENSORIOCGQUEUEDEPTH: return _reports->size(); - + case SENSORIOCRESET: /* XXX implement this */ return -EINVAL; - + default: /* give it to the superclass */ return I2C::ioctl(filp, cmd, arg); @@ -350,8 +350,8 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg) ssize_t PX4FLOW::read(struct file *filp, char *buffer, size_t buflen) { - unsigned count = buflen / sizeof(struct px4flow_report); - struct px4flow_report *rbuf = reinterpret_cast(buffer); + unsigned count = buflen / sizeof(struct optical_flow_s); + struct optical_flow_s *rbuf = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -425,7 +425,7 @@ PX4FLOW::measure() return ret; } ret = OK; - + return ret; } @@ -433,14 +433,14 @@ int PX4FLOW::collect() { int ret = -EIO; - + /* read from the sensor */ uint8_t val[22] = {0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0}; - + perf_begin(_sample_perf); - + ret = transfer(nullptr, 0, &val[0], 22); - + if (ret < 0) { log("error reading from sensor: %d", ret); @@ -448,7 +448,7 @@ PX4FLOW::collect() perf_end(_sample_perf); return ret; } - + // f.frame_count = val[1] << 8 | val[0]; // f.pixel_flow_x_sum= val[3] << 8 | val[2]; // f.pixel_flow_y_sum= val[5] << 8 | val[4]; @@ -466,7 +466,7 @@ PX4FLOW::collect() int16_t flowcy = val[9] << 8 | val[8]; int16_t gdist = val[21] << 8 | val[20]; - struct px4flow_report report; + struct optical_flow_s report; report.flow_comp_x_m = float(flowcx)/1000.0f; report.flow_comp_y_m = float(flowcy)/1000.0f; report.flow_raw_x= val[3] << 8 | val[2]; @@ -503,7 +503,7 @@ PX4FLOW::start() /* schedule a cycle to start things */ work_queue(HPWORK, &_work, (worker_t)&PX4FLOW::cycle_trampoline, this, 1); - + /* notify about state change */ struct subsystem_info_s info = { true, @@ -644,7 +644,7 @@ start() fail: - if (g_dev != nullptr) + if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; @@ -678,7 +678,7 @@ void stop() void test() { - struct px4flow_report report; + struct optical_flow_s report; ssize_t sz; int ret; @@ -777,7 +777,7 @@ px4flow_main(int argc, char *argv[]) */ if (!strcmp(argv[1], "start")) px4flow::start(); - + /* * Stop the driver */ diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 93f4fec92e..940e641445 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1396,7 +1396,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f); configure_stream("ATTITUDE_TARGET", 3.0f); configure_stream("DISTANCE_SENSOR", 0.5f); - configure_stream("OPTICAL_FLOW", 0.5f); + configure_stream("OPTICAL_FLOW", 20.0f); break; case MAVLINK_MODE_ONBOARD: From 0994006f96f2c030acd31bb14b32d2fd2127c6dd Mon Sep 17 00:00:00 2001 From: Nuno Marques Date: Mon, 25 Aug 2014 14:16:13 +0100 Subject: [PATCH 30/34] sdlog2: update vision log fields --- src/modules/sdlog2/sdlog2.c | 17 +++++++++++------ src/modules/sdlog2/sdlog2_messages.h | 12 ++++++++---- 2 files changed, 19 insertions(+), 10 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 59765415a6..ec4197e79f 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -76,6 +76,7 @@ #include #include #include +#include #include #include #include @@ -934,7 +935,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_global_position_s global_pos; struct position_setpoint_triplet_s triplet; struct vehicle_vicon_position_s vicon_pos; - struct vehicle_vision_position_s vision_pos; + struct vision_position_estimate_s vision_pos; struct optical_flow_s flow; struct rc_channels_s rc; struct differential_pressure_s diff_pres; @@ -1046,7 +1047,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); subs.triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); subs.vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position)); - subs.vision_pos_sub = orb_subscribe(ORB_ID(vehicle_vision_position)); + subs.vision_pos_sub = orb_subscribe(ORB_ID(vision_position_estimate)); subs.flow_sub = orb_subscribe(ORB_ID(optical_flow)); subs.rc_sub = orb_subscribe(ORB_ID(rc_channels)); subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed)); @@ -1465,14 +1466,18 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- VISION POSITION --- */ - if (copy_if_updated(ORB_ID(vehicle_vision_position), subs.vision_pos_sub, &buf.vision_pos)) { + if (copy_if_updated(ORB_ID(vision_position_estimate), subs.vision_pos_sub, &buf.vision_pos)) { log_msg.msg_type = LOG_VISN_MSG; log_msg.body.log_VISN.x = buf.vision_pos.x; log_msg.body.log_VISN.y = buf.vision_pos.y; log_msg.body.log_VISN.z = buf.vision_pos.z; - log_msg.body.log_VISN.pitch = buf.vision_pos.pitch; - log_msg.body.log_VISN.roll = buf.vision_pos.roll; - log_msg.body.log_VISN.yaw = buf.vision_pos.yaw; + log_msg.body.log_VISN.vx = buf.vision_pos.vx; + log_msg.body.log_VISN.vy = buf.vision_pos.vy; + log_msg.body.log_VISN.vz = buf.vision_pos.vz; + log_msg.body.log_VISN.qx = buf.vision_pos.q[0]; + log_msg.body.log_VISN.qy = buf.vision_pos.q[1]; + log_msg.body.log_VISN.qz = buf.vision_pos.q[2]; + log_msg.body.log_VISN.qw = buf.vision_pos.q[3]; LOGBUFFER_WRITE_AND_COUNT(VISN); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 82d83b5c1e..6741c7e258 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -397,9 +397,13 @@ struct log_VISN_s { float x; float y; float z; - float roll; - float pitch; - float yaw; + float vx; + float vy; + float vz; + float qx; + float qy; + float qz; + float qw; }; /********** SYSTEM MESSAGES, ID > 0x80 **********/ @@ -459,7 +463,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(EST1, "ffffffffffffffff", "s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27"), LOG_FORMAT(PWR, "fffBBBBB", "Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"), LOG_FORMAT(VICN, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"), - LOG_FORMAT(VISN, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"), + LOG_FORMAT(VISN, "ffffffffff", "X,Y,Z,VX,VY,VZ,QuatX,QuatY,QuatZ,QuatW"), LOG_FORMAT(GS0A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), From 3ef374c4264969928ce958ff3053f6238909479b Mon Sep 17 00:00:00 2001 From: Nuno Marques Date: Mon, 25 Aug 2014 14:20:17 +0100 Subject: [PATCH 31/34] sdlog2: added BOTTOM_DISTANCE again --- src/modules/sdlog2/sdlog2.c | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index ec4197e79f..02cc39dfc0 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1586,7 +1586,14 @@ int sdlog2_thread_main(int argc, char *argv[]) } } - + /* --- BOTTOM DISTANCE --- */ +- if (copy_if_updated(ORB_ID(sensor_range_finder), subs.range_finder_sub, &buf.range_finder)) { +- log_msg.msg_type = LOG_DIST_MSG; +- log_msg.body.log_DIST.bottom = buf.range_finder.distance; +- log_msg.body.log_DIST.bottom_rate = 0.0f; +- log_msg.body.log_DIST.flags = (buf.range_finder.valid ? 1 : 0); +- LOGBUFFER_WRITE_AND_COUNT(DIST); +- } /* --- ESTIMATOR STATUS --- */ if (copy_if_updated(ORB_ID(estimator_status), subs.estimator_status_sub, &buf.estimator_status)) { From ebd56aa2c12c3c55e4eb349429a765355afe6360 Mon Sep 17 00:00:00 2001 From: Nuno Marques Date: Mon, 25 Aug 2014 15:07:14 +0100 Subject: [PATCH 32/34] sdlog2: minor corrections --- src/modules/sdlog2/sdlog2.c | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 02cc39dfc0..07655808c0 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -935,7 +935,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_global_position_s global_pos; struct position_setpoint_triplet_s triplet; struct vehicle_vicon_position_s vicon_pos; - struct vision_position_estimate_s vision_pos; + struct vision_position_estimate vision_pos; struct optical_flow_s flow; struct rc_channels_s rc; struct differential_pressure_s diff_pres; @@ -1587,13 +1587,13 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- BOTTOM DISTANCE --- */ -- if (copy_if_updated(ORB_ID(sensor_range_finder), subs.range_finder_sub, &buf.range_finder)) { -- log_msg.msg_type = LOG_DIST_MSG; -- log_msg.body.log_DIST.bottom = buf.range_finder.distance; -- log_msg.body.log_DIST.bottom_rate = 0.0f; -- log_msg.body.log_DIST.flags = (buf.range_finder.valid ? 1 : 0); -- LOGBUFFER_WRITE_AND_COUNT(DIST); -- } + if (copy_if_updated(ORB_ID(sensor_range_finder), subs.range_finder_sub, &buf.range_finder)) { + log_msg.msg_type = LOG_DIST_MSG; + log_msg.body.log_DIST.bottom = buf.range_finder.distance; + log_msg.body.log_DIST.bottom_rate = 0.0f; + log_msg.body.log_DIST.flags = (buf.range_finder.valid ? 1 : 0); + LOGBUFFER_WRITE_AND_COUNT(DIST); + } /* --- ESTIMATOR STATUS --- */ if (copy_if_updated(ORB_ID(estimator_status), subs.estimator_status_sub, &buf.estimator_status)) { From d6810ae1f8e2b8c86d54acfe80094265e97a3232 Mon Sep 17 00:00:00 2001 From: Nuno Marques Date: Mon, 25 Aug 2014 15:37:51 +0100 Subject: [PATCH 33/34] sdlog2: minor improvements --- src/modules/sdlog2/sdlog2.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 07655808c0..dbda8ea6f3 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1589,11 +1589,11 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- BOTTOM DISTANCE --- */ if (copy_if_updated(ORB_ID(sensor_range_finder), subs.range_finder_sub, &buf.range_finder)) { log_msg.msg_type = LOG_DIST_MSG; - log_msg.body.log_DIST.bottom = buf.range_finder.distance; + log_msg.body.log_DIST.bottom = buf.range_finder.distance; log_msg.body.log_DIST.bottom_rate = 0.0f; log_msg.body.log_DIST.flags = (buf.range_finder.valid ? 1 : 0); - LOGBUFFER_WRITE_AND_COUNT(DIST); - } + LOGBUFFER_WRITE_AND_COUNT(DIST); + } /* --- ESTIMATOR STATUS --- */ if (copy_if_updated(ORB_ID(estimator_status), subs.estimator_status_sub, &buf.estimator_status)) { From eab701b896fa316132aff78a34362ca77549e581 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 26 Aug 2014 00:50:19 +0400 Subject: [PATCH 34/34] Improved UAVCAN status reporting --- src/modules/uavcan/sensors/gnss.cpp | 10 ++++++ src/modules/uavcan/sensors/gnss.hpp | 2 ++ src/modules/uavcan/sensors/sensor_bridge.cpp | 36 ++++++++++++++------ src/modules/uavcan/sensors/sensor_bridge.hpp | 15 +++++--- src/modules/uavcan/uavcan_main.cpp | 10 +++--- 5 files changed, 55 insertions(+), 18 deletions(-) diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp index 8548660fe1..0d67aad476 100644 --- a/src/modules/uavcan/sensors/gnss.cpp +++ b/src/modules/uavcan/sensors/gnss.cpp @@ -70,6 +70,16 @@ unsigned UavcanGnssBridge::get_num_redundant_channels() const return (_receiver_node_id < 0) ? 0 : 1; } +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); + } +} + void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure &msg) { // This bridge does not support redundant GNSS receivers yet. diff --git a/src/modules/uavcan/sensors/gnss.hpp b/src/modules/uavcan/sensors/gnss.hpp index c2b6e41956..e8466b4010 100644 --- a/src/modules/uavcan/sensors/gnss.hpp +++ b/src/modules/uavcan/sensors/gnss.hpp @@ -66,6 +66,8 @@ public: unsigned get_num_redundant_channels() const override; + void print_status() const override; + private: /** * GNSS fix message will be reported via this callback. diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp index a98596f9c5..9608ce6804 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.cpp +++ b/src/modules/uavcan/sensors/sensor_bridge.cpp @@ -58,7 +58,7 @@ void IUavcanSensorBridge::make_all(uavcan::INode &node, List= 0) { + if (_channels[i].node_id >= 0) { (void)unregister_class_devname(_class_devname, _channels[i].class_instance); } } @@ -66,13 +66,15 @@ UavcanCDevSensorBridgeBase::~UavcanCDevSensorBridgeBase() delete [] _channels; } -void UavcanCDevSensorBridgeBase::publish(const int redundancy_channel_id, const void *report) +void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report) { + assert(report != nullptr); + Channel *channel = nullptr; // Checking if such channel already exists for (unsigned i = 0; i < _max_channels; i++) { - if (_channels[i].redunancy_channel_id == redundancy_channel_id) { + if (_channels[i].node_id == node_id) { channel = _channels + i; break; } @@ -84,11 +86,11 @@ void UavcanCDevSensorBridgeBase::publish(const int redundancy_channel_id, const return; // Give up immediately - saves some CPU time } - log("adding channel %d...", redundancy_channel_id); + log("adding channel %d...", node_id); // Search for the first free channel for (unsigned i = 0; i < _max_channels; i++) { - if (_channels[i].redunancy_channel_id < 0) { + if (_channels[i].node_id < 0) { channel = _channels + i; break; } @@ -111,9 +113,9 @@ void UavcanCDevSensorBridgeBase::publish(const int redundancy_channel_id, const } // Publish to the appropriate topic, abort on failure - channel->orb_id = _orb_topics[class_instance]; - channel->redunancy_channel_id = redundancy_channel_id; - channel->class_instance = class_instance; + channel->orb_id = _orb_topics[class_instance]; + channel->node_id = node_id; + channel->class_instance = class_instance; channel->orb_advert = orb_advertise(channel->orb_id, report); if (channel->orb_advert < 0) { @@ -123,7 +125,7 @@ void UavcanCDevSensorBridgeBase::publish(const int redundancy_channel_id, const return; } - log("channel %d class instance %d ok", channel->redunancy_channel_id, channel->class_instance); + log("channel %d class instance %d ok", channel->node_id, channel->class_instance); } assert(channel != nullptr); @@ -134,9 +136,23 @@ unsigned UavcanCDevSensorBridgeBase::get_num_redundant_channels() const { unsigned out = 0; for (unsigned i = 0; i < _max_channels; i++) { - if (_channels[i].redunancy_channel_id >= 0) { + if (_channels[i].node_id >= 0) { out += 1; } } return out; } + +void UavcanCDevSensorBridgeBase::print_status() const +{ + printf("devname: %s\n", _class_devname); + + for (unsigned i = 0; i < _max_channels; i++) { + 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); + } + } +} diff --git a/src/modules/uavcan/sensors/sensor_bridge.hpp b/src/modules/uavcan/sensors/sensor_bridge.hpp index a13d0ab35d..1316f7ecc5 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.hpp +++ b/src/modules/uavcan/sensors/sensor_bridge.hpp @@ -68,6 +68,11 @@ public: */ virtual unsigned get_num_redundant_channels() const = 0; + /** + * Prints current status in a human readable format to stdout. + */ + virtual void print_status() const = 0; + /** * Sensor bridge factory. * Creates a bridge object by its ASCII name, e.g. "gnss", "mag". @@ -84,7 +89,7 @@ class UavcanCDevSensorBridgeBase : public IUavcanSensorBridge, public device::CD { struct Channel { - int redunancy_channel_id = -1; + int node_id = -1; orb_id_t orb_id = nullptr; orb_advert_t orb_advert = -1; int class_instance = -1; @@ -112,13 +117,15 @@ protected: /** * Sends one measurement into appropriate ORB topic. * New redundancy channels will be registered automatically. - * @param redundancy_channel_id Redundant sensor identifier (e.g. UAVCAN Node ID) - * @param report ORB message object + * @param node_id Sensor's Node ID + * @param report Pointer to ORB message object */ - void publish(const int redundancy_channel_id, const void *report); + void publish(const int node_id, const void *report); public: virtual ~UavcanCDevSensorBridgeBase(); unsigned get_num_redundant_channels() const override; + + void print_status() const override; }; diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 482fec1e0a..95c6ba13e0 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -548,14 +548,16 @@ UavcanNode::print_info() (void)pthread_mutex_lock(&_node_mutex); // ESC mixer status - warnx("ESC actuators control groups: sub: %u / req: %u / fds: %u", - (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num); - warnx("ESC mixer: %s", (_mixers == nullptr) ? "NONE" : "OK"); + printf("ESC actuators control groups: sub: %u / req: %u / fds: %u\n", + (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num); + printf("ESC mixer: %s\n", (_mixers == nullptr) ? "NONE" : "OK"); // Sensor bridges auto br = _sensor_bridges.getHead(); while (br != nullptr) { - warnx("Sensor '%s': channels: %u", br->get_name(), br->get_num_redundant_channels()); + printf("Sensor '%s':\n", br->get_name()); + br->print_status(); + printf("\n"); br = br->getSibling(); }