diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 3d4b7cd7f7..bab063897f 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -114,14 +114,14 @@ __BEGIN_DECLS /** * Default trim PWM in us */ -#define PWM_DEFAULT_TRIM 0 +#define PWM_DEFAULT_TRIM 1500 /** * Lowest PWM allowed as the maximum PWM */ #define PWM_LOWEST_MAX 200 -#endif // PX4_PWM_ALTERNATE_RANGES +#endif // not PX4_PWM_ALTERNATE_RANGES /** * Do not output a channel with this value @@ -222,10 +222,10 @@ struct pwm_output_rc_config { /** get the maximum PWM value the output will send */ #define PWM_SERVO_GET_MAX_PWM _PX4_IOC(_PWM_SERVO_BASE, 19) -/** set the maximum PWM value the output will send */ +/** set the TRIM value the output will send */ #define PWM_SERVO_SET_TRIM_PWM _PX4_IOC(_PWM_SERVO_BASE, 20) -/** get the maximum PWM value the output will send */ +/** get the TRIM value the output will send */ #define PWM_SERVO_GET_TRIM_PWM _PX4_IOC(_PWM_SERVO_BASE, 21) /** set the number of servos in (unsigned)arg - allows change of diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index a828051b69..0996b8dfb6 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -644,7 +644,7 @@ PX4FMU::set_mode(Mode mode) int PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate) { - DEVICE_DEBUG("set_pwm_rate %x %u %u", rate_map, default_rate, alt_rate); + PX4_DEBUG("set_pwm_rate %x %u %u", rate_map, default_rate, alt_rate); for (unsigned pass = 0; pass < 2; pass++) { for (unsigned group = 0; group < _max_actuators; group++) { @@ -720,12 +720,12 @@ PX4FMU::subscribe() for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (sub_groups & (1 << i)) { - DEVICE_DEBUG("subscribe to actuator_controls_%d", i); + PX4_DEBUG("subscribe to actuator_controls_%d", i); _control_subs[i] = orb_subscribe(_control_topics[i]); } if (unsub_groups & (1 << i)) { - DEVICE_DEBUG("unsubscribe from actuator_controls_%d", i); + PX4_DEBUG("unsubscribe from actuator_controls_%d", i); ::close(_control_subs[i]); _control_subs[i] = -1; } @@ -761,6 +761,8 @@ PX4FMU::update_pwm_rev_mask() void PX4FMU::update_pwm_trims() { + PX4_DEBUG("update_pwm_trims"); + if (_mixers == nullptr) { PX4_WARN("no mixers defined"); @@ -779,13 +781,13 @@ PX4FMU::update_pwm_trims() if (param_h != PARAM_INVALID) { param_get(param_h, &ival); values[i] = ival; - PX4_INFO("aux trim %d %d", i, values[i]); + PX4_DEBUG("%s: %d", pname, values[i]); } } /* copy the trim values to the mixer offsets */ unsigned n_out = _mixers->set_trims(values, _max_actuators); - PX4_INFO("set %d trims", n_out); + PX4_DEBUG("set %d trims", n_out); } } @@ -1019,7 +1021,7 @@ PX4FMU::cycle() update_rate_in_ms = 100; } - DEVICE_DEBUG("adjusted actuator update interval to %ums", update_rate_in_ms); + PX4_DEBUG("adjusted actuator update interval to %ums", update_rate_in_ms); for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { @@ -1159,7 +1161,7 @@ PX4FMU::cycle() /* 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, - _trim_pwm, _disarmed_pwm, _min_pwm, _max_pwm, outputs, pwm_limited, &_pwm_limit); + _disarmed_pwm, _min_pwm, _max_pwm, _trim_pwm, outputs, pwm_limited, &_pwm_limit); /* overwrite outputs in case of lockdown with disarmed PWM values */ @@ -1664,7 +1666,7 @@ PX4FMU::ioctl(file *filp, int cmd, unsigned long arg) break; default: - DEVICE_DEBUG("not in a PWM mode"); + PX4_DEBUG("not in a PWM mode"); break; } @@ -1681,6 +1683,8 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) { int ret = OK; + PX4_DEBUG("fmu ioctl cmd: %d, arg: %ld", cmd, arg); + lock(); switch (cmd) { @@ -1931,12 +1935,14 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) /* discard if too many values are sent */ if (pwm->channel_count > _max_actuators) { + PX4_DEBUG("error: too many trim values: %d", pwm->channel_count); ret = -EINVAL; break; } /* copy the trim values to the mixer offsets */ _mixers->set_trims(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; } @@ -2292,7 +2298,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) ret = _mixers->load_from_buf(buf, buflen); if (ret != 0) { - DEVICE_DEBUG("mixer load failed with %d", ret); + PX4_DEBUG("mixer load failed with %d", ret); delete _mixers; _mixers = nullptr; _groups_required = 0; @@ -2301,7 +2307,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) } else { _mixers->groups_required(_groups_required); - PX4_INFO("loaded mixers \n%s\n", buf); + PX4_DEBUG("loaded mixers \n%s\n", buf); } } diff --git a/src/modules/systemlib/mixer/mixer_group.cpp b/src/modules/systemlib/mixer/mixer_group.cpp index 574831bcb3..4f6d14246a 100644 --- a/src/modules/systemlib/mixer/mixer_group.cpp +++ b/src/modules/systemlib/mixer/mixer_group.cpp @@ -119,7 +119,13 @@ MixerGroup::set_trims(uint16_t *values, unsigned n) unsigned index = 0; while ((mixer != nullptr) && (index < n)) { - /* hardwired assumption that PWM output range is [1000, 2000] usec */ + /* + * 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; /* to be safe, clamp offset to range of [-100, 100] usec */ @@ -127,6 +133,7 @@ MixerGroup::set_trims(uint16_t *values, unsigned n) if (offset > 0.2f) { offset = 0.2f; } + debug("set trim: %d, offset: %5.3f", values[index], (double)offset); index += mixer->set_trim(offset); mixer = mixer->_next; } diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.c b/src/modules/systemlib/pwm_limit/pwm_limit.c index e14934155e..a157e29eed 100644 --- a/src/modules/systemlib/pwm_limit/pwm_limit.c +++ b/src/modules/systemlib/pwm_limit/pwm_limit.c @@ -192,6 +192,7 @@ void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_c break; case PWM_LIMIT_STATE_ON: + for (unsigned i = 0; i < num_channels; i++) { float control_value = output[i]; @@ -223,6 +224,7 @@ void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_c } else if (effective_pwm[i] > max_pwm[i]) { effective_pwm[i] = max_pwm[i]; } + } break;