dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

DroneCAN SocketCAN driver add FMU support

This commit is contained in:
Peter van der Perk 2022-12-13 02:06:13 +01:00 committed by GitHub
parent 9b3feee6ee
commit 33e39d68f7
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
16 changed files with 152 additions and 78 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
boards/nxp/mr-canhubk3/default.px4board
View File

@ -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

6
boards/nxp/mr-canhubk3/fmu.px4board
View File

@ -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

8
boards/nxp/mr-canhubk3/init/rc.board_defaults
View File

@ -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

7
boards/nxp/mr-canhubk3/nuttx-config/nsh/defconfig
View File

@ -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

5
src/drivers/uavcan/CMakeLists.txt
View File

@ -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")

11
src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/can.hpp
View File

@ -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 <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) {
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++) {

1
src/drivers/uavcan/uavcan_drivers/socketcan/driver/CMakeLists.txt
View File

@ -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)

10
src/drivers/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/clock.hpp
View File

@ -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
*/

31
src/drivers/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/socketcan.hpp
View File

@ -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 <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;
}
/**

56
src/drivers/uavcan/uavcan_drivers/socketcan/driver/src/clock.cpp Normal file
View File

@ -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);
}
}
}

19
src/drivers/uavcan/uavcan_drivers/socketcan/driver/src/socketcan.cpp
View File

@ -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++) {

10
src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/can.hpp
View File

@ -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 <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) {
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++) {

5
src/drivers/uavcan/uavcan_drivers/stm32h7/driver/include/uavcan_stm32h7/can.hpp
View File

@ -341,8 +341,7 @@ public:
*
* @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 = 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++) {

56
src/drivers/uavcan/uavcan_main.cpp
View File

@ -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) {

1
src/drivers/uavcan/uavcan_main.hpp
View File

@ -220,6 +220,7 @@ private:
uavcan_node::Allocator _pool_allocator;
bool _node_init{false};
uavcan::Node<> _node; ///< library instance
pthread_mutex_t _node_mutex;

2
src/drivers/uavcannode/UavcanNode.cpp
View File

@ -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);