aboutsummaryrefslogtreecommitdiff
path: root/src/modules/uavcan
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/uavcan')
-rw-r--r--src/modules/uavcan/.gitignore1
-rw-r--r--src/modules/uavcan/actuators/esc.cpp138
-rw-r--r--src/modules/uavcan/actuators/esc.hpp107
-rw-r--r--src/modules/uavcan/module.mk79
-rw-r--r--src/modules/uavcan/sensors/baro.cpp117
-rw-r--r--src/modules/uavcan/sensors/baro.hpp68
-rw-r--r--src/modules/uavcan/sensors/gnss.cpp175
-rw-r--r--src/modules/uavcan/sensors/gnss.hpp87
-rw-r--r--src/modules/uavcan/sensors/mag.cpp123
-rw-r--r--src/modules/uavcan/sensors/mag.hpp68
-rw-r--r--src/modules/uavcan/sensors/sensor_bridge.cpp158
-rw-r--r--src/modules/uavcan/sensors/sensor_bridge.hpp131
-rw-r--r--src/modules/uavcan/uavcan_clock.cpp81
-rw-r--r--src/modules/uavcan/uavcan_main.cpp628
-rw-r--r--src/modules/uavcan/uavcan_main.hpp125
-rw-r--r--src/modules/uavcan/uavcan_params.c73
16 files changed, 2159 insertions, 0 deletions
diff --git a/src/modules/uavcan/.gitignore b/src/modules/uavcan/.gitignore
new file mode 100644
index 000000000..24fbf171f
--- /dev/null
+++ b/src/modules/uavcan/.gitignore
@@ -0,0 +1 @@
+./dsdlc_generated/
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 <pavel.kirienko@gmail.com>
+ */
+
+#include "esc.hpp"
+#include <systemlib/err.h>
+
+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<unsigned>(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<uavcan::equipment::esc::Status> &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 <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <uavcan/uavcan.hpp>
+#include <uavcan/equipment/esc/RawCommand.hpp>
+#include <uavcan/equipment/esc/Status.hpp>
+#include <systemlib/perf_counter.h>
+
+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<uavcan::equipment::esc::Status> &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<UavcanEscController*,
+ void (UavcanEscController::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status>&)>
+ StatusCbBinder;
+
+ typedef uavcan::MethodBinder<UavcanEscController*, void (UavcanEscController::*)(const uavcan::TimerEvent&)>
+ TimerCbBinder;
+
+ /*
+ * libuavcan related things
+ */
+ uavcan::MonotonicTime _prev_cmd_pub; ///< rate limiting
+ uavcan::INode &_node;
+ uavcan::Publisher<uavcan::equipment::esc::RawCommand> _uavcan_pub_raw_cmd;
+ uavcan::Subscriber<uavcan::equipment::esc::Status, StatusCbBinder> _uavcan_sub_status;
+ uavcan::TimerEventForwarder<TimerCbBinder> _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/module.mk b/src/modules/uavcan/module.mk
new file mode 100644
index 000000000..f92bc754f
--- /dev/null
+++ b/src/modules/uavcan/module.mk
@@ -0,0 +1,79 @@
+############################################################################
+#
+# Copyright (C) 2013 PX4 Development Team. All rights reserved.
+# Author: Pavel Kirienko <pavel.kirienko@gmail.com>
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+# notice, this list of conditions and the following disclaimer in
+# the documentation and/or other materials provided with the
+# distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+# used to endorse or promote products derived from this software
+# without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# UAVCAN <--> uORB bridge
+#
+
+MODULE_COMMAND = uavcan
+
+MAXOPTIMIZATION = -Os
+
+# Main
+SRCS += uavcan_main.cpp \
+ uavcan_clock.cpp \
+ uavcan_params.c
+
+# Actuators
+SRCS += actuators/esc.cpp
+
+# Sensors
+SRCS += sensors/sensor_bridge.cpp \
+ sensors/gnss.cpp \
+ sensors/mag.cpp \
+ sensors/baro.cpp
+
+#
+# libuavcan
+#
+include $(UAVCAN_DIR)/libuavcan/include.mk
+SRCS += $(LIBUAVCAN_SRC)
+INCLUDE_DIRS += $(LIBUAVCAN_INC)
+# Since actual compiler mode is C++11, the library will default to UAVCAN_CPP11, but it will fail to compile
+# because this platform lacks most of the standard library and STL. Hence we need to force C++03 mode.
+override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 -DUAVCAN_NO_ASSERTIONS
+
+#
+# libuavcan drivers for STM32
+#
+include $(UAVCAN_DIR)/libuavcan_drivers/stm32/driver/include.mk
+SRCS += $(LIBUAVCAN_STM32_SRC)
+INCLUDE_DIRS += $(LIBUAVCAN_STM32_INC)
+override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_STM32_NUTTX -DUAVCAN_STM32_NUM_IFACES=2
+
+#
+# Invoke DSDL compiler
+#
+$(info $(shell $(LIBUAVCAN_DSDLC) $(UAVCAN_DSDL_DIR)))
+INCLUDE_DIRS += dsdlc_generated
diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp
new file mode 100644
index 000000000..80c5e3828
--- /dev/null
+++ b/src/modules/uavcan/sensors/baro.cpp
@@ -0,0 +1,117 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @author Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include "baro.hpp"
+#include <cmath>
+
+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) :
+UavcanCDevSensorBridgeBase("uavcan_baro", "/dev/uavcan/baro", BARO_DEVICE_PATH, BARO_TOPICS),
+_sub_air_data(node)
+{
+}
+
+int UavcanBarometerBridge::init()
+{
+ 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;
+ }
+ 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<uavcan::equipment::air_data::StaticAirData> &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(msg.getSrcNodeID().get(), &report);
+}
diff --git a/src/modules/uavcan/sensors/baro.hpp b/src/modules/uavcan/sensors/baro.hpp
new file mode 100644
index 000000000..9d470219e
--- /dev/null
+++ b/src/modules/uavcan/sensors/baro.hpp
@@ -0,0 +1,68 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @author Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include "sensor_bridge.hpp"
+#include <drivers/drv_baro.h>
+
+#include <uavcan/equipment/air_data/StaticAirData.hpp>
+
+class UavcanBarometerBridge : public UavcanCDevSensorBridgeBase
+{
+public:
+ static const char *const NAME;
+
+ UavcanBarometerBridge(uavcan::INode& node);
+
+ 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<uavcan::equipment::air_data::StaticAirData> &msg);
+
+ typedef uavcan::MethodBinder<UavcanBarometerBridge*,
+ void (UavcanBarometerBridge::*)
+ (const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticAirData>&)>
+ AirDataCbBinder;
+
+ uavcan::Subscriber<uavcan::equipment::air_data::StaticAirData, AirDataCbBinder> _sub_air_data;
+ unsigned _msl_pressure = 101325;
+};
diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp
new file mode 100644
index 000000000..0d67aad47
--- /dev/null
+++ b/src/modules/uavcan/sensors/gnss.cpp
@@ -0,0 +1,175 @@
+/****************************************************************************
+ *
+ * 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 <pavel.kirienko@gmail.com>
+ * @author Andrew Chambers <achamber@gmail.com>
+ *
+ */
+
+#include "gnss.hpp"
+#include <systemlib/err.h>
+#include <mathlib/mathlib.h>
+
+#define MM_PER_CM 10 // Millimeters per centimeter
+
+const char *const UavcanGnssBridge::NAME = "gnss";
+
+UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) :
+_node(node),
+_sub_fix(node),
+_report_pub(-1)
+{
+}
+
+int UavcanGnssBridge::init()
+{
+ int res = _sub_fix.start(FixCbBinder(this, &UavcanGnssBridge::gnss_fix_sub_cb));
+ if (res < 0)
+ {
+ warnx("GNSS fix sub failed %i", res);
+ return res;
+ }
+ return res;
+}
+
+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<uavcan::equipment::gnss::Fix> &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.
+ }
+ }
+
+ auto report = ::vehicle_gps_position_s();
+
+ 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..e8466b401
--- /dev/null
+++ b/src/modules/uavcan/sensors/gnss.hpp
@@ -0,0 +1,87 @@
+/****************************************************************************
+ *
+ * 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 <pavel.kirienko@gmail.com>
+ * @author Andrew Chambers <achamber@gmail.com>
+ */
+
+#pragma once
+
+#include <drivers/drv_hrt.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_gps_position.h>
+
+#include <uavcan/uavcan.hpp>
+#include <uavcan/equipment/gnss/Fix.hpp>
+
+#include "sensor_bridge.hpp"
+
+class UavcanGnssBridge : public IUavcanSensorBridge
+{
+public:
+ static const char *const NAME;
+
+ UavcanGnssBridge(uavcan::INode& node);
+
+ const char *get_name() const override { return NAME; }
+
+ int init() override;
+
+ unsigned get_num_redundant_channels() const override;
+
+ void print_status() const override;
+
+private:
+ /**
+ * GNSS fix message will be reported via this callback.
+ */
+ void gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg);
+
+ typedef uavcan::MethodBinder<UavcanGnssBridge*,
+ void (UavcanGnssBridge::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix>&)>
+ FixCbBinder;
+
+ uavcan::INode &_node;
+ uavcan::Subscriber<uavcan::equipment::gnss::Fix, FixCbBinder> _sub_fix;
+ int _receiver_node_id = -1;
+
+ 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
new file mode 100644
index 000000000..8e6a9a22f
--- /dev/null
+++ b/src/modules/uavcan/sensors/mag.cpp
@@ -0,0 +1,123 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @author Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include "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) :
+UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_DEVICE_PATH, MAG_TOPICS),
+_sub_mag(node)
+{
+ _scale.x_scale = 1.0F;
+ _scale.y_scale = 1.0F;
+ _scale.z_scale = 1.0F;
+}
+
+int UavcanMagnetometerBridge::init()
+{
+ int res = device::CDev::init();
+ if (res < 0) {
+ return res;
+ }
+
+ res = _sub_mag.start(MagCbBinder(this, &UavcanMagnetometerBridge::mag_sub_cb));
+ if (res < 0) {
+ log("failed to start uavcan sub: %d", res);
+ return res;
+ }
+ return 0;
+}
+
+int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+ switch (cmd) {
+ case MAGIOCSSCALE: {
+ std::memcpy(&_scale, reinterpret_cast<const void*>(arg), sizeof(_scale));
+ return 0;
+ }
+ case MAGIOCGSCALE: {
+ std::memcpy(reinterpret_cast<void*>(arg), &_scale, sizeof(_scale));
+ return 0;
+ }
+ case MAGIOCSELFTEST: {
+ return 0; // Nothing to do
+ }
+ case MAGIOCGEXTERNAL: {
+ 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);
+ }
+ }
+}
+
+void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::Magnetometer> &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.y_offset) * _scale.y_scale;
+ report.z = (msg.magnetic_field[2] - _scale.z_offset) * _scale.z_scale;
+
+ publish(msg.getSrcNodeID().get(), &report);
+}
diff --git a/src/modules/uavcan/sensors/mag.hpp b/src/modules/uavcan/sensors/mag.hpp
new file mode 100644
index 000000000..6d413a8f7
--- /dev/null
+++ b/src/modules/uavcan/sensors/mag.hpp
@@ -0,0 +1,68 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @author Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include "sensor_bridge.hpp"
+#include <drivers/drv_mag.h>
+
+#include <uavcan/equipment/ahrs/Magnetometer.hpp>
+
+class UavcanMagnetometerBridge : public UavcanCDevSensorBridgeBase
+{
+public:
+ static const char *const NAME;
+
+ UavcanMagnetometerBridge(uavcan::INode& node);
+
+ const char *get_name() const override { return NAME; }
+
+ int init() override;
+
+private:
+ int ioctl(struct file *filp, int cmd, unsigned long arg) override;
+
+ void mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::Magnetometer> &msg);
+
+ typedef uavcan::MethodBinder<UavcanMagnetometerBridge*,
+ void (UavcanMagnetometerBridge::*)
+ (const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::Magnetometer>&)>
+ MagCbBinder;
+
+ uavcan::Subscriber<uavcan::equipment::ahrs::Magnetometer, MagCbBinder> _sub_mag;
+ mag_scale _scale = {};
+};
diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp
new file mode 100644
index 000000000..9608ce680
--- /dev/null
+++ b/src/modules/uavcan/sensors/sensor_bridge.cpp
@@ -0,0 +1,158 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @author Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include "sensor_bridge.hpp"
+#include <cassert>
+
+#include "gnss.hpp"
+#include "mag.hpp"
+#include "baro.hpp"
+
+/*
+ * IUavcanSensorBridge
+ */
+void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge*> &list)
+{
+ list.add(new UavcanBarometerBridge(node));
+ list.add(new UavcanMagnetometerBridge(node));
+ list.add(new UavcanGnssBridge(node));
+}
+
+/*
+ * UavcanCDevSensorBridgeBase
+ */
+UavcanCDevSensorBridgeBase::~UavcanCDevSensorBridgeBase()
+{
+ for (unsigned i = 0; i < _max_channels; i++) {
+ if (_channels[i].node_id >= 0) {
+ (void)unregister_class_devname(_class_devname, _channels[i].class_instance);
+ }
+ }
+ delete [] _orb_topics;
+ delete [] _channels;
+}
+
+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].node_id == node_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...", node_id);
+
+ // Search for the first free channel
+ for (unsigned i = 0; i < _max_channels; i++) {
+ if (_channels[i].node_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->node_id = node_id;
+ channel->class_instance = class_instance;
+
+ 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;
+ }
+
+ log("channel %d class instance %d ok", channel->node_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].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
new file mode 100644
index 000000000..1316f7ecc
--- /dev/null
+++ b/src/modules/uavcan/sensors/sensor_bridge.hpp
@@ -0,0 +1,131 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @author Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <containers/List.hpp>
+#include <uavcan/uavcan.hpp>
+#include <drivers/device/device.h>
+#include <drivers/drv_orb_dev.h>
+
+/**
+ * A sensor bridge class must implement this interface.
+ */
+class IUavcanSensorBridge : uavcan::Noncopyable, public ListNode<IUavcanSensorBridge*>
+{
+public:
+ static constexpr unsigned MAX_NAME_LEN = 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;
+
+ /**
+ * Returns number of active redundancy channels.
+ */
+ 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".
+ * @return nullptr if such bridge can't be created.
+ */
+ static void make_all(uavcan::INode &node, List<IUavcanSensorBridge*> &list);
+};
+
+/**
+ * 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 node_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 <unsigned MaxChannels>
+ 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 node_id Sensor's Node ID
+ * @param report Pointer to ORB message object
+ */
+ 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_clock.cpp b/src/modules/uavcan/uavcan_clock.cpp
new file mode 100644
index 000000000..fe8ba406a
--- /dev/null
+++ b/src/modules/uavcan/uavcan_clock.cpp
@@ -0,0 +1,81 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <uavcan_stm32/uavcan_stm32.hpp>
+#include <drivers/drv_hrt.h>
+
+/**
+ * @file uavcan_clock.cpp
+ *
+ * Implements a clock for the CAN node.
+ *
+ * @author Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+namespace uavcan_stm32
+{
+namespace clock
+{
+
+uavcan::MonotonicTime getMonotonic()
+{
+ return uavcan::MonotonicTime::fromUSec(hrt_absolute_time());
+}
+
+uavcan::UtcTime getUtc()
+{
+ return uavcan::UtcTime();
+}
+
+void adjustUtc(uavcan::UtcDuration adjustment)
+{
+ (void)adjustment;
+}
+
+uavcan::uint64_t getUtcUSecFromCanInterrupt();
+
+uavcan::uint64_t getUtcUSecFromCanInterrupt()
+{
+ return 0;
+}
+
+} // namespace clock
+
+SystemClock &SystemClock::instance()
+{
+ static SystemClock inst;
+ return inst;
+}
+
+}
+
diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp
new file mode 100644
index 000000000..a8485ee44
--- /dev/null
+++ b/src/modules/uavcan/uavcan_main.cpp
@@ -0,0 +1,628 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <cstdlib>
+#include <cstring>
+#include <fcntl.h>
+#include <systemlib/err.h>
+#include <systemlib/systemlib.h>
+#include <systemlib/param/param.h>
+#include <systemlib/mixer/mixer.h>
+#include <systemlib/board_serial.h>
+#include <systemlib/scheduling_priorities.h>
+#include <version/version.h>
+#include <arch/board/board.h>
+#include <arch/chip/chip.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_pwm_output.h>
+
+#include "uavcan_main.hpp"
+
+/**
+ * @file uavcan_main.cpp
+ *
+ * Implements basic functinality of UAVCAN node.
+ *
+ * @author Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+/*
+ * UavcanNode
+ */
+UavcanNode *UavcanNode::_instance;
+
+UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) :
+ CDev("uavcan", UAVCAN_DEVICE_PATH),
+ _node(can_driver, system_clock),
+ _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);
+
+ const int res = pthread_mutex_init(&_node_mutex, nullptr);
+ if (res < 0) {
+ std::abort();
+ }
+}
+
+UavcanNode::~UavcanNode()
+{
+ if (_task != -1) {
+ /* tell the task we want it to go away */
+ _task_should_exit = true;
+
+ unsigned i = 10;
+
+ do {
+ /* wait 5ms - it should wake every 10ms or so worst-case */
+ usleep(5000);
+
+ /* if we have given up, kill it */
+ if (--i == 0) {
+ task_delete(_task);
+ break;
+ }
+
+ } while (_task != -1);
+ }
+
+ /* clean up the alternate device node */
+ // unregister_driver(PWM_OUTPUT_DEVICE_PATH);
+
+ ::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;
+}
+
+int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
+{
+ if (_instance != nullptr) {
+ warnx("Already started");
+ return -1;
+ }
+
+ /*
+ * GPIO config.
+ * Forced pull up on CAN2 is required for Pixhawk v1 where the second interface lacks a transceiver.
+ * If no transceiver is connected, the RX pin will float, occasionally causing CAN controller to
+ * fail during initialization.
+ */
+ stm32_configgpio(GPIO_CAN1_RX);
+ stm32_configgpio(GPIO_CAN1_TX);
+ stm32_configgpio(GPIO_CAN2_RX | GPIO_PULLUP);
+ stm32_configgpio(GPIO_CAN2_TX);
+
+ /*
+ * CAN driver init
+ */
+ static CanInitHelper can;
+ static bool can_initialized = false;
+
+ if (!can_initialized) {
+ const int can_init_res = can.init(bitrate);
+
+ if (can_init_res < 0) {
+ warnx("CAN driver init failed %i", can_init_res);
+ return can_init_res;
+ }
+
+ can_initialized = true;
+ }
+
+ /*
+ * Node init
+ */
+ _instance = new UavcanNode(can.driver, uavcan_stm32::SystemClock::instance());
+
+ if (_instance == nullptr) {
+ warnx("Out of memory");
+ return -1;
+ }
+
+ const int node_init_res = _instance->init(node_id);
+
+ if (node_init_res < 0) {
+ delete _instance;
+ _instance = nullptr;
+ warnx("Node init failed %i", node_init_res);
+ return node_init_res;
+ }
+
+ /*
+ * Start the task. Normally it should never exit.
+ */
+ static auto run_trampoline = [](int, char *[]) {return UavcanNode::_instance->run();};
+ _instance->_task = task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_ACTUATOR_OUTPUTS, StackSize,
+ static_cast<main_t>(run_trampoline), nullptr);
+
+ if (_instance->_task < 0) {
+ warnx("start failed: %d", errno);
+ return -errno;
+ }
+
+ return OK;
+}
+
+void UavcanNode::fill_node_info()
+{
+ /* software version */
+ uavcan::protocol::SoftwareVersion swver;
+
+ // Extracting the first 8 hex digits of FW_GIT and converting them to int
+ char fw_git_short[9] = {};
+ std::memmove(fw_git_short, FW_GIT, 8);
+ assert(fw_git_short[8] == '\0');
+ char *end = nullptr;
+ swver.vcs_commit = std::strtol(fw_git_short, &end, 16);
+ swver.optional_field_mask |= swver.OPTIONAL_FIELD_MASK_VCS_COMMIT;
+
+ warnx("SW version vcs_commit: 0x%08x", unsigned(swver.vcs_commit));
+
+ _node.setSoftwareVersion(swver);
+
+ /* hardware version */
+ uavcan::protocol::HardwareVersion hwver;
+
+ if (!std::strncmp(HW_ARCH, "PX4FMU_V1", 9)) {
+ hwver.major = 1;
+ } else if (!std::strncmp(HW_ARCH, "PX4FMU_V2", 9)) {
+ hwver.major = 2;
+ } else {
+ ; // All other values of HW_ARCH resolve to zero
+ }
+
+ uint8_t udid[12] = {}; // Someone seems to love magic numbers
+ get_board_serial(udid);
+ uavcan::copy(udid, udid + sizeof(udid), hwver.unique_id.begin());
+
+ _node.setHardwareVersion(hwver);
+}
+
+int UavcanNode::init(uavcan::NodeID node_id)
+{
+ int ret = -1;
+
+ // Do regular cdev init
+ ret = CDev::init();
+
+ if (ret != OK) {
+ return ret;
+ }
+
+ _node.setName("org.pixhawk.pixhawk");
+
+ _node.setNodeID(node_id);
+
+ fill_node_info();
+
+ // Actuators
+ ret = _esc_controller.init();
+ 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();
+}
+
+void UavcanNode::node_spin_once()
+{
+ const int spin_res = _node.spin(uavcan::MonotonicTime());
+ if (spin_res < 0) {
+ warnx("node spin error %i", spin_res);
+ }
+}
+
+int UavcanNode::run()
+{
+ (void)pthread_mutex_lock(&_node_mutex);
+
+ const unsigned PollTimeoutMs = 50;
+
+ // XXX figure out the output count
+ _output_count = 2;
+
+ _armed_sub = orb_subscribe(ORB_ID(actuator_armed));
+
+ actuator_outputs_s outputs;
+ memset(&outputs, 0, sizeof(outputs));
+
+ const int busevent_fd = ::open(uavcan_stm32::BusEvent::DevName, 0);
+ if (busevent_fd < 0)
+ {
+ warnx("Failed to open %s", uavcan_stm32::BusEvent::DevName);
+ _task_should_exit = true;
+ }
+
+ /*
+ * XXX Mixing logic/subscriptions shall be moved into UavcanEscController::update();
+ * IO multiplexing shall be done here.
+ */
+
+ _node.setStatusOk();
+
+ /*
+ * This event is needed to wake up the thread on CAN bus activity (RX/TX/Error).
+ * Please note that with such multiplexing it is no longer possible to rely only on
+ * the value returned from poll() to detect whether actuator control has timed out or not.
+ * Instead, all ORB events need to be checked individually (see below).
+ */
+ _poll_fds_num = 0;
+ _poll_fds[_poll_fds_num] = ::pollfd();
+ _poll_fds[_poll_fds_num].fd = busevent_fd;
+ _poll_fds[_poll_fds_num].events = POLLIN;
+ _poll_fds_num += 1;
+
+ while (!_task_should_exit) {
+ // update actuator controls subscriptions if needed
+ if (_groups_subscribed != _groups_required) {
+ subscribe();
+ _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...
+ if (poll_ret < 0) {
+ log("poll error %d", errno);
+ continue;
+ } else {
+ // get controls for required topics
+ bool controls_updated = false;
+ unsigned poll_id = 1;
+ for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+ if (_control_subs[i] > 0) {
+ if (_poll_fds[poll_id].revents & POLLIN) {
+ controls_updated = true;
+ orb_copy(_control_topics[i], _control_subs[i], &_controls[i]);
+ }
+ poll_id++;
+ }
+ }
+
+ // can we mix?
+ if (controls_updated && (_mixers != nullptr)) {
+
+ // XXX one output group has 8 outputs max,
+ // but this driver could well serve multiple groups.
+ unsigned num_outputs_max = 8;
+
+ // Do mixing
+ outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs_max);
+ outputs.timestamp = hrt_absolute_time();
+
+ // iterate actuators
+ for (unsigned i = 0; i < outputs.noutputs; i++) {
+ // last resort: catch NaN, INF and out-of-band errors
+ if (!isfinite(outputs.output[i])) {
+ /*
+ * Value is NaN, INF or out of band - set to the minimum value.
+ * This will be clearly visible on the servo status and will limit the risk of accidentally
+ * spinning motors. It would be deadly in flight.
+ */
+ outputs.output[i] = -1.0f;
+ }
+
+ // limit outputs to valid range
+
+ // never go below min
+ if (outputs.output[i] < -1.0f) {
+ outputs.output[i] = -1.0f;
+ }
+
+ // never go below max
+ if (outputs.output[i] > 1.0f) {
+ outputs.output[i] = 1.0f;
+ }
+ }
+
+ // Output to the bus
+ _esc_controller.update_outputs(outputs.output, outputs.noutputs);
+ }
+ }
+
+ // Check arming state
+ bool updated = false;
+ orb_check(_armed_sub, &updated);
+
+ if (updated) {
+ orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
+
+ // Update the armed status and check that we're not locked down
+ bool set_armed = _armed.armed && !_armed.lockdown;
+
+ arm_actuators(set_armed);
+ }
+ }
+
+ teardown();
+ warnx("exiting.");
+
+ exit(0);
+}
+
+int
+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;
+
+ input = controls[control_group].control[control_index];
+ return 0;
+}
+
+int
+UavcanNode::teardown()
+{
+ for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+ if (_control_subs[i] > 0) {
+ ::close(_control_subs[i]);
+ _control_subs[i] = -1;
+ }
+ }
+ return ::close(_armed_sub);
+}
+
+int
+UavcanNode::arm_actuators(bool arm)
+{
+ _is_armed = arm;
+ _esc_controller.arm_esc(arm);
+ return OK;
+}
+
+void
+UavcanNode::subscribe()
+{
+ // Subscribe/unsubscribe to required actuator control groups
+ uint32_t sub_groups = _groups_required & ~_groups_subscribed;
+ uint32_t unsub_groups = _groups_subscribed & ~_groups_required;
+ // the first fd used by CAN
+ _poll_fds_num = 1;
+ for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+ if (sub_groups & (1 << i)) {
+ warnx("subscribe to actuator_controls_%d", i);
+ _control_subs[i] = orb_subscribe(_control_topics[i]);
+ }
+ if (unsub_groups & (1 << i)) {
+ warnx("unsubscribe from actuator_controls_%d", i);
+ ::close(_control_subs[i]);
+ _control_subs[i] = -1;
+ }
+
+ if (_control_subs[i] > 0) {
+ _poll_fds[_poll_fds_num].fd = _control_subs[i];
+ _poll_fds[_poll_fds_num].events = POLLIN;
+ _poll_fds_num++;
+ }
+ }
+}
+
+int
+UavcanNode::ioctl(file *filp, int cmd, unsigned long arg)
+{
+ int ret = OK;
+
+ lock();
+
+ switch (cmd) {
+ case PWM_SERVO_ARM:
+ arm_actuators(true);
+ break;
+
+ case PWM_SERVO_SET_ARM_OK:
+ case PWM_SERVO_CLEAR_ARM_OK:
+ case PWM_SERVO_SET_FORCE_SAFETY_OFF:
+ // these are no-ops, as no safety switch
+ break;
+
+ case PWM_SERVO_DISARM:
+ arm_actuators(false);
+ break;
+
+ case MIXERIOCGETOUTPUTCOUNT:
+ *(unsigned *)arg = _output_count;
+ break;
+
+ case MIXERIOCRESET:
+ if (_mixers != nullptr) {
+ delete _mixers;
+ _mixers = nullptr;
+ _groups_required = 0;
+ }
+
+ break;
+
+ case MIXERIOCLOADBUF: {
+ const char *buf = (const char *)arg;
+ unsigned buflen = strnlen(buf, 1024);
+
+ if (_mixers == nullptr)
+ _mixers = new MixerGroup(control_callback, (uintptr_t)_controls);
+
+ if (_mixers == nullptr) {
+ _groups_required = 0;
+ ret = -ENOMEM;
+
+ } else {
+
+ ret = _mixers->load_from_buf(buf, buflen);
+
+ if (ret != 0) {
+ warnx("mixer load failed with %d", ret);
+ delete _mixers;
+ _mixers = nullptr;
+ _groups_required = 0;
+ ret = -EINVAL;
+ } else {
+
+ _mixers->groups_required(_groups_required);
+ }
+ }
+
+ break;
+ }
+
+ default:
+ ret = -ENOTTY;
+ break;
+ }
+
+ unlock();
+
+ if (ret == -ENOTTY) {
+ ret = CDev::ioctl(filp, cmd, arg);
+ }
+
+ return ret;
+}
+
+void
+UavcanNode::print_info()
+{
+ if (!_instance) {
+ warnx("not running, start first");
+ }
+
+ (void)pthread_mutex_lock(&_node_mutex);
+
+ // ESC mixer status
+ 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) {
+ printf("Sensor '%s':\n", br->get_name());
+ br->print_status();
+ printf("\n");
+ br = br->getSibling();
+ }
+
+ (void)pthread_mutex_unlock(&_node_mutex);
+}
+
+/*
+ * App entry point
+ */
+static void print_usage()
+{
+ warnx("usage: \n"
+ "\tuavcan {start|status|stop}");
+}
+
+extern "C" __EXPORT int uavcan_main(int argc, char *argv[]);
+
+int uavcan_main(int argc, char *argv[])
+{
+ if (argc < 2) {
+ print_usage();
+ ::exit(1);
+ }
+
+ if (!std::strcmp(argv[1], "start")) {
+ if (UavcanNode::instance()) {
+ errx(1, "already started");
+ }
+
+ // 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
+ int32_t bitrate = 0;
+ (void)param_get(param_find("UAVCAN_BITRATE"), &bitrate);
+
+ // Start
+ warnx("Node ID %u, bitrate %u", node_id, bitrate);
+ return UavcanNode::start(node_id, bitrate);
+ }
+
+ /* commands below require the app to be started */
+ UavcanNode *const inst = UavcanNode::instance();
+
+ if (!inst) {
+ errx(1, "application not running");
+ }
+
+ if (!std::strcmp(argv[1], "status") || !std::strcmp(argv[1], "info")) {
+ inst->print_info();
+ ::exit(0);
+ }
+
+ if (!std::strcmp(argv[1], "stop")) {
+ delete inst;
+ ::exit(0);
+ }
+
+ print_usage();
+ ::exit(1);
+}
diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp
new file mode 100644
index 000000000..be7db9741
--- /dev/null
+++ b/src/modules/uavcan/uavcan_main.hpp
@@ -0,0 +1,125 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <nuttx/config.h>
+
+#include <uavcan_stm32/uavcan_stm32.hpp>
+#include <drivers/device/device.h>
+
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/actuator_armed.h>
+
+#include "actuators/esc.hpp"
+#include "sensors/sensor_bridge.hpp"
+
+/**
+ * @file uavcan_main.hpp
+ *
+ * Defines basic functinality of UAVCAN node.
+ *
+ * @author Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#define NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN 4
+#define UAVCAN_DEVICE_PATH "/dev/uavcan/esc"
+
+/**
+ * A UAVCAN node.
+ */
+class UavcanNode : public device::CDev
+{
+ static constexpr unsigned MemPoolSize = 10752;
+ static constexpr unsigned RxQueueLenPerIface = 64;
+ static constexpr unsigned StackSize = 3000;
+
+public:
+ typedef uavcan::Node<MemPoolSize> Node;
+ typedef uavcan_stm32::CanInitHelper<RxQueueLenPerIface> CanInitHelper;
+
+ UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock);
+
+ virtual ~UavcanNode();
+
+ virtual int ioctl(file *filp, int cmd, unsigned long arg);
+
+ static int start(uavcan::NodeID node_id, uint32_t bitrate);
+
+ Node& get_node() { return _node; }
+
+ // 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();
+
+ int teardown();
+ int arm_actuators(bool arm);
+
+ void print_info();
+
+ static UavcanNode* instance() { return _instance; }
+
+private:
+ void fill_node_info();
+ int init(uavcan::NodeID node_id);
+ void node_spin_once();
+ int run();
+
+ int _task = -1; ///< handle to the OS task
+ bool _task_should_exit = false; ///< flag to indicate to tear down the CAN driver
+ int _armed_sub = -1; ///< uORB subscription of the arming status
+ actuator_armed_s _armed; ///< the arming request of the system
+ bool _is_armed = false; ///< the arming status of the actuators on the bus
+
+ unsigned _output_count = 0; ///< number of actuators currently available
+
+ static UavcanNode *_instance; ///< singleton pointer
+ Node _node; ///< library instance
+ pthread_mutex_t _node_mutex;
+
+ UavcanEscController _esc_controller;
+
+ List<IUavcanSensorBridge*> _sensor_bridges; ///< List of active sensor bridges
+
+ MixerGroup *_mixers = nullptr;
+
+ uint32_t _groups_required = 0;
+ uint32_t _groups_subscribed = 0;
+ int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {};
+ actuator_controls_s _controls[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {};
+ orb_id_t _control_topics[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {};
+ pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN + 1] = {}; ///< +1 for /dev/uavcan/busevent
+ unsigned _poll_fds_num = 0;
+};
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 <pavel.kirienko@gmail.com>
+ */
+
+#include <nuttx/config.h>
+#include <systemlib/param/param.h>
+
+/**
+ * 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);
+
+
+
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-16 18:07:54 +0000