PX4 Firmware
PX4 Autopilot Software http://px4.io
uavcan_servers.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2014-2017 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 #include <px4_platform_common/tasks.h>
35 #include <drivers/drv_hrt.h>
36 
37 #include <nuttx/config.h>
38 
39 #include <cstdlib>
40 #include <cstring>
41 #include <fcntl.h>
42 #include <dirent.h>
43 #include <pthread.h>
44 #include <mathlib/mathlib.h>
45 #include <systemlib/err.h>
46 #include <parameters/param.h>
47 #include <version/version.h>
48 #include <arch/board/board.h>
49 #include <arch/chip/chip.h>
50 
51 #include "uavcan_main.hpp"
52 #include "uavcan_servers.hpp"
54 
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>
58 
62 
63 #include <v2.0/common/mavlink.h>
64 
65 /**
66  * @file uavcan_servers.cpp
67  *
68  * Implements basic functionality of UAVCAN node.
69  *
70  * @author Pavel Kirienko <pavel.kirienko@gmail.com>
71  * David Sidrane <david_s5@nscdg.com>
72  */
73 
74 /*
75  * UavcanNode
76  */
78 
79 UavcanServers::UavcanServers(uavcan::INode &main_node) :
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),
91  _beep_pub(_subnode),
92  _enumeration_indication_sub(_subnode),
93  _enumeration_client(_subnode),
94  _enumeration_getset_client(_subnode),
95  _enumeration_save_client(_subnode)
96 {
97 }
98 
100 {
101  if (_mutex_inited) {
103  }
104 
105  _main_node.getDispatcher().removeRxFrameListener();
106 }
107 
108 int
110 {
111  UavcanServers *server = instance();
112 
113  if (server == nullptr) {
114  PX4_INFO("Already stopped");
115  return -1;
116  }
117 
118  if (server->_subnode_thread) {
119  PX4_INFO("stopping fw srv thread...");
120  server->_subnode_thread_should_exit = true;
121  (void)pthread_join(server->_subnode_thread, NULL);
122  }
123 
124  _instance = nullptr;
125 
126  server->_main_node.getDispatcher().removeRxFrameListener();
127 
128  delete server;
129  return 0;
130 }
131 
132 int
133 UavcanServers::start(uavcan::INode &main_node)
134 {
135  if (_instance != nullptr) {
136  PX4_INFO("Already started");
137  return -1;
138  }
139 
140  /*
141  * Node init
142  */
143  _instance = new UavcanServers(main_node);
144 
145  if (_instance == nullptr) {
146  PX4_ERR("Out of memory");
147  return -2;
148  }
149 
150  int rv = _instance->init();
151 
152  if (rv < 0) {
153  PX4_ERR("Node init failed: %d", rv);
154  delete _instance;
155  _instance = nullptr;
156  return rv;
157  }
158 
159  /*
160  * Start the thread. Normally it should never exit.
161  */
162  pthread_attr_t tattr;
163  struct sched_param param;
164 
165  pthread_attr_init(&tattr);
166  (void)pthread_attr_getschedparam(&tattr, &param);
167  tattr.stacksize = PX4_STACK_ADJUSTED(StackSize);
168  param.sched_priority = Priority;
169 
170  if (pthread_attr_setschedparam(&tattr, &param)) {
171  PX4_ERR("setting sched params failed");
172  }
173 
174  static auto run_trampoline = [](void *) {return UavcanServers::_instance->run(_instance);};
175 
176  rv = pthread_create(&_instance->_subnode_thread, &tattr, static_cast<pthread_startroutine_t>(run_trampoline), NULL);
177 
178  if (rv != 0) {
179  rv = -rv;
180  PX4_ERR("pthread_create() failed: %d", rv);
181  delete _instance;
182  _instance = nullptr;
183  }
184 
185  return rv;
186 }
187 
188 int
190 {
191  errno = 0;
192 
193  /*
194  * Initialize the mutex.
195  * giving it its path
196  */
197  int ret = Lock::init(_subnode_mutex);
198 
199  if (ret < 0) {
200  PX4_ERR("Lock init: %d", errno);
201  return ret;
202  }
203 
204  _mutex_inited = true;
205 
206  _subnode.setNodeID(_main_node.getNodeID());
207  _main_node.getDispatcher().installRxFrameListener(&_vdriver);
208 
209  /*
210  * Initialize the fw version checker.
211  * giving it its path
212  */
213  ret = _fw_version_checker.createFwPaths(UAVCAN_FIRMWARE_PATH);
214 
215  if (ret < 0) {
216  PX4_ERR("FirmwareVersionChecker init: %d, errno: %d", ret, errno);
217  return ret;
218  }
219 
220  /* Start fw file server back */
221 
222  ret = _fw_server.start();
223 
224  if (ret < 0) {
225  PX4_ERR("BasicFileServer init: %d, errno: %d", ret, errno);
226  return ret;
227  }
228 
229  /* Initialize storage back end for the node allocator using UAVCAN_NODE_DB_PATH directory */
230 
232 
233  if (ret < 0) {
234  PX4_ERR("FileStorageBackend init: %d, errno: %d", ret, errno);
235  return ret;
236  }
237 
238  /* Initialize trace in the UAVCAN_NODE_DB_PATH directory */
239 
240  ret = _tracer.init(UAVCAN_LOG_FILE);
241 
242  if (ret < 0) {
243  PX4_ERR("FileEventTracer init: %d, errno: %d", ret, errno);
244  return ret;
245  }
246 
247  /* hardware version */
248  uavcan::protocol::HardwareVersion hwver;
250 
251  /* Initialize the dynamic node id server */
252  ret = _server_instance.init(hwver.unique_id);
253 
254  if (ret < 0) {
255  PX4_ERR("CentralizedServer init: %d", ret);
256  return ret;
257  }
258 
259  /* Start node info retriever to fetch node info from new nodes */
260  ret = _node_info_retriever.start();
261 
262  if (ret < 0) {
263  PX4_ERR("NodeInfoRetriever init: %d", ret);
264  return ret;
265  }
266 
267  /* Start the fw version checker */
268  ret = _fw_upgrade_trigger.start(_node_info_retriever, _fw_version_checker.getFirmwarePath());
269 
270  if (ret < 0) {
271  PX4_ERR("FirmwareUpdateTrigger init: %d", ret);
272  return ret;
273  }
274 
275  /* Start the Node */
276  return 0;
277 }
278 
279 pthread_addr_t
280 UavcanServers::run(pthread_addr_t)
281 {
282  prctl(PR_SET_NAME, "uavcan fw srv", 0);
283 
285 
286  /*
287  Copy any firmware bundled in the ROMFS to the appropriate location on the
288  SD card, unless the user has copied other firmware for that device.
289  */
291 
292  /* the subscribe call needs to happen in the same thread,
293  * so not in the constructor */
294  uORB::Subscription armed_sub{ORB_ID(actuator_armed)};
295  uORB::Subscription vcmd_sub{ORB_ID(vehicle_command)};
296  uORB::Subscription param_request_sub{ORB_ID(uavcan_parameter_request)};
297 
298  /* Set up shared service clients */
306 
308  memset(_param_counts, 0, sizeof(_param_counts));
309 
310  _esc_enumeration_active = false;
311  memset(_esc_enumeration_ids, 0, sizeof(_esc_enumeration_ids));
313 
314  while (!_subnode_thread_should_exit) {
315 
316  if (_check_fw == true) {
317  _check_fw = false;
318  _node_info_retriever.invalidateAll();
319  }
320 
321  const int spin_res = _subnode.spin(uavcan::MonotonicDuration::fromMSec(10));
322 
323  if (spin_res < 0) {
324  PX4_ERR("node spin error %i", spin_res);
325  }
326 
327  // Check for parameter requests (get/set/list)
328  if (param_request_sub.updated() && !_param_list_in_progress && !_param_in_progress && !_count_in_progress) {
329  uavcan_parameter_request_s request{};
330  param_request_sub.copy(&request);
331 
332  if (_param_counts[request.node_id]) {
333  /*
334  * We know how many parameters are exposed by this node, so
335  * process the request.
336  */
337  if (request.message_type == MAVLINK_MSG_ID_PARAM_REQUEST_READ) {
338  uavcan::protocol::param::GetSet::Request req;
339 
340  if (request.param_index >= 0) {
341  req.index = request.param_index;
342 
343  } else {
344  req.name = (char *)request.param_id;
345  }
346 
347  int call_res = _param_getset_client.call(request.node_id, req);
348 
349  if (call_res < 0) {
350  PX4_ERR("UAVCAN command bridge: couldn't send GetSet: %d", call_res);
351 
352  } else {
353  _param_in_progress = true;
354  _param_index = request.param_index;
355  }
356 
357  } else if (request.message_type == MAVLINK_MSG_ID_PARAM_SET) {
358  uavcan::protocol::param::GetSet::Request req;
359 
360  if (request.param_index >= 0) {
361  req.index = request.param_index;
362 
363  } else {
364  req.name = (char *)request.param_id;
365  }
366 
367  if (request.param_type == MAV_PARAM_TYPE_REAL32) {
368  req.value.to<uavcan::protocol::param::Value::Tag::real_value>() = request.real_value;
369 
370  } else if (request.param_type == MAV_PARAM_TYPE_UINT8) {
371  req.value.to<uavcan::protocol::param::Value::Tag::boolean_value>() = request.int_value;
372 
373  } else {
374  req.value.to<uavcan::protocol::param::Value::Tag::integer_value>() = request.int_value;
375  }
376 
377  // Set the dirty bit for this node
378  set_node_params_dirty(request.node_id);
379 
380  int call_res = _param_getset_client.call(request.node_id, req);
381 
382  if (call_res < 0) {
383  PX4_ERR("UAVCAN command bridge: couldn't send GetSet: %d", call_res);
384 
385  } else {
386  _param_in_progress = true;
387  _param_index = request.param_index;
388  }
389 
390  } else if (request.message_type == MAVLINK_MSG_ID_PARAM_REQUEST_LIST) {
391  // This triggers the _param_list_in_progress case below.
392  _param_index = 0;
394  _param_list_node_id = request.node_id;
395  _param_list_all_nodes = false;
396 
397  PX4_INFO("UAVCAN command bridge: starting component-specific param list");
398  }
399 
400  } else if (request.node_id == MAV_COMP_ID_ALL) {
401  if (request.message_type == MAVLINK_MSG_ID_PARAM_REQUEST_LIST) {
402  /*
403  * This triggers the _param_list_in_progress case below,
404  * but additionally iterates over all active nodes.
405  */
406  _param_index = 0;
409  _param_list_all_nodes = true;
410 
411  PX4_INFO("UAVCAN command bridge: starting global param list with node %hhu", _param_list_node_id);
412 
415  }
416  }
417 
418  } else {
419  /*
420  * Need to know how many parameters this node has before we can
421  * continue; count them now and then process the request.
422  */
423  param_count(request.node_id);
424  }
425  }
426 
427  // Handle parameter listing index/node ID advancement
430  PX4_INFO("UAVCAN command bridge: completed param list for node %hhu", _param_list_node_id);
431  // Reached the end of the current node's parameter set.
432  _param_list_in_progress = false;
433 
434  if (_param_list_all_nodes) {
435  // We're listing all parameters for all nodes -- get the next node ID
436  uint8_t next_id = get_next_active_node_id(_param_list_node_id);
437 
438  if (next_id < 128) {
439  _param_list_node_id = next_id;
440 
441  /*
442  * If there is a next node ID, check if that node's parameters
443  * have been counted before. If not, do it now.
444  */
445  if (_param_counts[_param_list_node_id] == 0) {
446  param_count(_param_list_node_id);
447  }
448 
449  // Keep on listing.
450  _param_index = 0;
452  PX4_INFO("UAVCAN command bridge: started param list for node %hhu", _param_list_node_id);
453  }
454  }
455  }
456  }
457 
458  // Check if we're still listing, and need to get the next parameter
460  // Ready to request the next value -- _param_index is incremented
461  // after each successful fetch by cb_getset
462  uavcan::protocol::param::GetSet::Request req;
463  req.index = _param_index;
464 
465  int call_res = _param_getset_client.call(_param_list_node_id, req);
466 
467  if (call_res < 0) {
468  _param_list_in_progress = false;
469  PX4_ERR("UAVCAN command bridge: couldn't send param list GetSet: %d", call_res);
470 
471  } else {
472  _param_in_progress = true;
473  }
474  }
475 
476  // Check for ESC enumeration commands
477  if (vcmd_sub.updated() && !_cmd_in_progress) {
478  vehicle_command_s cmd{};
479  vcmd_sub.copy(&cmd);
480 
481  uint8_t cmd_ack_result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
482 
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);
486  int call_res;
487 
488  PX4_INFO("UAVCAN command bridge: received UAVCAN command ID %d, node ID %d", command_id, node_id);
489 
490  switch (command_id) {
491  case 0:
492  case 1: {
493  _esc_enumeration_active = command_id;
495  _esc_count = 0;
496  uavcan::protocol::enumeration::Begin::Request req;
497  // TODO: Incorrect implementation; the parameter name field should be left empty.
498  // Leaving it as-is to avoid breaking compatibility with non-compliant nodes.
499  req.parameter_name = "esc_index";
500  req.timeout_sec = _esc_enumeration_active ? 65535 : 0;
501  call_res = _enumeration_client.call(get_next_active_node_id(0), req);
502 
503  if (call_res < 0) {
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;
507 
508  } else {
510  }
511 
512  break;
513  }
514 
515  default: {
516  PX4_ERR("UAVCAN command bridge: unknown command ID %d", command_id);
517  cmd_ack_result = vehicle_command_ack_s::VEHICLE_RESULT_UNSUPPORTED;
518  break;
519  }
520  }
521 
522  } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE) {
523  int command_id = static_cast<int>(cmd.param1 + 0.5f);
524 
525  PX4_INFO("UAVCAN command bridge: received storage command ID %d", command_id);
526 
527  switch (command_id) {
528  case 1: {
529  // Param save request
530  int node_id;
531  node_id = get_next_dirty_node_id(1);
532 
533  if (node_id < 128) {
534  _param_save_opcode = uavcan::protocol::param::ExecuteOpcode::Request::OPCODE_SAVE;
535  param_opcode(node_id);
536  }
537 
538  break;
539  }
540 
541  case 2: {
542  // Command is a param erase request -- apply it to all active nodes by setting the dirty bit
543  _param_save_opcode = uavcan::protocol::param::ExecuteOpcode::Request::OPCODE_ERASE;
544 
545  for (int i = 1; i < 128; i = get_next_active_node_id(i)) {
547  }
548 
550  break;
551  }
552  }
553  }
554 
555  // Acknowledge the received command
556  vehicle_command_ack_s ack{};
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;
562 
564  }
565 
566  // Shut down once armed
567  // TODO (elsewhere): start up again once disarmed?
568  if (armed_sub.updated()) {
570  armed_sub.copy(&armed);
571 
573  PX4_INFO("UAVCAN command bridge: system armed, exiting now.");
574  break;
575  }
576  }
577  }
578 
580 
581  PX4_INFO("exiting");
582  return (pthread_addr_t) 0;
583 }
584 
585 void
586 UavcanServers::cb_getset(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result)
587 {
588  if (_count_in_progress) {
589  /*
590  * Currently in parameter count mode:
591  * Iterate over all parameters for the node to which the request was
592  * originally sent, in order to find the maximum parameter ID. If a
593  * request fails, set the node's parameter count to zero.
594  */
595  uint8_t node_id = result.getCallID().server_node_id.get();
596 
597  if (result.isSuccessful()) {
598  uavcan::protocol::param::GetSet::Response resp = result.getResponse();
599 
600  if (resp.name.size()) {
601  _count_index++;
602  _param_counts[node_id] = _count_index;
603 
604  uavcan::protocol::param::GetSet::Request req;
605  req.index = _count_index;
606 
607  int call_res = _param_getset_client.call(result.getCallID().server_node_id, req);
608 
609  if (call_res < 0) {
610  _count_in_progress = false;
611  _count_index = 0;
612  PX4_ERR("UAVCAN command bridge: couldn't send GetSet during param count: %d", call_res);
614  }
615 
616  } else {
617  _count_in_progress = false;
618  _count_index = 0;
619  PX4_INFO("UAVCAN command bridge: completed param count for node %hhu: %hhu", node_id, _param_counts[node_id]);
621  }
622 
623  } else {
624  _param_counts[node_id] = 0;
625  _count_in_progress = false;
626  _count_index = 0;
627  PX4_ERR("UAVCAN command bridge: GetSet error during param count");
628  }
629 
630  } else {
631  /*
632  * Currently in parameter get/set mode:
633  * Publish a uORB uavcan_parameter_value message containing the current value
634  * of the parameter.
635  */
636  if (result.isSuccessful()) {
637  uavcan::protocol::param::GetSet::Response param = result.getResponse();
638 
639  struct uavcan_parameter_value_s response;
640  response.node_id = result.getCallID().server_node_id.get();
641  strncpy(response.param_id, param.name.c_str(), sizeof(response.param_id) - 1);
642  response.param_id[16] = '\0';
643  response.param_index = _param_index;
644  response.param_count = _param_counts[response.node_id];
645 
646  if (param.value.is(uavcan::protocol::param::Value::Tag::integer_value)) {
647  response.param_type = MAV_PARAM_TYPE_INT64;
648  response.int_value = param.value.to<uavcan::protocol::param::Value::Tag::integer_value>();
649 
650  } else if (param.value.is(uavcan::protocol::param::Value::Tag::real_value)) {
651  response.param_type = MAV_PARAM_TYPE_REAL32;
652  response.real_value = param.value.to<uavcan::protocol::param::Value::Tag::real_value>();
653 
654  } else if (param.value.is(uavcan::protocol::param::Value::Tag::boolean_value)) {
655  response.param_type = MAV_PARAM_TYPE_UINT8;
656  response.int_value = param.value.to<uavcan::protocol::param::Value::Tag::boolean_value>();
657  }
658 
659  _param_response_pub.publish(response);
660 
661  } else {
662  PX4_ERR("UAVCAN command bridge: GetSet error");
663  }
664 
665  _param_in_progress = false;
666  _param_index++;
667  }
668 }
669 
670 void
672 {
673  uavcan::protocol::param::GetSet::Request req;
674  req.index = 0;
675  int call_res = _param_getset_client.call(node_id, req);
676 
677  if (call_res < 0) {
678  PX4_ERR("UAVCAN command bridge: couldn't start parameter count: %d", call_res);
679 
680  } else {
681  _count_in_progress = true;
682  _count_index = 0;
683  PX4_INFO("UAVCAN command bridge: starting param count");
684  }
685 }
686 
687 void
689 {
690  uavcan::protocol::param::ExecuteOpcode::Request opcode_req;
691  opcode_req.opcode = _param_save_opcode;
692  int call_res = _param_opcode_client.call(node_id, opcode_req);
693 
694  if (call_res < 0) {
695  PX4_ERR("UAVCAN command bridge: couldn't send ExecuteOpcode: %d", call_res);
696 
697  } else {
698  _cmd_in_progress = true;
699  PX4_INFO("UAVCAN command bridge: sent ExecuteOpcode");
700  }
701 }
702 
703 void
704 UavcanServers::cb_opcode(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result)
705 {
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();
709  success &= resp.ok;
710  _cmd_in_progress = false;
711 
712  if (!result.isSuccessful()) {
713  PX4_ERR("UAVCAN command bridge: save request for node %hhu timed out.", node_id);
714 
715  } else if (!result.getResponse().ok) {
716  PX4_ERR("UAVCAN command bridge: save request for node %hhu rejected.", node_id);
717 
718  } else {
719  PX4_INFO("UAVCAN command bridge: save request for node %hhu completed OK, restarting.", node_id);
720 
721  uavcan::protocol::RestartNode::Request restart_req;
722  restart_req.magic_number = restart_req.MAGIC_NUMBER;
723  int call_res = _param_restartnode_client.call(node_id, restart_req);
724 
725  if (call_res < 0) {
726  PX4_ERR("UAVCAN command bridge: couldn't send RestartNode: %d", call_res);
727 
728  } else {
729  PX4_ERR("UAVCAN command bridge: sent RestartNode");
730  _cmd_in_progress = true;
731  }
732  }
733 
734  if (!_cmd_in_progress) {
735  /*
736  * Something went wrong, so cb_restart is never going to be called as a result of this request.
737  * To ensure we try to execute the opcode on all nodes that permit it, get the next dirty node
738  * ID and keep processing here. The dirty bit on the current node is still set, so the
739  * save/erase attempt will occur when the next save/erase command is received over MAVLink.
740  */
741  node_id = get_next_dirty_node_id(node_id);
742 
743  if (node_id < 128) {
744  param_opcode(node_id);
745  }
746  }
747 }
748 
749 void
750 UavcanServers::cb_restart(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &result)
751 {
752  bool success = result.isSuccessful();
753  uint8_t node_id = result.getCallID().server_node_id.get();
754  uavcan::protocol::RestartNode::Response resp = result.getResponse();
755  success &= resp.ok;
756  _cmd_in_progress = false;
757 
758  if (success) {
759  PX4_INFO("UAVCAN command bridge: restart request for node %hhu completed OK.", node_id);
760 
761  // Clear the dirty flag
762  clear_node_params_dirty(node_id);
763 
764  } else {
765  PX4_ERR("UAVCAN command bridge: restart request for node %hhu failed.", node_id);
766  }
767 
768  // Get the next dirty node ID and send the same command to it
769  node_id = get_next_dirty_node_id(node_id);
770 
771  if (node_id < 128) {
772  param_opcode(node_id);
773  }
774 }
775 
776 uint8_t
778 {
779  base++;
780 
781  for (; base < 128 && (!_node_info_retriever.isNodeKnown(base) ||
782  _subnode.getNodeID().get() == base); base++);
783 
784  return base;
785 }
786 
787 uint8_t
789 {
790  base++;
791 
792  for (; base < 128 && !are_node_params_dirty(base); base++);
793 
794  return base;
795 }
796 
797 void
798 UavcanServers::beep(float frequency)
799 {
800  uavcan::equipment::indication::BeepCommand cmd;
801  cmd.frequency = frequency;
802  cmd.duration = 0.1F; // We don't want to incapacitate ESC for longer time that this
803  (void)_beep_pub.broadcast(cmd);
804 }
805 
806 void
807 UavcanServers::cb_enumeration_begin(const uavcan::ServiceCallResult<uavcan::protocol::enumeration::Begin> &result)
808 {
809  uint8_t next_id = get_next_active_node_id(result.getCallID().server_node_id.get());
810 
811  if (!result.isSuccessful()) {
812  PX4_ERR("UAVCAN ESC enumeration: begin request for node %hhu timed out.", result.getCallID().server_node_id.get());
813 
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);
817 
818  } else {
819  _esc_count++;
820  PX4_INFO("UAVCAN ESC enumeration: begin request for node %hhu completed OK.", result.getCallID().server_node_id.get());
821  }
822 
823  if (next_id < 128) {
824  // Still other active nodes to send the request to
825  uavcan::protocol::enumeration::Begin::Request req;
826  // TODO: Incorrect implementation; the parameter name field should be left empty.
827  // Leaving it as-is to avoid breaking compatibility with non-compliant nodes.
828  req.parameter_name = "esc_index";
829  req.timeout_sec = _esc_enumeration_active ? 65535 : 0;
830 
831  int call_res = _enumeration_client.call(next_id, req);
832 
833  if (call_res < 0) {
834  PX4_ERR("UAVCAN ESC enumeration: couldn't send Begin request: %d", call_res);
835 
836  } else {
837  PX4_INFO("UAVCAN ESC enumeration: sent Begin request");
838  }
839 
840  } else {
841  PX4_INFO("UAVCAN ESC enumeration: begun enumeration on all nodes.");
842  }
843 }
844 
845 void
846 UavcanServers::cb_enumeration_indication(const uavcan::ReceivedDataStructure<uavcan::protocol::enumeration::Indication>
847  &msg)
848 {
849  // Called whenever an ESC thinks it has received user input.
850  PX4_INFO("UAVCAN ESC enumeration: got indication");
851 
853  // Ignore any messages received when we're not expecting them
854  return;
855  }
856 
857  // First, check if we've already seen an indication from this ESC. If so, just ignore this indication.
858  int i = 0;
859 
860  for (; i < _esc_enumeration_index; i++) {
861  if (_esc_enumeration_ids[i] == msg.getSrcNodeID().get()) {
862  PX4_INFO("UAVCAN ESC enumeration: already enumerated ESC ID %hhu as index %d, ignored", _esc_enumeration_ids[i], i);
863  return;
864  }
865  }
866 
867  uavcan::protocol::param::GetSet::Request req;
868  req.name = msg.parameter_name; // 'esc_index' or something alike, the name is not standardized
869  req.value.to<uavcan::protocol::param::Value::Tag::integer_value>() = i;
870 
871  int call_res = _enumeration_getset_client.call(msg.getSrcNodeID(), req);
872 
873  if (call_res < 0) {
874  PX4_ERR("UAVCAN ESC enumeration: couldn't send GetSet: %d", call_res);
875 
876  } else {
877  PX4_INFO("UAVCAN ESC enumeration: sent GetSet to node %hhu (index %d)", _esc_enumeration_ids[i], i);
878  }
879 }
880 
881 void
882 UavcanServers::cb_enumeration_getset(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result)
883 {
884  if (!result.isSuccessful()) {
885  PX4_ERR("UAVCAN ESC enumeration: save request for node %hhu timed out.", result.getCallID().server_node_id.get());
886 
887  } else {
888  PX4_INFO("UAVCAN ESC enumeration: save request for node %hhu completed OK.", result.getCallID().server_node_id.get());
889 
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);
893  _esc_enumeration_index = math::max(_esc_enumeration_index, (uint8_t)(esc_index + 1));
894 
895  _esc_enumeration_ids[esc_index] = result.getCallID().server_node_id.get();
896 
897  uavcan::protocol::param::ExecuteOpcode::Request opcode_req;
898  opcode_req.opcode = opcode_req.OPCODE_SAVE;
899  int call_res = _enumeration_save_client.call(result.getCallID().server_node_id, opcode_req);
900 
901  if (call_res < 0) {
902  PX4_ERR("UAVCAN ESC enumeration: couldn't send ExecuteOpcode: %d", call_res);
903 
904  } else {
905  PX4_INFO("UAVCAN ESC enumeration: sent ExecuteOpcode to node %hhu (index %hhu)", _esc_enumeration_ids[esc_index],
906  esc_index);
907  }
908  }
909 }
910 
911 void
912 UavcanServers::cb_enumeration_save(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result)
913 {
914  const bool this_is_the_last_one =
915  (_esc_enumeration_index >= uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize - 1) ||
917 
918  if (!result.isSuccessful()) {
919  PX4_ERR("UAVCAN ESC enumeration: save request for node %hhu timed out.", result.getCallID().server_node_id.get());
921 
922  } else if (!result.getResponse().ok) {
923  PX4_ERR("UAVCAN ESC enumeration: save request for node %hhu rejected", result.getCallID().server_node_id.get());
925 
926  } else {
927  PX4_INFO("UAVCAN ESC enumeration: save request for node %hhu completed OK.", result.getCallID().server_node_id.get());
928  beep(this_is_the_last_one ? BeepFrequencySuccess : BeepFrequencyGenericIndication);
929  }
930 
931  PX4_INFO("UAVCAN ESC enumeration: completed %hhu of %hhu", _esc_enumeration_index, _esc_count);
932 
933  if (this_is_the_last_one) {
934  _esc_enumeration_active = false;
935 
936  // Tell all ESCs to stop enumerating
937  uavcan::protocol::enumeration::Begin::Request req;
938 
939  // TODO: Incorrect implementation; the parameter name field should be left empty.
940  // Leaving it as-is to avoid breaking compatibility with non-compliant nodes.
941  req.parameter_name = "esc_index";
942  req.timeout_sec = 0;
943  int call_res = _enumeration_client.call(get_next_active_node_id(0), req);
944 
945  if (call_res < 0) {
946  PX4_ERR("UAVCAN ESC enumeration: couldn't send Begin request to stop enumeration: %d", call_res);
947 
948  } else {
949  PX4_INFO("UAVCAN ESC enumeration: sent Begin request to stop enumeration");
950  }
951  }
952 }
953 
954 void
955 UavcanServers::unpackFwFromROMFS(const char *sd_path, const char *romfs_path)
956 {
957  /*
958  Copy the ROMFS firmware directory to the appropriate location on SD, without
959  overriding any firmware the user has already loaded there.
960 
961  The SD firmware directory structure is along the lines of:
962 
963  /fs/microsd/fw
964  - /c
965  - /nodename-v1-1.0.25d0137d.bin cache file (copy of /fs/microsd/fw/org.pixhawk.nodename-v1/1.1/nodename-v1-1.0.25d0137d.bin)
966  - /othernode-v3-1.6.25d0137d.bin cache file (copy of /fs/microsd/fw/com.example.othernode-v3/1.6/othernode-v3-1.6.25d0137d.bin)
967  - /org.pixhawk.nodename-v1 device directory for org.pixhawk.nodename-v1
968  - - /1.0 version directory for hardware 1.0
969  - - /1.1 version directory for hardware 1.1
970  - - - /nodename-v1-1.0.25d0137d.bin firmware file for org.pixhawk.nodename-v1 nodes, hardware version 1.1
971  - /com.example.othernode-v3 device directory for com.example.othernode-v3
972  - - /1.0 version directory for hardawre 1.0
973  - - - /othernode-v3-1.6.25d0137d.bin firmware file for com.example.othernode-v3, hardware version 1.6
974 
975  The ROMFS directory structure is the same, but located at /etc/uavcan/fw
976  Files located there are prefixed with _ to identify them a comming from the rom
977  file system.
978 
979  We iterate over all device directories in the ROMFS base directory, and create
980  corresponding device directories on the SD card if they don't already exist.
981 
982  In each device directory, we iterate over each version directory and create a
983  corresponding version directory on the SD card if it doesn't already exist.
984 
985  In each version directory, we remove any files with a name starting with "_"
986  in the corresponding directory on the SD card that don't match the bundled firmware
987  filename; if the directory is empty after that process, we copy the bundled firmware.
988 
989  todo:This code would benefit from the use of strcat.
990 
991  */
992  const size_t maxlen = UAVCAN_MAX_PATH_LENGTH;
993  const size_t sd_path_len = strlen(sd_path);
994  const size_t romfs_path_len = strlen(romfs_path);
995  struct stat sb;
996  int rv;
997  char dstpath[maxlen + 1];
998  char srcpath[maxlen + 1];
999 
1000  DIR *const romfs_dir = opendir(romfs_path);
1001 
1002  if (!romfs_dir) {
1003  return;
1004  }
1005 
1006  memcpy(dstpath, sd_path, sd_path_len + 1);
1007  memcpy(srcpath, romfs_path, romfs_path_len + 1);
1008 
1009  // Iterate over all device directories in ROMFS
1010  struct dirent *dev_dirent = NULL;
1011 
1012  while ((dev_dirent = readdir(romfs_dir)) != NULL) {
1013  // Skip if not a directory
1014  if (!DIRENT_ISDIRECTORY(dev_dirent->d_type)) {
1015  continue;
1016  }
1017 
1018  // Make sure the path fits
1019  size_t dev_dirname_len = strlen(dev_dirent->d_name);
1020  size_t srcpath_dev_len = romfs_path_len + 1 + dev_dirname_len;
1021 
1022  if (srcpath_dev_len > maxlen) {
1023  PX4_WARN("dev: srcpath '%s/%s' too long", romfs_path, dev_dirent->d_name);
1024  continue;
1025  }
1026 
1027  size_t dstpath_dev_len = sd_path_len + 1 + dev_dirname_len;
1028 
1029  if (dstpath_dev_len > maxlen) {
1030  PX4_WARN("dev: dstpath '%s/%s' too long", sd_path, dev_dirent->d_name);
1031  continue;
1032  }
1033 
1034  // Create the device name directory on the SD card if it doesn't already exist
1035  dstpath[sd_path_len] = '/';
1036  memcpy(&dstpath[sd_path_len + 1], dev_dirent->d_name, dev_dirname_len + 1);
1037 
1038  if (stat(dstpath, &sb) != 0 || !S_ISDIR(sb.st_mode)) {
1039  rv = mkdir(dstpath, S_IRWXU | S_IRWXG | S_IRWXO);
1040 
1041  if (rv != 0) {
1042  PX4_ERR("dev: couldn't create '%s'", dstpath);
1043  continue;
1044  }
1045  }
1046 
1047  // Set up the source path
1048  srcpath[romfs_path_len] = '/';
1049  memcpy(&srcpath[romfs_path_len + 1], dev_dirent->d_name, dev_dirname_len + 1);
1050 
1051  DIR *const dev_dir = opendir(srcpath);
1052 
1053  if (!dev_dir) {
1054  PX4_ERR("dev: couldn't open '%s'", srcpath);
1055  continue;
1056  }
1057 
1058  // Iterate over all version directories in the current ROMFS device directory
1059  struct dirent *ver_dirent = NULL;
1060 
1061  while ((ver_dirent = readdir(dev_dir)) != NULL) {
1062  // Skip if not a directory
1063  if (!DIRENT_ISDIRECTORY(ver_dirent->d_type)) {
1064  continue;
1065  }
1066 
1067  // Make sure the path fits
1068  size_t ver_dirname_len = strlen(ver_dirent->d_name);
1069  size_t srcpath_ver_len = srcpath_dev_len + 1 + ver_dirname_len;
1070 
1071  if (srcpath_ver_len > maxlen) {
1072  PX4_ERR("ver: srcpath '%s/%s' too long", srcpath, ver_dirent->d_name);
1073  continue;
1074  }
1075 
1076  size_t dstpath_ver_len = dstpath_dev_len + 1 + ver_dirname_len;
1077 
1078  if (dstpath_ver_len > maxlen) {
1079  PX4_ERR("ver: dstpath '%s/%s' too long", dstpath, ver_dirent->d_name);
1080  continue;
1081  }
1082 
1083  // Create the device version directory on the SD card if it doesn't already exist
1084  dstpath[dstpath_dev_len] = '/';
1085  memcpy(&dstpath[dstpath_dev_len + 1], ver_dirent->d_name, ver_dirname_len + 1);
1086 
1087  if (stat(dstpath, &sb) != 0 || !S_ISDIR(sb.st_mode)) {
1088  rv = mkdir(dstpath, S_IRWXU | S_IRWXG | S_IRWXO);
1089 
1090  if (rv != 0) {
1091  PX4_ERR("ver: couldn't create '%s'", dstpath);
1092  continue;
1093  }
1094  }
1095 
1096  // Set up the source path
1097  srcpath[srcpath_dev_len] = '/';
1098  memcpy(&srcpath[srcpath_dev_len + 1], ver_dirent->d_name, ver_dirname_len + 1);
1099 
1100  // Find the name of the bundled firmware file, or move on to the
1101  // next directory if there's no file here.
1102  DIR *const src_ver_dir = opendir(srcpath);
1103 
1104  if (!src_ver_dir) {
1105  PX4_ERR("ver: couldn't open '%s'", srcpath);
1106  continue;
1107  }
1108 
1109  struct dirent *src_fw_dirent = NULL;
1110 
1111  while ((src_fw_dirent = readdir(src_ver_dir)) != NULL &&
1112  !DIRENT_ISFILE(src_fw_dirent->d_type));
1113 
1114  if (!src_fw_dirent) {
1115  (void)closedir(src_ver_dir);
1116  continue;
1117  }
1118 
1119  size_t fw_len = strlen(src_fw_dirent->d_name);
1120 
1121  bool copy_fw = true;
1122 
1123  // Clear out any romfs_ files in the version directory on the SD card
1124  DIR *const dst_ver_dir = opendir(dstpath);
1125 
1126  if (!dst_ver_dir) {
1127  PX4_ERR("unlink: couldn't open '%s'", dstpath);
1128 
1129  } else {
1130  struct dirent *fw_dirent = NULL;
1131 
1132  while ((fw_dirent = readdir(dst_ver_dir)) != NULL) {
1133  // Skip if not a file
1134  if (!DIRENT_ISFILE(fw_dirent->d_type)) {
1135  continue;
1136  }
1137 
1138  if (!memcmp(&fw_dirent->d_name, src_fw_dirent->d_name, fw_len)) {
1139  /*
1140  * Exact match between SD card filename and ROMFS filename; must be the same version
1141  * so don't bother deleting and rewriting it.
1142  */
1143  copy_fw = false;
1144 
1145  } else if (!memcmp(fw_dirent->d_name, UAVCAN_ROMFS_FW_PREFIX, sizeof(UAVCAN_ROMFS_FW_PREFIX) - 1)) {
1146  size_t dst_fw_len = strlen(fw_dirent->d_name);
1147  size_t dstpath_fw_len = dstpath_ver_len + dst_fw_len;
1148 
1149  if (dstpath_fw_len > maxlen) {
1150  // sizeof(prefix) includes trailing NUL, cancelling out the +1 for the path separator
1151  PX4_ERR("unlink: path '%s/%s' too long", dstpath, fw_dirent->d_name);
1152 
1153  } else {
1154  // File name starts with "_", delete it.
1155  dstpath[dstpath_ver_len] = '/';
1156  memcpy(&dstpath[dstpath_ver_len + 1], fw_dirent->d_name, dst_fw_len + 1);
1157  unlink(dstpath);
1158  PX4_ERR("unlink: removed '%s'", dstpath);
1159  }
1160 
1161  } else {
1162  // User file, don't copy firmware
1163  copy_fw = false;
1164  }
1165  }
1166 
1167  (void)closedir(dst_ver_dir);
1168  }
1169 
1170  // If we need to, copy the file from ROMFS to the SD card
1171  if (copy_fw) {
1172  size_t srcpath_fw_len = srcpath_ver_len + 1 + fw_len;
1173  size_t dstpath_fw_len = dstpath_ver_len + fw_len;
1174 
1175  if (srcpath_fw_len > maxlen) {
1176  PX4_ERR("copy: srcpath '%s/%s' too long", srcpath, src_fw_dirent->d_name);
1177 
1178  } else if (dstpath_fw_len > maxlen) {
1179  PX4_ERR("copy: dstpath '%s/%s' too long", dstpath, src_fw_dirent->d_name);
1180 
1181  } else {
1182  // All OK, make the paths and copy the file
1183  srcpath[srcpath_ver_len] = '/';
1184  memcpy(&srcpath[srcpath_ver_len + 1], src_fw_dirent->d_name, fw_len + 1);
1185 
1186  dstpath[dstpath_ver_len] = '/';
1187  memcpy(&dstpath[dstpath_ver_len + 1], src_fw_dirent->d_name, fw_len + 1);
1188 
1189  rv = copyFw(dstpath, srcpath);
1190 
1191  if (rv != 0) {
1192  PX4_ERR("copy: '%s' -> '%s' failed: %d", srcpath, dstpath, rv);
1193 
1194  } else {
1195  PX4_INFO("copy: '%s' -> '%s' succeeded", srcpath, dstpath);
1196  }
1197  }
1198  }
1199 
1200  (void)closedir(src_ver_dir);
1201  }
1202 
1203  (void)closedir(dev_dir);
1204  }
1205 
1206  (void)closedir(romfs_dir);
1207 }
1208 
1209 int
1210 UavcanServers::copyFw(const char *dst, const char *src)
1211 {
1212  int rv = 0;
1213  uint8_t buffer[512] {};
1214 
1215  int dfd = open(dst, O_WRONLY | O_CREAT, 0666);
1216 
1217  if (dfd < 0) {
1218  PX4_ERR("copyFw: couldn't open dst");
1219  return -errno;
1220  }
1221 
1222  int sfd = open(src, O_RDONLY, 0);
1223 
1224  if (sfd < 0) {
1225  (void)close(dfd);
1226  PX4_ERR("copyFw: couldn't open src");
1227  return -errno;
1228  }
1229 
1230  ssize_t size = 0;
1231 
1232  do {
1233  size = read(sfd, buffer, sizeof(buffer));
1234 
1235  if (size < 0) {
1236  PX4_ERR("copyFw: couldn't read");
1237  rv = -errno;
1238 
1239  } else if (size > 0) {
1240  rv = 0;
1241  ssize_t remaining = size;
1242  ssize_t total_written = 0;
1243  ssize_t written = 0;
1244 
1245  do {
1246  written = write(dfd, &buffer[total_written], remaining);
1247 
1248  if (written < 0) {
1249  PX4_ERR("copyFw: couldn't write");
1250  rv = -errno;
1251 
1252  } else {
1253  total_written += written;
1254  remaining -= written;
1255  }
1256  } while (written > 0 && remaining > 0);
1257  }
1258  } while (rv == 0 && size != 0);
1259 
1260  (void)close(dfd);
1261  (void)close(sfd);
1262 
1263  return rv;
1264 }
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
static int stop()
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
LidarLite * instance
Definition: ll40ls.cpp:65
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]
Definition: px4io.c:89
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.
Definition: uORB.h:87
uavcan_posix::dynamic_node_id_server::FileStorageBackend _storage_backend
bool publish(const T &data)
Publish the struct.
Definition: Publication.hpp:68
static UavcanServers * _instance
singleton pointer
void param_opcode(uavcan::NodeID node_id)
static int init(pthread_mutex_t &thier_mutex_)
volatile bool _check_fw
uavcan_posix::dynamic_node_id_server::FileEventTracer _tracer
virtual ~UavcanServers()
static struct actuator_armed_s armed
Definition: Commander.cpp:139
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)
Definition: Limits.hpp:54
Tools for system version detection.
uavcan::SubNode _subnode
static void write(bootloader_app_shared_t *pshared)
uavcan::ServiceClient< uavcan::protocol::RestartNode, RestartNodeCallback > _param_restartnode_client
constexpr _Tp max(_Tp a, _Tp b)
Definition: Limits.hpp:60
#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
#define UAVCAN_LOG_FILE
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