implemented mapping between desired thrust and applied pwm in

multirotor mixer.

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

src/drivers/px4io/px4io.cpp

@@ -1295,6 +1295,14 @@ PX4IO::task_main()
 					}
 				}
 
+				/* thrust to pwm modelling factor */
+				parm_handle = param_find("THR_MDL_FAC");
+
+				if (parm_handle != PARAM_INVALID) {
+					param_get(parm_handle, &param_val);
+					(void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_THR_MDL_FAC, FLOAT_TO_REG(param_val));
+				}
+
 				/* maximum motor pwm slew rate */
 				parm_handle = param_find("MOT_SLEW_MAX");
 

src/modules/px4iofirmware/mixer.cpp

@@ -248,7 +248,13 @@ mixer_tick(void)
 		}
 
 		/* mix */
+		/* update parameter for mc thrust model if it updated */
+		if (update_mc_thrust_param) {
+			mixer_group.set_thrust_factor(REG_TO_FLOAT(r_setup_thr_fac));
+			update_mc_thrust_param = false;
+		}
 
+		/* mix */
 		/* poor mans mutex */
 		in_mixer = true;
 		mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT, &r_mixer_limits);
@@ -536,6 +542,12 @@ mixer_set_failsafe()
 		mixer_group.set_max_delta_out_once(delta_out_max);
 	}
 
+	/* update parameter for mc thrust model if it updated */
+	if (update_mc_thrust_param) {
+		mixer_group.set_thrust_factor(REG_TO_FLOAT(r_setup_thr_fac));
+		update_mc_thrust_param = false;
+	}
+
 	/* mix */
 	mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT, &r_mixer_limits);
 

src/modules/px4iofirmware/protocol.h

@@ -241,7 +241,9 @@ enum {							/* DSM bind states */
 
 #define PX4IO_P_SETUP_MOTOR_SLEW_MAX		24 	/* max motor slew rate */
 
-#define PX4IO_P_SETUP_THERMAL			25	/* thermal management */
+#define PX4IO_P_SETUP_THR_MDL_FAC 		25	/* factor for modelling static pwm output to thrust relationship */
+
+#define PX4IO_P_SETUP_THERMAL			26	/* thermal management */
 #define PX4IO_THERMAL_IGNORE			UINT16_MAX
 #define PX4IO_THERMAL_OFF			0
 #define PX4IO_THERMAL_FULL			10000

src/modules/px4iofirmware/px4io.h

@@ -127,6 +127,7 @@ extern uint16_t			r_page_servo_disarmed[];	/* PX4IO_PAGE_DISARMED_PWM */
 #define r_setup_scale_pitch	r_page_setup[PX4IO_P_SETUP_SCALE_PITCH]
 #define r_setup_scale_yaw	r_page_setup[PX4IO_P_SETUP_SCALE_YAW]
 #define r_setup_sbus_rate	r_page_setup[PX4IO_P_SETUP_SBUS_RATE]
+#define r_setup_thr_fac		r_page_setup[PX4IO_P_SETUP_THR_MDL_FAC]
 #define r_setup_slew_max	r_page_setup[PX4IO_P_SETUP_MOTOR_SLEW_MAX]
 
 #define r_control_values	(&r_page_controls[0])
@@ -148,6 +149,7 @@ struct sys_state_s {
 
 extern struct sys_state_s system_state;
 extern float dt;
+extern bool update_mc_thrust_param;
 
 /*
  * PWM limit structure

src/modules/px4iofirmware/registers.c

@@ -56,6 +56,7 @@
 static int	registers_set_one(uint8_t page, uint8_t offset, uint16_t value);
 static void	pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate);
 
+bool update_mc_thrust_param;
 /**
  * PAGE 0
  *
@@ -182,6 +183,7 @@ volatile uint16_t	r_page_setup[] = {
 	[PX4IO_P_SETUP_SCALE_PITCH] = 10000,
 	[PX4IO_P_SETUP_SCALE_YAW] = 10000,
 	[PX4IO_P_SETUP_MOTOR_SLEW_MAX] = 0,
+	[PX4IO_P_SETUP_THR_MDL_FAC] = 0,
 	[PX4IO_P_SETUP_THERMAL] = PX4IO_THERMAL_IGNORE
 };
 
@@ -709,12 +711,14 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 		case PX4IO_P_SETUP_SCALE_ROLL:
 		case PX4IO_P_SETUP_SCALE_PITCH:
 		case PX4IO_P_SETUP_SCALE_YAW:
+		case PX4IO_P_SETUP_MOTOR_SLEW_MAX:
 		case PX4IO_P_SETUP_SBUS_RATE:
 			r_page_setup[offset] = value;
 			sbus1_set_output_rate_hz(value);
 			break;
 
-		case PX4IO_P_SETUP_MOTOR_SLEW_MAX:
+		case PX4IO_P_SETUP_THR_MDL_FAC:
+			update_mc_thrust_param = true;
 			r_page_setup[offset] = value;
 			break;
 

src/modules/sensors/sensor_params.c

@@ -3277,6 +3277,18 @@ PARAM_DEFINE_INT32(PWM_AUX_MAX, 2000);
  */
 PARAM_DEFINE_INT32(PWM_AUX_DISARMED, 1500);
 
+/**
+ * Thrust to PWM model parameter
+ *
+ * Parameter used to model the relationship between static thrust and motor
+ * input PWM. Model is: thrust = (1-factor)*PWM + factor * PWM^2
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group PWM Outputs
+ */
+PARAM_DEFINE_FLOAT(THR_MDL_FAC, 0.65f);
+
 /**
  * Minimum motor rise time (slew rate limit).
  *

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