45 #include <px4_platform_common/px4_config.h> 46 #include <px4_platform_common/tasks.h> 54 #include <arch/board/board.h> 55 #include <arch/chip/chip.h> 65 #include <uavcan/util/templates.hpp> 67 #include <uavcan/protocol/param/ExecuteOpcode.hpp> 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),
92 int res = pthread_mutex_init(&
_node_mutex,
nullptr);
155 board_get_mfguid(mfgid);
156 uavcan::copy(mfgid, mfgid +
sizeof(mfgid), hwver.unique_id.begin());
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>());
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>()));
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>());
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());
189 uavcan::protocol::param::ExecuteOpcode::Response resp;
191 resp = result.getResponse();
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);
203 int call_res = client.call(remote_node_id, opcode_req);
206 while (client.hasPendingCalls()) {
212 std::printf(
"Failed to save parameters: %d\n", call_res);
222 uavcan::protocol::RestartNode::Response resp;
224 resp = result.getResponse();
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);
236 int call_res = client.call(remote_node_id, restart_req);
239 while (client.hasPendingCalls()) {
245 std::printf(
"Failed to reset node: %d\n", remote_node_id);
257 uavcan::protocol::param::GetSet::Response resp;
261 uavcan::protocol::param::GetSet::Request req;
267 std::printf(
"Failed to get param: %d\n", call_res);
272 if (resp.name.empty()) {
293 if (name !=
nullptr) {
297 uavcan::ServiceClient<uavcan::protocol::param::GetSet, GetSetCallback> client(
_node);
300 int call_res = client.call(remote_node_id, req);
304 while (client.hasPendingCalls()) {
319 uavcan::protocol::param::GetSet::Request req;
320 uavcan::protocol::param::GetSet::Response resp;
324 if (rv < 0 || resp.name.empty()) {
325 std::printf(
"Failed to retrieve param: %s\n", name);
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>();
338 if (i >= min && i <= max) {
339 req.value.to<uavcan::protocol::param::Value::Tag::integer_value>() = i;
342 std::printf(
"Invalid value for: %s must be between %lld and %lld\n", name, min, max);
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>();
351 if (f >= min && f <= max) {
352 req.value.to<uavcan::protocol::param::Value::Tag::real_value>() = f;
355 std::printf(
"Invalid value for: %s must be between %.4f and %.4f\n", name, static_cast<double>(min),
356 static_cast<double>(max));
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;
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;
371 if (rv < 0 || resp.name.empty()) {
372 std::printf(
"Failed to set param: %s\n", name);
387 uavcan::protocol::param::GetSet::Request req;
388 uavcan::protocol::param::GetSet::Response resp;
392 if (rv < 0 || resp.name.empty()) {
393 std::printf(
"Failed to get param: %s\n", name);
418 if (_servers ==
nullptr) {
445 if (_servers !=
nullptr) {
462 if (_servers !=
nullptr) {
471 rv = _servers->
stop();
508 PX4_WARN(
"Already started");
519 if (can ==
nullptr) {
523 if (can ==
nullptr) {
524 PX4_ERR(
"Out of memory");
528 const int can_init_res = can->init(bitrate);
530 if (can_init_res < 0) {
531 PX4_ERR(
"CAN driver init failed %i", can_init_res);
542 PX4_ERR(
"Out of memory");
546 const int node_init_res =
_instance->
init(node_id, can->driver.updateEvent());
548 if (node_init_res < 0) {
551 PX4_ERR(
"Node init failed %i", node_init_res);
552 return node_init_res;
564 uavcan::protocol::SoftwareVersion swver;
567 char fw_git_short[9] = {};
570 swver.vcs_commit = std::strtol(fw_git_short, &end, 16);
571 swver.optional_field_flags |= swver.OPTIONAL_FIELD_FLAG_VCS_COMMIT;
576 _node.setSoftwareVersion(swver);
579 uavcan::protocol::HardwareVersion hwver;
581 _node.setHardwareVersion(hwver);
605 _node.setName(
"org.pixhawk.pixhawk");
607 _node.setNodeID(node_id);
631 PX4_ERR(
"cannot init sensor bridge '%s' (%d)", br->get_name(), ret);
635 PX4_INFO(
"sensor bridge '%s' init ok", br->get_name());
644 PX4_ERR(
"ESC max output value assertion failed");
655 return _node.start();
661 const int spin_res =
_node.spinOnce();
664 PX4_ERR(
"node spin error %i", spin_res);
724 if (slave_init_res < 0) {
725 PX4_ERR(
"Failed to start time_sync_slave");
737 _master_timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000));
740 _node.setModeOperational();
837 const char *buf = (
const char *)arg;
838 unsigned buflen = strlen(buf);
845 const auto &hp_cmd = *
reinterpret_cast<uavcan::equipment::hardpoint::Command *
>(arg);
853 if (_servers ==
nullptr) {
877 if (ret == -ENOTTY) {
878 ret = CDev::ioctl(filp, cmd, arg);
886 unsigned num_control_groups_updated)
895 _mixing_output.update();
896 _mixing_output.updateSubscriptions(
false);
904 if (_mixing_output.mixers()) {
905 rotor_count = _mixing_output.mixers()->get_multirotor_count();
917 printf(
"Pool allocator status:\n");
918 printf(
"\tCapacity hard/soft: %u/%u blocks\n",
920 printf(
"\tReserved: %u blocks\n",
_pool_allocator.getNumReservedBlocks());
921 printf(
"\tAllocated: %u blocks\n",
_pool_allocator.getNumAllocatedBlocks());
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());
935 for (
unsigned i = 0; i <
_node.getDispatcher().getCanIOManager().getCanDriver().getNumIfaces(); i++) {
936 printf(
"CAN%u status:\n",
unsigned(i + 1));
938 auto iface =
_node.getDispatcher().getCanIOManager().getCanDriver().getIface(i);
939 printf(
"\tHW errors: %llu\n", iface->getErrorCount());
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);
956 printf(
"Sensor '%s':\n", br->get_name());
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]);
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>}");
1013 bool fw = argc > 2 && !std::strcmp(argv[2],
"fw");
1015 if (!std::strcmp(argv[1],
"start")) {
1021 PX4_ERR(
"Firmware Server Failed to Start %d", rv);
1029 PX4_INFO(
"already started");
1034 int32_t node_id = 1;
1037 if (node_id < 0 || node_id > uavcan::NodeID::Max || !uavcan::NodeID(node_id).isUnicast()) {
1038 PX4_ERR(
"Invalid Node ID %i", node_id);
1043 int32_t bitrate = 1000000;
1047 PX4_INFO(
"Node ID %u, bitrate %u", node_id, bitrate);
1055 errx(1,
"application not running");
1058 if (fw && !std::strcmp(argv[1],
"update")) {
1060 errx(1,
"firmware server is not running");
1067 if (fw && (!std::strcmp(argv[1],
"status") || !std::strcmp(argv[1],
"info"))) {
1072 if (!std::strcmp(argv[1],
"status") || !std::strcmp(argv[1],
"info")) {
1077 if (!std::strcmp(argv[1],
"shrink")) {
1091 int node_arg = !std::strcmp(argv[1],
"reset") ? 2 : 3;
1093 if (!std::strcmp(argv[1],
"param") || node_arg == 2) {
1094 if (argc < node_arg + 1) {
1095 errx(1,
"Node id required");
1098 int nodeid = atoi(argv[node_arg]);
1100 if (nodeid == 0 || nodeid > 127 || nodeid == inst->
get_node().getNodeID().get()) {
1101 errx(1,
"Invalid Node id");
1104 if (node_arg == 2) {
1108 }
else if (!std::strcmp(argv[2],
"list")) {
1112 }
else if (!std::strcmp(argv[2],
"save")) {
1116 }
else if (!std::strcmp(argv[2],
"get")) {
1118 errx(1,
"Name required");
1121 return inst->
get_param(nodeid, argv[4]);
1123 }
else if (!std::strcmp(argv[2],
"set")) {
1125 errx(1,
"Name required");
1129 errx(1,
"Value required");
1132 return inst->
set_param(nodeid, argv[4], argv[5]);
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]);
1142 if (hardpoint_id >= 0 && hardpoint_id < 256 &&
1143 command >= 0 && command < 65536) {
1147 errx(1,
"Invalid argument");
1151 errx(1,
"Invalid hardpoint command");
1157 if (!std::strcmp(argv[1],
"stop")) {
1167 PX4_ERR(
"Firmware Server Failed to Stop %d", rv);
#define MIXERIOCLOADBUF
Add mixer(s) from the buffer in (const char *)arg.
static int start(uavcan::NodeID node_id, uint32_t bitrate)
#define PWM_SERVO_SET_ARM_OK
set the 'ARM ok' bit, which activates the safety switch
const actuator_armed_s & armed() const
uavcan::TimerEventForwarder< TimerCallback > _master_timer
static void make_all(uavcan::INode &node, List< IUavcanSensorBridge *> &list)
Sensor bridge factory.
#define UAVCAN_DEVICE_PATH
measure the time elapsed performing an event
uavcan::Node _node
library instance
__EXPORT int param_get(param_t param, void *val)
Copy the value of a parameter.
uavcan::MethodBinder< UavcanNode *, void(UavcanNode::*)(const uavcan::ServiceCallResult< uavcan::protocol::param::GetSet > &)> GetSetCallback
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()
static int getHardwareVersion(uavcan::protocol::HardwareVersion &hwver)
Defines basic functinality of UAVCAN node.
UAVCAN_DRIVER::CanInitHelper< RxQueueLenPerIface > CanInitHelper
void lock()
Take the driver lock.
pthread_mutex_t _node_mutex
UavcanMixingInterface _mixing_interface
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()
#define PWM_SERVO_CLEAR_ARM_OK
clear the 'ARM ok' bit, which deactivates the safety switch
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
void attachITxQueueInjector(ITxQueueInjector **injector)
uavcan::MethodBinder< UavcanNode *, void(UavcanNode::*)(const uavcan::ServiceCallResult< uavcan::protocol::RestartNode > &)> RestartNodeCallback
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.
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
__EXPORT int uavcan_main(int argc, char *argv[])
void handle_time_sync(const uavcan::TimerEvent &)
UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock)
void perf_count(perf_counter_t handle)
Count a performance event.
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
bool _idle_throttle_when_armed
static constexpr unsigned MAX_RATE_HZ
XXX make this configurable.
static int max_output_value()
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
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 > &)
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 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)
constexpr _Tp min(_Tp a, _Tp b)
const char * px4_firmware_version_string(void)
Firmware version as human readable string (git tag)
__EXPORT param_t param_find(const char *name)
Look up a parameter by name.
#define MIXERIOCRESET
reset (clear) the mixer configuration
Tools for system version detection.
void setAllMaxValues(uint16_t value)
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)
void free_setget_response(void)
static UavcanNode * instance()
static constexpr uint16_t DISARMED_OUTPUT_VALUE
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
void set_rotor_count(uint8_t count)
Sets the number of rotors.
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)
uavcan::MethodBinder< UavcanNode *, void(UavcanNode::*)(const uavcan::TimerEvent &)> TimerCallback
void hardpoint_controller_set(uint8_t hardpoint_id, uint16_t command)
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 unlock()
Release the driver lock.
#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
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)
int reset_node(int remote_node_id)
void setAllMinValues(uint16_t value)