PX4 Firmware
PX4 Autopilot Software http://px4.io
uavcan_main.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 /**
35  * @file uavcan_main.cpp
36  *
37  * Implements basic functionality of UAVCAN node.
38  *
39  * @author Pavel Kirienko <pavel.kirienko@gmail.com>
40  * @author David Sidrane <david_s5@nscdg.com>
41  * @author Andreas Jochum <Andreas@NicaDrone.com>
42  *
43  */
44 
45 #include <px4_platform_common/px4_config.h>
46 #include <px4_platform_common/tasks.h>
47 
48 #include <cstdlib>
49 #include <cstring>
50 #include <fcntl.h>
51 #include <systemlib/err.h>
52 #include <parameters/param.h>
53 #include <version/version.h>
54 #include <arch/board/board.h>
55 #include <arch/chip/chip.h>
56 
57 #include <uORB/topics/esc_status.h>
58 
59 #include <drivers/drv_hrt.h>
60 #include <drivers/drv_mixer.h>
61 #include <drivers/drv_pwm_output.h>
62 
63 #include "uavcan_module.hpp"
64 #include "uavcan_main.hpp"
65 #include <uavcan/util/templates.hpp>
66 
67 #include <uavcan/protocol/param/ExecuteOpcode.hpp>
68 
69 //todo:The Inclusion of file_server_backend is killing
70 // #include <sys/types.h> and leaving OK undefined
71 # define OK 0
72 
73 /*
74  * UavcanNode
75  */
77 
78 UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) :
79  CDev(UAVCAN_DEVICE_PATH),
80  ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::uavcan),
81  ModuleParams(nullptr),
82  _node(can_driver, system_clock, _pool_allocator),
83  _esc_controller(_node),
84  _hardpoint_controller(_node),
85  _time_sync_master(_node),
86  _time_sync_slave(_node),
87  _node_status_monitor(_node),
88  _cycle_perf(perf_alloc(PC_ELAPSED, "uavcan: cycle time")),
89  _interval_perf(perf_alloc(PC_INTERVAL, "uavcan: cycle interval")),
90  _master_timer(_node)
91 {
92  int res = pthread_mutex_init(&_node_mutex, nullptr);
93 
94  if (res < 0) {
95  std::abort();
96  }
97 
98  res = px4_sem_init(&_server_command_sem, 0, 0);
99 
100  if (res < 0) {
101  std::abort();
102  }
103 
104  /* _server_command_sem use case is a signal */
105  px4_sem_setprotocol(&_server_command_sem, SEM_PRIO_NONE);
106 }
107 
109 {
110  fw_server(Stop);
111 
112  if (_instance) {
113 
114  /* tell the task we want it to go away */
115  _task_should_exit.store(true);
116  ScheduleNow();
117 
118  unsigned i = 10;
119 
120  do {
121  /* wait 5ms - it should wake every 10ms or so worst-case */
122  usleep(5000);
123 
124  if (--i == 0) {
125  break;
126  }
127 
128  } while (_instance);
129  }
130 
131  // Removing the sensor bridges
132  _sensor_bridges.clear();
133 
134  pthread_mutex_destroy(&_node_mutex);
135  px4_sem_destroy(&_server_command_sem);
136 
139 }
140 
141 int
142 UavcanNode::getHardwareVersion(uavcan::protocol::HardwareVersion &hwver)
143 {
144  int rv = -1;
145 
146  if (UavcanNode::instance()) {
147  if (!std::strncmp(px4_board_name(), "PX4_FMU_V2", 9)) {
148  hwver.major = 2;
149 
150  } else {
151  ; // All other values of px4_board_name() resolve to zero
152  }
153 
154  mfguid_t mfgid = {};
155  board_get_mfguid(mfgid);
156  uavcan::copy(mfgid, mfgid + sizeof(mfgid), hwver.unique_id.begin());
157  rv = 0;
158  }
159 
160  return rv;
161 }
162 
163 int
164 UavcanNode::print_params(uavcan::protocol::param::GetSet::Response &resp)
165 {
166  if (resp.value.is(uavcan::protocol::param::Value::Tag::integer_value)) {
167  return std::printf("name: %s %lld\n", resp.name.c_str(),
168  resp.value.to<uavcan::protocol::param::Value::Tag::integer_value>());
169 
170  } else if (resp.value.is(uavcan::protocol::param::Value::Tag::real_value)) {
171  return std::printf("name: %s %.4f\n", resp.name.c_str(),
172  static_cast<double>(resp.value.to<uavcan::protocol::param::Value::Tag::real_value>()));
173 
174  } else if (resp.value.is(uavcan::protocol::param::Value::Tag::boolean_value)) {
175  return std::printf("name: %s %d\n", resp.name.c_str(),
176  resp.value.to<uavcan::protocol::param::Value::Tag::boolean_value>());
177 
178  } else if (resp.value.is(uavcan::protocol::param::Value::Tag::string_value)) {
179  return std::printf("name: %s '%s'\n", resp.name.c_str(),
180  resp.value.to<uavcan::protocol::param::Value::Tag::string_value>().c_str());
181  }
182 
183  return -1;
184 }
185 
186 void
187 UavcanNode::cb_opcode(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result)
188 {
189  uavcan::protocol::param::ExecuteOpcode::Response resp;
190  _callback_success = result.isSuccessful();
191  resp = result.getResponse();
192  _callback_success &= resp.ok;
193 }
194 
195 int
196 UavcanNode::save_params(int remote_node_id)
197 {
198  uavcan::protocol::param::ExecuteOpcode::Request opcode_req;
199  opcode_req.opcode = opcode_req.OPCODE_SAVE;
200  uavcan::ServiceClient<uavcan::protocol::param::ExecuteOpcode, ExecuteOpcodeCallback> client(_node);
201  client.setCallback(ExecuteOpcodeCallback(this, &UavcanNode::cb_opcode));
202  _callback_success = false;
203  int call_res = client.call(remote_node_id, opcode_req);
204 
205  if (call_res >= 0) {
206  while (client.hasPendingCalls()) {
207  usleep(10000);
208  }
209  }
210 
211  if (!_callback_success) {
212  std::printf("Failed to save parameters: %d\n", call_res);
213  return -1;
214  }
215 
216  return 0;
217 }
218 
219 void
220 UavcanNode::cb_restart(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &result)
221 {
222  uavcan::protocol::RestartNode::Response resp;
223  _callback_success = result.isSuccessful();
224  resp = result.getResponse();
225  _callback_success &= resp.ok;
226 }
227 
228 int
229 UavcanNode::reset_node(int remote_node_id)
230 {
231  uavcan::protocol::RestartNode::Request restart_req;
232  restart_req.magic_number = restart_req.MAGIC_NUMBER;
233  uavcan::ServiceClient<uavcan::protocol::RestartNode, RestartNodeCallback> client(_node);
234  client.setCallback(RestartNodeCallback(this, &UavcanNode::cb_restart));
235  _callback_success = false;
236  int call_res = client.call(remote_node_id, restart_req);
237 
238  if (call_res >= 0) {
239  while (client.hasPendingCalls()) {
240  usleep(10000);
241  }
242  }
243 
244  if (!call_res) {
245  std::printf("Failed to reset node: %d\n", remote_node_id);
246  return -1;
247  }
248 
249  return 0;
250 }
251 
252 int
253 UavcanNode::list_params(int remote_node_id)
254 {
255  int rv = 0;
256  int index = 0;
257  uavcan::protocol::param::GetSet::Response resp;
258  set_setget_response(&resp);
259 
260  while (true) {
261  uavcan::protocol::param::GetSet::Request req;
262  req.index = index++;
263  _callback_success = false;
264  int call_res = get_set_param(remote_node_id, nullptr, req);
265 
266  if (call_res < 0 || !_callback_success) {
267  std::printf("Failed to get param: %d\n", call_res);
268  rv = -1;
269  break;
270  }
271 
272  if (resp.name.empty()) { // Empty name means no such param, which means we're finished
273  break;
274  }
275 
276  print_params(resp);
277  }
278 
280  return rv;
281 }
282 
283 void
284 UavcanNode::cb_setget(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result)
285 {
286  _callback_success = result.isSuccessful();
287  *_setget_response = result.getResponse();
288 }
289 
290 int
291 UavcanNode::get_set_param(int remote_node_id, const char *name, uavcan::protocol::param::GetSet::Request &req)
292 {
293  if (name != nullptr) {
294  req.name = name;
295  }
296 
297  uavcan::ServiceClient<uavcan::protocol::param::GetSet, GetSetCallback> client(_node);
298  client.setCallback(GetSetCallback(this, &UavcanNode::cb_setget));
299  _callback_success = false;
300  int call_res = client.call(remote_node_id, req);
301 
302  if (call_res >= 0) {
303 
304  while (client.hasPendingCalls()) {
305  usleep(10000);
306  }
307 
308  if (!_callback_success) {
309  call_res = -1;
310  }
311  }
312 
313  return call_res;
314 }
315 
316 int
317 UavcanNode::set_param(int remote_node_id, const char *name, char *value)
318 {
319  uavcan::protocol::param::GetSet::Request req;
320  uavcan::protocol::param::GetSet::Response resp;
321  set_setget_response(&resp);
322  int rv = get_set_param(remote_node_id, name, req);
323 
324  if (rv < 0 || resp.name.empty()) {
325  std::printf("Failed to retrieve param: %s\n", name);
326  rv = -1;
327 
328  } else {
329 
330  rv = 0;
331  req = {};
332 
333  if (resp.value.is(uavcan::protocol::param::Value::Tag::integer_value)) {
334  int64_t i = std::strtoull(value, NULL, 10);
335  int64_t min = resp.min_value.to<uavcan::protocol::param::NumericValue::Tag::integer_value>();
336  int64_t max = resp.max_value.to<uavcan::protocol::param::NumericValue::Tag::integer_value>();
337 
338  if (i >= min && i <= max) {
339  req.value.to<uavcan::protocol::param::Value::Tag::integer_value>() = i;
340 
341  } else {
342  std::printf("Invalid value for: %s must be between %lld and %lld\n", name, min, max);
343  rv = -1;
344  }
345 
346  } else if (resp.value.is(uavcan::protocol::param::Value::Tag::real_value)) {
347  float f = static_cast<float>(std::atof(value));
348  float min = resp.min_value.to<uavcan::protocol::param::NumericValue::Tag::real_value>();
349  float max = resp.max_value.to<uavcan::protocol::param::NumericValue::Tag::real_value>();
350 
351  if (f >= min && f <= max) {
352  req.value.to<uavcan::protocol::param::Value::Tag::real_value>() = f;
353 
354  } else {
355  std::printf("Invalid value for: %s must be between %.4f and %.4f\n", name, static_cast<double>(min),
356  static_cast<double>(max));
357  rv = -1;
358  }
359 
360  } else if (resp.value.is(uavcan::protocol::param::Value::Tag::boolean_value)) {
361  int8_t i = (value[0] == '1' || value[0] == 't') ? 1 : 0;
362  req.value.to<uavcan::protocol::param::Value::Tag::boolean_value>() = i;
363 
364  } else if (resp.value.is(uavcan::protocol::param::Value::Tag::string_value)) {
365  req.value.to<uavcan::protocol::param::Value::Tag::string_value>() = value;
366  }
367 
368  if (rv == 0) {
369  rv = get_set_param(remote_node_id, name, req);
370 
371  if (rv < 0 || resp.name.empty()) {
372  std::printf("Failed to set param: %s\n", name);
373  return -1;
374  }
375 
376  return 0;
377  }
378  }
379 
381  return rv;
382 }
383 
384 int
385 UavcanNode::get_param(int remote_node_id, const char *name)
386 {
387  uavcan::protocol::param::GetSet::Request req;
388  uavcan::protocol::param::GetSet::Response resp;
389  set_setget_response(&resp);
390  int rv = get_set_param(remote_node_id, name, req);
391 
392  if (rv < 0 || resp.name.empty()) {
393  std::printf("Failed to get param: %s\n", name);
394  rv = -1;
395 
396  } else {
397  print_params(resp);
398  rv = 0;
399  }
400 
402  return rv;
403 }
404 
405 void
407 {
408  _mixing_interface.updateParams();
409 }
410 
411 int
413 {
414  int rv = -1;
415  _fw_server_action.store((int)Busy);
417 
418  if (_servers == nullptr) {
419 
421 
422  if (rv >= 0) {
423  /*
424  * Set our pointer to to the injector
425  * This is a work around as
426  * main_node.getDispatcher().installRxFrameListener(driver.get());
427  * would require a dynamic cast and rtti is not enabled.
428  */
430  }
431  }
432 
433  _fw_server_action.store((int)None);
434  px4_sem_post(&_server_command_sem);
435  return rv;
436 }
437 
438 int
440 {
441  int rv = -1;
442  _fw_server_action.store((int)Busy);
444 
445  if (_servers != nullptr) {
447  rv = 0;
448  }
449 
450  _fw_server_action.store((int)None);
451  px4_sem_post(&_server_command_sem);
452  return rv;
453 }
454 
455 int
457 {
458  int rv = -1;
459  _fw_server_action.store((int)Busy);
461 
462  if (_servers != nullptr) {
463  /*
464  * Set our pointer to to the injector
465  * This is a work around as
466  * main_node.getDispatcher().remeveRxFrameListener();
467  * would require a dynamic cast and rtti is not enabled.
468  */
469  _tx_injector = nullptr;
470 
471  rv = _servers->stop();
472  }
473 
474  _fw_server_action.store((int)None);
475  px4_sem_post(&_server_command_sem);
476  return rv;
477 }
478 
479 int
481 {
482  int rv = -EAGAIN;
483 
484  switch (action) {
485  case Start:
486  case Stop:
487  case CheckFW:
488  if (_fw_server_action.load() == (int)None) {
489  _fw_server_action.store((int)action);
490  px4_sem_wait(&_server_command_sem);
491  rv = _fw_server_status;
492  }
493 
494  break;
495 
496  default:
497  rv = -EINVAL;
498  break;
499  }
500 
501  return rv;
502 }
503 
504 int
505 UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
506 {
507  if (_instance != nullptr) {
508  PX4_WARN("Already started");
509  return -1;
510  }
511 
512  /*
513  * CAN driver init
514  * Note that we instantiate and initialize CanInitHelper only once, because the STM32's bxCAN driver
515  * shipped with libuavcan does not support deinitialization.
516  */
517  static CanInitHelper *can = nullptr;
518 
519  if (can == nullptr) {
520 
521  can = new CanInitHelper();
522 
523  if (can == nullptr) { // We don't have exceptions so bad_alloc cannot be thrown
524  PX4_ERR("Out of memory");
525  return -1;
526  }
527 
528  const int can_init_res = can->init(bitrate);
529 
530  if (can_init_res < 0) {
531  PX4_ERR("CAN driver init failed %i", can_init_res);
532  return can_init_res;
533  }
534  }
535 
536  /*
537  * Node init
538  */
540 
541  if (_instance == nullptr) {
542  PX4_ERR("Out of memory");
543  return -1;
544  }
545 
546  const int node_init_res = _instance->init(node_id, can->driver.updateEvent());
547 
548  if (node_init_res < 0) {
549  delete _instance;
550  _instance = nullptr;
551  PX4_ERR("Node init failed %i", node_init_res);
552  return node_init_res;
553  }
554 
555  _instance->ScheduleOnInterval(ScheduleIntervalMs * 1000);
556 
557  return OK;
558 }
559 
560 void
562 {
563  /* software version */
564  uavcan::protocol::SoftwareVersion swver;
565 
566  // Extracting the first 8 hex digits of the git hash and converting them to int
567  char fw_git_short[9] = {};
568  std::memmove(fw_git_short, px4_firmware_version_string(), 8);
569  char *end = nullptr;
570  swver.vcs_commit = std::strtol(fw_git_short, &end, 16);
571  swver.optional_field_flags |= swver.OPTIONAL_FIELD_FLAG_VCS_COMMIT;
572 
573  // Too verbose for normal operation
574  //PX4_INFO("SW version vcs_commit: 0x%08x", unsigned(swver.vcs_commit));
575 
576  _node.setSoftwareVersion(swver);
577 
578  /* hardware version */
579  uavcan::protocol::HardwareVersion hwver;
580  getHardwareVersion(hwver);
581  _node.setHardwareVersion(hwver);
582 }
583 
584 void
586 {
587  if (_instance) {
588  // trigger the work queue (Note, this is called from IRQ context)
589  _instance->ScheduleNow();
590  }
591 }
592 
593 int
594 UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events)
595 {
596  // Do regular cdev init
597  int ret = CDev::init();
598 
599  if (ret != OK) {
600  return ret;
601  }
602 
603  bus_events.registerSignalCallback(UavcanNode::busevent_signal_trampoline);
604 
605  _node.setName("org.pixhawk.pixhawk");
606 
607  _node.setNodeID(node_id);
608 
609  fill_node_info();
610 
611  // Actuators
612  ret = _esc_controller.init();
613 
614  if (ret < 0) {
615  return ret;
616  }
617 
618  ret = _hardpoint_controller.init();
619 
620  if (ret < 0) {
621  return ret;
622  }
623 
624  // Sensor bridges
626 
627  for (const auto &br : _sensor_bridges) {
628  ret = br->init();
629 
630  if (ret < 0) {
631  PX4_ERR("cannot init sensor bridge '%s' (%d)", br->get_name(), ret);
632  return ret;
633  }
634 
635  PX4_INFO("sensor bridge '%s' init ok", br->get_name());
636  }
637 
639  _mixing_interface.mixingOutput().setAllMinValues(0); // Can be changed to 1 later, according to UAVCAN_ESC_IDLT
640 
641  // Ensure we don't exceed maximum limits and assumptions. FIXME: these should be static assertions
643  || UavcanEscController::max_output_value() > (int)UINT16_MAX) {
644  PX4_ERR("ESC max output value assertion failed");
645  return -EINVAL;
646  }
647 
650 
653 
654  /* Start the Node */
655  return _node.start();
656 }
657 
658 void
660 {
661  const int spin_res = _node.spinOnce();
662 
663  if (spin_res < 0) {
664  PX4_ERR("node spin error %i", spin_res);
665  }
666 
667 
668  if (_tx_injector != nullptr) {
670  }
671 }
672 
673 void
674 UavcanNode::handle_time_sync(const uavcan::TimerEvent &)
675 {
676  /*
677  * Check whether there are higher priority masters in the network.
678  * If there are, we need to activate the local slave in order to sync with them.
679  */
680  if (_time_sync_slave.isActive()) { // "Active" means that the slave tracks at least one remote master in the network
681  if (_node.getNodeID() < _time_sync_slave.getMasterNodeID()) {
682  /*
683  * We're the highest priority master in the network.
684  * We need to suppress the slave now to prevent it from picking up unwanted sync messages from
685  * lower priority masters.
686  */
687  _time_sync_slave.suppress(true); // SUPPRESS
688 
689  } else {
690  /*
691  * There is at least one higher priority master in the network.
692  * We need to allow the slave to adjust our local clock in order to be in sync.
693  */
694  _time_sync_slave.suppress(false); // UNSUPPRESS
695  }
696 
697  } else {
698  /*
699  * There are no other time sync masters in the network, so we're the only time source.
700  * The slave must be suppressed anyway to prevent it from disrupting the local clock if a new
701  * lower priority master suddenly appears in the network.
702  */
703  _time_sync_slave.suppress(true);
704  }
705 
706  /*
707  * Publish the sync message now, even if we're not a higher priority master.
708  * Other nodes will be able to pick the right master anyway.
709  */
710  _time_sync_master.publish();
711 }
712 
713 
714 
715 void
717 {
718  pthread_mutex_lock(&_node_mutex);
719 
720  if (_output_count == 0) {
721  // Set up the time synchronization
722  const int slave_init_res = _time_sync_slave.start();
723 
724  if (slave_init_res < 0) {
725  PX4_ERR("Failed to start time_sync_slave");
726  ScheduleClear();
727  return;
728  }
729 
730  /* When we have a system wide notion of time update (i.e the transition from the initial
731  * System RTC setting to the GPS) we would call UAVCAN_DRIVER::clock::setUtc() when that
732  * happens, but for now we use adjustUtc with a correction of the hrt so that the
733  * time bases are the same
734  */
735  UAVCAN_DRIVER::clock::adjustUtc(uavcan::UtcDuration::fromUSec(hrt_absolute_time()));
737  _master_timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000));
738 
739  _node_status_monitor.start();
740  _node.setModeOperational();
741 
742  update_params();
743 
744  // XXX figure out the output count
745  _output_count = 2;
746  }
747 
748 
751 
752  node_spin_once(); // expected to be non-blocking
753 
754  // Check arming state
757 
758  // check for parameter updates
760  // clear update
761  parameter_update_s pupdate;
762  _parameter_update_sub.copy(&pupdate);
763 
764  // update parameters from storage
765  update_params();
766  }
767 
768  switch ((eServerAction)_fw_server_action.load()) {
769  case Start:
771  break;
772 
773  case Stop:
775  break;
776 
777  case CheckFW:
779  break;
780 
781  case None:
782  default:
783  break;
784  }
785 
787 
788  pthread_mutex_unlock(&_node_mutex);
789 
790  if (_task_should_exit.load()) {
792  _mixing_interface.ScheduleClear();
793  ScheduleClear();
794  teardown();
795  _instance = nullptr;
796  }
797 }
798 
799 void
801 {
802  value &= _idle_throttle_when_armed_param > 0;
803 
804  if (value != _idle_throttle_when_armed) {
807  }
808 }
809 
810 int
812 {
813  px4_sem_post(&_server_command_sem);
814  return 0;
815 }
816 
817 int
818 UavcanNode::ioctl(file *filp, int cmd, unsigned long arg)
819 {
820  int ret = OK;
821 
822  lock();
823 
824  switch (cmd) {
828  // these are no-ops, as no safety switch
829  break;
830 
831  case MIXERIOCRESET:
833 
834  break;
835 
836  case MIXERIOCLOADBUF: {
837  const char *buf = (const char *)arg;
838  unsigned buflen = strlen(buf);
840  }
841  break;
842 
843 
845  const auto &hp_cmd = *reinterpret_cast<uavcan::equipment::hardpoint::Command *>(arg);
846  _hardpoint_controller.set_command(hp_cmd.hardpoint_id, hp_cmd.command);
847  }
848  break;
849 
852 
853  if (_servers == nullptr) {
854  // status unavailable
855  ret = -EINVAL;
856  break;
857 
858  } else if (_servers->guessIfAllDynamicNodesAreAllocated()) {
859  // node discovery complete
860  ret = -ETIME;
861  break;
862 
863  } else {
864  // node discovery in progress
865  ret = OK;
866  break;
867  }
868  }
869 
870  default:
871  ret = -ENOTTY;
872  break;
873  }
874 
875  unlock();
876 
877  if (ret == -ENOTTY) {
878  ret = CDev::ioctl(filp, cmd, arg);
879  }
880 
881  return ret;
882 }
883 
884 
885 bool UavcanMixingInterface::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
886  unsigned num_control_groups_updated)
887 {
888  _esc_controller.update_outputs(stop_motors, outputs, num_outputs);
889  return true;
890 }
891 
893 {
894  pthread_mutex_lock(&_node_mutex);
895  _mixing_output.update();
896  _mixing_output.updateSubscriptions(false);
897  pthread_mutex_unlock(&_node_mutex);
898 }
899 
901 {
902  int rotor_count = 0;
903 
904  if (_mixing_output.mixers()) {
905  rotor_count = _mixing_output.mixers()->get_multirotor_count();
906  }
907 
908  _esc_controller.set_rotor_count(rotor_count);
909 }
910 
911 void
913 {
914  (void)pthread_mutex_lock(&_node_mutex);
915 
916  // Memory status
917  printf("Pool allocator status:\n");
918  printf("\tCapacity hard/soft: %u/%u blocks\n",
919  _pool_allocator.getBlockCapacityHardLimit(), _pool_allocator.getBlockCapacity());
920  printf("\tReserved: %u blocks\n", _pool_allocator.getNumReservedBlocks());
921  printf("\tAllocated: %u blocks\n", _pool_allocator.getNumAllocatedBlocks());
922 
923  printf("\n");
924 
925  // UAVCAN node perfcounters
926  printf("UAVCAN node status:\n");
927  printf("\tInternal failures: %llu\n", _node.getInternalFailureCount());
928  printf("\tTransfer errors: %llu\n", _node.getDispatcher().getTransferPerfCounter().getErrorCount());
929  printf("\tRX transfers: %llu\n", _node.getDispatcher().getTransferPerfCounter().getRxTransferCount());
930  printf("\tTX transfers: %llu\n", _node.getDispatcher().getTransferPerfCounter().getTxTransferCount());
931 
932  printf("\n");
933 
934  // CAN driver status
935  for (unsigned i = 0; i < _node.getDispatcher().getCanIOManager().getCanDriver().getNumIfaces(); i++) {
936  printf("CAN%u status:\n", unsigned(i + 1));
937 
938  auto iface = _node.getDispatcher().getCanIOManager().getCanDriver().getIface(i);
939  printf("\tHW errors: %llu\n", iface->getErrorCount());
940 
941  auto iface_perf_cnt = _node.getDispatcher().getCanIOManager().getIfacePerfCounters(i);
942  printf("\tIO errors: %llu\n", iface_perf_cnt.errors);
943  printf("\tRX frames: %llu\n", iface_perf_cnt.frames_rx);
944  printf("\tTX frames: %llu\n", iface_perf_cnt.frames_tx);
945  }
946 
947  printf("\n");
948 
949  // ESC mixer status
951 
952  printf("\n");
953 
954  // Sensor bridges
955  for (const auto &br : _sensor_bridges) {
956  printf("Sensor '%s':\n", br->get_name());
957  br->print_status();
958  printf("\n");
959  }
960 
961  // Printing all nodes that are online
962  printf("Online nodes (Node ID, Health, Mode):\n");
963  _node_status_monitor.forEachNode([](uavcan::NodeID nid, uavcan::NodeStatusMonitor::NodeStatus ns) {
964  static constexpr const char *HEALTH[] = {"OK", "WARN", "ERR", "CRIT"};
965  static constexpr const char *MODES[] = {"OPERAT", "INIT", "MAINT", "SW_UPD", "?", "?", "?", "OFFLN"};
966  printf("\t% 3d %-10s %-10s\n", int(nid.get()), HEALTH[ns.health], MODES[ns.mode]);
967  });
968 
969  printf("\n");
970 
973 
974  (void)pthread_mutex_unlock(&_node_mutex);
975 }
976 
977 void
979 {
980  (void)pthread_mutex_lock(&_node_mutex);
981  _pool_allocator.shrink();
982  (void)pthread_mutex_unlock(&_node_mutex);
983 }
984 
985 void
986 UavcanNode::hardpoint_controller_set(uint8_t hardpoint_id, uint16_t command)
987 {
988  (void)pthread_mutex_lock(&_node_mutex);
989  _hardpoint_controller.set_command(hardpoint_id, command);
990  (void)pthread_mutex_unlock(&_node_mutex);
991 }
992 
993 /*
994  * App entry point
995  */
996 static void print_usage()
997 {
998  PX4_INFO("usage: \n"
999  "\tuavcan {start [fw]|status|stop [all|fw]|shrink|arm|disarm|update fw|\n"
1000  "\t param [set|get|list|save] <node-id> <name> <value>|reset <node-id>|\n"
1001  "\t hardpoint set <id> <command>}");
1002 }
1003 
1004 extern "C" __EXPORT int uavcan_main(int argc, char *argv[]);
1005 
1006 int uavcan_main(int argc, char *argv[])
1007 {
1008  if (argc < 2) {
1009  print_usage();
1010  ::exit(1);
1011  }
1012 
1013  bool fw = argc > 2 && !std::strcmp(argv[2], "fw");
1014 
1015  if (!std::strcmp(argv[1], "start")) {
1016  if (UavcanNode::instance()) {
1017  if (fw && UavcanServers::instance() == nullptr) {
1019 
1020  if (rv < 0) {
1021  PX4_ERR("Firmware Server Failed to Start %d", rv);
1022  ::exit(rv);
1023  }
1024 
1025  ::exit(0);
1026  }
1027 
1028  // Already running, no error
1029  PX4_INFO("already started");
1030  ::exit(0);
1031  }
1032 
1033  // Node ID
1034  int32_t node_id = 1;
1035  (void)param_get(param_find("UAVCAN_NODE_ID"), &node_id);
1036 
1037  if (node_id < 0 || node_id > uavcan::NodeID::Max || !uavcan::NodeID(node_id).isUnicast()) {
1038  PX4_ERR("Invalid Node ID %i", node_id);
1039  ::exit(1);
1040  }
1041 
1042  // CAN bitrate
1043  int32_t bitrate = 1000000;
1044  (void)param_get(param_find("UAVCAN_BITRATE"), &bitrate);
1045 
1046  // Start
1047  PX4_INFO("Node ID %u, bitrate %u", node_id, bitrate);
1048  return UavcanNode::start(node_id, bitrate);
1049  }
1050 
1051  /* commands below require the app to be started */
1052  UavcanNode *const inst = UavcanNode::instance();
1053 
1054  if (!inst) {
1055  errx(1, "application not running");
1056  }
1057 
1058  if (fw && !std::strcmp(argv[1], "update")) {
1059  if (UavcanServers::instance() == nullptr) {
1060  errx(1, "firmware server is not running");
1061  }
1062 
1064  ::exit(rv);
1065  }
1066 
1067  if (fw && (!std::strcmp(argv[1], "status") || !std::strcmp(argv[1], "info"))) {
1068  printf("Firmware Server is %s\n", UavcanServers::instance() ? "Running" : "Stopped");
1069  ::exit(0);
1070  }
1071 
1072  if (!std::strcmp(argv[1], "status") || !std::strcmp(argv[1], "info")) {
1073  inst->print_info();
1074  ::exit(0);
1075  }
1076 
1077  if (!std::strcmp(argv[1], "shrink")) {
1078  inst->shrink();
1079  ::exit(0);
1080  }
1081 
1082  /*
1083  * Parameter setting commands
1084  *
1085  * uavcan param list <node>
1086  * uavcan param save <node>
1087  * uavcan param get <node> <name>
1088  * uavcan param set <node> <name> <value>
1089  *
1090  */
1091  int node_arg = !std::strcmp(argv[1], "reset") ? 2 : 3;
1092 
1093  if (!std::strcmp(argv[1], "param") || node_arg == 2) {
1094  if (argc < node_arg + 1) {
1095  errx(1, "Node id required");
1096  }
1097 
1098  int nodeid = atoi(argv[node_arg]);
1099 
1100  if (nodeid == 0 || nodeid > 127 || nodeid == inst->get_node().getNodeID().get()) {
1101  errx(1, "Invalid Node id");
1102  }
1103 
1104  if (node_arg == 2) {
1105 
1106  return inst->reset_node(nodeid);
1107 
1108  } else if (!std::strcmp(argv[2], "list")) {
1109 
1110  return inst->list_params(nodeid);
1111 
1112  } else if (!std::strcmp(argv[2], "save")) {
1113 
1114  return inst->save_params(nodeid);
1115 
1116  } else if (!std::strcmp(argv[2], "get")) {
1117  if (argc < 5) {
1118  errx(1, "Name required");
1119  }
1120 
1121  return inst->get_param(nodeid, argv[4]);
1122 
1123  } else if (!std::strcmp(argv[2], "set")) {
1124  if (argc < 5) {
1125  errx(1, "Name required");
1126  }
1127 
1128  if (argc < 6) {
1129  errx(1, "Value required");
1130  }
1131 
1132  return inst->set_param(nodeid, argv[4], argv[5]);
1133  }
1134  }
1135 
1136  if (!std::strcmp(argv[1], "hardpoint")) {
1137  if (!std::strcmp(argv[2], "set") && argc > 4) {
1138  const int hardpoint_id = atoi(argv[3]);
1139  const int command = atoi(argv[4]);
1140 
1141  // Sanity check - weed out negative values, check against maximums
1142  if (hardpoint_id >= 0 && hardpoint_id < 256 &&
1143  command >= 0 && command < 65536) {
1144  inst->hardpoint_controller_set((uint8_t) hardpoint_id, (uint16_t) command);
1145 
1146  } else {
1147  errx(1, "Invalid argument");
1148  }
1149 
1150  } else {
1151  errx(1, "Invalid hardpoint command");
1152  }
1153 
1154  ::exit(0);
1155  }
1156 
1157  if (!std::strcmp(argv[1], "stop")) {
1158  if (fw) {
1159 
1160  int rv = inst->fw_server(UavcanNode::Stop);
1161 
1162  /* Let's recover any memory we can */
1163 
1164  inst->shrink();
1165 
1166  if (rv < 0) {
1167  PX4_ERR("Firmware Server Failed to Stop %d", rv);
1168  ::exit(rv);
1169  }
1170 
1171  ::exit(0);
1172 
1173  } else {
1174  delete inst;
1175  ::exit(0);
1176  }
1177  }
1178 
1179  print_usage();
1180  ::exit(1);
1181 }
virtual ~UavcanNode()
#define MIXERIOCLOADBUF
Add mixer(s) from the buffer in (const char *)arg.
Definition: drv_mixer.h:79
static int start(uavcan::NodeID node_id, uint32_t bitrate)
#define PWM_SERVO_SET_ARM_OK
set the &#39;ARM ok&#39; bit, which activates the safety switch
const actuator_armed_s & armed() const
uavcan::TimerEventForwarder< TimerCallback > _master_timer
void printStatus() const
static void make_all(uavcan::INode &node, List< IUavcanSensorBridge *> &list)
Sensor bridge factory.
#define UAVCAN_DEVICE_PATH
measure the time elapsed performing an event
Definition: perf_counter.h:56
int start_fw_server()
uavcan::Node _node
library instance
__EXPORT int param_get(param_t param, void *val)
Copy the value of a parameter.
Definition: parameters.cpp:589
uavcan::MethodBinder< UavcanNode *, void(UavcanNode::*)(const uavcan::ServiceCallResult< uavcan::protocol::param::GetSet > &)> GetSetCallback
void update_params()
void Run() override
static void busevent_signal_trampoline()
int print_params(uavcan::protocol::param::GetSet::Response &resp)
int get_set_param(int nodeid, const char *name, uavcan::protocol::param::GetSet::Request &req)
MixingOutput & mixingOutput()
Definition: uavcan_main.hpp:93
static int getHardwareVersion(uavcan::protocol::HardwareVersion &hwver)
Defines basic functinality of UAVCAN node.
UAVCAN_DRIVER::CanInitHelper< RxQueueLenPerIface > CanInitHelper
void lock()
Take the driver lock.
Definition: CDev.hpp:264
pthread_mutex_t _node_mutex
Definition: I2C.hpp:51
UavcanMixingInterface _mixing_interface
virtual int init()
Definition: CDev.cpp:119
int list_params(int remote_node_id)
void cb_restart(const uavcan::ServiceCallResult< uavcan::protocol::RestartNode > &result)
void setMaxTopicUpdateRate(unsigned max_topic_update_interval_us)
bool guessIfAllDynamicNodesAreAllocated()
bool _callback_success
#define PWM_SERVO_CLEAR_ARM_OK
clear the &#39;ARM ok&#39; bit, which deactivates the safety switch
static int stop()
static void print_usage()
#define UAVCAN_IOCS_HARDPOINT_SET
virtual void injectTxFramesInto(uavcan::INode &main_node)=0
Flush contents of TX queues into the main node.
unsigned _output_count
number of actuators currently available
LidarLite * instance
Definition: ll40ls.cpp:65
void attachITxQueueInjector(ITxQueueInjector **injector)
uavcan::MethodBinder< UavcanNode *, void(UavcanNode::*)(const uavcan::ServiceCallResult< uavcan::protocol::RestartNode > &)> RestartNodeCallback
void print_info()
void mixerChanged() override
called whenever the mixer gets updated/reset
int set_param(int remote_node_id, const char *name, char *value)
High-resolution timer with callouts and timekeeping.
int request_fw_check()
Global flash based parameter store.
int fw_server(eServerAction action)
px4::atomic_bool _task_should_exit
flag to indicate to tear down the CAN driver
Mixer ioctl interfaces.
__EXPORT int uavcan_main(int argc, char *argv[])
void handle_time_sync(const uavcan::TimerEvent &)
UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock)
Definition: uavcan_main.cpp:78
void perf_count(perf_counter_t handle)
Count a performance event.
A UAVCAN node.
UavcanEscController _esc_controller
void set_setget_response(uavcan::protocol::param::GetSet::Response *resp)
void perf_free(perf_counter_t handle)
Free a counter.
void init()
Activates/configures the hardware registers.
void cb_opcode(const uavcan::ServiceCallResult< uavcan::protocol::param::ExecuteOpcode > &result)
static constexpr unsigned ScheduleIntervalMs
ITxQueueInjector * _tx_injector
uavcan::protocol::param::GetSet::Response * _setget_response
#define perf_alloc(a, b)
Definition: px4io.h:59
bool _idle_throttle_when_armed
int _fw_server_status
static constexpr unsigned MAX_RATE_HZ
XXX make this configurable.
Definition: esc.hpp:61
static int max_output_value()
Definition: esc.hpp:76
bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) override
Callback to update the (physical) actuator outputs in the driver.
static struct actuator_armed_s armed
Definition: Commander.cpp:139
int get_param(int remote_node_id, const char *name)
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
int32_t _idle_throttle_when_armed_param
void resetMixerThreadSafe()
Reset (unload) the complete mixer, called from another thread.
A UAVCAN Server Sub node.
Simple error/warning functions, heavily inspired by the BSD functions of the same names...
#define PWM_SERVO_SET_FORCE_SAFETY_OFF
force safety switch off (to disable use of safety switch)
void requestCheckAllNodesFirmwareAndUpdate()
perf_counter_t _interval_perf
px4::atomic< int > _fw_server_action
uavcan::MethodBinder< UavcanNode *, void(UavcanNode::*)(const uavcan::ServiceCallResult< uavcan::protocol::param::ExecuteOpcode > &)> ExecuteOpcodeCallback
void perf_end(perf_counter_t handle)
End a performance event.
static int start(uavcan::INode &main_node)
bool updated()
Check if there is a new update.
int stop_fw_server()
int loadMixerThreadSafe(const char *buf, unsigned len)
Load (append) a new mixer from a buffer, called from another thread.
int save_params(int remote_node_id)
static __BEGIN_DECLS const char * px4_board_name(void)
get the board name as string (including the version if there are multiple)
Definition: version.h:54
constexpr _Tp min(_Tp a, _Tp b)
Definition: Limits.hpp:54
const char * px4_firmware_version_string(void)
Firmware version as human readable string (git tag)
Definition: version.c:338
__EXPORT param_t param_find(const char *name)
Look up a parameter by name.
Definition: parameters.cpp:370
#define MIXERIOCRESET
reset (clear) the mixer configuration
Definition: drv_mixer.h:71
Tools for system version detection.
void setAllMaxValues(uint16_t value)
const char * name
Definition: tests_main.c:58
List< IUavcanSensorBridge * > _sensor_bridges
List of active sensor bridges.
static UavcanNode * _instance
singleton pointer
px4_sem_t _server_command_sem
uavcan::NodeStatusMonitor _node_status_monitor
constexpr _Tp max(_Tp a, _Tp b)
Definition: Limits.hpp:60
void free_setget_response(void)
static UavcanNode * instance()
int teardown()
static constexpr uint16_t DISARMED_OUTPUT_VALUE
Definition: esc.hpp:62
struct @83::@85::@87 file
uORB::Subscription _parameter_update_sub
void perf_print_counter(perf_counter_t handle)
Print one performance counter to stdout.
void setAllDisarmedValues(uint16_t value)
uavcan::GlobalTimeSyncSlave _time_sync_slave
#define errx(eval,...)
Definition: err.h:89
Definition: bst.cpp:62
void set_rotor_count(uint8_t count)
Sets the number of rotors.
Definition: esc.hpp:74
void cb_setget(const uavcan::ServiceCallResult< uavcan::protocol::param::GetSet > &result)
void update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs)
Definition: esc.cpp:80
void Run() override
uavcan::MethodBinder< UavcanNode *, void(UavcanNode::*)(const uavcan::TimerEvent &)> TimerCallback
void hardpoint_controller_set(uint8_t hardpoint_id, uint16_t command)
#define OK
Definition: uavcan_main.cpp:71
int init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events)
void unregister()
unregister uORB subscription callbacks
virtual int ioctl(file *filp, int cmd, unsigned long arg)
uavcan::GlobalTimeSyncMaster _time_sync_master
void shrink()
void node_spin_once()
void fill_node_info()
void unlock()
Release the driver lock.
Definition: CDev.hpp:269
#define UAVCAN_IOCG_NODEID_INPROGRESS
bool copy(void *dst)
Copy the struct.
static UavcanServers * instance()
uavcan_node::Allocator _pool_allocator
void perf_begin(perf_counter_t handle)
Begin a performance event.
void enable_idle_throttle_when_armed(bool value)
measure the interval between instances of an event
Definition: perf_counter.h:57
perf_counter_t _cycle_perf
uavcan::Node & get_node()
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
void adjustUtc(uavcan::UtcDuration adjustment)
Performs UTC phase and frequency adjustment.
UavcanHardpointController _hardpoint_controller
void set_command(uint8_t hardpoint_id, uint16_t command)
Definition: hardpoint.cpp:67
int reset_node(int remote_node_id)
void setAllMinValues(uint16_t value)