From 70523b65274382c25c1c259970187749bdd61651 Mon Sep 17 00:00:00 2001 From: Roman Date: Fri, 23 Feb 2018 14:57:19 +0100 Subject: [PATCH 1/3] mixer_group: do not clamp offset to arbitrary value - allow the user to apply an offset that covers the entire servo range Signed-off-by: Roman --- src/lib/mixer/mixer_group.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/lib/mixer/mixer_group.cpp b/src/lib/mixer/mixer_group.cpp index c90ab2d3199d..13f38afbedc2 100644 --- a/src/lib/mixer/mixer_group.cpp +++ b/src/lib/mixer/mixer_group.cpp @@ -132,10 +132,10 @@ MixerGroup::set_trims(int16_t *values, unsigned n) /* convert from integer to float */ float offset = (float)values[index] / 10000; - /* to be safe, clamp offset to range of [-100, 100] usec */ - if (offset < -0.2f) { offset = -0.2f; } + /* to be safe, clamp offset to range of [-500, 500] usec */ + if (offset < -1.0f) { offset = -1.0f; } - if (offset > 0.2f) { offset = 0.2f; } + if (offset > 1.0f) { offset = 1.0f; } debug("set trim: %d, offset: %5.3f", values[index], (double)offset); index += mixer->set_trim(offset); From dc19b78730df50e20d6fac94604d03d024257f3e Mon Sep 17 00:00:00 2001 From: Roman Date: Fri, 23 Feb 2018 14:58:01 +0100 Subject: [PATCH 2/3] mixer lib: added methods to fetch the active trim values Signed-off-by: Roman --- src/lib/mixer/mixer.h | 31 +++++++++++++++++++++++++++++++ src/lib/mixer/mixer_group.cpp | 31 +++++++++++++++++++++++++++++++ src/lib/mixer/mixer_simple.cpp | 6 ++++++ 3 files changed, 68 insertions(+) diff --git a/src/lib/mixer/mixer.h b/src/lib/mixer/mixer.h index 3ccf0ba8672e..989a59e43ffd 100644 --- a/src/lib/mixer/mixer.h +++ b/src/lib/mixer/mixer.h @@ -204,6 +204,13 @@ class __EXPORT Mixer */ virtual unsigned set_trim(float trim) = 0; + /** + * @brief Get trim offset for this mixer + * + * @return the number of outputs this mixer feeds to + */ + virtual unsigned get_trim(float *trim) = 0; + /* * @brief Sets the thrust factor used to calculate mapping from desired thrust to pwm. * @@ -382,6 +389,13 @@ class __EXPORT MixerGroup : public Mixer return 0; } + unsigned get_trims(int16_t *values); + + unsigned get_trim(float *trim) + { + return 0; + } + /** * @brief Sets the thrust factor used to calculate mapping from desired thrust to pwm. * @@ -432,6 +446,11 @@ class __EXPORT NullMixer : public Mixer return 0; } + unsigned get_trim(float *trim) + { + return 0; + } + }; /** @@ -507,6 +526,8 @@ class __EXPORT SimpleMixer : public Mixer unsigned set_trim(float trim); + unsigned get_trim(float *trim); + protected: private: @@ -617,6 +638,11 @@ class __EXPORT MultirotorMixer : public Mixer return _rotor_count; } + unsigned get_trim(float *trim) + { + return _rotor_count; + } + /** * @brief Sets the thrust factor used to calculate mapping from desired thrust to pwm. * @@ -732,6 +758,11 @@ class __EXPORT HelicopterMixer : public Mixer return 4; } + unsigned get_trim(float *trim) + { + return 0; + } + private: mixer_heli_s _mixer_info; diff --git a/src/lib/mixer/mixer_group.cpp b/src/lib/mixer/mixer_group.cpp index 13f38afbedc2..e4fb98fbff7c 100644 --- a/src/lib/mixer/mixer_group.cpp +++ b/src/lib/mixer/mixer_group.cpp @@ -145,6 +145,37 @@ MixerGroup::set_trims(int16_t *values, unsigned n) return index; } +/* + * get_trims() has no effect except for the SimpleMixer implementation for which get_trim() + * always returns the value one and sets the trim value. + * The only other existing implementation is MultirotorMixer, which ignores the trim value + * and returns _rotor_count. + */ +unsigned +MixerGroup::get_trims(int16_t *values) +{ + Mixer *mixer = _first; + unsigned index_mixer = 0; + unsigned index = 0; + float trim; + + while (mixer != nullptr) { + trim = 0; + index_mixer += mixer->get_trim(&trim); + + // MultirotorMixer returns the number of motors so we + // loop through index_mixer and set the same trim value for all motors + while (index < index_mixer) { + values[index] = trim * 10000; + index++; + } + + mixer = mixer->_next; + } + + return index; +} + void MixerGroup::set_thrust_factor(float val) { diff --git a/src/lib/mixer/mixer_simple.cpp b/src/lib/mixer/mixer_simple.cpp index 92e4bbb8128a..a05b56f51b81 100644 --- a/src/lib/mixer/mixer_simple.cpp +++ b/src/lib/mixer/mixer_simple.cpp @@ -78,6 +78,12 @@ unsigned SimpleMixer::set_trim(float trim) return 1; } +unsigned SimpleMixer::get_trim(float *trim) +{ + *trim = _pinfo->output_scaler.offset; + return 1; +} + int SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler) { From 7d3dfde8c840bd09fe9a3621172bd5ad25af223d Mon Sep 17 00:00:00 2001 From: Roman Date: Fri, 23 Feb 2018 14:59:29 +0100 Subject: [PATCH 3/3] fmu: removed obsolete _trim_pwm member and implemented proper fetching of mixer trim values Signed-off-by: Roman --- src/drivers/px4fmu/fmu.cpp | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index baffe71d9537..32556a404edb 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -265,7 +265,6 @@ class PX4FMU : public device::CDev, public ModuleBase uint16_t _disarmed_pwm[_max_actuators]; uint16_t _min_pwm[_max_actuators]; uint16_t _max_pwm[_max_actuators]; - uint16_t _trim_pwm[_max_actuators]; uint16_t _reverse_pwm_mask; unsigned _num_failsafe_set; unsigned _num_disarmed_set; @@ -391,7 +390,6 @@ PX4FMU::PX4FMU(bool run_as_task) : for (unsigned i = 0; i < _max_actuators; i++) { _min_pwm[i] = PWM_DEFAULT_MIN; _max_pwm[i] = PWM_DEFAULT_MAX; - _trim_pwm[i] = PWM_DEFAULT_TRIM; } _control_topics[0] = ORB_ID(actuator_controls_0); @@ -2157,12 +2155,8 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) case PWM_SERVO_GET_TRIM_PWM: { struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - for (unsigned i = 0; i < _max_actuators; i++) { - pwm->values[i] = _trim_pwm[i]; - } + pwm->channel_count = _mixers->get_trims((int16_t *)pwm->values); - pwm->channel_count = _max_actuators; - arg = (unsigned long)&pwm; break; }