From a7bbcd5b04b13b68fc131debdd17e9e130534a2c Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 24 Aug 2022 16:50:02 -0400 Subject: [PATCH] delete lib/mixer and mixer_module static mixing --- .gitignore | 1 - CMakeLists.txt | 1 - .../actuators/modalai_esc/modalai_esc.cpp | 26 +- .../actuators/modalai_esc/modalai_esc.hpp | 8 +- src/drivers/cyphal/Cyphal.hpp | 2 - src/drivers/dshot/CMakeLists.txt | 1 - src/drivers/dshot/DShot.cpp | 130 ++-- src/drivers/linux_pwm_out/linux_pwm_out.cpp | 190 +----- src/drivers/linux_pwm_out/linux_pwm_out.hpp | 13 +- src/drivers/pca9685_pwm_out/CMakeLists.txt | 1 - src/drivers/pca9685_pwm_out/main.cpp | 226 ------- src/drivers/pwm_out/CMakeLists.txt | 1 - src/drivers/pwm_out/PWMOut.cpp | 555 ++---------------- src/drivers/pwm_out/PWMOut.hpp | 2 - src/drivers/px4io/px4io.cpp | 236 +------- src/drivers/tap_esc/CMakeLists.txt | 1 - src/drivers/tap_esc/TAP_ESC.cpp | 6 +- src/drivers/tap_esc/TAP_ESC.hpp | 5 +- src/drivers/tap_esc/drv_tap_esc.h | 2 - src/drivers/uavcan/CMakeLists.txt | 1 - src/drivers/uavcan/uavcan_main.cpp | 16 - src/lib/CMakeLists.txt | 1 - src/lib/mixer/.gitignore | 2 - src/lib/mixer/CMakeLists.txt | 56 -- src/lib/mixer/HelicopterMixer/CMakeLists.txt | 39 -- .../mixer/HelicopterMixer/HelicopterMixer.cpp | 240 -------- .../mixer/HelicopterMixer/HelicopterMixer.hpp | 111 ---- src/lib/mixer/MixerBase/CMakeLists.txt | 38 -- src/lib/mixer/MixerBase/Mixer.cpp | 124 ---- src/lib/mixer/MixerBase/Mixer.hpp | 279 --------- src/lib/mixer/MixerGroup.cpp | 253 -------- src/lib/mixer/MixerGroup.hpp | 172 ------ src/lib/mixer/MultirotorMixer/CMakeLists.txt | 113 ---- .../mixer/MultirotorMixer/MultirotorMixer.cpp | 498 ---------------- .../mixer/MultirotorMixer/MultirotorMixer.hpp | 255 -------- .../geometries/dodeca_bottom_cox.toml | 40 -- .../geometries/dodeca_top_cox.toml | 40 -- .../MultirotorMixer/geometries/hex_cox.toml | 40 -- .../MultirotorMixer/geometries/hex_plus.toml | 40 -- .../MultirotorMixer/geometries/hex_t.toml | 40 -- .../MultirotorMixer/geometries/hex_x.toml | 40 -- .../MultirotorMixer/geometries/octa_cox.toml | 50 -- .../geometries/octa_cox_wide.toml | 50 -- .../MultirotorMixer/geometries/octa_plus.toml | 50 -- .../MultirotorMixer/geometries/octa_x.toml | 50 -- .../geometries/quad_deadcat.toml | 30 - .../MultirotorMixer/geometries/quad_h.toml | 30 - .../MultirotorMixer/geometries/quad_plus.toml | 29 - .../geometries/quad_vtail.toml | 33 -- .../MultirotorMixer/geometries/quad_wide.toml | 30 - .../MultirotorMixer/geometries/quad_x.toml | 29 - .../MultirotorMixer/geometries/quad_x_cw.toml | 30 - .../geometries/quad_x_pusher.toml | 37 -- .../MultirotorMixer/geometries/quad_y.toml | 31 - .../geometries/tools/px_generate_mixers.py | 398 ------------- .../MultirotorMixer/geometries/tri_y.toml | 23 - .../geometries/twin_engine.toml | 19 - .../mixer/MultirotorMixer/mixer_multirotor.py | 389 ------------ .../MultirotorMixer/test_mixer_multirotor.cpp | 173 ------ src/lib/mixer/NullMixer/CMakeLists.txt | 39 -- src/lib/mixer/NullMixer/NullMixer.cpp | 67 --- src/lib/mixer/NullMixer/NullMixer.hpp | 75 --- src/lib/mixer/SimpleMixer/CMakeLists.txt | 39 -- src/lib/mixer/SimpleMixer/SimpleMixer.cpp | 390 ------------ src/lib/mixer/SimpleMixer/SimpleMixer.hpp | 153 ----- src/lib/mixer/load_mixer_file.cpp | 112 ---- src/lib/mixer/mixer_load.h | 51 -- src/lib/mixer_module/CMakeLists.txt | 3 +- src/lib/mixer_module/mixer_module.cpp | 540 ++--------------- src/lib/mixer_module/mixer_module.hpp | 55 +- src/modules/control_allocator/CMakeLists.txt | 1 - .../mavlink/streams/HIL_ACTUATOR_CONTROLS.hpp | 95 +-- src/modules/mc_att_control/mc_att_control.hpp | 1 - .../mc_att_control/mc_att_control_main.cpp | 2 +- .../simulation/pwm_out_sim/CMakeLists.txt | 1 - src/modules/simulation/pwm_out_sim/PWMSim.cpp | 4 +- .../SimulatorIgnitionBridge.hpp | 2 - .../simulator_mavlink/SimulatorMavlink.cpp | 97 +-- .../simulator_mavlink/SimulatorMavlink.hpp | 1 - 79 files changed, 219 insertions(+), 6835 deletions(-) delete mode 100644 src/lib/mixer/.gitignore delete mode 100644 src/lib/mixer/CMakeLists.txt delete mode 100644 src/lib/mixer/HelicopterMixer/CMakeLists.txt delete mode 100644 src/lib/mixer/HelicopterMixer/HelicopterMixer.cpp delete mode 100644 src/lib/mixer/HelicopterMixer/HelicopterMixer.hpp delete mode 100644 src/lib/mixer/MixerBase/CMakeLists.txt delete mode 100644 src/lib/mixer/MixerBase/Mixer.cpp delete mode 100644 src/lib/mixer/MixerBase/Mixer.hpp delete mode 100644 src/lib/mixer/MixerGroup.cpp delete mode 100644 src/lib/mixer/MixerGroup.hpp delete mode 100644 src/lib/mixer/MultirotorMixer/CMakeLists.txt delete mode 100644 src/lib/mixer/MultirotorMixer/MultirotorMixer.cpp delete mode 100644 src/lib/mixer/MultirotorMixer/MultirotorMixer.hpp delete mode 100644 src/lib/mixer/MultirotorMixer/geometries/dodeca_bottom_cox.toml delete mode 100644 src/lib/mixer/MultirotorMixer/geometries/dodeca_top_cox.toml delete mode 100644 src/lib/mixer/MultirotorMixer/geometries/hex_cox.toml delete mode 100644 src/lib/mixer/MultirotorMixer/geometries/hex_plus.toml delete mode 100644 src/lib/mixer/MultirotorMixer/geometries/hex_t.toml delete mode 100644 src/lib/mixer/MultirotorMixer/geometries/hex_x.toml delete mode 100644 src/lib/mixer/MultirotorMixer/geometries/octa_cox.toml delete mode 100644 src/lib/mixer/MultirotorMixer/geometries/octa_cox_wide.toml delete mode 100644 src/lib/mixer/MultirotorMixer/geometries/octa_plus.toml delete mode 100644 src/lib/mixer/MultirotorMixer/geometries/octa_x.toml delete mode 100644 src/lib/mixer/MultirotorMixer/geometries/quad_deadcat.toml delete mode 100644 src/lib/mixer/MultirotorMixer/geometries/quad_h.toml delete mode 100644 src/lib/mixer/MultirotorMixer/geometries/quad_plus.toml delete mode 100644 src/lib/mixer/MultirotorMixer/geometries/quad_vtail.toml delete mode 100644 src/lib/mixer/MultirotorMixer/geometries/quad_wide.toml delete mode 100644 src/lib/mixer/MultirotorMixer/geometries/quad_x.toml delete mode 100644 src/lib/mixer/MultirotorMixer/geometries/quad_x_cw.toml delete mode 100644 src/lib/mixer/MultirotorMixer/geometries/quad_x_pusher.toml delete mode 100644 src/lib/mixer/MultirotorMixer/geometries/quad_y.toml delete mode 100755 src/lib/mixer/MultirotorMixer/geometries/tools/px_generate_mixers.py delete mode 100644 src/lib/mixer/MultirotorMixer/geometries/tri_y.toml delete mode 100644 src/lib/mixer/MultirotorMixer/geometries/twin_engine.toml delete mode 100755 src/lib/mixer/MultirotorMixer/mixer_multirotor.py delete mode 100644 src/lib/mixer/MultirotorMixer/test_mixer_multirotor.cpp delete mode 100644 src/lib/mixer/NullMixer/CMakeLists.txt delete mode 100644 src/lib/mixer/NullMixer/NullMixer.cpp delete mode 100644 src/lib/mixer/NullMixer/NullMixer.hpp delete mode 100644 src/lib/mixer/SimpleMixer/CMakeLists.txt delete mode 100644 src/lib/mixer/SimpleMixer/SimpleMixer.cpp delete mode 100644 src/lib/mixer/SimpleMixer/SimpleMixer.hpp delete mode 100644 src/lib/mixer/load_mixer_file.cpp delete mode 100644 src/lib/mixer/mixer_load.h diff --git a/.gitignore b/.gitignore index f1945d12ed..9519aeb837 100644 --- a/.gitignore +++ b/.gitignore @@ -93,7 +93,6 @@ rules.ninja /googletest-*/ /logs /mavsdk_tests -/test_mixer_multirotor /unit-* /uORB/ DartConfiguration.tcl diff --git a/CMakeLists.txt b/CMakeLists.txt index df4ebe309f..408b0f417c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -414,7 +414,6 @@ if(BUILD_TESTING) DEPENDS px4 examples__dyn_hello - test_mixer_multirotor USES_TERMINAL COMMENT "Running tests" WORKING_DIRECTORY ${PX4_BINARY_DIR}) diff --git a/src/drivers/actuators/modalai_esc/modalai_esc.cpp b/src/drivers/actuators/modalai_esc/modalai_esc.cpp index 8e06645bcb..7bf128ecee 100644 --- a/src/drivers/actuators/modalai_esc/modalai_esc.cpp +++ b/src/drivers/actuators/modalai_esc/modalai_esc.cpp @@ -38,7 +38,6 @@ #include "modalai_esc.hpp" #include "modalai_esc_serial.hpp" -#define MODALAI_ESC_DEVICE_PATH "/dev/uart_esc" #define MODALAI_ESC_DEFAULT_PORT "/dev/ttyS1" #define MODALAI_ESC_VOXL_PORT "/dev/ttyS4" @@ -48,7 +47,6 @@ const char *_device; ModalaiEsc::ModalaiEsc() : - CDev(MODALAI_ESC_DEVICE_PATH), OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default), _cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")), _output_update_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": output update interval")) @@ -96,35 +94,15 @@ ModalaiEsc::~ModalaiEsc() _uart_port_bridge = nullptr; } - /* clean up the alternate device node */ - unregister_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH, _class_instance); - perf_free(_cycle_perf); perf_free(_output_update_perf); } int ModalaiEsc::init() { - /* do regular cdev init */ - int ret = CDev::init(); - - if (ret != OK) { - return ret; - } - - /* try to claim the generic PWM output device node as well - it's OK if we fail at this */ - _class_instance = register_class_devname(MODALAI_ESC_DEVICE_PATH); - - if (_class_instance == CLASS_DEVICE_PRIMARY) { - /* lets not be too verbose */ - } else if (_class_instance < 0) { - PX4_ERR("FAILED registering class device"); - } - - _mixing_output.setDriverInstance(_class_instance); /* Getting initial parameter values */ - ret = update_params(); + int ret = update_params(); if (ret != OK) { return ret; @@ -1135,8 +1113,6 @@ void ModalaiEsc::Run() return; } - SmartLock lock_guard(_lock); - perf_begin(_cycle_perf); /* Open serial port in this thread */ diff --git a/src/drivers/actuators/modalai_esc/modalai_esc.hpp b/src/drivers/actuators/modalai_esc/modalai_esc.hpp index d077534142..6c15692f36 100644 --- a/src/drivers/actuators/modalai_esc/modalai_esc.hpp +++ b/src/drivers/actuators/modalai_esc/modalai_esc.hpp @@ -34,7 +34,6 @@ #pragma once #include -#include #include #include #include @@ -51,7 +50,7 @@ #include "qc_esc_packet.h" #include "qc_esc_packet_types.h" -class ModalaiEsc : public cdev::CDev, public ModuleBase, public OutputModuleInterface +class ModalaiEsc : public ModuleBase, public OutputModuleInterface { public: ModalaiEsc(); @@ -76,9 +75,6 @@ public: bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) override; - /** @see OutputModuleInterface */ - void mixerChanged() override; - virtual int init(); typedef enum { @@ -183,8 +179,6 @@ private: ch_assign_t _output_map[MODALAI_ESC_OUTPUT_CHANNELS] {{1, 1}, {2, 1}, {3, 1}, {4, 1}}; MixingOutput _mixing_output{"MODALAI_ESC", MODALAI_ESC_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}; - int _class_instance{-1}; - perf_counter_t _cycle_perf; perf_counter_t _output_update_perf; diff --git a/src/drivers/cyphal/Cyphal.hpp b/src/drivers/cyphal/Cyphal.hpp index 203f11b601..b8550b74fb 100644 --- a/src/drivers/cyphal/Cyphal.hpp +++ b/src/drivers/cyphal/Cyphal.hpp @@ -91,8 +91,6 @@ public: bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) override; - void mixerChanged() override {}; - void printInfo() { _mixing_output.printStatus(); } MixingOutput &mixingOutput() { return _mixing_output; } diff --git a/src/drivers/dshot/CMakeLists.txt b/src/drivers/dshot/CMakeLists.txt index 757a7a8d24..82f7c3db51 100644 --- a/src/drivers/dshot/CMakeLists.txt +++ b/src/drivers/dshot/CMakeLists.txt @@ -48,7 +48,6 @@ px4_add_module( DEPENDS arch_io_pins arch_dshot - mixer mixer_module MODULE_CONFIG module.yaml diff --git a/src/drivers/dshot/DShot.cpp b/src/drivers/dshot/DShot.cpp index 09557de26a..4c4c36fcf7 100644 --- a/src/drivers/dshot/DShot.cpp +++ b/src/drivers/dshot/DShot.cpp @@ -47,10 +47,8 @@ DShot::DShot() : _mixing_output.setAllMinValues(DSHOT_MIN_THROTTLE); _mixing_output.setAllMaxValues(DSHOT_MAX_THROTTLE); - if (_mixing_output.useDynamicMixing()) { - // Avoid using the PWM failsafe params - _mixing_output.setAllFailsafeValues(UINT16_MAX); - } + // Avoid using the PWM failsafe params + _mixing_output.setAllFailsafeValues(UINT16_MAX); } DShot::~DShot() @@ -64,8 +62,6 @@ DShot::~DShot() int DShot::init() { - _mixing_output.setDriverInstance(0); - _output_mask = (1u << _num_outputs) - 1; // Getting initial parameter values @@ -102,75 +98,72 @@ int DShot::task_spawn(int argc, char *argv[]) void DShot::enable_dshot_outputs(const bool enabled) { if (enabled && !_outputs_initialized) { - if (_mixing_output.useDynamicMixing()) { + unsigned int dshot_frequency = 0; + uint32_t dshot_frequency_param = 0; - unsigned int dshot_frequency = 0; - uint32_t dshot_frequency_param = 0; + for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) { + uint32_t channels = io_timer_get_group(timer); - for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) { - uint32_t channels = io_timer_get_group(timer); + if (channels == 0) { + continue; + } - if (channels == 0) { - continue; - } + char param_name[17]; + snprintf(param_name, sizeof(param_name), "%s_TIM%u", _mixing_output.paramPrefix(), timer); - char param_name[17]; - snprintf(param_name, sizeof(param_name), "%s_TIM%u", _mixing_output.paramPrefix(), timer); + int32_t tim_config = 0; + param_t handle = param_find(param_name); + param_get(handle, &tim_config); + unsigned int dshot_frequency_request = 0; - int32_t tim_config = 0; - param_t handle = param_find(param_name); - param_get(handle, &tim_config); - unsigned int dshot_frequency_request = 0; + if (tim_config == -5) { + dshot_frequency_request = DSHOT150; - if (tim_config == -5) { - dshot_frequency_request = DSHOT150; + } else if (tim_config == -4) { + dshot_frequency_request = DSHOT300; - } else if (tim_config == -4) { - dshot_frequency_request = DSHOT300; + } else if (tim_config == -3) { + dshot_frequency_request = DSHOT600; - } else if (tim_config == -3) { - dshot_frequency_request = DSHOT600; + } else if (tim_config == -2) { + dshot_frequency_request = DSHOT1200; - } else if (tim_config == -2) { - dshot_frequency_request = DSHOT1200; + } else { + _output_mask &= ~channels; // don't use for dshot + } + + if (dshot_frequency_request != 0) { + if (dshot_frequency != 0 && dshot_frequency != dshot_frequency_request) { + PX4_WARN("Only supporting a single frequency, adjusting param %s", param_name); + param_set_no_notification(handle, &dshot_frequency_param); } else { - _output_mask &= ~channels; // don't use for dshot - } - - if (dshot_frequency_request != 0) { - if (dshot_frequency != 0 && dshot_frequency != dshot_frequency_request) { - PX4_WARN("Only supporting a single frequency, adjusting param %s", param_name); - param_set_no_notification(handle, &dshot_frequency_param); - - } else { - dshot_frequency = dshot_frequency_request; - dshot_frequency_param = tim_config; - } + dshot_frequency = dshot_frequency_request; + dshot_frequency_param = tim_config; } } + } - int ret = up_dshot_init(_output_mask, dshot_frequency); + int ret = up_dshot_init(_output_mask, dshot_frequency); - if (ret < 0) { - PX4_ERR("up_dshot_init failed (%i)", ret); - return; + if (ret < 0) { + PX4_ERR("up_dshot_init failed (%i)", ret); + return; + } + + _output_mask = ret; + + // disable unused functions + for (unsigned i = 0; i < _num_outputs; ++i) { + if (((1 << i) & _output_mask) == 0) { + _mixing_output.disableFunction(i); } + } - _output_mask = ret; - - // disable unused functions - for (unsigned i = 0; i < _num_outputs; ++i) { - if (((1 << i) & _output_mask) == 0) { - _mixing_output.disableFunction(i); - } - } - - if (_output_mask == 0) { - // exit the module if no outputs used - request_stop(); - return; - } + if (_output_mask == 0) { + // exit the module if no outputs used + request_stop(); + return; } _outputs_initialized = true; @@ -190,17 +183,10 @@ void DShot::update_telemetry_num_motors() int motor_count = 0; - if (_mixing_output.useDynamicMixing()) { - for (unsigned i = 0; i < _num_outputs; ++i) { - if (_mixing_output.isFunctionSet(i)) { - _telemetry->actuator_functions[motor_count] = (uint8_t)_mixing_output.outputFunction(i); - ++motor_count; - } - } - - } else { - if (_mixing_output.mixers()) { - motor_count = _mixing_output.mixers()->get_multirotor_count(); + for (unsigned i = 0; i < _num_outputs; ++i) { + if (_mixing_output.isFunctionSet(i)) { + _telemetry->actuator_functions[motor_count] = (uint8_t)_mixing_output.outputFunction(i); + ++motor_count; } } @@ -449,9 +435,7 @@ bool DShot::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], _current_command.clear(); } - if (stop_motors || num_control_groups_updated > 0 || _mixing_output.useDynamicMixing()) { - up_dshot_trigger(); - } + up_dshot_trigger(); return true; } @@ -471,7 +455,7 @@ void DShot::Run() _mixing_output.update(); // update output status if armed or if mixer is loaded - bool outputs_on = _mixing_output.armed().armed || _mixing_output.initialized(); + bool outputs_on = true; if (_outputs_on != outputs_on) { enable_dshot_outputs(outputs_on); diff --git a/src/drivers/linux_pwm_out/linux_pwm_out.cpp b/src/drivers/linux_pwm_out/linux_pwm_out.cpp index c3fceefca5..7712973051 100644 --- a/src/drivers/linux_pwm_out/linux_pwm_out.cpp +++ b/src/drivers/linux_pwm_out/linux_pwm_out.cpp @@ -41,22 +41,14 @@ using namespace pwm_out; LinuxPWMOut::LinuxPWMOut() : - CDev("/dev/pwm_out"), OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default), _cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")), _interval_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": interval")) { - if (!_mixing_output.useDynamicMixing()) { - _mixing_output.setAllMinValues(PWM_DEFAULT_MIN); - _mixing_output.setAllMaxValues(PWM_DEFAULT_MAX); - } } LinuxPWMOut::~LinuxPWMOut() { - /* clean up the alternate device node */ - unregister_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH, _class_instance); - perf_free(_cycle_perf); perf_free(_interval_perf); delete _pwm_out; @@ -64,21 +56,9 @@ LinuxPWMOut::~LinuxPWMOut() int LinuxPWMOut::init() { - /* do regular cdev init */ - int ret = CDev::init(); - - if (ret != OK) { - return ret; - } - - /* try to claim the generic PWM output device node as well - it's OK if we fail at this */ - _class_instance = register_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH); - - _mixing_output.setDriverInstance(_class_instance); - _pwm_out = new BOARD_PWM_OUT_IMPL(MAX_ACTUATORS); - ret = _pwm_out->init(); + int ret = _pwm_out->init(); if (ret != 0) { PX4_ERR("PWM output init failed"); @@ -87,8 +67,6 @@ int LinuxPWMOut::init() return ret; } - update_params(); - ScheduleNow(); return ret; @@ -134,8 +112,6 @@ void LinuxPWMOut::Run() return; } - SmartLock lock_guard(_lock); - perf_begin(_cycle_perf); perf_count(_interval_perf); @@ -148,7 +124,7 @@ void LinuxPWMOut::Run() _parameter_update_sub.copy(&pupdate); // update parameters from storage - update_params(); + updateParams(); } _mixing_output.updateSubscriptions(false); @@ -156,168 +132,6 @@ void LinuxPWMOut::Run() perf_end(_cycle_perf); } -void LinuxPWMOut::update_params() -{ - updateParams(); - - // skip update when armed or dynamic mixing enabled - if (_mixing_output.armed().armed || _mixing_output.useDynamicMixing()) { - return; - } - - int32_t pwm_min_default = PWM_DEFAULT_MIN; - int32_t pwm_max_default = PWM_DEFAULT_MAX; - int32_t pwm_disarmed_default = 0; - int32_t pwm_default_channels = 0; - - const char *prefix; - - if (_class_instance == CLASS_DEVICE_PRIMARY) { - prefix = "PWM_MAIN"; - - param_get(param_find("PWM_MAIN_MIN"), &pwm_min_default); - param_get(param_find("PWM_MAIN_MAX"), &pwm_max_default); - param_get(param_find("PWM_MAIN_DISARM"), &pwm_disarmed_default); - param_get(param_find("PWM_MAIN_OUT"), &pwm_default_channels); - - } else if (_class_instance == CLASS_DEVICE_SECONDARY) { - prefix = "PWM_AUX"; - - param_get(param_find("PWM_AUX_MIN"), &pwm_min_default); - param_get(param_find("PWM_AUX_MAX"), &pwm_max_default); - param_get(param_find("PWM_AUX_DISARM"), &pwm_disarmed_default); - param_get(param_find("PWM_AUX_OUT"), &pwm_default_channels); - - } else if (_class_instance == CLASS_DEVICE_TERTIARY) { - prefix = "PWM_EXTRA"; - - param_get(param_find("PWM_EXTRA_MIN"), &pwm_min_default); - param_get(param_find("PWM_EXTRA_MAX"), &pwm_max_default); - param_get(param_find("PWM_EXTRA_DISARM"), &pwm_disarmed_default); - - } else { - PX4_ERR("invalid class instance %d", _class_instance); - return; - } - - uint32_t single_ch = 0; - uint32_t pwm_default_channel_mask = 0; - - while ((single_ch = pwm_default_channels % 10)) { - pwm_default_channel_mask |= 1 << (single_ch - 1); - pwm_default_channels /= 10; - } - - char str[17]; - - for (unsigned i = 0; i < MAX_ACTUATORS; i++) { - // PWM_MAIN_MINx - { - sprintf(str, "%s_MIN%u", prefix, i + 1); - int32_t pwm_min = -1; - - if (param_get(param_find(str), &pwm_min) == PX4_OK && pwm_min >= 0) { - _mixing_output.minValue(i) = math::constrain(pwm_min, PWM_LOWEST_MIN, PWM_HIGHEST_MIN); - - if (pwm_min != _mixing_output.minValue(i)) { - int32_t pwm_min_new = _mixing_output.minValue(i); - param_set(param_find(str), &pwm_min_new); - } - - } else if (pwm_default_channel_mask & 1 << i) { - _mixing_output.minValue(i) = pwm_min_default; - } - } - - // PWM_MAIN_MAXx - { - sprintf(str, "%s_MAX%u", prefix, i + 1); - int32_t pwm_max = -1; - - if (param_get(param_find(str), &pwm_max) == PX4_OK && pwm_max >= 0) { - _mixing_output.maxValue(i) = math::constrain(pwm_max, PWM_LOWEST_MAX, PWM_HIGHEST_MAX); - - if (pwm_max != _mixing_output.maxValue(i)) { - int32_t pwm_max_new = _mixing_output.maxValue(i); - param_set(param_find(str), &pwm_max_new); - } - - } else if (pwm_default_channel_mask & 1 << i) { - _mixing_output.maxValue(i) = pwm_max_default; - } - } - - // PWM_MAIN_DISx - { - sprintf(str, "%s_DIS%u", prefix, i + 1); - int32_t pwm_dis = -1; - - if (param_get(param_find(str), &pwm_dis) == PX4_OK && pwm_dis >= 0) { - _mixing_output.disarmedValue(i) = math::constrain(pwm_dis, 0, PWM_HIGHEST_MAX); - - if (pwm_dis != _mixing_output.disarmedValue(i)) { - int32_t pwm_dis_new = _mixing_output.disarmedValue(i); - param_set(param_find(str), &pwm_dis_new); - } - - } else if (pwm_default_channel_mask & 1 << i) { - _mixing_output.disarmedValue(i) = pwm_disarmed_default; - } - } - - // PWM_MAIN_FAILx - { - sprintf(str, "%s_FAIL%u", prefix, i + 1); - int32_t pwm_failsafe = -1; - - if (param_get(param_find(str), &pwm_failsafe) == PX4_OK && pwm_failsafe >= 0) { - _mixing_output.failsafeValue(i) = math::constrain(pwm_failsafe, 0, PWM_HIGHEST_MAX); - - if (pwm_failsafe != _mixing_output.failsafeValue(i)) { - int32_t pwm_fail_new = _mixing_output.failsafeValue(i); - param_set(param_find(str), &pwm_fail_new); - } - - } else { - // if no channel specific failsafe value is configured, use the disarmed value - _mixing_output.failsafeValue(i) = _mixing_output.disarmedValue(i); - } - } - - // PWM_MAIN_REVx - { - sprintf(str, "%s_REV%u", prefix, i + 1); - int32_t pwm_rev = 0; - - if (param_get(param_find(str), &pwm_rev) == PX4_OK) { - uint16_t &reverse_pwm_mask = _mixing_output.reverseOutputMask(); - - if (pwm_rev >= 1) { - reverse_pwm_mask = reverse_pwm_mask | (2 << i); - - } else { - reverse_pwm_mask = reverse_pwm_mask & ~(2 << i); - } - } - } - } - - if (_mixing_output.mixers()) { - int16_t values[MAX_ACTUATORS] {}; - - for (unsigned i = 0; i < MAX_ACTUATORS; i++) { - sprintf(str, "%s_TRIM%u", prefix, i + 1); - - float pval = 0.0f; - param_get(param_find(str), &pval); - values[i] = roundf(10000 * pval); - } - - // copy the trim values to the mixer offsets - _mixing_output.mixers()->set_trims(values, MAX_ACTUATORS); - } -} - int LinuxPWMOut::custom_command(int argc, char *argv[]) { return print_usage("unknown command"); diff --git a/src/drivers/linux_pwm_out/linux_pwm_out.hpp b/src/drivers/linux_pwm_out/linux_pwm_out.hpp index df93ef1aa1..ca3ab3829e 100644 --- a/src/drivers/linux_pwm_out/linux_pwm_out.hpp +++ b/src/drivers/linux_pwm_out/linux_pwm_out.hpp @@ -33,9 +33,7 @@ #pragma once - #include -#include #include #include #include @@ -51,8 +49,7 @@ using namespace time_literals; - -class LinuxPWMOut : public cdev::CDev, public ModuleBase, public OutputModuleInterface +class LinuxPWMOut : public ModuleBase, public OutputModuleInterface { public: LinuxPWMOut(); @@ -72,9 +69,7 @@ public: /** @see ModuleBase::print_status() */ int print_status() override; - int ioctl(device::file_t *filp, int cmd, unsigned long arg) override; - - int init() override; + int init(); bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) override; @@ -82,14 +77,10 @@ public: private: static constexpr int MAX_ACTUATORS = 8; - void update_params(); - MixingOutput _mixing_output{"PWM_MAIN", MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - int _class_instance{-1}; - pwm_out::PWMOutBase *_pwm_out{nullptr}; perf_counter_t _cycle_perf; diff --git a/src/drivers/pca9685_pwm_out/CMakeLists.txt b/src/drivers/pca9685_pwm_out/CMakeLists.txt index a76fb49bbb..e93d2c4e97 100644 --- a/src/drivers/pca9685_pwm_out/CMakeLists.txt +++ b/src/drivers/pca9685_pwm_out/CMakeLists.txt @@ -40,6 +40,5 @@ px4_add_module( MODULE_CONFIG module.yaml DEPENDS - mixer mixer_module ) diff --git a/src/drivers/pca9685_pwm_out/main.cpp b/src/drivers/pca9685_pwm_out/main.cpp index 281fe42d22..1c141d5cfa 100644 --- a/src/drivers/pca9685_pwm_out/main.cpp +++ b/src/drivers/pca9685_pwm_out/main.cpp @@ -59,14 +59,11 @@ using namespace time_literals; class PCA9685Wrapper : public ModuleBase, public OutputModuleInterface { public: - PCA9685Wrapper(int schd_rate_limit = 400); ~PCA9685Wrapper() override ; int init(); - void mixerChanged() override; - /** @see ModuleBase */ static int task_spawn(int argc, char *argv[]); /** @see ModuleBase */ @@ -85,8 +82,6 @@ public: private: perf_counter_t _cycle_perf; - int _class_instance{-1}; - enum class STATE : uint8_t { INIT, WAIT_FOR_OSC, @@ -102,12 +97,6 @@ private: void Run() override; protected: - void updateParams() override; - - void updatePWMParams(); - - void updatePWMParamTrim(); - int _schd_rate_limit = 400; PCA9685 *pca9685 = nullptr; // driver handle. @@ -122,10 +111,6 @@ PCA9685Wrapper::PCA9685Wrapper(int schd_rate_limit) : _cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")), _schd_rate_limit(schd_rate_limit) { - if (!_mixing_output.useDynamicMixing()) { - _mixing_output.setAllMinValues(PWM_DEFAULT_MIN); - _mixing_output.setAllMaxValues(PWM_DEFAULT_MAX); - } } PCA9685Wrapper::~PCA9685Wrapper() @@ -157,205 +142,6 @@ int PCA9685Wrapper::init() return PX4_OK; } -void PCA9685Wrapper::updateParams() -{ - updatePWMParams(); - ModuleParams::updateParams(); -} - -void PCA9685Wrapper::updatePWMParams() -{ - if (_mixing_output.useDynamicMixing()) { - return; - } - - // update pwm params - const char *pname_format_pwm_ch_max[2] = {"PWM_MAIN_MAX%d", "PWM_AUX_MAX%d"}; - const char *pname_format_pwm_ch_min[2] = {"PWM_MAIN_MIN%d", "PWM_AUX_MIN%d"}; - const char *pname_format_pwm_ch_fail[2] = {"PWM_MAIN_FAIL%d", "PWM_AUX_FAIL%d"}; - const char *pname_format_pwm_ch_dis[2] = {"PWM_MAIN_DIS%d", "PWM_AUX_DIS%d"}; - const char *pname_format_pwm_ch_rev[2] = {"PWM_MAIN_REV%d", "PWM_AUX_REV%d"}; - - int32_t default_pwm_max = PWM_DEFAULT_MAX, - default_pwm_min = PWM_DEFAULT_MIN, - default_pwm_fail = PWM_DEFAULT_MIN, - default_pwm_dis = PWM_MOTOR_OFF; - - param_t param_h = param_find("PWM_MAIN_MAX"); - - if (param_h != PARAM_INVALID) { - param_get(param_h, &default_pwm_max); - - } else { - PX4_DEBUG("PARAM_INVALID: %s", "PWM_MAIN_MAX"); - } - - param_h = param_find("PWM_MAIN_MIN"); - - if (param_h != PARAM_INVALID) { - param_get(param_h, &default_pwm_min); - - } else { - PX4_DEBUG("PARAM_INVALID: %s", "PWM_MAIN_MIN"); - } - - param_h = param_find("PWM_MAIN_RATE"); - - if (param_h != PARAM_INVALID) { - int32_t pval = 0; - param_get(param_h, &pval); - - if (_last_fetched_Freq != pval) { - _last_fetched_Freq = pval; - _targetFreq = (float)pval; // update only if changed - } - - } else { - PX4_DEBUG("PARAM_INVALID: %s", "PWM_MAIN_RATE"); - } - - for (int i = 0; i < PCA9685_PWM_CHANNEL_COUNT; i++) { - char pname[16]; - uint8_t param_group, param_index; - - if (i <= 7) { // Main channel - param_group = 0; - param_index = i + 1; - - } else { // AUX - param_group = 1; - param_index = i - 8 + 1; - } - - sprintf(pname, pname_format_pwm_ch_max[param_group], param_index); - param_h = param_find(pname); - - if (param_h != PARAM_INVALID) { - int32_t pval = 0; - param_get(param_h, &pval); - - if (pval != -1) { - _mixing_output.maxValue(i) = pval; - - } else { - _mixing_output.maxValue(i) = default_pwm_max; - } - - } else { - PX4_DEBUG("PARAM_INVALID: %s", pname); - } - - sprintf(pname, pname_format_pwm_ch_min[param_group], param_index); - param_h = param_find(pname); - - if (param_h != PARAM_INVALID) { - int32_t pval = 0; - param_get(param_h, &pval); - - if (pval != -1) { - _mixing_output.minValue(i) = pval; - - } else { - _mixing_output.minValue(i) = default_pwm_min; - } - - } else { - PX4_DEBUG("PARAM_INVALID: %s", pname); - } - - sprintf(pname, pname_format_pwm_ch_fail[param_group], param_index); - param_h = param_find(pname); - - if (param_h != PARAM_INVALID) { - int32_t pval = 0; - param_get(param_h, &pval); - - if (pval != -1) { - _mixing_output.failsafeValue(i) = pval; - - } else { - _mixing_output.failsafeValue(i) = default_pwm_fail; - } - - } else { - PX4_DEBUG("PARAM_INVALID: %s", pname); - } - - sprintf(pname, pname_format_pwm_ch_dis[param_group], param_index); - param_h = param_find(pname); - - if (param_h != PARAM_INVALID) { - int32_t pval = 0; - param_get(param_h, &pval); - - if (pval != -1) { - _mixing_output.disarmedValue(i) = pval; - - } else { - _mixing_output.disarmedValue(i) = default_pwm_dis; - } - - } else { - PX4_DEBUG("PARAM_INVALID: %s", pname); - } - - sprintf(pname, pname_format_pwm_ch_rev[param_group], param_index); - param_h = param_find(pname); - - if (param_h != PARAM_INVALID) { - uint16_t &reverse_pwm_mask = _mixing_output.reverseOutputMask(); - int32_t pval = 0; - param_get(param_h, &pval); - reverse_pwm_mask &= (0xfffe << i); // clear this bit - reverse_pwm_mask |= (((uint16_t)(pval != 0)) << i); // set to new val - - } else { - PX4_DEBUG("PARAM_INVALID: %s", pname); - } - } - - if (_mixing_output.mixers()) { // only update trims if mixer loaded - updatePWMParamTrim(); - } -} - -void PCA9685Wrapper::updatePWMParamTrim() -{ - const char *pname_format_pwm_ch_trim[2] = {"PWM_MAIN_TRIM%d", "PWM_AUX_TRIM%d"}; - - int16_t trim_values[PCA9685_PWM_CHANNEL_COUNT] = {}; - - for (int i = 0; i < PCA9685_PWM_CHANNEL_COUNT; i++) { - char pname[16]; - - uint8_t param_group, param_index; - - if (i <= 7) { // Main channel - param_group = 0; - param_index = i + 1; - - } else { // AUX - param_group = 1; - param_index = i - 8 + 1; - } - - sprintf(pname, pname_format_pwm_ch_trim[param_group], param_index); - param_t param_h = param_find(pname); - int32_t val; - - if (param_h != PARAM_INVALID) { - param_get(param_h, &val); - trim_values[i] = (int16_t)val; - - } else { - PX4_DEBUG("PARAM_INVALID: %s", pname); - } - } - - unsigned n_out = _mixing_output.mixers()->set_trims(trim_values, PCA9685_PWM_CHANNEL_COUNT); - PX4_DEBUG("set %d trims", n_out); -} - bool PCA9685Wrapper::updateOutputs(bool stop_motors, uint16_t *outputs, unsigned num_outputs, unsigned num_control_groups_updated) { @@ -381,7 +167,6 @@ void PCA9685Wrapper::Run() switch (_state) { case STATE::INIT: pca9685->initReg(); - updatePWMParams(); // target frequency fetched, immediately apply it if (_targetFreq > 0.0f) { if (pca9685->setFreq(_targetFreq) != PX4_OK) { @@ -564,17 +349,6 @@ int PCA9685Wrapper::task_spawn(int argc, char **argv) { return PX4_ERROR; } -void PCA9685Wrapper::mixerChanged() -{ - OutputModuleInterface::mixerChanged(); - - if (_mixing_output.mixers()) { // only update trims if mixer loaded - updatePWMParamTrim(); - } - - _mixing_output.updateSubscriptions(false); -} - extern "C" __EXPORT int pca9685_pwm_out_main(int argc, char *argv[]){ return PCA9685Wrapper::main(argc, argv); } diff --git a/src/drivers/pwm_out/CMakeLists.txt b/src/drivers/pwm_out/CMakeLists.txt index 1e5fa884bc..b102064b19 100644 --- a/src/drivers/pwm_out/CMakeLists.txt +++ b/src/drivers/pwm_out/CMakeLists.txt @@ -49,6 +49,5 @@ px4_add_module( module.yaml DEPENDS arch_io_pins - mixer mixer_module ) diff --git a/src/drivers/pwm_out/PWMOut.cpp b/src/drivers/pwm_out/PWMOut.cpp index f4ed868976..a4b8a532f1 100644 --- a/src/drivers/pwm_out/PWMOut.cpp +++ b/src/drivers/pwm_out/PWMOut.cpp @@ -58,10 +58,6 @@ PWMOut::PWMOut(int instance, uint8_t output_base) : _cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")), _interval_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": interval")) { - if (!_mixing_output.useDynamicMixing()) { - _mixing_output.setAllMinValues(PWM_DEFAULT_MIN); - _mixing_output.setAllMaxValues(PWM_DEFAULT_MAX); - } } PWMOut::~PWMOut() @@ -94,14 +90,7 @@ int PWMOut::init() PX4_ERR("FAILED registering class device"); } - _mixing_output.setDriverInstance(_class_instance); - - if (_mixing_output.useDynamicMixing()) { - _num_outputs = FMU_MAX_ACTUATORS; - - } else { - _num_outputs = math::min(FMU_MAX_ACTUATORS - (int)_output_base, MAX_PER_INSTANCE); - } + _num_outputs = FMU_MAX_ACTUATORS; _pwm_mask = ((1u << _num_outputs) - 1) << _output_base; _mixing_output.setMaxNumOutputs(_num_outputs); @@ -114,116 +103,6 @@ int PWMOut::init() return 0; } -/* When set_pwm_rate is called from either of the 2 IOCTLs: - * - * PWM_SERVO_SET_UPDATE_RATE - Sets the "alternate" channel's rate to the callers's rate specified - * and the non "alternate" channels to the _pwm_default_rate. - * - * rate_map = _pwm_alt_rate_channels - * default_rate = _pwm_default_rate - * alt_rate = arg of IOCTL (see rates) - * - * PWM_SERVO_SET_SELECT_UPDATE_RATE - The caller's specified rate map selects the "alternate" channels - * to be set to the alt rate. (_pwm_alt_rate) - * All other channels are set to the default rate. (_pwm_default_rate) - * - * rate_map = arg of IOCTL - * default_rate = _pwm_default_rate - * alt_rate = _pwm_alt_rate - - * rate_map - A mask of 1's for the channels to be set to the - * alternate rate. - * N.B. All channels is a given group must be set - * to the same rate/mode. (default or alt) - * rates: - * alt_rate, default_rate For PWM is 25 or 400Hz - * For Oneshot there is no rate, 0 is therefore used - * to select Oneshot mode - */ -int PWMOut::set_pwm_rate(unsigned rate_map, unsigned default_rate, unsigned alt_rate) -{ - if (_mixing_output.useDynamicMixing()) { - return -EINVAL; - } - - PX4_DEBUG("pwm_out%u set_pwm_rate %x %u %u", _instance, rate_map, default_rate, alt_rate); - - for (unsigned pass = 0; pass < 2; pass++) { - - /* We should note that group is iterated over from 0 to FMU_MAX_ACTUATORS. - * This allows for the ideal worlds situation: 1 channel per group - * configuration. - * - * This is typically not what HW supports. A group represents a timer - * and channels belongs to a timer. - * Therefore all channels in a group are dependent on the timer's - * common settings and can not be independent in terms of count frequency - * (granularity of pulse width) and rate (period of repetition). - * - * To say it another way, all channels in a group must have the same - * rate and mode. (See rates above.) - */ - - for (unsigned group = 0; group < FMU_MAX_ACTUATORS; group++) { - - // get the channel mask for this rate group - uint32_t mask = _pwm_mask & up_pwm_servo_get_rate_group(group); - - if (mask == 0) { - continue; - } - - // all channels in the group must be either default or alt-rate - uint32_t alt = rate_map & mask; - - if (pass == 0) { - // preflight - if ((alt != 0) && (alt != mask)) { - PX4_WARN("rate group %u mask %" PRIx32 " bad overlap %" PRIx32, group, mask, alt); - // not a legal map, bail - return -EINVAL; - } - - } else { - // set it - errors here are unexpected - if (alt != 0) { - if (up_pwm_servo_set_rate_group_update(group, alt_rate) != OK) { - PX4_WARN("rate group set alt failed"); - return -EINVAL; - } - - } else { - if (up_pwm_servo_set_rate_group_update(group, default_rate) != OK) { - PX4_WARN("rate group set default failed"); - return -EINVAL; - } - } - } - } - } - - _pwm_alt_rate_channels = rate_map; - _pwm_default_rate = default_rate; - _pwm_alt_rate = alt_rate; - - // minimum rate for backup schedule - unsigned backup_schedule_rate_hz = math::min(_pwm_default_rate, _pwm_alt_rate); - - if (backup_schedule_rate_hz == 0) { - // OneShot rate is 0 - backup_schedule_rate_hz = 50; - } - - // constrain reasonably (1 to 50 Hz) - backup_schedule_rate_hz = math::constrain(backup_schedule_rate_hz, 1u, 50u); - - _backup_schedule_interval_us = roundf(1e6f / backup_schedule_rate_hz); - - _current_update_rate = 0; // force update - - return OK; -} - void PWMOut::update_current_rate() { /* @@ -274,7 +153,8 @@ int PWMOut::task_spawn(int argc, char *argv[]) } // only start one instance with dynamic mixing - if (dev->_mixing_output.useDynamicMixing()) { + //if (dev->_mixing_output.useDynamicMixing()) { + if (true) { break; } @@ -297,110 +177,65 @@ bool PWMOut::update_pwm_out_state(bool on) { if (on && !_pwm_initialized && _pwm_mask != 0) { - if (_mixing_output.useDynamicMixing()) { + for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) { + _timer_rates[timer] = -1; - for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) { - _timer_rates[timer] = -1; + uint32_t channels = io_timer_get_group(timer); - uint32_t channels = io_timer_get_group(timer); - - if (channels == 0) { - continue; - } - - char param_name[17]; - snprintf(param_name, sizeof(param_name), "%s_TIM%u", _mixing_output.paramPrefix(), timer); - - int32_t tim_config = 0; - param_t handle = param_find(param_name); - param_get(handle, &tim_config); - - if (tim_config > 0) { - _timer_rates[timer] = tim_config; - - } else if (tim_config == -1) { // OneShot - _timer_rates[timer] = 0; - - } else { - _pwm_mask &= ~channels; // don't use for pwm - } + if (channels == 0) { + continue; } - int ret = up_pwm_servo_init(_pwm_mask); + char param_name[17]; + snprintf(param_name, sizeof(param_name), "%s_TIM%u", _mixing_output.paramPrefix(), timer); - if (ret < 0) { - PX4_ERR("up_pwm_servo_init failed (%i)", ret); - return false; - } + int32_t tim_config = 0; + param_t handle = param_find(param_name); + param_get(handle, &tim_config); - _pwm_mask = ret; + if (tim_config > 0) { + _timer_rates[timer] = tim_config; - // set the timer rates - for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) { - uint32_t channels = _pwm_mask & up_pwm_servo_get_rate_group(timer); - - if (channels == 0) { - continue; - } - - ret = up_pwm_servo_set_rate_group_update(timer, _timer_rates[timer]); - - if (ret != 0) { - PX4_ERR("up_pwm_servo_set_rate_group_update failed for timer %i, rate %i (%i)", timer, _timer_rates[timer], ret); - _timer_rates[timer] = -1; - _pwm_mask &= ~channels; - } - } - - _pwm_initialized = true; - - // disable unused functions - for (unsigned i = 0; i < _num_outputs; ++i) { - if (((1 << i) & _pwm_mask) == 0) { - _mixing_output.disableFunction(i); - } - } - - } else { - // Collect all PWM masks from all instances - uint32_t pwm_mask_new = 0; - // Collect the PWM alt rate channels across all instances - uint32_t pwm_alt_rate_channels_new = 0; - - for (int i = 0; i < PWM_OUT_MAX_INSTANCES; i++) { - if (_objects[i].load()) { - - pwm_mask_new |= _objects[i].load()->get_pwm_mask(); - pwm_alt_rate_channels_new |= _objects[i].load()->get_alt_rate_channels(); - } - } - - // Initialize the PWM output state for all instances - // this is re-done once per instance, but harmless - int ret = up_pwm_servo_init(pwm_mask_new); - - if (ret >= 0) { - for (int i = 0; i < PWM_OUT_MAX_INSTANCES; i++) { - if (_objects[i].load()) { - _objects[i].load()->set_pwm_mask(_objects[i].load()->get_pwm_mask() & ret); - } - } - - // Set rate is not affecting non-masked channels, so can be called - // individually - set_pwm_rate(get_alt_rate_channels(), get_default_rate(), get_alt_rate()); - - _pwm_initialized = true; - - // Other instances need to call up_pwm_servo_arm again after we initialized - for (int i = 0; i < PWM_OUT_MAX_INSTANCES; i++) { - if (i != _instance) { - _require_arming[i].store(true); - } - } + } else if (tim_config == -1) { // OneShot + _timer_rates[timer] = 0; } else { - PX4_ERR("up_pwm_servo_init failed (%i)", ret); + _pwm_mask &= ~channels; // don't use for pwm + } + } + + int ret = up_pwm_servo_init(_pwm_mask); + + if (ret < 0) { + PX4_ERR("up_pwm_servo_init failed (%i)", ret); + return false; + } + + _pwm_mask = ret; + + // set the timer rates + for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) { + uint32_t channels = _pwm_mask & up_pwm_servo_get_rate_group(timer); + + if (channels == 0) { + continue; + } + + ret = up_pwm_servo_set_rate_group_update(timer, _timer_rates[timer]); + + if (ret != 0) { + PX4_ERR("up_pwm_servo_set_rate_group_update failed for timer %i, rate %i (%i)", timer, _timer_rates[timer], ret); + _timer_rates[timer] = -1; + _pwm_mask &= ~channels; + } + } + + _pwm_initialized = true; + + // disable unused functions + for (unsigned i = 0; i < _num_outputs; ++i) { + if (((1 << i) & _pwm_mask) == 0) { + _mixing_output.disableFunction(i); } } } @@ -452,16 +287,10 @@ void PWMOut::Run() perf_begin(_cycle_perf); perf_count(_interval_perf); - if (!_mixing_output.useDynamicMixing()) { - // push backup schedule - ScheduleDelayed(_backup_schedule_interval_us); - } - _mixing_output.update(); /* update PWM status if armed or if disarmed PWM values are set */ - bool pwm_on = _mixing_output.armed().armed || (_num_disarmed_set > 0) || _mixing_output.useDynamicMixing() - || _mixing_output.armed().in_esc_calibration_mode; + bool pwm_on = true; if (_pwm_on != pwm_on || _require_arming[_instance].load()) { @@ -477,13 +306,7 @@ void PWMOut::Run() _parameter_update_sub.copy(&pupdate); // update parameters from storage - if (_mixing_output.useDynamicMixing()) { // do not update PWM params for now (was interfering with VTOL PWM settings) - update_params(); - } - } - - if (_pwm_initialized && _current_update_rate == 0 && !_mixing_output.useDynamicMixing()) { - update_current_rate(); + update_params(); } // check at end of cycle (updateSubscriptions() can potentially change to a different WorkQueue thread) @@ -503,7 +326,7 @@ void PWMOut::update_params() updateParams(); // Automatically set the PWM rate and disarmed value when a channel is first set to a servo - if (_mixing_output.useDynamicMixing() && !_first_param_update) { + if (!_first_param_update) { for (size_t i = 0; i < _num_outputs; i++) { if ((previously_set_functions & (1u << i)) == 0 && _mixing_output.functionParamHandle(i) != PARAM_INVALID) { int32_t output_function; @@ -544,203 +367,6 @@ void PWMOut::update_params() } _first_param_update = false; - - if (_mixing_output.useDynamicMixing()) { - return; - } - - int32_t pwm_min_default = PWM_DEFAULT_MIN; - int32_t pwm_max_default = PWM_DEFAULT_MAX; - int32_t pwm_disarmed_default = 0; - int32_t pwm_rate_default = 50; - int32_t pwm_default_channels = 0; - - const char *prefix; - - if (_class_instance == CLASS_DEVICE_PRIMARY) { - prefix = "PWM_MAIN"; - - param_get(param_find("PWM_MAIN_MIN"), &pwm_min_default); - param_get(param_find("PWM_MAIN_MAX"), &pwm_max_default); - param_get(param_find("PWM_MAIN_DISARM"), &pwm_disarmed_default); - param_get(param_find("PWM_MAIN_RATE"), &pwm_rate_default); - param_get(param_find("PWM_MAIN_OUT"), &pwm_default_channels); - - } else if (_class_instance == CLASS_DEVICE_SECONDARY) { - prefix = "PWM_AUX"; - - param_get(param_find("PWM_AUX_MIN"), &pwm_min_default); - param_get(param_find("PWM_AUX_MAX"), &pwm_max_default); - param_get(param_find("PWM_AUX_DISARM"), &pwm_disarmed_default); - param_get(param_find("PWM_AUX_RATE"), &pwm_rate_default); - param_get(param_find("PWM_AUX_OUT"), &pwm_default_channels); - - } else if (_class_instance == CLASS_DEVICE_TERTIARY) { - prefix = "PWM_EXTRA"; - - param_get(param_find("PWM_EXTRA_MIN"), &pwm_min_default); - param_get(param_find("PWM_EXTRA_MAX"), &pwm_max_default); - param_get(param_find("PWM_EXTRA_DISARM"), &pwm_disarmed_default); - param_get(param_find("PWM_EXTRA_RATE"), &pwm_rate_default); - - } else { - PX4_ERR("invalid class instance %d", _class_instance); - return; - } - - uint32_t single_ch = 0; - uint32_t pwm_default_channel_mask = 0; - - while ((single_ch = pwm_default_channels % 10)) { - pwm_default_channel_mask |= 1 << (single_ch - 1); - pwm_default_channels /= 10; - } - - // update the counter - // this is needed to decide if disarmed PWM output should be turned on or not - int num_disarmed_set = 0; - - char str[17]; - - for (unsigned i = 0; i < _num_outputs; i++) { - // PWM_MAIN_MINx - { - sprintf(str, "%s_MIN%u", prefix, i + 1); - int32_t pwm_min = -1; - - if (param_get(param_find(str), &pwm_min) == PX4_OK) { - if (pwm_min >= 0 && pwm_min != 1000) { - _mixing_output.minValue(i) = math::constrain(pwm_min, (int32_t) PWM_LOWEST_MIN, (int32_t) PWM_HIGHEST_MIN); - - if (pwm_min != _mixing_output.minValue(i)) { - int32_t pwm_min_new = _mixing_output.minValue(i); - param_set(param_find(str), &pwm_min_new); - } - - } else if (pwm_default_channel_mask & 1 << i) { - _mixing_output.minValue(i) = pwm_min_default; - } - - } else { - PX4_ERR("param %s not found", str); - } - } - - // PWM_MAIN_MAXx - { - sprintf(str, "%s_MAX%u", prefix, i + 1); - int32_t pwm_max = -1; - - if (param_get(param_find(str), &pwm_max) == PX4_OK) { - if (pwm_max >= 0 && pwm_max != 2000) { - _mixing_output.maxValue(i) = math::constrain(pwm_max, (int32_t) PWM_LOWEST_MAX, (int32_t) PWM_HIGHEST_MAX); - - if (pwm_max != _mixing_output.maxValue(i)) { - int32_t pwm_max_new = _mixing_output.maxValue(i); - param_set(param_find(str), &pwm_max_new); - } - - } else if (pwm_default_channel_mask & 1 << i) { - _mixing_output.maxValue(i) = pwm_max_default; - } - - } else { - PX4_ERR("param %s not found", str); - } - } - - // PWM_MAIN_DISx - { - sprintf(str, "%s_DIS%u", prefix, i + 1); - int32_t pwm_dis = -1; - - if (param_get(param_find(str), &pwm_dis) == PX4_OK) { - if (pwm_dis >= 0 && pwm_dis != 900) { - _mixing_output.disarmedValue(i) = math::constrain(pwm_dis, (int32_t) 0, (int32_t) PWM_HIGHEST_MAX); - - if (pwm_dis != _mixing_output.disarmedValue(i)) { - int32_t pwm_dis_new = _mixing_output.disarmedValue(i); - param_set(param_find(str), &pwm_dis_new); - } - - } else if (pwm_default_channel_mask & 1 << i) { - _mixing_output.disarmedValue(i) = pwm_disarmed_default; - } - - } else { - PX4_ERR("param %s not found", str); - } - - if (_mixing_output.disarmedValue(i) > 0) { - num_disarmed_set++; - } - } - - // PWM_MAIN_FAILx - { - sprintf(str, "%s_FAIL%u", prefix, i + 1); - int32_t pwm_failsafe = -1; - - if (param_get(param_find(str), &pwm_failsafe) == PX4_OK) { - if (pwm_failsafe >= 0) { - _mixing_output.failsafeValue(i) = math::constrain(pwm_failsafe, (int32_t) 0, (int32_t) PWM_HIGHEST_MAX); - - if (pwm_failsafe != _mixing_output.failsafeValue(i)) { - int32_t pwm_fail_new = _mixing_output.failsafeValue(i); - param_set(param_find(str), &pwm_fail_new); - } - - } else { - // if no channel specific failsafe value is configured, use the disarmed value - _mixing_output.failsafeValue(i) = _mixing_output.disarmedValue(i); - } - - } else { - PX4_ERR("param %s not found", str); - } - } - - // PWM_MAIN_REVx - { - sprintf(str, "%s_REV%u", prefix, i + 1); - int32_t pwm_rev = 0; - - if (param_get(param_find(str), &pwm_rev) == PX4_OK) { - uint16_t &reverse_pwm_mask = _mixing_output.reverseOutputMask(); - - if (pwm_rev >= 1) { - reverse_pwm_mask = reverse_pwm_mask | (1 << i); - - } else { - reverse_pwm_mask = reverse_pwm_mask & ~(1 << i); - } - - } else { - PX4_ERR("param %s not found", str); - } - } - } - - if (_mixing_output.mixers()) { - int16_t values[FMU_MAX_ACTUATORS] {}; - - for (unsigned i = 0; i < _num_outputs; i++) { - sprintf(str, "%s_TRIM%u", prefix, i + 1); - - float pval = 0.0f; - - if (param_get(param_find(str), &pval) != PX4_OK) { - PX4_ERR("param %s not found", str); - } - - values[i] = roundf(10000 * pval); - } - - // copy the trim values to the mixer offsets - _mixing_output.mixers()->set_trims(values, _num_outputs); - } - - _num_disarmed_set = num_disarmed_set; } int PWMOut::ioctl(device::file_t *filp, int cmd, unsigned long arg) @@ -786,7 +412,7 @@ int PWMOut::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg) break; case PWM_SERVO_SET_UPDATE_RATE: - ret = set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, arg); + ret = -EINVAL; break; case PWM_SERVO_GET_UPDATE_RATE: @@ -794,7 +420,7 @@ int PWMOut::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg) break; case PWM_SERVO_SET_SELECT_UPDATE_RATE: - ret = set_pwm_rate(arg, _pwm_default_rate, _pwm_alt_rate); + ret = -EINVAL; break; case PWM_SERVO_GET_SELECT_UPDATE_RATE: @@ -823,38 +449,9 @@ int PWMOut::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg) break; } - case PWM_SERVO_SET_MIN_PWM: { - struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - - /* discard if too many values are sent */ - if (pwm->channel_count > FMU_MAX_ACTUATORS || _mixing_output.useDynamicMixing()) { - ret = -EINVAL; - break; - } - - for (unsigned i = 0; i < pwm->channel_count; i++) { - if (pwm->values[i] == 0) { - /* ignore 0 */ - } else if (pwm->values[i] > PWM_HIGHEST_MIN) { - _mixing_output.minValue(i) = PWM_HIGHEST_MIN; - - } - -#if PWM_LOWEST_MIN > 0 - - else if (pwm->values[i] < PWM_LOWEST_MIN) { - _mixing_output.minValue(i) = PWM_LOWEST_MIN; - } - -#endif - - else { - _mixing_output.minValue(i) = pwm->values[i]; - } - } - - break; - } + case PWM_SERVO_SET_MIN_PWM: + ret = -EINVAL; + break; case PWM_SERVO_GET_MIN_PWM: { struct pwm_output_values *pwm = (struct pwm_output_values *)arg; @@ -868,31 +465,9 @@ int PWMOut::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg) break; } - case PWM_SERVO_SET_MAX_PWM: { - struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - - /* discard if too many values are sent */ - if (pwm->channel_count > FMU_MAX_ACTUATORS || _mixing_output.useDynamicMixing()) { - ret = -EINVAL; - break; - } - - for (unsigned i = 0; i < pwm->channel_count; i++) { - if (pwm->values[i] == 0) { - /* ignore 0 */ - } else if (pwm->values[i] < PWM_LOWEST_MAX) { - _mixing_output.maxValue(i) = PWM_LOWEST_MAX; - - } else if (pwm->values[i] > PWM_HIGHEST_MAX) { - _mixing_output.maxValue(i) = PWM_HIGHEST_MAX; - - } else { - _mixing_output.maxValue(i) = pwm->values[i]; - } - } - - break; - } + case PWM_SERVO_SET_MAX_PWM: + ret = -EINVAL; + break; case PWM_SERVO_GET_MAX_PWM: { struct pwm_output_values *pwm = (struct pwm_output_values *)arg; @@ -997,7 +572,7 @@ int PWMOut::print_status() perf_print_counter(_interval_perf); _mixing_output.printStatus(); - if (_mixing_output.useDynamicMixing() && _pwm_initialized) { + if (_pwm_initialized) { for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) { if (_timer_rates[timer] >= 0) { PX4_INFO_RAW("Timer %i: rate: %3i", timer, _timer_rates[timer]); diff --git a/src/drivers/pwm_out/PWMOut.hpp b/src/drivers/pwm_out/PWMOut.hpp index 97efabd666..c7599ae26a 100644 --- a/src/drivers/pwm_out/PWMOut.hpp +++ b/src/drivers/pwm_out/PWMOut.hpp @@ -128,8 +128,6 @@ private: MixingOutput _mixing_output {PARAM_PREFIX, FMU_MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, true}; - uint32_t _backup_schedule_interval_us{1_s}; - unsigned _pwm_default_rate{50}; unsigned _pwm_alt_rate{50}; uint32_t _pwm_alt_rate_channels{0}; diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index a698e959fc..b9588a68ba 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -306,11 +306,6 @@ private: */ int io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t setbits); - /** - * Send mixer definition text to IO - */ - int mixer_send(const char *buf, unsigned buflen, unsigned retries = 3); - /** * Handle a status update from IO. * @@ -355,11 +350,6 @@ PX4IO::PX4IO(device::Device *interface) : OutputModuleInterface(MODULE_NAME, px4::serial_port_to_wq(PX4IO_SERIAL_DEVICE)), _interface(interface) { - if (!_mixing_output.useDynamicMixing()) { - _mixing_output.setAllMinValues(PWM_DEFAULT_MIN); - _mixing_output.setAllMaxValues(PWM_DEFAULT_MAX); - } - _mixing_output.setLowrateSchedulingInterval(20_ms); } @@ -487,7 +477,6 @@ int PX4IO::init() /* try to claim the generic PWM output device node as well - it's OK if we fail at this */ if (_param_sys_hitl.get() <= 0 && _param_sys_use_io.get() == 1) { _class_instance = register_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH); - _mixing_output.setDriverInstance(_class_instance); _mixing_output.setMaxTopicUpdateRate(MIN_TOPIC_UPDATE_INTERVAL); } @@ -712,7 +701,7 @@ void PX4IO::update_params() updateParams(); - if (!_mixing_output.armed().armed && _mixing_output.useDynamicMixing()) { + if (!_mixing_output.armed().armed) { // Automatically set the PWM rate and disarmed value when a channel is first set to a servo if (!_first_param_update) { @@ -764,177 +753,6 @@ void PX4IO::update_params() } _first_param_update = false; - - // skip update when armed or PWM disabled - if (_mixing_output.armed().armed || _class_instance == -1 || _mixing_output.useDynamicMixing()) { - return; - } - - int32_t pwm_min_default = PWM_DEFAULT_MIN; - int32_t pwm_max_default = PWM_DEFAULT_MAX; - int32_t pwm_disarmed_default = 0; - int32_t pwm_rate_default = 50; - int32_t pwm_default_channels = 0; - - const char *prefix = "PWM_MAIN"; - - param_get(param_find("PWM_MAIN_MIN"), &pwm_min_default); - param_get(param_find("PWM_MAIN_MAX"), &pwm_max_default); - param_get(param_find("PWM_MAIN_DISARM"), &pwm_disarmed_default); - param_get(param_find("PWM_MAIN_RATE"), &pwm_rate_default); - param_get(param_find("PWM_MAIN_OUT"), &pwm_default_channels); - - uint32_t single_ch = 0; - uint32_t pwm_default_channel_mask = 0; - - while ((single_ch = pwm_default_channels % 10)) { - pwm_default_channel_mask |= 1 << (single_ch - 1); - pwm_default_channels /= 10; - } - - char str[17]; - - // PWM_MAIN_MINx - if (!_pwm_min_configured) { - for (unsigned i = 0; i < _max_actuators; i++) { - sprintf(str, "%s_MIN%u", prefix, i + 1); - int32_t pwm_min = -1; - - if (param_get(param_find(str), &pwm_min) == PX4_OK) { - if (pwm_min >= 0 && pwm_min != 1000) { - _mixing_output.minValue(i) = math::constrain(pwm_min, static_cast(PWM_LOWEST_MIN), - static_cast(PWM_HIGHEST_MIN)); - - if (pwm_min != _mixing_output.minValue(i)) { - int32_t pwm_min_new = _mixing_output.minValue(i); - param_set(param_find(str), &pwm_min_new); - } - - } else if (pwm_default_channel_mask & 1 << i) { - _mixing_output.minValue(i) = pwm_min_default; - } - } - } - - _pwm_min_configured = true; - } - - // PWM_MAIN_MAXx - if (!_pwm_max_configured) { - for (unsigned i = 0; i < _max_actuators; i++) { - sprintf(str, "%s_MAX%u", prefix, i + 1); - int32_t pwm_max = -1; - - if (param_get(param_find(str), &pwm_max) == PX4_OK) { - if (pwm_max >= 0 && pwm_max != 2000) { - _mixing_output.maxValue(i) = math::constrain(pwm_max, static_cast(PWM_LOWEST_MAX), - static_cast(PWM_HIGHEST_MAX)); - - if (pwm_max != _mixing_output.maxValue(i)) { - int32_t pwm_max_new = _mixing_output.maxValue(i); - param_set(param_find(str), &pwm_max_new); - } - - } else if (pwm_default_channel_mask & 1 << i) { - _mixing_output.maxValue(i) = pwm_max_default; - } - } - } - - _pwm_max_configured = true; - } - - // PWM_MAIN_DISx - if (!_pwm_dis_configured) { - for (unsigned i = 0; i < _max_actuators; i++) { - sprintf(str, "%s_DIS%u", prefix, i + 1); - int32_t pwm_dis = -1; - - if (param_get(param_find(str), &pwm_dis) == PX4_OK) { - if (pwm_dis >= 0 && pwm_dis != 900) { - _mixing_output.disarmedValue(i) = math::constrain(pwm_dis, static_cast(0), - static_cast(PWM_HIGHEST_MAX)); - - if (pwm_dis != _mixing_output.disarmedValue(i)) { - int32_t pwm_dis_new = _mixing_output.disarmedValue(i); - param_set(param_find(str), &pwm_dis_new); - } - - } else if (pwm_default_channel_mask & 1 << i) { - _mixing_output.disarmedValue(i) = pwm_disarmed_default; - } - } - } - - _pwm_dis_configured = true; - updateDisarmed(); - } - - // PWM_MAIN_FAILx - if (!_pwm_fail_configured) { - for (unsigned i = 0; i < _max_actuators; i++) { - sprintf(str, "%s_FAIL%u", prefix, i + 1); - int32_t pwm_fail = -1; - - if (param_get(param_find(str), &pwm_fail) == PX4_OK) { - if (pwm_fail >= 0) { - _mixing_output.failsafeValue(i) = math::constrain(pwm_fail, static_cast(0), - static_cast(PWM_HIGHEST_MAX)); - - if (pwm_fail != _mixing_output.failsafeValue(i)) { - int32_t pwm_fail_new = _mixing_output.failsafeValue(i); - param_set(param_find(str), &pwm_fail_new); - } - - } else { - // if no channel specific failsafe value is configured, use the disarmed value - _mixing_output.failsafeValue(i) = _mixing_output.disarmedValue(i); - } - } - } - - _pwm_fail_configured = true; - updateFailsafe(); - } - - // PWM_MAIN_REVx - if (!_pwm_rev_configured) { - uint16_t &reverse_pwm_mask = _mixing_output.reverseOutputMask(); - reverse_pwm_mask = 0; - - for (unsigned i = 0; i < _max_actuators; i++) { - sprintf(str, "%s_REV%u", prefix, i + 1); - int32_t pwm_rev = -1; - - if (param_get(param_find(str), &pwm_rev) == PX4_OK) { - if (pwm_rev >= 1) { - reverse_pwm_mask |= (1 << i); - } - - } - } - - _pwm_rev_configured = true; - } - - // PWM_MAIN_TRIMx - { - int16_t values[8] {}; - - for (unsigned i = 0; i < _max_actuators; i++) { - sprintf(str, "%s_TRIM%u", prefix, i + 1); - float pwm_trim = 0.f; - - if (param_get(param_find(str), &pwm_trim) == PX4_OK) { - values[i] = roundf(10000 * pwm_trim); - } - } - - if (_mixing_output.mixers()) { - // copy the trim values to the mixer offsets - _mixing_output.mixers()->set_trims(values, _max_actuators); - } - } } void PX4IO::answer_command(const vehicle_command_s &cmd, uint8_t result) @@ -1187,30 +1005,13 @@ int PX4IO::io_get_status() status.pwm_failsafe[i] = io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i); } - if (_mixing_output.useDynamicMixing()) { - + { int i = 0; for (uint8_t offset = PX4IO_P_SETUP_PWM_RATE_GROUP0; offset <= PX4IO_P_SETUP_PWM_RATE_GROUP3; ++offset) { // This is a bit different than below, setting the groups, not the channels status.pwm_rate_hz[i++] = io_reg_get(PX4IO_PAGE_SETUP, offset); } - - } else { - // PWM rates, 0 = low rate, 1 = high rate - const uint16_t pwm_rate = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES); - - const int pwm_low_rate = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE); - const int pwm_high_rate = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE); - - for (unsigned i = 0; i < _max_actuators; i++) { - if (pwm_rate & (1 << i)) { - status.pwm_rate_hz[i] = pwm_high_rate; - - } else { - status.pwm_rate_hz[i] = pwm_low_rate; - } - } } uint16_t raw_inputs = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT); @@ -1544,13 +1345,7 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg) case PWM_SERVO_SET_UPDATE_RATE: PX4_DEBUG("PWM_SERVO_SET_UPDATE_RATE"); - if (_mixing_output.useDynamicMixing()) { - ret = -EINVAL; - break; - } - - /* set the requested alternate rate */ - ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE, arg); + ret = -EINVAL; break; case PWM_SERVO_GET_UPDATE_RATE: @@ -1562,26 +1357,7 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg) case PWM_SERVO_SET_SELECT_UPDATE_RATE: { PX4_DEBUG("PWM_SERVO_SET_SELECT_UPDATE_RATE"); - if (_mixing_output.useDynamicMixing()) { - ret = -EINVAL; - break; - } - - /* blindly clear the PWM update alarm - might be set for some other reason */ - io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, PX4IO_P_STATUS_ALARMS_PWM_ERROR); - - /* attempt to set the rate map */ - io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES, arg); - - /* check that the changes took */ - uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS); - - if (alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR) { - ret = -EINVAL; - io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, PX4IO_P_STATUS_ALARMS_PWM_ERROR); - PX4_ERR("failed setting PWM rate on IO"); - } - + ret = -EINVAL; break; } @@ -1624,7 +1400,7 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg) } for (unsigned i = 0; i < pwm->channel_count; i++) { - if (pwm->values[i] != 0 && !_mixing_output.useDynamicMixing()) { + if (pwm->values[i] != 0 && false) { _mixing_output.minValue(i) = math::constrain(pwm->values[i], (uint16_t)PWM_LOWEST_MIN, (uint16_t)PWM_HIGHEST_MIN); } } @@ -1654,7 +1430,7 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg) } for (unsigned i = 0; i < pwm->channel_count; i++) { - if (pwm->values[i] != 0 && !_mixing_output.useDynamicMixing()) { + if (pwm->values[i] != 0 && false) { _mixing_output.maxValue(i) = math::constrain(pwm->values[i], (uint16_t)PWM_LOWEST_MAX, (uint16_t)PWM_HIGHEST_MAX); } } diff --git a/src/drivers/tap_esc/CMakeLists.txt b/src/drivers/tap_esc/CMakeLists.txt index 4ef6473932..cf5acf0685 100644 --- a/src/drivers/tap_esc/CMakeLists.txt +++ b/src/drivers/tap_esc/CMakeLists.txt @@ -45,7 +45,6 @@ px4_add_module( module.yaml DEPENDS led - mixer mixer_module tunes ) diff --git a/src/drivers/tap_esc/TAP_ESC.cpp b/src/drivers/tap_esc/TAP_ESC.cpp index da5a0fdf6e..6118364266 100644 --- a/src/drivers/tap_esc/TAP_ESC.cpp +++ b/src/drivers/tap_esc/TAP_ESC.cpp @@ -36,7 +36,6 @@ #include TAP_ESC::TAP_ESC(char const *const device, uint8_t channels_count): - CDev(TAP_ESC_DEVICE_PATH), OutputModuleInterface(MODULE_NAME, px4::serial_port_to_wq(device)), _mixing_output{"TAP_ESC", channels_count, *this, MixingOutput::SchedulingPolicy::Auto, true}, _channels_count(channels_count) @@ -154,8 +153,7 @@ int TAP_ESC::init() usleep(2000); } - /* do regular cdev init */ - return CDev::init(); + return 0; } void TAP_ESC::send_esc_outputs(const uint16_t *pwm, const uint8_t motor_cnt) @@ -337,8 +335,6 @@ void TAP_ESC::Run() return; } - SmartLock lock_guard(_lock); - // push backup schedule ScheduleDelayed(20_ms); diff --git a/src/drivers/tap_esc/TAP_ESC.hpp b/src/drivers/tap_esc/TAP_ESC.hpp index 39f7eee210..673237a1ca 100644 --- a/src/drivers/tap_esc/TAP_ESC.hpp +++ b/src/drivers/tap_esc/TAP_ESC.hpp @@ -52,7 +52,6 @@ #include #include #include -#include #include #include @@ -80,7 +79,7 @@ using namespace time_literals; /* * This driver connects to TAP ESCs via serial. */ -class TAP_ESC : public cdev::CDev, public ModuleBase, public OutputModuleInterface +class TAP_ESC : public ModuleBase, public OutputModuleInterface { public: TAP_ESC(const char *device, uint8_t channels_count); @@ -98,7 +97,7 @@ public: /** @see ModuleBase::print_status() */ int print_status() override; - int init() override; + int init(); bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) override; diff --git a/src/drivers/tap_esc/drv_tap_esc.h b/src/drivers/tap_esc/drv_tap_esc.h index f34cf01453..1baa778e08 100644 --- a/src/drivers/tap_esc/drv_tap_esc.h +++ b/src/drivers/tap_esc/drv_tap_esc.h @@ -37,8 +37,6 @@ #include -#define TAP_ESC_DEVICE_PATH "/dev/tap_esc" - /* At the moment the only known use is with a current sensor */ #define ESC_HAVE_CURRENT_SENSOR diff --git a/src/drivers/uavcan/CMakeLists.txt b/src/drivers/uavcan/CMakeLists.txt index e8d4375bc5..1ea0213c2f 100644 --- a/src/drivers/uavcan/CMakeLists.txt +++ b/src/drivers/uavcan/CMakeLists.txt @@ -177,7 +177,6 @@ px4_add_module( button_publisher drivers_rangefinder led - mixer mixer_module version diff --git a/src/drivers/uavcan/uavcan_main.cpp b/src/drivers/uavcan/uavcan_main.cpp index b021560976..4c1f7d84eb 100644 --- a/src/drivers/uavcan/uavcan_main.cpp +++ b/src/drivers/uavcan/uavcan_main.cpp @@ -575,15 +575,6 @@ UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events) _mixing_interface_esc.mixingOutput().setAllDisarmedValues(UavcanEscController::DISARMED_OUTPUT_VALUE); - if (!_mixing_interface_esc.mixingOutput().useDynamicMixing()) { - // these are configurable with dynamic mixing - _mixing_interface_esc.mixingOutput().setAllMinValues(0); // Can be changed to 1 later, according to UAVCAN_ESC_IDLT - _mixing_interface_esc.mixingOutput().setAllMaxValues(UavcanEscController::max_output_value()); - - param_get(param_find("UAVCAN_ESC_IDLT"), &_idle_throttle_when_armed_param); - enable_idle_throttle_when_armed(true); - } - /* Set up shared service clients */ _param_getset_client.setCallback(GetSetCallback(this, &UavcanNode::cb_getset)); _param_opcode_client.setCallback(ExecuteOpcodeCallback(this, &UavcanNode::cb_opcode)); @@ -931,13 +922,6 @@ void UavcanNode::enable_idle_throttle_when_armed(bool value) { value &= _idle_throttle_when_armed_param > 0; - - if (!_mixing_interface_esc.mixingOutput().useDynamicMixing()) { - if (value != _idle_throttle_when_armed) { - _mixing_interface_esc.mixingOutput().setAllMinValues(value ? 1 : 0); - _idle_throttle_when_armed = value; - } - } } bool UavcanMixingInterfaceESC::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 7c56c4a8f1..b0ea548871 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -51,7 +51,6 @@ add_subdirectory(l1) add_subdirectory(led) add_subdirectory(matrix) add_subdirectory(mathlib) -add_subdirectory(mixer) add_subdirectory(mixer_module) add_subdirectory(motion_planning) add_subdirectory(npfg) diff --git a/src/lib/mixer/.gitignore b/src/lib/mixer/.gitignore deleted file mode 100644 index df7e57e698..0000000000 --- a/src/lib/mixer/.gitignore +++ /dev/null @@ -1,2 +0,0 @@ - -/test_mixer_multirotor diff --git a/src/lib/mixer/CMakeLists.txt b/src/lib/mixer/CMakeLists.txt deleted file mode 100644 index 9b4acf4f8c..0000000000 --- a/src/lib/mixer/CMakeLists.txt +++ /dev/null @@ -1,56 +0,0 @@ -############################################################################ -# -# Copyright (c) 2015-2019 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. -# -############################################################################ - -# required by other mixers -add_subdirectory(MixerBase) - -add_subdirectory(HelicopterMixer) -add_subdirectory(MultirotorMixer) -add_subdirectory(NullMixer) -add_subdirectory(SimpleMixer) - -add_library(mixer - MixerGroup.cpp - MixerGroup.hpp - load_mixer_file.cpp - mixer_load.h -) - -target_link_libraries(mixer - PRIVATE - HelicopterMixer - MultirotorMixer - NullMixer - SimpleMixer - ) -add_dependencies(mixer prebuild_targets) diff --git a/src/lib/mixer/HelicopterMixer/CMakeLists.txt b/src/lib/mixer/HelicopterMixer/CMakeLists.txt deleted file mode 100644 index 264f3335ef..0000000000 --- a/src/lib/mixer/HelicopterMixer/CMakeLists.txt +++ /dev/null @@ -1,39 +0,0 @@ -############################################################################ -# -# Copyright (c) 2015-2019 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. -# -############################################################################ - -add_library(HelicopterMixer - HelicopterMixer.cpp - HelicopterMixer.hpp -) -target_link_libraries(HelicopterMixer PRIVATE MixerBase) -add_dependencies(HelicopterMixer prebuild_targets) diff --git a/src/lib/mixer/HelicopterMixer/HelicopterMixer.cpp b/src/lib/mixer/HelicopterMixer/HelicopterMixer.cpp deleted file mode 100644 index e202367e97..0000000000 --- a/src/lib/mixer/HelicopterMixer/HelicopterMixer.cpp +++ /dev/null @@ -1,240 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2017 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. - * - ****************************************************************************/ - -/** - * @file mixer_helicopter.cpp - * - * Helicopter mixers. - */ - -#include "HelicopterMixer.hpp" - -#include -#include -#include - -#define debug(fmt, args...) do { } while(0) -//#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0) -//#include -//#define debug(fmt, args...) lowsyslog(fmt "\n", ##args) - -using math::constrain; - -HelicopterMixer::HelicopterMixer(ControlCallback control_cb, uintptr_t cb_handle, mixer_heli_s mixer_info) : - Mixer(control_cb, cb_handle), - _mixer_info(mixer_info) -{ -} - -HelicopterMixer * -HelicopterMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen) -{ - mixer_heli_s mixer_info; - unsigned swash_plate_servo_count = 0; - unsigned u[5]; - int s[5]; - int used; - - /* enforce that the mixer ends with a new line */ - if (!string_well_formed(buf, buflen)) { - return nullptr; - } - - if (sscanf(buf, "H: %u%n", &swash_plate_servo_count, &used) != 1) { - debug("helicopter parse failed on '%s'", buf); - return nullptr; - } - - if (swash_plate_servo_count < 3 || swash_plate_servo_count > 4) { - debug("only supporting swash plate with 3 or 4 servos"); - return nullptr; - } - - if (used > (int)buflen) { - debug("OVERFLOW: helicopter spec used %d of %u", used, buflen); - return nullptr; - } - - buf = skipline(buf, buflen); - - if (buf == nullptr) { - debug("no line ending, line is incomplete"); - return nullptr; - } - - buf = findtag(buf, buflen, 'T'); - - if ((buf == nullptr) || (buflen < 12)) { - debug("control parser failed finding tag, ret: '%s'", buf); - return nullptr; - } - - if (sscanf(buf, "T: %u %u %u %u %u", - &u[0], &u[1], &u[2], &u[3], &u[4]) != 5) { - debug("control parse failed on '%s'", buf); - return nullptr; - } - - for (unsigned i = 0; i < HELI_CURVES_NR_POINTS; i++) { - mixer_info.throttle_curve[i] = ((float) u[i]) / 10000.0f; - } - - buf = skipline(buf, buflen); - - if (buf == nullptr) { - debug("no line ending, line is incomplete"); - return nullptr; - } - - buf = findtag(buf, buflen, 'P'); - - if ((buf == nullptr) || (buflen < 12)) { - debug("control parser failed finding tag, ret: '%s'", buf); - return nullptr; - } - - if (sscanf(buf, "P: %d %d %d %d %d", - &s[0], &s[1], &s[2], &s[3], &s[4]) != 5) { - debug("control parse failed on '%s'", buf); - return nullptr; - } - - for (unsigned i = 0; i < HELI_CURVES_NR_POINTS; i++) { - mixer_info.pitch_curve[i] = ((float) s[i]) / 10000.0f; - } - - buf = skipline(buf, buflen); - - if (buf == nullptr) { - debug("no line ending, line is incomplete"); - return nullptr; - } - - mixer_info.control_count = swash_plate_servo_count; - - /* Now loop through the servos */ - for (unsigned i = 0; i < mixer_info.control_count; i++) { - - buf = findtag(buf, buflen, 'S'); - - if ((buf == nullptr) || (buflen < 12)) { - debug("control parser failed finding tag, ret: '%s'", buf); - return nullptr; - } - - if (sscanf(buf, "S: %u %u %d %d %d %d", - &u[0], - &u[1], - &s[0], - &s[1], - &s[2], - &s[3]) != 6) { - debug("control parse failed on '%s'", buf); - return nullptr; - } - - mixer_info.servos[i].angle = ((float) u[0]) * M_PI_F / 180.0f; - mixer_info.servos[i].arm_length = ((float) u[1]) / 10000.0f; - mixer_info.servos[i].scale = ((float) s[0]) / 10000.0f; - mixer_info.servos[i].offset = ((float) s[1]) / 10000.0f; - mixer_info.servos[i].min_output = ((float) s[2]) / 10000.0f; - mixer_info.servos[i].max_output = ((float) s[3]) / 10000.0f; - - buf = skipline(buf, buflen); - - if (buf == nullptr) { - debug("no line ending, line is incomplete"); - return nullptr; - } - } - - debug("remaining in buf: %d, first char: %c", buflen, buf[0]); - - HelicopterMixer *hm = new HelicopterMixer(control_cb, cb_handle, mixer_info); - - if (hm != nullptr) { - debug("loaded heli mixer with %d swash plate input(s)", mixer_info.control_count); - - } else { - debug("could not allocate memory for mixer"); - } - - return hm; -} - -unsigned -HelicopterMixer::mix(float *outputs, unsigned space) -{ - if (space < _mixer_info.control_count + 1u) { - return 0; - } - - /* Find index to use for curves */ - float thrust_cmd = get_control(0, 3); - int idx = (thrust_cmd / 0.25f); - - /* Make sure idx is in range */ - if (idx < 0) { - idx = 0; - - } else if (idx > HELI_CURVES_NR_POINTS - 2) { - /* We access idx + 1 below, so max legal index is (size - 2) */ - idx = HELI_CURVES_NR_POINTS - 2; - } - - /* Local throttle curve gradient and offset */ - float tg = (_mixer_info.throttle_curve[idx + 1] - _mixer_info.throttle_curve[idx]) / 0.25f; - float to = (_mixer_info.throttle_curve[idx]) - (tg * idx * 0.25f); - float throttle = constrain(2.0f * (tg * thrust_cmd + to) - 1.0f, -1.0f, 1.0f); - - /* Local pitch curve gradient and offset */ - float pg = (_mixer_info.pitch_curve[idx + 1] - _mixer_info.pitch_curve[idx]) / 0.25f; - float po = (_mixer_info.pitch_curve[idx]) - (pg * idx * 0.25f); - float collective_pitch = constrain((pg * thrust_cmd + po), -0.5f, 0.5f); - - float roll_cmd = get_control(0, 0); - float pitch_cmd = get_control(0, 1); - - outputs[0] = throttle; - - for (unsigned i = 0; i < _mixer_info.control_count; i++) { - outputs[i + 1] = collective_pitch - + cosf(_mixer_info.servos[i].angle) * pitch_cmd * _mixer_info.servos[i].arm_length - - sinf(_mixer_info.servos[i].angle) * roll_cmd * _mixer_info.servos[i].arm_length; - outputs[i + 1] *= _mixer_info.servos[i].scale; - outputs[i + 1] += _mixer_info.servos[i].offset; - outputs[i + 1] = constrain(outputs[i + 1], _mixer_info.servos[i].min_output, _mixer_info.servos[i].max_output); - } - - return _mixer_info.control_count + 1; -} diff --git a/src/lib/mixer/HelicopterMixer/HelicopterMixer.hpp b/src/lib/mixer/HelicopterMixer/HelicopterMixer.hpp deleted file mode 100644 index ce50a8f58c..0000000000 --- a/src/lib/mixer/HelicopterMixer/HelicopterMixer.hpp +++ /dev/null @@ -1,111 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012-2019 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 - -/** helicopter swash servo mixer */ -struct mixer_heli_servo_s { - float angle; - float arm_length; - float scale; - float offset; - float min_output; - float max_output; -}; - -#define HELI_CURVES_NR_POINTS 5 - -/** helicopter swash plate mixer */ -struct mixer_heli_s { - uint8_t control_count; /**< number of inputs */ - float throttle_curve[HELI_CURVES_NR_POINTS]; - float pitch_curve[HELI_CURVES_NR_POINTS]; - mixer_heli_servo_s servos[4]; /**< up to four inputs */ -}; - -/** - * Generic helicopter mixer for helicopters with swash plate. - * - * Collects four inputs (roll, pitch, yaw, thrust) and mixes them to servo commands - * for swash plate tilting and throttle- and pitch curves. - */ -class HelicopterMixer : public Mixer -{ -public: - /** - * Constructor. - * - * @param control_cb Callback invoked to read inputs. - * @param cb_handle Passed to control_cb. - * @param mixer_info Pointer to heli mixer configuration - */ - HelicopterMixer(ControlCallback control_cb, uintptr_t cb_handle, mixer_heli_s mixer_info); - virtual ~HelicopterMixer() = default; - - // no copy, assignment, move, move assignment - HelicopterMixer(const HelicopterMixer &) = delete; - HelicopterMixer &operator=(const HelicopterMixer &) = delete; - HelicopterMixer(HelicopterMixer &&) = delete; - HelicopterMixer &operator=(HelicopterMixer &&) = delete; - - /** - * Factory method. - * - * Given a pointer to a buffer containing a text description of the mixer, - * returns a pointer to a new instance of the mixer. - * - * @param control_cb The callback to invoke when fetching a - * control value. - * @param cb_handle Handle passed to the control callback. - * @param buf Buffer containing a text description of - * the mixer. - * @param buflen Length of the buffer in bytes, adjusted - * to reflect the bytes consumed. - * @return A new HelicopterMixer instance, or nullptr - * if the text format is bad. - */ - static HelicopterMixer *from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, - unsigned &buflen); - - unsigned mix(float *outputs, unsigned space) override; - - void groups_required(uint32_t &groups) override { groups |= (1 << 0); } - - unsigned set_trim(float trim) override { return 4; } - unsigned get_trim(float *trim) override { return 4; } - -private: - mixer_heli_s _mixer_info; -}; diff --git a/src/lib/mixer/MixerBase/CMakeLists.txt b/src/lib/mixer/MixerBase/CMakeLists.txt deleted file mode 100644 index d4b1ec2915..0000000000 --- a/src/lib/mixer/MixerBase/CMakeLists.txt +++ /dev/null @@ -1,38 +0,0 @@ -############################################################################ -# -# Copyright (c) 2015-2019 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. -# -############################################################################ - -add_library(MixerBase - Mixer.cpp - Mixer.hpp -) -add_dependencies(MixerBase prebuild_targets) diff --git a/src/lib/mixer/MixerBase/Mixer.cpp b/src/lib/mixer/MixerBase/Mixer.cpp deleted file mode 100644 index b38b759a3f..0000000000 --- a/src/lib/mixer/MixerBase/Mixer.cpp +++ /dev/null @@ -1,124 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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. - * - ****************************************************************************/ - -/** - * @file mixer.cpp - * - * Programmable multi-channel mixer library. - */ - -#include "Mixer.hpp" - -#include -#include -#include - -#define debug(fmt, args...) do { } while(0) -//#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0) - -float -Mixer::get_control(uint8_t group, uint8_t index) -{ - float value; - - _control_cb(_cb_handle, group, index, value); - - return value; -} - -const char * -Mixer::findtag(const char *buf, unsigned &buflen, char tag) -{ - while (buflen >= 2) { - if ((buf[0] == tag) && (buf[1] == ':')) { - return buf; - } - - buf++; - buflen--; - } - - return nullptr; -} - -char -Mixer::findnexttag(const char *buf, unsigned buflen) -{ - while (buflen >= 2) { - if (isupper(buf[0]) && buf[1] == ':') { - return buf[0]; - } - - buf++; - buflen--; - } - - return 0; -} - -const char * -Mixer::skipline(const char *buf, unsigned &buflen) -{ - const char *p; - - /* if we can find a CR or NL in the buffer, skip up to it */ - if ((p = (const char *)memchr(buf, '\r', buflen)) || (p = (const char *)memchr(buf, '\n', buflen))) { - /* skip up to it AND one beyond - could be on the NUL symbol now */ - buflen -= (p - buf) + 1; - return p + 1; - } - - return nullptr; -} - -bool -Mixer::string_well_formed(const char *buf, unsigned &buflen) -{ - /* enforce that the mixer ends with a new line */ - for (int i = buflen - 1; i >= 0; i--) { - if (buf[i] == '\0') { - continue; - } - - /* require a space or newline at the end of the buffer, fail on printable chars */ - if (buf[i] == '\n' || buf[i] == '\r') { - /* found a line ending, so no split symbols / numbers. good. */ - return true; - } - - } - - debug("pre-parser rejected: No newline in buf"); - - return false; -} diff --git a/src/lib/mixer/MixerBase/Mixer.hpp b/src/lib/mixer/MixerBase/Mixer.hpp deleted file mode 100644 index ecf4e20bdf..0000000000 --- a/src/lib/mixer/MixerBase/Mixer.hpp +++ /dev/null @@ -1,279 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012-2019 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. - * - ****************************************************************************/ - -/** - * @file Mixer.hpp - * - * Generic, programmable, procedural control signal mixers. - * - * This library implements a generic mixer interface that can be used - * by any driver or subsytem that wants to combine several control signals - * into a single output. - * - * Terminology - * =========== - * - * control value - * A mixer input value, typically provided by some controlling - * component of the system. - * - * control group - * A collection of controls provided by a single controlling component. - * - * actuator - * The mixer output value. - * - * - * Mixing basics - * ============= - * - * An actuator derives its value from the combination of one or more - * control values. Each of the control values is scaled according to - * the actuator's configuration and then combined to produce the - * actuator value, which may then be further scaled to suit the specific - * output type. - * - * Internally, all scaling is performed using floating point values. - * Inputs and outputs are clamped to the range -1.0 to 1.0. - * - * control control control - * | | | - * v v v - * scale scale scale - * | | | - * | v | - * +-------> mix <------+ - * | - * scale - * | - * v - * out - * - * Scaling - * ------- - * - * Each scaler allows the input value to be scaled independently for - * inputs greater/less than zero. An offset can be applied to the output, - * as well as lower and upper boundary constraints. - * Negative scaling factors cause the output to be inverted (negative input - * produces positive output). - * - * Scaler pseudocode: - * - * if (input < 0) - * output = (input * NEGATIVE_SCALE) + OFFSET - * else - * output = (input * POSITIVE_SCALE) + OFFSET - * - * if (output < LOWER_LIMIT) - * output = LOWER_LIMIT - * if (output > UPPER_LIMIT) - * output = UPPER_LIMIT - * - * - * Mixing - * ------ - * - * Mixing behaviour varies based on the specific mixer class; each - * mixer class describes its behaviour in more detail. - * - * - * Controls - * -------- - * - * The precise assignment of controls may vary depending on the - * application, but the following assignments should be used - * when appropriate. Some mixer classes have specific assumptions - * about the assignment of controls. - * - * control | standard meaning - * --------+----------------------- - * 0 | roll - * 1 | pitch - * 2 | yaw - * 3 | primary thrust - */ - -#pragma once - -#include -#include - -/** - * Abstract class defining a mixer mixing zero or more inputs to - * one or more outputs. - */ -class Mixer : public ListNode -{ -public: - enum class Airmode : int32_t { - disabled = 0, - roll_pitch = 1, - roll_pitch_yaw = 2 - }; - - /** - * Fetch a control value. - * - * @param handle Token passed when the callback is registered. - * @param control_group The group to fetch the control from. - * @param control_index The group-relative index to fetch the control from. - * @param control The returned control - * @return Zero if the value was fetched, nonzero otherwise. - */ - typedef int (* ControlCallback)(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &control); - - /** - * Constructor. - * - * @param control_cb Callback invoked when reading controls. - */ - Mixer(ControlCallback control_cb, uintptr_t cb_handle) : _control_cb(control_cb), _cb_handle(cb_handle) {} - virtual ~Mixer() = default; - - // no copy, assignment, move, move assignment - Mixer(const Mixer &) = delete; - Mixer &operator=(const Mixer &) = delete; - Mixer(Mixer &&) = delete; - Mixer &operator=(Mixer &&) = delete; - - /** - * Perform the mixing function. - * - * @param outputs Array into which mixed output(s) should be placed. - * @param space The number of available entries in the output array; - * @return The number of entries in the output array that were populated. - */ - virtual unsigned mix(float *outputs, unsigned space) = 0; - - /** - * Get the saturation status. - * - * @return Integer bitmask containing saturation_status from control_allocator_status.msg. - */ - virtual uint16_t get_saturation_status() { return 0; } - - /** - * Analyses the mix configuration and updates a bitmask of groups - * that are required. - * - * @param groups A bitmask of groups (0-31) that the mixer requires. - */ - virtual void groups_required(uint32_t &groups) {}; - - /** - * @brief Empty method, only implemented for MultirotorMixer and MixerGroup class. - * - * @param[in] delta_out_max Maximum delta output. - * - */ - virtual void set_max_delta_out_once(float delta_out_max) {} - - /** - * @brief Set trim offset for this mixer - * - * @return the number of outputs this mixer feeds to - */ - virtual unsigned set_trim(float trim) { return 0; } - - /** - * @brief Get trim offset for this mixer - * - * @return the number of outputs this mixer feeds to - */ - virtual unsigned get_trim(float *trim) { return 0; } - - /* - * @brief Sets the thrust factor used to calculate mapping from desired thrust to motor control signal output. - * - * @param[in] val The value - */ - virtual void set_thrust_factor(float val) {} - - /** - * @brief Set airmode. Airmode allows the mixer to increase the total thrust in order to unsaturate the motors. - * - * @param[in] airmode Select airmode type (0 = disabled, 1 = roll/pitch, 2 = roll/pitch/yaw) - */ - virtual void set_airmode(Airmode airmode) {}; - - virtual unsigned get_multirotor_count() { return 0; } - - virtual void set_dt_once(float dt) {} - -protected: - - /** client-supplied callback used when fetching control values */ - ControlCallback _control_cb; - uintptr_t _cb_handle; - - /** - * Invoke the client callback to fetch a control value. - * - * @param group Control group to fetch from. - * @param index Control index to fetch. - * @return The control value. - */ - float get_control(uint8_t group, uint8_t index); - - /** - * Find a tag - * - * @param buf The buffer to operate on. - * @param buflen length of the buffer. - * @param tag character to search for. - */ - static const char *findtag(const char *buf, unsigned &buflen, char tag); - - /** - * Find next tag and return it (0 is returned if no tag is found) - * - * @param buf The buffer to operate on. - * @param buflen length of the buffer. - */ - static char findnexttag(const char *buf, unsigned buflen); - - /** - * Skip a line - * - * @param buf The buffer to operate on. - * @param buflen length of the buffer. - * @return 0 / OK if a line could be skipped, 1 else - */ - static const char *skipline(const char *buf, unsigned &buflen); - - /** - * Check wether the string is well formed and suitable for parsing - */ - static bool string_well_formed(const char *buf, unsigned &buflen); -}; diff --git a/src/lib/mixer/MixerGroup.cpp b/src/lib/mixer/MixerGroup.cpp deleted file mode 100644 index b48f5cddee..0000000000 --- a/src/lib/mixer/MixerGroup.cpp +++ /dev/null @@ -1,253 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012-2019 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. - * - ****************************************************************************/ - -/** - * @file mixer_group.cpp - * - * Mixer collection. - */ - -#include "MixerGroup.hpp" - -#include "HelicopterMixer/HelicopterMixer.hpp" -#include "MultirotorMixer/MultirotorMixer.hpp" -#include "NullMixer/NullMixer.hpp" -#include "SimpleMixer/SimpleMixer.hpp" - -#define debug(fmt, args...) do { } while(0) -//#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0) -//#include -//#define debug(fmt, args...) syslog(fmt "\n", ##args) - -unsigned -MixerGroup::mix(float *outputs, unsigned space) -{ - unsigned index = 0; - - for (auto mixer : _mixers) { - index += mixer->mix(outputs + index, space - index); - - if (index >= space) { - break; - } - } - - return index; -} - -/* - * set_trims() has no effect except for the SimpleMixer implementation for which set_trim() - * always returns the value one. - * The only other existing implementation is MultirotorMixer, which ignores the trim value - * and returns _rotor_count. - */ -unsigned -MixerGroup::set_trims(int16_t *values, unsigned n) -{ - unsigned index = 0; - - for (auto mixer : _mixers) { - // convert from integer to float - // to be safe, clamp offset to range of [-500, 500] usec - float offset = math::constrain((float)values[index] / 10000, -1.0f, 1.0f); - - debug("set trim: %d, offset: %5.3f", values[index], (double)offset); - index += mixer->set_trim(offset); - - if (index >= n) { - break; - } - } - - return index; -} - -/* - * get_trims() has no effect except for the SimpleMixer implementation for which get_trim() - * always returns the value one and sets the trim value. - * The only other existing implementation is MultirotorMixer, which ignores the trim value - * and returns _rotor_count. - */ -unsigned -MixerGroup::get_trims(int16_t *values) -{ - unsigned index_mixer = 0; - unsigned index = 0; - - for (auto mixer : _mixers) { - float trim = 0; - index_mixer += mixer->get_trim(&trim); - - // MultirotorMixer returns the number of motors so we - // loop through index_mixer and set the same trim value for all motors - while (index < index_mixer) { - values[index] = trim * 10000; - index++; - } - } - - return index; -} - -void -MixerGroup::set_thrust_factor(float val) -{ - for (auto mixer : _mixers) { - mixer->set_thrust_factor(val); - } -} - -void -MixerGroup::set_airmode(Mixer::Airmode airmode) -{ - for (auto mixer : _mixers) { - mixer->set_airmode(airmode); - } -} - -unsigned -MixerGroup::get_multirotor_count() -{ - for (auto mixer : _mixers) { - unsigned rotor_count = mixer->get_multirotor_count(); - - if (rotor_count > 0) { - return rotor_count; - } - } - - return 0; -} - -uint16_t -MixerGroup::get_saturation_status() -{ - uint16_t sat = 0; - - for (auto mixer : _mixers) { - sat |= mixer->get_saturation_status(); - } - - return sat; -} - -void -MixerGroup::groups_required(uint32_t &groups) -{ - for (auto mixer : _mixers) { - mixer->groups_required(groups); - } -} - -int -MixerGroup::load_from_buf(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen) -{ - int ret = -1; - const char *end = buf + buflen; - - /* - * Loop until either we have emptied the buffer, or we have failed to - * allocate something when we expected to. - */ - while (buflen > 0) { - Mixer *m = nullptr; - const char *p = end - buflen; - unsigned resid = buflen; - - /* - * Use the next character as a hint to decide which mixer class to construct. - */ - switch (*p) { - case 'Z': - m = NullMixer::from_text(p, resid); - break; - - case 'M': - m = SimpleMixer::from_text(control_cb, cb_handle, p, resid); - break; - - case 'R': - m = MultirotorMixer::from_text(control_cb, cb_handle, p, resid); - break; - - case 'H': - m = HelicopterMixer::from_text(control_cb, cb_handle, p, resid); - break; - - default: - /* it's probably junk or whitespace, skip a byte and retry */ - buflen--; - continue; - } - - /* - * If we constructed something, add it to the group. - */ - if (m != nullptr) { - add_mixer(m); - - /* we constructed something */ - ret = 0; - - /* only adjust buflen if parsing was successful */ - buflen = resid; - debug("SUCCESS - buflen: %d", buflen); - - } else { - - /* - * There is data in the buffer that we expected to parse, but it didn't, - * so give up for now. - */ - break; - } - } - - /* nothing more in the buffer for us now */ - return ret; -} - -void MixerGroup::set_max_delta_out_once(float delta_out_max) -{ - for (auto mixer : _mixers) { - mixer->set_max_delta_out_once(delta_out_max); - } -} - -void -MixerGroup::set_dt_once(float dt) -{ - for (auto mixer : _mixers) { - mixer->set_dt_once(dt); - } -} diff --git a/src/lib/mixer/MixerGroup.hpp b/src/lib/mixer/MixerGroup.hpp deleted file mode 100644 index f5b7fedb50..0000000000 --- a/src/lib/mixer/MixerGroup.hpp +++ /dev/null @@ -1,172 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012-2019 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 "MixerBase/Mixer.hpp" - -/** - * Group of mixers, built up from single mixers and processed - * in order when mixing. - */ -class MixerGroup -{ -public: - MixerGroup() = default; - - ~MixerGroup() - { - reset(); - } - - // no copy, assignment, move, move assignment - MixerGroup(const MixerGroup &) = delete; - MixerGroup &operator=(const MixerGroup &) = delete; - MixerGroup(MixerGroup &&) = delete; - MixerGroup &operator=(MixerGroup &&) = delete; - - unsigned mix(float *outputs, unsigned space); - - uint16_t get_saturation_status(); - - void groups_required(uint32_t &groups); - - /** - * Add a mixer to the group. - * - * @param mixer The mixer to be added. - */ - void add_mixer(Mixer *mixer) { _mixers.add(mixer); } - - /** - * Remove all the mixers from the group. - */ - void reset() { _mixers.clear(); } - - /** - * Count the mixers in the group. - */ - unsigned count() const { return _mixers.size(); } - - /** - * Adds mixers to the group based on a text description in a buffer. - * - * Mixer definitions begin with a single capital letter and a colon. - * The actual format of the mixer definition varies with the individual - * mixers; they are summarised here, but see ROMFS/mixers/README for - * more details. - * - * Null Mixer - * .......... - * - * The null mixer definition has the form: - * - * Z: - * - * Simple Mixer - * ............ - * - * A simple mixer definition begins with: - * - * M: - * O: <-ve scale> <+ve scale> - * - * The second line O: can be omitted. In that case 'O: 10000 10000 0 -10000 10000' is used. - * The definition continues with entries describing the control - * inputs and their scaling, in the form: - * - * S: <-ve scale> <+ve scale> - * - * Multirotor Mixer - * ................ - * - * The multirotor mixer definition is a single line of the form: - * - * R: - * - * Helicopter Mixer - * ................ - * - * The helicopter mixer includes throttle and pitch curves - * - * H: - * T: <0> <2500> <5000> <7500> <10000> - * P: <-10000> <-5000> <0> <5000> <10000> - * - * The definition continues with entries describing - * the position of the servo, in the following form: - * - * S: - * - * @param buf The mixer configuration buffer. - * @param buflen The length of the buffer, updated to reflect - * bytes as they are consumed. - * @return Zero on successful load, nonzero otherwise. - */ - int load_from_buf(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen); - - /** - * @brief Update slew rate parameter. This tells instances of the class MultirotorMixer - * the maximum allowed change of the output values per cycle. - * The value is only valid for one cycle, in order to have continuous - * slew rate limiting this function needs to be called before every call - * to mix(). - * - * @param[in] delta_out_max Maximum delta output. - * - */ - void set_max_delta_out_once(float delta_out_max); - - /* - * Invoke the set_offset method of each mixer in the group - * for each value in page r_page_servo_control_trim - */ - unsigned set_trims(int16_t *v, unsigned n); - unsigned get_trims(int16_t *values); - - /** - * @brief Sets the thrust factor used to calculate mapping from desired thrust to motor control signal output. - * - * @param[in] val The value - */ - void set_thrust_factor(float val); - - void set_airmode(Mixer::Airmode airmode); - - unsigned get_multirotor_count(); - - void set_dt_once(float dt); - -private: - List _mixers; /**< linked list of mixers */ -}; diff --git a/src/lib/mixer/MultirotorMixer/CMakeLists.txt b/src/lib/mixer/MultirotorMixer/CMakeLists.txt deleted file mode 100644 index 0a902d95d1..0000000000 --- a/src/lib/mixer/MultirotorMixer/CMakeLists.txt +++ /dev/null @@ -1,113 +0,0 @@ -############################################################################ -# -# Copyright (c) 2015 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. -# -############################################################################ - -set(geometry_files - dodeca_bottom_cox.toml - dodeca_top_cox.toml - hex_cox.toml - hex_plus.toml - hex_t.toml - hex_x.toml - octa_cox.toml - octa_cox_wide.toml - octa_plus.toml - octa_x.toml - quad_deadcat.toml - quad_h.toml - quad_plus.toml - quad_vtail.toml - quad_wide.toml - quad_x_cw.toml - quad_x.toml - quad_x_pusher.toml - quad_y.toml - tri_y.toml - twin_engine.toml -) - -set(geometries_list) -foreach(geom_file ${geometry_files}) - list(APPEND geometries_list ${CMAKE_CURRENT_SOURCE_DIR}/geometries/${geom_file}) -endforeach() - -set(MIXER_TOOLS ${CMAKE_CURRENT_SOURCE_DIR}/geometries/tools) - -# generate mixers and normalize -add_custom_command( - OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor.generated.h - COMMAND ${PYTHON_EXECUTABLE} ${MIXER_TOOLS}/px_generate_mixers.py -f ${geometries_list} -o mixer_multirotor.generated.h - DEPENDS ${MIXER_TOOLS}/px_generate_mixers.py ${geometries_list} - ) -add_custom_command( - OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor_normalized.generated.h - COMMAND ${PYTHON_EXECUTABLE} ${MIXER_TOOLS}/px_generate_mixers.py --normalize -f ${geometries_list} -o mixer_multirotor_normalized.generated.h - DEPENDS ${MIXER_TOOLS}/px_generate_mixers.py ${geometries_list} - ) -add_custom_target(mixer_gen DEPENDS mixer_multirotor.generated.h ${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor_normalized.generated.h) - -# 6DOF mixers -add_custom_command( - OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor_6dof.generated.h - COMMAND ${PYTHON_EXECUTABLE} ${MIXER_TOOLS}/px_generate_mixers.py --sixdof -f ${geometries_list} -o mixer_multirotor_6dof.generated.h - DEPENDS ${MIXER_TOOLS}/px_generate_mixers.py ${geometries_list} - ) -add_custom_target(mixer_gen_6dof DEPENDS mixer_multirotor_6dof.generated.h) - -add_library(MultirotorMixer - MultirotorMixer.cpp - MultirotorMixer.hpp - - ${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor.generated.h - ${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor_normalized.generated.h - ${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor_6dof.generated.h -) -target_link_libraries(MultirotorMixer PRIVATE MixerBase) -target_include_directories(MultirotorMixer PRIVATE ${CMAKE_CURRENT_BINARY_DIR}) -add_dependencies(MultirotorMixer mixer_gen mixer_gen_6dof prebuild_targets) - -if(BUILD_TESTING) - - add_executable(test_mixer_multirotor - test_mixer_multirotor.cpp - MultirotorMixer.cpp - ) - target_compile_definitions(test_mixer_multirotor PRIVATE MIXER_MULTIROTOR_USE_MOCK_GEOMETRY) - target_compile_options(test_mixer_multirotor PRIVATE -Wno-unused-result) - target_link_libraries(test_mixer_multirotor PRIVATE MixerBase) - - add_test(NAME mixer_multirotor - COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/mixer_multirotor.py --test --mixer-multirotor-binary $ - WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR} - ) - -endif() diff --git a/src/lib/mixer/MultirotorMixer/MultirotorMixer.cpp b/src/lib/mixer/MultirotorMixer/MultirotorMixer.cpp deleted file mode 100644 index 5e1948940f..0000000000 --- a/src/lib/mixer/MultirotorMixer/MultirotorMixer.cpp +++ /dev/null @@ -1,498 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2018 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. - * - ****************************************************************************/ - -/** - * @file mixer_multirotor.cpp - * - * Multi-rotor mixers. - */ - -#include "MultirotorMixer.hpp" - -#include -#include -#include - -#include - -#ifdef MIXER_MULTIROTOR_USE_MOCK_GEOMETRY -enum class MultirotorGeometry : MultirotorGeometryUnderlyingType { - QUAD_X, - MAX_GEOMETRY -}; -namespace -{ -const MultirotorMixer::Rotor _config_quad_x[] = { - { -0.707107, 0.707107, 1.000000, 1.000000 }, - { 0.707107, -0.707107, 1.000000, 1.000000 }, - { 0.707107, 0.707107, -1.000000, 1.000000 }, - { -0.707107, -0.707107, -1.000000, 1.000000 }, -}; -const MultirotorMixer::Rotor *_config_index[] = { - &_config_quad_x[0] -}; -const unsigned _config_rotor_count[] = {4}; -const char *_config_key[] = {"4x"}; -} - -#else - -// This file is generated by the px_generate_mixers.py script which is invoked during the build process -// #include "mixer_multirotor.generated.h" -#include "mixer_multirotor_normalized.generated.h" - -#endif /* MIXER_MULTIROTOR_USE_MOCK_GEOMETRY */ - - -#define debug(fmt, args...) do { } while(0) -//#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0) -//#include -//#define debug(fmt, args...) syslog(fmt "\n", ##args) - -MultirotorMixer::MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle, MultirotorGeometry geometry) : - MultirotorMixer(control_cb, cb_handle, _config_index[(int)geometry], _config_rotor_count[(int)geometry]) -{ -} - -MultirotorMixer::MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle, const Rotor *rotors, - unsigned rotor_count) : - Mixer(control_cb, cb_handle), - _rotor_count(rotor_count), - _rotors(rotors), - _outputs_prev(new float[_rotor_count]), - _tmp_array(new float[_rotor_count]) -{ - for (unsigned i = 0; i < _rotor_count; ++i) { - _outputs_prev[i] = -1.f; - } -} - -MultirotorMixer::~MultirotorMixer() -{ - delete[] _outputs_prev; - delete[] _tmp_array; -} - -MultirotorMixer * -MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen) -{ - MultirotorGeometry geometry = MultirotorGeometry::MAX_GEOMETRY; - char geomname[16]; - - /* enforce that the mixer ends with a new line */ - if (!string_well_formed(buf, buflen)) { - return nullptr; - } - - if (sscanf(buf, "R: %15s", geomname) != 1) { - debug("multirotor parse failed on '%s'", buf); - return nullptr; - } - - buf = skipline(buf, buflen); - - if (buf == nullptr) { - debug("no line ending, line is incomplete"); - return nullptr; - } - - debug("remaining in buf: %d, first char: %c", buflen, buf[0]); - - for (MultirotorGeometryUnderlyingType i = 0; i < (MultirotorGeometryUnderlyingType)MultirotorGeometry::MAX_GEOMETRY; - i++) { - if (!strcmp(geomname, _config_key[i])) { - geometry = (MultirotorGeometry)i; - break; - } - } - - if (geometry == MultirotorGeometry::MAX_GEOMETRY) { - debug("unrecognised geometry '%s'", geomname); - return nullptr; - } - - debug("adding multirotor mixer '%s'", geomname); - - return new MultirotorMixer(control_cb, cb_handle, geometry); -} - -float -MultirotorMixer::compute_desaturation_gain(const float *desaturation_vector, const float *outputs, - saturation_status &sat_status, float min_output, float max_output) const -{ - float k_min = 0.f; - float k_max = 0.f; - - for (unsigned i = 0; i < _rotor_count; i++) { - // Avoid division by zero. If desaturation_vector[i] is zero, there's nothing we can do to unsaturate anyway - if (fabsf(desaturation_vector[i]) < FLT_EPSILON) { - continue; - } - - if (outputs[i] < min_output) { - float k = (min_output - outputs[i]) / desaturation_vector[i]; - - if (k < k_min) { k_min = k; } - - if (k > k_max) { k_max = k; } - - sat_status.flags.motor_neg = true; - } - - if (outputs[i] > max_output) { - float k = (max_output - outputs[i]) / desaturation_vector[i]; - - if (k < k_min) { k_min = k; } - - if (k > k_max) { k_max = k; } - - sat_status.flags.motor_pos = true; - } - } - - // Reduce the saturation as much as possible - return k_min + k_max; -} - -void -MultirotorMixer::minimize_saturation(const float *desaturation_vector, float *outputs, - saturation_status &sat_status, float min_output, float max_output, bool reduce_only) const -{ - float k1 = compute_desaturation_gain(desaturation_vector, outputs, sat_status, min_output, max_output); - - if (reduce_only && k1 > 0.f) { - return; - } - - for (unsigned i = 0; i < _rotor_count; i++) { - outputs[i] += k1 * desaturation_vector[i]; - } - - // Compute the desaturation gain again based on the updated outputs. - // In most cases it will be zero. It won't be if max(outputs) - min(outputs) > max_output - min_output. - // In that case adding 0.5 of the gain will equilibrate saturations. - float k2 = 0.5f * compute_desaturation_gain(desaturation_vector, outputs, sat_status, min_output, max_output); - - for (unsigned i = 0; i < _rotor_count; i++) { - outputs[i] += k2 * desaturation_vector[i]; - } -} - -void -MultirotorMixer::mix_airmode_rp(float roll, float pitch, float yaw, float thrust, float *outputs) -{ - // Airmode for roll and pitch, but not yaw - - // Mix without yaw - for (unsigned i = 0; i < _rotor_count; i++) { - outputs[i] = roll * _rotors[i].roll_scale + - pitch * _rotors[i].pitch_scale + - thrust * _rotors[i].thrust_scale; - - // Thrust will be used to unsaturate if needed - _tmp_array[i] = _rotors[i].thrust_scale; - } - - minimize_saturation(_tmp_array, outputs, _saturation_status); - - // Mix yaw independently - mix_yaw(yaw, outputs); -} - -void -MultirotorMixer::mix_airmode_rpy(float roll, float pitch, float yaw, float thrust, float *outputs) -{ - // Airmode for roll, pitch and yaw - - // Do full mixing - for (unsigned i = 0; i < _rotor_count; i++) { - outputs[i] = roll * _rotors[i].roll_scale + - pitch * _rotors[i].pitch_scale + - yaw * _rotors[i].yaw_scale + - thrust * _rotors[i].thrust_scale; - - // Thrust will be used to unsaturate if needed - _tmp_array[i] = _rotors[i].thrust_scale; - } - - minimize_saturation(_tmp_array, outputs, _saturation_status); - - // Unsaturate yaw (in case upper and lower bounds are exceeded) - // to prioritize roll/pitch over yaw. - for (unsigned i = 0; i < _rotor_count; i++) { - _tmp_array[i] = _rotors[i].yaw_scale; - } - - minimize_saturation(_tmp_array, outputs, _saturation_status); -} - -void -MultirotorMixer::mix_airmode_disabled(float roll, float pitch, float yaw, float thrust, float *outputs) -{ - // Airmode disabled: never allow to increase the thrust to unsaturate a motor - - // Mix without yaw - for (unsigned i = 0; i < _rotor_count; i++) { - outputs[i] = roll * _rotors[i].roll_scale + - pitch * _rotors[i].pitch_scale + - thrust * _rotors[i].thrust_scale; - - // Thrust will be used to unsaturate if needed - _tmp_array[i] = _rotors[i].thrust_scale; - } - - // only reduce thrust - minimize_saturation(_tmp_array, outputs, _saturation_status, 0.f, 1.f, true); - - // Reduce roll/pitch acceleration if needed to unsaturate - for (unsigned i = 0; i < _rotor_count; i++) { - _tmp_array[i] = _rotors[i].roll_scale; - } - - minimize_saturation(_tmp_array, outputs, _saturation_status); - - for (unsigned i = 0; i < _rotor_count; i++) { - _tmp_array[i] = _rotors[i].pitch_scale; - } - - minimize_saturation(_tmp_array, outputs, _saturation_status); - - // Mix yaw independently - mix_yaw(yaw, outputs); -} - -void MultirotorMixer::mix_yaw(float yaw, float *outputs) -{ - // Add yaw to outputs - for (unsigned i = 0; i < _rotor_count; i++) { - outputs[i] += yaw * _rotors[i].yaw_scale; - - // Yaw will be used to unsaturate if needed - _tmp_array[i] = _rotors[i].yaw_scale; - } - - // Change yaw acceleration to unsaturate the outputs if needed (do not change roll/pitch), - // and allow some yaw response at maximum thrust - minimize_saturation(_tmp_array, outputs, _saturation_status, 0.f, 1.15f); - - for (unsigned i = 0; i < _rotor_count; i++) { - _tmp_array[i] = _rotors[i].thrust_scale; - } - - // reduce thrust only - minimize_saturation(_tmp_array, outputs, _saturation_status, 0.f, 1.f, true); -} - -unsigned -MultirotorMixer::mix(float *outputs, unsigned space) -{ - if (space < _rotor_count) { - return 0; - } - - float roll = math::constrain(get_control(0, 0), -1.0f, 1.0f); - float pitch = math::constrain(get_control(0, 1), -1.0f, 1.0f); - float yaw = math::constrain(get_control(0, 2), -1.0f, 1.0f); - float thrust = math::constrain(get_control(0, 3), 0.0f, 1.0f); - - // clean out class variable used to capture saturation - _saturation_status.value = 0; - - // Do the mixing using the strategy given by the current Airmode configuration - switch (_airmode) { - case Airmode::roll_pitch: - mix_airmode_rp(roll, pitch, yaw, thrust, outputs); - break; - - case Airmode::roll_pitch_yaw: - mix_airmode_rpy(roll, pitch, yaw, thrust, outputs); - break; - - case Airmode::disabled: - default: // just in case: default to disabled - mix_airmode_disabled(roll, pitch, yaw, thrust, outputs); - break; - } - - // Apply thrust model and scale outputs to range [idle_speed, 1]. - // At this point the outputs are expected to be in [0, 1], but they can be outside, for example - // if a roll command exceeds the motor band limit. - for (unsigned i = 0; i < _rotor_count; i++) { - // Implement simple model for static relationship between applied motor pwm and motor thrust - // model: thrust = (1 - _thrust_factor) * PWM + _thrust_factor * PWM^2 - if (_thrust_factor > 0.0f) { - outputs[i] = -(1.0f - _thrust_factor) / (2.0f * _thrust_factor) + sqrtf((1.0f - _thrust_factor) * - (1.0f - _thrust_factor) / (4.0f * _thrust_factor * _thrust_factor) + (outputs[i] < 0.0f ? 0.0f : outputs[i] / - _thrust_factor)); - } - - outputs[i] = math::constrain((2.f * outputs[i] - 1.f), -1.f, 1.f); - } - - // Slew rate limiting and saturation checking - for (unsigned i = 0; i < _rotor_count; i++) { - bool clipping_high = false; - bool clipping_low_roll_pitch = false; - bool clipping_low_yaw = false; - - // Check for saturation against static limits. - // We only check for low clipping if airmode is disabled (or yaw - // clipping if airmode==roll/pitch), since in all other cases thrust will - // be reduced or boosted and we can keep the integrators enabled, which - // leads to better tracking performance. - if (outputs[i] < -0.99f) { - if (_airmode == Airmode::disabled) { - clipping_low_roll_pitch = true; - clipping_low_yaw = true; - - } else if (_airmode == Airmode::roll_pitch) { - clipping_low_yaw = true; - } - } - - // check for saturation against slew rate limits - if (_delta_out_max > 0.0f) { - float delta_out = outputs[i] - _outputs_prev[i]; - - if (delta_out > _delta_out_max) { - outputs[i] = _outputs_prev[i] + _delta_out_max; - clipping_high = true; - - } else if (delta_out < -_delta_out_max) { - outputs[i] = _outputs_prev[i] - _delta_out_max; - clipping_low_roll_pitch = true; - clipping_low_yaw = true; - } - } - - _outputs_prev[i] = outputs[i]; - - // update the saturation status report - update_saturation_status(i, clipping_high, clipping_low_roll_pitch, clipping_low_yaw); - } - - // this will force the caller of the mixer to always supply new slew rate values, otherwise no slew rate limiting will happen - _delta_out_max = 0.0f; - - return _rotor_count; -} - -/* - * This function update the control saturation status report using the following inputs: - * - * index: 0 based index identifying the motor that is saturating - * clipping_high: true if the motor demand is being limited in the positive direction - * clipping_low_roll_pitch: true if the motor demand is being limited in the negative direction (roll/pitch) - * clipping_low_yaw: true if the motor demand is being limited in the negative direction (yaw) -*/ -void -MultirotorMixer::update_saturation_status(unsigned index, bool clipping_high, bool clipping_low_roll_pitch, - bool clipping_low_yaw) -{ - // The motor is saturated at the upper limit - // check which control axes and which directions are contributing - if (clipping_high) { - if (_rotors[index].roll_scale > 0.0f) { - // A positive change in roll will increase saturation - _saturation_status.flags.roll_pos = true; - - } else if (_rotors[index].roll_scale < 0.0f) { - // A negative change in roll will increase saturation - _saturation_status.flags.roll_neg = true; - } - - // check if the pitch input is saturating - if (_rotors[index].pitch_scale > 0.0f) { - // A positive change in pitch will increase saturation - _saturation_status.flags.pitch_pos = true; - - } else if (_rotors[index].pitch_scale < 0.0f) { - // A negative change in pitch will increase saturation - _saturation_status.flags.pitch_neg = true; - } - - // check if the yaw input is saturating - if (_rotors[index].yaw_scale > 0.0f) { - // A positive change in yaw will increase saturation - _saturation_status.flags.yaw_pos = true; - - } else if (_rotors[index].yaw_scale < 0.0f) { - // A negative change in yaw will increase saturation - _saturation_status.flags.yaw_neg = true; - } - - // A positive change in thrust will increase saturation (Z neg is up) - _saturation_status.flags.thrust_neg = true; - } - - // The motor is saturated at the lower limit - // check which control axes and which directions are contributing - if (clipping_low_roll_pitch) { - // check if the roll input is saturating - if (_rotors[index].roll_scale > 0.0f) { - // A negative change in roll will increase saturation - _saturation_status.flags.roll_neg = true; - - } else if (_rotors[index].roll_scale < 0.0f) { - // A positive change in roll will increase saturation - _saturation_status.flags.roll_pos = true; - } - - // check if the pitch input is saturating - if (_rotors[index].pitch_scale > 0.0f) { - // A negative change in pitch will increase saturation - _saturation_status.flags.pitch_neg = true; - - } else if (_rotors[index].pitch_scale < 0.0f) { - // A positive change in pitch will increase saturation - _saturation_status.flags.pitch_pos = true; - } - - // A negative change in thrust will increase saturation (Z pos is down) - _saturation_status.flags.thrust_pos = true; - } - - if (clipping_low_yaw) { - // check if the yaw input is saturating - if (_rotors[index].yaw_scale > 0.0f) { - // A negative change in yaw will increase saturation - _saturation_status.flags.yaw_neg = true; - - } else if (_rotors[index].yaw_scale < 0.0f) { - // A positive change in yaw will increase saturation - _saturation_status.flags.yaw_pos = true; - } - } - - _saturation_status.flags.valid = true; -} diff --git a/src/lib/mixer/MultirotorMixer/MultirotorMixer.hpp b/src/lib/mixer/MultirotorMixer/MultirotorMixer.hpp deleted file mode 100644 index d8e7363417..0000000000 --- a/src/lib/mixer/MultirotorMixer/MultirotorMixer.hpp +++ /dev/null @@ -1,255 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012-2019 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 - -/** - * Supported multirotor geometries. - * - * Values are generated by the px_generate_mixers.py script and placed to mixer_multirotor_normalized.generated.h - */ -typedef uint8_t MultirotorGeometryUnderlyingType; -enum class MultirotorGeometry : MultirotorGeometryUnderlyingType; - -/** - * Multi-rotor mixer for pre-defined vehicle geometries. - * - * Collects four inputs (roll, pitch, yaw, thrust) and mixes them to - * a set of outputs based on the configured geometry. - */ -class MultirotorMixer : public Mixer -{ -public: - /** - - * Precalculated rotor mix. - */ - struct Rotor { - float roll_scale; /**< scales roll for this rotor */ - float pitch_scale; /**< scales pitch for this rotor */ - float yaw_scale; /**< scales yaw for this rotor */ - float thrust_scale; /**< scales thrust for this rotor */ - }; - - /** - * Constructor. - * - * @param control_cb Callback invoked to read inputs. - * @param cb_handle Passed to control_cb. - * @param geometry The selected geometry. - * @param roll_scale Scaling factor applied to roll inputs - * compared to thrust. - * @param pitch_scale Scaling factor applied to pitch inputs - * compared to thrust. - * @param yaw_wcale Scaling factor applied to yaw inputs compared - * to thrust. - * @param idle_speed Minimum rotor control output value; usually - * tuned to ensure that rotors never stall at the - * low end of their control range. - */ - MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle, MultirotorGeometry geometry); - - /** - * Constructor (for testing). - * - * @param control_cb Callback invoked to read inputs. - * @param cb_handle Passed to control_cb. - * @param rotors control allocation matrix - * @param rotor_count length of rotors array (= number of motors) - */ - MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle, const Rotor *rotors, unsigned rotor_count); - virtual ~MultirotorMixer(); - - // no copy, assignment, move, move assignment - MultirotorMixer(const MultirotorMixer &) = delete; - MultirotorMixer &operator=(const MultirotorMixer &) = delete; - MultirotorMixer(MultirotorMixer &&) = delete; - MultirotorMixer &operator=(MultirotorMixer &&) = delete; - - /** - * Factory method. - * - * Given a pointer to a buffer containing a text description of the mixer, - * returns a pointer to a new instance of the mixer. - * - * @param control_cb The callback to invoke when fetching a - * control value. - * @param cb_handle Handle passed to the control callback. - * @param buf Buffer containing a text description of - * the mixer. - * @param buflen Length of the buffer in bytes, adjusted - * to reflect the bytes consumed. - * @return A new MultirotorMixer instance, or nullptr - * if the text format is bad. - */ - static MultirotorMixer *from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, - unsigned &buflen); - - unsigned mix(float *outputs, unsigned space) override; - - uint16_t get_saturation_status() override { return _saturation_status.value; } - - void groups_required(uint32_t &groups) override { groups |= (1 << 0); } - - /** - * @brief Update slew rate parameter. This tells the multicopter mixer - * the maximum allowed change of the output values per cycle. - * The value is only valid for one cycle, in order to have continuous - * slew rate limiting this function needs to be called before every call - * to mix(). - * - * @param[in] delta_out_max Maximum delta output. - * - */ - void set_max_delta_out_once(float delta_out_max) override { _delta_out_max = delta_out_max; } - - unsigned set_trim(float trim) override { return _rotor_count; } - unsigned get_trim(float *trim) override { return _rotor_count; } - - /** - * @brief Sets the thrust factor used to calculate mapping from desired thrust to motor control signal output. - * - * @param[in] val The value - */ - void set_thrust_factor(float val) override { _thrust_factor = math::constrain(val, 0.0f, 1.0f); } - - void set_airmode(Airmode airmode) override { _airmode = airmode; } - - unsigned get_multirotor_count() override { return _rotor_count; } - - union saturation_status { - struct { - uint16_t valid : 1; // 0 - true when the saturation status is used - uint16_t motor_pos : 1; // 1 - true when any motor has saturated in the positive direction - uint16_t motor_neg : 1; // 2 - true when any motor has saturated in the negative direction - uint16_t roll_pos : 1; // 3 - true when a positive roll demand change will increase saturation - uint16_t roll_neg : 1; // 4 - true when a negative roll demand change will increase saturation - uint16_t pitch_pos : 1; // 5 - true when a positive pitch demand change will increase saturation - uint16_t pitch_neg : 1; // 6 - true when a negative pitch demand change will increase saturation - uint16_t yaw_pos : 1; // 7 - true when a positive yaw demand change will increase saturation - uint16_t yaw_neg : 1; // 8 - true when a negative yaw demand change will increase saturation - uint16_t thrust_pos : 1; // 9 - true when a positive thrust demand change will increase saturation - uint16_t thrust_neg : 1; //10 - true when a negative thrust demand change will increase saturation - } flags; - uint16_t value; - }; - -private: - /** - * Computes the gain k by which desaturation_vector has to be multiplied - * in order to unsaturate the output that has the greatest saturation. - * @see also minimize_saturation(). - * - * @return desaturation gain - */ - float compute_desaturation_gain(const float *desaturation_vector, const float *outputs, saturation_status &sat_status, - float min_output, float max_output) const; - - /** - * Minimize the saturation of the actuators by adding or substracting a fraction of desaturation_vector. - * desaturation_vector is the vector that added to the output outputs, modifies the thrust or angular - * acceleration on a specific axis. - * For example, if desaturation_vector is given to slide along the vertical thrust axis (thrust_scale), the - * saturation will be minimized by shifting the vertical thrust setpoint, without changing the - * roll/pitch/yaw accelerations. - * - * Note that as we only slide along the given axis, in extreme cases outputs can still contain values - * outside of [min_output, max_output]. - * - * @param desaturation_vector vector that is added to the outputs, e.g. thrust_scale - * @param outputs output vector that is modified - * @param sat_status saturation status output - * @param min_output minimum desired value in outputs - * @param max_output maximum desired value in outputs - * @param reduce_only if true, only allow to reduce (substract) a fraction of desaturation_vector - */ - void minimize_saturation(const float *desaturation_vector, float *outputs, saturation_status &sat_status, - float min_output = 0.f, float max_output = 1.f, bool reduce_only = false) const; - - /** - * Mix roll, pitch, yaw, thrust and set the outputs vector. - * - * Desaturation behavior: airmode for roll/pitch: - * thrust is increased/decreased as much as required to meet the demanded roll/pitch. - * Yaw is not allowed to increase the thrust, @see mix_yaw() for the exact behavior. - */ - inline void mix_airmode_rp(float roll, float pitch, float yaw, float thrust, float *outputs); - - /** - * Mix roll, pitch, yaw, thrust and set the outputs vector. - * - * Desaturation behavior: full airmode for roll/pitch/yaw: - * thrust is increased/decreased as much as required to meet demanded the roll/pitch/yaw, - * while giving priority to roll and pitch over yaw. - */ - inline void mix_airmode_rpy(float roll, float pitch, float yaw, float thrust, float *outputs); - - /** - * Mix roll, pitch, yaw, thrust and set the outputs vector. - * - * Desaturation behavior: no airmode, thrust is NEVER increased to meet the demanded - * roll/pitch/yaw. Instead roll/pitch/yaw is reduced as much as needed. - * Thrust can be reduced to unsaturate the upper side. - * @see mix_yaw() for the exact yaw behavior. - */ - inline void mix_airmode_disabled(float roll, float pitch, float yaw, float thrust, float *outputs); - - /** - * Mix yaw by updating an existing output vector (that already contains roll/pitch/thrust). - * - * Desaturation behavior: thrust is allowed to be decreased up to 15% in order to allow - * some yaw control on the upper end. On the lower end thrust will never be increased, - * but yaw is decreased as much as required. - * - * @param yaw demanded yaw - * @param outputs output vector that is updated - */ - inline void mix_yaw(float yaw, float *outputs); - - void update_saturation_status(unsigned index, bool clipping_high, bool clipping_low_roll_pitch, bool clipping_low_yaw); - - float _delta_out_max{0.0f}; - float _thrust_factor{0.0f}; - - Airmode _airmode{Airmode::disabled}; - - saturation_status _saturation_status{}; - - unsigned _rotor_count; - const Rotor *_rotors; - - float *_outputs_prev{nullptr}; - float *_tmp_array{nullptr}; -}; diff --git a/src/lib/mixer/MultirotorMixer/geometries/dodeca_bottom_cox.toml b/src/lib/mixer/MultirotorMixer/geometries/dodeca_bottom_cox.toml deleted file mode 100644 index 7470bff67b..0000000000 --- a/src/lib/mixer/MultirotorMixer/geometries/dodeca_bottom_cox.toml +++ /dev/null @@ -1,40 +0,0 @@ -# Generic Dodecacopter in X coax configuration, bottom half - -[info] -key = "6a" -description = "Generic Dodecacopter in X coax configuration, bottom half" - -[rotor_default] -axis = [0.0, 0.0, -1.0] -Ct = 1.0 -Cm = 0.05 - -[[rotors]] -name = "mid_right_bottom" -position = [0.0, 1.0, 0.1] -direction = "CCW" - -[[rotors]] -name = "mid_left_bottom" -position = [0.0, -1.0, 0.1] -direction = "CW" - -[[rotors]] -name = "front_left_bottom" -position = [0.866025, -0.5, 0.1] -direction = "CCW" - -[[rotors]] -name = "rear_right_bottom" -position = [-0.866025, 0.5, 0.1] -direction = "CW" - -[[rotors]] -name = "front_right_bottom" -position = [0.866025, 0.5, 0.1] -direction = "CW" - -[[rotors]] -name = "rear_left_bottom" -position = [-0.866025, -0.5, 0.1] -direction = "CCW" \ No newline at end of file diff --git a/src/lib/mixer/MultirotorMixer/geometries/dodeca_top_cox.toml b/src/lib/mixer/MultirotorMixer/geometries/dodeca_top_cox.toml deleted file mode 100644 index e875240bac..0000000000 --- a/src/lib/mixer/MultirotorMixer/geometries/dodeca_top_cox.toml +++ /dev/null @@ -1,40 +0,0 @@ -# Generic Dodecacopter in X coax configuration, top half - -[info] -key = "6m" -description = "Generic Dodecacopter in X coax configuration, top half" - -[rotor_default] -axis = [0.0, 0.0, -1.0] -Ct = 1.0 -Cm = 0.05 - -[[rotors]] -name = "mid_right_top" -position = [0.0, 1.0, -0.1] -direction = "CW" - -[[rotors]] -name = "mid_left_top" -position = [0.0, -1.0, -0.1] -direction = "CCW" - -[[rotors]] -name = "front_left_top" -position = [0.866025, -0.5, -0.1] -direction = "CW" - -[[rotors]] -name = "rear_right_top" -position = [-0.866025, 0.5, -0.1] -direction = "CCW" - -[[rotors]] -name = "front_right_top" -position = [0.866025, 0.5, -0.1] -direction = "CCW" - -[[rotors]] -name = "rear_left_top" -position = [-0.866025, -0.5, -0.1] -direction = "CW" \ No newline at end of file diff --git a/src/lib/mixer/MultirotorMixer/geometries/hex_cox.toml b/src/lib/mixer/MultirotorMixer/geometries/hex_cox.toml deleted file mode 100644 index a344c16e1f..0000000000 --- a/src/lib/mixer/MultirotorMixer/geometries/hex_cox.toml +++ /dev/null @@ -1,40 +0,0 @@ -# Generic Hexacopter in coaxial configuration - -[info] -key = "6c" -description = "Generic Hexacopter in coaxial configuration" - -[rotor_default] -axis = [0.0, 0.0, -1.0] -Ct = 1.0 -Cm = 0.05 - -[[rotors]] -name = "front_right_top" -position = [0.5, 0.866, -0.1] -direction = "CW" - -[[rotors]] -name = "front_right_bottom" -position = [0.5, 0.866, 0.1] -direction = "CCW" - -[[rotors]] -name = "rear_top" -position = [-1.0, 0.0, -0.1] -direction = "CW" - -[[rotors]] -name = "rear_bottom" -position = [-1.0, 0.0, 0.1] -direction = "CCW" - -[[rotors]] -name = "front_left_top" -position = [0.5, -0.866, -0.1] -direction = "CW" - -[[rotors]] -name = "front_left_bottom" -position = [0.5, -0.866, 0.1] -direction = "CCW" diff --git a/src/lib/mixer/MultirotorMixer/geometries/hex_plus.toml b/src/lib/mixer/MultirotorMixer/geometries/hex_plus.toml deleted file mode 100644 index 6b86cb40b6..0000000000 --- a/src/lib/mixer/MultirotorMixer/geometries/hex_plus.toml +++ /dev/null @@ -1,40 +0,0 @@ -# Generic Hexacopter in + configuration - -[info] -key = "6+" -description = "Generic Hexacopter in + configuration" - -[rotor_default] -axis = [0.0, 0.0, -1.0] -Ct = 1.0 -Cm = 0.05 - -[[rotors]] -name = "front" -position = [1.0, 0.0, 0.0] -direction = "CW" - -[[rotors]] -name = "rear" -position = [-1.0, 0.0, 0.0] -direction = "CCW" - -[[rotors]] -name = "rear_left" -position = [-0.5, -0.866025, 0.0] -direction = "CW" - -[[rotors]] -name = "front_right" -position = [0.5, 0.866025, 0.0] -direction = "CCW" - -[[rotors]] -name = "front_left" -position = [0.5, -0.866025, 0.0] -direction = "CCW" - -[[rotors]] -name = "rear_right" -position = [-0.5, 0.866025, 0.0] -direction = "CW" \ No newline at end of file diff --git a/src/lib/mixer/MultirotorMixer/geometries/hex_t.toml b/src/lib/mixer/MultirotorMixer/geometries/hex_t.toml deleted file mode 100644 index fc8c32b964..0000000000 --- a/src/lib/mixer/MultirotorMixer/geometries/hex_t.toml +++ /dev/null @@ -1,40 +0,0 @@ -# Generic Hexacopter in T configuration - -[info] -key = "6t" -description = "Generic Hexacopter in T configuration" - -[rotor_default] -axis = [0.0, 0.0, -1.0] -Ct = 1.0 -Cm = 0.05 - -[[rotors]] -name = "front_right_top" -position = [0.729, 0.684, 0.1] -direction = "CW" - -[[rotors]] -name = "front_right_bottom" -position = [0.729, 0.684, -0.1] -direction = "CCW" - -[[rotors]] -name = "rear_top" -position = [-1.0, 0.0, 0.1] -direction = "CW" - -[[rotors]] -name = "rear_bottom" -position = [-1.0, 0.0, -0.1] -direction = "CCW" - -[[rotors]] -name = "front_left_top" -position = [0.729, -0.684, 0.1] -direction = "CW" - -[[rotors]] -name = "front_left_bottom" -position = [0.729, -0.684, -0.1] -direction = "CCW" diff --git a/src/lib/mixer/MultirotorMixer/geometries/hex_x.toml b/src/lib/mixer/MultirotorMixer/geometries/hex_x.toml deleted file mode 100644 index a546a83f72..0000000000 --- a/src/lib/mixer/MultirotorMixer/geometries/hex_x.toml +++ /dev/null @@ -1,40 +0,0 @@ -# Generic Hexacopter in X configuration - -[info] -key = "6x" -description = "Generic Hexacopter in X configuration" - -[rotor_default] -axis = [0.0, 0.0, -1.0] -Ct = 1.0 -Cm = 0.05 - -[[rotors]] -name = "mid_right" -position = [0.0, 1.0, 0.0] -direction = "CW" - -[[rotors]] -name = "mid_left" -position = [0.0, -1.0, 0.0] -direction = "CCW" - -[[rotors]] -name = "front_left" -position = [0.866025, -0.5, 0.0] -direction = "CW" - -[[rotors]] -name = "rear_right" -position = [-0.866025, 0.5, 0.0] -direction = "CCW" - -[[rotors]] -name = "front_right" -position = [0.866025, 0.5, 0.0] -direction = "CCW" - -[[rotors]] -name = "rear_left" -position = [-0.866025, -0.5, 0.0] -direction = "CW" \ No newline at end of file diff --git a/src/lib/mixer/MultirotorMixer/geometries/octa_cox.toml b/src/lib/mixer/MultirotorMixer/geometries/octa_cox.toml deleted file mode 100644 index 6c42bfa1f7..0000000000 --- a/src/lib/mixer/MultirotorMixer/geometries/octa_cox.toml +++ /dev/null @@ -1,50 +0,0 @@ -# Generic Octacopter in coax configuration - -[info] -key = "8c" -description = "GenericOctacopter in coax configuration" - -[rotor_default] -axis = [0.0, 0.0, -1.0] -Ct = 1.0 -Cm = 0.05 - -[[rotors]] -name = "front_right_top" -position = [0.707107, 0.707107, -0.1] -direction = "CCW" - -[[rotors]] -name = "front_left_top" -position = [0.707107, -0.707107, -0.1] -direction = "CW" - -[[rotors]] -name = "rear_left_top" -position = [-0.707107, -0.707107, -0.1] -direction = "CCW" - -[[rotors]] -name = "rear_right_top" -position = [-0.707107, 0.707107, -0.1] -direction = "CW" - -[[rotors]] -name = "front_left_bottom" -position = [0.707107, -0.707107, 0.1] -direction = "CCW" - -[[rotors]] -name = "front_right_bottom" -position = [0.707107, 0.707107, 0.1] -direction = "CW" - -[[rotors]] -name = "rear_right_bottom" -position = [-0.707107, 0.707107, 0.1] -direction = "CCW" - -[[rotors]] -name = "rear_left_bottom" -position = [-0.707107, -0.707107, 0.1] -direction = "CW" \ No newline at end of file diff --git a/src/lib/mixer/MultirotorMixer/geometries/octa_cox_wide.toml b/src/lib/mixer/MultirotorMixer/geometries/octa_cox_wide.toml deleted file mode 100644 index 785c6a4c0e..0000000000 --- a/src/lib/mixer/MultirotorMixer/geometries/octa_cox_wide.toml +++ /dev/null @@ -1,50 +0,0 @@ -# Generic Octacopter in wide coax configuration - -[info] -key = "8cw" -description = "Generic Octacopter in wide coax configuration" - -[rotor_default] -axis = [0.0, 0.0, -1.0] -Ct = 1.0 -Cm = 0.05 - -[[rotors]] -name = "front_right_top" -position = [0.3746066, 0.927184, -0.1] -direction = "CCW" - -[[rotors]] -name = "front_left_top" -position = [0.3746066, -0.927184, -0.1] -direction = "CW" - -[[rotors]] -name = "rear_left_top" -position = [-0.62932, -0.777146, -0.1] -direction = "CCW" - -[[rotors]] -name = "rear_right_top" -position = [-0.62932, 0.777146, -0.1] -direction = "CW" - -[[rotors]] -name = "front_left_bottom" -position = [0.3746066, -0.927184, 0.1] -direction = "CCW" - -[[rotors]] -name = "front_right_bottom" -position = [0.3746066, 0.927184, 0.1] -direction = "CW" - -[[rotors]] -name = "rear_right_bottom" -position = [-0.62932, 0.777146, 0.1] -direction = "CCW" - -[[rotors]] -name = "rear_left_bottom" -position = [-0.62932, -0.777146, 0.1] -direction = "CW" \ No newline at end of file diff --git a/src/lib/mixer/MultirotorMixer/geometries/octa_plus.toml b/src/lib/mixer/MultirotorMixer/geometries/octa_plus.toml deleted file mode 100644 index 7c13bc728d..0000000000 --- a/src/lib/mixer/MultirotorMixer/geometries/octa_plus.toml +++ /dev/null @@ -1,50 +0,0 @@ -# Generic Octacopter in + configuration - -[info] -key = "8+" -description = "Generic Octacopter in + configuration" - -[rotor_default] -axis = [0.0, 0.0, -1.0] -Ct = 1.0 -Cm = 0.05 - -[[rotors]] -name = "front" -position = [1.0, 0.0, 0.0] -direction = "CW" - -[[rotors]] -name = "rear" -position = [-1.0, 0.0, 0.0] -direction = "CW" - -[[rotors]] -name = "front_right" -position = [0.707107, 0.707107, 0.0] -direction = "CCW" - -[[rotors]] -name = "rear_right" -position = [-0.707107, 0.707107, 0.0] -direction = "CCW" - -[[rotors]] -name = "front_left" -position = [0.707107, -0.707107, 0.0] -direction = "CCW" - -[[rotors]] -name = "rear_left" -position = [-0.707107, -0.707107, 0.0] -direction = "CCW" - -[[rotors]] -name = "left" -position = [0.0, -1.0, 0.0] -direction = "CW" - -[[rotors]] -name = "right" -position = [0.0, 1.0, 0.0] -direction = "CW" \ No newline at end of file diff --git a/src/lib/mixer/MultirotorMixer/geometries/octa_x.toml b/src/lib/mixer/MultirotorMixer/geometries/octa_x.toml deleted file mode 100644 index e85403b032..0000000000 --- a/src/lib/mixer/MultirotorMixer/geometries/octa_x.toml +++ /dev/null @@ -1,50 +0,0 @@ -# Generic Octacopter in X configuration - -[info] -key = "8x" -description = "Generic Octacopter in X configuration" - -[rotor_default] -axis = [0.0, 0.0, -1.0] -Ct = 1.0 -Cm = 0.05 - -[[rotors]] -name = "front_right" -position = [0.9238795, 0.3826834, 0.0] -direction = "CW" - -[[rotors]] -name = "rear_left" -position = [-0.9238795, -0.3826834, 0.0] -direction = "CW" - -[[rotors]] -name = "mid_front_right" -position = [0.3826834, 0.9238795, 0.0] -direction = "CCW" - -[[rotors]] -name = "rear_right" -position = [-0.9238795, 0.3826834, 0.0] -direction = "CCW" - -[[rotors]] -name = "front_left" -position = [0.9238795, -0.3826834, 0.0] -direction = "CCW" - -[[rotors]] -name = "mid_rear_left" -position = [-0.3826834, -0.9238795, 0.0] -direction = "CCW" - -[[rotors]] -name = "mid_front_left" -position = [0.3826834, -0.9238795, 0.0] -direction = "CW" - -[[rotors]] -name = "mid_rear_right" -position = [-0.3826834, 0.9238795, 0.0] -direction = "CW" \ No newline at end of file diff --git a/src/lib/mixer/MultirotorMixer/geometries/quad_deadcat.toml b/src/lib/mixer/MultirotorMixer/geometries/quad_deadcat.toml deleted file mode 100644 index 1e7febaef4..0000000000 --- a/src/lib/mixer/MultirotorMixer/geometries/quad_deadcat.toml +++ /dev/null @@ -1,30 +0,0 @@ -# SK450 DeadCat Quadcopter. -# Same geometry as quad_wide, except CG is located at intersection of rear arms, so front motors are more loaded. - -[info] -key = "4dc" -description = "SK450 DeadCat Quadcopter, CG at intersection of rear arms" - -[rotor_default] -direction = "CW" -axis = [0.0, 0.0, -1.0] -Ct = 1.0 -Cm = 0.01 - -[[rotors]] -name = "front_right" -position = [0.1155, 0.245, 0.0] -direction = "CCW" - -[[rotors]] -name = "rear_left" -position = [-0.1875, -0.1875, 0.0] -direction = "CCW" - -[[rotors]] -name = "front_left" -position = [0.1155, -0.245, 0.0] - -[[rotors]] -name = "rear_right" -position = [-0.1875, 0.1875, 0.0] diff --git a/src/lib/mixer/MultirotorMixer/geometries/quad_h.toml b/src/lib/mixer/MultirotorMixer/geometries/quad_h.toml deleted file mode 100644 index e58b5a5ed6..0000000000 --- a/src/lib/mixer/MultirotorMixer/geometries/quad_h.toml +++ /dev/null @@ -1,30 +0,0 @@ -# Generic Quadcopter in H configuration - -[info] -key = "4h" -description = "Generic Quadcopter in H configuration" - -[rotor_default] -axis = [0.0, 0.0, -1.0] -Ct = 1.0 -Cm = 0.05 - -[[rotors]] -name = "front_right" -position = [0.707107, 0.707107, 0.0] -direction = "CW" - -[[rotors]] -name = "rear_left" -position = [-0.707107, -0.707107, 0.0] -direction = "CW" - -[[rotors]] -name = "front_left" -position = [0.707107, -0.707107, 0.0] -direction = "CCW" - -[[rotors]] -name = "rear_right" -position = [-0.707107, 0.707107, 0.0] -direction = "CCW" \ No newline at end of file diff --git a/src/lib/mixer/MultirotorMixer/geometries/quad_plus.toml b/src/lib/mixer/MultirotorMixer/geometries/quad_plus.toml deleted file mode 100644 index e28f8db7f0..0000000000 --- a/src/lib/mixer/MultirotorMixer/geometries/quad_plus.toml +++ /dev/null @@ -1,29 +0,0 @@ -# Generic Quadcopter in + configuration - -[info] -key = "4+" -description = "Generic Quadcopter in + configuration" - -[rotor_default] -direction = "CW" -axis = [0.0, 0.0, -1.0] -Ct = 1.0 -Cm = 0.05 - -[[rotors]] -name = "right" -position = [0.0, 1.0, 0.0] -direction = "CCW" - -[[rotors]] -name = "left" -position = [0.0, -1.0, 0.0] -direction = "CCW" - -[[rotors]] -name = "front" -position = [1.0, 0.0, 0.0] - -[[rotors]] -name = "rear" -position = [-1.0, 0.0, 0.0] diff --git a/src/lib/mixer/MultirotorMixer/geometries/quad_vtail.toml b/src/lib/mixer/MultirotorMixer/geometries/quad_vtail.toml deleted file mode 100644 index 3b934018db..0000000000 --- a/src/lib/mixer/MultirotorMixer/geometries/quad_vtail.toml +++ /dev/null @@ -1,33 +0,0 @@ -# Quadcopter in Y configuration with rear props tilted at 45 degrees - -[info] -key = "4vt" -description = "Quadcopter in Y configuration with rear props tilted at 45 degrees" - -[rotor_default] -direction = "CW" -axis = [0.0, 0.0, -1.0] -Ct = 1.0 -Cm = 0.05 - -[[rotors]] -name = "front_right" -position = [0.2, 0.2, 0.0] -direction = "CW" - -[[rotors]] -name = "rear_left" -position = [-0.3, -0.1, -0.1] -axis = [0.0, 0.707106, -0.707106] -direction = "CW" - -[[rotors]] -name = "front_left" -position = [0.2, -0.2, 0.0] -direction = "CCW" - -[[rotors]] -name = "rear_right" -position = [-0.3, 0.1, -0.1] -axis = [0.0, -0.707106, -0.707106] -direction = "CCW" diff --git a/src/lib/mixer/MultirotorMixer/geometries/quad_wide.toml b/src/lib/mixer/MultirotorMixer/geometries/quad_wide.toml deleted file mode 100644 index d2fde7861b..0000000000 --- a/src/lib/mixer/MultirotorMixer/geometries/quad_wide.toml +++ /dev/null @@ -1,30 +0,0 @@ -# Generic Quadcopter in wide configuration - -[info] -key = "4w" -description = "Quadcopter in wide configuration. Same geometry as SK450 Deadcat except the CG is moved backward to load all motors equally" - -[rotor_default] -axis = [0.0, 0.0, -1.0] -Ct = 1.0 -Cm = 0.05 - -[[rotors]] -name = "front_right" -position = [0.1515, 0.245, 0.0] -direction = "CCW" - -[[rotors]] -name = "rear_left" -position = [-0.1515, -0.1875, 0.0] -direction = "CCW" - -[[rotors]] -name = "front_left" -position = [0.1515, -0.245, 0.0] -direction = "CW" - -[[rotors]] -name = "rear_right" -position = [-0.1515, 0.1875, 0.0] -direction = "CW" \ No newline at end of file diff --git a/src/lib/mixer/MultirotorMixer/geometries/quad_x.toml b/src/lib/mixer/MultirotorMixer/geometries/quad_x.toml deleted file mode 100644 index b4cf484c21..0000000000 --- a/src/lib/mixer/MultirotorMixer/geometries/quad_x.toml +++ /dev/null @@ -1,29 +0,0 @@ -# Generic Quadcopter in X configuration - -[info] -key = "4x" -description = "Generic Quadcopter in X configuration" - -[rotor_default] -direction = "CW" -axis = [0.0, 0.0, -1.0] -Ct = 1.0 -Cm = 0.05 - -[[rotors]] -name = "front_right" -position = [0.707107, 0.707107, 0.0] -direction = "CCW" - -[[rotors]] -name = "rear_left" -position = [-0.707107, -0.707107, 0.0] -direction = "CCW" - -[[rotors]] -name = "front_left" -position = [0.707107, -0.707107, 0.0] - -[[rotors]] -name = "rear_right" -position = [-0.707107, 0.707107, 0.0] diff --git a/src/lib/mixer/MultirotorMixer/geometries/quad_x_cw.toml b/src/lib/mixer/MultirotorMixer/geometries/quad_x_cw.toml deleted file mode 100644 index 1af7882b23..0000000000 --- a/src/lib/mixer/MultirotorMixer/geometries/quad_x_cw.toml +++ /dev/null @@ -1,30 +0,0 @@ -# Generic Quadcopter in X configuration -# with clock-wise motor numbering - -[info] -key = "4xcw" -description = "Quadcopter in X configuration with clock-wise motor numbering" - -[rotor_default] -direction = "CW" -axis = [0.0, 0.0, -1.0] -Ct = 1.0 -Cm = 0.05 - -[[rotors]] -name = "front_right" -position = [0.707107, 0.707107, 0.0] -direction = "CCW" - -[[rotors]] -name = "rear_right" -position = [-0.707107, 0.707107, 0.0] - -[[rotors]] -name = "rear_left" -position = [-0.707107, -0.707107, 0.0] -direction = "CCW" - -[[rotors]] -name = "front_left" -position = [0.707107, -0.707107, 0.0] diff --git a/src/lib/mixer/MultirotorMixer/geometries/quad_x_pusher.toml b/src/lib/mixer/MultirotorMixer/geometries/quad_x_pusher.toml deleted file mode 100644 index a63ff0652b..0000000000 --- a/src/lib/mixer/MultirotorMixer/geometries/quad_x_pusher.toml +++ /dev/null @@ -1,37 +0,0 @@ -# Quadcopter in X configuration, -# with added pusher motor in the back - -[info] -key = "4x1p" -description = "Quadcopter in X configuration, with added pusher motor in the back" - -[rotor_default] -direction = "CW" -axis = [0.0, 0.0, -1.0] -Ct = 1.0 -Cm = 0.05 - -[[rotors]] -name = "front_right" -position = [1.0, 1.0, 0.0] -direction = "CCW" - -[[rotors]] -name = "rear_left" -position = [-1.0, -1.0, 0.0] -direction = "CCW" - -[[rotors]] -name = "front_left" -position = [1.0, -1.0, 0.0] - -[[rotors]] -name = "rear_right" -position = [-1.0, 1.0, 0.0] - -[[rotors]] -name = "pusher" -position = [-1.0, 0.0, 0.0] -axis = [1.0, 0.0, 0.0] -Ct = 2.0 -Cm = 0.0 diff --git a/src/lib/mixer/MultirotorMixer/geometries/quad_y.toml b/src/lib/mixer/MultirotorMixer/geometries/quad_y.toml deleted file mode 100644 index b75756c8d9..0000000000 --- a/src/lib/mixer/MultirotorMixer/geometries/quad_y.toml +++ /dev/null @@ -1,31 +0,0 @@ -# Quadcopter in Y configuration with coax rear props - -[info] -key = "4y" -description = "Quadcopter in Y configuration with coax rear props" - -[rotor_default] -direction = "CW" -axis = [0.0, 0.0, -1.0] -Ct = 1.0 -Cm = 0.05 - -[[rotors]] -name = "front_right" -position = [0.2, 0.2, 0.0] -direction = "CCW" - -[[rotors]] -name = "rear_top" -position = [-0.2, 0.0, -0.1] -direction = "CCW" - -[[rotors]] -name = "front_left" -position = [0.2, -0.2, 0.0] -direction = "CW" - -[[rotors]] -name = "rear_bottom" -position = [-0.2, 0.0, 0.1] -direction = "CW" diff --git a/src/lib/mixer/MultirotorMixer/geometries/tools/px_generate_mixers.py b/src/lib/mixer/MultirotorMixer/geometries/tools/px_generate_mixers.py deleted file mode 100755 index 932a265fb4..0000000000 --- a/src/lib/mixer/MultirotorMixer/geometries/tools/px_generate_mixers.py +++ /dev/null @@ -1,398 +0,0 @@ -#!/usr/bin/env python -############################################################################# -# -# Copyright (C) 2013-2016 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. -# -############################################################################# - -""" -px_generate_mixers.py -Generates c/cpp header/source files for multirotor mixers -from geometry descriptions files (.toml format) -""" -import sys - -try: - import toml -except ImportError as e: - print("Failed to import toml: " + str(e)) - print("") - print("You may need to install it using:") - print(" pip3 install --user toml") - print("") - sys.exit(1) - -try: - import numpy as np -except ImportError as e: - print("Failed to import numpy: " + str(e)) - print("") - print("You may need to install it using:") - print(" pip3 install --user numpy") - print("") - sys.exit(1) - -__author__ = "Julien Lecoeur" -__copyright__ = "Copyright (C) 2013-2017 PX4 Development Team." -__license__ = "BSD" -__email__ = "julien.lecoeur@gmail.com" - - -def parse_geometry_toml(filename): - ''' - Parses toml geometry file and returns a dictionary with curated list of rotors - ''' - import os - - # Load toml file - d = toml.load(filename) - - # Check info section - if 'info' not in d: - raise AttributeError('{}: Error, missing info section'.format(filename)) - - # Check info section - for field in ['key', 'description']: - if field not in d['info']: - raise AttributeError('{}: Error, unspecified info field "{}"'.format(filename, field)) - - # Use filename as mixer name - d['info']['name'] = os.path.basename(filename).split('.')[0].lower() - - # Store filename - d['info']['filename'] = filename - - # Check default rotor config - if 'rotor_default' in d: - default = d['rotor_default'] - else: - default = {} - - # Convert rotors - rotor_list = [] - if 'rotors' in d: - for r in d['rotors']: - # Make sure all fields are defined, fill missing with default - for field in ['name', 'position', 'axis', 'direction', 'Ct', 'Cm']: - if field not in r: - if field in default: - r[field] = default[field] - else: - raise AttributeError('{}: Error, unspecified field "{}" for rotor "{}"' - .format(filename, field, r['name'])) - - # Check direction field - r['direction'] = r['direction'].upper() - if r['direction'] not in ['CW', 'CCW']: - raise AttributeError('{}: Error, invalid direction value "{}" for rotor "{}"' - .format(filename, r['direction'], r['name'])) - - # Check vector3 fields - for field in ['position', 'axis']: - if len(r[field]) != 3: - raise AttributeError('{}: Error, field "{}" for rotor "{}"' - .format(filename, field, r['name']) + - ' must be an array of length 3') - - # Add rotor to list - rotor_list.append(r) - - # Clean dictionary - geometry = {'info': d['info'], - 'rotors': rotor_list} - - return geometry - -def torque_matrix(center, axis, dirs, Ct, Cm): - ''' - Compute torque generated by rotors - ''' - # normalize rotor axis - ax = axis / np.linalg.norm(axis, axis=1)[:, np.newaxis] - torque = Ct * np.cross(center, ax) - Cm * ax * dirs - return torque - -def geometry_to_torque_matrix(geometry): - ''' - Compute torque matrix Am and Bm from geometry dictionnary - Am is a 3xN matrix where N is the number of rotors - Each column is the torque generated by one rotor - ''' - Am = torque_matrix(center=np.array([rotor['position'] for rotor in geometry['rotors']]), - axis=np.array([rotor['axis'] for rotor in geometry['rotors']]), - dirs=np.array([[1.0 if rotor['direction'] == 'CCW' else -1.0] - for rotor in geometry['rotors']]), - Ct=np.array([[rotor['Ct']] for rotor in geometry['rotors']]), - Cm=np.array([[rotor['Cm']] for rotor in geometry['rotors']])).T - return Am - -def thrust_matrix(axis, Ct): - ''' - Compute thrust generated by rotors - ''' - # Normalize rotor axis - ax = axis / np.linalg.norm(axis, axis=1)[:, np.newaxis] - thrust = Ct * ax - return thrust - -def geometry_to_thrust_matrix(geometry): - ''' - Compute thrust matrix At from geometry dictionnary - At is a 3xN matrix where N is the number of rotors - Each column is the thrust generated by one rotor - ''' - At = thrust_matrix(axis=np.array([rotor['axis'] for rotor in geometry['rotors']]), - Ct=np.array([[rotor['Ct']] for rotor in geometry['rotors']])).T - - return At - -def geometry_to_mix(geometry): - ''' - Compute combined torque & thrust matrix A and mix matrix B from geometry dictionnary - - A is a 6xN matrix where N is the number of rotors - Each column is the torque and thrust generated by one rotor - - B is a Nx6 matrix where N is the number of rotors - Each column is the command to apply to the servos to get - roll torque, pitch torque, yaw torque, x thrust, y thrust, z thrust - ''' - # Combined torque & thrust matrix - At = geometry_to_thrust_matrix(geometry) - Am = geometry_to_torque_matrix(geometry) - A = np.vstack([Am, At]) - - # Mix matrix computed as pseudoinverse of A - B = np.linalg.pinv(A) - - return A, B - -def normalize_mix_px4(B): - ''' - Normalize mix for PX4 - This is for compatibility only and should ideally not be used - ''' - B_norm = np.linalg.norm(B, axis=0) - B_max = np.abs(B).max(axis=0) - B_sum = np.sum(B, axis=0) - - # Same scale on roll and pitch - B_norm[0] = max(B_norm[0], B_norm[1]) / np.sqrt(B.shape[0] / 2.0) - B_norm[1] = B_norm[0] - - # Scale yaw separately - B_norm[2] = B_max[2] - - # Same scale on x, y - B_norm[3] = max(B_max[3], B_max[4]) - B_norm[4] = B_norm[3] - - # Scale z thrust separately - B_norm[5] = - B_sum[5] / np.count_nonzero(B[:,5]) - - # Normalize - B_norm[np.abs(B_norm) < 1e-3] = 1 - B_px = (B / B_norm) - - return B_px - -def generate_mixer_multirotor_header(geometries_list, use_normalized_mix=False, use_6dof=False): - ''' - Generate C header file with same format as multi_tables.py - TODO: rewrite using templates (see generation of uORB headers) - ''' - from io import StringIO - buf = StringIO() - - # Print Header - buf.write(u"/*\n") - buf.write(u"* This file is automatically generated by px_generate_mixers.py - do not edit.\n") - buf.write(u"*/\n") - buf.write(u"\n") - buf.write(u"#ifndef _MIXER_MULTI_TABLES\n") - buf.write(u"#define _MIXER_MULTI_TABLES\n") - buf.write(u"\n") - - # Print enum - buf.write(u"enum class MultirotorGeometry : MultirotorGeometryUnderlyingType {\n") - for i, geometry in enumerate(geometries_list): - buf.write(u"\t{},{}// {} (text key {})\n".format( - geometry['info']['name'].upper(), ' ' * (max(0, 30 - len(geometry['info']['name']))), - geometry['info']['description'], geometry['info']['key'])) - buf.write(u"\n\tMAX_GEOMETRY\n") - buf.write(u"}; // enum class MultirotorGeometry\n\n") - - # Print mixer gains - buf.write(u"namespace {\n") - for geometry in geometries_list: - # Get desired mix matrix - if use_normalized_mix: - mix = geometry['mix']['B_px'] - else: - mix = geometry['mix']['B'] - - buf.write(u"static constexpr MultirotorMixer::Rotor _config_{}[] {{\n".format(geometry['info']['name'])) - - for row in mix: - if use_6dof: - # 6dof mixer - buf.write(u"\t{{ {:9f}, {:9f}, {:9f}, {:9f}, {:9f}, {:9f} }},\n".format( - row[0], row[1], row[2], - row[3], row[4], row[5])) - else: - # 4dof mixer - buf.write(u"\t{{ {:9f}, {:9f}, {:9f}, {:9f} }},\n".format( - row[0], row[1], row[2], - -row[5])) # Upward thrust is positive TODO: to remove this, adapt PX4 to use NED correctly - - buf.write(u"};\n\n") - - # Print geometry indeces - buf.write(u"static constexpr const MultirotorMixer::Rotor *_config_index[] {\n") - for geometry in geometries_list: - buf.write(u"\t&_config_{}[0],\n".format(geometry['info']['name'])) - buf.write(u"};\n\n") - - # Print geometry rotor counts - buf.write(u"static constexpr unsigned _config_rotor_count[] {\n") - for geometry in geometries_list: - buf.write(u"\t{}, /* {} */\n".format(len(geometry['rotors']), geometry['info']['name'])) - buf.write(u"};\n\n") - - # Print geometry key - buf.write(u"const char* _config_key[] {\n") - for geometry in geometries_list: - buf.write(u"\t\"{}\",\t/* {} */\n".format(geometry['info']['key'], geometry['info']['name'])) - buf.write(u"};\n\n") - - # Print footer - buf.write(u"} // anonymous namespace\n\n") - buf.write(u"#endif /* _MIXER_MULTI_TABLES */\n\n") - - return buf.getvalue() - - -if __name__ == '__main__': - import argparse - import glob - import os - - # Parse arguments - parser = argparse.ArgumentParser( - description='Convert geometry .toml files to mixer headers') - parser.add_argument('-d', dest='dir', - help='directory with geometry files') - parser.add_argument('-f', dest='files', - help="files to convert (use only without -d)", - nargs="+") - parser.add_argument('-o', dest='outputfile', - help='output header file') - parser.add_argument('--verbose', help='Print details on standard output', - action='store_true') - parser.add_argument('--normalize', help='Use normalized mixers (compatibility mode)', - action='store_true') - parser.add_argument('--sixdof', help='Use 6dof mixers', - action='store_true') - args = parser.parse_args() - - # Find toml files - if args.files is not None: - filenames = args.files - elif args.dir is not None: - filenames = glob.glob(os.path.join(args.dir, '*.toml')) - else: - parser.print_usage() - raise Exception("Missing input directory (-d) or list of geometry files (-f)") - - # List of geometries - geometries_list = [] - - for filename in filenames: - # Parse geometry file - geometry = parse_geometry_toml(filename) - - # Compute torque and thrust matrices - A, B = geometry_to_mix(geometry) - - # Normalize mixer - B_px = normalize_mix_px4(B) - - # Store matrices in geometry - geometry['mix'] = {'A': A, 'B': B, 'B_px': B_px} - - # Add to list - geometries_list.append(geometry) - - if args.verbose: - print('\nFilename') - print(filename) - print('\nGeometry') - print(geometry) - print('\nA:') - print(A.round(2)) - print('\nB:') - print(B.round(2)) - print('\nNormalized Mix (as in PX4):') - print(B_px.round(2)) - print('\n-----------------------------') - - # Check that there are no duplicated mixer names or keys - for i in range(len(geometries_list)): - name_i = geometries_list[i]['info']['name'] - key_i = geometries_list[i]['info']['key'] - - for j in range(i + 1, len(geometries_list)): - name_j = geometries_list[j]['info']['name'] - key_j = geometries_list[j]['info']['key'] - - # Mixers cannot share the same name - if name_i == name_j: - raise ValueError('Duplicated mixer name "{}" in files {} and {}'.format( - name_i, - geometries_list[i]['info']['filename'], - geometries_list[j]['info']['filename'])) - - # Mixers cannot share the same key - if key_i == key_j: - raise ValueError('Duplicated mixer key "{}" for mixers "{}" and "{}"'.format( - key_i, name_i, name_j)) - - # Generate header file - header = generate_mixer_multirotor_header(geometries_list, - use_normalized_mix=args.normalize, - use_6dof=args.sixdof) - - if args.outputfile is not None: - # Write header file - with open(args.outputfile, 'w') as fd: - fd.write(header) - else: - # Print to standard output - print(header) diff --git a/src/lib/mixer/MultirotorMixer/geometries/tri_y.toml b/src/lib/mixer/MultirotorMixer/geometries/tri_y.toml deleted file mode 100644 index b41dbd8cbd..0000000000 --- a/src/lib/mixer/MultirotorMixer/geometries/tri_y.toml +++ /dev/null @@ -1,23 +0,0 @@ -# Tri Y - -[info] -key = "3y" -description = "Tri Y" - -[rotor_default] -axis = [0.0, 0.0, -1.0] -Ct = 1.0 -Cm = 0.0 -direction = "CW" - -[[rotors]] -name = "front_right" -position = [0.5, 0.866025, 0.0] - -[[rotors]] -name = "front_left" -position = [0.5, -0.866025, 0.0] - -[[rotors]] -name = "rear" -position = [-1.0, 0.0, 0.0] \ No newline at end of file diff --git a/src/lib/mixer/MultirotorMixer/geometries/twin_engine.toml b/src/lib/mixer/MultirotorMixer/geometries/twin_engine.toml deleted file mode 100644 index 6a547a1654..0000000000 --- a/src/lib/mixer/MultirotorMixer/geometries/twin_engine.toml +++ /dev/null @@ -1,19 +0,0 @@ -# Twin engine - -[info] -key = "2-" -description = "Twin engine" - -[rotor_default] -axis = [0.0, 0.0, -1.0] -Ct = 1.0 -Cm = 0.0 -direction = "CW" - -[[rotors]] -name = "right" -position = [0.0, 1.0, 0.0] - -[[rotors]] -name = "left" -position = [0.0, -1.0, 0.0] \ No newline at end of file diff --git a/src/lib/mixer/MultirotorMixer/mixer_multirotor.py b/src/lib/mixer/MultirotorMixer/mixer_multirotor.py deleted file mode 100755 index 2e80049d38..0000000000 --- a/src/lib/mixer/MultirotorMixer/mixer_multirotor.py +++ /dev/null @@ -1,389 +0,0 @@ -#!/usr/bin/env python -# -*- coding: utf-8 -*- - -""" -Mixer multirotor test and prototyping script. - -Author: Mathieu Bresciani , Beat Kueng -Description: This script can be used to prototype new mixer algorithms and test -it against the C++ implementation. -""" - - -from __future__ import print_function - -from argparse import ArgumentParser -import numpy as np -import numpy.matlib -import subprocess - - -# -------------------------------------------------- -# mixing algorithms -# -------------------------------------------------- - -def compute_desaturation_gain(u, u_min, u_max, desaturation_vector): - """ - Computes the gain k by which desaturation_vector has to be multiplied - in order to unsaturate the output that has the greatest saturation - """ - d_u_sat_plus = u_max - u - d_u_sat_minus = u_min - u - k = np.zeros(u.size*2) - for i in range(u.size): - if abs(desaturation_vector[i]) < 0.000001: - # avoid division by zero - continue - - if d_u_sat_minus[i] > 0.0: - k[2*i] = d_u_sat_minus[i] / desaturation_vector[i] - if d_u_sat_plus[i] < 0.0: - k[2*i+1] = d_u_sat_plus[i] / desaturation_vector[i] - - k_min = min(k) - k_max = max(k) - - # Reduce the saturation as much as possible - k = k_min + k_max - return k - - -def minimize_sat(u, u_min, u_max, desaturation_vector): - """ - Minimize the saturation of the actuators by - adding or substracting a fraction of desaturation_vector. - desaturation_vector is the vector that added to the output u, - modifies the thrust or angular acceleration on a - specific axis. - For example, if desaturation_vector is given - to slide along the vertical thrust axis, the saturation will - be minimized by shifting the vertical thrust setpoint, - without changing the roll/pitch/yaw accelerations. - """ - k_1 = compute_desaturation_gain(u, u_min, u_max, desaturation_vector) - u_1 = u + k_1 * desaturation_vector # Try to unsaturate - - - # Compute the desaturation gain again based on the updated outputs. - # In most cases it will be zero. It won't be if max(outputs) - min(outputs) - # > max_output - min_output. - # In that case adding 0.5 of the gain will equilibrate saturations. - k_2 = compute_desaturation_gain(u_1, u_min, u_max, desaturation_vector) - - k_opt = k_1 + 0.5 * k_2 - - u_prime = u + k_opt * desaturation_vector - return u_prime - -def mix_yaw(m_sp, u, P, u_min, u_max): - """ - Mix yaw by adding it to an existing output vector u - - Desaturation behavior: thrust is allowed to be decreased up to 15% in order to allow - some yaw control on the upper end. On the lower end thrust will never be increased, - but yaw is decreased as much as required. - """ - m_sp_yaw_only = np.matlib.zeros(m_sp.size).T - m_sp_yaw_only[2, 0] = m_sp[2, 0] - u_p = u + P * m_sp_yaw_only - - # Change yaw acceleration to unsaturate the outputs if needed (do not change roll/pitch), - # and allow some yaw response at maximum thrust - u_r_dot = P[:,2] - u_pp = minimize_sat(u_p, u_min, u_max+0.15, u_r_dot) - u_T = P[:, 3] - u_ppp = minimize_sat(u_pp, 0, u_max, u_T) - # reduce thrust only - if (u_ppp > (u_pp)).any(): - u_ppp = u_pp - return u_ppp - -def airmode_rp(m_sp, P, u_min, u_max): - """ - Mix roll, pitch, yaw and thrust. - - Desaturation behavior: airmode for roll/pitch: - thrust is increased/decreased as much as required to meet the demanded roll/pitch. - Yaw is not allowed to increase the thrust, @see mix_yaw() for the exact behavior. - """ - # Mix without yaw - m_sp_no_yaw = m_sp.copy() - m_sp_no_yaw[2, 0] = 0.0 - u = P * m_sp_no_yaw - - # Use thrust to unsaturate the outputs if needed - u_T = P[:, 3] - u_prime = minimize_sat(u, u_min, u_max, u_T) - - # Mix yaw axis independently - u_final = mix_yaw(m_sp, u_prime, P, u_min, u_max) - - return (u, u_final) - - -def airmode_rpy(m_sp, P, u_min, u_max): - """ - Mix roll, pitch, yaw and thrust. - - Desaturation behavior: full airmode for roll/pitch/yaw: - thrust is increased/decreased as much as required to meet demanded the roll/pitch/yaw. - """ - # Mix with yaw - u = P * m_sp - - # Use thrust to unsaturate the outputs if needed - u_T = P[:, 3] - u_prime = minimize_sat(u, u_min, u_max, u_T) - - # Unsaturate yaw (in case upper and lower bounds are exceeded) - # to prioritize roll/pitch over yaw. - u_T = P[:, 2] - u_prime_yaw = minimize_sat(u_prime, u_min, u_max, u_T) - - return (u, u_prime_yaw) - - -def normal_mode(m_sp, P, u_min, u_max): - """ - Mix roll, pitch, yaw and thrust. - - Desaturation behavior: no airmode, thrust is NEVER increased to meet the demanded - roll/pitch/yaw. Instead roll/pitch/yaw is reduced as much as needed. - Thrust can be reduced to unsaturate the upper side. - @see mix_yaw() for the exact yaw behavior. - """ - # Mix without yaw - m_sp_no_yaw = m_sp.copy() - m_sp_no_yaw[2, 0] = 0.0 - u = P * m_sp_no_yaw - - # Use thrust to unsaturate the outputs if needed - # by reducing the thrust only - u_T = P[:, 3] - u_prime = minimize_sat(u, u_min, u_max, u_T) - if (u_prime > (u)).any(): - u_prime = u - - # Reduce roll/pitch acceleration if needed to unsaturate - u_p_dot = P[:, 0] - u_p2 = minimize_sat(u_prime, u_min, u_max, u_p_dot) - u_q_dot = P[:, 1] - u_p3 = minimize_sat(u_p2, u_min, u_max, u_q_dot) - - # Mix yaw axis independently - u_final = mix_yaw(m_sp, u_p3, P, u_min, u_max) - return (u, u_final) - - -# -------------------------------------------------- -# test cases -# -------------------------------------------------- - -# normalized control allocation test matrices (B_px from px_generate_mixers.py) -# quad_x -P1 = np.matrix([ - [-0.71, 0.71, 1., 1. ], - [ 0.71, -0.71, 1., 1. ], - [ 0.71, 0.71, -1., 1. ], - [-0.71, -0.71, -1., 1. ]]) -# quad_wide -P2 = np.matrix([ - [-0.5, 0.71, 0.77, 1. ], - [ 0.5, -0.71, 1., 1. ], - [ 0.5, 0.71, -0.77, 1. ], - [-0.5, -0.71, -1., 1. ]]) -# hex_x -P3 = np.matrix([ - [-1., 0., -1., 1. ], - [ 1., -0., 1., 1. ], - [ 0.5, 0.87, -1., 1. ], - [-0.5, -0.87, 1., 1. ], - [-0.5, 0.87, 1., 1. ], - [ 0.5, -0.87, -1., 1. ]]) -# hex_cox -P4 = np.matrix([ - [-0.87, 0.5, -1., 1. ], - [-0.87, 0.5, 1., 1. ], - [ 0., -1., -1., 1. ], - [ 0., -1., 1., 1. ], - [ 0.87, 0.5, -1., 1. ], - [ 0.87, 0.5, 1., 1. ]]) -# octa_plus -P5 = np.matrix([ - [-0., 1., -1., 1. ], - [ 0., -1., -1., 1. ], - [-0.71, 0.71, 1., 1. ], - [-0.71, -0.71, 1., 1. ], - [ 0.71, 0.71, 1., 1. ], - [ 0.71, -0.71, 1., 1. ], - [ 1., 0., -1., 1. ], - [-1., -0., -1., 1. ]]) - -P_tests = [ P1, P2, P3, P4, P5 ] - -test_cases_input = np.matrix([ - # desired accelerations (must be within [-1, 1]): - #roll pitch yaw thrust - [ 0.0, 0.0, 0.0, 0.0], - [-0.05, 0.0, 0.0, 0.0], - [ 0.05, -0.05, 0.0, 0.0], - [ 0.05, 0.05, -0.025, 0.0], - [ 0.0, 0.2, -0.025, 0.0], - [ 0.2, 0.05, 0.09, 0.0], - [-0.125, 0.02, 0.04, 0.0], - - # extreme cases - [ 1.0, 0.0, 0.0, 0.0], - [ 0.0, -1.0, 0.0, 0.0], - [ 0.0, 0.0, 1.0, 0.0], - [ 1.0, 1.0, -1.0, 0.0], - [-1.0, 0.9, -0.9, 0.0], - [-1.0, 0.9, 0.0, 0.0], - ]) - -# use the following thrust values for all test cases (must be within [0, 1]) -thrust_values = [0, 0.1, 0.45, 0.9, 1.0] -test_cases = np.zeros((test_cases_input.shape[0] * len(thrust_values), 4)) -for i in range(test_cases_input.shape[0]): - for k in range(len(thrust_values)): - test_case = test_cases_input[i] - test_case[0, 3] = thrust_values[k] - test_cases[i * len(thrust_values) + k, :] = test_case - - -def run_tests(mixer_cb, P, test_mixer_binary, test_index=None): - """ - Run all (or a specific) tests for a certain mixer method an control - allocation matrix P - """ - B = np.linalg.pinv(P) - proc = subprocess.Popen( - test_mixer_binary, - #'cat > /tmp/test_'+str(mode_idx), shell=True, # just to test the output - stdout=subprocess.PIPE, - stdin=subprocess.PIPE) - proc.stdin.write("{:}\n".format(mode_idx).encode('utf-8')) # airmode - motor_count = P.shape[0] - proc.stdin.write("{:}\n".format(motor_count).encode('utf-8')) # motor count - # control allocation matrix - for row in P.getA(): - for col in row: - proc.stdin.write("{:.8f} ".format(col).encode('utf-8')) - proc.stdin.write("\n".encode('utf-8')) - proc.stdin.write("\n".encode('utf-8')) - - failed = False - try: - if test_index is None: - # go through all test cases - test_indices = range(test_cases.shape[0]) - else: - test_indices = [test_index] - for i in test_indices: - actuator_controls = test_cases[[i], :].T - proc.stdin.write("{:.8f} {:.8f} {:.8f} {:.8f}\n" - .format(actuator_controls[0, 0], actuator_controls[1, 0], - actuator_controls[2, 0], actuator_controls[3, 0]).encode('utf-8')) - - (u, u_new) = mixer_cb(actuator_controls, P, 0.0, 1.0) - # Saturate the outputs between 0 and 1 - u_new_sat = np.maximum(u_new, np.matlib.zeros(u.size).T) - u_new_sat = np.minimum(u_new_sat, np.matlib.ones(u.size).T) - # write expected outputs - for j in range(motor_count): - proc.stdin.write("{:.8f} ".format(u_new_sat[j, 0]).encode('utf-8')) - proc.stdin.write("\n".encode('utf-8')) - - proc.stdin.close() - except IOError as e: - failed = True - result = proc.stdout.read() - proc.wait() - if proc.returncode != 0: failed = True - if failed: - print("Error: test failed") - print("B:\n{}".format(B)) - print("P:\n{}".format(P)) - print(result) - raise Exception('Test failed') - -parser = ArgumentParser(description=__doc__) -parser.add_argument('--test', action='store_true', default=False, help='Run tests') -parser.add_argument("--mixer-multirotor-binary", - help="select test_mixer_multirotor binary file name", - default='./test_mixer_multirotor') -parser.add_argument("--mode", "-m", dest="mode", - help="mixer mode: none, rp, rpy", default=None) -parser.add_argument("-i", dest="index", type=int, - help="Select a single test to run (starting at 1)", default=None) - -args = parser.parse_args() -mixer_mode = args.mode - -if args.test: - mixer_binary = args.mixer_multirotor_binary - test_index = args.index - if test_index is not None: test_index -= 1 - for mode_idx, (airmode, mixer_cb) in enumerate([ - ('none', normal_mode), - ('rp', airmode_rp), - ('rpy', airmode_rpy)]): - if mixer_mode is not None and mixer_mode != airmode: - continue - print('Testing mode: '+airmode) - for P in P_tests: - run_tests(mixer_cb, P, mixer_binary, test_index) - exit(0) - -# -------------------------------------------------- -# Prototyping and corner case testing playground -# -------------------------------------------------- - -# Compute the control allocation matrix -# u = P * m -P = P1 # normal quad -#P = P2 # wide quad - -# Normalized actuator effectiveness matrix using the pseudo inverse of P -# m = B * u -B = np.linalg.pinv(P) - -# Desired accelerations (actuator controls, in [-1, 1]) -p_dot_sp = 0.0 # roll acceleration (p is the roll rate) -q_dot_sp = 0.1 # pitch acceleration -r_dot_sp = 0.1 # yaw acceleration -T_sp = 0.0 # vertical thrust -m_sp = np.matrix([p_dot_sp, q_dot_sp, r_dot_sp, T_sp]).T # Vector of desired "accelerations" - -# Airmode type (none/rp/rpy) -airmode = mixer_mode -if airmode is None: airmode = "none" - -# Actuators output saturations -u_max = 1.0 -u_min = 0.0 - -if airmode == "none": - (u, u_new) = normal_mode(m_sp, P, u_min, u_max) -elif airmode == "rp": - (u, u_new) = airmode_rp(m_sp, P, u_min, u_max) -elif airmode == "rpy": - (u, u_new) = airmode_rpy(m_sp, P, u_min, u_max) -else: - u = 0.0 - u_new = 0.0 - -# Saturate the outputs between 0 and 1 -u_new_sat = np.maximum(u_new, np.matlib.zeros(u.size).T) -u_new_sat = np.minimum(u_new_sat, np.matlib.ones(u.size).T) - -np.set_printoptions(suppress=True) - -# Display some results -print("u = \n{}\n".format(u)) -print("u_new = \n{}\n".format(u_new)) -print("u_new_sat = \n{}\n".format(u_new_sat)) -print("Desired accelerations = \n{}\n".format(m_sp)) -# Compute back the allocated accelerations -m_new = B * u_new_sat -print("Allocated accelerations = \n{}\n".format(m_new)) diff --git a/src/lib/mixer/MultirotorMixer/test_mixer_multirotor.cpp b/src/lib/mixer/MultirotorMixer/test_mixer_multirotor.cpp deleted file mode 100644 index 2e95399a26..0000000000 --- a/src/lib/mixer/MultirotorMixer/test_mixer_multirotor.cpp +++ /dev/null @@ -1,173 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2018 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. - * - ****************************************************************************/ - -/** - * testing binary that runs the multirotor mixer through test cases given - * via file or stdin and compares the mixer output against expected values. - */ - -#include "MultirotorMixer.hpp" - -#include -#include - -static const unsigned output_max = 16; -static float actuator_controls[output_max] {}; - -static int mixer_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &control); - -static int -mixer_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &control) -{ - control = actuator_controls[control_index]; - return 0; -} - -int main(int argc, char *argv[]) -{ - FILE *file_in = stdin; - - if (argc > 1) { - file_in = fopen(argv[1], "r"); - } - - unsigned rotor_count = 0; - MultirotorMixer::Rotor rotors[output_max]; - float actuator_outputs[output_max]; - - // read airmode - int airmode; - fscanf(file_in, "%i", &airmode); - - // read the motor count & control allocation matrix - fscanf(file_in, "%i", &rotor_count); - - if (rotor_count > output_max) { - return -1; - } - - for (unsigned i = 0; i < rotor_count; ++i) { - fscanf(file_in, "%f %f %f %f", &rotors[i].roll_scale, &rotors[i].pitch_scale, - &rotors[i].yaw_scale, &rotors[i].thrust_scale); - } - - MultirotorMixer mixer(mixer_callback, 0, rotors, rotor_count); - mixer.set_airmode((Mixer::Airmode)airmode); - - int test_counter = 0; - int num_failed = 0; - - while (!feof(file_in)) { - - // read actuator controls - unsigned count = 0; - - while (count < 4 && fscanf(file_in, "%f", &actuator_controls[count]) == 1) { - ++count; - } - - if (count < 4) { - break; - } - - // do the mixing - if (mixer.mix(actuator_outputs, output_max) != rotor_count) { - return -1; - } - - // Account for MultirotorMixer outputing [-1,1] and python script [0,1] - for (unsigned i = 0; i < rotor_count; i++) { - actuator_outputs[i] = (actuator_outputs[i] + 1.f) * .5f; - } - - // read expected outputs - count = 0; - float expected_output[output_max]; - bool failed = false; - - while (count < rotor_count && fscanf(file_in, "%f", &expected_output[count]) == 1) { - if (fabsf(expected_output[count] - actuator_outputs[count]) > 0.00001f) { - failed = true; - } - - ++count; - } - - if (count < rotor_count) { - break; - } - - if (failed) { - printf("test %i failed:\n", test_counter + 1); - printf("control input : %.3f %.3f %.3f %.3f\n", (double)actuator_controls[0], (double)actuator_controls[1], - (double)actuator_controls[2], (double)actuator_controls[3]); - printf("mixer output : "); - - for (unsigned i = 0; i < rotor_count; ++i) { - printf("%.3f ", (double)actuator_outputs[i]); - } - - printf("\n"); - printf("expected output: "); - - for (unsigned i = 0; i < rotor_count; ++i) { - printf("%.3f ", (double)expected_output[i]); - } - - printf("\n"); - printf("diff : "); - - for (unsigned i = 0; i < rotor_count; ++i) { - printf("%.3f ", (double)(expected_output[i] - actuator_outputs[i])); - } - - printf("\n"); - ++num_failed; - } - - ++test_counter; - } - - printf("tested %i cases: %i success, %i failed\n", test_counter, - test_counter - num_failed, num_failed); - - - if (file_in != stdin) { - fclose(file_in); - } - - return num_failed > 0 ? -1 : 0; -} diff --git a/src/lib/mixer/NullMixer/CMakeLists.txt b/src/lib/mixer/NullMixer/CMakeLists.txt deleted file mode 100644 index 6b2eeb3bbd..0000000000 --- a/src/lib/mixer/NullMixer/CMakeLists.txt +++ /dev/null @@ -1,39 +0,0 @@ -############################################################################ -# -# Copyright (c) 2015-2019 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. -# -############################################################################ - -add_library(NullMixer - NullMixer.cpp - NullMixer.hpp -) -target_link_libraries(NullMixer PRIVATE MixerBase) -add_dependencies(NullMixer prebuild_targets) diff --git a/src/lib/mixer/NullMixer/NullMixer.cpp b/src/lib/mixer/NullMixer/NullMixer.cpp deleted file mode 100644 index 636cddd62d..0000000000 --- a/src/lib/mixer/NullMixer/NullMixer.cpp +++ /dev/null @@ -1,67 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2019 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 "NullMixer.hpp" - -#include -#include -#include - -unsigned -NullMixer::mix(float *outputs, unsigned space) -{ - if (space > 0) { - *outputs = NAN; - return 1; - } - - return 0; -} - -NullMixer * -NullMixer::from_text(const char *buf, unsigned &buflen) -{ - NullMixer *nm = nullptr; - - /* enforce that the mixer ends with a new line */ - if (!string_well_formed(buf, buflen)) { - return nullptr; - } - - if ((buflen >= 2) && (buf[0] == 'Z') && (buf[1] == ':')) { - nm = new NullMixer; - buflen -= 2; - } - - return nm; -} diff --git a/src/lib/mixer/NullMixer/NullMixer.hpp b/src/lib/mixer/NullMixer/NullMixer.hpp deleted file mode 100644 index 4f098298b3..0000000000 --- a/src/lib/mixer/NullMixer/NullMixer.hpp +++ /dev/null @@ -1,75 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012-2019 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 - -/** - * Null mixer; returns zero. - * - * Used as a placeholder for output channels that are unassigned in groups. - */ -class NullMixer : public Mixer -{ -public: - NullMixer() : Mixer(nullptr, 0) {} - virtual ~NullMixer() = default; - - // no copy, assignment, move, move assignment - NullMixer(const NullMixer &) = delete; - NullMixer &operator=(const NullMixer &) = delete; - NullMixer(NullMixer &&) = delete; - NullMixer &operator=(NullMixer &&) = delete; - - /** - * Factory method. - * - * Given a pointer to a buffer containing a text description of the mixer, - * returns a pointer to a new instance of the mixer. - * - * @param buf Buffer containing a text description of - * the mixer. - * @param buflen Length of the buffer in bytes, adjusted - * to reflect the bytes consumed. - * @return A new NullMixer instance, or nullptr - * if the text format is bad. - */ - static NullMixer *from_text(const char *buf, unsigned &buflen); - - unsigned mix(float *outputs, unsigned space) override; - - unsigned set_trim(float trim) override { return 1; } - unsigned get_trim(float *trim) override { return 1; } - -}; diff --git a/src/lib/mixer/SimpleMixer/CMakeLists.txt b/src/lib/mixer/SimpleMixer/CMakeLists.txt deleted file mode 100644 index ef01747d26..0000000000 --- a/src/lib/mixer/SimpleMixer/CMakeLists.txt +++ /dev/null @@ -1,39 +0,0 @@ -############################################################################ -# -# Copyright (c) 2015-2019 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. -# -############################################################################ - -add_library(SimpleMixer - SimpleMixer.cpp - SimpleMixer.hpp -) -target_link_libraries(SimpleMixer PRIVATE MixerBase) -add_dependencies(SimpleMixer prebuild_targets) diff --git a/src/lib/mixer/SimpleMixer/SimpleMixer.cpp b/src/lib/mixer/SimpleMixer/SimpleMixer.cpp deleted file mode 100644 index 270b77ffcf..0000000000 --- a/src/lib/mixer/SimpleMixer/SimpleMixer.cpp +++ /dev/null @@ -1,390 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2015 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. - * - ****************************************************************************/ - -/** - * @file mixer_simple.cpp - * - * Simple summing mixer. - */ - -#include "SimpleMixer.hpp" - -#include -#include - -#define debug(fmt, args...) do { } while(0) -//#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0) - -SimpleMixer::SimpleMixer(ControlCallback control_cb, uintptr_t cb_handle, mixer_simple_s *mixinfo) : - Mixer(control_cb, cb_handle), - _pinfo(mixinfo) -{ -} - -SimpleMixer::~SimpleMixer() -{ - if (_pinfo != nullptr) { - free(_pinfo); - } -} - -unsigned SimpleMixer::set_trim(float trim) -{ - _pinfo->output_scaler.offset = trim; - return 1; -} - -unsigned SimpleMixer::get_trim(float *trim) -{ - *trim = _pinfo->output_scaler.offset; - return 1; -} - -void -SimpleMixer::set_dt_once(float dt) -{ - _dt = dt; -} - -int -SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler, float &slew_rate_rise_time) -{ - int ret; - int s[6]; - int n = -1; - - buf = findtag(buf, buflen, 'O'); - - if ((buf == nullptr) || (buflen < 12)) { - debug("output parser failed finding tag, ret: '%s'", buf); - return -1; - } - - if ((ret = sscanf(buf, "O: %d %d %d %d %d %d %n", - &s[0], &s[1], &s[2], &s[3], &s[4], &s[5], &n)) < 5) { - debug("out scaler parse failed on '%s' (got %d, consumed %d)", buf, ret, n); - return -1; - } - - // set slew rate limit to 0 if no 6th number is specified in mixer file - if (ret == 5) { - s[5] = 0; - } - - buf = skipline(buf, buflen); - - if (buf == nullptr) { - debug("no line ending, line is incomplete"); - return -1; - } - - scaler.negative_scale = s[0] / 10000.0f; - scaler.positive_scale = s[1] / 10000.0f; - scaler.offset = s[2] / 10000.0f; - scaler.min_output = s[3] / 10000.0f; - scaler.max_output = s[4] / 10000.0f; - slew_rate_rise_time = s[5] / 10000.0f; - - return 0; -} - -int -SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler, uint8_t &control_group, - uint8_t &control_index) -{ - unsigned u[2]; - int s[5]; - - buf = findtag(buf, buflen, 'S'); - - if ((buf == nullptr) || (buflen < 16)) { - debug("control parser failed finding tag, ret: '%s'", buf); - return -1; - } - - if (sscanf(buf, "S: %u %u %d %d %d %d %d", - &u[0], &u[1], &s[0], &s[1], &s[2], &s[3], &s[4]) != 7) { - debug("control parse failed on '%s'", buf); - return -1; - } - - buf = skipline(buf, buflen); - - if (buf == nullptr) { - debug("no line ending, line is incomplete"); - return -1; - } - - control_group = u[0]; - control_index = u[1]; - scaler.negative_scale = s[0] / 10000.0f; - scaler.positive_scale = s[1] / 10000.0f; - scaler.offset = s[2] / 10000.0f; - scaler.min_output = s[3] / 10000.0f; - scaler.max_output = s[4] / 10000.0f; - - return 0; -} - -SimpleMixer * -SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen) -{ - SimpleMixer *sm = nullptr; - mixer_simple_s *mixinfo = nullptr; - unsigned inputs; - int used; - const char *end = buf + buflen; - char next_tag; - - /* enforce that the mixer ends with a new line */ - if (!string_well_formed(buf, buflen)) { - return nullptr; - } - - /* get the base info for the mixer */ - if (sscanf(buf, "M: %u%n", &inputs, &used) != 1) { - debug("simple parse failed on '%s'", buf); - goto out; - } - - /* at least 1 input is required */ - if (inputs == 0) { - debug("simple parse got 0 inputs"); - goto out; - } - - buf = skipline(buf, buflen); - - if (buf == nullptr) { - debug("no line ending, line is incomplete"); - goto out; - } - - mixinfo = (mixer_simple_s *)malloc(MIXER_SIMPLE_SIZE(inputs)); - - if (mixinfo == nullptr) { - debug("could not allocate memory for mixer info"); - goto out; - } - - mixinfo->control_count = inputs; - - /* find the next tag */ - next_tag = findnexttag(end - buflen, buflen); - - if (next_tag == 'S') { - /* No output scalers specified. Use default values. - * Corresponds to: - * O: 10000 10000 0 -10000 10000 0 - */ - mixinfo->output_scaler.negative_scale = 1.0f; - mixinfo->output_scaler.positive_scale = 1.0f; - mixinfo->output_scaler.offset = 0.f; - mixinfo->output_scaler.min_output = -1.0f; - mixinfo->output_scaler.max_output = 1.0f; - mixinfo->slew_rate_rise_time = 0.0f; - - } else { - if (parse_output_scaler(end - buflen, buflen, mixinfo->output_scaler, mixinfo->slew_rate_rise_time)) { - debug("simple mixer parser failed parsing out scaler tag, ret: '%s'", buf); - goto out; - } - } - - for (unsigned i = 0; i < inputs; i++) { - if (parse_control_scaler(end - buflen, buflen, - mixinfo->controls[i].scaler, - mixinfo->controls[i].control_group, - mixinfo->controls[i].control_index)) { - debug("simple mixer parser failed parsing ctrl scaler tag, ret: '%s'", buf); - goto out; - } - } - - sm = new SimpleMixer(control_cb, cb_handle, mixinfo); - - if (sm != nullptr) { - mixinfo = nullptr; - debug("loaded mixer with %d input(s)", inputs); - - } else { - debug("could not allocate memory for mixer"); - } - -out: - - if (mixinfo != nullptr) { - free(mixinfo); - } - - return sm; -} - -unsigned -SimpleMixer::mix(float *outputs, unsigned space) -{ - float sum = 0.0f; - - if (_pinfo == nullptr) { - return 0; - } - - if (space < 1) { - return 0; - } - - for (unsigned i = 0; i < _pinfo->control_count; i++) { - float input = 0.0f; - - _control_cb(_cb_handle, - _pinfo->controls[i].control_group, - _pinfo->controls[i].control_index, - input); - - sum += scale(_pinfo->controls[i].scaler, input); - } - - *outputs = scale(_pinfo->output_scaler, sum); - - if (_dt > FLT_EPSILON && _pinfo->slew_rate_rise_time > FLT_EPSILON) { - - // factor 2 is needed because actuator outputs are in the range [-1,1] - const float output_delta_max = 2.0f * _dt / _pinfo->slew_rate_rise_time; - - float delta_out = *outputs - _output_prev; - - if (delta_out > output_delta_max) { - *outputs = _output_prev + output_delta_max; - - } else if (delta_out < -output_delta_max) { - *outputs = _output_prev - output_delta_max; - } - - } - - // this will force the caller of the mixer to always supply dt values, otherwise no slew rate limiting will happen - _dt = 0.f; - - _output_prev = *outputs; - - return 1; -} - -void -SimpleMixer::groups_required(uint32_t &groups) -{ - for (unsigned i = 0; i < _pinfo->control_count; i++) { - groups |= 1 << _pinfo->controls[i].control_group; - } -} - -int -SimpleMixer::check() -{ - float junk; - - /* sanity that presumes that a mixer includes a control no more than once */ - /* max of 32 groups due to groups_required API */ - if (_pinfo->control_count > 32) { - return -2; - } - - /* validate the output scaler */ - int ret = scale_check(_pinfo->output_scaler); - - if (ret != 0) { - return ret; - } - - /* validate input scalers */ - for (unsigned i = 0; i < _pinfo->control_count; i++) { - - /* verify that we can fetch the control */ - if (_control_cb(_cb_handle, - _pinfo->controls[i].control_group, - _pinfo->controls[i].control_index, - junk) != 0) { - return -3; - } - - /* validate the scaler */ - ret = scale_check(_pinfo->controls[i].scaler); - - if (ret != 0) { - return (10 * i + ret); - } - } - - return 0; -} - -float -SimpleMixer::scale(const mixer_scaler_s &scaler, float input) -{ - float output; - - if (input < 0.0f) { - output = (input * scaler.negative_scale) + scaler.offset; - - } else { - output = (input * scaler.positive_scale) + scaler.offset; - } - - return math::constrain(output, scaler.min_output, scaler.max_output); -} - -int -SimpleMixer::scale_check(mixer_scaler_s &scaler) -{ - if (scaler.offset > 1.001f) { - return 1; - } - - if (scaler.offset < -1.001f) { - return 2; - } - - if (scaler.min_output > scaler.max_output) { - return 3; - } - - if (scaler.min_output < -1.001f) { - return 4; - } - - if (scaler.max_output > 1.001f) { - return 5; - } - - return 0; -} diff --git a/src/lib/mixer/SimpleMixer/SimpleMixer.hpp b/src/lib/mixer/SimpleMixer/SimpleMixer.hpp deleted file mode 100644 index d2811e6d59..0000000000 --- a/src/lib/mixer/SimpleMixer/SimpleMixer.hpp +++ /dev/null @@ -1,153 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012-2019 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 - -/** simple channel scaler */ -struct mixer_scaler_s { - float negative_scale{1.0f}; - float positive_scale{1.0f}; - float offset{0.0f}; - float min_output{-1.0f}; - float max_output{1.0f}; -}; - -/** mixer input */ -struct mixer_control_s { - uint8_t control_group; /**< group from which the input reads */ - uint8_t control_index; /**< index within the control group */ - mixer_scaler_s scaler; /**< scaling applied to the input before use */ -}; - -#define MIXER_SIMPLE_SIZE(_icount) (sizeof(struct mixer_simple_s) + (_icount) * sizeof(struct mixer_control_s)) - -/** simple mixer */ -struct mixer_simple_s { - uint8_t control_count; /**< number of inputs */ - mixer_scaler_s output_scaler; /**< scaling for the output */ - float slew_rate_rise_time{0.0f}; /**< output max rise time (slew rate limit)*/ - mixer_control_s controls[]; /**< actual size of the array is set by control_count */ -}; - -/** - * Simple summing mixer. - * - * Collects zero or more inputs and mixes them to a single output. - */ -class SimpleMixer : public Mixer -{ -public: - /** - * Constructor - * - * @param mixinfo Mixer configuration. The pointer passed - * becomes the property of the mixer and - * will be freed when the mixer is deleted. - */ - SimpleMixer(ControlCallback control_cb, uintptr_t cb_handle, mixer_simple_s *mixinfo); - virtual ~SimpleMixer(); - - // no copy, assignment, move, move assignment - SimpleMixer(const SimpleMixer &) = delete; - SimpleMixer &operator=(const SimpleMixer &) = delete; - SimpleMixer(SimpleMixer &&) = delete; - SimpleMixer &operator=(SimpleMixer &&) = delete; - - /** - * Factory method with full external configuration. - * - * Given a pointer to a buffer containing a text description of the mixer, - * returns a pointer to a new instance of the mixer. - * - * @param control_cb The callback to invoke when fetching a - * control value. - * @param cb_handle Handle passed to the control callback. - * @param buf Buffer containing a text description of - * the mixer. - * @param buflen Length of the buffer in bytes, adjusted - * to reflect the bytes consumed. - * @return A new SimpleMixer instance, or nullptr - * if the text format is bad. - */ - static SimpleMixer *from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, - unsigned &buflen); - - unsigned mix(float *outputs, unsigned space) override; - - void groups_required(uint32_t &groups) override; - - /** - * Check that the mixer configuration as loaded is sensible. - * - * Note that this function will call control_cb, but only cares about - * error returns, not the input value. - * - * @return Zero if the mixer makes sense, nonzero otherwise. - */ - int check(); - - unsigned set_trim(float trim) override; - unsigned get_trim(float *trim) override; - void set_dt_once(float dt) override; - -private: - - /** - * Perform simpler linear scaling. - * - * @param scaler The scaler configuration. - * @param input The value to be scaled. - * @return The scaled value. - */ - static float scale(const mixer_scaler_s &scaler, float input); - - /** - * Validate a scaler - * - * @param scaler The scaler to be validated. - * @return Zero if good, nonzero otherwise. - */ - static int scale_check(struct mixer_scaler_s &scaler); - - static int parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler, float &slew_rate_rise_time); - static int parse_control_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler, uint8_t &control_group, - uint8_t &control_index); - - float _output_prev{0.f}; - float _dt{0.f}; - - mixer_simple_s *_pinfo; - -}; diff --git a/src/lib/mixer/load_mixer_file.cpp b/src/lib/mixer/load_mixer_file.cpp deleted file mode 100644 index 52fd84b6de..0000000000 --- a/src/lib/mixer/load_mixer_file.cpp +++ /dev/null @@ -1,112 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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. - * - ****************************************************************************/ - -/** - * @file mixer_load.c - * - * Programmable multi-channel mixer library. - */ - -#include -#include -#include - -#include "mixer_load.h" - -int load_mixer_file(const char *fname, char *buf, unsigned maxlen) -{ - FILE *fp; - char line[120]; - - /* open the mixer definition file */ - fp = fopen(fname, "r"); - - if (fp == nullptr) { - printf("file not found\n"); - return -1; - } - - /* read valid lines from the file into a buffer */ - buf[0] = '\0'; - - for (;;) { - - /* get a line, bail on error/EOF */ - line[0] = '\0'; - - if (fgets(line, sizeof(line), fp) == nullptr) { - break; - } - - /* if the line doesn't look like a mixer definition line, skip it */ - if ((strlen(line) < 2) || !isupper(line[0]) || (line[1] != ':')) { - continue; - } - - /* compact whitespace in the buffer */ - char *t, *f; - - for (f = line; *f != '\0'; f++) { - /* scan for space characters */ - if (*f == ' ') { - /* look for additional spaces */ - t = f + 1; - - while (*t == ' ') { - t++; - } - - if (*t == '\0') { - /* strip trailing whitespace */ - *f = '\0'; - - } else if (t > (f + 1)) { - memmove(f + 1, t, strlen(t) + 1); - } - } - } - - /* if the line is too long to fit in the buffer, bail */ - if ((strlen(line) + strlen(buf) + 1) >= maxlen) { - printf("line too long\n"); - fclose(fp); - return -1; - } - - /* add the line to the buffer */ - strcat(buf, line); - } - - fclose(fp); - return 0; -} diff --git a/src/lib/mixer/mixer_load.h b/src/lib/mixer/mixer_load.h deleted file mode 100644 index d61e93b4f8..0000000000 --- a/src/lib/mixer/mixer_load.h +++ /dev/null @@ -1,51 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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. - * - ****************************************************************************/ - -/** - * @file mixer_load.h - * - */ - - -#ifndef _SYSTEMLIB_MIXER_LOAD_H -#define _SYSTEMLIB_MIXER_LOAD_H value - -#include - -__BEGIN_DECLS - -__EXPORT int load_mixer_file(const char *fname, char *buf, unsigned maxlen); - -__END_DECLS - -#endif diff --git a/src/lib/mixer_module/CMakeLists.txt b/src/lib/mixer_module/CMakeLists.txt index 816d8385d1..783dfe3dde 100644 --- a/src/lib/mixer_module/CMakeLists.txt +++ b/src/lib/mixer_module/CMakeLists.txt @@ -63,6 +63,5 @@ px4_add_library(mixer_module add_dependencies(mixer_module output_functions_header) target_compile_options(mixer_module PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) target_include_directories(mixer_module PRIVATE ${CMAKE_CURRENT_BINARY_DIR}) -target_link_libraries(mixer_module PRIVATE mixer) -px4_add_functional_gtest(SRC mixer_module_tests.cpp LINKLIBS mixer_module mixer) +px4_add_functional_gtest(SRC mixer_module_tests.cpp LINKLIBS mixer_module) diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index 2acc4c204d..4fe528f359 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -33,8 +33,6 @@ #include "mixer_module.hpp" -#include - #include #include @@ -68,21 +66,15 @@ static const FunctionProvider all_function_providers[] = { }; MixingOutput::MixingOutput(const char *param_prefix, uint8_t max_num_outputs, OutputModuleInterface &interface, - SchedulingPolicy scheduling_policy, bool support_esc_calibration, bool ramp_up) - : ModuleParams(&interface), - _output_ramp_up(ramp_up), - _control_subs{ - {&interface, ORB_ID(actuator_controls_0)}, - {&interface, ORB_ID(actuator_controls_1)}, - {&interface, ORB_ID(actuator_controls_2)}, - {&interface, ORB_ID(actuator_controls_3)}, -}, -_scheduling_policy(scheduling_policy), -_support_esc_calibration(support_esc_calibration), -_max_num_outputs(max_num_outputs < MAX_ACTUATORS ? max_num_outputs : MAX_ACTUATORS), -_interface(interface), -_control_latency_perf(perf_alloc(PC_ELAPSED, "control latency")), -_param_prefix(param_prefix) + SchedulingPolicy scheduling_policy, bool support_esc_calibration, bool ramp_up) : + ModuleParams(&interface), + _output_ramp_up(ramp_up), + _scheduling_policy(scheduling_policy), + _support_esc_calibration(support_esc_calibration), + _max_num_outputs(max_num_outputs < MAX_ACTUATORS ? max_num_outputs : MAX_ACTUATORS), + _interface(interface), + _control_latency_perf(perf_alloc(PC_ELAPSED, "control latency")), + _param_prefix(param_prefix) { /* Safely initialize armed flags */ _armed.armed = false; @@ -94,28 +86,20 @@ _param_prefix(param_prefix) px4_sem_init(&_lock, 0, 1); - _use_dynamic_mixing = _param_sys_ctrl_alloc.get(); + initParamHandles(); - if (_use_dynamic_mixing) { - initParamHandles(); - - for (unsigned i = 0; i < MAX_ACTUATORS; i++) { - _failsafe_value[i] = UINT16_MAX; - } - - updateParams(); - - } else { - _control_allocator_status_pub.advertise(); + for (unsigned i = 0; i < MAX_ACTUATORS; i++) { + _failsafe_value[i] = UINT16_MAX; } + updateParams(); + _outputs_pub.advertise(); } MixingOutput::~MixingOutput() { perf_free(_control_latency_perf); - delete _mixers; px4_sem_destroy(&_lock); cleanupFunctions(); @@ -153,28 +137,12 @@ void MixingOutput::printStatus() const PX4_INFO("Switched to rate_ctrl work queue"); } - - 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: %3i, value: %i, failsafe: %d, disarmed: %d, min: %d, max: %d\n", i, - (int)_function_assignment[i], _current_output_value[i], - actualFailsafeValue(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++) { + PX4_INFO_RAW("Channel %i: func: %3i, value: %i, failsafe: %d, disarmed: %d, min: %d, max: %d\n", i, + (int)_function_assignment[i], _current_output_value[i], + actualFailsafeValue(i), _disarmed_value[i], _min_value[i], _max_value[i]); } } @@ -182,154 +150,54 @@ void MixingOutput::updateParams() { ModuleParams::updateParams(); - // update mixer if we have one - if (_mixers) { - if (_param_mot_slew_max.get() <= FLT_EPSILON) { - _mixers->set_max_delta_out_once(0.f); + _param_mot_ordering.set(0); // not used with dynamic mixing + + bool function_changed = false; + + 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 } - _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; - - 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]) { - 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 (_param_handles[i].disarmed != PARAM_INVALID && param_get(_param_handles[i].disarmed, &val) == 0) { + _disarmed_value[i] = val; } - _reverse_output_mask = 0; - int32_t rev_range_param; - - if (_param_handle_rev_range != PARAM_INVALID && param_get(_param_handle_rev_range, &rev_range_param) == 0) { - _reverse_output_mask = rev_range_param; + if (_param_handles[i].min != PARAM_INVALID && param_get(_param_handles[i].min, &val) == 0) { + _min_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; - } - - // must be locked to potentially change WorkQueue - lock(); - - if (_scheduling_policy == SchedulingPolicy::Auto) { - // first clear everything - unregister(); - _interface.ScheduleClear(); - - // if subscribed to control group 0 or 1 then move to the rate_ctrl WQ - const bool sub_group_0 = (_groups_required & (1 << 0)); - const bool sub_group_1 = (_groups_required & (1 << 1)); - - if (allow_wq_switch && !_wq_switched && (sub_group_0 || sub_group_1)) { - if (_interface.ChangeWorkQueue(px4::wq_configurations::rate_ctrl)) { - // let the new WQ handle the subscribe update - _wq_switched = true; - _interface.ScheduleNow(); - unlock(); - return false; - } + if (_param_handles[i].max != PARAM_INVALID && param_get(_param_handles[i].max, &val) == 0) { + _max_value[i] = val; } - bool sub_group_0_callback_registered = false; - bool sub_group_1_callback_registered = false; - - // register callback to all required actuator control groups - for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { - - if (limit_callbacks_to_primary) { - // don't register additional callbacks if actuator_controls_0 or actuator_controls_1 are already registered - if ((i > 1) && (sub_group_0_callback_registered || sub_group_1_callback_registered)) { - break; - } - } - - if (_groups_required & (1 << i)) { - if (_control_subs[i].registerCallback()) { - PX4_DEBUG("subscribed to actuator_controls_%d", i); - - if (limit_callbacks_to_primary) { - if (i == 0) { - sub_group_0_callback_registered = true; - - } else if (i == 1) { - sub_group_1_callback_registered = true; - } - } - - } else { - PX4_ERR("actuator_controls_%d register callback failed!", i); - } - } + if (_min_value[i] > _max_value[i]) { + uint16_t tmp = _min_value[i]; + _min_value[i] = _max_value[i]; + _max_value[i] = tmp; } - // if nothing required keep periodic schedule (so the module can update other things) - if (_groups_required == 0) { - // TODO: this might need to be configurable depending on the module - _interface.ScheduleOnInterval(100_ms); + if (_param_handles[i].failsafe != PARAM_INVALID && param_get(_param_handles[i].failsafe, &val) == 0) { + _failsafe_value[i] = val; } } - _groups_subscribed = _groups_required; - setMaxTopicUpdateRate(_max_topic_update_interval_us); + _reverse_output_mask = 0; + int32_t rev_range_param; - PX4_DEBUG("_groups_required 0x%08" PRIx32, _groups_required); - PX4_DEBUG("_groups_subscribed 0x%08" PRIx32, _groups_subscribed); + if (_param_handle_rev_range != PARAM_INVALID && param_get(_param_handle_rev_range, &rev_range_param) == 0) { + _reverse_output_mask = rev_range_param; + } - unlock(); - - return true; + if (function_changed) { + _need_function_update = true; + } } void MixingOutput::cleanupFunctions() @@ -346,7 +214,7 @@ void MixingOutput::cleanupFunctions() } } -bool MixingOutput::updateSubscriptionsDynamicMixer(bool allow_wq_switch, bool limit_callbacks_to_primary) +bool MixingOutput::updateSubscriptions(bool allow_wq_switch, bool limit_callbacks_to_primary) { if (!_need_function_update || _armed.armed) { return false; @@ -491,17 +359,8 @@ void MixingOutput::setMaxTopicUpdateRate(unsigned max_topic_update_interval_us) { _max_topic_update_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); - } - } + if (_subscription_callback) { + _subscription_callback->set_interval_us(_max_topic_update_interval_us); } } @@ -539,45 +398,18 @@ void MixingOutput::setAllDisarmedValues(uint16_t value) void MixingOutput::unregister() { - for (auto &control_sub : _control_subs) { - control_sub.unregisterCallback(); - } - if (_subscription_callback) { _subscription_callback->unregisterCallback(); } } -void MixingOutput::updateOutputSlewrateMultirotorMixer() -{ - const hrt_abstime now = hrt_absolute_time(); - const float dt = math::constrain((now - _time_last_dt_update_multicopter) / 1e6f, 0.0001f, 0.02f); - _time_last_dt_update_multicopter = now; - - // maximum value the outputs of the multirotor mixer are allowed to change in this cycle - // factor 2 is needed because actuator outputs are in the range [-1,1] - const float delta_out_max = 2.0f * 1000.0f * dt / (_max_value[0] - _min_value[0]) / _param_mot_slew_max.get(); - _mixers->set_max_delta_out_once(delta_out_max); -} - -void MixingOutput::updateOutputSlewrateSimplemixer() -{ - const hrt_abstime now = hrt_absolute_time(); - const float dt = math::constrain((now - _time_last_dt_update_simple_mixer) / 1e6f, 0.0001f, 0.02f); - _time_last_dt_update_simple_mixer = now; - - // set dt for slew rate limiter in SimpleMixer (is reset internally after usig it, so needs to be set on every update) - _mixers->set_dt_once(dt); -} - - unsigned MixingOutput::motorTest() { test_motor_s test_motor; bool had_update = false; while (_motor_test.test_motor_sub.update(&test_motor)) { - if (test_motor.driver_instance != _driver_instance || + if (test_motor.driver_instance != 0 || test_motor.timestamp == 0 || hrt_elapsed_time(&test_motor.timestamp) > 100_ms) { continue; @@ -634,123 +466,6 @@ unsigned MixingOutput::motorTest() } bool MixingOutput::update() -{ - if (_use_dynamic_mixing) { - return updateDynamicMixer(); - - } else { - return updateStaticMixer(); - } -} -bool MixingOutput::updateStaticMixer() -{ - if (!_mixers) { - // do nothing until we have a valid mixer - return false; - } - - // 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; - - if (_armed.armed) { - _motor_test.in_test_mode = false; - } - } - - if (_param_mot_slew_max.get() > FLT_EPSILON) { - updateOutputSlewrateMultirotorMixer(); - } - - updateOutputSlewrateSimplemixer(); // update dt for output slew rate in simple mixer - - unsigned n_updates = 0; - - /* get controls for required topics */ - for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { - if (_groups_subscribed & (1 << i)) { - if (_control_subs[i].copy(&_controls[i])) { - n_updates++; - } - - /* During ESC calibration, we overwrite the throttle value. */ - if (i == 0 && _armed.in_esc_calibration_mode) { - - /* Set all controls to 0 */ - memset(&_controls[i], 0, sizeof(_controls[i])); - - /* except thrust to maximum. */ - _controls[i].control[actuator_controls_s::INDEX_THROTTLE] = 1.0f; - - /* Switch off the output limit ramp for the calibration. */ - _output_state = OutputLimitState::ON; - } - } - } - - // check for motor test (after topic updates) - if (!_armed.armed && !_armed.manual_lockdown) { - unsigned num_motor_test = motorTest(); - - if (num_motor_test > 0) { - if (_interface.updateOutputs(false, _current_output_value, num_motor_test, 1)) { - actuator_outputs_s actuator_outputs{}; - setAndPublishActuatorOutputs(num_motor_test, actuator_outputs); - } - - return true; - } - } - - /* do mixing */ - float outputs[MAX_ACTUATORS] {}; - const unsigned mixed_num_outputs = _mixers->mix(outputs, _max_num_outputs); - - /* the output limit call takes care of out of band errors, NaN and constrains */ - output_limit_calc(_throttle_armed, mixed_num_outputs, outputs); - - /* overwrite outputs in case of force_failsafe with _failsafe_value values */ - if (_armed.force_failsafe) { - for (size_t i = 0; i < mixed_num_outputs; i++) { - _current_output_value[i] = _failsafe_value[i]; - } - } - - bool stop_motors = mixed_num_outputs == 0 || !_throttle_armed; - - /* 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]; - } - - stop_motors = true; - } - - /* apply _param_mot_ordering */ - reorderOutputs(_current_output_value); - - /* now return the outputs to the driver */ - if (_interface.updateOutputs(stop_motors, _current_output_value, mixed_num_outputs, n_updates)) { - actuator_outputs_s actuator_outputs{}; - setAndPublishActuatorOutputs(mixed_num_outputs, actuator_outputs); - - publishMixerStatus(actuator_outputs); - updateLatencyPerfCounter(actuator_outputs); - } - - return true; -} - -bool MixingOutput::updateDynamicMixer() { // check arming state if (_armed_sub.update(&_armed)) { @@ -1019,84 +734,15 @@ MixingOutput::setAndPublishActuatorOutputs(unsigned num_outputs, actuator_output _outputs_pub.publish(actuator_outputs); } -void -MixingOutput::publishMixerStatus(const actuator_outputs_s &actuator_outputs) -{ - MultirotorMixer::saturation_status saturation_status; - saturation_status.value = _mixers->get_saturation_status(); - - if (saturation_status.flags.valid) { - control_allocator_status_s sat{}; - sat.timestamp = hrt_absolute_time(); - sat.torque_setpoint_achieved = true; - sat.thrust_setpoint_achieved = true; - - // Note: the values '-1', '1' and '0' are just to indicate a negative, - // positive or no saturation to the rate controller. The actual magnitude - // is not used. - if (saturation_status.flags.roll_pos) { - sat.unallocated_torque[0] = 1.f; - sat.torque_setpoint_achieved = false; - - } else if (saturation_status.flags.roll_neg) { - sat.unallocated_torque[0] = -1.f; - sat.torque_setpoint_achieved = false; - } - - if (saturation_status.flags.pitch_pos) { - sat.unallocated_torque[1] = 1.f; - sat.torque_setpoint_achieved = false; - - } else if (saturation_status.flags.pitch_neg) { - sat.unallocated_torque[1] = -1.f; - sat.torque_setpoint_achieved = false; - } - - if (saturation_status.flags.yaw_pos) { - sat.unallocated_torque[2] = 1.f; - sat.torque_setpoint_achieved = false; - - } else if (saturation_status.flags.yaw_neg) { - sat.unallocated_torque[2] = -1.f; - sat.torque_setpoint_achieved = false; - } - - if (saturation_status.flags.thrust_pos) { - sat.unallocated_thrust[2] = 1.f; - sat.thrust_setpoint_achieved = false; - - } else if (saturation_status.flags.thrust_neg) { - sat.unallocated_thrust[2] = -1.f; - sat.thrust_setpoint_achieved = false; - } - - _control_allocator_status_pub.publish(sat); - } -} - void MixingOutput::updateLatencyPerfCounter(const actuator_outputs_s &actuator_outputs) { - 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; + // 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 (_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; - } + if (_function_allocated[0]->getLatestSampleTimestamp(timestamp_sample)) { + perf_set_elapsed(_control_latency_perf, actuator_outputs.timestamp - timestamp_sample); } } } @@ -1104,10 +750,6 @@ MixingOutput::updateLatencyPerfCounter(const actuator_outputs_s &actuator_output uint16_t MixingOutput::actualFailsafeValue(int index) const { - if (!_use_dynamic_mixing) { - return _failsafe_value[index]; - } - uint16_t value = 0; if (_failsafe_value[index] == UINT16_MAX) { // if set to default, use the one provided by the function @@ -1126,34 +768,6 @@ MixingOutput::actualFailsafeValue(int index) const return value; } -void -MixingOutput::reorderOutputs(uint16_t values[MAX_ACTUATORS]) -{ - if (MAX_ACTUATORS < 4) { - return; - } - - if ((MotorOrdering)_param_mot_ordering.get() == MotorOrdering::Betaflight) { - /* - * Betaflight default motor ordering: - * 4 2 - * ^ - * 3 1 - */ - const uint16_t value_tmp[4] = {values[0], values[1], values[2], values[3] }; - values[0] = value_tmp[3]; - values[1] = value_tmp[0]; - values[2] = value_tmp[1]; - values[3] = value_tmp[2]; - } - - /* else: PX4, no need to reorder - * 3 1 - * ^ - * 2 4 - */ -} - int MixingOutput::reorderedMotorIndex(int index) const { if ((MotorOrdering)_param_mot_ordering.get() == MotorOrdering::Betaflight) { @@ -1170,37 +784,3 @@ int MixingOutput::reorderedMotorIndex(int index) const return index; } - -int MixingOutput::controlCallback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input) -{ - const MixingOutput *output = (const MixingOutput *)handle; - - input = output->_controls[control_group].control[control_index]; - - /* limit control input */ - input = math::constrain(input, -1.f, 1.f); - - /* motor spinup phase - lock throttle to zero */ - if (output->_output_state == OutputLimitState::RAMP) { - if ((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE || - control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) && - control_index == actuator_controls_s::INDEX_THROTTLE) { - /* limit the throttle output to zero during motor spinup, - * as the motors cannot follow any demand yet - */ - input = 0.0f; - } - } - - /* throttle not arming - mark throttle input as invalid */ - if (output->armNoThrottle() && !output->_armed.in_esc_calibration_mode) { - if ((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE || - control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) && - control_index == actuator_controls_s::INDEX_THROTTLE) { - /* set the throttle to an invalid value */ - input = NAN; - } - } - - return 0; -} diff --git a/src/lib/mixer_module/mixer_module.hpp b/src/lib/mixer_module/mixer_module.hpp index aeaf99e509..170456f98c 100644 --- a/src/lib/mixer_module/mixer_module.hpp +++ b/src/lib/mixer_module/mixer_module.hpp @@ -48,7 +48,6 @@ #include #include -#include #include #include #include @@ -57,9 +56,7 @@ #include #include #include -#include #include -#include #include #include @@ -74,8 +71,10 @@ class OutputModuleInterface : public px4::ScheduledWorkItem, public ModuleParams public: static constexpr int MAX_ACTUATORS = PWM_OUTPUT_MAX_CHANNELS; - OutputModuleInterface(const char *name, const px4::wq_config_t &config) - : px4::ScheduledWorkItem(name, config), ModuleParams(nullptr) {} + OutputModuleInterface(const char *name, const px4::wq_config_t &config) : + px4::ScheduledWorkItem(name, config), + ModuleParams(nullptr) + {} /** * Callback to update the (physical) actuator outputs in the driver @@ -125,21 +124,21 @@ public: ~MixingOutput(); - void setDriverInstance(uint8_t instance) { _driver_instance = instance; } - 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; } + 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; } + bool isFunctionSet(int index) const { return _functions[index] != nullptr; } OutputFunction outputFunction(int index) const { return _function_assignment[index]; } @@ -150,7 +149,7 @@ public: bool update(); /** - * Check for subscription updates (e.g. after a mixer is loaded). + * Check for subscription updates. * Call this at the very end of Run() if allow_wq_switch * @param allow_wq_switch if true * @param limit_callbacks_to_primary set to only register callbacks for primary actuator controls (if used) @@ -167,10 +166,6 @@ 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); void setAllDisarmedValues(uint16_t value); void setAllMinValues(uint16_t value); @@ -219,11 +214,6 @@ 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(); bool armNoThrottle() const { @@ -232,14 +222,10 @@ private: unsigned motorTest(); - void updateOutputSlewrateMultirotorMixer(); - void updateOutputSlewrateSimplemixer(); void setAndPublishActuatorOutputs(unsigned num_outputs, actuator_outputs_s &actuator_outputs); void publishMixerStatus(const actuator_outputs_s &actuator_outputs); void updateLatencyPerfCounter(const actuator_outputs_s &actuator_outputs); - static int controlCallback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input); - void cleanupFunctions(); void initParamHandles(); @@ -263,12 +249,6 @@ private: Betaflight = 1 }; - /** - * Reorder outputs according to _param_mot_ordering - * @param values values to reorder - */ - inline void reorderOutputs(uint16_t values[MAX_ACTUATORS]); - void lock() { do {} while (px4_sem_wait(&_lock) != 0); } void unlock() { px4_sem_post(&_lock); } @@ -292,30 +272,20 @@ private: const bool _output_ramp_up; ///< if true, motors will ramp up from disarmed to min_output after arming uORB::Subscription _armed_sub{ORB_ID(actuator_armed)}; - uORB::SubscriptionCallbackWorkItem _control_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; uORB::PublicationMulti _outputs_pub{ORB_ID(actuator_outputs)}; - uORB::PublicationMulti _control_allocator_status_pub{ORB_ID(control_allocator_status)}; - actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS] {}; actuator_armed_s _armed{}; - hrt_abstime _time_last_dt_update_multicopter{0}; - hrt_abstime _time_last_dt_update_simple_mixer{0}; - unsigned _max_topic_update_interval_us{0}; ///< max _control_subs topic update interval (0=unlimited) + unsigned _max_topic_update_interval_us{0}; ///< max topic update interval (0=unlimited) bool _throttle_armed{false}; bool _ignore_lockdown{false}; ///< if true, ignore the _armed.lockdown flag (for HIL outputs) - MixerGroup *_mixers{nullptr}; - uint32_t _groups_required{0}; - uint32_t _groups_subscribed{1u << 31}; ///< initialize to a different value than _groups_required and outside of (1 << NUM_ACTUATOR_CONTROL_GROUPS) - const SchedulingPolicy _scheduling_policy; const bool _support_esc_calibration; bool _wq_switched{false}; - uint8_t _driver_instance{0}; ///< for boards that supports multiple outputs (e.g. PX4IO + FMU) uint8_t _max_num_outputs; struct MotorTest { @@ -334,7 +304,6 @@ private: 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}; const char *const _param_prefix; ParamHandles _param_handles[MAX_ACTUATORS]; diff --git a/src/modules/control_allocator/CMakeLists.txt b/src/modules/control_allocator/CMakeLists.txt index a3ab199f26..cf7045d91a 100644 --- a/src/modules/control_allocator/CMakeLists.txt +++ b/src/modules/control_allocator/CMakeLists.txt @@ -51,6 +51,5 @@ px4_add_module( mathlib ActuatorEffectiveness ControlAllocation - mixer px4_work_queue ) diff --git a/src/modules/mavlink/streams/HIL_ACTUATOR_CONTROLS.hpp b/src/modules/mavlink/streams/HIL_ACTUATOR_CONTROLS.hpp index b528920b2f..48d5480928 100644 --- a/src/modules/mavlink/streams/HIL_ACTUATOR_CONTROLS.hpp +++ b/src/modules/mavlink/streams/HIL_ACTUATOR_CONTROLS.hpp @@ -57,19 +57,12 @@ public: private: explicit MavlinkStreamHILActuatorControls(Mavlink *mavlink) : MavlinkStream(mavlink) { - int32_t sys_ctrl_alloc = 0; - param_get(param_find("SYS_CTRL_ALLOC"), &sys_ctrl_alloc); - _use_dynamic_mixing = sys_ctrl_alloc >= 1; - - if (_use_dynamic_mixing) { - _act_sub = uORB::Subscription{ORB_ID(actuator_outputs_sim)}; - } + _act_sub = uORB::Subscription{ORB_ID(actuator_outputs_sim)}; } uORB::Subscription _act_sub{ORB_ID(actuator_outputs)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; - bool _use_dynamic_mixing{false}; bool send() override { @@ -79,90 +72,8 @@ private: mavlink_hil_actuator_controls_t msg{}; msg.time_usec = act.timestamp; - if (_use_dynamic_mixing) { - for (unsigned i = 0; i < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; i++) { - msg.controls[i] = act.output[i]; - } - - } else { - static constexpr float pwm_center = (PWM_DEFAULT_MAX + PWM_DEFAULT_MIN) / 2; - - unsigned system_type = _mavlink->get_system_type(); - - /* scale outputs depending on system type */ - if (system_type == MAV_TYPE_QUADROTOR || - system_type == MAV_TYPE_HEXAROTOR || - system_type == MAV_TYPE_OCTOROTOR || - system_type == MAV_TYPE_VTOL_TAILSITTER_DUOROTOR || - system_type == MAV_TYPE_VTOL_TAILSITTER_QUADROTOR || - system_type == MAV_TYPE_VTOL_FIXEDROTOR) { - - /* multirotors: set number of rotor outputs depending on type */ - - unsigned n; - - switch (system_type) { - case MAV_TYPE_QUADROTOR: - n = 4; - break; - - case MAV_TYPE_HEXAROTOR: - n = 6; - break; - - case MAV_TYPE_VTOL_TAILSITTER_DUOROTOR: - n = 2; - break; - - case MAV_TYPE_VTOL_TAILSITTER_QUADROTOR: - n = 4; - break; - - case MAV_TYPE_VTOL_FIXEDROTOR: - n = 8; - break; - - default: - n = 8; - break; - } - - for (unsigned i = 0; i < 16; i++) { - if (act.output[i] > PWM_DEFAULT_MIN / 2) { - if (i < n) { - /* scale PWM out 900..2100 us to 0..1 for rotors */ - msg.controls[i] = (act.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN); - - } else { - /* scale PWM out 900..2100 us to -1..1 for other channels */ - msg.controls[i] = (act.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2); - } - - } else { - /* send 0 when disarmed and for disabled channels */ - msg.controls[i] = 0.0f; - } - } - - } else { - /* fixed wing: scale throttle to 0..1 and other channels to -1..1 */ - for (unsigned i = 0; i < 16; i++) { - if (act.output[i] > PWM_DEFAULT_MIN / 2) { - if (i != 3) { - /* scale PWM out 900..2100 us to -1..1 for normal channels */ - msg.controls[i] = (act.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2); - - } else { - /* scale PWM out 900..2100 us to 0..1 for throttle */ - msg.controls[i] = (act.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN); - } - - } else { - /* set 0 for disabled channels */ - msg.controls[i] = 0.0f; - } - } - } + for (unsigned i = 0; i < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; i++) { + msg.controls[i] = act.output[i]; } // mode (MAV_MODE_FLAG) diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp index 8aea8b6e14..c176a0266b 100644 --- a/src/modules/mc_att_control/mc_att_control.hpp +++ b/src/modules/mc_att_control/mc_att_control.hpp @@ -33,7 +33,6 @@ #pragma once -#include // Airmode #include #include #include diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 8789a3dd52..b4d09217b0 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -117,7 +117,7 @@ MulticopterAttitudeControl::generate_attitude_setpoint(const Quatf &q, float dt, _man_yaw_sp = yaw; } else if (math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f) > 0.05f - || _param_mc_airmode.get() == (int32_t)Mixer::Airmode::roll_pitch_yaw) { + || _param_mc_airmode.get() == 2) { const float yaw_rate = math::radians(_param_mpc_man_y_max.get()); attitude_setpoint.yaw_sp_move_rate = _manual_control_setpoint.r * yaw_rate; diff --git a/src/modules/simulation/pwm_out_sim/CMakeLists.txt b/src/modules/simulation/pwm_out_sim/CMakeLists.txt index f4b3d7ef21..2802df3874 100644 --- a/src/modules/simulation/pwm_out_sim/CMakeLists.txt +++ b/src/modules/simulation/pwm_out_sim/CMakeLists.txt @@ -45,6 +45,5 @@ px4_add_module( MODULE_CONFIG ${module_config} DEPENDS - mixer mixer_module ) diff --git a/src/modules/simulation/pwm_out_sim/PWMSim.cpp b/src/modules/simulation/pwm_out_sim/PWMSim.cpp index c052e2a155..a7d23ea693 100644 --- a/src/modules/simulation/pwm_out_sim/PWMSim.cpp +++ b/src/modules/simulation/pwm_out_sim/PWMSim.cpp @@ -139,7 +139,7 @@ PWMSim::ioctl(device::file_t *filp, int cmd, unsigned long arg) struct pwm_output_values *pwm = (struct pwm_output_values *)arg; for (unsigned i = 0; i < pwm->channel_count; i++) { - if (i < OutputModuleInterface::MAX_ACTUATORS && !_mixing_output.useDynamicMixing()) { + if (i < OutputModuleInterface::MAX_ACTUATORS && false) { _mixing_output.minValue(i) = pwm->values[i]; } } @@ -151,7 +151,7 @@ PWMSim::ioctl(device::file_t *filp, int cmd, unsigned long arg) struct pwm_output_values *pwm = (struct pwm_output_values *)arg; for (unsigned i = 0; i < pwm->channel_count; i++) { - if (i < OutputModuleInterface::MAX_ACTUATORS && !_mixing_output.useDynamicMixing()) { + if (i < OutputModuleInterface::MAX_ACTUATORS && false) { _mixing_output.maxValue(i) = pwm->values[i]; } } diff --git a/src/modules/simulation/simulator_ignition_bridge/SimulatorIgnitionBridge.hpp b/src/modules/simulation/simulator_ignition_bridge/SimulatorIgnitionBridge.hpp index b9bf0af93f..70efe2c392 100644 --- a/src/modules/simulation/simulator_ignition_bridge/SimulatorIgnitionBridge.hpp +++ b/src/modules/simulation/simulator_ignition_bridge/SimulatorIgnitionBridge.hpp @@ -84,8 +84,6 @@ private: bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) override; - void mixerChanged() override {} - void Run() override; bool updateClock(const uint64_t tv_sec, const uint64_t tv_nsec); diff --git a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp index 0069814279..3e8ed33143 100644 --- a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp +++ b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp @@ -91,10 +91,6 @@ static constexpr vehicle_odometry_s vehicle_odometry_empty { SimulatorMavlink::SimulatorMavlink() : ModuleParams(nullptr) { - int32_t sys_ctrl_alloc = 0; - param_get(param_find("SYS_CTRL_ALLOC"), &sys_ctrl_alloc); - _use_dynamic_mixing = sys_ctrl_alloc >= 1; - for (int i = 0; i < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; ++i) { char param_name[17]; snprintf(param_name, sizeof(param_name), "%s_%s%d", "PWM_MAIN", "FUNC", i + 1); @@ -123,91 +119,9 @@ void SimulatorMavlink::actuator_controls_from_outputs(mavlink_hil_actuator_contr bool armed = (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); - int _system_type = _param_mav_type.get(); - - if (_use_dynamic_mixing) { - if (armed) { - for (unsigned i = 0; i < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; i++) { - msg->controls[i] = _actuator_outputs.output[i]; - } - } - - } else { - /* 'pos_thrust_motors_count' indicates number of motor channels which are configured with 0..1 range (positive thrust) - all other motors are configured for -1..1 range */ - unsigned pos_thrust_motors_count; - bool is_fixed_wing; - - switch (_system_type) { - case MAV_TYPE_AIRSHIP: - case MAV_TYPE_VTOL_TAILSITTER_DUOROTOR: - case MAV_TYPE_COAXIAL: - pos_thrust_motors_count = 2; - is_fixed_wing = false; - break; - - case MAV_TYPE_TRICOPTER: - pos_thrust_motors_count = 3; - is_fixed_wing = false; - break; - - case MAV_TYPE_QUADROTOR: - case MAV_TYPE_VTOL_TAILSITTER_QUADROTOR: - case MAV_TYPE_VTOL_TILTROTOR: - pos_thrust_motors_count = 4; - is_fixed_wing = false; - break; - - case MAV_TYPE_VTOL_FIXEDROTOR: - pos_thrust_motors_count = 5; - is_fixed_wing = false; - break; - - case MAV_TYPE_HEXAROTOR: - pos_thrust_motors_count = 6; - is_fixed_wing = false; - break; - - case MAV_TYPE_OCTOROTOR: - pos_thrust_motors_count = 8; - is_fixed_wing = false; - break; - - case MAV_TYPE_SUBMARINE: - pos_thrust_motors_count = 0; - is_fixed_wing = false; - break; - - case MAV_TYPE_FIXED_WING: - pos_thrust_motors_count = 0; - is_fixed_wing = true; - break; - - default: - pos_thrust_motors_count = 0; - is_fixed_wing = false; - break; - } - + if (armed) { for (unsigned i = 0; i < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; i++) { - if (!armed) { - /* send 0 when disarmed and for disabled channels */ - msg->controls[i] = 0.0f; - - } else if ((is_fixed_wing && i == 4) || - (!is_fixed_wing && i < pos_thrust_motors_count)) { //multirotor, rotor channel - /* scale PWM out PWM_DEFAULT_MIN..PWM_DEFAULT_MAX us to 0..1 for rotors */ - msg->controls[i] = (_actuator_outputs.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN); - msg->controls[i] = math::constrain(msg->controls[i], 0.f, 1.f); - - } else { - const float pwm_center = (PWM_DEFAULT_MAX + PWM_DEFAULT_MIN) / 2; - const float pwm_delta = (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2; - - /* scale PWM out PWM_DEFAULT_MIN..PWM_DEFAULT_MAX us to -1..1 for other channels */ - msg->controls[i] = (_actuator_outputs.output[i] - pwm_center) / pwm_delta; - msg->controls[i] = math::constrain(msg->controls[i], -1.f, 1.f); - } + msg->controls[i] = _actuator_outputs.output[i]; } } @@ -1065,12 +979,7 @@ void SimulatorMavlink::send() // Subscribe to topics. // Only subscribe to the first actuator_outputs to fill a single HIL_ACTUATOR_CONTROLS. - if (_use_dynamic_mixing) { - _actuator_outputs_sub = orb_subscribe_multi(ORB_ID(actuator_outputs_sim), 0); - - } else { - _actuator_outputs_sub = orb_subscribe_multi(ORB_ID(actuator_outputs), 0); - } + _actuator_outputs_sub = orb_subscribe_multi(ORB_ID(actuator_outputs_sim), 0); // Before starting, we ought to send a heartbeat to initiate the SITL // simulator to start sending sensor data which will set the time and diff --git a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp index 6df848ee0f..e3822d36e3 100644 --- a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp +++ b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp @@ -296,7 +296,6 @@ private: float _last_magx{0.0f}; float _last_magy{0.0f}; float _last_magz{0.0f}; - bool _use_dynamic_mixing{false}; float _last_baro_pressure{0.0f}; float _last_baro_temperature{0.0f};