Ardupilot UAVCAN 学习《4》
作者：互联网
目录
文章目录
摘要
本节主要记录如何通过ardupilot的uavcan来实现一个Service帧信息，进而配置UAVCAN电调。有问题可以一起交流@18129927205
1.初始化一个Service 请求数据帧
还记得一个数据帧有Message帧（广播）和Service帧（非广播），这里我们重点学习如何创建Service帧。在创建Service帧时，需要创建客户端和服务器，这里的客户端指的是请求方，服务器指的是应答方。
这里我们通过飞控配置电调的指示灯，首先看下电调指示灯配置文档，如下图。
这些就是我们要参考的文档知识。
1.定义UAVCAN 驱动文件
在目录ardupilot/modules/uavcan/dsdl/uavcan/protocol中定义如下文件，注意文件名的含义（212是我们的Node ID不是CAN ID ）。
在目录ardupilot/modules/uavcan/libuavcan/dsdl_compiler/pyuavcan/dsdl/uavcan/protocol中定义如下文件，注意文件名的含义。
注意两个文件完全一样，内容如下：
2.初始化驱动文件
- void Copter::setup()—》 //API函数初始化接口
- init_ardupilot()—》 //设备层初始化接口
- BoardConfig_CAN.init(); //ardupilot板层can初始化函数接口
void AP_BoardConfig_CAN::init()
{
for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_INTERFACES; i++) //最多支持两个can总线
{
//_driver_number驱动号
_st_driver_number[i] = (int8_t) _var_info_can[i]._driver_number;
//_can_debug调试号
_st_can_debug[i] = (int8_t) _var_info_can[i]._can_debug;
}
//初始化can总线
setup_canbus();
}
4. setup_canbus();
5. _var_info_can_protocol[i]._uavcan->try_init() == true
到这里进入核心初始化代码，在文件目录ardupilot/libraries文件下找到AP_UAVCAN文件夹会看到AP_UAVCAN.cpp和AP_UAVCAN.h文件。
1》增加头文件
#include <uavcan/protocol/Led.hpp>
备注：这个文件是在编译时自动生成的Led.hpp，文件来源于我们定义的212.Led.uavcan那个文件
2》增加客户端函数和控制函数
//定义LED控制函数
static void led_ctrl_Info(const uavcan::ServiceCallResult<uavcan::protocol::Led>& resp);
//定义客户端函数
static uavcan::ServiceClient<uavcan::protocol::Led>* Led_Ctrl_client[MAX_NUMBER_OF_CAN_DRIVERS];
3》初始化客户端和回调函数
bool AP_UAVCAN::try_init(void)
{
//父类对象
if (_parent_can_mgr == nullptr)
{
return false;
}
//初始化OK吗
if (_initialized)
{
return true;
}
//对象初始化OK吗
if (!_parent_can_mgr->is_initialized())
{
return false;
}
_uavcan_i = UINT8_MAX;
for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++)
{
if (_parent_can_mgr == hal.can_mgr[i])
{
_uavcan_i = i;
break;
}
}
if(_uavcan_i == UINT8_MAX)
{
return false;
}
//获取结点
auto *node = get_node();
if (node == nullptr)
{
return false;
}
//结点初始化OK
if (node->isStarted())
{
return false;
}
uint8_t driver_index = _uavcan_i;
uavcan::NodeID self_node_id(_uavcan_node); //设置节点ID=10
node->setNodeID(self_node_id);
char ndname[20];
snprintf(ndname, sizeof(ndname), "org.ardupilot:%u", _uavcan_i);
uavcan::NodeStatusProvider::NodeName name(ndname);
node->setName(name);
// Standard type uavcan.protocol.SoftwareVersion
//标准型uavcan.protocol.SoftwareVersion协议
uavcan::protocol::SoftwareVersion sw_version;
sw_version.major = AP_UAVCAN_SW_VERS_MAJOR; //1
sw_version.minor = AP_UAVCAN_SW_VERS_MINOR; //1
node->setSoftwareVersion(sw_version); //设置软件版本
uavcan::protocol::HardwareVersion hw_version; // Standard type uavcan.protocol.HardwareVersion
//设置硬件版本1.0
hw_version.major = AP_UAVCAN_HW_VERS_MAJOR;
hw_version.minor = AP_UAVCAN_HW_VERS_MINOR;
node->setHardwareVersion(hw_version);
//初始化节点
const int node_start_res = node->start();
if (node_start_res < 0)
{
debug_uavcan(1, "UAVCAN: node start problem\n\r");
}
//初始化LED控制客户端
Led_Ctrl_client[driver_index] = new uavcan::ServiceClient<uavcan::protocol::Led>(*_node);
if (Led_Ctrl_client[driver_index] == nullptr)
{
return false;
}
//初始化LED控制函数
int res = Led_Ctrl_client[driver_index]->init();
//调用Led
Led_Ctrl_client[driver_index]->setCallback(led_ctrl_Info);
//设置优先级
Led_Ctrl_client[driver_index]->setPriority(16);
//设置服务节点是1
uavcan::NodeID service_res_node_id(1);
uavcan::protocol::Led::Request request;
request.option=1;
request.color=1;
request.blink=0;
//直接调用
Led_Ctrl_client[0]->call(service_res_node_id,request);
//直接调用
if (res < 0)
{
return false;
}
......
**后续省略**
.....
}
到这里初始化函数基本完成，对于控制函数可以暂时不写，如果你想接收服务器端的数据过来，可以在控制函数中进行解析收到的服务器数据。
3.分析初始化函数
重点两个函数需要分析：init()和call()函数
找到ardupilot/modules/uavcan/libuavcan/include/uavcan/node/service_client.hpp
可以找到这两个定义
这里我们只先分析init函数，对应call我们在loop中进行分析
int init()
{
//PublisherType publisher_;
return publisher_.init();
}
可以看出publisher属于PublisherType publisher_;的一个对象，PublisherType属于ServiceClient的一个私有模板数据成员
而typedef GenericPublisher<DataType, RequestType> PublisherType;中的第二个参数RequestType，属于typedef typename DataType::Request RequestType;
可以看出都是属于Request类型的
我们分析下面这个函数
int init(TransferPriority priority)
{
return publisher_.init(priority);
}
执行
这个下面函数Init方法可以在第一次发布之前调用，但不必 因为发布服务器可以自动初始化。
int init()
{
return checkInit();
}
template <typename DataSpec, typename DataStruct>
int GenericPublisher<DataSpec, DataStruct>::checkInit()
{
if (isInited())
{
return 0;
}
return doInit(DataTypeKind(DataSpec::DataTypeKind), DataSpec::getDataTypeFullName(), CanTxQueue::Qos(Qos));
}
注意这里面又两个函数
bool GenericPublisherBase::isInited() const
{
return sender_.isInitialized();
}
int GenericPublisherBase::doInit(DataTypeKind dtkind, const char* dtname, CanTxQueue::Qos qos)
{
if (isInited())
{
return 0;
}
GlobalDataTypeRegistry::instance().freeze();
const DataTypeDescriptor* const descr = GlobalDataTypeRegistry::instance().find(dtkind, dtname);
if (descr == UAVCAN_NULLPTR)
{
UAVCAN_TRACE("GenericPublisher", "Type [%s] is not registered", dtname);
return -ErrUnknownDataType;
}
//初始化
sender_.init(*descr, qos);
return 0;
}
继续执行
/**
* 传送ID初始化
*/
void TransferSender::init(const DataTypeDescriptor& dtid, CanTxQueue::Qos qos)
{
UAVCAN_ASSERT(!isInitialized());
qos_ = qos;
data_type_id_ = dtid.getID();
crc_base_ = dtid.getSignature().toTransferCRC();
}
建议这部分代码先不要深入研究，等用起来后再仔细分析。
2.周期执行一个Service 请求
void Scheduler::_uavcan_thread(void *arg)
{
Scheduler *sched = (Scheduler *)arg;
chRegSetThreadName("apm_uavcan");
while (!sched->_hal_initialized)
{
sched->delay_microseconds(20000);
}
while (true)
{
sched->delay_microseconds(300);
for (int i = 0; i < MAX_NUMBER_OF_CAN_INTERFACES; i++)
{
if (AP_UAVCAN::get_uavcan(i) != nullptr)
{
//调用定时器函数
CANManager::from(hal.can_mgr[i])->_timer_tick();
}
}
}
}
void CANManager::_timer_tick()
{
if (!initialized_) return;
if (p_uavcan != nullptr)
{
p_uavcan->do_cyclic(); //循环处理
}
}
void AP_UAVCAN::do_cyclic(void)
{
static uint8_t count=0;
if (!_initialized)
{
hal.scheduler->delay_microseconds(1000);
return;
}
//获取结点
auto *node = get_node();
count++;
if(count>=20)
{
//设置服务节点是1
uavcan::NodeID service_res_node_id(1);
uavcan::protocol::Led::Request request;
request.option=1;
request.color=1;
request.blink=0;
//直接调用
Led_Ctrl_client[0]->call(service_res_node_id,request);
count=0;
}
//开始执行
const int error = node->spin(uavcan::MonotonicDuration::fromMSec(1)); //获取解析数据
if (error < 0)
{
hal.scheduler->delay_microseconds(100);
return;
}
......
此处省略
......
}
重点函数：
Led_Ctrl_client[0]->call(service_res_node_id,request);
这个函数传入了两个参数，第一个是服务节点ID，第二个是客户端请求的带的参数对象
template <typename DataType_, typename Callback_>
int ServiceClient<DataType_, Callback_>::call(NodeID server_node_id, const RequestType& request,
ServiceCallID& out_call_id)
{
if (!coerceOrFallback<bool>(callback_, true))
{
printf("Invalid callback\r\n");
UAVCAN_TRACE("ServiceClient", "Invalid callback");
return -ErrInvalidConfiguration;
}
/*
* 不依赖于结构数据类型的公共过程
* Common procedures that don't depend on the struct data type
*/
const int prep_res =prepareToCall(SubscriberType::getNode(), DataType::getDataTypeFullName(), server_node_id, out_call_id);
if (prep_res < 0)
{
UAVCAN_TRACE("ServiceClient", "Failed to prepare the call, error: %i", prep_res);
return prep_res;
}
/*
* Initializing the call state - this will start the subscriber ad-hoc
* 初始化呼叫状态-这将临时启动订户
*/
const int call_state_res = addCallState(out_call_id);
if (call_state_res < 0)
{
UAVCAN_TRACE("ServiceClient", "Failed to add call state, error: %i", call_state_res);
return call_state_res;
}
/*
* 配置侦听器以便它只接受匹配的响应
* TODO移动到init（），但这需要对GenericSubscriber进行一些重构<>（删除TransferForwarder）
* Configuring the listener so it will accept only the matching responses
* TODO move to init(), but this requires to somewhat refactor GenericSubscriber<> (remove TransferForwarder)
*/
TransferListenerWithFilter* const tl = SubscriberType::getTransferListener();
if (tl == UAVCAN_NULLPTR)
{
UAVCAN_ASSERT(0); // Must have been created
cancelCall(out_call_id);
return -ErrLogic;
}
tl->installAcceptanceFilter(this);
/*
*发布请求------Publishing the request
*/
const int publisher_res = publisher_.publish(request, TransferTypeServiceRequest, server_node_id,
out_call_id.transfer_id);
if (publisher_res < 0)
{
cancelCall(out_call_id);
return publisher_res;
}
UAVCAN_ASSERT(server_node_id == out_call_id.server_node_id);
return publisher_res;
}
重点分析一下几个函数
1.callback_函数
这个函数指的是哪个函数呢？还得的初始化时我们设置的回调函数吗？
Led_Ctrl_client[driver_index]->setCallback(led_ctrl_Info);
从这里可以看出callback_指的就是led_ctrl_Info（）函数。
2.prepareToCall（）函数
const int prep_res =prepareToCall(SubscriberType::getNode(), DataType::getDataTypeFullName(), server_node_id, out_call_id);
这里面又四个参数：要弄明白这四个参数的含义
- SubscriberType::getNode()：获取节点对象
- DataType::getDataTypeFullName()：获取驱动名称
- server_node_id：也就是服务器ID
- out_call_id：我们的Node ID
int ServiceClientBase::prepareToCall(INode& node,
const char* dtname,
NodeID server_node_id,
ServiceCallID& out_call_id)
int ServiceClientBase::prepareToCall(INode& node,
const char* dtname,
NodeID server_node_id,
ServiceCallID& out_call_id)
{
/*
* 确保不会因为输入数据无效而导致传输错误
* Making sure we're not going to get transport error because of invalid input data
*/
//判断服务节点是不是非广播，服务节点等于NODE ID吗
if (!server_node_id.isUnicast() || (server_node_id == node.getNodeID()))
{
UAVCAN_TRACE("ServiceClient", "Invalid Server Node ID");
return -ErrInvalidParam;
}
//参数传递
out_call_id.server_node_id = server_node_id;
/*
* 确定数据类型ID
* Determining the Data Type ID
*/
if (data_type_descriptor_ == UAVCAN_NULLPTR)
{
GlobalDataTypeRegistry::instance().freeze();
//查找到我们定义的.uavcan名字
data_type_descriptor_ = GlobalDataTypeRegistry::instance().find(DataTypeKindService, dtname);
if (data_type_descriptor_ == UAVCAN_NULLPTR)
{
UAVCAN_TRACE("ServiceClient", "Type [%s] is not registered", dtname);
return -ErrUnknownDataType;
}
UAVCAN_TRACE("ServiceClient", "Data type descriptor inited: %s", data_type_descriptor_->toString().c_str());
}
UAVCAN_ASSERT(data_type_descriptor_ != UAVCAN_NULLPTR);
/*
* 确定传输ID
* Determining the Transfer ID
*/
const OutgoingTransferRegistryKey otr_key(data_type_descriptor_->getID(),
TransferTypeServiceRequest, server_node_id);
const MonotonicTime otr_deadline = node.getMonotonicTime() + TransferSender::getDefaultMaxTransferInterval();
TransferID* const otr_tid =
node.getDispatcher().getOutgoingTransferRegistry().accessOrCreate(otr_key, otr_deadline);
if (!otr_tid)
{
UAVCAN_TRACE("ServiceClient", "OTR access failure, dtd=%s", data_type_descriptor_->toString().c_str());
return -ErrMemory;
}
out_call_id.transfer_id = *otr_tid;
otr_tid->increment();
return 0;
}
这个函数主要实现：1.判断服务节点是不是非广播，服务节点等于NODE ID吗，2.确定数据类型ID 3.确定传输ID
3.发布请求函数
const int publisher_res = publisher_.publish(request, TransferTypeServiceRequest, server_node_id,
out_call_id.transfer_id);
template <typename DataSpec, typename DataStruct>
int GenericPublisher<DataSpec, DataStruct>::genericPublish(const DataStruct& message, TransferType transfer_type,
NodeID dst_node_id, TransferID* tid,
MonotonicTime blocking_deadline)
{
const int res = checkInit();
if (res < 0)
{
return res;
}
Buffer buffer;
const int encode_res = doEncode(message, buffer);
if (encode_res < 0)
{
return encode_res;
}
return GenericPublisherBase::genericPublish(buffer, transfer_type, dst_node_id, tid, blocking_deadline);
}
int GenericPublisherBase::genericPublish(const StaticTransferBufferImpl& buffer, TransferType transfer_type,
NodeID dst_node_id, TransferID* tid, MonotonicTime blocking_deadline)
{
/*
* tid必须不等于0，我们执行if
*/
if (tid)
{
return sender_.send(buffer.getRawPtr(), buffer.getMaxWritePos(), getTxDeadline(),
blocking_deadline, transfer_type, dst_node_id, *tid);
}
else
{
return sender_.send(buffer.getRawPtr(), buffer.getMaxWritePos(), getTxDeadline(),
blocking_deadline, transfer_type, dst_node_id);
}
}
int TransferSender::send(const uint8_t* payload, unsigned payload_len, MonotonicTime tx_deadline,
MonotonicTime blocking_deadline, TransferType transfer_type, NodeID dst_node_id,
TransferID tid) const
{
Frame frame(data_type_id_, transfer_type, dispatcher_.getNodeID(), dst_node_id, tid);
frame.setPriority(priority_);
frame.setStartOfTransfer(true);
UAVCAN_TRACE("TransferSender", "%s", frame.toString().c_str());
/*
* Checking if we're allowed to send.
* In passive mode we can send only anonymous transfers, if they are enabled.
*/
if (dispatcher_.isPassiveMode())
{
const bool allow = allow_anonymous_transfers_ &&
(transfer_type == TransferTypeMessageBroadcast) &&
(int(payload_len) <= frame.getPayloadCapacity());
if (!allow)
{
return -ErrPassiveMode;
}
}
dispatcher_.getTransferPerfCounter().addTxTransfer();
/*
*发送结构---- Sending frames
*/
if (frame.getPayloadCapacity() >= payload_len) // Single Frame Transfer
{
const int res = frame.setPayload(payload, payload_len);
if (res != int(payload_len))
{
UAVCAN_ASSERT(0);
UAVCAN_TRACE("TransferSender", "Frame payload write failure, %i", res);
registerError();
return (res < 0) ? res : -ErrLogic;
}
frame.setEndOfTransfer(true);
UAVCAN_ASSERT(frame.isStartOfTransfer() && frame.isEndOfTransfer() && !frame.getToggle());
const CanIOFlags flags = frame.getSrcNodeID().isUnicast() ? flags_ : (flags_ | CanIOFlagAbortOnError);
//发送出去
return dispatcher_.send(frame, tx_deadline, blocking_deadline, qos_, flags, iface_mask_);
}
else // Multi Frame Transfer
{
UAVCAN_ASSERT(!dispatcher_.isPassiveMode());
UAVCAN_ASSERT(frame.getSrcNodeID().isUnicast());
int offset = 0;
{
TransferCRC crc = crc_base_;
crc.add(payload, payload_len);
static const int BUFLEN = sizeof(static_cast<CanFrame*>(0)->data);
uint8_t buf[BUFLEN];
buf[0] = uint8_t(crc.get() & 0xFFU); // Transfer CRC, little endian
buf[1] = uint8_t((crc.get() >> 8) & 0xFF);
(void)copy(payload, payload + BUFLEN - 2, buf + 2);
const int write_res = frame.setPayload(buf, BUFLEN);
if (write_res < 2)
{
UAVCAN_TRACE("TransferSender", "Frame payload write failure, %i", write_res);
registerError();
return write_res;
}
offset = write_res - 2;
UAVCAN_ASSERT(int(payload_len) > offset);
}
int num_sent = 0;
while (true)
{
const int send_res = dispatcher_.send(frame, tx_deadline, blocking_deadline, qos_, flags_, iface_mask_);
if (send_res < 0)
{
registerError();
return send_res;
}
num_sent++;
if (frame.isEndOfTransfer())
{
return num_sent; // Number of frames transmitted
}
frame.setStartOfTransfer(false);
frame.flipToggle();
UAVCAN_ASSERT(offset >= 0);
const int write_res = frame.setPayload(payload + offset, payload_len - unsigned(offset));
if (write_res < 0)
{
UAVCAN_TRACE("TransferSender", "Frame payload write failure, %i", write_res);
registerError();
return write_res;
}
offset += write_res;
UAVCAN_ASSERT(offset <= int(payload_len));
if (offset >= int(payload_len))
{
frame.setEndOfTransfer(true);
}
}
}
UAVCAN_ASSERT(0);
return -ErrLogic; // Return path analysis is apparently broken. There should be no warning, this 'return' is unreachable.
}
注意这个函数：dispatcher_.send(frame, tx_deadline, blocking_deadline, qos_, flags, iface_mask_);
int Dispatcher::send(const Frame& frame, MonotonicTime tx_deadline, MonotonicTime blocking_deadline,
CanTxQueue::Qos qos, CanIOFlags flags, uint8_t iface_mask)
{
if (frame.getSrcNodeID() != getNodeID())
{
UAVCAN_ASSERT(0);
return -ErrLogic;
}
CanFrame can_frame;
//形成CAN协议数据格式类型
if (!frame.compile(can_frame))
{
UAVCAN_TRACE("Dispatcher", "Unable to send: frame is malformed: %s", frame.toString().c_str());
UAVCAN_ASSERT(0);
return -ErrLogic;
}
//准备发送出去
return canio_.send(can_frame, tx_deadline, blocking_deadline, iface_mask, qos, flags);
}
这里面有两个函数需要注意。
frame.compile(can_frame)
bool Frame::compile(CanFrame& out_can_frame) const
{
if (!isValid())
{
UAVCAN_ASSERT(0); // This is an application error, so we need to maximize it.
return false;
}
/*
*CAN ID字段------ CAN ID field
*/
//获取CAN ID
out_can_frame.id = CanFrame::FlagEFF |
bitpack<0, 7>(src_node_id_.get()) |
bitpack<24, 5>(transfer_priority_.get());
//判断数据帧的类型
if (transfer_type_ == TransferTypeMessageBroadcast)
{
out_can_frame.id |=
bitpack<7, 1>(0U) |
bitpack<8, 16>(data_type_id_.get());
}
else
{
const bool request_not_response = transfer_type_ == TransferTypeServiceRequest;
out_can_frame.id |=
bitpack<7, 1>(1U) |
bitpack<8, 7>(dst_node_id_.get()) |
bitpack<15, 1>(request_not_response ? 1U : 0U) |
bitpack<16, 8>(data_type_id_.get());
}
/*
*有效帧----- Payload
*/
uint8_t tail = transfer_id_.get(); //还记得刚开始说的这个tail吗
if (start_of_transfer_)
{
tail |= (1U << 7);
}
if (end_of_transfer_)
{
tail |= (1U << 6);
}
if (toggle_)
{
tail |= (1U << 5);
}
UAVCAN_ASSERT(payload_len_ < sizeof(static_cast<CanFrame*>(UAVCAN_NULLPTR)->data));
out_can_frame.dlc = static_cast<uint8_t>(payload_len_);
(void)copy(payload_, payload_ + payload_len_, out_can_frame.data);
out_can_frame.data[out_can_frame.dlc] = tail;
out_can_frame.dlc++;
/*
* Discriminator鉴别器
*/
if (src_node_id_.isBroadcast())
{
TransferCRC crc;
crc.add(out_can_frame.data, out_can_frame.dlc); //value_ = uint16_t(uint16_t((value_ << 8) ^ Table[uint16_t((value_ >> 8) ^ byte) & 0xFFU]) & 0xFFFFU);
out_can_frame.id |= bitpack<10, 14>(crc.get() & ((1U << 14) - 1U));
}
return true;
}
这个函数很重要，是形成CAN ID 字段的关键，如果前面没有设置对，在这里打印检测也可以定位问题。
canio_.send(can_frame, tx_deadline, blocking_deadline, iface_mask, qos, flags)
int CanIOManager::send(const CanFrame& frame, MonotonicTime tx_deadline, MonotonicTime blocking_deadline,
uint8_t iface_mask, CanTxQueue::Qos qos, CanIOFlags flags)
{
const uint8_t num_ifaces = getNumIfaces();
const uint8_t all_ifaces_mask = uint8_t((1U << num_ifaces) - 1);
iface_mask &= all_ifaces_mask;
if (blocking_deadline > tx_deadline)
{
blocking_deadline = tx_deadline;
}
int retval = 0;
while (true) // Somebody please refactor this.
{
if (iface_mask == 0)
{
break;
}
CanSelectMasks masks;
masks.write = iface_mask | makePendingTxMask();
{
// Building the list of next pending frames per iface.
// The driver will give them a scrutinizing look before deciding whether he wants to accept them.
const CanFrame* pending_tx[MaxCanIfaces] = {};
for (int i = 0; i < num_ifaces; i++)
{
CanTxQueue& q = *tx_queues_[i];
if (iface_mask & (1 << i)) // I hate myself so much right now.
{
pending_tx[i] = q.topPriorityHigherOrEqual(frame) ? q.getTopPriorityPendingFrame() : &frame;
}
else
{
pending_tx[i] = q.getTopPriorityPendingFrame();
}
}
//号召选择
const int select_res = callSelect(masks, pending_tx, blocking_deadline);
if (select_res < 0)
{
return -ErrDriver;
}
UAVCAN_ASSERT(masks.read == 0);
}
// Transmission
for (uint8_t i = 0; i < num_ifaces; i++)
{
if (masks.write & (1 << i))
{
int res = 0;
if (iface_mask & (1 << i))
{
if (tx_queues_[i]->topPriorityHigherOrEqual(frame))
{
res = sendFromTxQueue(i); // May return 0 if nothing to transmit (e.g. expired)
}
if (res <= 0)
{
//重要函数
res = sendToIface(i, frame, tx_deadline, flags);
if (res > 0)
{
iface_mask &= uint8_t(~(1 << i)); // Mark transmitted
}
}
}
else
{
res = sendFromTxQueue(i);
}
if (res > 0)
{
retval++;
}
}
}
// Timeout. Enqueue the frame if wasn't transmitted and leave.
const bool timed_out = sysclock_.getMonotonic() >= blocking_deadline;
if (masks.write == 0 || timed_out)
{
if (!timed_out)
{
UAVCAN_TRACE("CanIOManager", "Send: Premature timeout in select(), will try again");
continue;
}
for (uint8_t i = 0; i < num_ifaces; i++)
{
if (iface_mask & (1 << i))
{
tx_queues_[i]->push(frame, tx_deadline, qos, flags);
}
}
break;
}
}
return retval;
}
核心底层接口函数sendToIface(i, frame, tx_deadline, flags);
int CanIOManager::sendToIface(uint8_t iface_index, const CanFrame& frame, MonotonicTime tx_deadline, CanIOFlags flags)
{
UAVCAN_ASSERT(iface_index < MaxCanIfaces);
ICanIface* const iface = driver_.getIface(iface_index);
if (iface == UAVCAN_NULLPTR)
{
UAVCAN_ASSERT(0); // Nonexistent interface
return -ErrLogic;
}
const int res = iface->send(frame, tx_deadline, flags);
if (res != 1)
{
UAVCAN_TRACE("CanIOManager", "Send failed: code %i, iface %i, frame %s",
res, iface_index, frame.toString().c_str());
}
if (res > 0)
{
counters_[iface_index].frames_tx += unsigned(res);
}
return res;
}
uavcan::int16_t CanIface::send(const uavcan::CanFrame& frame, uavcan::MonotonicTime tx_deadline,
uavcan::CanIOFlags flags)
{
if (frame.isErrorFrame() || frame.dlc > 8)
{
return -ErrUnsupportedFrame;
}
/*
* Normally we should perform the same check as in @ref canAcceptNewTxFrame(), because
* it is possible that the highest-priority frame between select() and send() could have been
* replaced with a lower priority one due to TX timeout. But we don't do this check because:
*
* - It is a highly unlikely scenario.
*
* - Frames do not timeout on a properly functioning bus. Since frames do not timeout, the new
* frame can only have higher priority, which doesn't break the logic.
*
* - If high-priority frames are timing out in the TX queue, there's probably a lot of other
* issues to take care of before this one becomes relevant.
*
* - It takes CPU time. Not just CPU time, but critical section time, which is expensive.
*/
CriticalSectionLocker lock;
/*
* Seeking for an empty slot
*/
uavcan::uint8_t txmailbox = 0xFF;
if ((can_->TSR & bxcan::TSR_TME0) == bxcan::TSR_TME0)
{
txmailbox = 0;
}
else if ((can_->TSR & bxcan::TSR_TME1) == bxcan::TSR_TME1)
{
txmailbox = 1;
}
else if ((can_->TSR & bxcan::TSR_TME2) == bxcan::TSR_TME2)
{
txmailbox = 2;
}
else
{
return 0; // No transmission for you.
}
peak_tx_mailbox_index_ = uavcan::max(peak_tx_mailbox_index_, txmailbox); // Statistics
/*
* Setting up the mailbox
*/
bxcan::TxMailboxType& mb = can_->TxMailbox[txmailbox];
//最终把数据合并，传递到发送邮箱寄存器
if (frame.isExtended())
{
//注意这个寄存器地址要王右移3位
mb.TIR = ((frame.id & uavcan::CanFrame::MaskExtID) << 3) | bxcan::TIR_IDE;
}
else
{
mb.TIR = ((frame.id & uavcan::CanFrame::MaskStdID) << 21);
}
if (frame.isRemoteTransmissionRequest())
{
mb.TIR |= bxcan::TIR_RTR;
}
mb.TDTR = frame.dlc;
mb.TDHR = (uavcan::uint32_t(frame.data[7]) << 24) |
(uavcan::uint32_t(frame.data[6]) << 16) |
(uavcan::uint32_t(frame.data[5]) << 8) |
(uavcan::uint32_t(frame.data[4]) << 0);
mb.TDLR = (uavcan::uint32_t(frame.data[3]) << 24) |
(uavcan::uint32_t(frame.data[2]) << 16) |
(uavcan::uint32_t(frame.data[1]) << 8) |
(uavcan::uint32_t(frame.data[0]) << 0);
mb.TIR |= bxcan::TIR_TXRQ; // Go.
/*
* Registering the pending transmission so we can track its deadline and loopback it as needed
*/
TxItem& txi = pending_tx_[txmailbox];
txi.deadline = tx_deadline;
txi.frame = frame;
txi.loopback = (flags & uavcan::CanIOFlagLoopback) != 0;
txi.abort_on_error = (flags & uavcan::CanIOFlagAbortOnError) != 0;
txi.pending = true;
return 1;
}
到这里基本实现了把服务帧数据发送出去。
3.CAN调试方法
所谓工欲善其事必先利其器，我们最好买个USB-CAN 模块方便实时监控我们发的和收到的数据是否正常，接线方式可以见下面的方式。
标签：node,Ardupilot,return,res,frame,学习,UAVCAN,id 来源： https://blog.csdn.net/lixiaoweimashixiao/article/details/106575152