diff --git a/src/drivers/uavcan/CMakeLists.txt b/src/drivers/uavcan/CMakeLists.txt index f11022d232..8915fe8932 100644 --- a/src/drivers/uavcan/CMakeLists.txt +++ b/src/drivers/uavcan/CMakeLists.txt @@ -160,6 +160,9 @@ px4_add_module( sensors/cbat.cpp sensors/ice_status.cpp + MODULE_CONFIG + module.yaml + DEPENDS px4_uavcan_dsdlc diff --git a/src/drivers/uavcan/actuators/esc.cpp b/src/drivers/uavcan/actuators/esc.cpp index 541d2a4fc7..e4ee07b58f 100644 --- a/src/drivers/uavcan/actuators/esc.cpp +++ b/src/drivers/uavcan/actuators/esc.cpp @@ -79,14 +79,6 @@ UavcanEscController::init() void UavcanEscController::update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs) { - if (num_outputs > uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize) { - num_outputs = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize; - } - - if (num_outputs > esc_status_s::CONNECTED_ESC_MAX) { - num_outputs = esc_status_s::CONNECTED_ESC_MAX; - } - /* * Rate limiting - we don't want to congest the bus */ diff --git a/src/drivers/uavcan/actuators/esc.hpp b/src/drivers/uavcan/actuators/esc.hpp index f59fb20b99..ac40bf989c 100644 --- a/src/drivers/uavcan/actuators/esc.hpp +++ b/src/drivers/uavcan/actuators/esc.hpp @@ -57,10 +57,13 @@ class UavcanEscController { public: - static constexpr int MAX_ACTUATORS = MixingOutput::MAX_ACTUATORS; + static constexpr int MAX_ACTUATORS = esc_status_s::CONNECTED_ESC_MAX; static constexpr unsigned MAX_RATE_HZ = 200; ///< XXX make this configurable static constexpr uint16_t DISARMED_OUTPUT_VALUE = UINT16_MAX; + static_assert(uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize >= MAX_ACTUATORS, "Too many actuators"); + + UavcanEscController(uavcan::INode &node); ~UavcanEscController(); diff --git a/src/drivers/uavcan/module.yaml b/src/drivers/uavcan/module.yaml new file mode 100644 index 0000000000..fd12f0db56 --- /dev/null +++ b/src/drivers/uavcan/module.yaml @@ -0,0 +1,12 @@ + +module_name: UAVCAN +actuator_output: + output_groups: + - param_prefix: UAVCAN_EC + channel_label: 'UAVCAN ESC' + standard_params: + min: { min: 0, max: 8191, default: 1 } + max: { min: 0, max: 8191, default: 8191 } + failsafe: { min: 0, max: 8191 } + num_channels: 8 + diff --git a/src/drivers/uavcan/uavcan_main.cpp b/src/drivers/uavcan/uavcan_main.cpp index 850e25724a..14be0f46b7 100644 --- a/src/drivers/uavcan/uavcan_main.cpp +++ b/src/drivers/uavcan/uavcan_main.cpp @@ -561,6 +561,7 @@ UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) } _instance->ScheduleOnInterval(ScheduleIntervalMs * 1000); + _instance->_mixing_interface.ScheduleNow(); return OK; } @@ -661,9 +662,6 @@ UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events) PX4_DEBUG("sensor bridge '%s' init ok", br->get_name()); } - _mixing_interface.mixingOutput().setAllDisarmedValues(UavcanEscController::DISARMED_OUTPUT_VALUE); - _mixing_interface.mixingOutput().setAllMinValues(0); // Can be changed to 1 later, according to UAVCAN_ESC_IDLT - // Ensure we don't exceed maximum limits and assumptions. FIXME: these should be static assertions if (UavcanEscController::max_output_value() >= UavcanEscController::DISARMED_OUTPUT_VALUE || UavcanEscController::max_output_value() > (int)UINT16_MAX) { @@ -671,11 +669,19 @@ UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events) return -EINVAL; } - _mixing_interface.mixingOutput().setAllMaxValues(UavcanEscController::max_output_value()); + _mixing_interface.mixingOutput().setAllDisarmedValues(UavcanEscController::DISARMED_OUTPUT_VALUE); + + if (!_mixing_interface.mixingOutput().useDynamicMixing()) { + // these are configurable with dynamic mixing + _mixing_interface.mixingOutput().setAllMinValues(0); // Can be changed to 1 later, according to UAVCAN_ESC_IDLT + _mixing_interface.mixingOutput().setAllMaxValues(UavcanEscController::max_output_value()); + + param_get(param_find("UAVCAN_ESC_IDLT"), &_idle_throttle_when_armed_param); + enable_idle_throttle_when_armed(true); + } + _mixing_interface.mixingOutput().setMaxTopicUpdateRate(1000000 / UavcanEscController::MAX_RATE_HZ); - param_get(param_find("UAVCAN_ESC_IDLT"), &_idle_throttle_when_armed_param); - enable_idle_throttle_when_armed(true); /* Start the Node */ return _node.start(); @@ -831,9 +837,11 @@ UavcanNode::enable_idle_throttle_when_armed(bool value) { value &= _idle_throttle_when_armed_param > 0; - if (value != _idle_throttle_when_armed) { - _mixing_interface.mixingOutput().setAllMinValues(value ? 1 : 0); - _idle_throttle_when_armed = value; + if (!_mixing_interface.mixingOutput().useDynamicMixing()) { + if (value != _idle_throttle_when_armed) { + _mixing_interface.mixingOutput().setAllMinValues(value ? 1 : 0); + _idle_throttle_when_armed = value; + } } } @@ -931,8 +939,15 @@ void UavcanMixingInterface::mixerChanged() { int rotor_count = 0; - if (_mixing_output.mixers()) { - rotor_count = _mixing_output.mixers()->get_multirotor_count(); + if (_mixing_output.useDynamicMixing()) { + for (unsigned i = 0; i < MAX_ACTUATORS; ++i) { + rotor_count += _mixing_output.isFunctionSet(i); + } + + } else { + if (_mixing_output.mixers()) { + rotor_count = _mixing_output.mixers()->get_multirotor_count(); + } } _esc_controller.set_rotor_count(rotor_count); diff --git a/src/drivers/uavcan/uavcan_main.hpp b/src/drivers/uavcan/uavcan_main.hpp index 8ea0e816f9..3da8024afc 100644 --- a/src/drivers/uavcan/uavcan_main.hpp +++ b/src/drivers/uavcan/uavcan_main.hpp @@ -104,7 +104,7 @@ private: friend class UavcanNode; pthread_mutex_t &_node_mutex; UavcanEscController &_esc_controller; - MixingOutput _mixing_output{"UAVCAN_EC", MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}; + MixingOutput _mixing_output{"UAVCAN_EC", UavcanEscController::MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}; }; /**