From 38fa65a47eba7232638ad15a6ab0db6081016a29 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Sat, 11 Sep 2021 17:29:53 +0200 Subject: [PATCH] control_allocator: remove direct mixer, add actuator_{motors,servos} instead --- .../airframes/10017_iris_ctrlalloc | 3 +- .../airframes/6012_typhoon_h480_ctrlalloc | 4 +- .../init.d/airframes/4018_s500_ctrlalloc | 5 +- .../init.d/airframes/6003_hexa_x_ctrlalloc | 7 +- .../px4fmu_common/mixers-sitl/CMakeLists.txt | 1 - .../tiltrotor_sitl_direct.main.mix | 14 -- ROMFS/px4fmu_common/mixers/CMakeLists.txt | 1 - ROMFS/px4fmu_common/mixers/direct.main.mix | 11 -- msg/CMakeLists.txt | 2 + msg/actuator_controls.msg | 5 +- msg/actuator_motors.msg | 9 ++ msg/actuator_servos.msg | 9 ++ .../AllocatedActuatorMixer.cpp | 149 ------------------ .../AllocatedActuatorMixer.hpp | 103 ------------ .../AllocatedActuatorMixer/CMakeLists.txt | 39 ----- src/lib/mixer/CMakeLists.txt | 2 - src/lib/mixer/MixerGroup.cpp | 5 - src/lib/mixer_module/mixer_module.cpp | 18 +-- .../ControlAllocation/ControlAllocation.cpp | 4 +- .../control_allocator/ControlAllocator.cpp | 21 +-- .../control_allocator/ControlAllocator.hpp | 8 +- 21 files changed, 49 insertions(+), 371 deletions(-) delete mode 100644 ROMFS/px4fmu_common/mixers-sitl/tiltrotor_sitl_direct.main.mix delete mode 100644 ROMFS/px4fmu_common/mixers/direct.main.mix create mode 100644 msg/actuator_motors.msg create mode 100644 msg/actuator_servos.msg delete mode 100644 src/lib/mixer/AllocatedActuatorMixer/AllocatedActuatorMixer.cpp delete mode 100644 src/lib/mixer/AllocatedActuatorMixer/AllocatedActuatorMixer.hpp delete mode 100644 src/lib/mixer/AllocatedActuatorMixer/CMakeLists.txt diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10017_iris_ctrlalloc b/ROMFS/px4fmu_common/init.d-posix/airframes/10017_iris_ctrlalloc index 1ae6a3303c..5222b5f337 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/10017_iris_ctrlalloc +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10017_iris_ctrlalloc @@ -46,4 +46,5 @@ param set-default CA_MC_R3_PY 0.1875 param set-default CA_MC_R3_CT 6.5 param set-default CA_MC_R3_KM -0.05 -set MIXER direct +set MIXER skip +set MIXER_AUX none diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/6012_typhoon_h480_ctrlalloc b/ROMFS/px4fmu_common/init.d-posix/airframes/6012_typhoon_h480_ctrlalloc index 545f59a69d..670d2b4fcd 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/6012_typhoon_h480_ctrlalloc +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/6012_typhoon_h480_ctrlalloc @@ -78,5 +78,5 @@ param set-default CA_MC_R5_KM -0.05 set MAV_TYPE 13 -# set MIXER hexa_x -set MIXER direct +set MIXER skip +set MIXER_AUX none diff --git a/ROMFS/px4fmu_common/init.d/airframes/4018_s500_ctrlalloc b/ROMFS/px4fmu_common/init.d/airframes/4018_s500_ctrlalloc index e834c387c8..4343065289 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4018_s500_ctrlalloc +++ b/ROMFS/px4fmu_common/init.d/airframes/4018_s500_ctrlalloc @@ -12,7 +12,8 @@ . ${R}etc/init.d/rc.mc_defaults -set MIXER none +set MIXER skip +set MIXER_AUX none param set-default SYS_CTRL_ALLOC 1 @@ -51,6 +52,4 @@ param set-default CA_MC_R3_PY 0.177 param set-default CA_MC_R3_CT 6.5 param set-default CA_MC_R3_KM -0.05 -set MIXER direct -set MIXER_AUX direct_aux diff --git a/ROMFS/px4fmu_common/init.d/airframes/6003_hexa_x_ctrlalloc b/ROMFS/px4fmu_common/init.d/airframes/6003_hexa_x_ctrlalloc index 6f28c3e2d8..703fa7d7a5 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/6003_hexa_x_ctrlalloc +++ b/ROMFS/px4fmu_common/init.d/airframes/6003_hexa_x_ctrlalloc @@ -66,8 +66,5 @@ param set-default CA_MC_R5_PY -0.1375 param set-default CA_MC_R5_CT 6.5 param set-default CA_MC_R5_KM -0.05 -set MIXER direct -set PWM_OUT 123456 - -set MIXER_AUX direct_aux -set PWM_AUX_OUT 123456 +set MIXER skip +set MIXER_AUX none diff --git a/ROMFS/px4fmu_common/mixers-sitl/CMakeLists.txt b/ROMFS/px4fmu_common/mixers-sitl/CMakeLists.txt index eaab17db6d..ea7eefd805 100644 --- a/ROMFS/px4fmu_common/mixers-sitl/CMakeLists.txt +++ b/ROMFS/px4fmu_common/mixers-sitl/CMakeLists.txt @@ -44,5 +44,4 @@ px4_add_romfs_files( tiltrotor_sitl.main.mix uuv_x_sitl.main.mix vectored6dof_sitl.main.mix - tiltrotor_sitl_direct.main.mix ) diff --git a/ROMFS/px4fmu_common/mixers-sitl/tiltrotor_sitl_direct.main.mix b/ROMFS/px4fmu_common/mixers-sitl/tiltrotor_sitl_direct.main.mix deleted file mode 100644 index ea3adaa0b7..0000000000 --- a/ROMFS/px4fmu_common/mixers-sitl/tiltrotor_sitl_direct.main.mix +++ /dev/null @@ -1,14 +0,0 @@ -Mixer for quad tiltrotor (x motor configuration) -================================================ - -A: 0 -A: 1 -A: 2 -A: 3 -A: 4 -A: 5 -A: 6 -A: 7 -A: 8 -A: 9 -A: 10 diff --git a/ROMFS/px4fmu_common/mixers/CMakeLists.txt b/ROMFS/px4fmu_common/mixers/CMakeLists.txt index 1be2ff75ab..54c6ca32ee 100644 --- a/ROMFS/px4fmu_common/mixers/CMakeLists.txt +++ b/ROMFS/px4fmu_common/mixers/CMakeLists.txt @@ -46,7 +46,6 @@ px4_add_romfs_files( coax.main.mix delta.main.mix deltaquad.main.mix - direct.main.mix dodeca_bottom_cox.aux.mix dodeca_top_cox.main.mix firefly6.aux.mix diff --git a/ROMFS/px4fmu_common/mixers/direct.main.mix b/ROMFS/px4fmu_common/mixers/direct.main.mix deleted file mode 100644 index 527f5eb7a1..0000000000 --- a/ROMFS/px4fmu_common/mixers/direct.main.mix +++ /dev/null @@ -1,11 +0,0 @@ -# Direct mixer -# @board px4_fmu-v2 exclude - -A: 0 -A: 1 -A: 2 -A: 3 -A: 4 -A: 5 -A: 6 -A: 7 diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 168c25176a..3bee66456a 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -40,7 +40,9 @@ set(msg_files actuator_armed.msg actuator_controls.msg actuator_controls_status.msg + actuator_motors.msg actuator_outputs.msg + actuator_servos.msg adc_report.msg airspeed.msg airspeed_validated.msg diff --git a/msg/actuator_controls.msg b/msg/actuator_controls.msg index c4b52ea48d..25ff45f432 100644 --- a/msg/actuator_controls.msg +++ b/msg/actuator_controls.msg @@ -1,6 +1,6 @@ uint64 timestamp # time since system start (microseconds) uint8 NUM_ACTUATOR_CONTROLS = 8 -uint8 NUM_ACTUATOR_CONTROL_GROUPS = 6 +uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4 uint8 INDEX_ROLL = 0 uint8 INDEX_PITCH = 1 uint8 INDEX_YAW = 2 @@ -16,13 +16,10 @@ uint8 GROUP_INDEX_ATTITUDE = 0 uint8 GROUP_INDEX_ATTITUDE_ALTERNATE = 1 uint8 GROUP_INDEX_GIMBAL = 2 uint8 GROUP_INDEX_MANUAL_PASSTHROUGH = 3 -uint8 GROUP_INDEX_ALLOCATED_PART1 = 4 -uint8 GROUP_INDEX_ALLOCATED_PART2 = 5 uint8 GROUP_INDEX_PAYLOAD = 6 uint64 timestamp_sample # the timestamp the data this control response is based on was sampled float32[8] control # TOPICS actuator_controls actuator_controls_0 actuator_controls_1 actuator_controls_2 actuator_controls_3 -# TOPICS actuator_controls_4 actuator_controls_5 # TOPICS actuator_controls_virtual_fw actuator_controls_virtual_mc diff --git a/msg/actuator_motors.msg b/msg/actuator_motors.msg new file mode 100644 index 0000000000..b4a6ed85cf --- /dev/null +++ b/msg/actuator_motors.msg @@ -0,0 +1,9 @@ +# Motor control message +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp the data this control response is based on was sampled + +uint8 NUM_CONTROLS = 8 +float32[8] control # range: [-1, 1], where 1 means maximum positive thrust, + # -1 maximum negative (if not supported by the output, <0 maps to NaN), + # and NaN maps to disarmed (stop the motors) + diff --git a/msg/actuator_servos.msg b/msg/actuator_servos.msg new file mode 100644 index 0000000000..6600d0d2e3 --- /dev/null +++ b/msg/actuator_servos.msg @@ -0,0 +1,9 @@ +# Servo control message +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp the data this control response is based on was sampled + +uint8 NUM_CONTROLS = 8 +float32[8] control # range: [-1, 1], where 1 means maximum positive position, + # -1 maximum negative, + # and NaN maps to disarmed + diff --git a/src/lib/mixer/AllocatedActuatorMixer/AllocatedActuatorMixer.cpp b/src/lib/mixer/AllocatedActuatorMixer/AllocatedActuatorMixer.cpp deleted file mode 100644 index 9e03335c12..0000000000 --- a/src/lib/mixer/AllocatedActuatorMixer/AllocatedActuatorMixer.cpp +++ /dev/null @@ -1,149 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2020 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_AllocatedActuatorMixer.cpp - * - * Mixer for allocated actuators. - * - * @author Julien Lecoeur - */ - -#include "AllocatedActuatorMixer.hpp" - -#include -#include -#include - -// #define debug(fmt, args...) do { } while(0) -#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0) - -AllocatedActuatorMixer::AllocatedActuatorMixer(ControlCallback control_cb, - uintptr_t cb_handle, - uint8_t index) : - Mixer(control_cb, cb_handle) -{ - if (index < 8) { - _control_group = 4; - _control_index = index; - - } else if (index < 16) { - _control_group = 5; - _control_index = index - 8; - - } else { - debug("'A:' invalid index"); - } -} - -unsigned AllocatedActuatorMixer::set_trim(float trim) -{ - return 1; -} - -unsigned AllocatedActuatorMixer::get_trim(float *trim) -{ - *trim = 0.0f; - return 1; -} - -int -AllocatedActuatorMixer::parse(const char *buf, unsigned &buflen, uint8_t &index) -{ - int ret; - int i; - - // enforce that the mixer ends with a new line - if (!string_well_formed(buf, buflen)) { - return -1; - } - - // parse line - if ((ret = sscanf(buf, "A: %d", &i)) != 1) { - debug("'A:' parser: failed on '%s'", buf); - return -1; - } - - buf = skipline(buf, buflen); - - if (buf == nullptr) { - debug("'A:' parser: no line ending, line is incomplete"); - return -1; - } - - // check parsed index - if (i < 16) { - index = i; - - } else { - debug("'A:' parser: invalid index"); - return -1; - } - - return 0; -} - -AllocatedActuatorMixer * -AllocatedActuatorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, - unsigned &buflen) -{ - uint8_t index; - - if (parse(buf, buflen, index) == 0) { - return new AllocatedActuatorMixer(control_cb, cb_handle, index); - - } else { - return nullptr; - } -} - -unsigned -AllocatedActuatorMixer::mix(float *outputs, unsigned space) -{ - if (space < 1) { - return 0; - } - - _control_cb(_cb_handle, - _control_group, - _control_index, - *outputs); - - return 1; -} - -void -AllocatedActuatorMixer::groups_required(uint32_t &groups) -{ - groups |= 1 << _control_group; -} diff --git a/src/lib/mixer/AllocatedActuatorMixer/AllocatedActuatorMixer.hpp b/src/lib/mixer/AllocatedActuatorMixer/AllocatedActuatorMixer.hpp deleted file mode 100644 index b36cecfe0d..0000000000 --- a/src/lib/mixer/AllocatedActuatorMixer/AllocatedActuatorMixer.hpp +++ /dev/null @@ -1,103 +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_AllocatedActuatorMixer.hpp - * - * Mixer for allocated actuators. - * - * @author Julien Lecoeur - */ - -#pragma once - -#include - -/** - * Mixer for allocated actuators. - * - * Copies a single actuator to a single output. - */ -class AllocatedActuatorMixer : public Mixer -{ -public: - /** - * Constructor - * - * @param index Actuator index (0..15) - */ - AllocatedActuatorMixer(ControlCallback control_cb, - uintptr_t cb_handle, - uint8_t index); - virtual ~AllocatedActuatorMixer() = default; - - // no copy, assignment, move, move assignment - AllocatedActuatorMixer(const AllocatedActuatorMixer &) = delete; - AllocatedActuatorMixer &operator=(const AllocatedActuatorMixer &) = delete; - AllocatedActuatorMixer(AllocatedActuatorMixer &&) = delete; - AllocatedActuatorMixer &operator=(AllocatedActuatorMixer &&) = 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 AllocatedActuatorMixer instance, or nullptr - * if the text format is bad. - */ - static AllocatedActuatorMixer *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; - unsigned set_trim(float trim) override; - unsigned get_trim(float *trim) override; - -private: - uint8_t _control_group; /**< group from which the input reads */ - uint8_t _control_index; /**< index within the control group */ - - static int parse(const char *buf, - unsigned &buflen, - uint8_t &index); -}; diff --git a/src/lib/mixer/AllocatedActuatorMixer/CMakeLists.txt b/src/lib/mixer/AllocatedActuatorMixer/CMakeLists.txt deleted file mode 100644 index 0a8526545c..0000000000 --- a/src/lib/mixer/AllocatedActuatorMixer/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(AllocatedActuatorMixer - AllocatedActuatorMixer.cpp - AllocatedActuatorMixer.hpp -) -target_link_libraries(AllocatedActuatorMixer PRIVATE MixerBase) -add_dependencies(AllocatedActuatorMixer prebuild_targets) diff --git a/src/lib/mixer/CMakeLists.txt b/src/lib/mixer/CMakeLists.txt index 088e6fa366..9b4acf4f8c 100644 --- a/src/lib/mixer/CMakeLists.txt +++ b/src/lib/mixer/CMakeLists.txt @@ -34,7 +34,6 @@ # required by other mixers add_subdirectory(MixerBase) -add_subdirectory(AllocatedActuatorMixer) add_subdirectory(HelicopterMixer) add_subdirectory(MultirotorMixer) add_subdirectory(NullMixer) @@ -49,7 +48,6 @@ add_library(mixer target_link_libraries(mixer PRIVATE - AllocatedActuatorMixer HelicopterMixer MultirotorMixer NullMixer diff --git a/src/lib/mixer/MixerGroup.cpp b/src/lib/mixer/MixerGroup.cpp index 3321b42253..b48f5cddee 100644 --- a/src/lib/mixer/MixerGroup.cpp +++ b/src/lib/mixer/MixerGroup.cpp @@ -39,7 +39,6 @@ #include "MixerGroup.hpp" -#include "AllocatedActuatorMixer/AllocatedActuatorMixer.hpp" #include "HelicopterMixer/HelicopterMixer.hpp" #include "MultirotorMixer/MultirotorMixer.hpp" #include "NullMixer/NullMixer.hpp" @@ -193,10 +192,6 @@ MixerGroup::load_from_buf(Mixer::ControlCallback control_cb, uintptr_t cb_handle m = NullMixer::from_text(p, resid); break; - case 'A': - m = AllocatedActuatorMixer::from_text(control_cb, cb_handle, p, resid); - break; - case 'M': m = SimpleMixer::from_text(control_cb, cb_handle, p, resid); break; diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index 9558022c31..31639d80bc 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -50,8 +50,6 @@ MixingOutput::MixingOutput(uint8_t max_num_outputs, OutputModuleInterface &inter {&interface, ORB_ID(actuator_controls_1)}, {&interface, ORB_ID(actuator_controls_2)}, {&interface, ORB_ID(actuator_controls_3)}, - {&interface, ORB_ID(actuator_controls_4)}, - {&interface, ORB_ID(actuator_controls_5)}, }, _scheduling_policy(scheduling_policy), _support_esc_calibration(support_esc_calibration), @@ -542,11 +540,9 @@ int MixingOutput::controlCallback(uintptr_t handle, uint8_t control_group, uint8 /* motor spinup phase - lock throttle to zero */ if (output->_output_limit.state == OUTPUT_LIMIT_STATE_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) || - (control_group == actuator_controls_s::GROUP_INDEX_ALLOCATED_PART1 || - control_group == actuator_controls_s::GROUP_INDEX_ALLOCATED_PART2)) { + 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 */ @@ -556,11 +552,9 @@ int MixingOutput::controlCallback(uintptr_t handle, uint8_t control_group, uint8 /* 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) || - (control_group == actuator_controls_s::GROUP_INDEX_ALLOCATED_PART1 || - control_group == actuator_controls_s::GROUP_INDEX_ALLOCATED_PART2)) { + 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; } diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp index 9fac9f55c8..034e455db2 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp @@ -97,10 +97,10 @@ const for (size_t i = 0; i < ControlAllocation::NUM_ACTUATORS; i++) { if (_actuator_min(i) < _actuator_max(i)) { - actuator_normalized(i) = -1.0f + 2.0f * (actuator(i) - _actuator_min(i)) / (_actuator_max(i) - _actuator_min(i)); + actuator_normalized(i) = (actuator(i) - _actuator_min(i)) / (_actuator_max(i) - _actuator_min(i)); } else { - actuator_normalized(i) = -1.0f + 2.0f * (_actuator_trim(i) - _actuator_min(i)) / (_actuator_max(i) - _actuator_min(i)); + actuator_normalized(i) = (_actuator_trim(i) - _actuator_min(i)) / (_actuator_max(i) - _actuator_min(i)); } } diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index dc7991ebb9..156147595f 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -436,26 +436,21 @@ ControlAllocator::publish_control_allocator_status() void ControlAllocator::publish_legacy_actuator_controls() { - // For compatibility with the current mixer system, - // publish normalized version on actuator_controls_4/5 - actuator_controls_s actuator_controls_4{}; - actuator_controls_s actuator_controls_5{}; - actuator_controls_4.timestamp = hrt_absolute_time(); - actuator_controls_5.timestamp = hrt_absolute_time(); - actuator_controls_4.timestamp_sample = _timestamp_sample; - actuator_controls_5.timestamp_sample = _timestamp_sample; + actuator_motors_s actuator_motors; + actuator_motors.timestamp = hrt_absolute_time(); + actuator_motors.timestamp_sample = _timestamp_sample; matrix::Vector actuator_sp = _control_allocation->getActuatorSetpoint(); matrix::Vector actuator_sp_normalized = _control_allocation->normalizeActuatorSetpoint( actuator_sp); - for (size_t i = 0; i < 8; i++) { - actuator_controls_4.control[i] = (PX4_ISFINITE(actuator_sp_normalized(i))) ? actuator_sp_normalized(i) : 0.0f; - actuator_controls_5.control[i] = (PX4_ISFINITE(actuator_sp_normalized(i + 8))) ? actuator_sp_normalized(i + 8) : 0.0f; + for (size_t i = 0; i < actuator_motors_s::NUM_CONTROLS; i++) { + actuator_motors.control[i] = PX4_ISFINITE(actuator_sp_normalized(i)) ? actuator_sp_normalized(i) : NAN; } - _actuator_controls_4_pub.publish(actuator_controls_4); - _actuator_controls_5_pub.publish(actuator_controls_5); + _actuator_motors_pub.publish(actuator_motors); + + // TODO: servos } int ControlAllocator::task_spawn(int argc, char *argv[]) diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index 7f9b61db76..b72e5bb17a 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -59,7 +59,8 @@ #include #include #include -#include +#include +#include #include #include #include @@ -140,9 +141,8 @@ private: uORB::Publication _vehicle_actuator_setpoint_pub{ORB_ID(vehicle_actuator_setpoint)}; /**< actuator setpoint publication */ uORB::Publication _control_allocator_status_pub{ORB_ID(control_allocator_status)}; /**< actuator setpoint publication */ - // actuator_controls publication handles (temporary hack to plug actuator_setpoint into the mixer system) - uORB::Publication _actuator_controls_4_pub{ORB_ID(actuator_controls_4)}; /**< actuator controls 4 publication */ - uORB::Publication _actuator_controls_5_pub{ORB_ID(actuator_controls_5)}; /**< actuator controls 5 publication */ + uORB::Publication _actuator_motors_pub{ORB_ID(actuator_motors)}; + uORB::Publication _actuator_servos_pub{ORB_ID(actuator_servos)}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};