dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

lib/mixer_module: split functions into separate headers 

- this arguably improves visibility/discoverability
This commit is contained in:
Daniel Agar 2022-07-27 13:28:02 -04:00 committed by Beat Küng
parent f7ff0a9961
commit 9ed861e0a3
16 changed files with 742 additions and 434 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
src/lib/mixer_module/CMakeLists.txt
View File

@ -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)

2
src/lib/mixer_module/actuator_test.cpp
View File

@ -33,6 +33,8 @@
#include "actuator_test.hpp"
#include "functions/FunctionMotors.hpp"
using namespace time_literals;
ActuatorTest::ActuatorTest(const OutputFunction function_assignments[MAX_ACTUATORS])

2
src/lib/mixer_module/actuator_test.hpp
View File

@ -33,7 +33,7 @@
#pragma once
#include "functions.hpp"
#include <mixer_module/output_functions.hpp>
#include <drivers/drv_pwm_output.h>
#include <uORB/topics/actuator_test.h>

135
src/lib/mixer_module/functions.cpp
View File

@ -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];
}
}

295
src/lib/mixer_module/functions.hpp
View File

@ -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 <limits.h>
#include <mixer_module/output_functions.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_motors.h>
#include <uORB/topics/actuator_servos.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/landing_gear.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
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 };
};

82
src/lib/mixer_module/functions/FunctionActuatorSet.hpp Normal file
View File

@ -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 <uORB/topics/vehicle_command.h>
/**
* 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] {};
};

50
src/lib/mixer_module/functions/FunctionConstantMax.hpp Normal file
View File

@ -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; }
};

50
src/lib/mixer_module/functions/FunctionConstantMin.hpp Normal file
View File

@ -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; }
};

65
src/lib/mixer_module/functions/FunctionGimbal.hpp Normal file
View File

@ -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 <uORB/topics/actuator_controls.h>
/**
* 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 };
};

68
src/lib/mixer_module/functions/FunctionLandingGear.hpp Normal file
View File

@ -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 <uORB/topics/landing_gear.h>
/**
* 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};
};

84
src/lib/mixer_module/functions/FunctionManualRC.hpp Normal file
View File

@ -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 <uORB/topics/manual_control_setpoint.h>
/**
* 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] {};
};

123
src/lib/mixer_module/functions/FunctionMotors.hpp Normal file
View File

@ -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 <uORB/topics/actuator_servos.h>
/**
* 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;
};

50
src/lib/mixer_module/functions/FunctionParachute.hpp Normal file
View File

@ -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; }
};

75
src/lib/mixer_module/functions/FunctionProviderBase.hpp Normal file
View File

@ -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 <limits.h>
#include <mixer_module/output_functions.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
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; }
};

69
src/lib/mixer_module/functions/FunctionServos.hpp Normal file
View File

@ -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 <uORB/topics/actuator_servos.h>
/**
* 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{};
};

11
src/lib/mixer_module/mixer_module.hpp
View File

@ -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 <board_config.h>
#include <drivers/drv_pwm_output.h>