34 #include <px4_platform_common/tasks.h> 37 #include <nuttx/config.h> 44 #include <mathlib/mathlib.h> 48 #include <arch/board/board.h> 49 #include <arch/chip/chip.h> 55 #include <uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp> 56 #include <uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp> 57 #include <uavcan_posix/firmware_version_checker.hpp> 63 #include <v2.0/common/mavlink.h> 80 _vdriver(NumIfaces, UAVCAN_DRIVER::SystemClock::
instance(), main_node.getAllocator(), VirtualIfaceBlockAllocationQuota),
81 _subnode(_vdriver, UAVCAN_DRIVER::SystemClock::
instance(), main_node.getAllocator()),
82 _main_node(main_node),
83 _server_instance(_subnode, _storage_backend, _tracer),
84 _fileserver_backend(_subnode),
85 _node_info_retriever(_subnode),
86 _fw_upgrade_trigger(_subnode, _fw_version_checker),
87 _fw_server(_subnode, _fileserver_backend),
88 _param_getset_client(_subnode),
89 _param_opcode_client(_subnode),
90 _param_restartnode_client(_subnode),
92 _enumeration_indication_sub(_subnode),
93 _enumeration_client(_subnode),
94 _enumeration_getset_client(_subnode),
95 _enumeration_save_client(_subnode)
105 _main_node.getDispatcher().removeRxFrameListener();
113 if (server ==
nullptr) {
114 PX4_INFO(
"Already stopped");
119 PX4_INFO(
"stopping fw srv thread...");
126 server->
_main_node.getDispatcher().removeRxFrameListener();
136 PX4_INFO(
"Already started");
146 PX4_ERR(
"Out of memory");
153 PX4_ERR(
"Node init failed: %d", rv);
162 pthread_attr_t tattr;
163 struct sched_param param;
165 pthread_attr_init(&tattr);
166 (void)pthread_attr_getschedparam(&tattr, ¶m);
167 tattr.stacksize = PX4_STACK_ADJUSTED(
StackSize);
170 if (pthread_attr_setschedparam(&tattr, ¶m)) {
171 PX4_ERR(
"setting sched params failed");
174 static auto run_trampoline = [](
void *) {
return UavcanServers::_instance->
run(
_instance);};
176 rv = pthread_create(&
_instance->
_subnode_thread, &tattr, static_cast<pthread_startroutine_t>(run_trampoline), NULL);
180 PX4_ERR(
"pthread_create() failed: %d", rv);
200 PX4_ERR(
"Lock init: %d", errno);
216 PX4_ERR(
"FirmwareVersionChecker init: %d, errno: %d", ret, errno);
225 PX4_ERR(
"BasicFileServer init: %d, errno: %d", ret, errno);
234 PX4_ERR(
"FileStorageBackend init: %d, errno: %d", ret, errno);
243 PX4_ERR(
"FileEventTracer init: %d, errno: %d", ret, errno);
248 uavcan::protocol::HardwareVersion hwver;
255 PX4_ERR(
"CentralizedServer init: %d", ret);
263 PX4_ERR(
"NodeInfoRetriever init: %d", ret);
271 PX4_ERR(
"FirmwareUpdateTrigger init: %d", ret);
282 prctl(PR_SET_NAME,
"uavcan fw srv", 0);
321 const int spin_res =
_subnode.spin(uavcan::MonotonicDuration::fromMSec(10));
324 PX4_ERR(
"node spin error %i", spin_res);
330 param_request_sub.copy(&request);
337 if (request.message_type == MAVLINK_MSG_ID_PARAM_REQUEST_READ) {
338 uavcan::protocol::param::GetSet::Request req;
340 if (request.param_index >= 0) {
344 req.name = (
char *)request.param_id;
350 PX4_ERR(
"UAVCAN command bridge: couldn't send GetSet: %d", call_res);
357 }
else if (request.message_type == MAVLINK_MSG_ID_PARAM_SET) {
358 uavcan::protocol::param::GetSet::Request req;
360 if (request.param_index >= 0) {
361 req.index = request.param_index;
364 req.name = (
char *)request.param_id;
367 if (request.param_type == MAV_PARAM_TYPE_REAL32) {
368 req.value.to<uavcan::protocol::param::Value::Tag::real_value>() = request.real_value;
370 }
else if (request.param_type == MAV_PARAM_TYPE_UINT8) {
371 req.value.to<uavcan::protocol::param::Value::Tag::boolean_value>() = request.int_value;
374 req.value.to<uavcan::protocol::param::Value::Tag::integer_value>() = request.int_value;
383 PX4_ERR(
"UAVCAN command bridge: couldn't send GetSet: %d", call_res);
390 }
else if (request.message_type == MAVLINK_MSG_ID_PARAM_REQUEST_LIST) {
397 PX4_INFO(
"UAVCAN command bridge: starting component-specific param list");
400 }
else if (request.node_id == MAV_COMP_ID_ALL) {
401 if (request.message_type == MAVLINK_MSG_ID_PARAM_REQUEST_LIST) {
411 PX4_INFO(
"UAVCAN command bridge: starting global param list with node %hhu",
_param_list_node_id);
430 PX4_INFO(
"UAVCAN command bridge: completed param list for node %hhu", _param_list_node_id);
439 _param_list_node_id = next_id;
452 PX4_INFO(
"UAVCAN command bridge: started param list for node %hhu", _param_list_node_id);
462 uavcan::protocol::param::GetSet::Request req;
469 PX4_ERR(
"UAVCAN command bridge: couldn't send param list GetSet: %d", call_res);
481 uint8_t cmd_ack_result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
483 if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_UAVCAN) {
484 int command_id =
static_cast<int>(cmd.param1 + 0.5f);
485 int node_id =
static_cast<int>(cmd.param2 + 0.5f);
488 PX4_INFO(
"UAVCAN command bridge: received UAVCAN command ID %d, node ID %d", command_id, node_id);
490 switch (command_id) {
496 uavcan::protocol::enumeration::Begin::Request req;
499 req.parameter_name =
"esc_index";
504 PX4_ERR(
"UAVCAN ESC enumeration: couldn't send initial Begin request: %d", call_res);
506 cmd_ack_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
516 PX4_ERR(
"UAVCAN command bridge: unknown command ID %d", command_id);
517 cmd_ack_result = vehicle_command_ack_s::VEHICLE_RESULT_UNSUPPORTED;
522 }
else if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE) {
523 int command_id =
static_cast<int>(cmd.param1 + 0.5f);
525 PX4_INFO(
"UAVCAN command bridge: received storage command ID %d", command_id);
527 switch (command_id) {
558 ack.command = cmd.command;
559 ack.result = cmd_ack_result;
560 ack.target_system = cmd.source_system;
561 ack.target_component = cmd.source_component;
568 if (armed_sub.updated()) {
570 armed_sub.copy(&
armed);
573 PX4_INFO(
"UAVCAN command bridge: system armed, exiting now.");
582 return (pthread_addr_t) 0;
595 uint8_t node_id = result.getCallID().server_node_id.get();
597 if (result.isSuccessful()) {
598 uavcan::protocol::param::GetSet::Response resp = result.getResponse();
600 if (resp.name.size()) {
604 uavcan::protocol::param::GetSet::Request req;
612 PX4_ERR(
"UAVCAN command bridge: couldn't send GetSet during param count: %d", call_res);
619 PX4_INFO(
"UAVCAN command bridge: completed param count for node %hhu: %hhu", node_id,
_param_counts[node_id]);
627 PX4_ERR(
"UAVCAN command bridge: GetSet error during param count");
636 if (result.isSuccessful()) {
637 uavcan::protocol::param::GetSet::Response param = result.getResponse();
640 response.
node_id = result.getCallID().server_node_id.get();
641 strncpy(response.
param_id, param.name.c_str(),
sizeof(response.
param_id) - 1);
646 if (param.value.is(uavcan::protocol::param::Value::Tag::integer_value)) {
648 response.
int_value = param.value.to<uavcan::protocol::param::Value::Tag::integer_value>();
650 }
else if (param.value.is(uavcan::protocol::param::Value::Tag::real_value)) {
652 response.
real_value = param.value.to<uavcan::protocol::param::Value::Tag::real_value>();
654 }
else if (param.value.is(uavcan::protocol::param::Value::Tag::boolean_value)) {
656 response.
int_value = param.value.to<uavcan::protocol::param::Value::Tag::boolean_value>();
662 PX4_ERR(
"UAVCAN command bridge: GetSet error");
673 uavcan::protocol::param::GetSet::Request req;
678 PX4_ERR(
"UAVCAN command bridge: couldn't start parameter count: %d", call_res);
683 PX4_INFO(
"UAVCAN command bridge: starting param count");
690 uavcan::protocol::param::ExecuteOpcode::Request opcode_req;
695 PX4_ERR(
"UAVCAN command bridge: couldn't send ExecuteOpcode: %d", call_res);
699 PX4_INFO(
"UAVCAN command bridge: sent ExecuteOpcode");
706 bool success = result.isSuccessful();
707 uint8_t
node_id = result.getCallID().server_node_id.get();
708 uavcan::protocol::param::ExecuteOpcode::Response resp = result.getResponse();
712 if (!result.isSuccessful()) {
713 PX4_ERR(
"UAVCAN command bridge: save request for node %hhu timed out.", node_id);
715 }
else if (!result.getResponse().ok) {
716 PX4_ERR(
"UAVCAN command bridge: save request for node %hhu rejected.", node_id);
719 PX4_INFO(
"UAVCAN command bridge: save request for node %hhu completed OK, restarting.", node_id);
721 uavcan::protocol::RestartNode::Request restart_req;
722 restart_req.magic_number = restart_req.MAGIC_NUMBER;
726 PX4_ERR(
"UAVCAN command bridge: couldn't send RestartNode: %d", call_res);
729 PX4_ERR(
"UAVCAN command bridge: sent RestartNode");
752 bool success = result.isSuccessful();
753 uint8_t
node_id = result.getCallID().server_node_id.get();
754 uavcan::protocol::RestartNode::Response resp = result.getResponse();
759 PX4_INFO(
"UAVCAN command bridge: restart request for node %hhu completed OK.", node_id);
765 PX4_ERR(
"UAVCAN command bridge: restart request for node %hhu failed.", node_id);
782 _subnode.getNodeID().get() == base); base++);
800 uavcan::equipment::indication::BeepCommand cmd;
801 cmd.frequency = frequency;
811 if (!result.isSuccessful()) {
812 PX4_ERR(
"UAVCAN ESC enumeration: begin request for node %hhu timed out.", result.getCallID().server_node_id.get());
814 }
else if (result.getResponse().error) {
815 PX4_ERR(
"UAVCAN ESC enumeration: begin request for node %hhu rejected: %hhu", result.getCallID().server_node_id.get(),
816 result.getResponse().error);
820 PX4_INFO(
"UAVCAN ESC enumeration: begin request for node %hhu completed OK.", result.getCallID().server_node_id.get());
825 uavcan::protocol::enumeration::Begin::Request req;
828 req.parameter_name =
"esc_index";
834 PX4_ERR(
"UAVCAN ESC enumeration: couldn't send Begin request: %d", call_res);
837 PX4_INFO(
"UAVCAN ESC enumeration: sent Begin request");
841 PX4_INFO(
"UAVCAN ESC enumeration: begun enumeration on all nodes.");
850 PX4_INFO(
"UAVCAN ESC enumeration: got indication");
862 PX4_INFO(
"UAVCAN ESC enumeration: already enumerated ESC ID %hhu as index %d, ignored",
_esc_enumeration_ids[i], i);
867 uavcan::protocol::param::GetSet::Request req;
868 req.name = msg.parameter_name;
869 req.value.to<uavcan::protocol::param::Value::Tag::integer_value>() = i;
874 PX4_ERR(
"UAVCAN ESC enumeration: couldn't send GetSet: %d", call_res);
877 PX4_INFO(
"UAVCAN ESC enumeration: sent GetSet to node %hhu (index %d)",
_esc_enumeration_ids[i], i);
884 if (!result.isSuccessful()) {
885 PX4_ERR(
"UAVCAN ESC enumeration: save request for node %hhu timed out.", result.getCallID().server_node_id.get());
888 PX4_INFO(
"UAVCAN ESC enumeration: save request for node %hhu completed OK.", result.getCallID().server_node_id.get());
890 uavcan::protocol::param::GetSet::Response resp = result.getResponse();
891 uint8_t esc_index = (uint8_t)resp.value.to<uavcan::protocol::param::Value::Tag::integer_value>();
892 esc_index =
math::min((uint8_t)(uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize - 1), esc_index);
897 uavcan::protocol::param::ExecuteOpcode::Request opcode_req;
898 opcode_req.opcode = opcode_req.OPCODE_SAVE;
902 PX4_ERR(
"UAVCAN ESC enumeration: couldn't send ExecuteOpcode: %d", call_res);
905 PX4_INFO(
"UAVCAN ESC enumeration: sent ExecuteOpcode to node %hhu (index %hhu)",
_esc_enumeration_ids[esc_index],
914 const bool this_is_the_last_one =
918 if (!result.isSuccessful()) {
919 PX4_ERR(
"UAVCAN ESC enumeration: save request for node %hhu timed out.", result.getCallID().server_node_id.get());
922 }
else if (!result.getResponse().ok) {
923 PX4_ERR(
"UAVCAN ESC enumeration: save request for node %hhu rejected", result.getCallID().server_node_id.get());
927 PX4_INFO(
"UAVCAN ESC enumeration: save request for node %hhu completed OK.", result.getCallID().server_node_id.get());
933 if (this_is_the_last_one) {
937 uavcan::protocol::enumeration::Begin::Request req;
941 req.parameter_name =
"esc_index";
946 PX4_ERR(
"UAVCAN ESC enumeration: couldn't send Begin request to stop enumeration: %d", call_res);
949 PX4_INFO(
"UAVCAN ESC enumeration: sent Begin request to stop enumeration");
993 const size_t sd_path_len = strlen(sd_path);
994 const size_t romfs_path_len = strlen(romfs_path);
997 char dstpath[maxlen + 1];
998 char srcpath[maxlen + 1];
1000 DIR *
const romfs_dir = opendir(romfs_path);
1006 memcpy(dstpath, sd_path, sd_path_len + 1);
1007 memcpy(srcpath, romfs_path, romfs_path_len + 1);
1010 struct dirent *dev_dirent = NULL;
1012 while ((dev_dirent = readdir(romfs_dir)) != NULL) {
1014 if (!DIRENT_ISDIRECTORY(dev_dirent->d_type)) {
1019 size_t dev_dirname_len = strlen(dev_dirent->d_name);
1020 size_t srcpath_dev_len = romfs_path_len + 1 + dev_dirname_len;
1022 if (srcpath_dev_len > maxlen) {
1023 PX4_WARN(
"dev: srcpath '%s/%s' too long", romfs_path, dev_dirent->d_name);
1027 size_t dstpath_dev_len = sd_path_len + 1 + dev_dirname_len;
1029 if (dstpath_dev_len > maxlen) {
1030 PX4_WARN(
"dev: dstpath '%s/%s' too long", sd_path, dev_dirent->d_name);
1035 dstpath[sd_path_len] =
'/';
1036 memcpy(&dstpath[sd_path_len + 1], dev_dirent->d_name, dev_dirname_len + 1);
1038 if (stat(dstpath, &sb) != 0 || !S_ISDIR(sb.st_mode)) {
1039 rv = mkdir(dstpath, S_IRWXU | S_IRWXG | S_IRWXO);
1042 PX4_ERR(
"dev: couldn't create '%s'", dstpath);
1048 srcpath[romfs_path_len] =
'/';
1049 memcpy(&srcpath[romfs_path_len + 1], dev_dirent->d_name, dev_dirname_len + 1);
1051 DIR *
const dev_dir = opendir(srcpath);
1054 PX4_ERR(
"dev: couldn't open '%s'", srcpath);
1059 struct dirent *ver_dirent = NULL;
1061 while ((ver_dirent = readdir(dev_dir)) != NULL) {
1063 if (!DIRENT_ISDIRECTORY(ver_dirent->d_type)) {
1068 size_t ver_dirname_len = strlen(ver_dirent->d_name);
1069 size_t srcpath_ver_len = srcpath_dev_len + 1 + ver_dirname_len;
1071 if (srcpath_ver_len > maxlen) {
1072 PX4_ERR(
"ver: srcpath '%s/%s' too long", srcpath, ver_dirent->d_name);
1076 size_t dstpath_ver_len = dstpath_dev_len + 1 + ver_dirname_len;
1078 if (dstpath_ver_len > maxlen) {
1079 PX4_ERR(
"ver: dstpath '%s/%s' too long", dstpath, ver_dirent->d_name);
1084 dstpath[dstpath_dev_len] =
'/';
1085 memcpy(&dstpath[dstpath_dev_len + 1], ver_dirent->d_name, ver_dirname_len + 1);
1087 if (stat(dstpath, &sb) != 0 || !S_ISDIR(sb.st_mode)) {
1088 rv = mkdir(dstpath, S_IRWXU | S_IRWXG | S_IRWXO);
1091 PX4_ERR(
"ver: couldn't create '%s'", dstpath);
1097 srcpath[srcpath_dev_len] =
'/';
1098 memcpy(&srcpath[srcpath_dev_len + 1], ver_dirent->d_name, ver_dirname_len + 1);
1102 DIR *
const src_ver_dir = opendir(srcpath);
1105 PX4_ERR(
"ver: couldn't open '%s'", srcpath);
1109 struct dirent *src_fw_dirent = NULL;
1111 while ((src_fw_dirent = readdir(src_ver_dir)) != NULL &&
1112 !DIRENT_ISFILE(src_fw_dirent->d_type));
1114 if (!src_fw_dirent) {
1115 (void)closedir(src_ver_dir);
1119 size_t fw_len = strlen(src_fw_dirent->d_name);
1121 bool copy_fw =
true;
1124 DIR *
const dst_ver_dir = opendir(dstpath);
1127 PX4_ERR(
"unlink: couldn't open '%s'", dstpath);
1130 struct dirent *fw_dirent = NULL;
1132 while ((fw_dirent = readdir(dst_ver_dir)) != NULL) {
1134 if (!DIRENT_ISFILE(fw_dirent->d_type)) {
1138 if (!memcmp(&fw_dirent->d_name, src_fw_dirent->d_name, fw_len)) {
1146 size_t dst_fw_len = strlen(fw_dirent->d_name);
1147 size_t dstpath_fw_len = dstpath_ver_len + dst_fw_len;
1149 if (dstpath_fw_len > maxlen) {
1151 PX4_ERR(
"unlink: path '%s/%s' too long", dstpath, fw_dirent->d_name);
1155 dstpath[dstpath_ver_len] =
'/';
1156 memcpy(&dstpath[dstpath_ver_len + 1], fw_dirent->d_name, dst_fw_len + 1);
1158 PX4_ERR(
"unlink: removed '%s'", dstpath);
1167 (void)closedir(dst_ver_dir);
1172 size_t srcpath_fw_len = srcpath_ver_len + 1 + fw_len;
1173 size_t dstpath_fw_len = dstpath_ver_len + fw_len;
1175 if (srcpath_fw_len > maxlen) {
1176 PX4_ERR(
"copy: srcpath '%s/%s' too long", srcpath, src_fw_dirent->d_name);
1178 }
else if (dstpath_fw_len > maxlen) {
1179 PX4_ERR(
"copy: dstpath '%s/%s' too long", dstpath, src_fw_dirent->d_name);
1183 srcpath[srcpath_ver_len] =
'/';
1184 memcpy(&srcpath[srcpath_ver_len + 1], src_fw_dirent->d_name, fw_len + 1);
1186 dstpath[dstpath_ver_len] =
'/';
1187 memcpy(&dstpath[dstpath_ver_len + 1], src_fw_dirent->d_name, fw_len + 1);
1189 rv =
copyFw(dstpath, srcpath);
1192 PX4_ERR(
"copy: '%s' -> '%s' failed: %d", srcpath, dstpath, rv);
1195 PX4_INFO(
"copy: '%s' -> '%s' succeeded", srcpath, dstpath);
1200 (void)closedir(src_ver_dir);
1203 (void)closedir(dev_dir);
1206 (void)closedir(romfs_dir);
1213 uint8_t buffer[512] {};
1215 int dfd = open(dst, O_WRONLY | O_CREAT, 0666);
1218 PX4_ERR(
"copyFw: couldn't open dst");
1222 int sfd = open(src, O_RDONLY, 0);
1226 PX4_ERR(
"copyFw: couldn't open src");
1233 size =
read(sfd, buffer,
sizeof(buffer));
1236 PX4_ERR(
"copyFw: couldn't read");
1239 }
else if (size > 0) {
1241 ssize_t remaining = size;
1242 ssize_t total_written = 0;
1243 ssize_t written = 0;
1246 written =
write(dfd, &buffer[total_written], remaining);
1249 PX4_ERR(
"copyFw: couldn't write");
1253 total_written += written;
1254 remaining -= written;
1256 }
while (written > 0 && remaining > 0);
1258 }
while (rv == 0 && size != 0);
void clear_node_params_dirty(uint8_t node_id)
uavcan::ServiceClient< uavcan::protocol::param::ExecuteOpcode, ExecuteOpcodeCallback > _param_opcode_client
uavcan::ServiceClient< uavcan::protocol::param::GetSet, GetSetCallback > _enumeration_getset_client
void cb_enumeration_begin(const uavcan::ServiceCallResult< uavcan::protocol::enumeration::Begin > &result)
uint8_t _param_counts[128]
void cb_opcode(const uavcan::ServiceCallResult< uavcan::protocol::param::ExecuteOpcode > &result)
uavcan::ServiceClient< uavcan::protocol::param::GetSet, GetSetCallback > _param_getset_client
VirtualCanDriver _vdriver
uORB::Publication< uavcan_parameter_value_s > _param_response_pub
static int getHardwareVersion(uavcan::protocol::HardwareVersion &hwver)
Defines basic functinality of UAVCAN node.
void set_node_params_dirty(uint8_t node_id)
uint8_t _param_save_opcode
uORB::PublicationQueued< vehicle_command_ack_s > _command_ack_pub
uavcan::FirmwareUpdateTrigger _fw_upgrade_trigger
bool publish(const T &data)
Publish the struct.
void cb_getset(const uavcan::ServiceCallResult< uavcan::protocol::param::GetSet > &result)
#define UAVCAN_MAX_PATH_LENGTH
volatile bool _subnode_thread_should_exit
static constexpr unsigned Priority
void cb_enumeration_getset(const uavcan::ServiceCallResult< uavcan::protocol::param::GetSet > &result)
uavcan::INode & _main_node
uavcan::NodeInfoRetriever _node_info_retriever
void cb_enumeration_save(const uavcan::ServiceCallResult< uavcan::protocol::param::ExecuteOpcode > &result)
static int deinit(pthread_mutex_t &thier_mutex_)
uavcan::MethodBinder< UavcanServers *, void(UavcanServers::*)(const uavcan::ServiceCallResult< uavcan::protocol::param::ExecuteOpcode > &)> ExecuteOpcodeCallback
UavcanServers(uavcan::INode &main_node)
uavcan::MethodBinder< UavcanServers *, void(UavcanServers::*)(const uavcan::ReceivedDataStructure< uavcan::protocol::enumeration::Indication > &)> EnumerationIndicationCallback
High-resolution timer with callouts and timekeeping.
bool are_node_params_dirty(uint8_t node_id) const
static constexpr float BeepFrequencyError
Global flash based parameter store.
uavcan::MethodBinder< UavcanServers *, void(UavcanServers::*)(const uavcan::ServiceCallResult< uavcan::protocol::RestartNode > &)> RestartNodeCallback
uint8_t get_next_dirty_node_id(uint8_t base)
uint8_t get_next_active_node_id(uint8_t base)
static char msg[NUM_MSG][CONFIG_USART1_TXBUFSIZE]
static void read(bootloader_app_shared_t *pshared)
bool _esc_enumeration_active
void unpackFwFromROMFS(const char *sd_path, const char *romfs_path)
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
uavcan_posix::dynamic_node_id_server::FileStorageBackend _storage_backend
bool publish(const T &data)
Publish the struct.
bool _param_list_all_nodes
static UavcanServers * _instance
singleton pointer
void param_opcode(uavcan::NodeID node_id)
static int init(pthread_mutex_t &thier_mutex_)
uavcan_posix::dynamic_node_id_server::FileEventTracer _tracer
static struct actuator_armed_s armed
A UAVCAN Server Sub node.
#define UAVCAN_ROMFS_FW_PREFIX
Simple error/warning functions, heavily inspired by the BSD functions of the same names...
uavcan::dynamic_node_id_server::CentralizedServer _server_instance
server singleton pointer
static int start(uavcan::INode &main_node)
uavcan::ServiceClient< uavcan::protocol::enumeration::Begin, EnumerationBeginCallback > _enumeration_client
int copyFw(const char *dst, const char *src)
uavcan::ServiceClient< uavcan::protocol::param::ExecuteOpcode, ExecuteOpcodeCallback > _enumeration_save_client
void cb_enumeration_indication(const uavcan::ReceivedDataStructure< uavcan::protocol::enumeration::Indication > &msg)
constexpr _Tp min(_Tp a, _Tp b)
Tools for system version detection.
static void write(bootloader_app_shared_t *pshared)
uavcan::ServiceClient< uavcan::protocol::RestartNode, RestartNodeCallback > _param_restartnode_client
constexpr _Tp max(_Tp a, _Tp b)
#define UAVCAN_ROMFS_FW_PATH
#define UAVCAN_FIRMWARE_PATH
static constexpr float BeepFrequencySuccess
pthread_addr_t run(pthread_addr_t)
uavcan::BasicFileServer _fw_server
uavcan::MethodBinder< UavcanServers *, void(UavcanServers::*)(const uavcan::ServiceCallResult< uavcan::protocol::enumeration::Begin > &)> EnumerationBeginCallback
static constexpr float BeepFrequencyGenericIndication
Defines basic functionality of UAVCAN node.
#define UAVCAN_NODE_DB_PATH
void cb_restart(const uavcan::ServiceCallResult< uavcan::protocol::RestartNode > &result)
uavcan_posix::FirmwareVersionChecker _fw_version_checker
bool _param_list_in_progress
pthread_mutex_t _subnode_mutex
uint8_t _param_list_node_id
void param_count(uavcan::NodeID node_id)
uint8_t _esc_enumeration_index
static constexpr unsigned StackSize
static UavcanServers * instance()
void beep(float frequency)
uavcan::MethodBinder< UavcanServers *, void(UavcanServers::*)(const uavcan::ServiceCallResult< uavcan::protocol::param::GetSet > &)> GetSetCallback
pthread_t _subnode_thread
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
uint8_t _esc_enumeration_ids[uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize]
uavcan::Subscriber< uavcan::protocol::enumeration::Indication, EnumerationIndicationCallback > _enumeration_indication_sub
uavcan::Publisher< uavcan::equipment::indication::BeepCommand > _beep_pub