fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

fix argument order in pwm_limit_calc call, clean up

Browse Source 
note that FMU does not update AUX pwm outputs if no RC signal
This commit is contained in:
Mark Whitehorn 2016-11-06 11:39:32 -07:00 committed by Lorenz Meier
parent f3c3d1f7f9
commit 009a413438
4 changed files with 30 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/drivers/drv_pwm_output.h
View File

@ -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

26
src/drivers/px4fmu/fmu.cpp
View File

@ -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);
}
}

9
src/modules/systemlib/mixer/mixer_group.cpp
View File

@ -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;
}

2
src/modules/systemlib/pwm_limit/pwm_limit.c
View File

@ -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;