From b606674fc77aa772fba66ad09dd3879a62a7f44c Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 1 Aug 2014 01:04:24 +0400 Subject: Removed misleading comment from the UAVCAN module makefile --- src/modules/uavcan/module.mk | 4 ---- 1 file changed, 4 deletions(-) (limited to 'src/modules/uavcan/module.mk') diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index 1ef6f0cfa..3865f2468 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -65,10 +65,6 @@ override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_STM32_NUTTX -DUAVCAN_STM32_NUM # # Invoke DSDL compiler -# TODO: Add make target for this, or invoke dsdlc manually. -# The second option assumes that the generated headers shall be saved -# under the version control, which may be undesirable. -# The first option requires any Python and the Python Mako library for the sources to be built. # $(info $(shell $(LIBUAVCAN_DSDLC) $(UAVCAN_DSDL_DIR))) INCLUDE_DIRS += dsdlc_generated -- cgit v1.2.3 From 0d9f6b6e2ec86f7fa95bc2e7650256e1392544b2 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 22 Aug 2014 01:52:23 +0400 Subject: UAVCAN: Refactored and generalized sensor bridge support --- src/modules/uavcan/actuators/esc.cpp | 138 ++++++++++++++++++++++++ src/modules/uavcan/actuators/esc.hpp | 107 ++++++++++++++++++ src/modules/uavcan/esc_controller.cpp | 138 ------------------------ src/modules/uavcan/esc_controller.hpp | 107 ------------------ src/modules/uavcan/gnss_receiver.cpp | 153 -------------------------- src/modules/uavcan/gnss_receiver.hpp | 84 --------------- src/modules/uavcan/module.mk | 14 ++- src/modules/uavcan/sensors/gnss.cpp | 156 +++++++++++++++++++++++++++ src/modules/uavcan/sensors/gnss.hpp | 88 +++++++++++++++ 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 ++-- 13 files changed, 731 insertions(+), 514 deletions(-) create mode 100644 src/modules/uavcan/actuators/esc.cpp create mode 100644 src/modules/uavcan/actuators/esc.hpp delete mode 100644 src/modules/uavcan/esc_controller.cpp delete mode 100644 src/modules/uavcan/esc_controller.hpp delete mode 100644 src/modules/uavcan/gnss_receiver.cpp delete mode 100644 src/modules/uavcan/gnss_receiver.hpp create mode 100644 src/modules/uavcan/sensors/gnss.cpp create mode 100644 src/modules/uavcan/sensors/gnss.hpp create mode 100644 src/modules/uavcan/sensors/sensor_bridge.cpp create mode 100644 src/modules/uavcan/sensors/sensor_bridge.hpp (limited to 'src/modules/uavcan/module.mk') diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp new file mode 100644 index 000000000..223d94731 --- /dev/null +++ b/src/modules/uavcan/actuators/esc.cpp @@ -0,0 +1,138 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file esc.cpp + * + * @author Pavel Kirienko + */ + +#include "esc.hpp" +#include + +UavcanEscController::UavcanEscController(uavcan::INode &node) : + _node(node), + _uavcan_pub_raw_cmd(node), + _uavcan_sub_status(node), + _orb_timer(node) +{ +} + +UavcanEscController::~UavcanEscController() +{ + perf_free(_perfcnt_invalid_input); + perf_free(_perfcnt_scaling_error); +} + +int UavcanEscController::init() +{ + // ESC status subscription + int res = _uavcan_sub_status.start(StatusCbBinder(this, &UavcanEscController::esc_status_sub_cb)); + if (res < 0) + { + warnx("ESC status sub failed %i", res); + return res; + } + + // ESC status will be relayed from UAVCAN bus into ORB at this rate + _orb_timer.setCallback(TimerCbBinder(this, &UavcanEscController::orb_pub_timer_cb)); + _orb_timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000 / ESC_STATUS_UPDATE_RATE_HZ)); + + return res; +} + +void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) +{ + if ((outputs == nullptr) || (num_outputs > MAX_ESCS)) { + perf_count(_perfcnt_invalid_input); + return; + } + + /* + * Rate limiting - we don't want to congest the bus + */ + const auto timestamp = _node.getMonotonicTime(); + if ((timestamp - _prev_cmd_pub).toUSec() < (1000000 / MAX_RATE_HZ)) { + return; + } + _prev_cmd_pub = timestamp; + + /* + * Fill the command message + * If unarmed, we publish an empty message anyway + */ + uavcan::equipment::esc::RawCommand msg; + + if (_armed) { + for (unsigned i = 0; i < num_outputs; i++) { + + float scaled = (outputs[i] + 1.0F) * 0.5F * uavcan::equipment::esc::RawCommand::CMD_MAX; + if (scaled < 1.0F) { + scaled = 1.0F; // Since we're armed, we don't want to stop it completely + } + + if (scaled < uavcan::equipment::esc::RawCommand::CMD_MIN) { + scaled = uavcan::equipment::esc::RawCommand::CMD_MIN; + perf_count(_perfcnt_scaling_error); + } else if (scaled > uavcan::equipment::esc::RawCommand::CMD_MAX) { + scaled = uavcan::equipment::esc::RawCommand::CMD_MAX; + perf_count(_perfcnt_scaling_error); + } else { + ; // Correct value + } + + msg.cmd.push_back(static_cast(scaled)); + } + } + + /* + * Publish the command message to the bus + * Note that for a quadrotor it takes one CAN frame + */ + (void)_uavcan_pub_raw_cmd.broadcast(msg); +} + +void UavcanEscController::arm_esc(bool arm) +{ + _armed = arm; +} + +void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure &msg) +{ + // TODO save status into a local storage; publish to ORB later from orb_pub_timer_cb() +} + +void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent&) +{ + // TODO publish to ORB +} diff --git a/src/modules/uavcan/actuators/esc.hpp b/src/modules/uavcan/actuators/esc.hpp new file mode 100644 index 000000000..cf0988210 --- /dev/null +++ b/src/modules/uavcan/actuators/esc.hpp @@ -0,0 +1,107 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file esc.hpp + * + * UAVCAN <--> ORB bridge for ESC messages: + * uavcan.equipment.esc.RawCommand + * uavcan.equipment.esc.RPMCommand + * uavcan.equipment.esc.Status + * + * @author Pavel Kirienko + */ + +#pragma once + +#include +#include +#include +#include + +class UavcanEscController +{ +public: + UavcanEscController(uavcan::INode& node); + ~UavcanEscController(); + + int init(); + + void update_outputs(float *outputs, unsigned num_outputs); + + void arm_esc(bool arm); + +private: + /** + * ESC status message reception will be reported via this callback. + */ + void esc_status_sub_cb(const uavcan::ReceivedDataStructure &msg); + + /** + * ESC status will be published to ORB from this callback (fixed rate). + */ + void orb_pub_timer_cb(const uavcan::TimerEvent &event); + + + static constexpr unsigned MAX_RATE_HZ = 100; ///< XXX make this configurable + static constexpr unsigned ESC_STATUS_UPDATE_RATE_HZ = 5; + static constexpr unsigned MAX_ESCS = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize; + + typedef uavcan::MethodBinder&)> + StatusCbBinder; + + typedef uavcan::MethodBinder + TimerCbBinder; + + /* + * libuavcan related things + */ + uavcan::MonotonicTime _prev_cmd_pub; ///< rate limiting + uavcan::INode &_node; + uavcan::Publisher _uavcan_pub_raw_cmd; + uavcan::Subscriber _uavcan_sub_status; + uavcan::TimerEventForwarder _orb_timer; + + /* + * ESC states + */ + bool _armed = false; + uavcan::equipment::esc::Status _states[MAX_ESCS]; + + /* + * Perf counters + */ + perf_counter_t _perfcnt_invalid_input = perf_alloc(PC_COUNT, "uavcan_esc_invalid_input"); + perf_counter_t _perfcnt_scaling_error = perf_alloc(PC_COUNT, "uavcan_esc_scaling_error"); +}; diff --git a/src/modules/uavcan/esc_controller.cpp b/src/modules/uavcan/esc_controller.cpp deleted file mode 100644 index 406eba88c..000000000 --- a/src/modules/uavcan/esc_controller.cpp +++ /dev/null @@ -1,138 +0,0 @@ -/**************************************************************************** - * - * 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. - * - ****************************************************************************/ - -/** - * @file esc_controller.cpp - * - * @author Pavel Kirienko - */ - -#include "esc_controller.hpp" -#include - -UavcanEscController::UavcanEscController(uavcan::INode &node) : - _node(node), - _uavcan_pub_raw_cmd(node), - _uavcan_sub_status(node), - _orb_timer(node) -{ -} - -UavcanEscController::~UavcanEscController() -{ - perf_free(_perfcnt_invalid_input); - perf_free(_perfcnt_scaling_error); -} - -int UavcanEscController::init() -{ - // ESC status subscription - int res = _uavcan_sub_status.start(StatusCbBinder(this, &UavcanEscController::esc_status_sub_cb)); - if (res < 0) - { - warnx("ESC status sub failed %i", res); - return res; - } - - // ESC status will be relayed from UAVCAN bus into ORB at this rate - _orb_timer.setCallback(TimerCbBinder(this, &UavcanEscController::orb_pub_timer_cb)); - _orb_timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000 / ESC_STATUS_UPDATE_RATE_HZ)); - - return res; -} - -void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) -{ - if ((outputs == nullptr) || (num_outputs > MAX_ESCS)) { - perf_count(_perfcnt_invalid_input); - return; - } - - /* - * Rate limiting - we don't want to congest the bus - */ - const auto timestamp = _node.getMonotonicTime(); - if ((timestamp - _prev_cmd_pub).toUSec() < (1000000 / MAX_RATE_HZ)) { - return; - } - _prev_cmd_pub = timestamp; - - /* - * Fill the command message - * If unarmed, we publish an empty message anyway - */ - uavcan::equipment::esc::RawCommand msg; - - if (_armed) { - for (unsigned i = 0; i < num_outputs; i++) { - - float scaled = (outputs[i] + 1.0F) * 0.5F * uavcan::equipment::esc::RawCommand::CMD_MAX; - if (scaled < 1.0F) { - scaled = 1.0F; // Since we're armed, we don't want to stop it completely - } - - if (scaled < uavcan::equipment::esc::RawCommand::CMD_MIN) { - scaled = uavcan::equipment::esc::RawCommand::CMD_MIN; - perf_count(_perfcnt_scaling_error); - } else if (scaled > uavcan::equipment::esc::RawCommand::CMD_MAX) { - scaled = uavcan::equipment::esc::RawCommand::CMD_MAX; - perf_count(_perfcnt_scaling_error); - } else { - ; // Correct value - } - - msg.cmd.push_back(static_cast(scaled)); - } - } - - /* - * Publish the command message to the bus - * Note that for a quadrotor it takes one CAN frame - */ - (void)_uavcan_pub_raw_cmd.broadcast(msg); -} - -void UavcanEscController::arm_esc(bool arm) -{ - _armed = arm; -} - -void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure &msg) -{ - // TODO save status into a local storage; publish to ORB later from orb_pub_timer_cb() -} - -void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent&) -{ - // TODO publish to ORB -} diff --git a/src/modules/uavcan/esc_controller.hpp b/src/modules/uavcan/esc_controller.hpp deleted file mode 100644 index 559ede561..000000000 --- a/src/modules/uavcan/esc_controller.hpp +++ /dev/null @@ -1,107 +0,0 @@ -/**************************************************************************** - * - * 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. - * - ****************************************************************************/ - -/** - * @file esc_controller.hpp - * - * UAVCAN <--> ORB bridge for ESC messages: - * uavcan.equipment.esc.RawCommand - * uavcan.equipment.esc.RPMCommand - * uavcan.equipment.esc.Status - * - * @author Pavel Kirienko - */ - -#pragma once - -#include -#include -#include -#include - -class UavcanEscController -{ -public: - UavcanEscController(uavcan::INode& node); - ~UavcanEscController(); - - int init(); - - void update_outputs(float *outputs, unsigned num_outputs); - - void arm_esc(bool arm); - -private: - /** - * ESC status message reception will be reported via this callback. - */ - void esc_status_sub_cb(const uavcan::ReceivedDataStructure &msg); - - /** - * ESC status will be published to ORB from this callback (fixed rate). - */ - void orb_pub_timer_cb(const uavcan::TimerEvent &event); - - - static constexpr unsigned MAX_RATE_HZ = 100; ///< XXX make this configurable - static constexpr unsigned ESC_STATUS_UPDATE_RATE_HZ = 5; - static constexpr unsigned MAX_ESCS = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize; - - typedef uavcan::MethodBinder&)> - StatusCbBinder; - - typedef uavcan::MethodBinder - TimerCbBinder; - - /* - * libuavcan related things - */ - uavcan::MonotonicTime _prev_cmd_pub; ///< rate limiting - uavcan::INode &_node; - uavcan::Publisher _uavcan_pub_raw_cmd; - uavcan::Subscriber _uavcan_sub_status; - uavcan::TimerEventForwarder _orb_timer; - - /* - * ESC states - */ - bool _armed = false; - uavcan::equipment::esc::Status _states[MAX_ESCS]; - - /* - * Perf counters - */ - perf_counter_t _perfcnt_invalid_input = perf_alloc(PC_COUNT, "uavcan_esc_invalid_input"); - perf_counter_t _perfcnt_scaling_error = perf_alloc(PC_COUNT, "uavcan_esc_scaling_error"); -}; diff --git a/src/modules/uavcan/gnss_receiver.cpp b/src/modules/uavcan/gnss_receiver.cpp deleted file mode 100644 index ba1fe5e49..000000000 --- a/src/modules/uavcan/gnss_receiver.cpp +++ /dev/null @@ -1,153 +0,0 @@ -/**************************************************************************** - * - * 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. - * - ****************************************************************************/ - -/** - * @file gnss_receiver.cpp - * - * @author Pavel Kirienko - * @author Andrew Chambers - * - */ - -#include "gnss_receiver.hpp" -#include -#include - -#define MM_PER_CM 10 // Millimeters per centimeter - -UavcanGnssReceiver::UavcanGnssReceiver(uavcan::INode &node) : -_node(node), -_uavcan_sub_status(node), -_report_pub(-1) -{ -} - -int UavcanGnssReceiver::init() -{ - int res = -1; - - // GNSS fix subscription - res = _uavcan_sub_status.start(FixCbBinder(this, &UavcanGnssReceiver::gnss_fix_sub_cb)); - if (res < 0) - { - warnx("GNSS fix sub failed %i", res); - return res; - } - - // Clear the uORB GPS report - memset(&_report, 0, sizeof(_report)); - - return res; -} - -void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure &msg) -{ - _report.timestamp_position = hrt_absolute_time(); - _report.lat = msg.lat_1e7; - _report.lon = msg.lon_1e7; - _report.alt = msg.alt_1e2 * MM_PER_CM; // Convert from centimeter (1e2) to millimeters (1e3) - - _report.timestamp_variance = _report.timestamp_position; - - - // Check if the msg contains valid covariance information - const bool valid_position_covariance = !msg.position_covariance.empty(); - const bool valid_velocity_covariance = !msg.velocity_covariance.empty(); - - if (valid_position_covariance) { - float pos_cov[9]; - msg.position_covariance.unpackSquareMatrix(pos_cov); - - // Horizontal position uncertainty - const float horizontal_pos_variance = math::max(pos_cov[0], pos_cov[4]); - _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; - } else { - _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]); - - /* There is a nonlinear relationship between the velocity vector and the heading. - * Use Jacobian to transform velocity covariance to heading covariance - * - * Nonlinear equation: - * heading = atan2(vel_e_m_s, vel_n_m_s) - * For math, see http://en.wikipedia.org/wiki/Atan2#Derivative - * - * To calculate the variance of heading from the variance of velocity, - * cov(heading) = J(velocity)*cov(velocity)*J(velocity)^T - */ - float vel_n = msg.ned_velocity[0]; - float vel_e = msg.ned_velocity[1]; - float vel_n_sq = vel_n * vel_n; - float vel_e_sq = vel_e * vel_e; - _report.c_variance_rad = - (vel_e_sq * vel_cov[0] + - -2 * vel_n * vel_e * vel_cov[1] + // Covariance matrix is symmetric - vel_n_sq* vel_cov[4]) / ((vel_n_sq + vel_e_sq) * (vel_n_sq + vel_e_sq)); - - } else { - _report.s_variance_m_s = -1.0F; - _report.c_variance_rad = -1.0F; - } - - _report.fix_type = msg.status; - - _report.timestamp_velocity = _report.timestamp_position; - _report.vel_n_m_s = msg.ned_velocity[0]; - _report.vel_e_m_s = msg.ned_velocity[1]; - _report.vel_d_m_s = msg.ned_velocity[2]; - _report.vel_m_s = sqrtf(_report.vel_n_m_s * _report.vel_n_m_s + _report.vel_e_m_s * _report.vel_e_m_s + _report.vel_d_m_s * _report.vel_d_m_s); - _report.cog_rad = atan2f(_report.vel_e_m_s, _report.vel_n_m_s); - _report.vel_ned_valid = true; - - _report.timestamp_time = _report.timestamp_position; - _report.time_gps_usec = uavcan::UtcTime(msg.gnss_timestamp).toUSec(); // Convert to microseconds - - _report.satellites_used = msg.sats_used; - - if (_report_pub > 0) { - orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); - - } else { - _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); - } - -} diff --git a/src/modules/uavcan/gnss_receiver.hpp b/src/modules/uavcan/gnss_receiver.hpp deleted file mode 100644 index 18df8da2f..000000000 --- a/src/modules/uavcan/gnss_receiver.hpp +++ /dev/null @@ -1,84 +0,0 @@ -/**************************************************************************** - * - * 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. - * - ****************************************************************************/ - -/** - * @file gnss_receiver.hpp - * - * UAVCAN --> ORB bridge for GNSS messages: - * uavcan.equipment.gnss.Fix - * - * @author Pavel Kirienko - * @author Andrew Chambers - */ - -#pragma once - -#include - -#include -#include - -#include -#include - -class UavcanGnssReceiver -{ -public: - UavcanGnssReceiver(uavcan::INode& node); - - int init(); - -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 _uavcan_sub_status; - - /* - * uORB - */ - 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/module.mk b/src/modules/uavcan/module.mk index 3865f2468..2b9729045 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/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp new file mode 100644 index 000000000..4fc0743ff --- /dev/null +++ b/src/modules/uavcan/sensors/gnss.cpp @@ -0,0 +1,156 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file gnss.cpp + * + * @author Pavel Kirienko + * @author Andrew Chambers + * + */ + +#include "gnss.hpp" +#include +#include + +#define MM_PER_CM 10 // Millimeters per centimeter + +UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) : +_node(node), +_uavcan_sub_status(node), +_report_pub(-1) +{ +} + +const char *UavcanGnssBridge::get_name() const { return "gnss"; } + +int UavcanGnssBridge::init() +{ + int res = -1; + + // GNSS fix subscription + res = _uavcan_sub_status.start(FixCbBinder(this, &UavcanGnssBridge::gnss_fix_sub_cb)); + if (res < 0) + { + warnx("GNSS fix sub failed %i", res); + return res; + } + + // Clear the uORB GPS report + memset(&_report, 0, sizeof(_report)); + + warnx("gnss sensor bridge init ok"); + return res; +} + +void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure &msg) +{ + _report.timestamp_position = hrt_absolute_time(); + _report.lat = msg.lat_1e7; + _report.lon = msg.lon_1e7; + _report.alt = msg.alt_1e2 * MM_PER_CM; // Convert from centimeter (1e2) to millimeters (1e3) + + _report.timestamp_variance = _report.timestamp_position; + + + // Check if the msg contains valid covariance information + const bool valid_position_covariance = !msg.position_covariance.empty(); + const bool valid_velocity_covariance = !msg.velocity_covariance.empty(); + + if (valid_position_covariance) { + float pos_cov[9]; + msg.position_covariance.unpackSquareMatrix(pos_cov); + + // Horizontal position uncertainty + const float horizontal_pos_variance = math::max(pos_cov[0], pos_cov[4]); + _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; + } else { + _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]); + + /* There is a nonlinear relationship between the velocity vector and the heading. + * Use Jacobian to transform velocity covariance to heading covariance + * + * Nonlinear equation: + * heading = atan2(vel_e_m_s, vel_n_m_s) + * For math, see http://en.wikipedia.org/wiki/Atan2#Derivative + * + * To calculate the variance of heading from the variance of velocity, + * cov(heading) = J(velocity)*cov(velocity)*J(velocity)^T + */ + float vel_n = msg.ned_velocity[0]; + float vel_e = msg.ned_velocity[1]; + float vel_n_sq = vel_n * vel_n; + float vel_e_sq = vel_e * vel_e; + _report.c_variance_rad = + (vel_e_sq * vel_cov[0] + + -2 * vel_n * vel_e * vel_cov[1] + // Covariance matrix is symmetric + vel_n_sq* vel_cov[4]) / ((vel_n_sq + vel_e_sq) * (vel_n_sq + vel_e_sq)); + + } else { + _report.s_variance_m_s = -1.0F; + _report.c_variance_rad = -1.0F; + } + + _report.fix_type = msg.status; + + _report.timestamp_velocity = _report.timestamp_position; + _report.vel_n_m_s = msg.ned_velocity[0]; + _report.vel_e_m_s = msg.ned_velocity[1]; + _report.vel_d_m_s = msg.ned_velocity[2]; + _report.vel_m_s = sqrtf(_report.vel_n_m_s * _report.vel_n_m_s + _report.vel_e_m_s * _report.vel_e_m_s + _report.vel_d_m_s * _report.vel_d_m_s); + _report.cog_rad = atan2f(_report.vel_e_m_s, _report.vel_n_m_s); + _report.vel_ned_valid = true; + + _report.timestamp_time = _report.timestamp_position; + _report.time_gps_usec = uavcan::UtcTime(msg.gnss_timestamp).toUSec(); // Convert to microseconds + + _report.satellites_used = msg.sats_used; + + if (_report_pub > 0) { + orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); + + } else { + _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 new file mode 100644 index 000000000..eec595ad1 --- /dev/null +++ b/src/modules/uavcan/sensors/gnss.hpp @@ -0,0 +1,88 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file gnss.hpp + * + * UAVCAN --> ORB bridge for GNSS messages: + * uavcan.equipment.gnss.Fix + * + * @author Pavel Kirienko + * @author Andrew Chambers + */ + +#pragma once + +#include + +#include +#include + +#include +#include + +#include "sensor_bridge.hpp" + +class UavcanGnssBridge : public IUavcanSensorBridge +{ +public: + UavcanGnssBridge(uavcan::INode& node); + + const char *get_name() const override; + + int init() 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 _uavcan_sub_status; + + /* + * uORB + */ + 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/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp new file mode 100644 index 000000000..ba27d6c71 --- /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 000000000..1667c2b57 --- /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 4535b6d6a..fc5521aa7 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 05b66fd7b..bca1aa530 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; -- cgit v1.2.3 From 29dbe8aed582f833f2994daaaf7ab227f7a7cf45 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 22 Aug 2014 14:27:32 +0400 Subject: 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 (limited to 'src/modules/uavcan/module.mk') diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index 2b9729045..de411e1e2 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 000000000..41a1a5270 --- /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 000000000..7f23a0b8f --- /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 ba27d6c71..08fca73c5 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; } -- cgit v1.2.3 From 6870cd4d3d68941945d303b707c4b05bd5d1e6e4 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 22 Aug 2014 20:15:04 +0400 Subject: 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 (limited to 'src/modules/uavcan/module.mk') diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index de411e1e2..26ff7102d 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 000000000..3afcc3f1c --- /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 000000000..f6aa01216 --- /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 2bd662d5c..f826c8fd2 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; } -- cgit v1.2.3 From 6a8971e28f492073a951d96065df30034853bea7 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 23 Aug 2014 17:23:59 +0400 Subject: 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 (limited to 'src/modules/uavcan/module.mk') diff --git a/ROMFS/px4fmu_common/init.d/rc.uavcan b/ROMFS/px4fmu_common/init.d/rc.uavcan new file mode 100644 index 000000000..abb76b400 --- /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 c9e6a27ca..195771905 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 26ff7102d..93a1bf96f 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 8607af145..a15f83696 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; - - if (argc > 3) { - bitrate = atol(argv[3]); - } + // CAN bitrate + int32_t bitrate = 0; + (void)param_get(param_find("UAVCAN_BITRATE"), &bitrate); - 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 000000000..e6ea8a8fb --- /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); + + + -- cgit v1.2.3 From 4e0d7c6b0e52d3eecba65f4415d4c7372dfd8a49 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 23 Aug 2014 23:14:59 +0400 Subject: 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(-) (limited to 'src/modules/uavcan/module.mk') diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index 93a1bf96f..f92bc754f 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 3afcc3f1c..ef4f0dbba 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 f6aa01216..9d470219e 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 6b69d163f..f2bb28087 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 db3a515fa..9488c5fe5 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 4f8a5e104..aaa3a4463 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 4bc5129a2..6d413a8f7 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 5752d5524..05d589b58 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 976d9a03d..d27ccb8a0 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; +}; -- cgit v1.2.3