From fd76e5488e85ba291a544b7749dec482fdba7f0d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Sat, 11 Sep 2021 18:11:27 +0200 Subject: [PATCH] mixer_module: add output functions --- src/lib/mixer_module/CMakeLists.txt | 5 +- src/lib/mixer_module/functions.cpp | 86 +++++ src/lib/mixer_module/functions.hpp | 146 ++++++++ src/lib/mixer_module/mixer_module.cpp | 399 +++++++++++++++++++++- src/lib/mixer_module/mixer_module.hpp | 61 +++- src/lib/mixer_module/output_functions.hpp | 65 ++++ 6 files changed, 740 insertions(+), 22 deletions(-) create mode 100644 src/lib/mixer_module/functions.cpp create mode 100644 src/lib/mixer_module/functions.hpp create mode 100644 src/lib/mixer_module/output_functions.hpp diff --git a/src/lib/mixer_module/CMakeLists.txt b/src/lib/mixer_module/CMakeLists.txt index a322b8513d..bc65a51536 100644 --- a/src/lib/mixer_module/CMakeLists.txt +++ b/src/lib/mixer_module/CMakeLists.txt @@ -31,6 +31,9 @@ # ############################################################################ -px4_add_library(mixer_module mixer_module.cpp) +px4_add_library(mixer_module + mixer_module.cpp + functions.cpp + ) target_link_libraries(mixer_module PRIVATE output_limit) target_compile_options(mixer_module PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) diff --git a/src/lib/mixer_module/functions.cpp b/src/lib/mixer_module/functions.cpp new file mode 100644 index 0000000000..6e83289b02 --- /dev/null +++ b/src/lib/mixer_module/functions.cpp @@ -0,0 +1,86 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "functions.hpp" + +FunctionMotors::FunctionMotors(const Context &context) + : _topic(&context.work_item, ORB_ID(actuator_motors)), + _reversible_motors(context.reversible_motors) +{ + for (int i = 0; i < actuator_motors_s::NUM_CONTROLS; ++i) { + _data.control[i] = NAN; + } +} +void FunctionMotors::update() +{ + bool updated = _topic.update(&_data); + + if (updated && !_reversible_motors) { + for (int i = 0; i < actuator_motors_s::NUM_CONTROLS; ++i) { + if (_data.control[i] < -FLT_EPSILON) { + _data.control[i] = NAN; + + } else { + // remap from [0, 1] to [-1, 1] + _data.control[i] = _data.control[i] * 2.f - 1.f; + } + } + } +} + +FunctionActuatorSet::FunctionActuatorSet() +{ + for (int i = 0; i < max_num_actuators; ++i) { + _data[i] = NAN; + } +} + +void FunctionActuatorSet::update() +{ + vehicle_command_s vehicle_command; + + while (_topic.update(&vehicle_command)) { + if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_SET_ACTUATOR) { + int index = (int)(vehicle_command.param7 + 0.5f); + + if (index == 0) { + _data[0] = vehicle_command.param1; + _data[1] = vehicle_command.param2; + _data[2] = vehicle_command.param3; + _data[3] = vehicle_command.param4; + _data[4] = vehicle_command.param5; + _data[5] = vehicle_command.param6; + } + } + } +} diff --git a/src/lib/mixer_module/functions.hpp b/src/lib/mixer_module/functions.hpp new file mode 100644 index 0000000000..7708fdf555 --- /dev/null +++ b/src/lib/mixer_module/functions.hpp @@ -0,0 +1,146 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include + +#include "output_functions.hpp" + +#include +#include +#include +#include +#include +#include + +class FunctionProviderBase +{ +public: + struct Context { + px4::WorkItem &work_item; + bool reversible_motors; + }; + + FunctionProviderBase() = default; + virtual ~FunctionProviderBase() = default; + + virtual void update() = 0; + + /** + * Get the current output value for a given function + * @return NAN (=disarmed) or value in range [-1, 1] + */ + virtual float value(OutputFunction func) = 0; + + virtual float defaultFailsafeValue(OutputFunction func) const { return NAN; } + virtual bool allowPrearmControl() const { return true; } + + virtual uORB::SubscriptionCallbackWorkItem *subscriptionCallback() { return nullptr; } + + virtual bool getLatestSampleTimestamp(hrt_abstime &t) const { return false; } +}; + +/** + * Functions: ConstantMin + */ +class FunctionConstantMin : public FunctionProviderBase +{ +public: + static FunctionProviderBase *allocate(const Context &context) { return new FunctionConstantMin(); } + + float value(OutputFunction func) override { return -1.f; } + void update() override { } + + float defaultFailsafeValue(OutputFunction func) const override { return -1.f; } +}; + +/** + * Functions: ConstantMax + */ +class FunctionConstantMax : public FunctionProviderBase +{ +public: + static FunctionProviderBase *allocate(const Context &context) { return new FunctionConstantMax(); } + + float value(OutputFunction func) override { return 1.f; } + void update() override { } + + float defaultFailsafeValue(OutputFunction func) const override { return 1.f; } +}; + +/** + * Functions: Motor1 ... MotorMax + */ +class FunctionMotors : public FunctionProviderBase +{ +public: + static_assert(actuator_motors_s::NUM_CONTROLS == (int)OutputFunction::MotorMax - (int)OutputFunction::Motor1 + 1, + "Unexpected num motors"); + + FunctionMotors(const Context &context); + static FunctionProviderBase *allocate(const Context &context) { return new FunctionMotors(context); } + + void update() override; + float value(OutputFunction func) override { return _data.control[(int)func - (int)OutputFunction::Motor1]; } + + bool allowPrearmControl() const override { return false; } + + uORB::SubscriptionCallbackWorkItem *subscriptionCallback() override { return &_topic; } + + bool getLatestSampleTimestamp(hrt_abstime &t) const override { t = _data.timestamp_sample; return t != 0; } +private: + uORB::SubscriptionCallbackWorkItem _topic; + actuator_motors_s _data{}; + const bool _reversible_motors; +}; + +/** + * Functions: ActuatorSet1 ... ActuatorSet6 + */ +class FunctionActuatorSet : public FunctionProviderBase +{ +public: + FunctionActuatorSet(); + static FunctionProviderBase *allocate(const Context &context) { return new FunctionActuatorSet(); } + + void update() override; + float value(OutputFunction func) override { return _data[(int)func - (int)OutputFunction::ActuatorSet1]; } + +private: + static constexpr int max_num_actuators = 6; + + uORB::Subscription _topic{ORB_ID(vehicle_command)}; + float _data[max_num_actuators]; +}; + diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index 31639d80bc..cac36a5546 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -41,6 +41,26 @@ using namespace time_literals; +struct FunctionProvider { + using Constructor = FunctionProviderBase * (*)(const FunctionProviderBase::Context &context); + FunctionProvider(OutputFunction min_func_, OutputFunction max_func_, Constructor constructor_) + : min_func(min_func_), max_func(max_func_), constructor(constructor_) {} + FunctionProvider(OutputFunction func, Constructor constructor_) + : min_func(func), max_func(func), constructor(constructor_) {} + + OutputFunction min_func; + OutputFunction max_func; + Constructor constructor; +}; + +static const FunctionProvider all_function_providers[] = { + // Providers higher up take precedence for subscription callback in case there are multiple + {OutputFunction::ConstantMin, &FunctionConstantMin::allocate}, + {OutputFunction::ConstantMax, &FunctionConstantMax::allocate}, + {OutputFunction::Motor1, OutputFunction::MotorMax, &FunctionMotors::allocate}, + {OutputFunction::ActuatorSet1, OutputFunction::ActuatorSet6, &FunctionActuatorSet::allocate}, +}; + MixingOutput::MixingOutput(uint8_t max_num_outputs, OutputModuleInterface &interface, SchedulingPolicy scheduling_policy, bool support_esc_calibration, bool ramp_up) @@ -75,6 +95,8 @@ _control_latency_perf(perf_alloc(PC_ELAPSED, "control latency")) uORB::Publication test_motor_pub{ORB_ID(test_motor)}; test_motor_pub.publish(test); _motor_test.test_motor_sub.subscribe(); + + _use_dynamic_mixing = _param_sys_ctrl_alloc.get(); } MixingOutput::~MixingOutput() @@ -82,6 +104,8 @@ MixingOutput::~MixingOutput() perf_free(_control_latency_perf); delete _mixers; px4_sem_destroy(&_lock); + + cleanupFunctions(); } void MixingOutput::printStatus() const @@ -92,15 +116,29 @@ void MixingOutput::printStatus() const PX4_INFO("Switched to rate_ctrl work queue"); } - PX4_INFO("Mixer loaded: %s", _mixers ? "yes" : "no"); - PX4_INFO("Driver instance: %i", _driver_instance); - PX4_INFO("Channel Configuration:"); + if (!_use_dynamic_mixing) { + PX4_INFO("Mixer loaded: %s", _mixers ? "yes" : "no"); + PX4_INFO("Driver instance: %i", _driver_instance); + } + + PX4_INFO_RAW("Channel Configuration:\n"); + + if (_use_dynamic_mixing) { + for (unsigned i = 0; i < _max_num_outputs; i++) { + PX4_INFO_RAW("Channel %i: func: %i, value: %i, failsafe: %d, disarmed: %d, min: %d, max: %d\n", i, + (int)_function_assignment[i], _current_output_value[i], + _failsafe_value[i], _disarmed_value[i], _min_value[i], _max_value[i]); + } + + } else { + for (unsigned i = 0; i < _max_num_outputs; i++) { + int reordered_i = reorderedMotorIndex(i); + PX4_INFO_RAW("Channel %i: value: %i, failsafe: %d, disarmed: %d, min: %d, max: %d\n", reordered_i, + _current_output_value[i], + _failsafe_value[reordered_i], _disarmed_value[reordered_i], _min_value[reordered_i], _max_value[reordered_i]); + } - for (unsigned i = 0; i < _max_num_outputs; i++) { - int reordered_i = reorderedMotorIndex(i); - PX4_INFO("Channel %i: value: %i, failsafe: %d, disarmed: %d, min: %d, max: %d", reordered_i, _current_output_value[i], - _failsafe_value[reordered_i], _disarmed_value[reordered_i], _min_value[reordered_i], _max_value[reordered_i]); } } @@ -117,9 +155,66 @@ void MixingOutput::updateParams() _mixers->set_thrust_factor(_param_thr_mdl_fac.get()); _mixers->set_airmode((Mixer::Airmode)_param_mc_airmode.get()); } + + if (_use_dynamic_mixing) { + _param_mot_ordering.set(0); // not used with dynamic mixing + + bool function_changed = false; + + _reverse_output_mask = 0; + + for (unsigned i = 0; i < _max_num_outputs; i++) { + int32_t val; + + if (_param_handles[i].function != PARAM_INVALID && param_get(_param_handles[i].function, &val) == 0) { + if (val != (int32_t)_function_assignment[i]) { + function_changed = true; + } + + // we set _function_assignment[i] later to ensure _functions[i] is updated at the same time + } + + if (_param_handles[i].disarmed != PARAM_INVALID && param_get(_param_handles[i].disarmed, &val) == 0) { + _disarmed_value[i] = val; + } + + if (_param_handles[i].min != PARAM_INVALID && param_get(_param_handles[i].min, &val) == 0) { + _min_value[i] = val; + } + + if (_param_handles[i].max != PARAM_INVALID && param_get(_param_handles[i].max, &val) == 0) { + _max_value[i] = val; + } + + if (_min_value[i] > _max_value[i]) { + _reverse_output_mask |= 1 << i; + uint16_t tmp = _min_value[i]; + _min_value[i] = _max_value[i]; + _max_value[i] = tmp; + } + + if (_param_handles[i].failsafe != PARAM_INVALID && param_get(_param_handles[i].failsafe, &val) == 0) { + _failsafe_value[i] = val; + } + } + + if (function_changed) { + _need_function_update = true; + } + } } bool MixingOutput::updateSubscriptions(bool allow_wq_switch, bool limit_callbacks_to_primary) +{ + if (_use_dynamic_mixing) { + return updateSubscriptionsDynamicMixer(allow_wq_switch, limit_callbacks_to_primary); + + } else { + return updateSubscriptionsStaticMixer(allow_wq_switch, limit_callbacks_to_primary); + } +} + +bool MixingOutput::updateSubscriptionsStaticMixer(bool allow_wq_switch, bool limit_callbacks_to_primary) { if (_groups_subscribed == _groups_required) { return false; @@ -197,13 +292,159 @@ bool MixingOutput::updateSubscriptions(bool allow_wq_switch, bool limit_callback return true; } + +void MixingOutput::cleanupFunctions() +{ + if (_subscription_callback) { + _subscription_callback->unregisterCallback(); + _subscription_callback = nullptr; + } + + for (int i = 0; i < MAX_ACTUATORS; ++i) { + delete _function_allocated[i]; + _function_allocated[i] = nullptr; + _functions[i] = nullptr; + } +} + +bool MixingOutput::updateSubscriptionsDynamicMixer(bool allow_wq_switch, bool limit_callbacks_to_primary) +{ + if (!_need_function_update || _armed.armed) { + return false; + } + + // must be locked to potentially change WorkQueue + lock(); + + _has_backup_schedule = false; + + if (_scheduling_policy == SchedulingPolicy::Auto) { + // first clear everything + unregister(); + _interface.ScheduleClear(); + + bool switch_requested = false; + + // potentially switch work queue if we run motor outputs + for (unsigned i = 0; i < _max_num_outputs; i++) { + if (_function_assignment[i] >= OutputFunction::Motor1 && _function_assignment[i] <= OutputFunction::MotorMax) { + switch_requested = true; + } + } + + if (allow_wq_switch && !_wq_switched && switch_requested) { + if (_interface.ChangeWorkQeue(px4::wq_configurations::rate_ctrl)) { + // let the new WQ handle the subscribe update + _wq_switched = true; + _interface.ScheduleNow(); + unlock(); + return false; + } + } + } + + // Now update the functions + + cleanupFunctions(); + + const FunctionProviderBase::Context context{_interface, _reversible_motors}; + int provider_indexes[MAX_ACTUATORS] {}; + int next_provider = 0; + int subscription_callback_provider_index = INT_MAX; + bool all_disabled = true; + + for (int i = 0; i < _max_num_outputs; ++i) { + for (int p = 0; p < (int)(sizeof(all_function_providers) / sizeof(all_function_providers[0])); ++p) { + if (_function_assignment[i] >= all_function_providers[p].min_func && + _function_assignment[i] <= all_function_providers[p].max_func) { + all_disabled = false; + int found_index = -1; + + for (int existing = 0; existing < next_provider; ++existing) { + if (provider_indexes[existing] == p) { + found_index = existing; + break; + } + } + + if (found_index >= 0) { + _functions[i] = _function_allocated[found_index]; + + } else { + _function_allocated[next_provider] = all_function_providers[p].constructor(context); + + if (_function_allocated[next_provider]) { + _functions[i] = _function_allocated[next_provider]; + provider_indexes[next_provider++] = p; + + // lowest provider takes precedence for scheduling + if (p < subscription_callback_provider_index && _functions[i]->subscriptionCallback()) { + subscription_callback_provider_index = p; + _subscription_callback = _functions[i]->subscriptionCallback(); + } + + } else { + PX4_ERR("function alloc failed"); + } + } + + break; + } + } + } + + hrt_abstime fixed_rate_scheduling_interval = 4_ms; // schedule at 250Hz + + if (_max_topic_update_interval_us > fixed_rate_scheduling_interval) { + fixed_rate_scheduling_interval = _max_topic_update_interval_us; + } + + if (_scheduling_policy == SchedulingPolicy::Auto) { + if (_subscription_callback) { + if (_subscription_callback->registerCallback()) { + PX4_DEBUG("Scheduling via callback"); + _has_backup_schedule = true; + _interface.ScheduleDelayed(50_ms); + + } else { + PX4_ERR("registerCallback failed, scheduling at fixed rate"); + _interface.ScheduleOnInterval(fixed_rate_scheduling_interval); + } + + } else if (all_disabled) { + _interface.ScheduleOnInterval(300_ms); + PX4_DEBUG("Scheduling at low rate"); + + } else { + _interface.ScheduleOnInterval(fixed_rate_scheduling_interval); + PX4_DEBUG("Scheduling at fixed rate"); + } + } + + setMaxTopicUpdateRate(_max_topic_update_interval_us); + _need_function_update = false; + + unlock(); + + _interface.mixerChanged(); + + return true; +} + void MixingOutput::setMaxTopicUpdateRate(unsigned max_topic_update_interval_us) { _max_topic_update_interval_us = max_topic_update_interval_us; - for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { - if (_groups_subscribed & (1 << i)) { - _control_subs[i].set_interval_us(_max_topic_update_interval_us); + if (_use_dynamic_mixing) { + if (_subscription_callback) { + _subscription_callback->set_interval_us(_max_topic_update_interval_us); + } + + } else { + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (_groups_subscribed & (1 << i)) { + _control_subs[i].set_interval_us(_max_topic_update_interval_us); + } } } } @@ -211,6 +452,7 @@ void MixingOutput::setMaxTopicUpdateRate(unsigned max_topic_update_interval_us) void MixingOutput::setAllMinValues(uint16_t value) { for (unsigned i = 0; i < MAX_ACTUATORS; i++) { + _param_handles[i].min = PARAM_INVALID; _min_value[i] = value; } } @@ -218,6 +460,7 @@ void MixingOutput::setAllMinValues(uint16_t value) void MixingOutput::setAllMaxValues(uint16_t value) { for (unsigned i = 0; i < MAX_ACTUATORS; i++) { + _param_handles[i].max = PARAM_INVALID; _max_value[i] = value; } } @@ -225,6 +468,7 @@ void MixingOutput::setAllMaxValues(uint16_t value) void MixingOutput::setAllFailsafeValues(uint16_t value) { for (unsigned i = 0; i < MAX_ACTUATORS; i++) { + _param_handles[i].failsafe = PARAM_INVALID; _failsafe_value[i] = value; } } @@ -232,6 +476,7 @@ void MixingOutput::setAllFailsafeValues(uint16_t value) void MixingOutput::setAllDisarmedValues(uint16_t value) { for (unsigned i = 0; i < MAX_ACTUATORS; i++) { + _param_handles[i].disarmed = PARAM_INVALID; _disarmed_value[i] = value; } } @@ -241,6 +486,10 @@ void MixingOutput::unregister() for (auto &control_sub : _control_subs) { control_sub.unregisterCallback(); } + + if (_subscription_callback) { + _subscription_callback->unregisterCallback(); + } } void MixingOutput::updateOutputSlewrateMultirotorMixer() @@ -329,6 +578,15 @@ unsigned MixingOutput::motorTest() } bool MixingOutput::update() +{ + if (_use_dynamic_mixing) { + return updateDynamicMixer(); + + } else { + return updateStaticMixer(); + } +} +bool MixingOutput::updateStaticMixer() { if (!_mixers) { handleCommands(); @@ -415,7 +673,7 @@ bool MixingOutput::update() bool stop_motors = mixed_num_outputs == 0 || !_throttle_armed; - /* overwrite outputs in case of lockdown or parachute triggering with disarmed values */ + /* overwrite outputs in case of lockdown with disarmed values */ if (_armed.lockdown || _armed.manual_lockdown) { for (size_t i = 0; i < mixed_num_outputs; i++) { _current_output_value[i] = _disarmed_value[i]; @@ -441,6 +699,99 @@ bool MixingOutput::update() return true; } +bool MixingOutput::updateDynamicMixer() +{ + // check arming state + if (_armed_sub.update(&_armed)) { + _armed.in_esc_calibration_mode &= _support_esc_calibration; + + if (_ignore_lockdown) { + _armed.lockdown = false; + } + + /* Update the armed status and check that we're not locked down. + * We also need to arm throttle for the ESC calibration. */ + _throttle_armed = (_armed.armed && !_armed.lockdown) || _armed.in_esc_calibration_mode; + } + + // only used for sitl with lockstep + bool has_updates = _subscription_callback && _subscription_callback->updated(); + + // update topics + for (int i = 0; i < MAX_ACTUATORS && _function_allocated[i]; ++i) { + _function_allocated[i]->update(); + } + + if (_has_backup_schedule) { + _interface.ScheduleDelayed(50_ms); + } + + // check for motor test (after topic updates) + if (!_armed.armed && !_armed.manual_lockdown) { + // TODO + } + + // get output values + float outputs[MAX_ACTUATORS]; + bool all_disabled = true; + + for (int i = 0; i < _max_num_outputs; ++i) { + if (_functions[i]) { + all_disabled = false; + + if (_armed.armed || (_armed.prearmed && _functions[i]->allowPrearmControl())) { + outputs[i] = _functions[i]->value(_function_assignment[i]); + + } else { + outputs[i] = NAN; + } + + } else { + outputs[i] = NAN; + } + } + + if (!all_disabled) { + limitAndUpdateOutputs(outputs, has_updates); + } + + return true; +} + +void +MixingOutput::limitAndUpdateOutputs(float outputs[MAX_ACTUATORS], bool has_updates) +{ + /* the output limit call takes care of out of band errors, NaN and constrains */ + output_limit_calc(_throttle_armed, armNoThrottle(), _max_num_outputs, _reverse_output_mask, + _disarmed_value, _min_value, _max_value, outputs, _current_output_value, &_output_limit); + + /* overwrite outputs in case of force_failsafe with _failsafe_value values */ + if (_armed.force_failsafe) { + for (size_t i = 0; i < _max_num_outputs; i++) { + _current_output_value[i] = _failsafe_value[i]; + } + } + + bool stop_motors = !_throttle_armed; + + /* overwrite outputs in case of lockdown with disarmed values */ + if (_armed.lockdown || _armed.manual_lockdown) { + for (size_t i = 0; i < _max_num_outputs; i++) { + _current_output_value[i] = _disarmed_value[i]; + } + + stop_motors = true; + } + + /* now return the outputs to the driver */ + if (_interface.updateOutputs(stop_motors, _current_output_value, _max_num_outputs, has_updates)) { + actuator_outputs_s actuator_outputs{}; + setAndPublishActuatorOutputs(_max_num_outputs, actuator_outputs); + + updateLatencyPerfCounter(actuator_outputs); + } +} + void MixingOutput::setAndPublishActuatorOutputs(unsigned num_outputs, actuator_outputs_s &actuator_outputs) { @@ -472,14 +823,26 @@ MixingOutput::publishMixerStatus(const actuator_outputs_s &actuator_outputs) void MixingOutput::updateLatencyPerfCounter(const actuator_outputs_s &actuator_outputs) { - // use first valid timestamp_sample for latency tracking - for (int i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { - const bool required = _groups_required & (1 << i); - const hrt_abstime ×tamp_sample = _controls[i].timestamp_sample; + if (_use_dynamic_mixing) { + // Just check the first function. It means we only get the latency if motors are assigned first, which is the default + if (_function_allocated[0]) { + hrt_abstime timestamp_sample; - if (required && (timestamp_sample > 0)) { - perf_set_elapsed(_control_latency_perf, actuator_outputs.timestamp - timestamp_sample); - break; + if (_function_allocated[0]->getLatestSampleTimestamp(timestamp_sample)) { + perf_set_elapsed(_control_latency_perf, actuator_outputs.timestamp - timestamp_sample); + } + } + + } else { + // use first valid timestamp_sample for latency tracking + for (int i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { + const bool required = _groups_required & (1 << i); + const hrt_abstime ×tamp_sample = _controls[i].timestamp_sample; + + if (required && (timestamp_sample > 0)) { + perf_set_elapsed(_control_latency_perf, actuator_outputs.timestamp - timestamp_sample); + break; + } } } } diff --git a/src/lib/mixer_module/mixer_module.hpp b/src/lib/mixer_module/mixer_module.hpp index 7cddbc79ee..4019eb77e4 100644 --- a/src/lib/mixer_module/mixer_module.hpp +++ b/src/lib/mixer_module/mixer_module.hpp @@ -33,6 +33,8 @@ #pragma once +#include "functions.hpp" + #include #include #include @@ -114,6 +116,18 @@ public: void printStatus() const; + bool useDynamicMixing() const { return _use_dynamic_mixing; } + + /** + * Permanently disable an output function + */ + void disableFunction(int index) { _param_handles[index].function = PARAM_INVALID; _need_function_update = true; } + + /** + * Check if a function is configured, i.e. not set to Disabled and initialized + */ + bool isFunctionSet(int index) const { return !_use_dynamic_mixing || _functions[index] != nullptr; }; + /** * Call this regularly from Run(). It will call interface.updateOutputs(). * @return true if outputs were updated @@ -127,7 +141,7 @@ public: * @param limit_callbacks_to_primary set to only register callbacks for primary actuator controls (if used) * @return true if subscriptions got changed */ - bool updateSubscriptions(bool allow_wq_switch, bool limit_callbacks_to_primary = false); + bool updateSubscriptions(bool allow_wq_switch = false, bool limit_callbacks_to_primary = false); /** * unregister uORB subscription callbacks @@ -155,6 +169,8 @@ public: const actuator_armed_s &armed() const { return _armed; } + bool initialized() const { return _use_dynamic_mixing || _mixers != nullptr; } + MixerGroup *mixers() const { return _mixers; } void setAllFailsafeValues(uint16_t value); @@ -178,12 +194,23 @@ public: void setIgnoreLockdown(bool ignore_lockdown) { _ignore_lockdown = ignore_lockdown; } - void setMaxNumOutputs(uint8_t max_num_outputs) { _max_num_outputs = max_num_outputs; } + /** + * Set the maximum number of outputs. This can only be used to reduce the maximum. + */ + void setMaxNumOutputs(uint8_t max_num_outputs) { if (max_num_outputs < _max_num_outputs) { _max_num_outputs = max_num_outputs; } } + + const char *paramPrefix() const { return _param_prefix; } protected: void updateParams() override; private: + bool updateSubscriptionsStaticMixer(bool allow_wq_switch, bool limit_callbacks_to_primary); + bool updateSubscriptionsDynamicMixer(bool allow_wq_switch, bool limit_callbacks_to_primary); + + bool updateStaticMixer(); + bool updateDynamicMixer(); + void handleCommands(); bool armNoThrottle() const @@ -201,6 +228,20 @@ private: static int controlCallback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input); + void cleanupFunctions(); + + void initParamHandles(); + + void limitAndUpdateOutputs(float outputs[MAX_ACTUATORS], bool has_updates); + + struct ParamHandles { + param_t function{PARAM_INVALID}; + param_t disarmed{PARAM_INVALID}; + param_t min{PARAM_INVALID}; + param_t max{PARAM_INVALID}; + param_t failsafe{PARAM_INVALID}; + }; + enum class MotorOrdering : int32_t { PX4 = 0, Betaflight = 1 @@ -276,11 +317,25 @@ private: perf_counter_t _control_latency_perf; + /* SYS_CTRL_ALLOC == 1 */ + FunctionProviderBase *_function_allocated[MAX_ACTUATORS] {}; ///< unique allocated functions + FunctionProviderBase *_functions[MAX_ACTUATORS] {}; ///< currently assigned functions + OutputFunction _function_assignment[MAX_ACTUATORS] {}; + bool _need_function_update{true}; + bool _use_dynamic_mixing{false}; ///< set to _param_sys_ctrl_alloc on init (avoid changing after startup) + bool _has_backup_schedule{false}; + bool _reversible_motors = + false; ///< whether or not the output module supports reversible motors (range [-1, 0] for motors) + + uORB::SubscriptionCallbackWorkItem *_subscription_callback{nullptr}; ///< current scheduling callback + + DEFINE_PARAMETERS( (ParamInt) _param_mc_airmode, ///< multicopter air-mode (ParamFloat) _param_mot_slew_max, (ParamFloat) _param_thr_mdl_fac, ///< thrust to motor control signal modelling factor - (ParamInt) _param_mot_ordering + (ParamInt) _param_mot_ordering, + (ParamBool) _param_sys_ctrl_alloc ) }; diff --git a/src/lib/mixer_module/output_functions.hpp b/src/lib/mixer_module/output_functions.hpp new file mode 100644 index 0000000000..480f9f911c --- /dev/null +++ b/src/lib/mixer_module/output_functions.hpp @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +// this will be auto-generated... + +#include + +enum class OutputFunction : int32_t { + Disabled = 0, + + ConstantMin = 1, + ConstantMax = 2, + + Motor1 = 101, + Motor2 = 102, + Motor3 = 103, + Motor4 = 104, + Motor5 = 105, + Motor6 = 106, + Motor7 = 107, + Motor8 = 108, + + MotorMax = Motor8, + + + ActuatorSet1 = 300, + ActuatorSet2 = 301, + ActuatorSet3 = 302, + ActuatorSet4 = 303, + ActuatorSet5 = 304, + ActuatorSet6 = 305, + +};