diff --git a/src/lib/actuator_effectiveness/ActuatorEffectiveness.hpp b/src/lib/actuator_effectiveness/ActuatorEffectiveness.hpp index 0ddd4988f3..12ce9c7f4f 100644 --- a/src/lib/actuator_effectiveness/ActuatorEffectiveness.hpp +++ b/src/lib/actuator_effectiveness/ActuatorEffectiveness.hpp @@ -51,12 +51,13 @@ enum class AllocationMethod { PSEUDO_INVERSE = 0, SEQUENTIAL_DESATURATION = 1, AUTO = 2, + METRIC = 3, }; enum class ActuatorType { MOTORS = 0, SERVOS, - + THRUSTERS, COUNT }; diff --git a/src/lib/actuator_effectiveness/ActuatorEffectivenessRotors.cpp b/src/lib/actuator_effectiveness/ActuatorEffectivenessRotors.cpp new file mode 100644 index 0000000000..e8c2841084 --- /dev/null +++ b/src/lib/actuator_effectiveness/ActuatorEffectivenessRotors.cpp @@ -0,0 +1,312 @@ +/**************************************************************************** + * + * 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 ActuatorEffectivenessRotors.hpp + * + * Actuator effectiveness computed from rotors position and orientation + * + * @author Julien Lecoeur + */ + +#include "ActuatorEffectivenessRotors.hpp" + +// #include "ActuatorEffectivenessTilts.hpp" + +using namespace matrix; + +ActuatorEffectivenessRotors::ActuatorEffectivenessRotors(ModuleParams *parent, AxisConfiguration axis_config, + bool tilt_support) + : ModuleParams(parent), _axis_config(axis_config), _tilt_support(tilt_support) +{ + for (int i = 0; i < NUM_ROTORS_MAX; ++i) { + char buffer[17]; + snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_PX", i); + _param_handles[i].position_x = param_find(buffer); + snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_PY", i); + _param_handles[i].position_y = param_find(buffer); + snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_PZ", i); + _param_handles[i].position_z = param_find(buffer); + + if (_axis_config == AxisConfiguration::Configurable) { + snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_AX", i); + _param_handles[i].axis_x = param_find(buffer); + snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_AY", i); + _param_handles[i].axis_y = param_find(buffer); + snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_AZ", i); + _param_handles[i].axis_z = param_find(buffer); + } + + snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_CT", i); + _param_handles[i].thrust_coef = param_find(buffer); + + snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_KM", i); + _param_handles[i].moment_ratio = param_find(buffer); + + if (_tilt_support) { + snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_TILT", i); + _param_handles[i].tilt_index = param_find(buffer); + } + } + + updateParams(); +} + +void ActuatorEffectivenessRotors::updateParams() +{ + ModuleParams::updateParams(); + + _geometry.num_rotors = math::min(NUM_ROTORS_MAX, static_cast(_param_ca_rotor_count.get())); + + for (int i = 0; i < _geometry.num_rotors; ++i) { + Vector3f &position = _geometry.rotors[i].position; + param_get(_param_handles[i].position_x, &position(0)); + param_get(_param_handles[i].position_y, &position(1)); + param_get(_param_handles[i].position_z, &position(2)); + + Vector3f &axis = _geometry.rotors[i].axis; + + switch (_axis_config) { + case AxisConfiguration::Configurable: + param_get(_param_handles[i].axis_x, &axis(0)); + param_get(_param_handles[i].axis_y, &axis(1)); + param_get(_param_handles[i].axis_z, &axis(2)); + break; + + case AxisConfiguration::FixedForward: + axis = Vector3f(1.f, 0.f, 0.f); + break; + + case AxisConfiguration::FixedUpwards: + axis = Vector3f(0.f, 0.f, -1.f); + break; + } + + param_get(_param_handles[i].thrust_coef, &_geometry.rotors[i].thrust_coef); + param_get(_param_handles[i].moment_ratio, &_geometry.rotors[i].moment_ratio); + + if (_tilt_support) { + int32_t tilt_param{0}; + param_get(_param_handles[i].tilt_index, &tilt_param); + _geometry.rotors[i].tilt_index = tilt_param - 1; + + } else { + _geometry.rotors[i].tilt_index = -1; + } + } +} + +bool +ActuatorEffectivenessRotors::addActuators(Configuration &configuration) +{ + if (configuration.num_actuators[(int)ActuatorType::SERVOS] > 0) { + PX4_ERR("Wrong actuator ordering: servos need to be after motors"); + return false; + } + + int num_actuators = computeEffectivenessMatrix(_geometry, + configuration.effectiveness_matrices[configuration.selected_matrix], + configuration.num_actuators_matrix[configuration.selected_matrix]); + configuration.actuatorsAdded(ActuatorType::MOTORS, num_actuators); + return true; +} + +int +ActuatorEffectivenessRotors::computeEffectivenessMatrix(const Geometry &geometry, + EffectivenessMatrix &effectiveness, int actuator_start_index) +{ + int num_actuators = 0; + + for (int i = 0; i < geometry.num_rotors; i++) { + + if (i + actuator_start_index >= NUM_ACTUATORS) { + break; + } + + ++num_actuators; + + // Get rotor axis + Vector3f axis = geometry.rotors[i].axis; + + // Normalize axis + float axis_norm = axis.norm(); + + if (axis_norm > FLT_EPSILON) { + axis /= axis_norm; + + } else { + // Bad axis definition, ignore this rotor + continue; + } + + // Get rotor position + const Vector3f &position = geometry.rotors[i].position; + + // Get coefficients + float ct = geometry.rotors[i].thrust_coef; + float km = geometry.rotors[i].moment_ratio; + + if (geometry.propeller_torque_disabled) { + km = 0.f; + } + + if (geometry.propeller_torque_disabled_non_upwards) { + bool upwards = fabsf(axis(0)) < 0.1f && fabsf(axis(1)) < 0.1f && axis(2) < -0.5f; + + if (!upwards) { + km = 0.f; + } + } + + if (fabsf(ct) < FLT_EPSILON) { + continue; + } + + // Compute thrust generated by this rotor + matrix::Vector3f thrust = ct * axis; + + // Compute moment generated by this rotor + matrix::Vector3f moment = ct * position.cross(axis) - ct * km * axis; + + // Fill corresponding items in effectiveness matrix + for (size_t j = 0; j < 3; j++) { + effectiveness(j, i + actuator_start_index) = moment(j); + effectiveness(j + 3, i + actuator_start_index) = thrust(j); + } + + if (geometry.yaw_by_differential_thrust_disabled) { + // set yaw effectiveness to 0 if yaw is controlled by other means (e.g. tilts) + effectiveness(2, i + actuator_start_index) = 0.f; + } + + if (geometry.three_dimensional_thrust_disabled) { + // Special case tiltrotor: instead of passing a 3D thrust vector (that would mostly have a x-component in FW, and z in MC), + // pass the vector magnitude as z-component, plus the collective tilt. Passing 3D thrust plus tilt is not feasible as they + // can't be allocated independently, and with the current controller it's not possible to have collective tilt calculated + // by the allocator directly. + + effectiveness(0 + 3, i + actuator_start_index) = 0.f; + effectiveness(1 + 3, i + actuator_start_index) = 0.f; + effectiveness(2 + 3, i + actuator_start_index) = -ct; + } + } + + return num_actuators; +} + +// TODO(@Jaeyoung-Lim): This should not be in the Rotors effectiveness, so we need to move it elsewhere +/* +uint32_t ActuatorEffectivenessRotors::updateAxisFromTilts(const ActuatorEffectivenessTilts &tilts, + float collective_tilt_control) +{ + if (!PX4_ISFINITE(collective_tilt_control)) { + collective_tilt_control = -1.f; + } + + uint32_t nontilted_motors = 0; + + for (int i = 0; i < _geometry.num_rotors; ++i) { + int tilt_index = _geometry.rotors[i].tilt_index; + + if (tilt_index == -1 || tilt_index >= tilts.count()) { + nontilted_motors |= 1u << i; + continue; + } + + const ActuatorEffectivenessTilts::Params &tilt = tilts.config(tilt_index); + const float tilt_angle = math::lerp(tilt.min_angle, tilt.max_angle, (collective_tilt_control + 1.f) / 2.f); + const float tilt_direction = math::radians((float)tilt.tilt_direction); + _geometry.rotors[i].axis = tiltedAxis(tilt_angle, tilt_direction); + } + + return nontilted_motors; +} +*/ + +Vector3f ActuatorEffectivenessRotors::tiltedAxis(float tilt_angle, float tilt_direction) +{ + Vector3f axis{0.f, 0.f, -1.f}; + return Dcmf{Eulerf{0.f, -tilt_angle, tilt_direction}} * axis; +} + +uint32_t ActuatorEffectivenessRotors::getMotors() const +{ + uint32_t motors = 0; + + for (int i = 0; i < _geometry.num_rotors; ++i) { + motors |= 1u << i; + } + + return motors; +} + +uint32_t ActuatorEffectivenessRotors::getUpwardsMotors() const +{ + uint32_t upwards_motors = 0; + + for (int i = 0; i < _geometry.num_rotors; ++i) { + const Vector3f &axis = _geometry.rotors[i].axis; + + if (fabsf(axis(0)) < 0.1f && fabsf(axis(1)) < 0.1f && axis(2) < -0.5f) { + upwards_motors |= 1u << i; + } + } + + return upwards_motors; +} + +uint32_t ActuatorEffectivenessRotors::getForwardsMotors() const +{ + uint32_t forward_motors = 0; + + for (int i = 0; i < _geometry.num_rotors; ++i) { + const Vector3f &axis = _geometry.rotors[i].axis; + + if (axis(0) > 0.5f && fabsf(axis(1)) < 0.1f && fabsf(axis(2)) < 0.1f) { + forward_motors |= 1u << i; + } + } + + return forward_motors; +} + +bool +ActuatorEffectivenessRotors::getEffectivenessMatrix(Configuration &configuration, + EffectivenessUpdateReason external_update) +{ + if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) { + return false; + } + + return addActuators(configuration); +} diff --git a/src/lib/actuator_effectiveness/ActuatorEffectivenessRotors.hpp b/src/lib/actuator_effectiveness/ActuatorEffectivenessRotors.hpp new file mode 100644 index 0000000000..3844df4c84 --- /dev/null +++ b/src/lib/actuator_effectiveness/ActuatorEffectivenessRotors.hpp @@ -0,0 +1,156 @@ +/**************************************************************************** + * + * 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 ActuatorEffectivenessRotors.hpp + * + * Actuator effectiveness computed from rotors position and orientation + * + * @author Julien Lecoeur + */ + +#pragma once + +#include "ActuatorEffectiveness.hpp" + +#include +#include +#include + +class ActuatorEffectivenessTilts; + +using namespace time_literals; + +class ActuatorEffectivenessRotors : public ModuleParams, public ActuatorEffectiveness +{ +public: + enum class AxisConfiguration { + Configurable, ///< axis can be configured + FixedForward, ///< axis is fixed, pointing forwards (positive X) + FixedUpwards, ///< axis is fixed, pointing upwards (negative Z) + }; + + static constexpr int NUM_ROTORS_MAX = 12; + + struct RotorGeometry { + matrix::Vector3f position; + matrix::Vector3f axis; + float thrust_coef; + float moment_ratio; + int tilt_index; + }; + + struct Geometry { + RotorGeometry rotors[NUM_ROTORS_MAX]; + int num_rotors{0}; + bool propeller_torque_disabled{false}; + bool yaw_by_differential_thrust_disabled{false}; + bool propeller_torque_disabled_non_upwards{false}; ///< keeps propeller torque enabled for upward facing motors + bool three_dimensional_thrust_disabled{false}; ///< for handling of tiltrotor VTOL, as they pass in 1D thrust and collective tilt + }; + + ActuatorEffectivenessRotors(ModuleParams *parent, AxisConfiguration axis_config = AxisConfiguration::Configurable, + bool tilt_support = false); + virtual ~ActuatorEffectivenessRotors() = default; + + bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override; + + void getDesiredAllocationMethod(AllocationMethod allocation_method_out[MAX_NUM_MATRICES]) const override + { + allocation_method_out[0] = AllocationMethod::SEQUENTIAL_DESATURATION; + } + + void getNormalizeRPY(bool normalize[MAX_NUM_MATRICES]) const override + { + normalize[0] = true; + } + + static int computeEffectivenessMatrix(const Geometry &geometry, + EffectivenessMatrix &effectiveness, int actuator_start_index = 0); + + bool addActuators(Configuration &configuration); + + const char *name() const override { return "Rotors"; } + + /** + * Sets the motor axis from tilt configurations and current tilt control. + * @param tilts configured tilt servos + * @param tilt_control current tilt control in [-1, 1] (can be NAN) + * @return the motors as bitset which are not tiltable + */ + uint32_t updateAxisFromTilts(const ActuatorEffectivenessTilts &tilts, float tilt_control); + + const Geometry &geometry() const { return _geometry; } + + /** + * Get the tilted axis {0, 0, -1} rotated by -tilt_angle around y, then + * rotated by tilt_direction around z. + */ + static matrix::Vector3f tiltedAxis(float tilt_angle, float tilt_direction); + + void enablePropellerTorque(bool enable) { _geometry.propeller_torque_disabled = !enable; } + + void enableYawByDifferentialThrust(bool enable) { _geometry.yaw_by_differential_thrust_disabled = !enable; } + + void enablePropellerTorqueNonUpwards(bool enable) { _geometry.propeller_torque_disabled_non_upwards = !enable; } + + void enableThreeDimensionalThrust(bool enable) { _geometry.three_dimensional_thrust_disabled = !enable; } + + uint32_t getMotors() const; + uint32_t getUpwardsMotors() const; + uint32_t getForwardsMotors() const; + +private: + void updateParams() override; + const AxisConfiguration _axis_config; + const bool _tilt_support; ///< if true, tilt servo assignment params are loaded + + struct ParamHandles { + param_t position_x; + param_t position_y; + param_t position_z; + param_t axis_x; + param_t axis_y; + param_t axis_z; + param_t thrust_coef; + param_t moment_ratio; + param_t tilt_index; + }; + ParamHandles _param_handles[NUM_ROTORS_MAX]; + + Geometry _geometry{}; + + DEFINE_PARAMETERS( + (ParamInt) _param_ca_rotor_count + ) +}; diff --git a/src/lib/actuator_effectiveness/ActuatorEffectivenessRotorsTest.cpp b/src/lib/actuator_effectiveness/ActuatorEffectivenessRotorsTest.cpp new file mode 100644 index 0000000000..603248ab9d --- /dev/null +++ b/src/lib/actuator_effectiveness/ActuatorEffectivenessRotorsTest.cpp @@ -0,0 +1,135 @@ +/**************************************************************************** + * + * 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 ActuatorEffectivenessRotors.cpp + * + * Tests for Control Allocation Algorithms + * + * @author Julien Lecoeur + */ + +#include +#include "ActuatorEffectivenessRotors.hpp" + +using namespace matrix; + +TEST(ActuatorEffectivenessRotors, AllZeroCase) +{ + // Quad wide geometry + ActuatorEffectivenessRotors::Geometry geometry = {}; + geometry.rotors[0].position(0) = 1.0f; + geometry.rotors[0].position(1) = 1.0f; + geometry.rotors[0].position(2) = 0.0f; + geometry.rotors[0].axis(0) = 0.0f; + geometry.rotors[0].axis(1) = 0.0f; + geometry.rotors[0].axis(2) = -1.0f; + geometry.rotors[0].thrust_coef = 1.0f; + geometry.rotors[0].moment_ratio = 0.05f; + + geometry.rotors[1].position(0) = -1.0f; + geometry.rotors[1].position(1) = -1.0f; + geometry.rotors[1].position(2) = 0.0f; + geometry.rotors[1].axis(0) = 0.0f; + geometry.rotors[1].axis(1) = 0.0f; + geometry.rotors[1].axis(2) = -1.0f; + geometry.rotors[1].thrust_coef = 1.0f; + geometry.rotors[1].moment_ratio = 0.05f; + + geometry.rotors[2].position(0) = 1.0f; + geometry.rotors[2].position(1) = -1.0f; + geometry.rotors[2].position(2) = 0.0f; + geometry.rotors[2].axis(0) = 0.0f; + geometry.rotors[2].axis(1) = 0.0f; + geometry.rotors[2].axis(2) = -1.0f; + geometry.rotors[2].thrust_coef = 1.0f; + geometry.rotors[2].moment_ratio = -0.05f; + + geometry.rotors[3].position(0) = -1.0f; + geometry.rotors[3].position(1) = 1.0f; + geometry.rotors[3].position(2) = 0.0f; + geometry.rotors[3].axis(0) = 0.0f; + geometry.rotors[3].axis(1) = 0.0f; + geometry.rotors[3].axis(2) = -1.0f; + geometry.rotors[3].thrust_coef = 1.0f; + geometry.rotors[3].moment_ratio = -0.05f; + + geometry.num_rotors = 4; + + ActuatorEffectiveness::EffectivenessMatrix effectiveness; + effectiveness.setZero(); + ActuatorEffectivenessRotors::computeEffectivenessMatrix(geometry, effectiveness); + + const float expected[ActuatorEffectiveness::NUM_AXES][ActuatorEffectiveness::NUM_ACTUATORS] = { + {-1.0f, 1.0f, 1.0f, -1.0f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 1.0f, -1.0f, 1.0f, -1.0f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.05f, 0.05f, -0.05f, -0.05f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + {-1.0f, -1.0f, -1.0f, -1.0f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f} + }; + ActuatorEffectiveness::EffectivenessMatrix effectiveness_expected(expected); + + EXPECT_EQ(effectiveness, effectiveness_expected); +} + +TEST(ActuatorEffectivenessRotors, Tilt) +{ + Vector3f axis_expected{0.f, 0.f, -1.f}; + Vector3f axis = ActuatorEffectivenessRotors::tiltedAxis(0.f, 0.f); + EXPECT_EQ(axis, axis_expected); + + axis_expected = Vector3f{1.f, 0.f, 0.f}; + axis = ActuatorEffectivenessRotors::tiltedAxis(M_PI_F / 2.f, 0.f); + EXPECT_EQ(axis, axis_expected); + + axis_expected = Vector3f{1.f / sqrtf(2.f), 0.f, -1.f / sqrtf(2.f)}; + axis = ActuatorEffectivenessRotors::tiltedAxis(M_PI_F / 2.f / 2.f, 0.f); + EXPECT_EQ(axis, axis_expected); + + axis_expected = Vector3f{-1.f, 0.f, 0.f}; + axis = ActuatorEffectivenessRotors::tiltedAxis(-M_PI_F / 2.f, 0.f); + EXPECT_EQ(axis, axis_expected); + + axis_expected = Vector3f{0.f, 0.f, -1.f}; + axis = ActuatorEffectivenessRotors::tiltedAxis(0.f, M_PI_F / 2.f); + EXPECT_EQ(axis, axis_expected); + + axis_expected = Vector3f{0.f, 1.f, 0.f}; + axis = ActuatorEffectivenessRotors::tiltedAxis(M_PI_F / 2.f, M_PI_F / 2.f); + EXPECT_EQ(axis, axis_expected); + + axis_expected = Vector3f{0.f, -1.f, 0.f}; + axis = ActuatorEffectivenessRotors::tiltedAxis(-M_PI_F / 2.f, M_PI_F / 2.f); + EXPECT_EQ(axis, axis_expected); +} diff --git a/src/lib/actuator_effectiveness/ActuatorEffectivenessThrusters.cpp b/src/lib/actuator_effectiveness/ActuatorEffectivenessThrusters.cpp new file mode 100644 index 0000000000..d70dcf047f --- /dev/null +++ b/src/lib/actuator_effectiveness/ActuatorEffectivenessThrusters.cpp @@ -0,0 +1,205 @@ +/**************************************************************************** + * + * 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 ActuatorEffectivenessThrusters.hpp + * + * Actuator effectiveness computed from thrusters position and orientation + * + * @author Pedro Roque, + */ + +#include "ActuatorEffectivenessThrusters.hpp" + +using namespace matrix; + +ActuatorEffectivenessThrusters::ActuatorEffectivenessThrusters(ModuleParams *parent, AxisConfiguration axis_config, + bool tilt_support) + : ModuleParams(parent), _axis_config(axis_config), _tilt_support(tilt_support) +{ + + for (int i = 0; i < NUM_THRUSTERS_MAX; ++i) { + char buffer[17]; + snprintf(buffer, sizeof(buffer), "CA_THRUSTER%u_PX", i); + _param_handles[i].position_x = param_find(buffer); + snprintf(buffer, sizeof(buffer), "CA_THRUSTER%u_PY", i); + _param_handles[i].position_y = param_find(buffer); + snprintf(buffer, sizeof(buffer), "CA_THRUSTER%u_PZ", i); + _param_handles[i].position_z = param_find(buffer); + + if (_axis_config == AxisConfiguration::Configurable) { + snprintf(buffer, sizeof(buffer), "CA_THRUSTER%u_AX", i); + _param_handles[i].axis_x = param_find(buffer); + snprintf(buffer, sizeof(buffer), "CA_THRUSTER%u_AY", i); + _param_handles[i].axis_y = param_find(buffer); + snprintf(buffer, sizeof(buffer), "CA_THRUSTER%u_AZ", i); + _param_handles[i].axis_z = param_find(buffer); + } + + snprintf(buffer, sizeof(buffer), "CA_THRUSTER%u_CT", i); + _param_handles[i].thrust_coef = param_find(buffer); + + if (_tilt_support) { + PX4_ERR("Tilt support not implemented"); + } + } + + _count_handle = param_find("CA_THRUSTER_CNT"); + + updateParams(); +} + +void ActuatorEffectivenessThrusters::updateParams() +{ + ModuleParams::updateParams(); + + int32_t count = 0; + + if (param_get(_count_handle, &count) != 0) { + PX4_ERR("param_get failed"); + return; + } + + _geometry.num_thrusters = math::min(NUM_THRUSTERS_MAX, (int)count); + + for (int i = 0; i < _geometry.num_thrusters; ++i) { + Vector3f &position = _geometry.thrusters[i].position; + param_get(_param_handles[i].position_x, &position(0)); + param_get(_param_handles[i].position_y, &position(1)); + param_get(_param_handles[i].position_z, &position(2)); + + Vector3f &axis = _geometry.thrusters[i].axis; + + switch (_axis_config) { + case AxisConfiguration::Configurable: + param_get(_param_handles[i].axis_x, &axis(0)); + param_get(_param_handles[i].axis_y, &axis(1)); + param_get(_param_handles[i].axis_z, &axis(2)); + break; + } + + param_get(_param_handles[i].thrust_coef, &_geometry.thrusters[i].thrust_coef); + } +} + +bool +ActuatorEffectivenessThrusters::addActuators(Configuration &configuration) +{ + if (configuration.num_actuators[(int)ActuatorType::SERVOS] > 0) { + PX4_ERR("Wrong actuator ordering: servos need to be after motors"); + return false; + } + + int num_actuators = computeEffectivenessMatrix(_geometry, + configuration.effectiveness_matrices[configuration.selected_matrix], + configuration.num_actuators_matrix[configuration.selected_matrix]); + configuration.actuatorsAdded(ActuatorType::THRUSTERS, num_actuators); + return true; +} + +int +ActuatorEffectivenessThrusters::computeEffectivenessMatrix(const Geometry &geometry, + EffectivenessMatrix &effectiveness, int actuator_start_index) +{ + int num_actuators = 0; + + for (int i = 0; i < geometry.num_thrusters; i++) { + + if (i + actuator_start_index >= NUM_ACTUATORS) { + break; + } + + ++num_actuators; + + // Get rotor axis + Vector3f axis = geometry.thrusters[i].axis; + + // Normalize axis + float axis_norm = axis.norm(); + + if (axis_norm > FLT_EPSILON) { + axis /= axis_norm; + + } else { + // Bad axis definition, ignore this rotor + continue; + } + + // Get rotor position + const Vector3f &position = geometry.thrusters[i].position; + + // Get coefficients + float ct = geometry.thrusters[i].thrust_coef; + + if (fabsf(ct) < FLT_EPSILON) { + continue; + } + + // Compute thrust generated by this rotor + matrix::Vector3f thrust = ct * axis; + + // Compute moment generated by this rotor + matrix::Vector3f moment = ct * position.cross(axis); + + // Fill corresponding items in effectiveness matrix + for (size_t j = 0; j < 3; j++) { + effectiveness(j, i + actuator_start_index) = moment(j); + effectiveness(j + 3, i + actuator_start_index) = thrust(j); + } + } + + return num_actuators; +} + + +uint32_t ActuatorEffectivenessThrusters::getThrusters() const +{ + uint32_t thrusters = 0; + + for (int i = 0; i < _geometry.num_thrusters; ++i) { + thrusters |= 1u << i; + } + + return thrusters; +} + +bool +ActuatorEffectivenessThrusters::getEffectivenessMatrix(Configuration &configuration, + EffectivenessUpdateReason external_update) +{ + if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) { + return false; + } + + return addActuators(configuration); +} diff --git a/src/lib/actuator_effectiveness/ActuatorEffectivenessThrusters.hpp b/src/lib/actuator_effectiveness/ActuatorEffectivenessThrusters.hpp new file mode 100644 index 0000000000..be112059c2 --- /dev/null +++ b/src/lib/actuator_effectiveness/ActuatorEffectivenessThrusters.hpp @@ -0,0 +1,121 @@ +/**************************************************************************** + * + * 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 ActuatorEffectivenessThrusters.hpp + * + * Actuator effectiveness computed from thrusters position and orientation + * + * @author Pedro Roque, + */ + +#pragma once + +#include "ActuatorEffectiveness.hpp" + +#include +#include +#include + +class ActuatorEffectivenessTilts; + +using namespace time_literals; + +class ActuatorEffectivenessThrusters : public ModuleParams, public ActuatorEffectiveness +{ +public: + enum class AxisConfiguration { + Configurable, ///< axis can be configured + }; + + static constexpr int NUM_THRUSTERS_MAX = 12; + + struct ThrusterGeometry { + matrix::Vector3f position; + matrix::Vector3f axis; + float thrust_coef; + }; + + struct Geometry { + ThrusterGeometry thrusters[NUM_THRUSTERS_MAX]; + int num_thrusters{0}; + }; + + ActuatorEffectivenessThrusters(ModuleParams *parent, AxisConfiguration axis_config = AxisConfiguration::Configurable, + bool tilt_support = false); + virtual ~ActuatorEffectivenessThrusters() = default; + + bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override; + + void getDesiredAllocationMethod(AllocationMethod allocation_method_out[MAX_NUM_MATRICES]) const override + { + allocation_method_out[0] = + AllocationMethod::SEQUENTIAL_DESATURATION; // TODO(@Jaeyoung-Lim): needs to be updated based on metric mixer + } + + void getNormalizeRPY(bool normalize[MAX_NUM_MATRICES]) const override + { + // TODO(@Jaeyoung-Lim): needs to be updated based on metric mixer + normalize[0] = true; + } + + static int computeEffectivenessMatrix(const Geometry &geometry, + EffectivenessMatrix &effectiveness, int actuator_start_index = 0); + + bool addActuators(Configuration &configuration); + + const char *name() const override { return "Thrusters"; } + + const Geometry &geometry() const { return _geometry; } + + uint32_t getThrusters() const; + +private: + void updateParams() override; + const AxisConfiguration _axis_config; + const bool _tilt_support; ///< if true, tilt servo assignment params are loaded + + struct ParamHandles { + param_t position_x; + param_t position_y; + param_t position_z; + param_t axis_x; + param_t axis_y; + param_t axis_z; + param_t thrust_coef; + }; + ParamHandles _param_handles[NUM_THRUSTERS_MAX]; + param_t _count_handle; + + Geometry _geometry{}; +}; diff --git a/src/lib/actuator_effectiveness/ActuatorEffectivenessThrustersTest.cpp b/src/lib/actuator_effectiveness/ActuatorEffectivenessThrustersTest.cpp new file mode 100644 index 0000000000..603248ab9d --- /dev/null +++ b/src/lib/actuator_effectiveness/ActuatorEffectivenessThrustersTest.cpp @@ -0,0 +1,135 @@ +/**************************************************************************** + * + * 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 ActuatorEffectivenessRotors.cpp + * + * Tests for Control Allocation Algorithms + * + * @author Julien Lecoeur + */ + +#include +#include "ActuatorEffectivenessRotors.hpp" + +using namespace matrix; + +TEST(ActuatorEffectivenessRotors, AllZeroCase) +{ + // Quad wide geometry + ActuatorEffectivenessRotors::Geometry geometry = {}; + geometry.rotors[0].position(0) = 1.0f; + geometry.rotors[0].position(1) = 1.0f; + geometry.rotors[0].position(2) = 0.0f; + geometry.rotors[0].axis(0) = 0.0f; + geometry.rotors[0].axis(1) = 0.0f; + geometry.rotors[0].axis(2) = -1.0f; + geometry.rotors[0].thrust_coef = 1.0f; + geometry.rotors[0].moment_ratio = 0.05f; + + geometry.rotors[1].position(0) = -1.0f; + geometry.rotors[1].position(1) = -1.0f; + geometry.rotors[1].position(2) = 0.0f; + geometry.rotors[1].axis(0) = 0.0f; + geometry.rotors[1].axis(1) = 0.0f; + geometry.rotors[1].axis(2) = -1.0f; + geometry.rotors[1].thrust_coef = 1.0f; + geometry.rotors[1].moment_ratio = 0.05f; + + geometry.rotors[2].position(0) = 1.0f; + geometry.rotors[2].position(1) = -1.0f; + geometry.rotors[2].position(2) = 0.0f; + geometry.rotors[2].axis(0) = 0.0f; + geometry.rotors[2].axis(1) = 0.0f; + geometry.rotors[2].axis(2) = -1.0f; + geometry.rotors[2].thrust_coef = 1.0f; + geometry.rotors[2].moment_ratio = -0.05f; + + geometry.rotors[3].position(0) = -1.0f; + geometry.rotors[3].position(1) = 1.0f; + geometry.rotors[3].position(2) = 0.0f; + geometry.rotors[3].axis(0) = 0.0f; + geometry.rotors[3].axis(1) = 0.0f; + geometry.rotors[3].axis(2) = -1.0f; + geometry.rotors[3].thrust_coef = 1.0f; + geometry.rotors[3].moment_ratio = -0.05f; + + geometry.num_rotors = 4; + + ActuatorEffectiveness::EffectivenessMatrix effectiveness; + effectiveness.setZero(); + ActuatorEffectivenessRotors::computeEffectivenessMatrix(geometry, effectiveness); + + const float expected[ActuatorEffectiveness::NUM_AXES][ActuatorEffectiveness::NUM_ACTUATORS] = { + {-1.0f, 1.0f, 1.0f, -1.0f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 1.0f, -1.0f, 1.0f, -1.0f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.05f, 0.05f, -0.05f, -0.05f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + {-1.0f, -1.0f, -1.0f, -1.0f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f} + }; + ActuatorEffectiveness::EffectivenessMatrix effectiveness_expected(expected); + + EXPECT_EQ(effectiveness, effectiveness_expected); +} + +TEST(ActuatorEffectivenessRotors, Tilt) +{ + Vector3f axis_expected{0.f, 0.f, -1.f}; + Vector3f axis = ActuatorEffectivenessRotors::tiltedAxis(0.f, 0.f); + EXPECT_EQ(axis, axis_expected); + + axis_expected = Vector3f{1.f, 0.f, 0.f}; + axis = ActuatorEffectivenessRotors::tiltedAxis(M_PI_F / 2.f, 0.f); + EXPECT_EQ(axis, axis_expected); + + axis_expected = Vector3f{1.f / sqrtf(2.f), 0.f, -1.f / sqrtf(2.f)}; + axis = ActuatorEffectivenessRotors::tiltedAxis(M_PI_F / 2.f / 2.f, 0.f); + EXPECT_EQ(axis, axis_expected); + + axis_expected = Vector3f{-1.f, 0.f, 0.f}; + axis = ActuatorEffectivenessRotors::tiltedAxis(-M_PI_F / 2.f, 0.f); + EXPECT_EQ(axis, axis_expected); + + axis_expected = Vector3f{0.f, 0.f, -1.f}; + axis = ActuatorEffectivenessRotors::tiltedAxis(0.f, M_PI_F / 2.f); + EXPECT_EQ(axis, axis_expected); + + axis_expected = Vector3f{0.f, 1.f, 0.f}; + axis = ActuatorEffectivenessRotors::tiltedAxis(M_PI_F / 2.f, M_PI_F / 2.f); + EXPECT_EQ(axis, axis_expected); + + axis_expected = Vector3f{0.f, -1.f, 0.f}; + axis = ActuatorEffectivenessRotors::tiltedAxis(-M_PI_F / 2.f, M_PI_F / 2.f); + EXPECT_EQ(axis, axis_expected); +} diff --git a/src/lib/actuator_effectiveness/CMakeLists.txt b/src/lib/actuator_effectiveness/CMakeLists.txt index f4cdf0d0ac..8df0043110 100644 --- a/src/lib/actuator_effectiveness/CMakeLists.txt +++ b/src/lib/actuator_effectiveness/CMakeLists.txt @@ -34,6 +34,10 @@ px4_add_library(ActuatorEffectiveness ActuatorEffectiveness.cpp ActuatorEffectiveness.hpp + ActuatorEffectivenessRotors.cpp + ActuatorEffectivenessRotors.hpp + ActuatorEffectivenessThrusters.cpp + ActuatorEffectivenessThrusters.hpp ) target_compile_options(ActuatorEffectiveness PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) @@ -42,3 +46,6 @@ target_link_libraries(ActuatorEffectiveness PRIVATE mathlib ) + +px4_add_functional_gtest(SRC ActuatorEffectivenessRotorsTest.cpp LINKLIBS ActuatorEffectiveness) +px4_add_functional_gtest(SRC ActuatorEffectivenessThrustersTest.cpp LINKLIBS ActuatorEffectiveness) diff --git a/src/lib/mixer_module/functions/FunctionThrusters.hpp b/src/lib/mixer_module/functions/FunctionThrusters.hpp new file mode 100644 index 0000000000..61449c021f --- /dev/null +++ b/src/lib/mixer_module/functions/FunctionThrusters.hpp @@ -0,0 +1,124 @@ +/**************************************************************************** + * + * Copyright (c) 2021-2022 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 "FunctionProviderBase.hpp" + +/** + * Functions: Thruster1 ... ThrusterMax + */ +class FunctionThrusters : public FunctionProviderBase +{ +public: + static_assert(actuator_motors_s::NUM_CONTROLS == (int)OutputFunction::ThrusterMax - (int)OutputFunction::Thruster1 + 1, + "Unexpected num thrusters"); + + static_assert(actuator_motors_s::ACTUATOR_FUNCTION_THRUSTER1 == (int)OutputFunction::Thruster1, + "Unexpected thruster idx"); + + FunctionThrusters(const Context &context) : + _topic(&context.work_item, ORB_ID(actuator_motors)), + _thrust_factor(context.thrust_factor) + { + for (int i = 0; i < actuator_motors_s::NUM_CONTROLS; ++i) { + _data.control[i] = NAN; + } + } + + static FunctionProviderBase *allocate(const Context &context) { return new FunctionThrusters(context); } + + void update() override + { + if (_topic.update(&_data)) { + updateValues(_data.reversible_flags, _thrust_factor, _data.control, actuator_motors_s::NUM_CONTROLS); + } + } + + float value(OutputFunction func) override { return _data.control[(int)func - (int)OutputFunction::Thruster1]; } + + bool allowPrearmControl() const override { return false; } + + uORB::SubscriptionCallbackWorkItem *subscriptionCallback() override { return &_topic; } + + bool getLatestSampleTimestamp(hrt_abstime &t) const override { t = _data.timestamp_sample; return t != 0; } + + static inline void updateValues(uint32_t reversible, float thrust_factor, float *values, int num_values) + { + // TODO(@Pedro-Roque): for now bypass this + /*if (thrust_factor > 0.f && thrust_factor <= 1.f) { + + // thrust factor + // rel_thrust = factor * x^2 + (1-factor) * x, + const float a = thrust_factor; + const float b = (1.f - thrust_factor); + + // don't recompute for all values (ax^2+bx+c=0) + const float tmp1 = b / (2.f * a); + const float tmp2 = b * b / (4.f * a * a); + + for (int i = 0; i < num_values; ++i) { + float control = values[i]; + + if (control > 0.f) { + values[i] = -tmp1 + sqrtf(tmp2 + (control / a)); + + } else if (control < -0.f) { + values[i] = tmp1 - sqrtf(tmp2 - (control / a)); + + } else { + values[i] = 0.f; + } + } + } + + for (int i = 0; i < num_values; ++i) { + if ((reversible & (1u << i)) == 0) { + if (values[i] < -FLT_EPSILON) { + values[i] = NAN; + + } else { + // remap from [0, 1] to [-1, 1] + values[i] = values[i] * 2.f - 1.f; + } + } + }*/ + } + + bool reversible(OutputFunction func) const override { return false; } + +private: + uORB::SubscriptionCallbackWorkItem _topic; + actuator_motors_s _data{}; + const float &_thrust_factor; +};