From 52a2ef34fd47819cdb6b3312a1fb3f93443407ef Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 8 Nov 2021 14:17:21 +0100 Subject: [PATCH] mixer_module: add THR_MDL_FAC for SYS_CTRL_ALLOC=1 --- src/lib/mixer_module/actuator_test.cpp | 13 ++------- src/lib/mixer_module/actuator_test.hpp | 2 +- src/lib/mixer_module/functions.cpp | 17 +++-------- src/lib/mixer_module/functions.hpp | 29 +++++++++++++++++++ src/lib/mixer_module/mixer_module.cpp | 4 +-- src/lib/mixer_module/motor_params.c | 2 ++ ...ontrolAllocationSequentialDesaturation.cpp | 2 -- 7 files changed, 41 insertions(+), 28 deletions(-) diff --git a/src/lib/mixer_module/actuator_test.cpp b/src/lib/mixer_module/actuator_test.cpp index 4bbd73dc1f..dc7c2bc967 100644 --- a/src/lib/mixer_module/actuator_test.cpp +++ b/src/lib/mixer_module/actuator_test.cpp @@ -41,7 +41,7 @@ ActuatorTest::ActuatorTest(const OutputFunction function_assignments[MAX_ACTUATO reset(); } -void ActuatorTest::update(int num_outputs, bool reversible_motors) +void ActuatorTest::update(int num_outputs, bool reversible_motors, float thrust_curve) { const hrt_abstime now = hrt_absolute_time(); @@ -73,15 +73,8 @@ void ActuatorTest::update(int num_outputs, bool reversible_motors) float value = actuator_test.value; // handle motors - if (actuator_test.function >= (int)OutputFunction::Motor1 && actuator_test.function <= (int)OutputFunction::MotorMax - && !reversible_motors) { - if (value < -FLT_EPSILON) { - value = NAN; - - } else { - // remap from [0, 1] to [-1, 1] - value = value * 2.f - 1.f; - } + if (actuator_test.function >= (int)OutputFunction::Motor1 && actuator_test.function <= (int)OutputFunction::MotorMax) { + FunctionMotors::updateValues(reversible_motors, thrust_curve, &value, 1); } _current_outputs[i] = value; diff --git a/src/lib/mixer_module/actuator_test.hpp b/src/lib/mixer_module/actuator_test.hpp index b6617334f5..d75174c540 100644 --- a/src/lib/mixer_module/actuator_test.hpp +++ b/src/lib/mixer_module/actuator_test.hpp @@ -55,7 +55,7 @@ public: void reset(); - void update(int num_outputs, bool reversible_motors); + void update(int num_outputs, bool reversible_motors, float thrust_curve); void overrideValues(float outputs[MAX_ACTUATORS], int num_outputs); diff --git a/src/lib/mixer_module/functions.cpp b/src/lib/mixer_module/functions.cpp index 73d39672aa..2d9c20c1b4 100644 --- a/src/lib/mixer_module/functions.cpp +++ b/src/lib/mixer_module/functions.cpp @@ -35,7 +35,8 @@ FunctionMotors::FunctionMotors(const Context &context) : _topic(&context.work_item, ORB_ID(actuator_motors)), - _reversible_motors(context.reversible_motors) + _reversible_motors(context.reversible_motors), + _thrust_factor(context.thrust_factor) { for (int i = 0; i < actuator_motors_s::NUM_CONTROLS; ++i) { _data.control[i] = NAN; @@ -43,18 +44,8 @@ FunctionMotors::FunctionMotors(const Context &context) } void FunctionMotors::update() { - bool updated = _topic.update(&_data); - - if (updated && !_reversible_motors) { - for (int i = 0; i < actuator_motors_s::NUM_CONTROLS; ++i) { - if (_data.control[i] < -FLT_EPSILON) { - _data.control[i] = NAN; - - } else { - // remap from [0, 1] to [-1, 1] - _data.control[i] = _data.control[i] * 2.f - 1.f; - } - } + if (_topic.update(&_data)) { + updateValues(_reversible_motors, _thrust_factor, _data.control, actuator_motors_s::NUM_CONTROLS); } } diff --git a/src/lib/mixer_module/functions.hpp b/src/lib/mixer_module/functions.hpp index 4dd51c2058..8394f570e4 100644 --- a/src/lib/mixer_module/functions.hpp +++ b/src/lib/mixer_module/functions.hpp @@ -53,6 +53,7 @@ public: struct Context { px4::WorkItem &work_item; bool reversible_motors; + const float &thrust_factor; }; FunctionProviderBase() = default; @@ -122,12 +123,40 @@ public: uORB::SubscriptionCallbackWorkItem *subscriptionCallback() override { return &_topic; } bool getLatestSampleTimestamp(hrt_abstime &t) const override { t = _data.timestamp_sample; return t != 0; } + + static inline void updateValues(bool reversible, float thrust_factor, float *values, int num_values); private: uORB::SubscriptionCallbackWorkItem _topic; actuator_motors_s _data{}; const bool _reversible_motors; + const float &_thrust_factor; }; +void FunctionMotors::updateValues(bool reversible, float thrust_factor, float *values, int num_values) +{ + if (thrust_factor > FLT_EPSILON) { + for (int i = 0; i < num_values; ++i) { + float control = values[i]; + control = matrix::sign(control) * (-(1.0f - thrust_factor) / (2.0f * thrust_factor) + sqrtf((1.0f - thrust_factor) * + (1.0f - thrust_factor) / (4.0f * thrust_factor * thrust_factor) + (fabsf(control) / thrust_factor))); + values[i] = control; + } + } + + if (!reversible) { + for (int i = 0; i < num_values; ++i) { + 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 */ diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index 76e99e9cbb..648fc5800a 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -385,7 +385,7 @@ bool MixingOutput::updateSubscriptionsDynamicMixer(bool allow_wq_switch, bool li cleanupFunctions(); - const FunctionProviderBase::Context context{_interface, _reversible_motors}; + const FunctionProviderBase::Context context{_interface, _reversible_motors, _param_thr_mdl_fac.reference()}; int provider_indexes[MAX_ACTUATORS] {}; int next_provider = 0; int subscription_callback_provider_index = INT_MAX; @@ -776,7 +776,7 @@ bool MixingOutput::updateDynamicMixer() } // check for actuator test - _actuator_test.update(_max_num_outputs, _reversible_motors); + _actuator_test.update(_max_num_outputs, _reversible_motors, _param_thr_mdl_fac.get()); // get output values float outputs[MAX_ACTUATORS]; diff --git a/src/lib/mixer_module/motor_params.c b/src/lib/mixer_module/motor_params.c index 94d246f74b..d60cfaed2b 100644 --- a/src/lib/mixer_module/motor_params.c +++ b/src/lib/mixer_module/motor_params.c @@ -66,6 +66,8 @@ PARAM_DEFINE_FLOAT(MOT_SLEW_MAX, 0.0f); * * @min 0.0 * @max 1.0 + * @decimal 1 + * @increment 0.1 * @group PWM Outputs */ PARAM_DEFINE_FLOAT(THR_MDL_FAC, 0.0f); diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp index 4fe2720ace..8cc0679e5e 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp @@ -61,8 +61,6 @@ ControlAllocationSequentialDesaturation::allocate() break; } - // TODO: thrust model (THR_MDL_FAC) - // Clip clipActuatorSetpoint(_actuator_sp);