diff --git a/src/lib/mixer_module/CMakeLists.txt b/src/lib/mixer_module/CMakeLists.txt index 0500e265d7..3f9822027f 100644 --- a/src/lib/mixer_module/CMakeLists.txt +++ b/src/lib/mixer_module/CMakeLists.txt @@ -44,14 +44,25 @@ add_custom_command(OUTPUT ${functions_header} add_custom_target(output_functions_header DEPENDS ${functions_header}) px4_add_library(mixer_module + functions/FunctionActuatorSet.hpp + functions/FunctionConstantMax.hpp + functions/FunctionConstantMin.hpp + functions/FunctionGimbal.hpp + functions/FunctionLandingGear.hpp + functions/FunctionManualRC.hpp + functions/FunctionMotors.hpp + functions/FunctionParachute.hpp + functions/FunctionServos.hpp + actuator_test.cpp + actuator_test.hpp mixer_module.cpp - functions.cpp + mixer_module.hpp ) add_dependencies(mixer_module output_functions_header) target_compile_options(mixer_module PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) - +target_include_directories(mixer_module PRIVATE ${CMAKE_CURRENT_BINARY_DIR}) px4_add_functional_gtest(SRC mixer_module_tests.cpp LINKLIBS mixer_module mixer) diff --git a/src/lib/mixer_module/actuator_test.cpp b/src/lib/mixer_module/actuator_test.cpp index b022327429..3ff8d4a7c3 100644 --- a/src/lib/mixer_module/actuator_test.cpp +++ b/src/lib/mixer_module/actuator_test.cpp @@ -33,6 +33,8 @@ #include "actuator_test.hpp" +#include "functions/FunctionMotors.hpp" + using namespace time_literals; ActuatorTest::ActuatorTest(const OutputFunction function_assignments[MAX_ACTUATORS]) diff --git a/src/lib/mixer_module/actuator_test.hpp b/src/lib/mixer_module/actuator_test.hpp index 26fcd80e21..e3e4816b7e 100644 --- a/src/lib/mixer_module/actuator_test.hpp +++ b/src/lib/mixer_module/actuator_test.hpp @@ -33,7 +33,7 @@ #pragma once -#include "functions.hpp" +#include #include #include diff --git a/src/lib/mixer_module/functions.cpp b/src/lib/mixer_module/functions.cpp deleted file mode 100644 index 4b70676e9f..0000000000 --- a/src/lib/mixer_module/functions.cpp +++ /dev/null @@ -1,135 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2021 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include "functions.hpp" - -FunctionMotors::FunctionMotors(const Context &context) - : _topic(&context.work_item, ORB_ID(actuator_motors)), - _thrust_factor(context.thrust_factor) -{ - for (int i = 0; i < actuator_motors_s::NUM_CONTROLS; ++i) { - _data.control[i] = NAN; - } -} -void FunctionMotors::update() -{ - if (_topic.update(&_data)) { - updateValues(_data.reversible_flags, _thrust_factor, _data.control, actuator_motors_s::NUM_CONTROLS); - } -} - -FunctionServos::FunctionServos(const Context &context) - : _topic(&context.work_item, ORB_ID(actuator_servos)) -{ - for (int i = 0; i < actuator_servos_s::NUM_CONTROLS; ++i) { - _data.control[i] = NAN; - } -} - -FunctionActuatorSet::FunctionActuatorSet() -{ - for (int i = 0; i < max_num_actuators; ++i) { - _data[i] = NAN; - } -} - -void FunctionActuatorSet::update() -{ - vehicle_command_s vehicle_command; - - while (_topic.update(&vehicle_command)) { - if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_SET_ACTUATOR) { - int index = (int)(vehicle_command.param7 + 0.5f); - - if (index == 0) { - _data[0] = vehicle_command.param1; - _data[1] = vehicle_command.param2; - _data[2] = vehicle_command.param3; - _data[3] = vehicle_command.param4; - _data[4] = vehicle_command.param5; - _data[5] = vehicle_command.param6; - } - } - } -} - -void FunctionLandingGear::update() -{ - landing_gear_s landing_gear; - - if (_topic.update(&landing_gear)) { - if (landing_gear.landing_gear == landing_gear_s::GEAR_DOWN) { - _data = -1.f; - - } else if (landing_gear.landing_gear == landing_gear_s::GEAR_UP) { - _data = 1.f; - } - } -} - -FunctionManualRC::FunctionManualRC() -{ - for (int i = 0; i < num_data_points; ++i) { - _data[i] = NAN; - } -} - -void FunctionManualRC::update() -{ - manual_control_setpoint_s manual_control_setpoint; - - if (_topic.update(&manual_control_setpoint)) { - _data[0] = manual_control_setpoint.y; // roll - _data[1] = manual_control_setpoint.x; // pitch - _data[2] = manual_control_setpoint.z * 2.f - 1.f; // throttle - _data[3] = manual_control_setpoint.r; // yaw - _data[4] = manual_control_setpoint.flaps; - _data[5] = manual_control_setpoint.aux1; - _data[6] = manual_control_setpoint.aux2; - _data[7] = manual_control_setpoint.aux3; - _data[8] = manual_control_setpoint.aux4; - _data[9] = manual_control_setpoint.aux5; - _data[10] = manual_control_setpoint.aux6; - } -} - -void FunctionGimbal::update() -{ - actuator_controls_s actuator_controls; - - if (_topic.update(&actuator_controls)) { - _data[0] = actuator_controls.control[0]; - _data[1] = actuator_controls.control[1]; - _data[2] = actuator_controls.control[2]; - } -} diff --git a/src/lib/mixer_module/functions.hpp b/src/lib/mixer_module/functions.hpp deleted file mode 100644 index 1059baee2a..0000000000 --- a/src/lib/mixer_module/functions.hpp +++ /dev/null @@ -1,295 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2021 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#pragma once - -#include - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -class FunctionProviderBase -{ -public: - struct Context { - px4::WorkItem &work_item; - const float &thrust_factor; - }; - - FunctionProviderBase() = default; - virtual ~FunctionProviderBase() = default; - - virtual void update() = 0; - - /** - * Get the current output value for a given function - * @return NAN (=disarmed) or value in range [-1, 1] - */ - virtual float value(OutputFunction func) = 0; - - virtual float defaultFailsafeValue(OutputFunction func) const { return NAN; } - virtual bool allowPrearmControl() const { return true; } - - virtual uORB::SubscriptionCallbackWorkItem *subscriptionCallback() { return nullptr; } - - virtual bool getLatestSampleTimestamp(hrt_abstime &t) const { return false; } - - /** - * Check whether the output (motor) is configured to be reversible - */ - virtual bool reversible(OutputFunction func) const { return false; } -}; - -/** - * Functions: Constant_Min - */ -class FunctionConstantMin : public FunctionProviderBase -{ -public: - static FunctionProviderBase *allocate(const Context &context) { return new FunctionConstantMin(); } - - float value(OutputFunction func) override { return -1.f; } - void update() override { } - - float defaultFailsafeValue(OutputFunction func) const override { return -1.f; } -}; - -/** - * Functions: Constant_Max - */ -class FunctionConstantMax : public FunctionProviderBase -{ -public: - static FunctionProviderBase *allocate(const Context &context) { return new FunctionConstantMax(); } - - float value(OutputFunction func) override { return 1.f; } - void update() override { } - - float defaultFailsafeValue(OutputFunction func) const override { return 1.f; } -}; - -/** - * Functions: Motor1 ... MotorMax - */ -class FunctionMotors : public FunctionProviderBase -{ -public: - static_assert(actuator_motors_s::NUM_CONTROLS == (int)OutputFunction::MotorMax - (int)OutputFunction::Motor1 + 1, - "Unexpected num motors"); - - static_assert(actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1 == (int)OutputFunction::Motor1, "Unexpected motor idx"); - - FunctionMotors(const Context &context); - static FunctionProviderBase *allocate(const Context &context) { return new FunctionMotors(context); } - - void update() override; - float value(OutputFunction func) override { return _data.control[(int)func - (int)OutputFunction::Motor1]; } - - bool allowPrearmControl() const override { return false; } - - uORB::SubscriptionCallbackWorkItem *subscriptionCallback() override { return &_topic; } - - bool getLatestSampleTimestamp(hrt_abstime &t) const override { t = _data.timestamp_sample; return t != 0; } - - static inline void updateValues(uint32_t reversible, float thrust_factor, float *values, int num_values); - - bool reversible(OutputFunction func) const override - { return _data.reversible_flags & (1u << ((int)func - (int)OutputFunction::Motor1)); } -private: - uORB::SubscriptionCallbackWorkItem _topic; - actuator_motors_s _data{}; - const float &_thrust_factor; -}; - -void FunctionMotors::updateValues(uint32_t reversible, float thrust_factor, float *values, int num_values) -{ - 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; - } - } - } -} - - -/** - * Functions: Servo1 ... ServoMax - */ -class FunctionServos : public FunctionProviderBase -{ -public: - static_assert(actuator_servos_s::NUM_CONTROLS == (int)OutputFunction::ServoMax - (int)OutputFunction::Servo1 + 1, - "Unexpected num servos"); - - FunctionServos(const Context &context); - static FunctionProviderBase *allocate(const Context &context) { return new FunctionServos(context); } - - void update() override { _topic.update(&_data); } - float value(OutputFunction func) override { return _data.control[(int)func - (int)OutputFunction::Servo1]; } - - uORB::SubscriptionCallbackWorkItem *subscriptionCallback() override { return &_topic; } - - float defaultFailsafeValue(OutputFunction func) const override { return 0.f; } -private: - uORB::SubscriptionCallbackWorkItem _topic; - actuator_servos_s _data{}; -}; - - -/** - * Functions: Offboard_Actuator_Set1 ... Offboard_Actuator_Set6 - */ -class FunctionActuatorSet : public FunctionProviderBase -{ -public: - FunctionActuatorSet(); - static FunctionProviderBase *allocate(const Context &context) { return new FunctionActuatorSet(); } - - void update() override; - float value(OutputFunction func) override { return _data[(int)func - (int)OutputFunction::Offboard_Actuator_Set1]; } - -private: - static constexpr int max_num_actuators = 6; - - uORB::Subscription _topic{ORB_ID(vehicle_command)}; - float _data[max_num_actuators]; -}; - -/** - * Functions: Landing_Gear - */ -class FunctionLandingGear : public FunctionProviderBase -{ -public: - FunctionLandingGear() = default; - static FunctionProviderBase *allocate(const Context &context) { return new FunctionLandingGear(); } - - void update() override; - float value(OutputFunction func) override { return _data; } - -private: - uORB::Subscription _topic{ORB_ID(landing_gear)}; - float _data{-1.f}; -}; - -/** - * Functions: Parachute - */ -class FunctionParachute : public FunctionProviderBase -{ -public: - FunctionParachute() = default; - static FunctionProviderBase *allocate(const Context &context) { return new FunctionParachute(); } - - void update() override {} - float value(OutputFunction func) override { return -1.f; } - float defaultFailsafeValue(OutputFunction func) const override { return 1.f; } -}; - -/** - * Functions: RC_Roll .. RCAUX_Max - */ -class FunctionManualRC : public FunctionProviderBase -{ -public: - FunctionManualRC(); - static FunctionProviderBase *allocate(const Context &context) { return new FunctionManualRC(); } - - void update() override; - float value(OutputFunction func) override { return _data[(int)func - (int)OutputFunction::RC_Roll]; } - -private: - static constexpr int num_data_points = 11; - - static_assert(num_data_points == (int)OutputFunction::RC_AUXMax - (int)OutputFunction::RC_Roll + 1, - "number of functions mismatch"); - - uORB::Subscription _topic{ORB_ID(manual_control_setpoint)}; - float _data[num_data_points]; -}; - -/** - * Functions: Gimbal_Roll .. Gimbal_Yaw - */ -class FunctionGimbal : public FunctionProviderBase -{ -public: - FunctionGimbal() = default; - static FunctionProviderBase *allocate(const Context &context) { return new FunctionGimbal(); } - - void update() override; - float value(OutputFunction func) override { return _data[(int)func - (int)OutputFunction::Gimbal_Roll]; } - -private: - uORB::Subscription _topic{ORB_ID(actuator_controls_2)}; - float _data[3] { NAN, NAN, NAN }; -}; diff --git a/src/lib/mixer_module/functions/FunctionActuatorSet.hpp b/src/lib/mixer_module/functions/FunctionActuatorSet.hpp new file mode 100644 index 0000000000..c9369addd5 --- /dev/null +++ b/src/lib/mixer_module/functions/FunctionActuatorSet.hpp @@ -0,0 +1,82 @@ +/**************************************************************************** + * + * 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" + +#include + +/** + * Functions: Offboard_Actuator_Set1 ... Offboard_Actuator_Set6 + */ +class FunctionActuatorSet : public FunctionProviderBase +{ +public: + FunctionActuatorSet() + { + for (int i = 0; i < max_num_actuators; ++i) { + _data[i] = NAN; + } + } + + static FunctionProviderBase *allocate(const Context &context) { return new FunctionActuatorSet(); } + + void update() override + { + vehicle_command_s vehicle_command; + + while (_topic.update(&vehicle_command)) { + if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_SET_ACTUATOR) { + int index = (int)(vehicle_command.param7 + 0.5f); + + if (index == 0) { + _data[0] = vehicle_command.param1; + _data[1] = vehicle_command.param2; + _data[2] = vehicle_command.param3; + _data[3] = vehicle_command.param4; + _data[4] = vehicle_command.param5; + _data[5] = vehicle_command.param6; + } + } + } + } + + float value(OutputFunction func) override { return _data[(int)func - (int)OutputFunction::Offboard_Actuator_Set1]; } + +private: + static constexpr int max_num_actuators = 6; + + uORB::Subscription _topic{ORB_ID(vehicle_command)}; + float _data[max_num_actuators] {}; +}; diff --git a/src/lib/mixer_module/functions/FunctionConstantMax.hpp b/src/lib/mixer_module/functions/FunctionConstantMax.hpp new file mode 100644 index 0000000000..317a209ee2 --- /dev/null +++ b/src/lib/mixer_module/functions/FunctionConstantMax.hpp @@ -0,0 +1,50 @@ +/**************************************************************************** + * + * 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: Constant_Max + */ +class FunctionConstantMax : public FunctionProviderBase +{ +public: + static FunctionProviderBase *allocate(const Context &context) { return new FunctionConstantMax(); } + + float value(OutputFunction func) override { return 1.f; } + void update() override { } + + float defaultFailsafeValue(OutputFunction func) const override { return 1.f; } +}; diff --git a/src/lib/mixer_module/functions/FunctionConstantMin.hpp b/src/lib/mixer_module/functions/FunctionConstantMin.hpp new file mode 100644 index 0000000000..1516d80520 --- /dev/null +++ b/src/lib/mixer_module/functions/FunctionConstantMin.hpp @@ -0,0 +1,50 @@ +/**************************************************************************** + * + * 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: Constant_Min + */ +class FunctionConstantMin : public FunctionProviderBase +{ +public: + static FunctionProviderBase *allocate(const Context &context) { return new FunctionConstantMin(); } + + float value(OutputFunction func) override { return -1.f; } + void update() override { } + + float defaultFailsafeValue(OutputFunction func) const override { return -1.f; } +}; diff --git a/src/lib/mixer_module/functions/FunctionGimbal.hpp b/src/lib/mixer_module/functions/FunctionGimbal.hpp new file mode 100644 index 0000000000..513a344460 --- /dev/null +++ b/src/lib/mixer_module/functions/FunctionGimbal.hpp @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * 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" + +#include + +/** + * Functions: Gimbal_Roll .. Gimbal_Yaw + */ +class FunctionGimbal : public FunctionProviderBase +{ +public: + FunctionGimbal() = default; + static FunctionProviderBase *allocate(const Context &context) { return new FunctionGimbal(); } + + void update() override + { + actuator_controls_s actuator_controls; + + if (_topic.update(&actuator_controls)) { + _data[0] = actuator_controls.control[0]; + _data[1] = actuator_controls.control[1]; + _data[2] = actuator_controls.control[2]; + } + } + + float value(OutputFunction func) override { return _data[(int)func - (int)OutputFunction::Gimbal_Roll]; } + +private: + uORB::Subscription _topic{ORB_ID(actuator_controls_2)}; + float _data[3] { NAN, NAN, NAN }; +}; diff --git a/src/lib/mixer_module/functions/FunctionLandingGear.hpp b/src/lib/mixer_module/functions/FunctionLandingGear.hpp new file mode 100644 index 0000000000..80c87bc9cd --- /dev/null +++ b/src/lib/mixer_module/functions/FunctionLandingGear.hpp @@ -0,0 +1,68 @@ +/**************************************************************************** + * + * 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" + +#include + +/** + * Functions: Landing_Gear + */ +class FunctionLandingGear : public FunctionProviderBase +{ +public: + FunctionLandingGear() = default; + static FunctionProviderBase *allocate(const Context &context) { return new FunctionLandingGear(); } + + void update() override + { + landing_gear_s landing_gear; + + if (_topic.update(&landing_gear)) { + if (landing_gear.landing_gear == landing_gear_s::GEAR_DOWN) { + _data = -1.f; + + } else if (landing_gear.landing_gear == landing_gear_s::GEAR_UP) { + _data = 1.f; + } + } + } + + float value(OutputFunction func) override { return _data; } + +private: + uORB::Subscription _topic{ORB_ID(landing_gear)}; + float _data{-1.f}; +}; diff --git a/src/lib/mixer_module/functions/FunctionManualRC.hpp b/src/lib/mixer_module/functions/FunctionManualRC.hpp new file mode 100644 index 0000000000..9ec2db2b6d --- /dev/null +++ b/src/lib/mixer_module/functions/FunctionManualRC.hpp @@ -0,0 +1,84 @@ +/**************************************************************************** + * + * 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" + +#include + +/** + * Functions: RC_Roll .. RCAUX_Max + */ +class FunctionManualRC : public FunctionProviderBase +{ +public: + FunctionManualRC() + { + for (int i = 0; i < num_data_points; ++i) { + _data[i] = NAN; + } + } + + static FunctionProviderBase *allocate(const Context &context) { return new FunctionManualRC(); } + + void update() override + { + manual_control_setpoint_s manual_control_setpoint; + + if (_topic.update(&manual_control_setpoint)) { + _data[0] = manual_control_setpoint.y; // roll + _data[1] = manual_control_setpoint.x; // pitch + _data[2] = manual_control_setpoint.z * 2.f - 1.f; // throttle + _data[3] = manual_control_setpoint.r; // yaw + _data[4] = manual_control_setpoint.flaps; + _data[5] = manual_control_setpoint.aux1; + _data[6] = manual_control_setpoint.aux2; + _data[7] = manual_control_setpoint.aux3; + _data[8] = manual_control_setpoint.aux4; + _data[9] = manual_control_setpoint.aux5; + _data[10] = manual_control_setpoint.aux6; + } + } + + float value(OutputFunction func) override { return _data[(int)func - (int)OutputFunction::RC_Roll]; } + +private: + static constexpr int num_data_points = 11; + + static_assert(num_data_points == (int)OutputFunction::RC_AUXMax - (int)OutputFunction::RC_Roll + 1, + "number of functions mismatch"); + + uORB::Subscription _topic{ORB_ID(manual_control_setpoint)}; + float _data[num_data_points] {}; +}; diff --git a/src/lib/mixer_module/functions/FunctionMotors.hpp b/src/lib/mixer_module/functions/FunctionMotors.hpp new file mode 100644 index 0000000000..6873a3919f --- /dev/null +++ b/src/lib/mixer_module/functions/FunctionMotors.hpp @@ -0,0 +1,123 @@ +/**************************************************************************** + * + * 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" + +#include + +/** + * Functions: Motor1 ... MotorMax + */ +class FunctionMotors : public FunctionProviderBase +{ +public: + static_assert(actuator_motors_s::NUM_CONTROLS == (int)OutputFunction::MotorMax - (int)OutputFunction::Motor1 + 1, + "Unexpected num motors"); + + static_assert(actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1 == (int)OutputFunction::Motor1, "Unexpected motor idx"); + + FunctionMotors(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 FunctionMotors(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::Motor1]; } + + bool allowPrearmControl() const override { return false; } + + uORB::SubscriptionCallbackWorkItem *subscriptionCallback() override { return &_topic; } + + bool getLatestSampleTimestamp(hrt_abstime &t) const override { t = _data.timestamp_sample; return t != 0; } + + static inline void updateValues(uint32_t reversible, float thrust_factor, float *values, int num_values) + { + 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 _data.reversible_flags & (1u << ((int)func - (int)OutputFunction::Motor1)); } + +private: + uORB::SubscriptionCallbackWorkItem _topic; + actuator_motors_s _data{}; + const float &_thrust_factor; +}; diff --git a/src/lib/mixer_module/functions/FunctionParachute.hpp b/src/lib/mixer_module/functions/FunctionParachute.hpp new file mode 100644 index 0000000000..d81f623325 --- /dev/null +++ b/src/lib/mixer_module/functions/FunctionParachute.hpp @@ -0,0 +1,50 @@ +/**************************************************************************** + * + * 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: Parachute + */ +class FunctionParachute : public FunctionProviderBase +{ +public: + FunctionParachute() = default; + static FunctionProviderBase *allocate(const Context &context) { return new FunctionParachute(); } + + void update() override {} + float value(OutputFunction func) override { return -1.f; } + float defaultFailsafeValue(OutputFunction func) const override { return 1.f; } +}; diff --git a/src/lib/mixer_module/functions/FunctionProviderBase.hpp b/src/lib/mixer_module/functions/FunctionProviderBase.hpp new file mode 100644 index 0000000000..f3a648bb28 --- /dev/null +++ b/src/lib/mixer_module/functions/FunctionProviderBase.hpp @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * 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 + +#include + +#include +#include + +#include + +class FunctionProviderBase +{ +public: + struct Context { + px4::WorkItem &work_item; + const float &thrust_factor; + }; + + FunctionProviderBase() = default; + virtual ~FunctionProviderBase() = default; + + virtual void update() = 0; + + /** + * Get the current output value for a given function + * @return NAN (=disarmed) or value in range [-1, 1] + */ + virtual float value(OutputFunction func) = 0; + + virtual float defaultFailsafeValue(OutputFunction func) const { return NAN; } + virtual bool allowPrearmControl() const { return true; } + + virtual uORB::SubscriptionCallbackWorkItem *subscriptionCallback() { return nullptr; } + + virtual bool getLatestSampleTimestamp(hrt_abstime &t) const { return false; } + + /** + * Check whether the output (motor) is configured to be reversible + */ + virtual bool reversible(OutputFunction func) const { return false; } +}; diff --git a/src/lib/mixer_module/functions/FunctionServos.hpp b/src/lib/mixer_module/functions/FunctionServos.hpp new file mode 100644 index 0000000000..5b69d326d8 --- /dev/null +++ b/src/lib/mixer_module/functions/FunctionServos.hpp @@ -0,0 +1,69 @@ +/**************************************************************************** + * + * 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" + +#include + +/** + * Functions: Servo1 ... ServoMax + */ +class FunctionServos : public FunctionProviderBase +{ +public: + static_assert(actuator_servos_s::NUM_CONTROLS == (int)OutputFunction::ServoMax - (int)OutputFunction::Servo1 + 1, + "Unexpected num servos"); + + FunctionServos(const Context &context) : + _topic(&context.work_item, ORB_ID(actuator_servos)) + { + for (int i = 0; i < actuator_servos_s::NUM_CONTROLS; ++i) { + _data.control[i] = NAN; + } + } + + static FunctionProviderBase *allocate(const Context &context) { return new FunctionServos(context); } + + void update() override { _topic.update(&_data); } + float value(OutputFunction func) override { return _data.control[(int)func - (int)OutputFunction::Servo1]; } + + uORB::SubscriptionCallbackWorkItem *subscriptionCallback() override { return &_topic; } + + float defaultFailsafeValue(OutputFunction func) const override { return 0.f; } + +private: + uORB::SubscriptionCallbackWorkItem _topic; + actuator_servos_s _data{}; +}; diff --git a/src/lib/mixer_module/mixer_module.hpp b/src/lib/mixer_module/mixer_module.hpp index b717855577..1c16ec6b2e 100644 --- a/src/lib/mixer_module/mixer_module.hpp +++ b/src/lib/mixer_module/mixer_module.hpp @@ -34,7 +34,16 @@ #pragma once #include "actuator_test.hpp" -#include "functions.hpp" + +#include "functions/FunctionActuatorSet.hpp" +#include "functions/FunctionConstantMax.hpp" +#include "functions/FunctionConstantMin.hpp" +#include "functions/FunctionGimbal.hpp" +#include "functions/FunctionLandingGear.hpp" +#include "functions/FunctionManualRC.hpp" +#include "functions/FunctionMotors.hpp" +#include "functions/FunctionParachute.hpp" +#include "functions/FunctionServos.hpp" #include #include