From 5ac0a3043b22216499c98d209a581d1185cb0e2f Mon Sep 17 00:00:00 2001 From: Lasse Date: Tue, 8 Oct 2019 19:33:42 +0200 Subject: [PATCH] Clarify Documentation of THR_MDL_FAC The documentation of the thrust model parameter `THR_MDL_FAC` did not mention both thrust and "PWM" being relative values. Also the use of the term PWM could be misleading, since the model is applicable to CAN ESCs as well. This commit rephrases the user documentation string and a few source code comments, but no logic changes are made. Closes PX4/Firmware#13105 --- src/lib/mixer/mixer.h | 6 +++--- src/lib/mixer_module/mixer_module.hpp | 2 +- src/modules/px4iofirmware/protocol.h | 2 +- src/modules/sensors/motor_params.c | 10 +++++++--- 4 files changed, 12 insertions(+), 8 deletions(-) diff --git a/src/lib/mixer/mixer.h b/src/lib/mixer/mixer.h index 5bf02129cb..b6572f5396 100644 --- a/src/lib/mixer/mixer.h +++ b/src/lib/mixer/mixer.h @@ -241,7 +241,7 @@ public: virtual unsigned get_trim(float *trim) = 0; /* - * @brief Sets the thrust factor used to calculate mapping from desired thrust to pwm. + * @brief Sets the thrust factor used to calculate mapping from desired thrust to motor control signal output. * * @param[in] val The value */ @@ -444,7 +444,7 @@ public: } /** - * @brief Sets the thrust factor used to calculate mapping from desired thrust to pwm. + * @brief Sets the thrust factor used to calculate mapping from desired thrust to motor control signal output. * * @param[in] val The value */ @@ -708,7 +708,7 @@ public: } /** - * @brief Sets the thrust factor used to calculate mapping from desired thrust to pwm. + * @brief Sets the thrust factor used to calculate mapping from desired thrust to motor control signal output. * * @param[in] val The value */ diff --git a/src/lib/mixer_module/mixer_module.hpp b/src/lib/mixer_module/mixer_module.hpp index e60a2847ad..3b0721a78f 100644 --- a/src/lib/mixer_module/mixer_module.hpp +++ b/src/lib/mixer_module/mixer_module.hpp @@ -233,7 +233,7 @@ private: DEFINE_PARAMETERS( (ParamInt) _param_mc_airmode, ///< multicopter air-mode (ParamFloat) _param_mot_slew_max, - (ParamFloat) _param_thr_mdl_fac, ///< thrust to pwm modelling factor + (ParamFloat) _param_thr_mdl_fac, ///< thrust to motor control signal modelling factor (ParamInt) _param_mot_ordering, (ParamInt) _param_cbrk_io_safety diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 3dde775ef6..3cfcee2d12 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -235,7 +235,7 @@ enum { /* DSM bind states */ #define PX4IO_P_SETUP_MOTOR_SLEW_MAX 24 /**< max motor slew rate */ -#define PX4IO_P_SETUP_THR_MDL_FAC 25 /**< factor for modelling static pwm output to thrust relationship */ +#define PX4IO_P_SETUP_THR_MDL_FAC 25 /**< factor for modelling motor control signal output to static thrust relationship */ #define PX4IO_P_SETUP_THERMAL 26 /**< thermal management */ diff --git a/src/modules/sensors/motor_params.c b/src/modules/sensors/motor_params.c index fa625230da..94d246f74b 100644 --- a/src/modules/sensors/motor_params.c +++ b/src/modules/sensors/motor_params.c @@ -55,10 +55,14 @@ PARAM_DEFINE_FLOAT(MOT_SLEW_MAX, 0.0f); /** - * Thrust to PWM model parameter + * Thrust to motor control signal model parameter * - * Parameter used to model the relationship between static thrust and motor - * input PWM. Model is: thrust = (1-factor)*PWM + factor * PWM^2 + * Parameter used to model the nonlinear relationship between + * motor control signal (e.g. PWM) and static thrust. + * + * The model is: rel_thrust = factor * rel_signal^2 + (1-factor) * rel_signal, + * where rel_thrust is the normalized thrust between 0 and 1, and + * rel_signal is the relative motor control signal between 0 and 1. * * @min 0.0 * @max 1.0