简介
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 initialized
kv_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); // SUPPRESS
std::cout << "I am the highest priority master; the next one has Node ID "
<< int(slave.getMasterNodeID().get()) << std::endl;
}
else
{
//当前网络有更高优先级的主机，不再压制从机模式，
slave.suppress(false); // UNSUPPRESS
std::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 values
hwver.major = 1;
node.setHardwareVersion(hwver);
uavcan::protocol::SoftwareVersion swver; // TODO initialize correct values
swver.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;
}
}
}