forked from Archive/PX4-Autopilot
DroneCAN SocketCAN driver add FMU support
This commit is contained in:
parent
9b3feee6ee
commit
33e39d68f7
|
@ -15,8 +15,6 @@ CONFIG_MODULES_SENSORS=y
|
||||||
CONFIG_SYSTEMCMDS_I2CDETECT=y
|
CONFIG_SYSTEMCMDS_I2CDETECT=y
|
||||||
CONFIG_SYSTEMCMDS_LED_CONTROL=y
|
CONFIG_SYSTEMCMDS_LED_CONTROL=y
|
||||||
CONFIG_SYSTEMCMDS_MFT=y
|
CONFIG_SYSTEMCMDS_MFT=y
|
||||||
CONFIG_SYSTEMCMDS_MIXER=y
|
|
||||||
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
|
|
||||||
CONFIG_SYSTEMCMDS_MTD=y
|
CONFIG_SYSTEMCMDS_MTD=y
|
||||||
CONFIG_SYSTEMCMDS_PARAM=y
|
CONFIG_SYSTEMCMDS_PARAM=y
|
||||||
CONFIG_SYSTEMCMDS_REBOOT=y
|
CONFIG_SYSTEMCMDS_REBOOT=y
|
||||||
|
|
|
@ -2,14 +2,14 @@
|
||||||
CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS1"
|
CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS1"
|
||||||
CONFIG_BOARD_SERIAL_RC="/dev/ttyS5"
|
CONFIG_BOARD_SERIAL_RC="/dev/ttyS5"
|
||||||
CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS2"
|
CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS2"
|
||||||
|
CONFIG_BOARD_UAVCAN_INTERFACES=1
|
||||||
CONFIG_COMMON_LIGHT=y
|
CONFIG_COMMON_LIGHT=y
|
||||||
CONFIG_DRIVERS_GPS=y
|
CONFIG_DRIVERS_GPS=y
|
||||||
CONFIG_DRIVERS_IRLOCK=y
|
CONFIG_DRIVERS_IRLOCK=y
|
||||||
CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y
|
CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y
|
||||||
CONFIG_DRIVERS_MAGNETOMETER_LIS3MDL=y
|
CONFIG_DRIVERS_MAGNETOMETER_LIS3MDL=y
|
||||||
CONFIG_DRIVERS_OSD=y
|
|
||||||
CONFIG_DRIVERS_RC_INPUT=y
|
CONFIG_DRIVERS_RC_INPUT=y
|
||||||
CONFIG_DRIVERS_RPM=y
|
CONFIG_DRIVERS_UAVCAN=y
|
||||||
CONFIG_EXAMPLES_FAKE_GPS=y
|
CONFIG_EXAMPLES_FAKE_GPS=y
|
||||||
CONFIG_MODULES_AIRSPEED_SELECTOR=y
|
CONFIG_MODULES_AIRSPEED_SELECTOR=y
|
||||||
CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
|
CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
|
||||||
|
@ -38,7 +38,6 @@ CONFIG_MODULES_MC_RATE_CONTROL=y
|
||||||
CONFIG_MODULES_NAVIGATOR=y
|
CONFIG_MODULES_NAVIGATOR=y
|
||||||
CONFIG_MODULES_RC_UPDATE=y
|
CONFIG_MODULES_RC_UPDATE=y
|
||||||
CONFIG_MODULES_ROVER_POS_CONTROL=y
|
CONFIG_MODULES_ROVER_POS_CONTROL=y
|
||||||
CONFIG_MODULES_SIH=y
|
|
||||||
CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
|
CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
|
||||||
CONFIG_MODULES_VTOL_ATT_CONTROL=y
|
CONFIG_MODULES_VTOL_ATT_CONTROL=y
|
||||||
CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
|
CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
|
||||||
|
@ -47,7 +46,6 @@ CONFIG_SYSTEMCMDS_DUMPFILE=y
|
||||||
CONFIG_SYSTEMCMDS_NETMAN=y
|
CONFIG_SYSTEMCMDS_NETMAN=y
|
||||||
CONFIG_SYSTEMCMDS_NSHTERM=y
|
CONFIG_SYSTEMCMDS_NSHTERM=y
|
||||||
CONFIG_SYSTEMCMDS_PERF=y
|
CONFIG_SYSTEMCMDS_PERF=y
|
||||||
CONFIG_SYSTEMCMDS_PWM=y
|
|
||||||
CONFIG_SYSTEMCMDS_REFLECT=y
|
CONFIG_SYSTEMCMDS_REFLECT=y
|
||||||
CONFIG_SYSTEMCMDS_SERIAL_TEST=y
|
CONFIG_SYSTEMCMDS_SERIAL_TEST=y
|
||||||
CONFIG_SYSTEMCMDS_TUNE_CONTROL=y
|
CONFIG_SYSTEMCMDS_TUNE_CONTROL=y
|
||||||
|
|
|
@ -15,3 +15,11 @@ param set-default MAV_1_REMOTE_PRT 14550
|
||||||
param set-default MAV_1_UDP_PRT 14550
|
param set-default MAV_1_UDP_PRT 14550
|
||||||
|
|
||||||
param set-default SENS_EXT_I2C_PRB 0
|
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
|
|
@ -78,6 +78,7 @@ CONFIG_DEBUG_FEATURES=y
|
||||||
CONFIG_DEBUG_FULLOPT=y
|
CONFIG_DEBUG_FULLOPT=y
|
||||||
CONFIG_DEBUG_HARDFAULT_ALERT=y
|
CONFIG_DEBUG_HARDFAULT_ALERT=y
|
||||||
CONFIG_DEBUG_SYMBOLS=y
|
CONFIG_DEBUG_SYMBOLS=y
|
||||||
|
CONFIG_DEBUG_TCBINFO=y
|
||||||
CONFIG_DEFAULT_SMALL=y
|
CONFIG_DEFAULT_SMALL=y
|
||||||
CONFIG_DEV_FIFO_SIZE=0
|
CONFIG_DEV_FIFO_SIZE=0
|
||||||
CONFIG_DEV_PIPE_MAXSIZE=1024
|
CONFIG_DEV_PIPE_MAXSIZE=1024
|
||||||
|
@ -103,7 +104,7 @@ CONFIG_HAVE_CXX=y
|
||||||
CONFIG_HAVE_CXXINITIALIZE=y
|
CONFIG_HAVE_CXXINITIALIZE=y
|
||||||
CONFIG_I2C=y
|
CONFIG_I2C=y
|
||||||
CONFIG_I2C_RESET=y
|
CONFIG_I2C_RESET=y
|
||||||
CONFIG_IDLETHREAD_STACKSIZE=800
|
CONFIG_IDLETHREAD_STACKSIZE=900
|
||||||
CONFIG_INIT_ENTRYPOINT="nsh_main"
|
CONFIG_INIT_ENTRYPOINT="nsh_main"
|
||||||
CONFIG_INIT_STACKSIZE=2944
|
CONFIG_INIT_STACKSIZE=2944
|
||||||
CONFIG_IOB_NBUFFERS=24
|
CONFIG_IOB_NBUFFERS=24
|
||||||
|
@ -145,7 +146,10 @@ CONFIG_NET_ARP_IPIN=y
|
||||||
CONFIG_NET_ARP_SEND=y
|
CONFIG_NET_ARP_SEND=y
|
||||||
CONFIG_NET_BROADCAST=y
|
CONFIG_NET_BROADCAST=y
|
||||||
CONFIG_NET_CAN=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_FILTER_MAX=1
|
||||||
|
CONFIG_NET_CAN_RAW_TX_DEADLINE=y
|
||||||
CONFIG_NET_CAN_SOCK_OPTS=y
|
CONFIG_NET_CAN_SOCK_OPTS=y
|
||||||
CONFIG_NET_ETH_PKTSIZE=1518
|
CONFIG_NET_ETH_PKTSIZE=1518
|
||||||
CONFIG_NET_ICMP=y
|
CONFIG_NET_ICMP=y
|
||||||
|
@ -155,6 +159,7 @@ CONFIG_NET_TCP=y
|
||||||
CONFIG_NET_TCPBACKLOG=y
|
CONFIG_NET_TCPBACKLOG=y
|
||||||
CONFIG_NET_TCP_DELAYED_ACK=y
|
CONFIG_NET_TCP_DELAYED_ACK=y
|
||||||
CONFIG_NET_TCP_WRITE_BUFFERS=y
|
CONFIG_NET_TCP_WRITE_BUFFERS=y
|
||||||
|
CONFIG_NET_TIMESTAMP=y
|
||||||
CONFIG_NET_UDP=y
|
CONFIG_NET_UDP=y
|
||||||
CONFIG_NET_UDP_CHECKSUMS=y
|
CONFIG_NET_UDP_CHECKSUMS=y
|
||||||
CONFIG_NET_UDP_WRITE_BUFFERS=y
|
CONFIG_NET_UDP_WRITE_BUFFERS=y
|
||||||
|
|
|
@ -40,7 +40,10 @@ set(UAVCAN_USE_CPP03 ON CACHE BOOL "uavcan cpp03")
|
||||||
set(UAVCAN_PLATFORM "generic")
|
set(UAVCAN_PLATFORM "generic")
|
||||||
|
|
||||||
if(CONFIG_ARCH_CHIP)
|
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_DRIVER "kinetis")
|
||||||
set(UAVCAN_TIMER 1)
|
set(UAVCAN_TIMER 1)
|
||||||
elseif(${CONFIG_ARCH_CHIP} MATCHES "stm32h7")
|
elseif(${CONFIG_ARCH_CHIP} MATCHES "stm32h7")
|
||||||
|
|
|
@ -360,18 +360,13 @@ public:
|
||||||
* This function can either initialize the driver at a fixed bit rate, or it can perform
|
* 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.
|
* 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.
|
* @param inout_bitrate Fixed bit rate or zero. Zero invokes the bit rate detection process.
|
||||||
* 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.
|
|
||||||
* If auto detection was used, the function will update the argument
|
* 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.
|
* 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*.
|
* @return Negative value on error; non-negative on success. Refer to constants Err*.
|
||||||
*/
|
*/
|
||||||
template <typename DelayCallable>
|
int init(uavcan::uint32_t &inout_bitrate = BitRateAutoDetect)
|
||||||
int init(DelayCallable delay_callable, uavcan::uint32_t &inout_bitrate = BitRateAutoDetect)
|
|
||||||
{
|
{
|
||||||
if (inout_bitrate > 0) {
|
if (inout_bitrate > 0) {
|
||||||
return driver.init(inout_bitrate, CanIface::NormalMode);
|
return driver.init(inout_bitrate, CanIface::NormalMode);
|
||||||
|
@ -389,7 +384,7 @@ public:
|
||||||
|
|
||||||
const int res = driver.init(inout_bitrate, CanIface::SilentMode);
|
const int res = driver.init(inout_bitrate, CanIface::SilentMode);
|
||||||
|
|
||||||
delay_callable();
|
usleep(1000000);
|
||||||
|
|
||||||
if (res >= 0) {
|
if (res >= 0) {
|
||||||
for (uavcan::uint8_t iface = 0; iface < driver.getNumIfaces(); iface++) {
|
for (uavcan::uint8_t iface = 0; iface < driver.getNumIfaces(); iface++) {
|
||||||
|
|
|
@ -6,6 +6,7 @@ add_compile_options(-Wno-unused-variable)
|
||||||
add_library(uavcan_socketcan_driver STATIC
|
add_library(uavcan_socketcan_driver STATIC
|
||||||
src/socketcan.cpp
|
src/socketcan.cpp
|
||||||
src/thread.cpp
|
src/thread.cpp
|
||||||
|
src/clock.cpp
|
||||||
)
|
)
|
||||||
|
|
||||||
add_dependencies(uavcan_socketcan_driver uavcan)
|
add_dependencies(uavcan_socketcan_driver uavcan)
|
||||||
|
|
|
@ -46,6 +46,16 @@
|
||||||
|
|
||||||
namespace uavcan_socketcan
|
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
|
* Different adjustment modes can be used for time synchronization
|
||||||
*/
|
*/
|
||||||
|
|
|
@ -71,7 +71,7 @@ class CanIface : public uavcan::ICanIface
|
||||||
SystemClock clock;
|
SystemClock clock;
|
||||||
|
|
||||||
public:
|
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::int16_t send(const uavcan::CanFrame &frame,
|
||||||
uavcan::MonotonicTime tx_deadline,
|
uavcan::MonotonicTime tx_deadline,
|
||||||
|
@ -109,13 +109,13 @@ public:
|
||||||
CanDriver() : update_event_(*this)
|
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)) {
|
if (index > (UAVCAN_SOCKETCAN_NUM_IFACES - 1)) {
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
|
|
||||||
return if_[index].socketInit(name);
|
return if_[index].socketInit(index);
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
@ -180,10 +180,10 @@ public:
|
||||||
*/
|
*/
|
||||||
int init(uavcan::uint32_t bitrate)
|
int init(uavcan::uint32_t bitrate)
|
||||||
{
|
{
|
||||||
driver.initIface(0, "can0");
|
for (int i = 0; i < UAVCAN_SOCKETCAN_NUM_IFACES; i++) {
|
||||||
#if UAVCAN_SOCKETCAN_NUM_IFACES > 1
|
driver.initIface(i);
|
||||||
driver.initIface(1, "can1");
|
}
|
||||||
#endif
|
|
||||||
return driver.init(bitrate);
|
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
|
* 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.
|
* 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.
|
* @param bitrate Fixed bit rate or zero. Zero invokes the bit rate detection process.
|
||||||
* 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.
|
|
||||||
* If auto detection was used, the function will update the argument
|
* 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.
|
* 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*.
|
* @return Negative value on error; non-negative on success. Refer to constants Err*.
|
||||||
*/
|
*/
|
||||||
template <typename DelayCallable>
|
|
||||||
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;
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
|
|
@ -0,0 +1,56 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||||
|
* Kinetis Port Author David Sidrane <david_s5@nscdg.com>
|
||||||
|
* 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 <uavcan_nuttx/thread.hpp>
|
||||||
|
#include <uavcan_nuttx/socketcan.hpp>
|
||||||
|
#include <px4_platform_common/log.h>
|
||||||
|
|
||||||
|
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);
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
}
|
|
@ -69,7 +69,7 @@ struct BitTimingSettings {
|
||||||
|
|
||||||
} // namespace
|
} // namespace
|
||||||
|
|
||||||
uavcan::uint32_t CanIface::socketInit(const char *can_iface_name)
|
uavcan::uint32_t CanIface::socketInit(uint32_t index)
|
||||||
{
|
{
|
||||||
|
|
||||||
struct sockaddr_can addr;
|
struct sockaddr_can addr;
|
||||||
|
@ -86,7 +86,7 @@ uavcan::uint32_t CanIface::socketInit(const char *can_iface_name)
|
||||||
return -1;
|
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_name[IFNAMSIZ - 1] = '\0';
|
||||||
ifr.ifr_ifindex = if_nametoindex(ifr.ifr_name);
|
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_usec = tx_deadline.toUSec() % 1000000ULL;
|
||||||
_send_tv->tv_sec = (tx_deadline.toUSec() - _send_tv->tv_usec) / 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) {
|
if (res > 0) {
|
||||||
return 1;
|
return 1;
|
||||||
|
@ -271,12 +271,10 @@ uavcan::uint32_t CanDriver::detectBitRate(void (*idle_callback)())
|
||||||
|
|
||||||
int CanDriver::init(uavcan::uint32_t bitrate)
|
int CanDriver::init(uavcan::uint32_t bitrate)
|
||||||
{
|
{
|
||||||
pfds[0].fd = if_[0].getFD();
|
for (int i = 0; i < UAVCAN_SOCKETCAN_NUM_IFACES; i++) {
|
||||||
pfds[0].events = POLLIN | POLLOUT;
|
pfds[i].fd = if_[i].getFD();
|
||||||
#if UAVCAN_SOCKETCAN_NUM_IFACES > 1
|
pfds[i].events = POLLIN | POLLOUT;
|
||||||
pfds[1].fd = if_[1].getFD();
|
}
|
||||||
pfds[1].events = POLLIN | POLLOUT;
|
|
||||||
#endif
|
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* TODO add filter configuration ioctl
|
* TODO add filter configuration ioctl
|
||||||
|
@ -308,8 +306,7 @@ uavcan::int16_t CanDriver::select(uavcan::CanSelectMasks &inout_masks,
|
||||||
}
|
}
|
||||||
|
|
||||||
inout_masks.read = 0;
|
inout_masks.read = 0;
|
||||||
//FIXME NuttX SocketCAN implement POLLOUT
|
inout_masks.write = 0;
|
||||||
inout_masks.write = 0x3;
|
|
||||||
|
|
||||||
if (poll(pfds, UAVCAN_SOCKETCAN_NUM_IFACES, timeout_usec / 1000) > 0) {
|
if (poll(pfds, UAVCAN_SOCKETCAN_NUM_IFACES, timeout_usec / 1000) > 0) {
|
||||||
for (int i = 0; i < UAVCAN_SOCKETCAN_NUM_IFACES; i++) {
|
for (int i = 0; i < UAVCAN_SOCKETCAN_NUM_IFACES; i++) {
|
||||||
|
|
|
@ -320,18 +320,14 @@ public:
|
||||||
* This function can either initialize the driver at a fixed bit rate, or it can perform
|
* 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.
|
* 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.
|
* @param inout_bitrate Fixed bit rate or zero. Zero invokes the bit rate detection process.
|
||||||
* 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.
|
|
||||||
* If auto detection was used, the function will update the argument
|
* 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.
|
* 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*.
|
* @return Negative value on error; non-negative on success. Refer to constants Err*.
|
||||||
*/
|
*/
|
||||||
template <typename DelayCallable>
|
template <typename DelayCallable>
|
||||||
int init(DelayCallable delay_callable, uavcan::uint32_t &inout_bitrate = BitRateAutoDetect)
|
int init(uavcan::uint32_t &inout_bitrate = BitRateAutoDetect)
|
||||||
{
|
{
|
||||||
if (inout_bitrate > 0) {
|
if (inout_bitrate > 0) {
|
||||||
return driver.init(inout_bitrate, CanIface::NormalMode, enabledInterfaces_);
|
return driver.init(inout_bitrate, CanIface::NormalMode, enabledInterfaces_);
|
||||||
|
@ -349,7 +345,7 @@ public:
|
||||||
|
|
||||||
const int res = driver.init(inout_bitrate, CanIface::SilentMode, enabledInterfaces_);
|
const int res = driver.init(inout_bitrate, CanIface::SilentMode, enabledInterfaces_);
|
||||||
|
|
||||||
delay_callable();
|
usleep(1000000);
|
||||||
|
|
||||||
if (res >= 0) {
|
if (res >= 0) {
|
||||||
for (uavcan::uint8_t iface = 0; iface < driver.getNumIfaces(); iface++) {
|
for (uavcan::uint8_t iface = 0; iface < driver.getNumIfaces(); iface++) {
|
||||||
|
|
|
@ -341,8 +341,7 @@ public:
|
||||||
*
|
*
|
||||||
* @return Negative value on error; non-negative on success. Refer to constants Err*.
|
* @return Negative value on error; non-negative on success. Refer to constants Err*.
|
||||||
*/
|
*/
|
||||||
template <typename DelayCallable>
|
int init(uavcan::uint32_t &inout_bitrate = BitRateAutoDetect)
|
||||||
int init(DelayCallable delay_callable, uavcan::uint32_t &inout_bitrate = BitRateAutoDetect)
|
|
||||||
{
|
{
|
||||||
if (inout_bitrate > 0) {
|
if (inout_bitrate > 0) {
|
||||||
return driver.init(inout_bitrate, CanIface::NormalMode, enabledInterfaces_);
|
return driver.init(inout_bitrate, CanIface::NormalMode, enabledInterfaces_);
|
||||||
|
@ -360,7 +359,7 @@ public:
|
||||||
|
|
||||||
const int res = driver.init(inout_bitrate, CanIface::SilentMode, enabledInterfaces_);
|
const int res = driver.init(inout_bitrate, CanIface::SilentMode, enabledInterfaces_);
|
||||||
|
|
||||||
delay_callable();
|
usleep(1000000);
|
||||||
|
|
||||||
if (res >= 0) {
|
if (res >= 0) {
|
||||||
for (uavcan::uint8_t iface = 0; iface < driver.getNumIfaces(); iface++) {
|
for (uavcan::uint8_t iface = 0; iface < driver.getNumIfaces(); iface++) {
|
||||||
|
|
|
@ -74,6 +74,8 @@
|
||||||
*/
|
*/
|
||||||
UavcanNode *UavcanNode::_instance;
|
UavcanNode *UavcanNode::_instance;
|
||||||
|
|
||||||
|
static UavcanNode::CanInitHelper *can = nullptr;
|
||||||
|
|
||||||
UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) :
|
UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) :
|
||||||
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::uavcan),
|
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::uavcan),
|
||||||
ModuleParams(nullptr),
|
ModuleParams(nullptr),
|
||||||
|
@ -404,28 +406,15 @@ UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
|
||||||
return -1;
|
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) {
|
if (can == nullptr) {
|
||||||
|
|
||||||
can = new CanInitHelper(board_get_can_interfaces());
|
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");
|
PX4_ERR("Out of memory");
|
||||||
return -1;
|
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;
|
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->ScheduleOnInterval(ScheduleIntervalMs * 1000);
|
||||||
_instance->_mixing_interface_esc.ScheduleNow();
|
_instance->_mixing_interface_esc.ScheduleNow();
|
||||||
_instance->_mixing_interface_servo.ScheduleNow();
|
_instance->_mixing_interface_servo.ScheduleNow();
|
||||||
|
@ -634,6 +614,36 @@ UavcanNode::handle_time_sync(const uavcan::TimerEvent &)
|
||||||
void
|
void
|
||||||
UavcanNode::Run()
|
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);
|
pthread_mutex_lock(&_node_mutex);
|
||||||
|
|
||||||
if (_output_count == 0) {
|
if (_output_count == 0) {
|
||||||
|
|
|
@ -220,6 +220,7 @@ private:
|
||||||
|
|
||||||
uavcan_node::Allocator _pool_allocator;
|
uavcan_node::Allocator _pool_allocator;
|
||||||
|
|
||||||
|
bool _node_init{false};
|
||||||
uavcan::Node<> _node; ///< library instance
|
uavcan::Node<> _node; ///< library instance
|
||||||
pthread_mutex_t _node_mutex;
|
pthread_mutex_t _node_mutex;
|
||||||
|
|
||||||
|
|
|
@ -367,7 +367,7 @@ void UavcanNode::Run()
|
||||||
if (!_initialized) {
|
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) {
|
if (can_init_res < 0) {
|
||||||
PX4_ERR("CAN driver init failed %i", can_init_res);
|
PX4_ERR("CAN driver init failed %i", can_init_res);
|
||||||
|
|
Loading…
Reference in New Issue