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

delete lib/mixer and mixer_module static mixing

Browse Source
This commit is contained in:
Daniel Agar
2022-09-09 09:14:09 -04:00
parent 0019ffbea6
commit a7bbcd5b04
79 changed files with 219 additions and 6835 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/pwm_out/CMakeLists.txt
-1
View File
@@ -49,6 +49,5 @@ px4_add_module(
module.yaml
DEPENDS
arch_io_pins
mixer
mixer_module
)
src/drivers/pwm_out/PWMOut.cpp
+65 -490
View File
@@ -58,10 +58,6 @@ PWMOut::PWMOut(int instance, uint8_t output_base) :
_cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")),
_interval_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": interval"))
{
if (!_mixing_output.useDynamicMixing()) {
_mixing_output.setAllMinValues(PWM_DEFAULT_MIN);
_mixing_output.setAllMaxValues(PWM_DEFAULT_MAX);
}
}
PWMOut::~PWMOut()
@@ -94,14 +90,7 @@ int PWMOut::init()
PX4_ERR("FAILED registering class device");
}
_mixing_output.setDriverInstance(_class_instance);
if (_mixing_output.useDynamicMixing()) {
_num_outputs = FMU_MAX_ACTUATORS;
} else {
_num_outputs = math::min(FMU_MAX_ACTUATORS - (int)_output_base, MAX_PER_INSTANCE);
}
_num_outputs = FMU_MAX_ACTUATORS;
_pwm_mask = ((1u << _num_outputs) - 1) << _output_base;
_mixing_output.setMaxNumOutputs(_num_outputs);
@@ -114,116 +103,6 @@ int PWMOut::init()
return 0;
}
/* When set_pwm_rate is called from either of the 2 IOCTLs:
*
* PWM_SERVO_SET_UPDATE_RATE - Sets the "alternate" channel's rate to the callers's rate specified
* and the non "alternate" channels to the _pwm_default_rate.
*
* rate_map = _pwm_alt_rate_channels
* default_rate = _pwm_default_rate
* alt_rate = arg of IOCTL (see rates)
*
* PWM_SERVO_SET_SELECT_UPDATE_RATE - The caller's specified rate map selects the "alternate" channels
* to be set to the alt rate. (_pwm_alt_rate)
* All other channels are set to the default rate. (_pwm_default_rate)
*
* rate_map = arg of IOCTL
* default_rate = _pwm_default_rate
* alt_rate = _pwm_alt_rate
* rate_map - A mask of 1's for the channels to be set to the
* alternate rate.
* N.B. All channels is a given group must be set
* to the same rate/mode. (default or alt)
* rates:
* alt_rate, default_rate For PWM is 25 or 400Hz
* For Oneshot there is no rate, 0 is therefore used
* to select Oneshot mode
*/
int PWMOut::set_pwm_rate(unsigned rate_map, unsigned default_rate, unsigned alt_rate)
{
if (_mixing_output.useDynamicMixing()) {
return -EINVAL;
}
PX4_DEBUG("pwm_out%u set_pwm_rate %x %u %u", _instance, rate_map, default_rate, alt_rate);
for (unsigned pass = 0; pass < 2; pass++) {
/* We should note that group is iterated over from 0 to FMU_MAX_ACTUATORS.
* This allows for the ideal worlds situation: 1 channel per group
* configuration.
*
* This is typically not what HW supports. A group represents a timer
* and channels belongs to a timer.
* Therefore all channels in a group are dependent on the timer's
* common settings and can not be independent in terms of count frequency
* (granularity of pulse width) and rate (period of repetition).
*
* To say it another way, all channels in a group must have the same
* rate and mode. (See rates above.)
*/
for (unsigned group = 0; group < FMU_MAX_ACTUATORS; group++) {
// get the channel mask for this rate group
uint32_t mask = _pwm_mask & up_pwm_servo_get_rate_group(group);
if (mask == 0) {
continue;
}
// all channels in the group must be either default or alt-rate
uint32_t alt = rate_map & mask;
if (pass == 0) {
// preflight
if ((alt != 0) && (alt != mask)) {
PX4_WARN("rate group %u mask %" PRIx32 " bad overlap %" PRIx32, group, mask, alt);
// not a legal map, bail
return -EINVAL;
}
} else {
// set it - errors here are unexpected
if (alt != 0) {
if (up_pwm_servo_set_rate_group_update(group, alt_rate) != OK) {
PX4_WARN("rate group set alt failed");
return -EINVAL;
}
} else {
if (up_pwm_servo_set_rate_group_update(group, default_rate) != OK) {
PX4_WARN("rate group set default failed");
return -EINVAL;
}
}
}
}
}
_pwm_alt_rate_channels = rate_map;
_pwm_default_rate = default_rate;
_pwm_alt_rate = alt_rate;
// minimum rate for backup schedule
unsigned backup_schedule_rate_hz = math::min(_pwm_default_rate, _pwm_alt_rate);
if (backup_schedule_rate_hz == 0) {
// OneShot rate is 0
backup_schedule_rate_hz = 50;
}
// constrain reasonably (1 to 50 Hz)
backup_schedule_rate_hz = math::constrain(backup_schedule_rate_hz, 1u, 50u);
_backup_schedule_interval_us = roundf(1e6f / backup_schedule_rate_hz);
_current_update_rate = 0; // force update
return OK;
}
void PWMOut::update_current_rate()
{
/*
@@ -274,7 +153,8 @@ int PWMOut::task_spawn(int argc, char *argv[])
}
// only start one instance with dynamic mixing
if (dev->_mixing_output.useDynamicMixing()) {
//if (dev->_mixing_output.useDynamicMixing()) {
if (true) {
break;
}
@@ -297,110 +177,65 @@ bool PWMOut::update_pwm_out_state(bool on)
{
if (on && !_pwm_initialized && _pwm_mask != 0) {
if (_mixing_output.useDynamicMixing()) {
for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) {
_timer_rates[timer] = -1;
for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) {
_timer_rates[timer] = -1;
uint32_t channels = io_timer_get_group(timer);
uint32_t channels = io_timer_get_group(timer);
if (channels == 0) {
continue;
}
char param_name[17];
snprintf(param_name, sizeof(param_name), "%s_TIM%u", _mixing_output.paramPrefix(), timer);
int32_t tim_config = 0;
param_t handle = param_find(param_name);
param_get(handle, &tim_config);
if (tim_config > 0) {
_timer_rates[timer] = tim_config;
} else if (tim_config == -1) { // OneShot
_timer_rates[timer] = 0;
} else {
_pwm_mask &= ~channels; // don't use for pwm
}
if (channels == 0) {
continue;
}
int ret = up_pwm_servo_init(_pwm_mask);
char param_name[17];
snprintf(param_name, sizeof(param_name), "%s_TIM%u", _mixing_output.paramPrefix(), timer);
if (ret < 0) {
PX4_ERR("up_pwm_servo_init failed (%i)", ret);
return false;
}
int32_t tim_config = 0;
param_t handle = param_find(param_name);
param_get(handle, &tim_config);
_pwm_mask = ret;
if (tim_config > 0) {
_timer_rates[timer] = tim_config;
// set the timer rates
for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) {
uint32_t channels = _pwm_mask & up_pwm_servo_get_rate_group(timer);
if (channels == 0) {
continue;
}
ret = up_pwm_servo_set_rate_group_update(timer, _timer_rates[timer]);
if (ret != 0) {
PX4_ERR("up_pwm_servo_set_rate_group_update failed for timer %i, rate %i (%i)", timer, _timer_rates[timer], ret);
_timer_rates[timer] = -1;
_pwm_mask &= ~channels;
}
}
_pwm_initialized = true;
// disable unused functions
for (unsigned i = 0; i < _num_outputs; ++i) {
if (((1 << i) & _pwm_mask) == 0) {
_mixing_output.disableFunction(i);
}
}
} else {
// Collect all PWM masks from all instances
uint32_t pwm_mask_new = 0;
// Collect the PWM alt rate channels across all instances
uint32_t pwm_alt_rate_channels_new = 0;
for (int i = 0; i < PWM_OUT_MAX_INSTANCES; i++) {
if (_objects[i].load()) {
pwm_mask_new |= _objects[i].load()->get_pwm_mask();
pwm_alt_rate_channels_new |= _objects[i].load()->get_alt_rate_channels();
}
}
// Initialize the PWM output state for all instances
// this is re-done once per instance, but harmless
int ret = up_pwm_servo_init(pwm_mask_new);
if (ret >= 0) {
for (int i = 0; i < PWM_OUT_MAX_INSTANCES; i++) {
if (_objects[i].load()) {
_objects[i].load()->set_pwm_mask(_objects[i].load()->get_pwm_mask() & ret);
}
}
// Set rate is not affecting non-masked channels, so can be called
// individually
set_pwm_rate(get_alt_rate_channels(), get_default_rate(), get_alt_rate());
_pwm_initialized = true;
// Other instances need to call up_pwm_servo_arm again after we initialized
for (int i = 0; i < PWM_OUT_MAX_INSTANCES; i++) {
if (i != _instance) {
_require_arming[i].store(true);
}
}
} else if (tim_config == -1) { // OneShot
_timer_rates[timer] = 0;
} else {
PX4_ERR("up_pwm_servo_init failed (%i)", ret);
_pwm_mask &= ~channels; // don't use for pwm
}
}
int ret = up_pwm_servo_init(_pwm_mask);
if (ret < 0) {
PX4_ERR("up_pwm_servo_init failed (%i)", ret);
return false;
}
_pwm_mask = ret;
// set the timer rates
for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) {
uint32_t channels = _pwm_mask & up_pwm_servo_get_rate_group(timer);
if (channels == 0) {
continue;
}
ret = up_pwm_servo_set_rate_group_update(timer, _timer_rates[timer]);
if (ret != 0) {
PX4_ERR("up_pwm_servo_set_rate_group_update failed for timer %i, rate %i (%i)", timer, _timer_rates[timer], ret);
_timer_rates[timer] = -1;
_pwm_mask &= ~channels;
}
}
_pwm_initialized = true;
// disable unused functions
for (unsigned i = 0; i < _num_outputs; ++i) {
if (((1 << i) & _pwm_mask) == 0) {
_mixing_output.disableFunction(i);
}
}
}
@@ -452,16 +287,10 @@ void PWMOut::Run()
perf_begin(_cycle_perf);
perf_count(_interval_perf);
if (!_mixing_output.useDynamicMixing()) {
// push backup schedule
ScheduleDelayed(_backup_schedule_interval_us);
}
_mixing_output.update();
/* update PWM status if armed or if disarmed PWM values are set */
bool pwm_on = _mixing_output.armed().armed || (_num_disarmed_set > 0) || _mixing_output.useDynamicMixing()
|| _mixing_output.armed().in_esc_calibration_mode;
bool pwm_on = true;
if (_pwm_on != pwm_on || _require_arming[_instance].load()) {
@@ -477,13 +306,7 @@ void PWMOut::Run()
_parameter_update_sub.copy(&pupdate);
// update parameters from storage
if (_mixing_output.useDynamicMixing()) { // do not update PWM params for now (was interfering with VTOL PWM settings)
update_params();
}
}
if (_pwm_initialized && _current_update_rate == 0 && !_mixing_output.useDynamicMixing()) {
update_current_rate();
update_params();
}
// check at end of cycle (updateSubscriptions() can potentially change to a different WorkQueue thread)
@@ -503,7 +326,7 @@ void PWMOut::update_params()
updateParams();
// Automatically set the PWM rate and disarmed value when a channel is first set to a servo
if (_mixing_output.useDynamicMixing() && !_first_param_update) {
if (!_first_param_update) {
for (size_t i = 0; i < _num_outputs; i++) {
if ((previously_set_functions & (1u << i)) == 0 && _mixing_output.functionParamHandle(i) != PARAM_INVALID) {
int32_t output_function;
@@ -544,203 +367,6 @@ void PWMOut::update_params()
}
_first_param_update = false;
if (_mixing_output.useDynamicMixing()) {
return;
}
int32_t pwm_min_default = PWM_DEFAULT_MIN;
int32_t pwm_max_default = PWM_DEFAULT_MAX;
int32_t pwm_disarmed_default = 0;
int32_t pwm_rate_default = 50;
int32_t pwm_default_channels = 0;
const char *prefix;
if (_class_instance == CLASS_DEVICE_PRIMARY) {
prefix = "PWM_MAIN";
param_get(param_find("PWM_MAIN_MIN"), &pwm_min_default);
param_get(param_find("PWM_MAIN_MAX"), &pwm_max_default);
param_get(param_find("PWM_MAIN_DISARM"), &pwm_disarmed_default);
param_get(param_find("PWM_MAIN_RATE"), &pwm_rate_default);
param_get(param_find("PWM_MAIN_OUT"), &pwm_default_channels);
} else if (_class_instance == CLASS_DEVICE_SECONDARY) {
prefix = "PWM_AUX";
param_get(param_find("PWM_AUX_MIN"), &pwm_min_default);
param_get(param_find("PWM_AUX_MAX"), &pwm_max_default);
param_get(param_find("PWM_AUX_DISARM"), &pwm_disarmed_default);
param_get(param_find("PWM_AUX_RATE"), &pwm_rate_default);
param_get(param_find("PWM_AUX_OUT"), &pwm_default_channels);
} else if (_class_instance == CLASS_DEVICE_TERTIARY) {
prefix = "PWM_EXTRA";
param_get(param_find("PWM_EXTRA_MIN"), &pwm_min_default);
param_get(param_find("PWM_EXTRA_MAX"), &pwm_max_default);
param_get(param_find("PWM_EXTRA_DISARM"), &pwm_disarmed_default);
param_get(param_find("PWM_EXTRA_RATE"), &pwm_rate_default);
} else {
PX4_ERR("invalid class instance %d", _class_instance);
return;
}
uint32_t single_ch = 0;
uint32_t pwm_default_channel_mask = 0;
while ((single_ch = pwm_default_channels % 10)) {
pwm_default_channel_mask |= 1 << (single_ch - 1);
pwm_default_channels /= 10;
}
// update the counter
// this is needed to decide if disarmed PWM output should be turned on or not
int num_disarmed_set = 0;
char str[17];
for (unsigned i = 0; i < _num_outputs; i++) {
// PWM_MAIN_MINx
{
sprintf(str, "%s_MIN%u", prefix, i + 1);
int32_t pwm_min = -1;
if (param_get(param_find(str), &pwm_min) == PX4_OK) {
if (pwm_min >= 0 && pwm_min != 1000) {
_mixing_output.minValue(i) = math::constrain(pwm_min, (int32_t) PWM_LOWEST_MIN, (int32_t) PWM_HIGHEST_MIN);
if (pwm_min != _mixing_output.minValue(i)) {
int32_t pwm_min_new = _mixing_output.minValue(i);
param_set(param_find(str), &pwm_min_new);
}
} else if (pwm_default_channel_mask & 1 << i) {
_mixing_output.minValue(i) = pwm_min_default;
}
} else {
PX4_ERR("param %s not found", str);
}
}
// PWM_MAIN_MAXx
{
sprintf(str, "%s_MAX%u", prefix, i + 1);
int32_t pwm_max = -1;
if (param_get(param_find(str), &pwm_max) == PX4_OK) {
if (pwm_max >= 0 && pwm_max != 2000) {
_mixing_output.maxValue(i) = math::constrain(pwm_max, (int32_t) PWM_LOWEST_MAX, (int32_t) PWM_HIGHEST_MAX);
if (pwm_max != _mixing_output.maxValue(i)) {
int32_t pwm_max_new = _mixing_output.maxValue(i);
param_set(param_find(str), &pwm_max_new);
}
} else if (pwm_default_channel_mask & 1 << i) {
_mixing_output.maxValue(i) = pwm_max_default;
}
} else {
PX4_ERR("param %s not found", str);
}
}
// PWM_MAIN_DISx
{
sprintf(str, "%s_DIS%u", prefix, i + 1);
int32_t pwm_dis = -1;
if (param_get(param_find(str), &pwm_dis) == PX4_OK) {
if (pwm_dis >= 0 && pwm_dis != 900) {
_mixing_output.disarmedValue(i) = math::constrain(pwm_dis, (int32_t) 0, (int32_t) PWM_HIGHEST_MAX);
if (pwm_dis != _mixing_output.disarmedValue(i)) {
int32_t pwm_dis_new = _mixing_output.disarmedValue(i);
param_set(param_find(str), &pwm_dis_new);
}
} else if (pwm_default_channel_mask & 1 << i) {
_mixing_output.disarmedValue(i) = pwm_disarmed_default;
}
} else {
PX4_ERR("param %s not found", str);
}
if (_mixing_output.disarmedValue(i) > 0) {
num_disarmed_set++;
}
}
// PWM_MAIN_FAILx
{
sprintf(str, "%s_FAIL%u", prefix, i + 1);
int32_t pwm_failsafe = -1;
if (param_get(param_find(str), &pwm_failsafe) == PX4_OK) {
if (pwm_failsafe >= 0) {
_mixing_output.failsafeValue(i) = math::constrain(pwm_failsafe, (int32_t) 0, (int32_t) PWM_HIGHEST_MAX);
if (pwm_failsafe != _mixing_output.failsafeValue(i)) {
int32_t pwm_fail_new = _mixing_output.failsafeValue(i);
param_set(param_find(str), &pwm_fail_new);
}
} else {
// if no channel specific failsafe value is configured, use the disarmed value
_mixing_output.failsafeValue(i) = _mixing_output.disarmedValue(i);
}
} else {
PX4_ERR("param %s not found", str);
}
}
// PWM_MAIN_REVx
{
sprintf(str, "%s_REV%u", prefix, i + 1);
int32_t pwm_rev = 0;
if (param_get(param_find(str), &pwm_rev) == PX4_OK) {
uint16_t &reverse_pwm_mask = _mixing_output.reverseOutputMask();
if (pwm_rev >= 1) {
reverse_pwm_mask = reverse_pwm_mask | (1 << i);
} else {
reverse_pwm_mask = reverse_pwm_mask & ~(1 << i);
}
} else {
PX4_ERR("param %s not found", str);
}
}
}
if (_mixing_output.mixers()) {
int16_t values[FMU_MAX_ACTUATORS] {};
for (unsigned i = 0; i < _num_outputs; i++) {
sprintf(str, "%s_TRIM%u", prefix, i + 1);
float pval = 0.0f;
if (param_get(param_find(str), &pval) != PX4_OK) {
PX4_ERR("param %s not found", str);
}
values[i] = roundf(10000 * pval);
}
// copy the trim values to the mixer offsets
_mixing_output.mixers()->set_trims(values, _num_outputs);
}
_num_disarmed_set = num_disarmed_set;
}
int PWMOut::ioctl(device::file_t *filp, int cmd, unsigned long arg)
@@ -786,7 +412,7 @@ int PWMOut::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg)
break;
case PWM_SERVO_SET_UPDATE_RATE:
ret = set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, arg);
ret = -EINVAL;
break;
case PWM_SERVO_GET_UPDATE_RATE:
@@ -794,7 +420,7 @@ int PWMOut::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg)
break;
case PWM_SERVO_SET_SELECT_UPDATE_RATE:
ret = set_pwm_rate(arg, _pwm_default_rate, _pwm_alt_rate);
ret = -EINVAL;
break;
case PWM_SERVO_GET_SELECT_UPDATE_RATE:
@@ -823,38 +449,9 @@ int PWMOut::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg)
break;
}
case PWM_SERVO_SET_MIN_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
/* discard if too many values are sent */
if (pwm->channel_count > FMU_MAX_ACTUATORS || _mixing_output.useDynamicMixing()) {
ret = -EINVAL;
break;
}
for (unsigned i = 0; i < pwm->channel_count; i++) {
if (pwm->values[i] == 0) {
/* ignore 0 */
} else if (pwm->values[i] > PWM_HIGHEST_MIN) {
_mixing_output.minValue(i) = PWM_HIGHEST_MIN;
}
#if PWM_LOWEST_MIN > 0
else if (pwm->values[i] < PWM_LOWEST_MIN) {
_mixing_output.minValue(i) = PWM_LOWEST_MIN;
}
#endif
else {
_mixing_output.minValue(i) = pwm->values[i];
}
}
break;
}
case PWM_SERVO_SET_MIN_PWM:
ret = -EINVAL;
break;
case PWM_SERVO_GET_MIN_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
@@ -868,31 +465,9 @@ int PWMOut::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg)
break;
}
case PWM_SERVO_SET_MAX_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
/* discard if too many values are sent */
if (pwm->channel_count > FMU_MAX_ACTUATORS || _mixing_output.useDynamicMixing()) {
ret = -EINVAL;
break;
}
for (unsigned i = 0; i < pwm->channel_count; i++) {
if (pwm->values[i] == 0) {
/* ignore 0 */
} else if (pwm->values[i] < PWM_LOWEST_MAX) {
_mixing_output.maxValue(i) = PWM_LOWEST_MAX;
} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
_mixing_output.maxValue(i) = PWM_HIGHEST_MAX;
} else {
_mixing_output.maxValue(i) = pwm->values[i];
}
}
break;
}
case PWM_SERVO_SET_MAX_PWM:
ret = -EINVAL;
break;
case PWM_SERVO_GET_MAX_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
@@ -997,7 +572,7 @@ int PWMOut::print_status()
perf_print_counter(_interval_perf);
_mixing_output.printStatus();
if (_mixing_output.useDynamicMixing() && _pwm_initialized) {
if (_pwm_initialized) {
for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) {
if (_timer_rates[timer] >= 0) {
PX4_INFO_RAW("Timer %i: rate: %3i", timer, _timer_rates[timer]);
src/drivers/pwm_out/PWMOut.hpp
-2
View File
@@ -128,8 +128,6 @@ private:
MixingOutput _mixing_output {PARAM_PREFIX, FMU_MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, true};
uint32_t _backup_schedule_interval_us{1_s};
unsigned _pwm_default_rate{50};
unsigned _pwm_alt_rate{50};
uint32_t _pwm_alt_rate_channels{0};