mixer_module: add THR_MDL_FAC for SYS_CTRL_ALLOC=1

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
+					if (actuator_test.function >= (int)OutputFunction::Motor1 && actuator_test.function <= (int)OutputFunction::MotorMax) {
-					    && !reversible_motors) {
+						FunctionMotors::updateValues(reversible_motors, thrust_curve, &value, 1);
-						if (value < -FLT_EPSILON) {
-							value = NAN;
-
-						} else {
-							// remap from [0, 1] to [-1, 1]
-							value = value * 2.f - 1.f;
-						}
 					}
 
 					_current_outputs[i] = value;

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

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 (_topic.update(&_data)) {
-
+		updateValues(_reversible_motors, _thrust_factor, _data.control, actuator_motors_s::NUM_CONTROLS);
-	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;
-			}
-		}
 	}
 }
 

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
  */

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

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

src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp

@@ -61,8 +61,6 @@ ControlAllocationSequentialDesaturation::allocate()
 		break;
 	}
 
-	// TODO: thrust model (THR_MDL_FAC)
-
 	// Clip
 	clipActuatorSetpoint(_actuator_sp);