diff options
| -rw-r--r-- | makefiles/config_px4cannode-v1_default.mk | 26 | ||||
| -rw-r--r-- | nuttx-configs/px4cannode-v1/nsh/defconfig | 19 | ||||
| -rw-r--r-- | nuttx-configs/px4cannode-v1/scripts/ld.script | 12 | ||||
| -rw-r--r-- | src/modules/systemlib/param/param.c | 2 | ||||
| -rw-r--r-- | src/modules/uavcannode/.gitignore | 1 | ||||
| -rw-r--r-- | src/modules/uavcannode/module.mk | 75 | ||||
| -rw-r--r-- | src/modules/uavcannode/uavcannode_clock.cpp | 81 | ||||
| -rw-r--r-- | src/modules/uavcannode/uavcannode_main.cpp | 405 | ||||
| -rw-r--r-- | src/modules/uavcannode/uavcannode_main.hpp | 101 | ||||
| -rw-r--r-- | src/modules/uavcannode/uavcannode_params.c | 73 |
10 files changed, 779 insertions, 16 deletions
diff --git a/makefiles/config_px4cannode-v1_default.mk b/makefiles/config_px4cannode-v1_default.mk index 913b6eb53..de3902877 100644 --- a/makefiles/config_px4cannode-v1_default.mk +++ b/makefiles/config_px4cannode-v1_default.mk @@ -16,22 +16,46 @@ MODULES += drivers/boards/px4cannode-v1 MODULES += systemcmds/reboot MODULES += systemcmds/top MODULES += systemcmds/config -MODULES += systemcmds/nshterm MODULES += systemcmds/ver # +# General system control +# +MODULES += modules/uavcannode + +# # Library modules # MODULES += modules/systemlib + # # Unit tests # #MODULES += modules/unit_test #MODULES += modules/commander/commander_tests +# Generate parameter XML file +GEN_PARAM_XML = 1 + + # # Demo apps # +# +# Transitional support - add commands from the NuttX export archive. +# +# In general, these should move to modules over time. +# +# Each entry here is <command>.<priority>.<stacksize>.<entrypoint> but we use a helper macro +# to make the table a bit more readable. +# +define _B + $(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4) +endef + +BUILTIN_COMMANDS := \ + $(call _B, null, , 60, null_main) + diff --git a/nuttx-configs/px4cannode-v1/nsh/defconfig b/nuttx-configs/px4cannode-v1/nsh/defconfig index f03f8c324..1599c8b5b 100644 --- a/nuttx-configs/px4cannode-v1/nsh/defconfig +++ b/nuttx-configs/px4cannode-v1/nsh/defconfig @@ -24,7 +24,7 @@ CONFIG_BUILD_FLAT=y # Binary Output Formats # # CONFIG_RRLOAD_BINARY is not set -CONFIG_INTELHEX_BINARY=y +# CONFIG_INTELHEX_BINARY is not set # CONFIG_MOTOROLA_SREC is not set CONFIG_RAW_BINARY=y # CONFIG_UBOOT_UIMAGE is not set @@ -152,11 +152,6 @@ CONFIG_ARMV7M_TOOLCHAIN_CODESOURCERYL=y # CONFIG_ARMV7M_TOOLCHAIN_GNU_EABIL is not set # CONFIG_ARMV7M_STACKCHECK is not set # CONFIG_ARMV7M_ITMSYSLOG is not set -CONFIG_CAN_EXTID=y -CONFIG_CAN1_BAUD=250000 -CONFIG_CAN_TSEG1=6 -CONFIG_CAN_TSEG2=7 -CONFIG_CAN_LOOPBACK=y # # STM32 Configuration Options @@ -329,7 +324,7 @@ CONFIG_STM32_HAVE_SPI3=y # CONFIG_STM32_ADC2 is not set # CONFIG_STM32_ADC3 is not set # CONFIG_STM32_BKP is not set -CONFIG_STM32_CAN1=y +# CONFIG_STM32_CAN1 is not set # CONFIG_STM32_CRC is not set CONFIG_STM32_DMA1=y CONFIG_STM32_DMA2=y @@ -359,7 +354,7 @@ CONFIG_STM32_USART1=y # CONFIG_STM32_IWDG is not set # CONFIG_STM32_WWDG is not set CONFIG_STM32_SPI=y -CONFIG_STM32_CAN=y +# CONFIG_STM32_CAN is not set # # Alternate Pin Mapping @@ -615,9 +610,7 @@ CONFIG_PTHREAD_STACK_DEFAULT=464 # CONFIG_DRVR_WRITEBUFFER is not set # CONFIG_DRVR_READAHEAD is not set # CONFIG_RAMDISK is not set -CONFIG_CAN=y -CONFIG_CAN_FIFOSIZE=8 -CONFIG_CAN_NPENDINGRTR=4 +# CONFIG_CAN is not set # CONFIG_ARCH_HAVE_PWM_PULSECOUNT is not set # CONFIG_PWM is not set CONFIG_ARCH_HAVE_I2CRESET=y @@ -844,7 +837,7 @@ CONFIG_BUILTIN_PROXY_STACKSIZE=768 # Examples # # CONFIG_EXAMPLES_BUTTONS is not set -CONFIG_EXAMPLES_CAN=y +# CONFIG_EXAMPLES_CAN is not set # CONFIG_EXAMPLES_CONFIGDATA is not set # CONFIG_EXAMPLES_CPUHOG is not set # CONFIG_EXAMPLES_CXXTEST is not set @@ -864,7 +857,7 @@ CONFIG_EXAMPLES_CAN=y # CONFIG_EXAMPLES_NRF24L01TERM is not set CONFIG_EXAMPLES_NSH=y # CONFIG_EXAMPLES_NSH_CXXINITIALIZE is not set -# CONFIG_EXAMPLES_NULL is not set +CONFIG_EXAMPLES_NULL=y # CONFIG_EXAMPLES_NX is not set # CONFIG_EXAMPLES_NXTERM is not set # CONFIG_EXAMPLES_NXFFS is not set diff --git a/nuttx-configs/px4cannode-v1/scripts/ld.script b/nuttx-configs/px4cannode-v1/scripts/ld.script index 29719d454..f058f647c 100644 --- a/nuttx-configs/px4cannode-v1/scripts/ld.script +++ b/nuttx-configs/px4cannode-v1/scripts/ld.script @@ -47,8 +47,9 @@ MEMORY } OUTPUT_ARCH(arm) + ENTRY(__start) /* treat __start as the anchor for dead code stripping */ -EXTERN(_vectors) /* force the vectors to be included in the output */ +EXTERN(_vectors) /* force the vectors to be included in the output */ /* * Ensure that abort() is present in the final object. The exception handling * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). @@ -84,6 +85,15 @@ SECTIONS _einit = ABSOLUTE(.); } > flash + /* + * Construction data for parameters. + */ + __param ALIGN(4): { + __param_start = ABSOLUTE(.); + KEEP(*(__param)) + __param_end = ABSOLUTE(.); + } > flash + .ARM.extab : { *(.ARM.extab*) } > flash diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index d0d24960e..d2ed3d33e 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -75,7 +75,7 @@ extern struct param_info_s *param_info_base; extern struct param_info_s *param_info_limit; #else - extern char __param_start, __param_end; + __EXPORT char __param_start, __param_end; static const struct param_info_s *param_info_base = (struct param_info_s *) &__param_start; static const struct param_info_s *param_info_limit = (struct param_info_s *) &__param_end; #endif diff --git a/src/modules/uavcannode/.gitignore b/src/modules/uavcannode/.gitignore new file mode 100644 index 000000000..24fbf171f --- /dev/null +++ b/src/modules/uavcannode/.gitignore @@ -0,0 +1 @@ +./dsdlc_generated/ diff --git a/src/modules/uavcannode/module.mk b/src/modules/uavcannode/module.mk new file mode 100644 index 000000000..8e76fe89c --- /dev/null +++ b/src/modules/uavcannode/module.mk @@ -0,0 +1,75 @@ +############################################################################ +# +# Copyright (c) 2013-2015 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 <--> som Simple Sensor +# + +MODULE_COMMAND = uavcannode + +MAXOPTIMIZATION = -O3 + +# Main +SRCS += uavcannode_main.cpp \ + uavcannode_clock.cpp \ + uavcannode_params.c + + +# Sensors + +# +# libuavcan +# +include $(PX4_LIB_DIR)uavcan/libuavcan/include.mk +# Use the relitive path to keep the genrated files in the BUILD_DIR +SRCS += $(subst $(PX4_MODULE_SRC),../../,$(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 $(PX4_LIB_DIR)uavcan/libuavcan_drivers/stm32/driver/include.mk +# Use the relitive path to keep the genrated files in the BUILD_DIR +SRCS += $(subst $(PX4_MODULE_SRC),../../,$(LIBUAVCAN_STM32_SRC)) +INCLUDE_DIRS += $(LIBUAVCAN_STM32_INC) +override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_STM32_NUTTX -DUAVCAN_STM32_NUM_IFACES=1 + +# +# Invoke DSDL compiler +# +$(info $(shell $(LIBUAVCAN_DSDLC) $(UAVCAN_DSDL_DIR))) +INCLUDE_DIRS += dsdlc_generated diff --git a/src/modules/uavcannode/uavcannode_clock.cpp b/src/modules/uavcannode/uavcannode_clock.cpp new file mode 100644 index 000000000..fe8ba406a --- /dev/null +++ b/src/modules/uavcannode/uavcannode_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/uavcannode/uavcannode_main.cpp b/src/modules/uavcannode/uavcannode_main.cpp new file mode 100644 index 000000000..5809184b0 --- /dev/null +++ b/src/modules/uavcannode/uavcannode_main.cpp @@ -0,0 +1,405 @@ +/**************************************************************************** + * + * + * 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 "uavcannode_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() +{ + 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); + } + + _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); +#if defined(GPIO_CAN2_RX) + stm32_configgpio(GPIO_CAN2_RX | GPIO_PULLUP); + stm32_configgpio(GPIO_CAN2_TX); +#endif + /* + * 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; + + hwver.major = 1; + + 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.cannode"); + + _node.setNodeID(node_id); + + fill_node_info(); + + 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); + } +} + +/* + add a fd to the list of polled events. This assumes you want + POLLIN for now. + */ +int UavcanNode::add_poll_fd(int fd) +{ + int ret = _poll_fds_num; + if (_poll_fds_num >= UAVCAN_NUM_POLL_FDS) { + errx(1, "uavcan: too many poll fds, exiting"); + } + _poll_fds[_poll_fds_num] = ::pollfd(); + _poll_fds[_poll_fds_num].fd = fd; + _poll_fds[_poll_fds_num].events = POLLIN; + _poll_fds_num += 1; + return ret; +} + + +int UavcanNode::run() +{ + (void)pthread_mutex_lock(&_node_mutex); + + const unsigned PollTimeoutMs = 50; + + 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; + } + + + _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). + */ + add_poll_fd(busevent_fd); + + while (!_task_should_exit) { + // 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 { + // Do Somthing + } + + } + + teardown(); + warnx("exiting."); + + exit(0); +} + +int +UavcanNode::teardown() +{ + return 0; +} + + + +int +UavcanNode::ioctl(file *filp, int cmd, unsigned long arg) +{ + int ret = OK; + + lock(); + + switch (cmd) { + + + + 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); + + + (void)pthread_mutex_unlock(&_node_mutex); +} + +/* + * App entry point + */ +static void print_usage() +{ + warnx("usage: \n" + "\tuavcan {start|status|stop|arm|disarm}"); +} + +extern "C" __EXPORT int uavcannode_main(int argc, char *argv[]); + +int uavcannode_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/uavcannode/uavcannode_main.hpp b/src/modules/uavcannode/uavcannode_main.hpp new file mode 100644 index 000000000..4b1ee6efe --- /dev/null +++ b/src/modules/uavcannode/uavcannode_main.hpp @@ -0,0 +1,101 @@ +/**************************************************************************** + * + * Copyright (c) 2015 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> + +/** + * @file uavcan_main.hpp + * + * Defines basic functinality of UAVCAN node. + * + * @author Pavel Kirienko <pavel.kirienko@gmail.com> + */ + +#define NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN 1 +#define UAVCAN_DEVICE_PATH "/dev/uavcan/node" + +// we add two to allow for actuator_direct and busevent +#define UAVCAN_NUM_POLL_FDS (NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN+2) + +/** + * A UAVCAN node. + */ +class UavcanNode : public device::CDev +{ + static constexpr unsigned MemPoolSize = 8092; ///< Refer to the libuavcan manual to learn why + static constexpr unsigned RxQueueLenPerIface = 64; + static constexpr unsigned StackSize = 768; + +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; } + + int teardown(); + + 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 add_poll_fd(int fd); ///< add a fd to poll list, returning index into _poll_fds[] + + + int _task = -1; ///< handle to the OS task + bool _task_should_exit = false; ///< flag to indicate to tear down the CAN driver + + static UavcanNode *_instance; ///< singleton pointer + Node _node; ///< library instance + pthread_mutex_t _node_mutex; + + pollfd _poll_fds[UAVCAN_NUM_POLL_FDS] = {}; + unsigned _poll_fds_num = 0; + +}; diff --git a/src/modules/uavcannode/uavcannode_params.c b/src/modules/uavcannode/uavcannode_params.c new file mode 100644 index 000000000..e6ea8a8fb --- /dev/null +++ b/src/modules/uavcannode/uavcannode_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); + + + |