implemented mapping between desired thrust and applied pwm in

multirotor mixer.

Signed-off-by: Roman <bapstroman@gmail.com>

src/modules/systemlib/mixer/mixer.h

@@ -199,8 +199,20 @@ public:
 	 */
 	virtual void 			set_max_delta_out_once(float delta_out_max) {};
 
+	/**
+	 * @brief Set trim offset for this mixer
+	 *
+	 * @return the number of outputs this mixer feeds to
+	 */
 	virtual unsigned set_trim(float trim) = 0;
 
+	/*
+	 * @brief      Sets the thrust factor used to calculate mapping from desired thrust to pwm.
+	 *
+	 * @param[in]  val   The value
+	 */
+	virtual void 			set_thrust_factor(float val) {};
+
 protected:
 	/** client-supplied callback used when fetching control values */
 	ControlCallback			_control_cb;
@@ -367,6 +379,13 @@ public:
 		return 0;
 	}
 
+	/**
+	 * @brief      Sets the thrust factor used to calculate mapping from desired thrust to pwm.
+	 *
+	 * @param[in]  val   The value
+	 */
+	virtual void	set_thrust_factor(float val);
+
 private:
 	Mixer				*_first;	/**< linked list of mixers */
 
@@ -601,13 +620,21 @@ public:
 		return _rotor_count;
 	}
 
+	/**
+	 * @brief      Sets the thrust factor used to calculate mapping from desired thrust to pwm.
+	 *
+	 * @param[in]  val   The value
+	 */
+	virtual void			set_thrust_factor(float val) {_thrust_factor = val;}
+
 private:
 	float				_roll_scale;
 	float				_pitch_scale;
 	float				_yaw_scale;
 	float				_idle_speed;
-
 	float 				_delta_out_max;
+	float 				_thrust_factor;
+
 
 	orb_advert_t			_limits_pub;
 	multirotor_motor_limits_s 	_limits;

src/modules/systemlib/mixer/mixer_group.cpp

@@ -141,6 +141,18 @@ MixerGroup::set_trims(int16_t *values, unsigned n)
 	return index;
 }
 
+void
+MixerGroup::set_thrust_factor(float val)
+{
+	Mixer	*mixer = _first;
+
+	while (mixer != nullptr) {
+		mixer->set_thrust_factor(val);
+		mixer = mixer->_next;
+	}
+
+}
+
 uint16_t
 MixerGroup::get_saturation_status()
 {

src/modules/systemlib/mixer/mixer_multirotor.cpp

@@ -91,6 +91,7 @@ MultirotorMixer::MultirotorMixer(ControlCallback control_cb,
 	_yaw_scale(yaw_scale),
 	_idle_speed(-1.0f + idle_speed * 2.0f),	/* shift to output range here to avoid runtime calculation */
 	_delta_out_max(0.0f),
+	_thrust_factor(0.0f),
 	_limits_pub(),
 	_rotor_count(_config_rotor_count[(MultirotorGeometryUnderlyingType)geometry]),
 	_rotors(_config_index[(MultirotorGeometryUnderlyingType)geometry]),
@@ -358,6 +359,18 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
 			     yaw * _rotors[i].yaw_scale +
 			     thrust + boost;
 
+		/*
+			implement simple model for static relationship between applied motor pwm and motor thrust
+			model: thrust = (1 - _thrust_factor) * PWM + _thrust_factor * PWM^2
+			this model assumes normalized input / output in the range [0,1] so this is the right place
+			to do it as at this stage the outputs are in that range.
+		 */
+		if (_thrust_factor > 0.0f) {
+			outputs[i] = -(1.0f - _thrust_factor) / (2.0f * _thrust_factor) + sqrtf((1.0f - _thrust_factor) *
+					(1.0f - _thrust_factor) / (4.0f * _thrust_factor * _thrust_factor) + (outputs[i] < 0.0f ? 0.0f : outputs[i] /
+							_thrust_factor));
+		}
+
 		outputs[i] = constrain(_idle_speed + (outputs[i] * (1.0f - _idle_speed)), _idle_speed, 1.0f);
 
 	}