From a0c8a78a14d38be35af6dd9ae5955a4a16b91427 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Wed, 19 Oct 2016 22:30:08 -0600 Subject: [PATCH] use trim values to set mixer:scaler.offset clamp mixer output offset to [-.2,.2] ([-2000, 2000] in mixer file) add 8 main PWM trim parameters add long desc to parameters and bump minor parameter version --- Tools/px4params/xmlout.py | 2 +- src/drivers/px4io/px4io.cpp | 24 ++++++ src/drivers/px4io/px4io_params.c | 88 ++++++++++++++++++++ src/modules/px4iofirmware/mixer.cpp | 7 ++ src/modules/systemlib/mixer/mixer.h | 25 ++++++ src/modules/systemlib/mixer/mixer_group.cpp | 22 +++++ src/modules/systemlib/mixer/mixer_simple.cpp | 6 ++ src/modules/systemlib/pwm_limit/pwm_limit.c | 16 ++-- 8 files changed, 181 insertions(+), 9 deletions(-) diff --git a/Tools/px4params/xmlout.py b/Tools/px4params/xmlout.py index fbee44f405..7bea1bf727 100644 --- a/Tools/px4params/xmlout.py +++ b/Tools/px4params/xmlout.py @@ -25,7 +25,7 @@ class XMLOutput(): xml_version = ET.SubElement(xml_parameters, "parameter_version_major") xml_version.text = "1" xml_version = ET.SubElement(xml_parameters, "parameter_version_minor") - xml_version.text = "9" + xml_version.text = "10" importtree = ET.parse(inject_xml_file_name) injectgroups = importtree.getroot().findall("group") for igroup in injectgroups: diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index f937bc5a2b..df00611e5b 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1204,6 +1204,30 @@ PX4IO::task_main() (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_REVERSE, pwm_invert_mask); + // update trim values + struct pwm_output_values pwm_values; + + memset(&pwm_values, 0, sizeof(pwm_values)); + ret = io_reg_get(PX4IO_PAGE_CONTROL_TRIM_PWM, 0, (uint16_t *)pwm_values.values, _max_actuators); + + for (unsigned i = 0; i < _max_actuators; i++) { + char pname[16]; + int32_t ival; + + /* fetch the trim values from parameters */ + sprintf(pname, "PWM_MAIN_TRIM%u", i + 1); + param_t param_h = param_find(pname); + + if (param_h != PARAM_INVALID) { + + param_get(param_h, &ival); + pwm_values.values[i] = ival; + } + } + + /* copy values to registers in IO */ + ret = io_reg_set(PX4IO_PAGE_CONTROL_TRIM_PWM, 0, pwm_values.values, _max_actuators); + float param_val; param_t parm_handle; diff --git a/src/drivers/px4io/px4io_params.c b/src/drivers/px4io/px4io_params.c index 2be6c5efc3..85f16c0a2a 100644 --- a/src/drivers/px4io/px4io_params.c +++ b/src/drivers/px4io/px4io_params.c @@ -130,6 +130,94 @@ PARAM_DEFINE_INT32(PWM_MAIN_REV7, 0); */ PARAM_DEFINE_INT32(PWM_MAIN_REV8, 0); +/** + * Trim value for main output channel 1 + * + * Set to neutral period in usec + * + * @min 1400 + * @max 1600 + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_MAIN_TRIM1, 1500); + +/** + * Trim value for main output channel 2 + * + * Set to neutral period in usec + * + * @min 1400 + * @max 1600 + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_MAIN_TRIM2, 1500); + +/** + * Trim value for main output channel 3 + * + * Set to neutral period in usec + * + * @min 1400 + * @max 1600 + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_MAIN_TRIM3, 1500); + +/** + * Trim value for main output channel 4 + * + * Set to neutral period in usec + * + * @min 1400 + * @max 1600 + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_MAIN_TRIM4, 1500); + +/** + * Trim value for main output channel 5 + * + * Set to neutral period in usec + * + * @min 1400 + * @max 1600 + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_MAIN_TRIM5, 1500); + +/** + * Trim value for main output channel 6 + * + * Set to neutral period in usec + * + * @min 1400 + * @max 1600 + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_MAIN_TRIM6, 1500); + +/** + * Trim value for main output channel 7 + * + * Set to neutral period in usec + * + * @min 1400 + * @max 1600 + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_MAIN_TRIM7, 1500); + +/** + * Trim value for main output channel 8 + * + * Set to neutral period in usec + * + * @min 1400 + * @max 1600 + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_MAIN_TRIM8, 1500); + /** * S.BUS out * diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 52a05030a2..cac73342c4 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -212,6 +212,13 @@ mixer_tick(void) r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FAILSAFE); } + /* + * Set simple mixer trim values + * (there should be a "dirty" flag to indicate that r_page_servo_control_trim has changed) + */ + mixer_group.set_trims(r_page_servo_control_trim, PX4IO_SERVO_COUNT); + + /* * Run the mixers. */ diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h index b6ba9cf722..6dd277edde 100644 --- a/src/modules/systemlib/mixer/mixer.h +++ b/src/modules/systemlib/mixer/mixer.h @@ -192,6 +192,7 @@ public: */ virtual void set_max_delta_out_once(float delta_out_max) {}; + virtual unsigned set_trim(float trim) = 0; protected: /** client-supplied callback used when fetching control values */ @@ -333,6 +334,17 @@ public: */ virtual void set_max_delta_out_once(float delta_out_max); + /* + * Invoke the set_offset method of each mixer in the group + * for each value in page r_page_servo_control_trim + */ + unsigned set_trims(uint16_t *v, unsigned n); + + unsigned set_trim(float trim) + { + return 0; + } + private: Mixer *_first; /**< linked list of mixers */ @@ -369,6 +381,12 @@ public: virtual unsigned mix(float *outputs, unsigned space, uint16_t *status_reg); virtual void groups_required(uint32_t &groups); + virtual void set_offset(float trim) {}; + unsigned set_trim(float trim) + { + return 0; + } + }; /** @@ -445,6 +463,8 @@ public: */ int check(); + unsigned set_trim(float trim); + protected: private: @@ -551,6 +571,11 @@ public: */ virtual void set_max_delta_out_once(float delta_out_max) {_delta_out_max = delta_out_max;} + unsigned set_trim(float trim) + { + return _rotor_count; + } + private: float _roll_scale; float _pitch_scale; diff --git a/src/modules/systemlib/mixer/mixer_group.cpp b/src/modules/systemlib/mixer/mixer_group.cpp index 1da681df28..574831bcb3 100644 --- a/src/modules/systemlib/mixer/mixer_group.cpp +++ b/src/modules/systemlib/mixer/mixer_group.cpp @@ -112,6 +112,28 @@ MixerGroup::mix(float *outputs, unsigned space, uint16_t *status_reg) return index; } +unsigned +MixerGroup::set_trims(uint16_t *values, unsigned n) +{ + Mixer *mixer = _first; + unsigned index = 0; + + while ((mixer != nullptr) && (index < n)) { + /* hardwired assumption that PWM output range is [1000, 2000] usec */ + float offset = ((float)values[index] - 1500) / 500; + + /* to be safe, clamp offset to range of [-100, 100] usec */ + if (offset < -0.2f) { offset = -0.2f; } + + if (offset > 0.2f) { offset = 0.2f; } + + index += mixer->set_trim(offset); + mixer = mixer->_next; + } + + return index; +} + unsigned MixerGroup::count() { diff --git a/src/modules/systemlib/mixer/mixer_simple.cpp b/src/modules/systemlib/mixer/mixer_simple.cpp index 31108c1a67..13015d1f2c 100644 --- a/src/modules/systemlib/mixer/mixer_simple.cpp +++ b/src/modules/systemlib/mixer/mixer_simple.cpp @@ -72,6 +72,12 @@ SimpleMixer::~SimpleMixer() } } +unsigned SimpleMixer::set_trim(float trim) +{ + _pinfo->output_scaler.offset = trim; + return 1; +} + int SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler) { diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.c b/src/modules/systemlib/pwm_limit/pwm_limit.c index f4d7087263..e14934155e 100644 --- a/src/modules/systemlib/pwm_limit/pwm_limit.c +++ b/src/modules/systemlib/pwm_limit/pwm_limit.c @@ -206,15 +206,15 @@ void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_c control_value = -1.0f * control_value; } - if (trim_pwm[i] == 0) { - effective_pwm[i] = control_value * (max_pwm[i] - min_pwm[i]) / 2 + (max_pwm[i] + min_pwm[i]) / 2; +// if (trim_pwm[i] == 0) { + effective_pwm[i] = control_value * (max_pwm[i] - min_pwm[i]) / 2 + (max_pwm[i] + min_pwm[i]) / 2; - } else if (control_value < 0) { - effective_pwm[i] = control_value * (trim_pwm[i] - min_pwm[i]) + trim_pwm[i]; - - } else { - effective_pwm[i] = control_value * (max_pwm[i] - trim_pwm[i]) + trim_pwm[i]; - } +// } else if (control_value < 0) { +// effective_pwm[i] = control_value * (trim_pwm[i] - min_pwm[i]) + trim_pwm[i]; +// +// } else { +// effective_pwm[i] = control_value * (max_pwm[i] - trim_pwm[i]) + trim_pwm[i]; +// } /* last line of defense against invalid inputs */ if (effective_pwm[i] < min_pwm[i]) {