From 79dc676c8f5e7bedc0ac3a3bc1651ab100be1705 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 20 Nov 2019 21:26:43 -0500 Subject: [PATCH] mixer: move MixerGroup out of Mixer hierarchy and delete mixer.h header --- .../df_bebop_bus_wrapper.cpp | 6 +- src/drivers/linux_pwm_out/linux_pwm_out.cpp | 6 +- src/drivers/mkblctrl/mkblctrl.cpp | 6 +- src/drivers/pwm_out_sim/PWMSim.hpp | 3 +- src/drivers/px4io/px4io.cpp | 3 +- .../qurt/fc_addon/uart_esc/uart_esc_main.cpp | 2 +- .../snapdragon_pwm_out/snapdragon_pwm_out.cpp | 60 +++----- src/drivers/tap_esc/tap_esc.cpp | 7 +- src/drivers/uavcan/uavcan_main.cpp | 1 - src/drivers/uavcan/uavcan_servers.cpp | 1 - src/drivers/uavcanesc/uavcanesc_main.cpp | 2 +- src/drivers/uavcannode/uavcannode_main.cpp | 2 +- src/lib/mixer/HelicopterMixer.cpp | 11 +- src/lib/mixer/HelicopterMixer.hpp | 6 +- src/lib/mixer/Mixer.cpp | 8 - src/lib/mixer/Mixer.hpp | 105 +++++++++++-- src/lib/mixer/MixerGroup.cpp | 40 ++--- src/lib/mixer/MixerGroup.hpp | 34 +++-- src/lib/mixer/MultirotorMixer.cpp | 35 ++--- src/lib/mixer/MultirotorMixer.hpp | 13 +- src/lib/mixer/NullMixer.cpp | 7 +- src/lib/mixer/NullMixer.hpp | 10 +- src/lib/mixer/SimpleMixer.cpp | 6 +- src/lib/mixer/SimpleMixer.hpp | 19 ++- src/lib/mixer/load_mixer_file.cpp | 4 +- src/lib/mixer/mixer.h | 138 ------------------ src/lib/mixer/test_mixer_multirotor.cpp | 3 +- src/lib/mixer_module/mixer_module.cpp | 6 +- src/lib/mixer_module/mixer_module.hpp | 3 +- src/modules/mc_att_control/mc_att_control.hpp | 2 +- .../MulticopterRateControl.hpp | 1 - .../RateControl/RateControl.hpp | 2 +- src/modules/px4iofirmware/mixer.cpp | 8 +- src/systemcmds/mixer/mixer.cpp | 2 +- src/systemcmds/tests/test_autodeclination.cpp | 1 - src/systemcmds/tests/test_mixer.cpp | 22 +-- 36 files changed, 233 insertions(+), 352 deletions(-) delete mode 100644 src/lib/mixer/mixer.h diff --git a/src/drivers/driver_framework_wrapper/df_bebop_bus_wrapper/df_bebop_bus_wrapper.cpp b/src/drivers/driver_framework_wrapper/df_bebop_bus_wrapper/df_bebop_bus_wrapper.cpp index 548b537f6d..7feaab4aaa 100644 --- a/src/drivers/driver_framework_wrapper/df_bebop_bus_wrapper/df_bebop_bus_wrapper.cpp +++ b/src/drivers/driver_framework_wrapper/df_bebop_bus_wrapper/df_bebop_bus_wrapper.cpp @@ -56,7 +56,7 @@ #include #include -#include +#include #include #include @@ -307,7 +307,7 @@ int initialize_mixers(const char *mixers_filename) } if (_mixers == nullptr) { - _mixers = new MixerGroup(mixers_control_callback, (uintptr_t)_controls); + _mixers = new MixerGroup(); } if (_mixers == nullptr) { @@ -315,7 +315,7 @@ int initialize_mixers(const char *mixers_filename) return -1; } else { - int ret = _mixers->load_from_buf(buf, buflen); + int ret = _mixers->load_from_buf(mixers_control_callback, (uintptr_t)_controls, buf, buflen); if (ret != 0) { PX4_ERR("Unable to parse mixers file"); diff --git a/src/drivers/linux_pwm_out/linux_pwm_out.cpp b/src/drivers/linux_pwm_out/linux_pwm_out.cpp index 3bd25dfdaa..603ac9c87b 100644 --- a/src/drivers/linux_pwm_out/linux_pwm_out.cpp +++ b/src/drivers/linux_pwm_out/linux_pwm_out.cpp @@ -51,7 +51,7 @@ #include #include -#include +#include #include #include #include @@ -155,12 +155,12 @@ int initialize_mixer(const char *mixer_filename) unsigned buflen = sizeof(buf); memset(buf, '\0', buflen); - _mixer_group = new MixerGroup(mixer_control_callback, (uintptr_t) &_controls); + _mixer_group = new MixerGroup(); // PX4_INFO("Trying to initialize mixer from config file %s", mixer_filename); if (load_mixer_file(mixer_filename, buf, buflen) == 0) { - if (_mixer_group->load_from_buf(buf, buflen) == 0) { + if (_mixer_group->load_from_buf(mixer_control_callback, (uintptr_t) &_controls, buf, buflen) == 0) { PX4_INFO("Loaded mixer from file %s", mixer_filename); return 0; diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index b58fc98324..c11a0b5e2c 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -72,7 +72,7 @@ #include #include -#include +#include #include #include @@ -1045,7 +1045,7 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg) unsigned buflen = strlen(buf); if (_mixers == nullptr) { - _mixers = new MixerGroup(control_callback, (uintptr_t)&_controls); + _mixers = new MixerGroup(); } if (_mixers == nullptr) { @@ -1053,7 +1053,7 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg) } else { - ret = _mixers->load_from_buf(buf, buflen); + ret = _mixers->load_from_buf(control_callback, (uintptr_t)&_controls, buf, buflen); if (ret != 0) { DEVICE_DEBUG("mixer load failed with %d", ret); diff --git a/src/drivers/pwm_out_sim/PWMSim.hpp b/src/drivers/pwm_out_sim/PWMSim.hpp index 191b5da0a6..7282484237 100644 --- a/src/drivers/pwm_out_sim/PWMSim.hpp +++ b/src/drivers/pwm_out_sim/PWMSim.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2018 PX4 Development Team. All rights reserved. + * 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 @@ -37,7 +37,6 @@ #include #include #include -#include #include #include #include diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index aff695a78e..686c3e45b8 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -70,7 +70,8 @@ #include #include -#include +#include +#include #include #include #include diff --git a/src/drivers/qurt/fc_addon/uart_esc/uart_esc_main.cpp b/src/drivers/qurt/fc_addon/uart_esc/uart_esc_main.cpp index 1b76cbc289..52a67e7768 100644 --- a/src/drivers/qurt/fc_addon/uart_esc/uart_esc_main.cpp +++ b/src/drivers/qurt/fc_addon/uart_esc/uart_esc_main.cpp @@ -46,7 +46,7 @@ #include #include -#include +#include #include #include diff --git a/src/drivers/snapdragon_pwm_out/snapdragon_pwm_out.cpp b/src/drivers/snapdragon_pwm_out/snapdragon_pwm_out.cpp index 9375f55ebf..ffad33f4af 100644 --- a/src/drivers/snapdragon_pwm_out/snapdragon_pwm_out.cpp +++ b/src/drivers/snapdragon_pwm_out/snapdragon_pwm_out.cpp @@ -48,12 +48,13 @@ #include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include + #include /* @@ -174,55 +175,30 @@ void update_params(Mixer::Airmode &airmode) int initialize_mixer(const char *mixer_filename) { - char buf[2048]; size_t buflen = sizeof(buf); PX4_INFO("Trying to initialize mixer from config file %s", mixer_filename); int fd_load = ::open(mixer_filename, O_RDONLY); - if (fd_load != -1) { - int nRead = ::read(fd_load, buf, buflen); - close(fd_load); + int nRead = ::read(fd_load, buf, buflen); + close(fd_load); - if (nRead > 0) { - _mixer = MultirotorMixer::from_text(mixer_control_callback, (uintptr_t)&_controls, buf, buflen); + if (nRead > 0) { + _mixer = MultirotorMixer::from_text(mixer_control_callback, (uintptr_t)&_controls, buf, buflen); - if (_mixer != nullptr) { - PX4_INFO("Successfully initialized mixer from config file"); - return 0; - - } else { - PX4_ERR("Unable to parse from mixer config file"); - return -1; - } + if (_mixer != nullptr) { + PX4_INFO("Successfully initialized mixer from config file"); + return 0; } else { - PX4_WARN("Unable to read from mixer config file"); - return -2; - } - - } else { - PX4_WARN("No mixer config file found, using default mixer."); - - /* Mixer file loading failed, fall back to default mixer configuration for - * QUAD_X airframe. */ - float roll_scale = 1; - float pitch_scale = 1; - float yaw_scale = 1; - float deadband = 0; - - _mixer = new MultirotorMixer(mixer_control_callback, (uintptr_t)&_controls, - MultirotorGeometry::QUAD_X, - roll_scale, pitch_scale, yaw_scale, deadband); - - if (_mixer == nullptr) { + PX4_ERR("Unable to parse from mixer config file"); return -1; } - return 0; - + } else { + PX4_WARN("Unable to read from mixer config file"); + return -2; } - } void subscribe() diff --git a/src/drivers/tap_esc/tap_esc.cpp b/src/drivers/tap_esc/tap_esc.cpp index fe7fa136f9..911be2bb0a 100644 --- a/src/drivers/tap_esc/tap_esc.cpp +++ b/src/drivers/tap_esc/tap_esc.cpp @@ -59,7 +59,7 @@ #include #include -#include +#include #include "tap_esc_common.h" @@ -677,7 +677,7 @@ int TAP_ESC::ioctl(cdev::file_t *filp, int cmd, unsigned long arg) unsigned buflen = strlen(buf); if (_mixers == nullptr) { - _mixers = new MixerGroup(control_callback_trampoline, (uintptr_t)this); + _mixers = new MixerGroup(); } if (_mixers == nullptr) { @@ -685,8 +685,7 @@ int TAP_ESC::ioctl(cdev::file_t *filp, int cmd, unsigned long arg) ret = -ENOMEM; } else { - - ret = _mixers->load_from_buf(buf, buflen); + ret = _mixers->load_from_buf(control_callback_trampoline, (uintptr_t)this, buf, buflen); if (ret != 0) { PX4_DEBUG("mixer load failed with %d", ret); diff --git a/src/drivers/uavcan/uavcan_main.cpp b/src/drivers/uavcan/uavcan_main.cpp index f3c69a5034..4b080a62bc 100644 --- a/src/drivers/uavcan/uavcan_main.cpp +++ b/src/drivers/uavcan/uavcan_main.cpp @@ -50,7 +50,6 @@ #include #include #include -#include #include #include #include diff --git a/src/drivers/uavcan/uavcan_servers.cpp b/src/drivers/uavcan/uavcan_servers.cpp index 9b736fa167..8d8bc83acd 100644 --- a/src/drivers/uavcan/uavcan_servers.cpp +++ b/src/drivers/uavcan/uavcan_servers.cpp @@ -44,7 +44,6 @@ #include #include #include -#include #include #include #include diff --git a/src/drivers/uavcanesc/uavcanesc_main.cpp b/src/drivers/uavcanesc/uavcanesc_main.cpp index 7e2105bc66..902cdb6f5a 100644 --- a/src/drivers/uavcanesc/uavcanesc_main.cpp +++ b/src/drivers/uavcanesc/uavcanesc_main.cpp @@ -43,7 +43,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/src/drivers/uavcannode/uavcannode_main.cpp b/src/drivers/uavcannode/uavcannode_main.cpp index f378f4d45e..5f1ee7da9f 100644 --- a/src/drivers/uavcannode/uavcannode_main.cpp +++ b/src/drivers/uavcannode/uavcannode_main.cpp @@ -46,7 +46,7 @@ #include #include #include -#include +#include #include __BEGIN_DECLS #include diff --git a/src/lib/mixer/HelicopterMixer.cpp b/src/lib/mixer/HelicopterMixer.cpp index f331f926a2..e202367e97 100644 --- a/src/lib/mixer/HelicopterMixer.cpp +++ b/src/lib/mixer/HelicopterMixer.cpp @@ -37,8 +37,6 @@ * Helicopter mixers. */ -#include "mixer.h" - #include "HelicopterMixer.hpp" #include @@ -52,9 +50,9 @@ using math::constrain; -HelicopterMixer::HelicopterMixer(ControlCallback control_cb, uintptr_t cb_handle, mixer_heli_s *mixer_info) : +HelicopterMixer::HelicopterMixer(ControlCallback control_cb, uintptr_t cb_handle, mixer_heli_s mixer_info) : Mixer(control_cb, cb_handle), - _mixer_info(*mixer_info) + _mixer_info(mixer_info) { } @@ -182,10 +180,7 @@ HelicopterMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl debug("remaining in buf: %d, first char: %c", buflen, buf[0]); - HelicopterMixer *hm = new HelicopterMixer( - control_cb, - cb_handle, - &mixer_info); + 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); diff --git a/src/lib/mixer/HelicopterMixer.hpp b/src/lib/mixer/HelicopterMixer.hpp index e932708b0f..68ee9dcac6 100644 --- a/src/lib/mixer/HelicopterMixer.hpp +++ b/src/lib/mixer/HelicopterMixer.hpp @@ -33,6 +33,8 @@ #pragma once +#include "Mixer.hpp" + /** helicopter swash servo mixer */ struct mixer_heli_servo_s { float angle; @@ -50,7 +52,7 @@ 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]; - struct mixer_heli_servo_s servos[4]; /**< up to four inputs */ + mixer_heli_servo_s servos[4]; /**< up to four inputs */ }; /** @@ -69,7 +71,7 @@ public: * @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); + HelicopterMixer(ControlCallback control_cb, uintptr_t cb_handle, mixer_heli_s mixer_info); virtual ~HelicopterMixer() = default; // no copy, assignment, move, move assignment diff --git a/src/lib/mixer/Mixer.cpp b/src/lib/mixer/Mixer.cpp index d5e86fee86..b38b759a3f 100644 --- a/src/lib/mixer/Mixer.cpp +++ b/src/lib/mixer/Mixer.cpp @@ -37,8 +37,6 @@ * Programmable multi-channel mixer library. */ -#include "mixer.h" - #include "Mixer.hpp" #include @@ -48,12 +46,6 @@ #define debug(fmt, args...) do { } while(0) //#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0) -Mixer::Mixer(ControlCallback control_cb, uintptr_t cb_handle) : - _control_cb(control_cb), - _cb_handle(cb_handle) -{ -} - float Mixer::get_control(uint8_t group, uint8_t index) { diff --git a/src/lib/mixer/Mixer.hpp b/src/lib/mixer/Mixer.hpp index 5cf2f35a1f..c6a37699d4 100644 --- a/src/lib/mixer/Mixer.hpp +++ b/src/lib/mixer/Mixer.hpp @@ -31,16 +31,102 @@ * ****************************************************************************/ +/** + * @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 -/** simple channel scaler */ -struct mixer_scaler_s { - float negative_scale; - float positive_scale; - float offset; - float min_output; - float max_output; -}; +#include /** * Abstract class defining a mixer mixing zero or more inputs to @@ -74,7 +160,7 @@ public: * * @param control_cb Callback invoked when reading controls. */ - Mixer(ControlCallback control_cb, uintptr_t cb_handle); + 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 @@ -146,6 +232,7 @@ public: virtual unsigned get_multirotor_count() { return 0; } protected: + /** client-supplied callback used when fetching control values */ ControlCallback _control_cb; uintptr_t _cb_handle; diff --git a/src/lib/mixer/MixerGroup.cpp b/src/lib/mixer/MixerGroup.cpp index 727119d400..7f69d44edb 100644 --- a/src/lib/mixer/MixerGroup.cpp +++ b/src/lib/mixer/MixerGroup.cpp @@ -37,39 +37,19 @@ * Mixer collection. */ -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "mixer.h" +#include "MixerGroup.hpp" #include "MixerGroup.hpp" +#include "NullMixer.hpp" +#include "SimpleMixer.hpp" +#include "MultirotorMixer.hpp" +#include "HelicopterMixer.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) -MixerGroup::MixerGroup(ControlCallback control_cb, uintptr_t cb_handle) : - Mixer(control_cb, cb_handle) -{ -} - -MixerGroup::~MixerGroup() -{ - reset(); -} - void MixerGroup::add_mixer(Mixer *mixer) { @@ -187,7 +167,7 @@ MixerGroup::set_thrust_factor(float val) } void -MixerGroup::set_airmode(Airmode airmode) +MixerGroup::set_airmode(Mixer::Airmode airmode) { Mixer *mixer = _first; @@ -255,7 +235,7 @@ MixerGroup::groups_required(uint32_t &groups) } int -MixerGroup::load_from_buf(const char *buf, unsigned &buflen) +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; @@ -278,15 +258,15 @@ MixerGroup::load_from_buf(const char *buf, unsigned &buflen) break; case 'M': - m = SimpleMixer::from_text(_control_cb, _cb_handle, p, resid); + m = SimpleMixer::from_text(control_cb, cb_handle, p, resid); break; case 'R': - m = MultirotorMixer::from_text(_control_cb, _cb_handle, p, resid); + m = MultirotorMixer::from_text(control_cb, cb_handle, p, resid); break; case 'H': - m = HelicopterMixer::from_text(_control_cb, _cb_handle, p, resid); + m = HelicopterMixer::from_text(control_cb, cb_handle, p, resid); break; default: diff --git a/src/lib/mixer/MixerGroup.hpp b/src/lib/mixer/MixerGroup.hpp index 65eac5c790..3f17867e96 100644 --- a/src/lib/mixer/MixerGroup.hpp +++ b/src/lib/mixer/MixerGroup.hpp @@ -33,15 +33,21 @@ #pragma once +#include "Mixer.hpp" + /** * Group of mixers, built up from single mixers and processed * in order when mixing. */ -class MixerGroup : public Mixer +class MixerGroup { public: - MixerGroup(ControlCallback control_cb, uintptr_t cb_handle); - virtual ~MixerGroup(); + MixerGroup() = default; + + ~MixerGroup() + { + reset(); + } // no copy, assignment, move, move assignment MixerGroup(const MixerGroup &) = delete; @@ -49,9 +55,11 @@ public: MixerGroup(MixerGroup &&) = delete; MixerGroup &operator=(MixerGroup &&) = delete; - unsigned mix(float *outputs, unsigned space) override; - uint16_t get_saturation_status() override; - void groups_required(uint32_t &groups) override; + unsigned mix(float *outputs, unsigned space); + + uint16_t get_saturation_status(); + + void groups_required(uint32_t &groups); /** * Add a mixer to the group. @@ -125,7 +133,7 @@ public: * bytes as they are consumed. * @return Zero on successful load, nonzero otherwise. */ - int load_from_buf(const char *buf, unsigned &buflen); + 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 @@ -137,25 +145,25 @@ public: * @param[in] delta_out_max Maximum delta output. * */ - void set_max_delta_out_once(float delta_out_max) override; + 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); + 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) override; + void set_thrust_factor(float val); - void set_airmode(Airmode airmode) override; + void set_airmode(Mixer::Airmode airmode); - unsigned get_multirotor_count() override; + unsigned get_multirotor_count(); private: Mixer *_first{nullptr}; /**< linked list of mixers */ diff --git a/src/lib/mixer/MultirotorMixer.cpp b/src/lib/mixer/MultirotorMixer.cpp index 6c0393dffd..47b41a58b0 100644 --- a/src/lib/mixer/MultirotorMixer.cpp +++ b/src/lib/mixer/MultirotorMixer.cpp @@ -37,7 +37,7 @@ * Multi-rotor mixers. */ -#include "mixer.h" +#include "MultirotorMixer.hpp" #include #include @@ -45,8 +45,6 @@ #include -#include "MultirotorMixer.hpp" - #ifdef MIXER_MULTIROTOR_USE_MOCK_GEOMETRY enum class MultirotorGeometry : MultirotorGeometryUnderlyingType { QUAD_X, @@ -81,28 +79,18 @@ const char *_config_key[] = {"4x"}; //#include //#define debug(fmt, args...) syslog(fmt "\n", ##args) -MultirotorMixer::MultirotorMixer(ControlCallback control_cb, - uintptr_t cb_handle, - MultirotorGeometry geometry, - float roll_scale, - float pitch_scale, - float yaw_scale, - float idle_speed) : - Mixer(control_cb, cb_handle), - _roll_scale(roll_scale), - _pitch_scale(pitch_scale), - _yaw_scale(yaw_scale), - _idle_speed(-1.0f + idle_speed * 2.0f), /* shift to output range here to avoid runtime calculation */ - _rotor_count(_config_rotor_count[(MultirotorGeometryUnderlyingType)geometry]), - _rotors(_config_index[(MultirotorGeometryUnderlyingType)geometry]), - _outputs_prev(new float[_rotor_count]), - _tmp_array(new float[_rotor_count]) +MultirotorMixer::MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle, MultirotorGeometry geometry, + float roll_scale, float pitch_scale, float yaw_scale, float idle_speed) : + MultirotorMixer(control_cb, cb_handle, _config_index[(int)geometry], _config_rotor_count[(int)geometry]) { - for (unsigned i = 0; i < _rotor_count; ++i) { - _outputs_prev[i] = _idle_speed; - } + _roll_scale = roll_scale; + _pitch_scale = pitch_scale; + _yaw_scale = yaw_scale; + _idle_speed = -1.0f + idle_speed * 2.0f; /* shift to output range here to avoid runtime calculation */ } -MultirotorMixer::MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle, Rotor *rotors, unsigned rotor_count) : + +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), @@ -423,7 +411,6 @@ MultirotorMixer::mix(float *outputs, unsigned space) outputs[i] = _outputs_prev[i] - _delta_out_max; clipping_low_roll_pitch = true; clipping_low_yaw = true; - } } diff --git a/src/lib/mixer/MultirotorMixer.hpp b/src/lib/mixer/MultirotorMixer.hpp index ac7a120506..96c375852b 100644 --- a/src/lib/mixer/MultirotorMixer.hpp +++ b/src/lib/mixer/MultirotorMixer.hpp @@ -33,6 +33,8 @@ #pragma once +#include "Mixer.hpp" + /** * Supported multirotor geometries. * @@ -77,13 +79,8 @@ public: * 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, - float roll_scale, - float pitch_scale, - float yaw_scale, - float idle_speed); + MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle, MultirotorGeometry geometry, + float roll_scale, float pitch_scale, float yaw_scale, float idle_speed); /** * Constructor (for testing). @@ -93,7 +90,7 @@ public: * @param rotors control allocation matrix * @param rotor_count length of rotors array (= number of motors) */ - MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle, Rotor *rotors, unsigned rotor_count); + MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle, const Rotor *rotors, unsigned rotor_count); virtual ~MultirotorMixer(); // no copy, assignment, move, move assignment diff --git a/src/lib/mixer/NullMixer.cpp b/src/lib/mixer/NullMixer.cpp index 8fe7e13c8c..636cddd62d 100644 --- a/src/lib/mixer/NullMixer.cpp +++ b/src/lib/mixer/NullMixer.cpp @@ -31,17 +31,12 @@ * ****************************************************************************/ -#include "mixer.h" +#include "NullMixer.hpp" #include #include #include -NullMixer::NullMixer() : - Mixer(nullptr, 0) -{ -} - unsigned NullMixer::mix(float *outputs, unsigned space) { diff --git a/src/lib/mixer/NullMixer.hpp b/src/lib/mixer/NullMixer.hpp index acd220774f..8a4843ece5 100644 --- a/src/lib/mixer/NullMixer.hpp +++ b/src/lib/mixer/NullMixer.hpp @@ -33,6 +33,8 @@ #pragma once +#include "Mixer.hpp" + /** * Null mixer; returns zero. * @@ -41,7 +43,7 @@ class NullMixer : public Mixer { public: - NullMixer(); + NullMixer() : Mixer(nullptr, 0) {} virtual ~NullMixer() = default; // no copy, assignment, move, move assignment @@ -65,9 +67,9 @@ public: */ static NullMixer *from_text(const char *buf, unsigned &buflen); - unsigned mix(float *outputs, unsigned space) override; + unsigned mix(float *outputs, unsigned space) override; - unsigned set_trim(float trim) override { return 1; } - unsigned get_trim(float *trim) override { return 1; } + unsigned set_trim(float trim) override { return 1; } + unsigned get_trim(float *trim) override { return 1; } }; diff --git a/src/lib/mixer/SimpleMixer.cpp b/src/lib/mixer/SimpleMixer.cpp index 8b127fb079..50413607d2 100644 --- a/src/lib/mixer/SimpleMixer.cpp +++ b/src/lib/mixer/SimpleMixer.cpp @@ -37,7 +37,8 @@ * Simple summing mixer. */ -#include "mixer.h" +#include "SimpleMixer.hpp" + #include #include @@ -190,7 +191,6 @@ SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, c 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 @@ -202,7 +202,6 @@ SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, c mixinfo->output_scaler.max_output = 1.0f; } else { - if (parse_output_scaler(end - buflen, buflen, mixinfo->output_scaler)) { debug("simple mixer parser failed parsing out scaler tag, ret: '%s'", buf); goto out; @@ -217,7 +216,6 @@ SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, c debug("simple mixer parser failed parsing ctrl scaler tag, ret: '%s'", buf); goto out; } - } sm = new SimpleMixer(control_cb, cb_handle, mixinfo); diff --git a/src/lib/mixer/SimpleMixer.hpp b/src/lib/mixer/SimpleMixer.hpp index b57ea150ce..e6809ef4b3 100644 --- a/src/lib/mixer/SimpleMixer.hpp +++ b/src/lib/mixer/SimpleMixer.hpp @@ -33,11 +33,22 @@ #pragma once +#include "Mixer.hpp" + +/** 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 */ - struct mixer_scaler_s scaler; /**< scaling applied to the input before use */ + 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)) @@ -45,8 +56,8 @@ struct mixer_control_s { /** simple mixer */ struct mixer_simple_s { uint8_t control_count; /**< number of inputs */ - struct mixer_scaler_s output_scaler; /**< scaling for the output */ - struct mixer_control_s controls[0]; /**< actual size of the array is set by control_count */ + mixer_scaler_s output_scaler; /**< scaling for the output */ + mixer_control_s controls[]; /**< actual size of the array is set by control_count */ }; /** @@ -132,6 +143,6 @@ private: static int parse_control_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler, uint8_t &control_group, uint8_t &control_index); - mixer_simple_s *_pinfo{nullptr}; + mixer_simple_s *_pinfo; }; diff --git a/src/lib/mixer/load_mixer_file.cpp b/src/lib/mixer/load_mixer_file.cpp index 6d04a3d3b9..52fd84b6de 100644 --- a/src/lib/mixer/load_mixer_file.cpp +++ b/src/lib/mixer/load_mixer_file.cpp @@ -45,8 +45,8 @@ int load_mixer_file(const char *fname, char *buf, unsigned maxlen) { - FILE *fp; - char line[120]; + FILE *fp; + char line[120]; /* open the mixer definition file */ fp = fopen(fname, "r"); diff --git a/src/lib/mixer/mixer.h b/src/lib/mixer/mixer.h deleted file mode 100644 index e8b0d3d643..0000000000 --- a/src/lib/mixer/mixer.h +++ /dev/null @@ -1,138 +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.h - * - * 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 - -#include "Mixer.hpp" -#include "MixerGroup.hpp" -#include "NullMixer.hpp" -#include "SimpleMixer.hpp" -#include "MultirotorMixer.hpp" -#include "HelicopterMixer.hpp" diff --git a/src/lib/mixer/test_mixer_multirotor.cpp b/src/lib/mixer/test_mixer_multirotor.cpp index edba644f59..46af77012f 100644 --- a/src/lib/mixer/test_mixer_multirotor.cpp +++ b/src/lib/mixer/test_mixer_multirotor.cpp @@ -36,7 +36,8 @@ * via file or stdin and compares the mixer output against expected values. */ -#include "mixer.h" +#include "MultirotorMixer.hpp" + #include #include diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index 380071b883..0ded9924c6 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -33,6 +33,8 @@ #include "mixer_module.hpp" +#include + #include #include @@ -543,7 +545,7 @@ void MixingOutput::resetMixer() int MixingOutput::loadMixer(const char *buf, unsigned len) { if (_mixers == nullptr) { - _mixers = new MixerGroup(controlCallback, (uintptr_t)this); + _mixers = new MixerGroup(); } if (_mixers == nullptr) { @@ -551,7 +553,7 @@ int MixingOutput::loadMixer(const char *buf, unsigned len) return -ENOMEM; } - int ret = _mixers->load_from_buf(buf, len); + int ret = _mixers->load_from_buf(controlCallback, (uintptr_t)this, buf, len); if (ret != 0) { PX4_ERR("mixer load failed with %d", ret); diff --git a/src/lib/mixer_module/mixer_module.hpp b/src/lib/mixer_module/mixer_module.hpp index 11b1943a89..8fbeaca48d 100644 --- a/src/lib/mixer_module/mixer_module.hpp +++ b/src/lib/mixer_module/mixer_module.hpp @@ -35,7 +35,7 @@ #include #include -#include +#include #include #include #include @@ -52,7 +52,6 @@ #include #include - /** * @class OutputModuleInterface * Base class for an output module. diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp index 69bfa5654c..24b4fb8840 100644 --- a/src/modules/mc_att_control/mc_att_control.hpp +++ b/src/modules/mc_att_control/mc_att_control.hpp @@ -33,7 +33,7 @@ #pragma once -#include +#include #include #include #include diff --git a/src/modules/mc_rate_control/MulticopterRateControl.hpp b/src/modules/mc_rate_control/MulticopterRateControl.hpp index 76e0848388..7bf82c4ac9 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.hpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.hpp @@ -37,7 +37,6 @@ #include #include -#include #include #include #include diff --git a/src/modules/mc_rate_control/RateControl/RateControl.hpp b/src/modules/mc_rate_control/RateControl/RateControl.hpp index 195951195e..b3d495c21a 100644 --- a/src/modules/mc_rate_control/RateControl/RateControl.hpp +++ b/src/modules/mc_rate_control/RateControl/RateControl.hpp @@ -42,7 +42,7 @@ #include #include -#include +#include #include class RateControl diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 1f53f62f74..96ce0846a4 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -51,7 +51,7 @@ #include #include -#include +#include #include #include @@ -96,7 +96,7 @@ static volatile mixer_source source; static int mixer_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &control); static int mixer_mix_threadsafe(float *outputs, volatile uint16_t *limits); -static MixerGroup mixer_group(mixer_callback, 0); +static MixerGroup mixer_group; int mixer_mix_threadsafe(float *outputs, volatile uint16_t *limits) { @@ -114,7 +114,7 @@ int mixer_mix_threadsafe(float *outputs, volatile uint16_t *limits) } void -mixer_tick(void) +mixer_tick() { /* check if the mixer got modified */ mixer_handle_text_create_mixer(); @@ -540,7 +540,7 @@ mixer_handle_text_create_mixer() /* process the text buffer, adding new mixers as their descriptions can be parsed */ unsigned resid = mixer_text_length; - mixer_group.load_from_buf(&mixer_text[0], resid); + mixer_group.load_from_buf(mixer_callback, 0, &mixer_text[0], resid); /* if anything was parsed */ if (resid != mixer_text_length) { diff --git a/src/systemcmds/mixer/mixer.cpp b/src/systemcmds/mixer/mixer.cpp index 75c7ca5e37..671b5dcadb 100644 --- a/src/systemcmds/mixer/mixer.cpp +++ b/src/systemcmds/mixer/mixer.cpp @@ -49,7 +49,7 @@ #include #include -#include +#include #include #include diff --git a/src/systemcmds/tests/test_autodeclination.cpp b/src/systemcmds/tests/test_autodeclination.cpp index 908af9c555..01c1308f68 100644 --- a/src/systemcmds/tests/test_autodeclination.cpp +++ b/src/systemcmds/tests/test_autodeclination.cpp @@ -43,7 +43,6 @@ #include #include #include -#include #include #include diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index 89183bbf58..cadc6f4072 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -43,8 +43,8 @@ #include #include -#include -#include +#include +#include #include #include #include @@ -95,7 +95,7 @@ class MixerTest : public UnitTest { public: virtual bool run_tests(); - MixerTest(); + MixerTest() = default; private: bool mixerTest(); @@ -112,11 +112,6 @@ private: MixerGroup mixer_group; }; -MixerTest::MixerTest() : - mixer_group(mixer_callback, 0) -{ -} - bool MixerTest::run_tests() { ut_run_test(loadIOPass); @@ -251,11 +246,8 @@ bool MixerTest::load_mixer(const char *filename, unsigned expected_count, bool v } bool MixerTest::load_mixer(const char *filename, const char *buf, unsigned loaded, unsigned expected_count, - const unsigned chunk_size, - bool verbose) + const unsigned chunk_size, bool verbose) { - - /* load the mixer in chunks, like * in the case of a remote load, * e.g. on PX4IO. @@ -264,7 +256,7 @@ bool MixerTest::load_mixer(const char *filename, const char *buf, unsigned loade /* load at once test */ unsigned xx = loaded; mixer_group.reset(); - mixer_group.load_from_buf(&buf[0], xx); + mixer_group.load_from_buf(mixer_callback, 0, &buf[0], xx); if (expected_count > 0) { ut_compare("check number of mixers loaded", mixer_group.count(), expected_count); @@ -275,7 +267,7 @@ bool MixerTest::load_mixer(const char *filename, const char *buf, unsigned loade empty_buf[0] = ' '; empty_buf[1] = '\0'; mixer_group.reset(); - mixer_group.load_from_buf(&empty_buf[0], empty_load); + mixer_group.load_from_buf(mixer_callback, 0, &empty_buf[0], empty_load); if (verbose) { PX4_INFO("empty buffer load: loaded %u mixers, used: %u", mixer_group.count(), empty_load); @@ -310,7 +302,7 @@ bool MixerTest::load_mixer(const char *filename, const char *buf, unsigned loade /* process the text buffer, adding new mixers as their descriptions can be parsed */ resid = mixer_text_length; - mixer_group.load_from_buf(&mixer_text[0], resid); + mixer_group.load_from_buf(mixer_callback, 0, &mixer_text[0], resid); /* if anything was parsed */ if (resid != mixer_text_length) {