aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_UAVCAN: add support for optionally publishing CANFD frames

Browse Source
This commit is contained in:
Siddharth Purohit 2021-05-03 18:28:40 +05:30 committed by Andrew Tridgell
parent 8aa047d876
commit ef1bdc2e5f
3 changed files with 28 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_UAVCAN/AP_UAVCAN.cpp
View File

@ -103,7 +103,7 @@ const AP_Param::GroupInfo AP_UAVCAN::var_info[] = {
// @Param: OPTION
// @DisplayName: UAVCAN options
// @Description: Option flags
// @Bitmask: 0:ClearDNADatabase,1:IgnoreDNANodeConflicts
// @Bitmask: 0:ClearDNADatabase,1:IgnoreDNANodeConflicts,2:EnableCanfd
// @User: Advanced
AP_GROUPINFO("OPTION", 5, AP_UAVCAN, _options, 0),
@ -271,6 +271,13 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters)
}
_node->setHardwareVersion(hw_version);
}
#if UAVCAN_SUPPORT_CANFD
if (option_is_set(Options::CANFD_ENABLED)) {
_node->enableCanFd();
}
#endif
int start_res = _node->start();
if (start_res < 0) {
debug_uavcan(AP_CANManager::LOG_ERROR, "UAVCAN: node start problem, error %d\n\r", start_res);
@ -369,7 +376,7 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters)
snprintf(_thread_name, sizeof(_thread_name), "uavcan_%u", driver_index);
if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_UAVCAN::loop, void), _thread_name, 4096, AP_HAL::Scheduler::PRIORITY_CAN, 0)) {
if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_UAVCAN::loop, void), _thread_name, UAVCAN_STACK_SIZE, AP_HAL::Scheduler::PRIORITY_CAN, 0)) {
_node->setModeOfflineAndPublish();
debug_uavcan(AP_CANManager::LOG_ERROR, "UAVCAN: couldn't create thread\n\r");
return;

17
libraries/AP_UAVCAN/AP_UAVCAN.h
View File

@ -34,12 +34,26 @@
#ifndef UAVCAN_NODE_POOL_SIZE
#if HAL_CANFD_SUPPORTED
#define UAVCAN_NODE_POOL_SIZE 16384
#else
#define UAVCAN_NODE_POOL_SIZE 8192
#endif
#endif
#if HAL_CANFD_SUPPORTED
#define UAVCAN_STACK_SIZE 8192
#else
#define UAVCAN_STACK_SIZE 4096
#endif
#ifndef UAVCAN_NODE_POOL_BLOCK_SIZE
#if HAL_CANFD_SUPPORTED
#define UAVCAN_NODE_POOL_BLOCK_SIZE 128
#else
#define UAVCAN_NODE_POOL_BLOCK_SIZE 64
#endif
#endif
#ifndef UAVCAN_SRV_NUMBER
#define UAVCAN_SRV_NUMBER 18
@ -116,7 +130,7 @@ public:
void init(uint8_t driver_index, bool enable_filters) override;
bool add_interface(AP_HAL::CANIface* can_iface) override;
uavcan::Node<0>* get_node() { return _node; }
uint8_t get_driver_index() const { return _driver_index; }
@ -206,6 +220,7 @@ public:
enum class Options : uint16_t {
DNA_CLEAR_DATABASE = (1U<<0),
DNA_IGNORE_DUPLICATE_NODE = (1U<<1),
CANFD_ENABLED = (1U<<2),
};
// check if a option is set

6
libraries/AP_UAVCAN/AP_UAVCAN_IfaceMgr.cpp
View File

@ -48,7 +48,7 @@ int16_t CanIface::send(const CanFrame& frame, MonotonicTime tx_deadline, CanIOFl
if (can_iface_ == UAVCAN_NULLPTR) {
return -1;
}
return can_iface_->send(AP_HAL::CANFrame(frame.id, frame.data, frame.dlc), tx_deadline.toUSec(), flags);
return can_iface_->send(AP_HAL::CANFrame(frame.id, frame.data, AP_HAL::CANFrame::dlcToDataLength(frame.dlc), frame.isCanFDFrame()), tx_deadline.toUSec(), flags);
}
/**
@ -83,7 +83,7 @@ int16_t CanIface::receive(CanFrame& out_frame, MonotonicTime& out_ts_monotonic,
if (ret < 0) {
return ret;
}
out_frame = CanFrame(frame.id, (const uint8_t*)frame.data, frame.dlc);
out_frame = CanFrame(frame.id, (const uint8_t*)frame.data, AP_HAL::CANFrame::dlcToDataLength(frame.dlc), frame.canfd);
out_flags = flags;
if (rx_timestamp != 0) {
out_ts_utc = uavcan::UtcTime::fromUSec(SystemClock::instance().getAdjustUsec() + rx_timestamp);
@ -185,7 +185,7 @@ CanSelectMasks CanIfaceMgr::makeSelectMasks(const CanSelectMasks in_mask, const
msk.write |= (write ? 1 : 0) << i;
}
} else {
AP_HAL::CANFrame frame {pending_tx[i]->id, pending_tx[i]->data, pending_tx[i]->dlc};
AP_HAL::CANFrame frame {pending_tx[i]->id, pending_tx[i]->data, AP_HAL::CANFrame::dlcToDataLength(pending_tx[i]->dlc)};
if (iface->can_iface_->select(read, write, &frame, 0)) {
msk.read |= (read ? 1 : 0) << i;
msk.write |= (write ? 1 : 0) << i;