From dbc149c22494735eef65b271917b95ab0d4884b0 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Wed, 9 Nov 2016 13:45:36 -0700 Subject: [PATCH] change trim units from absolute usec to normalized values --- Tools/px4params/xmlout.py | 2 +- src/drivers/drv_pwm_output.h | 2 +- src/drivers/px4fmu/fmu.cpp | 13 +++---- src/drivers/px4fmu/px4fmu_params.c | 42 ++++++++++++--------- src/modules/px4iofirmware/mixer.cpp | 2 +- src/modules/px4iofirmware/px4io.h | 2 +- src/modules/px4iofirmware/registers.c | 4 +- src/modules/systemlib/mixer/mixer.h | 2 +- src/modules/systemlib/mixer/mixer_group.cpp | 18 ++++----- src/modules/systemlib/pwm_limit/pwm_limit.c | 10 +---- src/modules/systemlib/pwm_limit/pwm_limit.h | 2 +- 11 files changed, 48 insertions(+), 51 deletions(-) diff --git a/Tools/px4params/xmlout.py b/Tools/px4params/xmlout.py index dc188f4940..f10d5f4fd2 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 = "11" + xml_version.text = "12" importtree = ET.parse(inject_xml_file_name) injectgroups = importtree.getroot().findall("group") for igroup in injectgroups: diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index bab063897f..cfc02c8fb6 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -114,7 +114,7 @@ __BEGIN_DECLS /** * Default trim PWM in us */ -#define PWM_DEFAULT_TRIM 1500 +#define PWM_DEFAULT_TRIM 0 /** * Lowest PWM allowed as the maximum PWM diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index b0f629eb11..9da33c6a82 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -775,19 +775,19 @@ PX4FMU::update_pwm_trims() } else { - uint16_t values[_max_actuators] = {}; + int16_t values[_max_actuators] = {}; for (unsigned i = 0; i < _max_actuators; i++) { char pname[16]; - int32_t ival; + float pval; /* fill the struct from parameters */ sprintf(pname, "PWM_AUX_TRIM%d", i + 1); param_t param_h = param_find(pname); if (param_h != PARAM_INVALID) { - param_get(param_h, &ival); - values[i] = ival; + param_get(param_h, &pval); + values[i] = (int16_t)(10000 * pval); PX4_DEBUG("%s: %d", pname, values[i]); } } @@ -1176,9 +1176,8 @@ PX4FMU::cycle() uint16_t pwm_limited[_max_actuators]; /* the PWM limit call takes care of out of band errors, NaN and constrains */ - // TODO: remove trim_pwm parameter pwm_limit_calc(_throttle_armed, arm_nothrottle(), num_outputs, _reverse_pwm_mask, - _disarmed_pwm, _min_pwm, _max_pwm, _trim_pwm, outputs, pwm_limited, &_pwm_limit); + _disarmed_pwm, _min_pwm, _max_pwm, outputs, pwm_limited, &_pwm_limit); /* overwrite outputs in case of lockdown with disarmed PWM values */ @@ -1963,7 +1962,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) } /* copy the trim values to the mixer offsets */ - _mixers->set_trims(pwm->values, pwm->channel_count); + _mixers->set_trims((int16_t *)pwm->values, pwm->channel_count); PX4_DEBUG("set_trims: %d, %d, %d, %d", pwm->values[0], pwm->values[1], pwm->values[2], pwm->values[3]); break; diff --git a/src/drivers/px4fmu/px4fmu_params.c b/src/drivers/px4fmu/px4fmu_params.c index d89feef3f4..19afcd77a3 100644 --- a/src/drivers/px4fmu/px4fmu_params.c +++ b/src/drivers/px4fmu/px4fmu_params.c @@ -113,64 +113,70 @@ PARAM_DEFINE_INT32(PWM_AUX_REV6, 0); * * Set to neutral period in usec * - * @min 1400 - * @max 1600 + * @min -0.2 + * @max 0.2 + * @decimal 2 * @group PWM Outputs */ -PARAM_DEFINE_INT32(PWM_AUX_TRIM1, 1500); +PARAM_DEFINE_FLOAT(PWM_AUX_TRIM1, 0); /** * Trim value for FMU PWM output channel 2 * * Set to neutral period in usec * - * @min 1400 - * @max 1600 + * @min -0.2 + * @max 0.2 + * @decimal 2 * @group PWM Outputs */ -PARAM_DEFINE_INT32(PWM_AUX_TRIM2, 1500); +PARAM_DEFINE_FLOAT(PWM_AUX_TRIM2, 0); /** * Trim value for FMU PWM output channel 3 * * Set to neutral period in usec * - * @min 1400 - * @max 1600 + * @min -0.2 + * @max 0.2 + * @decimal 2 * @group PWM Outputs */ -PARAM_DEFINE_INT32(PWM_AUX_TRIM3, 1500); +PARAM_DEFINE_FLOAT(PWM_AUX_TRIM3, 0); /** * Trim value for FMU PWM output channel 4 * * Set to neutral period in usec * - * @min 1400 - * @max 1600 + * @min -0.2 + * @max 0.2 + * @decimal 2 * @group PWM Outputs */ -PARAM_DEFINE_INT32(PWM_AUX_TRIM4, 1500); +PARAM_DEFINE_FLOAT(PWM_AUX_TRIM4, 0); /** * Trim value for FMU PWM output channel 5 * * Set to neutral period in usec * - * @min 1400 - * @max 1600 + * @min -0.2 + * @max 0.2 + * @decimal 2 * @group PWM Outputs */ -PARAM_DEFINE_INT32(PWM_AUX_TRIM5, 1500); +PARAM_DEFINE_FLOAT(PWM_AUX_TRIM5, 0); /** * Trim value for FMU PWM output channel 6 * * Set to neutral period in usec * - * @min 1400 - * @max 1600 + * @min -0.2 + * @max 0.2 + * @decimal 2 * @group PWM Outputs */ -PARAM_DEFINE_INT32(PWM_AUX_TRIM6, 1500); +PARAM_DEFINE_FLOAT(PWM_AUX_TRIM6, 0); diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index cac73342c4..909ce3d2a0 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -256,7 +256,7 @@ mixer_tick(void) /* the pwm limit call takes care of out of band errors */ pwm_limit_calc(should_arm, should_arm_nothrottle, mixed, r_setup_pwm_reverse, r_page_servo_disarmed, - r_page_servo_control_min, r_page_servo_control_max, r_page_servo_control_trim, outputs, r_page_servos, &pwm_limit); + r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); /* clamp unused outputs to zero */ for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) { diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 1daa9eaa66..a1bb804dd2 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -90,7 +90,7 @@ extern uint16_t r_page_rc_input_config[]; /* PX4IO_PAGE_RC_INPUT_CONFIG */ extern uint16_t r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */ extern uint16_t r_page_servo_control_min[]; /* PX4IO_PAGE_CONTROL_MIN_PWM */ extern uint16_t r_page_servo_control_max[]; /* PX4IO_PAGE_CONTROL_MAX_PWM */ -extern uint16_t r_page_servo_control_trim[]; /* PX4IO_PAGE_CONTROL_TRIM_PWM */ +extern int16_t r_page_servo_control_trim[]; /* PX4IO_PAGE_CONTROL_TRIM_PWM */ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */ /* diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 8b875f731f..22029b91f8 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -260,10 +260,10 @@ uint16_t r_page_servo_control_max[PX4IO_SERVO_COUNT] = { PWM_DEFAULT_MAX, PWM_D /** * PAGE 108 * - * trim PWM values for center position + * trim values for center position * */ -uint16_t r_page_servo_control_trim[PX4IO_SERVO_COUNT] = { PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM }; +int16_t r_page_servo_control_trim[PX4IO_SERVO_COUNT] = { PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM }; /** * PAGE 109 diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h index 6dd277edde..0b433071d0 100644 --- a/src/modules/systemlib/mixer/mixer.h +++ b/src/modules/systemlib/mixer/mixer.h @@ -338,7 +338,7 @@ public: * 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_trims(int16_t *v, unsigned n); unsigned set_trim(float trim) { diff --git a/src/modules/systemlib/mixer/mixer_group.cpp b/src/modules/systemlib/mixer/mixer_group.cpp index 4f6d14246a..252e513899 100644 --- a/src/modules/systemlib/mixer/mixer_group.cpp +++ b/src/modules/systemlib/mixer/mixer_group.cpp @@ -112,21 +112,21 @@ MixerGroup::mix(float *outputs, unsigned space, uint16_t *status_reg) return index; } +/* + * set_trims() has no effect except for the SimpleMixer implementation for which set_trim() + * always returns the value one. + * The only other existing implementation is MultirotorMixer, which ignores the trim value + * and returns _rotor_count. + */ unsigned -MixerGroup::set_trims(uint16_t *values, unsigned n) +MixerGroup::set_trims(int16_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 - * - * This only works with SimpleMixer::set_trim(float) which always returns the value one, - * but the only other existing implementation is MultirotorMixer, which ignores - * the trim value. - */ - float offset = ((float)values[index] - 1500) / 500; + /* 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; } diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.c b/src/modules/systemlib/pwm_limit/pwm_limit.c index a157e29eed..92eead22db 100644 --- a/src/modules/systemlib/pwm_limit/pwm_limit.c +++ b/src/modules/systemlib/pwm_limit/pwm_limit.c @@ -56,7 +56,7 @@ void pwm_limit_init(pwm_limit_t *limit) } void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_channels, const uint16_t reverse_mask, - const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, const uint16_t *trim_pwm, + const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, const float *output, uint16_t *effective_pwm, pwm_limit_t *limit) { @@ -207,16 +207,8 @@ 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; -// } 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]) { effective_pwm[i] = min_pwm[i]; diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.h b/src/modules/systemlib/pwm_limit/pwm_limit.h index fd6629fa43..5347f9b5eb 100644 --- a/src/modules/systemlib/pwm_limit/pwm_limit.h +++ b/src/modules/systemlib/pwm_limit/pwm_limit.h @@ -73,7 +73,7 @@ __EXPORT void pwm_limit_init(pwm_limit_t *limit); __EXPORT void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_channels, const uint16_t reverse_mask, const uint16_t *disarmed_pwm, - const uint16_t *min_pwm, const uint16_t *max_pwm, const uint16_t *trim_pwm, + const uint16_t *min_pwm, const uint16_t *max_pwm, const float *output, uint16_t *effective_pwm, pwm_limit_t *limit); __END_DECLS