PX4中的DroneCAN的实现库Libuavcan及基础功能示例

简介

Libuavcan是一个用C++编写的可移植的跨平台库，对C++标准库的依赖小。它可以由几乎任何符合标准的C++编译器编译，并且可以在几乎任何体系结构/OS上使用。

在 DroneCAN 中，Libuavcan 有一个 DSDL 编译器，将 DSDL 文件转换为 hpp 头文件。编译器名为 libuavcan_dsdlc，调用方式如下：

libuavcan_dsdlc source_dir

其中 source_dir 是包含要编译的数据类型定义的根命名空间目录，例如 dsdl/uavcan。

在编译应用程序和/或库之前，必须在构建过程中调用DSDL编译器来生成头文件。

PX4中DSDL编译流程

结合上述的编译命令，再结合《UAVCAN_V1的实现库libcanard与数据格式DSDL》中 uavcan_v1 调用脚本生成头文件的过程。在 PX4 中，可以看出 DroneCAN 在 PX4 中的编译方式：

在 uavcan 模块的 CMakeLists 文件中，先设置 DSDL 文件的路径以及输出头文件的路径，之后调用 libuavcan_dsdlc 命令生成响应的文件：

节点初始化和启动

为了将UAVCAN功能添加到应用程序中，我们必须创建UAVCAN:：node类的节点对象，这是库 调用 API 函数的基础。该类需要访问 CAN 底层驱动程序和系统时钟，这是特定于平台的组件，因此它们通过以下接口与库隔离：

uavcan::ICanDriver //CAN驱动

uavcan::ICanIface //CAN实例

uavcan::ISystemClock //系统时钟

具体在 PX4 中的体现，在 uavcannode 进程中的 uavcannode 类，在创建节点的时候，都会获取 can 设备以及系统时钟：

在 STM32 底层的 can 驱动中，会调用 ICanIface 类来获取实例：

也就是说，创建一个 uavcan 节点，必须包含上面的这三个类。

内存管理

Libuavcan 不使用堆。

动态内存

动态内存分配由恒定时间的无碎片块内存分配器管理。专用于块分配的内存池的大小通过节点类uavcan:：node 的模板参数来定义。如果未提供模板参数，则节点将期望应用程序向构造函数提供对自定义分配器的引用。

库使用动态内存完成的功能为：

1）为接收多帧传输分配临时缓冲器；

2）保持接收器状态；

3）保存传输ID映射；

4）优先发送队列。

内存池大小的计算

通常，内存池的大小在 4KB（对于简单节点）和 512KB（对于非常复杂的节点）之间。

内存池的最小安全大小可以计算为以下值总和的两倍：

对于每个传入的数据类型，将其最大序列化长度乘以可能发布它的节点数，并将结果相加。

将所有传出数据类型的最大串行化长度相加，乘以节点可用的CAN接口数加1。

示例：

CAN 实例数	2
传入数据类型 A 的发布节点数	3
传入数据类型 A 的序列化后最大字节数	100 bytes
传入数据类型 B 的发布节点数	32
传入数据类型 B 的序列化后最大字节数	24 bytes
传出数据类型 X 的序列化后最大字节数	256 bytes
传出数据类型 Z 的序列化后最大字节数	10 bytes

最终需要分配的内存大小为：

2 * ((3 * 100 + 32 * 24) + (2 + 1) * (256 + 10)) = 3732 bytes

线程

从线程的角度来看，可以使用以下配置：

单线程 ---完全在一个线程中运行。

多线程 ---在两个线程中运行：

主线程，适用于硬实时、低延迟通信；

辅助线程，适用于阻塞、I/O密集型或CPU密集型任务，但不适用于实时任务。

单线程配置

在单线程配置中，库的线程应该在 Node<>::spin() 内阻塞。或者周期性地调用 Node<>::spin() 或Node<>::spinOnce()。

典型示例：

