diff --git a/src/lib/mixer/mixer.h b/src/lib/mixer/mixer.h index 3ccf0ba867..989a59e43f 100644 --- a/src/lib/mixer/mixer.h +++ b/src/lib/mixer/mixer.h @@ -204,6 +204,13 @@ public: */ 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 @@ public: 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 @@ public: return 0; } + unsigned get_trim(float *trim) + { + return 0; + } + }; /** @@ -507,6 +526,8 @@ public: unsigned set_trim(float trim); + unsigned get_trim(float *trim); + protected: private: @@ -617,6 +638,11 @@ public: 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 @@ public: 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 13f38afbed..e4fb98fbff 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 92e4bbb812..a05b56f51b 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) {