diff --git a/boards/nxp/mr-canhubk3/default.px4board b/boards/nxp/mr-canhubk3/default.px4board index 15e6f15bf0..277c62de78 100644 --- a/boards/nxp/mr-canhubk3/default.px4board +++ b/boards/nxp/mr-canhubk3/default.px4board @@ -15,8 +15,6 @@ CONFIG_MODULES_SENSORS=y CONFIG_SYSTEMCMDS_I2CDETECT=y CONFIG_SYSTEMCMDS_LED_CONTROL=y CONFIG_SYSTEMCMDS_MFT=y -CONFIG_SYSTEMCMDS_MIXER=y -CONFIG_SYSTEMCMDS_MOTOR_TEST=y CONFIG_SYSTEMCMDS_MTD=y CONFIG_SYSTEMCMDS_PARAM=y CONFIG_SYSTEMCMDS_REBOOT=y diff --git a/boards/nxp/mr-canhubk3/fmu.px4board b/boards/nxp/mr-canhubk3/fmu.px4board index afaefec9cd..366bf7fa09 100644 --- a/boards/nxp/mr-canhubk3/fmu.px4board +++ b/boards/nxp/mr-canhubk3/fmu.px4board @@ -2,14 +2,14 @@ CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS1" CONFIG_BOARD_SERIAL_RC="/dev/ttyS5" CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS2" +CONFIG_BOARD_UAVCAN_INTERFACES=1 CONFIG_COMMON_LIGHT=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_IRLOCK=y CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y CONFIG_DRIVERS_MAGNETOMETER_LIS3MDL=y -CONFIG_DRIVERS_OSD=y CONFIG_DRIVERS_RC_INPUT=y -CONFIG_DRIVERS_RPM=y +CONFIG_DRIVERS_UAVCAN=y CONFIG_EXAMPLES_FAKE_GPS=y CONFIG_MODULES_AIRSPEED_SELECTOR=y CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y @@ -38,7 +38,6 @@ CONFIG_MODULES_MC_RATE_CONTROL=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_ROVER_POS_CONTROL=y -CONFIG_MODULES_SIH=y CONFIG_MODULES_TEMPERATURE_COMPENSATION=y CONFIG_MODULES_VTOL_ATT_CONTROL=y CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y @@ -47,7 +46,6 @@ CONFIG_SYSTEMCMDS_DUMPFILE=y CONFIG_SYSTEMCMDS_NETMAN=y CONFIG_SYSTEMCMDS_NSHTERM=y CONFIG_SYSTEMCMDS_PERF=y -CONFIG_SYSTEMCMDS_PWM=y CONFIG_SYSTEMCMDS_REFLECT=y CONFIG_SYSTEMCMDS_SERIAL_TEST=y CONFIG_SYSTEMCMDS_TUNE_CONTROL=y diff --git a/boards/nxp/mr-canhubk3/init/rc.board_defaults b/boards/nxp/mr-canhubk3/init/rc.board_defaults index 40fe6fcdfb..d9bd59a7fd 100644 --- a/boards/nxp/mr-canhubk3/init/rc.board_defaults +++ b/boards/nxp/mr-canhubk3/init/rc.board_defaults @@ -15,3 +15,11 @@ param set-default MAV_1_REMOTE_PRT 14550 param set-default MAV_1_UDP_PRT 14550 param set-default SENS_EXT_I2C_PRB 0 + +if param greater -s UAVCAN_ENABLE 0 +then + ifup can0 + ifup can1 + ifup can2 + ifup can3 +fi \ No newline at end of file diff --git a/boards/nxp/mr-canhubk3/nuttx-config/nsh/defconfig b/boards/nxp/mr-canhubk3/nuttx-config/nsh/defconfig index 889be6c7ba..addbed16f7 100644 --- a/boards/nxp/mr-canhubk3/nuttx-config/nsh/defconfig +++ b/boards/nxp/mr-canhubk3/nuttx-config/nsh/defconfig @@ -78,6 +78,7 @@ CONFIG_DEBUG_FEATURES=y CONFIG_DEBUG_FULLOPT=y CONFIG_DEBUG_HARDFAULT_ALERT=y CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEBUG_TCBINFO=y CONFIG_DEFAULT_SMALL=y CONFIG_DEV_FIFO_SIZE=0 CONFIG_DEV_PIPE_MAXSIZE=1024 @@ -103,7 +104,7 @@ CONFIG_HAVE_CXX=y CONFIG_HAVE_CXXINITIALIZE=y CONFIG_I2C=y CONFIG_I2C_RESET=y -CONFIG_IDLETHREAD_STACKSIZE=800 +CONFIG_IDLETHREAD_STACKSIZE=900 CONFIG_INIT_ENTRYPOINT="nsh_main" CONFIG_INIT_STACKSIZE=2944 CONFIG_IOB_NBUFFERS=24 @@ -145,7 +146,10 @@ CONFIG_NET_ARP_IPIN=y CONFIG_NET_ARP_SEND=y CONFIG_NET_BROADCAST=y CONFIG_NET_CAN=y +CONFIG_NET_CAN_EXTID=y +CONFIG_NET_CAN_NOTIFIER=y CONFIG_NET_CAN_RAW_FILTER_MAX=1 +CONFIG_NET_CAN_RAW_TX_DEADLINE=y CONFIG_NET_CAN_SOCK_OPTS=y CONFIG_NET_ETH_PKTSIZE=1518 CONFIG_NET_ICMP=y @@ -155,6 +159,7 @@ CONFIG_NET_TCP=y CONFIG_NET_TCPBACKLOG=y CONFIG_NET_TCP_DELAYED_ACK=y CONFIG_NET_TCP_WRITE_BUFFERS=y +CONFIG_NET_TIMESTAMP=y CONFIG_NET_UDP=y CONFIG_NET_UDP_CHECKSUMS=y CONFIG_NET_UDP_WRITE_BUFFERS=y diff --git a/src/drivers/uavcan/CMakeLists.txt b/src/drivers/uavcan/CMakeLists.txt index 1ea0213c2f..e410bcb447 100644 --- a/src/drivers/uavcan/CMakeLists.txt +++ b/src/drivers/uavcan/CMakeLists.txt @@ -40,7 +40,10 @@ set(UAVCAN_USE_CPP03 ON CACHE BOOL "uavcan cpp03") set(UAVCAN_PLATFORM "generic") if(CONFIG_ARCH_CHIP) - if(${CONFIG_ARCH_CHIP} MATCHES "kinetis") + if(${CONFIG_NET_CAN} MATCHES "y") + set(UAVCAN_DRIVER "socketcan") + set(UAVCAN_TIMER 1) + elseif(${CONFIG_ARCH_CHIP} MATCHES "kinetis") set(UAVCAN_DRIVER "kinetis") set(UAVCAN_TIMER 1) elseif(${CONFIG_ARCH_CHIP} MATCHES "stm32h7") diff --git a/src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/can.hpp b/src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/can.hpp index 44e0d1ad4f..f03f755a1b 100644 --- a/src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/can.hpp +++ b/src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/can.hpp @@ -360,18 +360,13 @@ public: * This function can either initialize the driver at a fixed bit rate, or it can perform * automatic bit rate detection. For theory please refer to the CiA application note #801. * - * @param delay_callable A callable entity that suspends execution for strictly more than one second. - * The callable entity will be invoked without arguments. - * @ref getRecommendedListeningDelay(). - * - * @param inout_bitrate Fixed bit rate or zero. Zero invokes the bit rate detection process. + * @param inout_bitrate Fixed bit rate or zero. Zero invokes the bit rate detection process. * If auto detection was used, the function will update the argument * with established bit rate. In case of an error the value will be undefined. * * @return Negative value on error; non-negative on success. Refer to constants Err*. */ - template - int init(DelayCallable delay_callable, uavcan::uint32_t &inout_bitrate = BitRateAutoDetect) + int init(uavcan::uint32_t &inout_bitrate = BitRateAutoDetect) { if (inout_bitrate > 0) { return driver.init(inout_bitrate, CanIface::NormalMode); @@ -389,7 +384,7 @@ public: const int res = driver.init(inout_bitrate, CanIface::SilentMode); - delay_callable(); + usleep(1000000); if (res >= 0) { for (uavcan::uint8_t iface = 0; iface < driver.getNumIfaces(); iface++) { diff --git a/src/drivers/uavcan/uavcan_drivers/socketcan/driver/CMakeLists.txt b/src/drivers/uavcan/uavcan_drivers/socketcan/driver/CMakeLists.txt index e3c5344d79..0b7b785b86 100644 --- a/src/drivers/uavcan/uavcan_drivers/socketcan/driver/CMakeLists.txt +++ b/src/drivers/uavcan/uavcan_drivers/socketcan/driver/CMakeLists.txt @@ -6,6 +6,7 @@ add_compile_options(-Wno-unused-variable) add_library(uavcan_socketcan_driver STATIC src/socketcan.cpp src/thread.cpp + src/clock.cpp ) add_dependencies(uavcan_socketcan_driver uavcan) diff --git a/src/drivers/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/clock.hpp b/src/drivers/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/clock.hpp index 562fe054ec..42ac63f54c 100644 --- a/src/drivers/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/clock.hpp +++ b/src/drivers/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/clock.hpp @@ -46,6 +46,16 @@ namespace uavcan_socketcan { +namespace clock +{ +/** + * Performs UTC phase and frequency adjustment. + * The UTC time will be zero until first adjustment has been performed. + * This function is thread safe. + */ +void adjustUtc(uavcan::UtcDuration adjustment); +//FIXME +} /** * Different adjustment modes can be used for time synchronization */ diff --git a/src/drivers/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/socketcan.hpp b/src/drivers/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/socketcan.hpp index 75b9045489..12fc861400 100644 --- a/src/drivers/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/socketcan.hpp +++ b/src/drivers/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/socketcan.hpp @@ -71,7 +71,7 @@ class CanIface : public uavcan::ICanIface SystemClock clock; public: - uavcan::uint32_t socketInit(const char *can_iface_name); + uavcan::uint32_t socketInit(uint32_t index); uavcan::int16_t send(const uavcan::CanFrame &frame, uavcan::MonotonicTime tx_deadline, @@ -109,13 +109,13 @@ public: CanDriver() : update_event_(*this) {} - uavcan::int32_t initIface(uint32_t index, const char *name) + uavcan::int32_t initIface(uint32_t index) { if (index > (UAVCAN_SOCKETCAN_NUM_IFACES - 1)) { return -1; } - return if_[index].socketInit(name); + return if_[index].socketInit(index); } /** @@ -180,10 +180,10 @@ public: */ int init(uavcan::uint32_t bitrate) { - driver.initIface(0, "can0"); -#if UAVCAN_SOCKETCAN_NUM_IFACES > 1 - driver.initIface(1, "can1"); -#endif + for (int i = 0; i < UAVCAN_SOCKETCAN_NUM_IFACES; i++) { + driver.initIface(i); + } + return driver.init(bitrate); } @@ -191,23 +191,20 @@ public: * This function can either initialize the driver at a fixed bit rate, or it can perform * automatic bit rate detection. For theory please refer to the CiA application note #801. * - * @param delay_callable A callable entity that suspends execution for strictly more than one second. - * The callable entity will be invoked without arguments. - * @ref getRecommendedListeningDelay(). - * - * @param inout_bitrate Fixed bit rate or zero. Zero invokes the bit rate detection process. + * @param bitrate Fixed bit rate or zero. Zero invokes the bit rate detection process. * If auto detection was used, the function will update the argument * with established bit rate. In case of an error the value will be undefined. * * @return Negative value on error; non-negative on success. Refer to constants Err*. */ - template - int init(DelayCallable delay_callable, uavcan::uint32_t &inout_bitrate = 1000000) - { - if (inout_bitrate > 0) { - return driver.init(inout_bitrate); + int init(uavcan::uint32_t &bitrate = 1000000) + { + if (bitrate > 0) { + return driver.init(bitrate); } + + return -1; } /** diff --git a/src/drivers/uavcan/uavcan_drivers/socketcan/driver/src/clock.cpp b/src/drivers/uavcan/uavcan_drivers/socketcan/driver/src/clock.cpp new file mode 100644 index 0000000000..6b030da825 --- /dev/null +++ b/src/drivers/uavcan/uavcan_drivers/socketcan/driver/src/clock.cpp @@ -0,0 +1,56 @@ +/**************************************************************************** + * + * Copyright (C) 2014 Pavel Kirienko + * Kinetis Port Author David Sidrane + * NuttX SocketCAN port Copyright (C) 2022 NXP Semiconductors + * + * 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 +#include +#include + +namespace uavcan_socketcan +{ +namespace clock +{ +/** + * Performs UTC phase and frequency adjustment. + * The UTC time will be zero until first adjustment has been performed. + * This function is thread safe. + */ +void adjustUtc(uavcan::UtcDuration adjustment) +{ + //printf("Adjust UTC\n"); + //clock::adjustUtc(adjustment); +} + +} +} diff --git a/src/drivers/uavcan/uavcan_drivers/socketcan/driver/src/socketcan.cpp b/src/drivers/uavcan/uavcan_drivers/socketcan/driver/src/socketcan.cpp index 422722b6ca..b3a8ce1836 100644 --- a/src/drivers/uavcan/uavcan_drivers/socketcan/driver/src/socketcan.cpp +++ b/src/drivers/uavcan/uavcan_drivers/socketcan/driver/src/socketcan.cpp @@ -69,7 +69,7 @@ struct BitTimingSettings { } // namespace -uavcan::uint32_t CanIface::socketInit(const char *can_iface_name) +uavcan::uint32_t CanIface::socketInit(uint32_t index) { struct sockaddr_can addr; @@ -86,7 +86,7 @@ uavcan::uint32_t CanIface::socketInit(const char *can_iface_name) return -1; } - strncpy(ifr.ifr_name, can_iface_name, IFNAMSIZ - 1); + snprintf(ifr.ifr_name, IFNAMSIZ, "can%li", index); ifr.ifr_name[IFNAMSIZ - 1] = '\0'; ifr.ifr_ifindex = if_nametoindex(ifr.ifr_name); @@ -194,7 +194,7 @@ uavcan::int16_t CanIface::send(const uavcan::CanFrame &frame, uavcan::MonotonicT _send_tv->tv_usec = tx_deadline.toUSec() % 1000000ULL; _send_tv->tv_sec = (tx_deadline.toUSec() - _send_tv->tv_usec) / 1000000ULL; - res = sendmsg(_fd, &_send_msg, 0); + res = sendmsg(_fd, &_send_msg, MSG_DONTWAIT); if (res > 0) { return 1; @@ -271,12 +271,10 @@ uavcan::uint32_t CanDriver::detectBitRate(void (*idle_callback)()) int CanDriver::init(uavcan::uint32_t bitrate) { - pfds[0].fd = if_[0].getFD(); - pfds[0].events = POLLIN | POLLOUT; -#if UAVCAN_SOCKETCAN_NUM_IFACES > 1 - pfds[1].fd = if_[1].getFD(); - pfds[1].events = POLLIN | POLLOUT; -#endif + for (int i = 0; i < UAVCAN_SOCKETCAN_NUM_IFACES; i++) { + pfds[i].fd = if_[i].getFD(); + pfds[i].events = POLLIN | POLLOUT; + } /* * TODO add filter configuration ioctl @@ -308,8 +306,7 @@ uavcan::int16_t CanDriver::select(uavcan::CanSelectMasks &inout_masks, } inout_masks.read = 0; - //FIXME NuttX SocketCAN implement POLLOUT - inout_masks.write = 0x3; + inout_masks.write = 0; if (poll(pfds, UAVCAN_SOCKETCAN_NUM_IFACES, timeout_usec / 1000) > 0) { for (int i = 0; i < UAVCAN_SOCKETCAN_NUM_IFACES; i++) { diff --git a/src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/can.hpp b/src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/can.hpp index bc25be2a9a..67abcf53c5 100644 --- a/src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/can.hpp +++ b/src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/can.hpp @@ -320,18 +320,14 @@ public: * This function can either initialize the driver at a fixed bit rate, or it can perform * automatic bit rate detection. For theory please refer to the CiA application note #801. * - * @param delay_callable A callable entity that suspends execution for strictly more than one second. - * The callable entity will be invoked without arguments. - * @ref getRecommendedListeningDelay(). - * - * @param inout_bitrate Fixed bit rate or zero. Zero invokes the bit rate detection process. + * @param inout_bitrate Fixed bit rate or zero. Zero invokes the bit rate detection process. * If auto detection was used, the function will update the argument * with established bit rate. In case of an error the value will be undefined. * * @return Negative value on error; non-negative on success. Refer to constants Err*. */ template - int init(DelayCallable delay_callable, uavcan::uint32_t &inout_bitrate = BitRateAutoDetect) + int init(uavcan::uint32_t &inout_bitrate = BitRateAutoDetect) { if (inout_bitrate > 0) { return driver.init(inout_bitrate, CanIface::NormalMode, enabledInterfaces_); @@ -349,7 +345,7 @@ public: const int res = driver.init(inout_bitrate, CanIface::SilentMode, enabledInterfaces_); - delay_callable(); + usleep(1000000); if (res >= 0) { for (uavcan::uint8_t iface = 0; iface < driver.getNumIfaces(); iface++) { diff --git a/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/include/uavcan_stm32h7/can.hpp b/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/include/uavcan_stm32h7/can.hpp index 22f8ff739a..9720b24967 100644 --- a/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/include/uavcan_stm32h7/can.hpp +++ b/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/include/uavcan_stm32h7/can.hpp @@ -341,8 +341,7 @@ public: * * @return Negative value on error; non-negative on success. Refer to constants Err*. */ - template - int init(DelayCallable delay_callable, uavcan::uint32_t &inout_bitrate = BitRateAutoDetect) + int init(uavcan::uint32_t &inout_bitrate = BitRateAutoDetect) { if (inout_bitrate > 0) { return driver.init(inout_bitrate, CanIface::NormalMode, enabledInterfaces_); @@ -360,7 +359,7 @@ public: const int res = driver.init(inout_bitrate, CanIface::SilentMode, enabledInterfaces_); - delay_callable(); + usleep(1000000); if (res >= 0) { for (uavcan::uint8_t iface = 0; iface < driver.getNumIfaces(); iface++) { diff --git a/src/drivers/uavcan/uavcan_main.cpp b/src/drivers/uavcan/uavcan_main.cpp index 24b677da6f..6fcfd4faf9 100644 --- a/src/drivers/uavcan/uavcan_main.cpp +++ b/src/drivers/uavcan/uavcan_main.cpp @@ -74,6 +74,8 @@ */ UavcanNode *UavcanNode::_instance; +static UavcanNode::CanInitHelper *can = nullptr; + UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) : ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::uavcan), ModuleParams(nullptr), @@ -404,28 +406,15 @@ UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) return -1; } - /* - * CAN driver init - * Note that we instantiate and initialize CanInitHelper only once, because the STM32's bxCAN driver - * shipped with libuavcan does not support deinitialization. - */ - static CanInitHelper *can = nullptr; - if (can == nullptr) { can = new CanInitHelper(board_get_can_interfaces()); - if (can == nullptr) { // We don't have exceptions so bad_alloc cannot be thrown + if (can == nullptr) { // We don't have exceptions so bad_alloc cannot be thrown PX4_ERR("Out of memory"); return -1; } - const int can_init_res = can->init(bitrate); - - if (can_init_res < 0) { - PX4_ERR("CAN driver init failed %i", can_init_res); - return can_init_res; - } } /* @@ -438,15 +427,6 @@ UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) return -1; } - const int node_init_res = _instance->init(node_id, can->driver.updateEvent()); - - if (node_init_res < 0) { - delete _instance; - _instance = nullptr; - PX4_ERR("Node init failed %i", node_init_res); - return node_init_res; - } - _instance->ScheduleOnInterval(ScheduleIntervalMs * 1000); _instance->_mixing_interface_esc.ScheduleNow(); _instance->_mixing_interface_servo.ScheduleNow(); @@ -634,6 +614,36 @@ UavcanNode::handle_time_sync(const uavcan::TimerEvent &) void UavcanNode::Run() { + if (!_node_init) { + // Node ID + int32_t node_id = 1; + (void)param_get(param_find("UAVCAN_NODE_ID"), &node_id); + + if (node_id < 0 || node_id > uavcan::NodeID::Max || !uavcan::NodeID(node_id).isUnicast()) { + PX4_ERR("Invalid Node ID %" PRId32, node_id); + ::exit(1); + } + + // CAN bitrate + int32_t bitrate = 1000000; + (void)param_get(param_find("UAVCAN_BITRATE"), &bitrate); + + /* + * CAN driver init + * Note that we instantiate and initialize CanInitHelper only once, because the STM32's bxCAN driver + * shipped with libuavcan does not support deinitialization. + */ + const int can_init_res = can->init(bitrate); + + if (can_init_res < 0) { + PX4_ERR("CAN driver init failed %i", can_init_res); + } + + _instance->init(node_id, can->driver.updateEvent()); + + _node_init = true; + } + pthread_mutex_lock(&_node_mutex); if (_output_count == 0) { diff --git a/src/drivers/uavcan/uavcan_main.hpp b/src/drivers/uavcan/uavcan_main.hpp index 2bb8a434c0..f896fc742e 100644 --- a/src/drivers/uavcan/uavcan_main.hpp +++ b/src/drivers/uavcan/uavcan_main.hpp @@ -220,6 +220,7 @@ private: uavcan_node::Allocator _pool_allocator; + bool _node_init{false}; uavcan::Node<> _node; ///< library instance pthread_mutex_t _node_mutex; diff --git a/src/drivers/uavcannode/UavcanNode.cpp b/src/drivers/uavcannode/UavcanNode.cpp index bd4448fb4c..b88c2b0565 100644 --- a/src/drivers/uavcannode/UavcanNode.cpp +++ b/src/drivers/uavcannode/UavcanNode.cpp @@ -367,7 +367,7 @@ void UavcanNode::Run() if (!_initialized) { - const int can_init_res = _can->init(_bitrate); + const int can_init_res = _can->init((uint32_t)_bitrate); if (can_init_res < 0) { PX4_ERR("CAN driver init failed %i", can_init_res);