for (;;)
{/*线程调用时间 100ms*/const int error = node.spin(uavcan::MonotonicDuration::fromMSec(100));if (error < 0){std::cerr << "Transient error: " << error << std::endl; // 处理发送错误}
}

Node<>::spin() 和 Node<>::spinOnce() 之间的区别如下：

spin() ---阻塞超时，然后返回，即使某些CAN帧或计时器事件仍在等待处理。

spinOnce() ---在处理完所有可用的CAN帧和计时器事件后，它立即返回，而不是阻塞。

应用示例

以下举出了常用功能的示例：

1）发布与订阅

2）服务类消息

3）时间同步

4）固件升级

还有许多其余的例程，可以进入官网查看相关的解释：https://dronecan.github.io/Implementations/Libuavcan/Tutorials/10._Dynamic_node_ID_allocation/

发布与订阅

以下为一个示例，发布与订阅调试主题的消息。

发布

#include <iostream>
#include <cstdlib>
#include <unistd.h>
#include <uavcan/uavcan.hpp>
//将使用uavcan.protocol.debug.KeyValue类型的消息来进行测试
#include <uavcan/protocol/debug/KeyValue.hpp> // uavcan.protocol.debug.KeyValue
extern uavcan::ICanDriver& getCanDriver();  //获取CAN驱动
extern uavcan::ISystemClock& getSystemClock(); //获取系统时钟
constexpr unsigned NodeMemoryPoolSize = 16384;
typedef uavcan::Node<NodeMemoryPoolSize> Node;
static Node& getNode()
{static Node node(getCanDriver(), getSystemClock());return node;
}
int main(int argc, const char** argv)
{if (argc < 2){std::cerr << "Usage: " << argv[0] << " <node-id>" << std::endl;return 1;}const int self_node_id = std::stoi(argv[1]);auto& node = getNode();node.setNodeID(self_node_id);node.setName("org.uavcan.tutorial.publisher");//启动节点const int node_start_res = node.start();if (node_start_res < 0){throw std::runtime_error("Failed to start the node; error: " + std::to_string(node_start_res));}//创建对应的消息类对象发布器uavcan::Publisher<uavcan::protocol::debug::KeyValue> kv_pub(node);const int kv_pub_init_res = kv_pub.init();if (kv_pub_init_res < 0){throw std::runtime_error("Failed to start the publisher; error: " + std::to_string(kv_pub_init_res));}//设置发布时间间隔kv_pub.setTxTimeout(uavcan::MonotonicDuration::fromMSec(1000));//设置优先级kv_pub.setPriority(uavcan::TransferPriority::MiddleLower);//运行节点node.setModeOperational();while (true){//创建进程，阻塞时间设置为1秒(1秒调用一次)const int spin_res = node.spin(uavcan::MonotonicDuration::fromMSec(1000));if (spin_res < 0){std::cerr << "Transient failure: " << spin_res << std::endl;}//创建消息实例对象uavcan::protocol::debug::KeyValue kv_msg;  // Always zero initializedkv_msg.value = std::rand() / float(RAND_MAX);//设置消息kv_msg.key = "a";   // "a"kv_msg.key += "b";  // "ab"kv_msg.key += "c";  // "abc"//发布消息const int pub_res = kv_pub.broadcast(kv_msg);if (pub_res < 0){std::cerr << "KV publication failure: " << pub_res << std::endl;}}
}

订阅

#include <iostream>
#include <cstdlib>
#include <unistd.h>
#include <uavcan/uavcan.hpp>
#include <uavcan/protocol/debug/KeyValue.hpp>
#include <uavcan/protocol/debug/LogMessage.hpp>
extern uavcan::ICanDriver& getCanDriver();
extern uavcan::ISystemClock& getSystemClock();
constexpr unsigned NodeMemoryPoolSize = 16384;
typedef uavcan::Node<NodeMemoryPoolSize> Node;
static Node& getNode()
{static Node node(getCanDriver(), getSystemClock());return node;
}
int main(int argc, const char** argv)
{if (argc < 2){std::cerr << "Usage: " << argv[0] << " <node-id>" << std::endl;return 1;}const int self_node_id = std::stoi(argv[1]);auto& node = getNode();node.setNodeID(self_node_id);node.setName("org.uavcan.tutorial.subscriber");//运行节点const int node_start_res = node.start();if (node_start_res < 0){throw std::runtime_error("Failed to start the node; error: " + std::to_string(node_start_res));}/**订阅uavcan.protocol.debug.LogMessage类型的标准日志消息。*收到的消息将通过回调传递给应用程序。*回调参数的类型可以是以下两种类型之一：*-T&*-uavcan::接收数据结构<T>&*对于第一个选项，ReceivedDataStructure<T>&将隐式转换为T&。*类uavcan:：ReceivedDataStructure使用从*传输层，如源节点ID、时间戳、传输ID、冗余接口的索引提取*/uavcan::Subscriber<uavcan::protocol::debug::LogMessage> log_sub(node);const int log_sub_start_res = log_sub.start([&](const uavcan::ReceivedDataStructure<uavcan::protocol::debug::LogMessage>& msg){//消息流以 YAML 格式传输std::cout << msg << std::endl;});if (log_sub_start_res < 0){throw std::runtime_error("Failed to start the log subscriber; error: " + std::to_string(log_sub_start_res));}/**订阅uavcan.protocol.debug.KeyValue类型的消息。*这一次，设置另一种类型的消息订阅，采用另一种回调参数类型*则将只是T&而不是uavcan:：ReceivedDataStructure<T>&。*回调将通过std:：cout（也可参考uavcan:：OStream）以YAML格式打印消息。*/uavcan::Subscriber<uavcan::protocol::debug::KeyValue> kv_sub(node);const int kv_sub_start_res =kv_sub.start([&](const uavcan::protocol::debug::KeyValue& msg) { std::cout << msg << std::endl; });if (kv_sub_start_res < 0){throw std::runtime_error("Failed to start the key/value subscriber; error: " + std::to_string(kv_sub_start_res));}//运行节点node.setModeOperational();while (true){//创建线程const int res = node.spin(uavcan::MonotonicDuration::getInfinite());if (res < 0){std::cerr << "Transient failure: " << res << std::endl;}}
}

服务类型消息

以下是服务类型消息的代码示例：

服务端 ---提供固件升级 uavcan.protocol.file.BeginFirmwareUpdate 类型的服务消息。

客户端 ---在指定节点上调用相同的服务类型。服务器的节点 ID 作为 main 函数的命令行参数提供给应用程序。

服务端

#include <cstdio>
#include <iostream>
#include <unistd.h>
#include <uavcan/uavcan.hpp>
#include <uavcan/protocol/file/BeginFirmwareUpdate.hpp>
extern uavcan::ICanDriver& getCanDriver();
extern uavcan::ISystemClock& getSystemClock();
constexpr unsigned NodeMemoryPoolSize = 16384;
typedef uavcan::Node<NodeMemoryPoolSize> Node;
static Node& getNode()
{static Node node(getCanDriver(), getSystemClock());return node;
}
int main(int argc, const char** argv)
{if (argc < 2){std::cerr << "Usage: " << argv[0] << " <node-id>" << std::endl;return 1;}const int self_node_id = std::stoi(argv[1]);auto& node = getNode();node.setNodeID(self_node_id);node.setName("org.uavcan.tutorial.server");const int node_start_res = node.start();if (node_start_res < 0){throw std::runtime_error("Failed to start the node; error: " + std::to_string(node_start_res));}/**创建服务器。 此程序中，服务器只是接收请求，没有做其他的处理程序*服务回调接受两个参数：*-对请求结构（输入）的引用*-对默认初始化响应结构（输出）的引用*输入的类型可以是以下两种之一：*-T:：请求&*-uavcan:：ReceivedDataStructure<T:：Request>&*输出的类型严格为T：：Response&。*注意，对于服务数据结构，不可能实例化T本身*/using uavcan::protocol::file::BeginFirmwareUpdate;uavcan::ServiceServer<BeginFirmwareUpdate> srv(node);const int srv_start_res = srv.start([&](const uavcan::ReceivedDataStructure<BeginFirmwareUpdate::Request>& req, BeginFirmwareUpdate::Response& rsp){std::cout << req << std::endl;rsp.error = rsp.ERROR_UNKNOWN;rsp.optional_error_message = "Our sun is dying";});if (srv_start_res < 0){std::exit(1);  // 错误处理}//开启节点node.setModeOperational();while (true){const int res = node.spin(uavcan::MonotonicDuration::fromMSec(1000));if (res < 0){std::printf("Transient failure: %d\n", res);}}
}

客户端

#include <iostream>
#include <unistd.h>
#include <uavcan/uavcan.hpp>
#include <uavcan/protocol/file/BeginFirmwareUpdate.hpp>
extern uavcan::ICanDriver& getCanDriver();
extern uavcan::ISystemClock& getSystemClock();
constexpr unsigned NodeMemoryPoolSize = 16384;
typedef uavcan::Node<NodeMemoryPoolSize> Node;
static Node& getNode()
{static Node node(getCanDriver(), getSystemClock());return node;
}
int main(int argc, const char** argv)
{if (argc < 3){std::cerr << "Usage: " << argv[0] << " <node-id> <server-node-id>" << std::endl;return 1;}const uavcan::NodeID self_node_id = std::stoi(argv[1]);const uavcan::NodeID server_node_id = std::stoi(argv[2]);auto& node = getNode();node.setNodeID(self_node_id);node.setName("org.uavcan.tutorial.client");const int node_start_res = node.start();if (node_start_res < 0){throw std::runtime_error("Failed to start the node; error: " + std::to_string(node_start_res));}//创建客户端using uavcan::protocol::file::BeginFirmwareUpdate;uavcan::ServiceClient<BeginFirmwareUpdate> client(node);const int client_init_res = client.init();if (client_init_res < 0){throw std::runtime_error("Failed to init the client; error: " + std::to_string(client_init_res));}//设置回调client.setCallback([](const uavcan::ServiceCallResult<BeginFirmwareUpdate>& call_result){if (call_result.isSuccessful())  // Whether the call was successful, i.e. whether the response was received{// The result can be directly streamed; the output will be formatted in human-readable YAML.std::cout << call_result << std::endl;}else{std::cerr << "Service call to node "<< static_cast<int>(call_result.getCallID().server_node_id.get())<< " has failed" << std::endl;}});//设置请求时间间隔client.setRequestTimeout(uavcan::MonotonicDuration::fromMSec(200));//设置优先级client.setPriority(uavcan::TransferPriority::OneHigherThanLowest);//调用远程服务，设置固件路径BeginFirmwareUpdate::Request request;request.image_file_remote_path.path = "/foo/bar";/* 可以使用一个客户端执行多个请求的回调*  int call(NodeID server_node_id, const RequestType& request)*      初始化一个新的请求回调**  int call(NodeID server_node_id, const RequestType& request, ServiceCallID& out_call_id)*      启动新的非阻塞调用并通过引用返回其ServiceCallID描述符。*      描述符允许查询呼叫进度或稍后取消呼叫。**  void cancelCall(ServiceCallID call_id)*      使用描述符取消回调**  void cancelAllCalls()*      取消所有回调**  bool hasPendingCallToServer(NodeID server_node_id) const*      客户端对象当前是否对给定服务器有挂起的调用。**  unsigned getNumPendingCalls() const*      返回当前挂起的回调总数。**  bool hasPendingCalls() const*      客户端对象当前是否有任何挂起的回调。*/const int call_res = client.call(server_node_id, request);if (call_res < 0){throw std::runtime_error("Unable to perform service call: " + std::to_string(call_res));}//创建线程node.setModeOperational();while (client.hasPendingCalls())  // 是否有回调产生被挂起还未执行const int res = node.spin(uavcan::MonotonicDuration::fromMSec(10));if (res < 0){std::cerr << "Transient failure: " << res << std::endl;}}return 0;
}

时间同步

以下示例演示了如何使用 libuavcan 实现网络范围的时间同步。

包含两个应用程序：

时间同步从机 ---一个非常简单的应用程序，用于将本地时钟与网络时间同步。

时间同步主机 ---功能齐全的主机，可以与同一网络中的其他冗余主机一起工作。

时间同步中的主机逻辑稍微复杂了一些： 如果当前模块是主机的话，会同时再创建一个从机运行，这是做的一个冗余机制，当检测到网络中有更高优先级的主机时，当前节点会启用从机 (suppress(false))，之后与新主机进行同步。

从机

#include <iostream>
#include <cstdio>
#include <unistd.h>
#include <uavcan/uavcan.hpp>
/**标准应用程序级函数的实现位于uavcan/protocol/中。*同一路径还包含标准数据类型uavcan.procol.*。*/
#include <uavcan/protocol/global_time_sync_slave.hpp>
extern uavcan::ICanDriver& getCanDriver();
extern uavcan::ISystemClock& getSystemClock();
constexpr unsigned NodeMemoryPoolSize = 16384;
int main(int argc, const char** argv)
{if (argc < 2){std::cerr << "Usage: " << argv[0] << " <node-id>" << std::endl;return 1;}const int self_node_id = std::stoi(argv[1]);uavcan::Node<NodeMemoryPoolSize> node(getCanDriver(), getSystemClock());node.setNodeID(self_node_id);node.setName("org.uavcan.tutorial.time_sync_slave");const int node_start_res = node.start();if (node_start_res < 0){throw std::runtime_error("Failed to start the node; error: " + std::to_string(node_start_res));}//创建时间同步从设备 每个节点只能存在一个时间同步从设备//每次从设备能够确定时钟相位误差时，它都会调用//平台驱动程序方法 //uavcan:：ISystemClock:：adjustUtc（uavcan：：UtcDuration adjustment）。//通常从设备每1-2秒调整一次，不过取决于主设备的广播时间uavcan::GlobalTimeSyncSlave slave(node);const int slave_init_res = slave.start();if (slave_init_res < 0){throw std::runtime_error("Failed to start the time sync slave; error: " + std::to_string(slave_init_res));}//创建线程node.setModeOperational();while (true){const int spin_res = node.spin(uavcan::MonotonicDuration::fromMSec(1000));if (spin_res < 0){std::cerr << "Transient failure: " << spin_res << std::endl;}//每秒打印一次从设备状态const bool active = slave.isActive(); //是否可以与主机进行同步const int master_node_id = slave.getMasterNodeID().get();  // 如果 (active == false)，则返回无效节点const long msec_since_last_adjustment = (node.getMonotonicTime() - slave.getLastAdjustmentTime()).toMSec();std::printf("Time sync slave status:\n""    Active: %d\n""    Master Node ID: %d\n""    Last clock adjustment was %ld ms ago\n\n",int(active), master_node_id, msec_since_last_adjustment);/** 注意，libuavcan使用两种不同的时间尺度：** 1.单调时间-由类uavcan:：MonotonicTime和uavcan：：MonotomicDuration表示。*这个时间是稳定和单调的；它测量了一些从过去未指定的开始的时间量，它通常不会跳跃或*明显改变速率。*在Linux上，它通过clock_gettime（clock_MONOTONIC，…）访问。**2.UTC时间-由类uavcan:：UtcTime和uavcan：：UtcDuration表示。*这是可以（但不一定）与网络时间同步的实时时间。*这个时间不是稳定单调的，因为它可以改变速率并前后跳跃，以消除与全球网络时间的差异。*请注意，尽管它的名称是UTC，但这个时间刻度不需要严格是UTC时间，不过建议使用UTC。*在Linux上，它可以通过gettimeofday（…）访问。**这两个时钟都可以通过方法INode::getMonotonicTime()和INode::getUtcTime()访问。*注意，时间表示是类型安全的，因为不可能在同一表达式中混合UTC和单调时间（编译将失败）。*/uavcan::MonotonicTime mono_time = node.getMonotonicTime();uavcan::UtcTime utc_time = node.getUtcTime();//打印当前时间std::printf("Current time in seconds: Monotonic: %s   UTC: %s\n",mono_time.toString().c_str(), utc_time.toString().c_str());//从 1234 us 开始计时，加上此时计算后的时间mono_time += uavcan::MonotonicDuration::fromUSec(1234);utc_time += uavcan::UtcDuration::fromUSec(1234);//打印从 1234 us 计时之后计算的时间std::printf("1234 usec later: Monotonic: %s   UTC: %s\n",mono_time.toString().c_str(), utc_time.toString().c_str());}
}

fromUSec 函数用于设置从哪个 us 时间开始计时，即设置开始时间点。

主机

#include <iostream>
#include <cstdio>
#include <unistd.h>
#include <uavcan/uavcan.hpp>
#include <uavcan/protocol/global_time_sync_master.hpp>
#include <uavcan/protocol/global_time_sync_slave.hpp>
extern uavcan::ICanDriver& getCanDriver();
extern uavcan::ISystemClock& getSystemClock();
constexpr unsigned NodeMemoryPoolSize = 16384;
int main(int argc, const char** argv)
{if (argc < 2){std::cerr << "Usage: " << argv[0] << " <node-id>" << std::endl;return 1;}const int self_node_id = std::stoi(argv[1]);uavcan::Node<NodeMemoryPoolSize> node(getCanDriver(), getSystemClock());node.setNodeID(self_node_id);node.setName("org.uavcan.tutorial.time_sync_master");const int node_start_res = node.start();if (node_start_res < 0){throw std::runtime_error("Failed to start the node; error: " + std::to_string(node_start_res));}//初始化时间同步主机，每个节点最多只能存在一个时间同步主机uavcan::GlobalTimeSyncMaster master(node);const int master_init_res = master.init();if (master_init_res < 0){throw std::runtime_error("Failed to start the time sync master; error: " + std::to_string(master_init_res));}//再创建了一个时间同步从机，这是做了一个冗余机制，如果发现总线中有优先级//更高的主机，则此节点由主机变为从机，与新主机进行同步，每个节点也最多只有//一个从机。uavcan::GlobalTimeSyncSlave slave(node);const int slave_init_res = slave.start();if (slave_init_res < 0){throw std::runtime_error("Failed to start the time sync slave; error: " + std::to_string(slave_init_res));}//创建一个定时器，每秒发布一次时间同步主题消息uavcan::Timer master_timer(node);master_timer.setCallback([&](const uavcan::TimerEvent&){//检查网络中是否由优先级更高的主机，如果有，则节点切换到从机if (slave.isActive())  // "Active" means that the slave tracks at least one remote master in the network{if (node.getNodeID() < slave.getMasterNodeID()){//当前节点是最高优先级的主机 调用 suppress 函数，压制从机模式slave.suppress(true);  // SUPPRESSstd::cout << "I am the highest priority master; the next one has Node ID "<< int(slave.getMasterNodeID().get()) << std::endl;}else{//当前网络有更高优先级的主机，不再压制从机模式，slave.suppress(false);  // UNSUPPRESSstd::cout << "Syncing with a higher priority master with Node ID "<< int(slave.getMasterNodeID().get()) << std::endl;}}else{//当前节点主机是唯一时钟源slave.suppress(true);std::cout << "No other masters detected in the network" << std::endl;}//发布同步消息const int res = master.publish();if (res < 0){std::cout << "Time sync master transient failure: " << res << std::endl;}});master_timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000));//创建线程node.setModeOperational();while (true){const int spin_res = node.spin(uavcan::MonotonicDuration::getInfinite());if (spin_res < 0){std::cerr << "Transient failure: " << spin_res << std::endl;}}
}

固件升级

固件升级分两个模块，一个是升级固件模块(类比飞控)，另一个是被升级模块(类比电调)。

升级固件模块 ---此模块运行活动节点监视器，当远程节点响应 uavcan.protocol.GetNodeInfo请求时，模块会检查其固件映像是否比当前运行的节点固件版本更新。如果是这种情况，应用程序会向节点发送 uavcan.protocol.file.BeginFirmwareUpdate 类型的请求。该模块还实现了文件服务器，用于固件更新过程。

被升级模块 ---响应 uavcan.protocol.file.BeginFirmwareUpdate 类型的请求，以及使用服务uavcan.propertiesfile.Read 下载文件。

升级固件模块

#include <iostream>
#include <cstdlib>
#include <cctype>
#include <vector>
#include <algorithm>
#include <unistd.h>
#include <uavcan/uavcan.hpp>
#include <uavcan/protocol/firmware_update_trigger.hpp>  // uavcan::FirmwareUpdateTrigger
#include <uavcan/protocol/node_info_retriever.hpp>      // uavcan::NodeInfoRetriever (see tutorial "Node discovery")
//需要使用 POSIX 标准函数
#include <uavcan_posix/basic_file_server_backend.hpp>
#include <glob.h>                                       // POSIX glob() function
extern uavcan::ICanDriver& getCanDriver();
extern uavcan::ISystemClock& getSystemClock();/*
工作方式如下：uavcan::FirmwareUpdateTrigger通过接口 uavcan::INodeInfoListener
订阅来自 uavcan::NodeInfoRetriever 的节点信息报告。
每当 FirmwareUpdateTrigger 接收到有关新节点的信息时，它都会通过
界面 uavcan::IFirmwareVersionChecker将此信息转发给应用程序。
然后，应用程序将使用提供的信息检查节点是否需要固件更新，并将检查结果
报告回 FirmwareUpdateTrigger。如果节点需要更新，FirmwareUpdateTrigger将向其
发送请求 uavcan.protocol.file.BeginFirmwareUpdate；否则该节点将被忽略，
直到它重新启动或重新出现在总线上。如果节点未响应更新请求，FirmwareUpdateTrigger 
将重试。
如果节点响应错误，FirmwareUpdateTrigger 将使用相同的接口询问应用程序是否需要重试。
*/
class ExampleFirmwareVersionChecker final : public uavcan::IFirmwareVersionChecker
{/*** 当类获得对GetNodeInfo请求的响应时，将调用此方法。* @param node_id                   从中接收此GetNodeInfo响应的节点ID。* @param node_info                 实际节点信息结构* @param out_firmware_file_path    通过此参数返回固件路径*                                  注意，必须通过uavcan.protocol.file.Read服务访问此路径。* @return                          True - 开始发送更新请求*                                  False - 改节点被忽略*/bool shouldRequestFirmwareUpdate(uavcan::NodeID node_id,const uavcan::protocol::GetNodeInfo::Response& node_info,FirmwareFilePath& out_firmware_file_path)override{//需要根据输入来决定节点是否需要更新。//PX4和APM都利用libuavcan的POSIX平台驱动程序提供的类-//uavcan_posix：：FirmwareVersionChecker-实现通用固件版本检查算法。//该算法的工作原理如下：//1. 检查文件系统是否具有给定节点的固件文件。//2. 将给定节点的本地固件映像的CRC与该节点当前正在运行的固件的CRC进行比较//(后者可通过该方法中的节点信息参数获得)。//3. 如果CRC不匹配，则请求更新，否则不要请求更新。        std::cout << "Checking firmware version of node " << int(node_id.get()) << "; node info:\n"<< node_info << std::endl;/** 正在查找匹配的固件*/auto files = findAvailableFirmwareFiles(node_info);if (files.empty()){std::cout << "No firmware files found for this node" << std::endl;return false;}std::cout << "Matching firmware files:";for (auto x: files){std::cout << "\n\t" << x << "\n" << parseFirmwareFileName(x.c_str()) << std::endl;}/** 查找具有最高版本号的固件*/std::string best_file_name;unsigned best_combined_version = 0;for (auto x: files){const auto inf = parseFirmwareFileName(x.c_str());const unsigned combined_version = (unsigned(inf.software_version.major) << 8) + inf.software_version.minor;if (combined_version >= best_combined_version){best_combined_version = combined_version;best_file_name = x;}}std::cout << "Preferred firmware: " << best_file_name << std::endl;const auto best_firmware_info = parseFirmwareFileName(best_file_name.c_str());/** 将最新固件与实际固件进行比较，如果两者不同，才会更新*/if (best_firmware_info.software_version.major == node_info.software_version.major &&best_firmware_info.software_version.minor == node_info.software_version.minor &&best_firmware_info.software_version.vcs_commit == node_info.software_version.vcs_commit){std::cout << "Firmware is already up-to-date" << std::endl;return false;}/**固件文件路径：不得超过40个字符。*当前使用的固件文件名格式可能会导致超过长度限制，因此*通过计算哈希CRC64 并将其用作固件文件的符号链接的名称来解决。*/out_firmware_file_path = makeFirmwareFileSymlinkName(best_file_name.c_str(), best_file_name.length());(void)std::remove(out_firmware_file_path.c_str());const int symlink_res = ::symlink(best_file_name.c_str(), out_firmware_file_path.c_str());if (symlink_res < 0){std::cout << "Could not create symlink: " << symlink_res << std::endl;return false;}std::cout << "Firmware file symlink: " << out_firmware_file_path.c_str() << std::endl;return true;}/***当节点以错误响应更新请求时，将调用此方法。如果请求只是超时，则不会调用此方法。*注意，如果在响应到达时节点已被删除，则不会调用此方法。*特殊情况：如果节点以ERROR_IN_PROGRESS响应，则类将假定不再需要进一步的请求。*不会调用此方法。** @param node_id                   返回错误的节点ID     * @param error_response            错误响应内容* @param out_firmware_file_path    固件路径，如果需要设置新的路径，则设置，如果*                                  还是旧路径，则采用原来的值                                                                                                                                 * @return                          True - 该类将继续发送具有新固件路径的更新请求。*                                  False - 改节点将忽略*/bool shouldRetryFirmwareUpdate(uavcan::NodeID node_id,const uavcan::protocol::file::BeginFirmwareUpdate::Response& error_response,FirmwareFilePath& out_firmware_file_path)override{/** 如果节点响应错误，则取消更新*/std::cout << "The node " << int(node_id.get()) << " has rejected the update request; file path was:\n"<< "\t" << out_firmware_file_path.c_str()<< "\nresponse was:\n"<< error_response << std::endl;return false;}/*** 当节点响应更新请求并进行确认时，将调用此节点。* @param node_id   确认请求的节点ID* @param response  实际响应*/void handleFirmwareUpdateConfirmation(uavcan::NodeID node_id,const uavcan::protocol::file::BeginFirmwareUpdate::Response& response)override{std::cout << "Node " << int(node_id.get()) << " has confirmed the update request; response was:\n"<< response << std::endl;}//计算固件的符号链接的名称static FirmwareFilePath makeFirmwareFileSymlinkName(const char* file_name, unsigned file_name_length){uavcan::DataTypeSignatureCRC hash;hash.add(reinterpret_cast<const std::uint8_t*>(file_name), file_name_length);auto hash_val = hash.get();static const char Charset[] = "0123456789abcdefghijklmnopqrstuvwxyz";static const unsigned CharsetSize = sizeof(Charset) - 1;FirmwareFilePath out;while (hash_val > 0){out.push_back(Charset[hash_val % CharsetSize]);hash_val /= CharsetSize;}out += ".bin";return out;}//从固件中提取版本信息static uavcan::protocol::GetNodeInfo::Response parseFirmwareFileName(const char* name){// 必须使用静态变量避免堆分配static const auto extract_uint8 = [](unsigned& pos, const char* name){std::uint8_t res = 0;pos++;while (std::isdigit(name[pos])){res = res * 10 + int(name[pos] - '0');pos++;}return res;};uavcan::protocol::GetNodeInfo::Response res;unsigned pos = 0;while (name[pos] != '-'){res.name.push_back(name[pos]);pos++;}res.hardware_version.major = extract_uint8(pos, name);res.hardware_version.minor = extract_uint8(pos, name);res.software_version.major = extract_uint8(pos, name);res.software_version.minor = extract_uint8(pos, name);pos++;res.software_version.vcs_commit = ::strtoul(name + pos, nullptr, 16);res.software_version.optional_field_flags = res.software_version.OPTIONAL_FIELD_FLAG_VCS_COMMIT;return res;}//返回可用于给定节点信息结构的固件文件static std::vector<std::string> findAvailableFirmwareFiles(const uavcan::protocol::GetNodeInfo::Response& info){std::vector<std::string> glob_matches;const std::string glob_pattern = std::string(info.name.c_str()) + "-" +std::to_string(info.hardware_version.major) + "." +std::to_string(info.hardware_version.minor) + "-*.uavcan.bin";auto result = ::glob_t();const int res = ::glob(glob_pattern.c_str(), 0, nullptr, &result);if (res != 0){::globfree(&result);if (res == GLOB_NOMATCH){return glob_matches;}throw std::runtime_error("Can't glob()");}for(unsigned i = 0; i < result.gl_pathc; ++i){glob_matches.emplace_back(result.gl_pathv[i]);}::globfree(&result);return glob_matches;}
};
int main(int argc, const char** argv)
{if (argc < 2){std::cerr << "Usage: " << argv[0] << " <node-id>" << std::endl;return 1;}const int self_node_id = std::stoi(argv[1]);/** 初始化节点*通常，充当固件更新器的节点还将实现动态节点ID分配器。**在大多数实际应用程序中，依赖于阻塞API的功能（例如此固件更新功能）*必须在辅助线程中实现，以便不干扰主线程的实时处理。*在该固件更新程序的情况下，干扰可能由相对密集的处理*和阻止对文件系统API的调用引起。*/uavcan::Node<16384> node(getCanDriver(), getSystemClock());node.setNodeID(self_node_id);node.setName("org.uavcan.tutorial.updater");const int node_start_res = node.start();if (node_start_res < 0){throw std::runtime_error("Failed to start the node; error: " + std::to_string(node_start_res));}/** 初始化节点信息检索器* 它将由固件版本检查器来调用*/uavcan::NodeInfoRetriever node_info_retriever(node);const int retriever_res = node_info_retriever.start();if (retriever_res < 0){throw std::runtime_error("Failed to start the node info retriever: " + std::to_string(retriever_res));}/** 初始化固件更新触发器**此类监视uavcan:：NodeInfoRetriever的输出，并使用该输出决定哪些节点需要*更新固件。当检测到需要更新的节点时，该类发送服务请求*uavcan.protocol.file.BeginFirmware 更新。*/ExampleFirmwareVersionChecker checker;uavcan::FirmwareUpdateTrigger trigger(node, checker);const int trigger_res = trigger.start(node_info_retriever);if (trigger_res < 0){throw std::runtime_error("Failed to start the firmware update trigger: " + std::to_string(trigger_res));}/** 初始化文件服务器*/uavcan_posix::BasicFileServerBackend file_server_backend(node);uavcan::FileServer file_server(node, file_server_backend);const int file_server_res = file_server.start();if (file_server_res < 0){throw std::runtime_error("Failed to start the file server: " + std::to_string(file_server_res));}std::cout << "Started successfully" << std::endl;/** 创建进程，运行节点*/node.setModeOperational();while (true){const int res = node.spin(uavcan::MonotonicDuration::getInfinite());if (res < 0){std::cerr << "Transient failure: " << res << std::endl;}}
}

被升级模块

#include <iostream>
#include <cstdlib>
#include <vector>
#include <iomanip>
#include <unistd.h>
#include <uavcan/uavcan.hpp>
#include <uavcan/protocol/file/BeginFirmwareUpdate.hpp>
#include <uavcan/protocol/file/Read.hpp>
extern uavcan::ICanDriver& getCanDriver();
extern uavcan::ISystemClock& getSystemClock();
/*** 此类将从指定位置下载固件到内存*/
class FirmwareLoader : private uavcan::TimerBase
{
public:/*** 状态转换过程* 正在进行 ---[后台工作中]----> 成功*                       \ --> 失败*/enum class Status{InProgress,Success,Failure};
private:const uavcan::NodeID source_node_id_;const uavcan::protocol::file::Path::FieldTypes::path source_path_;std::vector<std::uint8_t> image_;typedef uavcan::MethodBinder<FirmwareLoader*,void (FirmwareLoader::*)(const uavcan::ServiceCallResult<uavcan::protocol::file::Read>&)>ReadResponseCallback;uavcan::ServiceClient<uavcan::protocol::file::Read, ReadResponseCallback> read_client_;Status status_ = Status::InProgress;void handleTimerEvent(const uavcan::TimerEvent&) final override{if (!read_client_.hasPendingCalls()){uavcan::protocol::file::Read::Request req;req.path.path = source_path_;req.offset = image_.size();const int res = read_client_.call(source_node_id_, req);if (res < 0){std::cerr << "Read call failed: " << res << std::endl;}}}void handleReadResponse(const uavcan::ServiceCallResult<uavcan::protocol::file::Read>& result){if (result.isSuccessful() &&result.getResponse().error.value == 0){auto& data = result.getResponse().data;image_.insert(image_.end(), data.begin(), data.end());if (data.size() < data.capacity())  // Termination condition{status_ = Status::Success;uavcan::TimerBase::stop();}}else{status_ = Status::Failure;uavcan::TimerBase::stop();}}
public:/*** 创建对象后开始下载* 销毁对象可以取消进程*/FirmwareLoader(uavcan::INode& node,uavcan::NodeID source_node_id,const uavcan::protocol::file::Path::FieldTypes::path& source_path) :uavcan::TimerBase(node),source_node_id_(source_node_id),source_path_(source_path),read_client_(node){image_.reserve(1024);   // 任意值/** 响应优先级等于请求优先级* 通常情况下，文件 IO 执行的优先级应设置得非常低*/read_client_.setPriority(uavcan::TransferPriority::OneHigherThanLowest);read_client_.setCallback(ReadResponseCallback(this, &FirmwareLoader::handleReadResponse));/** 设置频率进行限速*/uavcan::TimerBase::startPeriodic(uavcan::MonotonicDuration::fromMSec(200));}/*** 此函数可以用于检测是否完成下载*/Status getStatus() const { return status_; }/*** 返回下载的固件镜像*/const std::vector<std::uint8_t>& getImage() const { return image_; }
};
/*** This function is used to display the downloaded image.*/
template <typename InputIterator>
void printHexDump(InputIterator begin, const InputIterator end)
{struct RAIIFlagsSaver{const std::ios::fmtflags flags_ = std::cout.flags();~RAIIFlagsSaver() { std::cout.flags(flags_); }} _flags_saver;static constexpr unsigned BytesPerRow = 16;unsigned offset = 0;std::cout << std::hex << std::setfill('0');do{std::cout << std::setw(8) << offset << "  ";offset += BytesPerRow;{auto it = begin;for (unsigned i = 0; i < BytesPerRow; ++i){if (i == 8){std::cout << ' ';}if (it != end){std::cout << std::setw(2) << unsigned(*it) << ' ';++it;}else{std::cout << "   ";}}}std::cout << "  ";for (unsigned i = 0; i < BytesPerRow; ++i){if (begin != end){std::cout << ((unsigned(*begin) >= 32U && unsigned(*begin) <= 126U) ? char(*begin) : '.');++begin;}else{std::cout << ' ';}}std::cout << std::endl;}while (begin != end);
}
int main(int argc, const char** argv)
{if (argc < 2){std::cerr << "Usage: " << argv[0] << " <node-id>" << std::endl;return 1;}const int self_node_id = std::stoi(argv[1]);/** 初始化节点* 软件版本与硬件版本在固件更新中至关重要*/uavcan::Node<16384> node(getCanDriver(), getSystemClock());node.setNodeID(self_node_id);node.setName("org.uavcan.tutorial.updatee");uavcan::protocol::HardwareVersion hwver;    // TODO initialize correct valueshwver.major = 1;node.setHardwareVersion(hwver);uavcan::protocol::SoftwareVersion swver;    // TODO initialize correct valuesswver.major = 1;node.setSoftwareVersion(swver);const int node_start_res = node.start();if (node_start_res < 0){throw std::runtime_error("Failed to start the node; error: " + std::to_string(node_start_res));}/** 固件下载的存储对象*/uavcan::LazyConstructor<FirmwareLoader> fw_loader;/** 初始化 BeginFirmwareUpdate 服务.*/uavcan::ServiceServer<uavcan::protocol::file::BeginFirmwareUpdate> bfu_server(node);const int bfu_res = bfu_server.start([&fw_loader, &node](const uavcan::ReceivedDataStructure<uavcan::protocol::file::BeginFirmwareUpdate::Request>& req,uavcan::protocol::file::BeginFirmwareUpdate::Response resp){std::cout << "Firmware update request:\n" << req << std::endl;if (fw_loader.isConstructed()){resp.error = resp.ERROR_IN_PROGRESS;}else{const auto source_node_id = (req.source_node_id == 0) ? req.getSrcNodeID() : req.source_node_id;fw_loader.construct<uavcan::INode&, uavcan::NodeID, decltype(req.image_file_remote_path.path)>(node, source_node_id, req.image_file_remote_path.path);}std::cout << "Response:\n" << resp << std::endl;});if (bfu_res < 0){throw std::runtime_error("Failed to start the BeginFirmwareUpdate server: " + std::to_string(bfu_res));}/** 运行节点(后台运行)*/while (true){if (fw_loader.isConstructed()){node.setModeSoftwareUpdate();if (fw_loader->getStatus() != FirmwareLoader::Status::InProgress){if (fw_loader->getStatus() == FirmwareLoader::Status::Success){auto image = fw_loader->getImage();std::cout << "Firmware download succeeded [" << image.size() << " bytes]" << std::endl;printHexDump(std::begin(image), std::end(image));// TODO: save the firmware image somewhere.}else{std::cout << "Firmware download failed" << std::endl;// TODO: handle the error, e.g. retry download, send a log message, etc.}fw_loader.destroy();}}else{node.setModeOperational();}const int res = node.spin(uavcan::MonotonicDuration::fromMSec(500));if (res < 0){std::cerr << "Transient failure: " << res << std::endl;}}
}