pwm_out, dshot: add dynamic mixing support

This commit is contained in:
Beat Küng 2021-09-17 11:56:42 +02:00 committed by Daniel Agar
parent 3ff6014a3c
commit c1e5e666f0
6 changed files with 305 additions and 68 deletions

View File

@ -96,10 +96,16 @@ then
if [ $OUTPUT_MODE = $OUTPUT_CMD -o $OUTPUT_MODE = io ]
then
if ! $OUTPUT_CMD start
if param compare SYS_CTRL_ALLOC 1
then
echo "$OUTPUT_CMD start failed"
tune_control play error
pwm_out start
dshot start
else
if ! $OUTPUT_CMD start
then
echo "$OUTPUT_CMD start failed"
tune_control play error
fi
fi
fi
fi
@ -214,10 +220,10 @@ fi
if [ $OUTPUT_MODE = pwm_out -o $OUTPUT_MODE = io ]
then
if [ $PWM_OUT != none ]
if [ $PWM_OUT != none -a $PWM_MAIN_RATE != none ]
then
# Set PWM output frequency.
if [ $PWM_MAIN_RATE != none ]
if ! param compare SYS_CTRL_ALLOC 1
then
pwm rate -c ${PWM_OUT} -r ${PWM_MAIN_RATE}
fi

View File

@ -33,6 +33,8 @@
#include "DShot.h"
#include <px4_arch/io_timer.h>
char DShot::_telemetry_device[] {};
px4::atomic_bool DShot::_request_telemetry_init{false};
@ -43,7 +45,7 @@ DShot::DShot() :
_mixing_output.setAllDisarmedValues(DSHOT_DISARM_VALUE);
_mixing_output.setAllMinValues(DSHOT_MIN_THROTTLE);
_mixing_output.setAllMaxValues(DSHOT_MAX_THROTTLE);
_mixing_output.setAllFailsafeValues(UINT16_MAX);
}
DShot::~DShot()
@ -114,39 +116,112 @@ int DShot::task_spawn(int argc, char *argv[])
void DShot::enable_dshot_outputs(const bool enabled)
{
if (enabled && !_outputs_initialized) {
DShotConfig config = (DShotConfig)_param_dshot_config.get();
if (_mixing_output.useDynamicMixing()) {
unsigned int dshot_frequency = DSHOT600;
unsigned int dshot_frequency = 0;
uint32_t dshot_frequency_param = 0;
switch (config) {
case DShotConfig::DShot150:
dshot_frequency = DSHOT150;
break;
for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) {
uint32_t channels = io_timer_get_group(timer);
case DShotConfig::DShot300:
dshot_frequency = DSHOT300;
break;
if (channels == 0) {
continue;
}
case DShotConfig::DShot600:
dshot_frequency = DSHOT600;
break;
char param_name[17];
snprintf(param_name, sizeof(param_name), "%s_TIM%u", _mixing_output.paramPrefix(), timer);
case DShotConfig::DShot1200:
dshot_frequency = DSHOT1200;
break;
int32_t tim_config = 0;
param_t handle = param_find(param_name);
param_get(handle, &tim_config);
unsigned int dshot_frequency_request = 0;
default:
break;
if (tim_config == -5) {
dshot_frequency_request = DSHOT150;
} else if (tim_config == -4) {
dshot_frequency_request = DSHOT300;
} else if (tim_config == -3) {
dshot_frequency_request = DSHOT600;
} else if (tim_config == -2) {
dshot_frequency_request = DSHOT1200;
} else {
_output_mask &= ~channels; // don't use for dshot
}
if (dshot_frequency_request != 0) {
if (dshot_frequency != 0 && dshot_frequency != dshot_frequency_request) {
PX4_WARN("Only supporting a single frequency, adjusting param %s", param_name);
param_set_no_notification(handle, &dshot_frequency_param);
} else {
dshot_frequency = dshot_frequency_request;
dshot_frequency_param = tim_config;
}
}
}
int ret = up_dshot_init(_output_mask, dshot_frequency);
if (ret < 0) {
PX4_ERR("up_dshot_init failed (%i)", ret);
return;
}
_output_mask = ret;
// disable unused functions
for (unsigned i = 0; i < _num_outputs; ++i) {
if (((1 << i) & _output_mask) == 0) {
_mixing_output.disableFunction(i);
}
}
if (_output_mask == 0) {
// exit the module if no outputs used
request_stop();
return;
}
} else {
DShotConfig config = (DShotConfig)_param_dshot_config.get();
unsigned int dshot_frequency = DSHOT600;
switch (config) {
case DShotConfig::DShot150:
dshot_frequency = DSHOT150;
break;
case DShotConfig::DShot300:
dshot_frequency = DSHOT300;
break;
case DShotConfig::DShot600:
dshot_frequency = DSHOT600;
break;
case DShotConfig::DShot1200:
dshot_frequency = DSHOT1200;
break;
default:
break;
}
int ret = up_dshot_init(_output_mask, dshot_frequency);
if (ret < 0) {
PX4_ERR("up_dshot_init failed (%i)", ret);
return;
}
_output_mask = ret;
}
int ret = up_dshot_init(_output_mask, dshot_frequency);
if (ret < 0) {
PX4_ERR("up_dshot_init failed (%i)", ret);
return;
}
_output_mask = ret;
_outputs_initialized = true;
}
@ -414,7 +489,7 @@ void DShot::Run()
_mixing_output.update();
// update output status if armed or if mixer is loaded
bool outputs_on = _mixing_output.armed().armed || _mixing_output.mixers();
bool outputs_on = _mixing_output.armed().armed || _mixing_output.initialized();
if (_outputs_on != outputs_on) {
enable_dshot_outputs(outputs_on);

View File

@ -37,6 +37,8 @@ px4_add_module(
SRCS
PWMOut.cpp
PWMOut.hpp
MODULE_CONFIG
module.yaml
DEPENDS
arch_io_pins
mixer

View File

@ -56,8 +56,10 @@ 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"))
{
_mixing_output.setAllMinValues(PWM_DEFAULT_MIN);
_mixing_output.setAllMaxValues(PWM_DEFAULT_MAX);
if (!_mixing_output.useDynamicMixing()) {
_mixing_output.setAllMinValues(PWM_DEFAULT_MIN);
_mixing_output.setAllMaxValues(PWM_DEFAULT_MAX);
}
}
PWMOut::~PWMOut()
@ -92,7 +94,13 @@ int PWMOut::init()
_mixing_output.setDriverInstance(_class_instance);
_num_outputs = math::min(FMU_MAX_ACTUATORS - (int)_output_base, MAX_PER_INSTANCE);
if (_mixing_output.useDynamicMixing()) {
_num_outputs = FMU_MAX_ACTUATORS;
} else {
_num_outputs = math::min(FMU_MAX_ACTUATORS - (int)_output_base, MAX_PER_INSTANCE);
}
_pwm_mask = ((1u << _num_outputs) - 1) << _output_base;
_mixing_output.setMaxNumOutputs(_num_outputs);
@ -132,6 +140,10 @@ int PWMOut::init()
*/
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++) {
@ -264,6 +276,11 @@ int PWMOut::task_spawn(int argc, char *argv[])
return PX4_ERROR;
}
// only start one instance with dynamic mixing
if (dev->_mixing_output.useDynamicMixing()) {
break;
}
} else {
PX4_ERR("alloc failed");
}
@ -283,44 +300,110 @@ bool PWMOut::update_pwm_out_state(bool on)
{
if (on && !_pwm_initialized && _pwm_mask != 0) {
// 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;
if (_mixing_output.useDynamicMixing()) {
for (int i = 0; i < PWM_OUT_MAX_INSTANCES; i++) {
if (_objects[i].load()) {
for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) {
_timer_rates[timer] = -1;
pwm_mask_new |= _objects[i].load()->get_pwm_mask();
pwm_alt_rate_channels_new |= _objects[i].load()->get_alt_rate_channels();
}
}
uint32_t channels = io_timer_get_group(timer);
// 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 (channels == 0) {
continue;
}
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);
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
}
}
// 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());
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;
_all_instances_ready.fetch_add(1);
// disable unused functions
for (unsigned i = 0; i < _num_outputs; ++i) {
if (((1 << i) & _pwm_mask) == 0) {
_mixing_output.disableFunction(i);
}
}
} else {
PX4_ERR("up_pwm_servo_init failed (%i)", ret);
// 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;
_all_instances_ready.fetch_add(1);
} else {
PX4_ERR("up_pwm_servo_init failed (%i)", ret);
}
}
}
up_pwm_servo_arm(on, _pwm_mask);
return _all_instances_ready.load() == PWM_OUT_MAX_INSTANCES;
return _all_instances_ready.load() == PWM_OUT_MAX_INSTANCES || _mixing_output.useDynamicMixing();
}
bool PWMOut::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
@ -332,7 +415,12 @@ bool PWMOut::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
/* output to the servos */
if (_pwm_initialized) {
for (size_t i = 0; i < math::min(_num_outputs, num_outputs); i++) {
for (size_t i = 0; i < num_outputs; i++) {
if (!_mixing_output.isFunctionSet(i)) {
// do not run any signal on disabled channels
outputs[i] = 0;
}
if (_pwm_mask & (1 << (i + _output_base))) {
up_pwm_servo_set(_output_base + i, outputs[i]);
}
@ -362,13 +450,16 @@ void PWMOut::Run()
perf_begin(_cycle_perf);
perf_count(_interval_perf);
// push backup schedule
ScheduleDelayed(_backup_schedule_interval_us);
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.armed().in_esc_calibration_mode;
bool pwm_on = _mixing_output.armed().armed || (_num_disarmed_set > 0) || _mixing_output.useDynamicMixing()
|| _mixing_output.armed().in_esc_calibration_mode;
if (_pwm_on != pwm_on) {
@ -384,10 +475,12 @@ void PWMOut::Run()
_parameter_update_sub.copy(&pupdate);
// update parameters from storage
// update_params(); // do not update PWM params for now (was interfering with VTOL PWM settings)
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) {
if (_pwm_initialized && _current_update_rate == 0 && !_mixing_output.useDynamicMixing()) {
update_current_rate();
}
@ -401,6 +494,10 @@ void PWMOut::update_params()
{
updateParams();
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;
@ -636,7 +733,7 @@ int PWMOut::pwm_ioctl(file *filp, int cmd, unsigned long arg)
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
/* discard if too many values are sent */
if (pwm->channel_count > FMU_MAX_ACTUATORS) {
if (pwm->channel_count > FMU_MAX_ACTUATORS || _mixing_output.useDynamicMixing()) {
ret = -EINVAL;
break;
}
@ -681,7 +778,7 @@ int PWMOut::pwm_ioctl(file *filp, int cmd, unsigned long arg)
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
/* discard if too many values are sent */
if (pwm->channel_count > FMU_MAX_ACTUATORS) {
if (pwm->channel_count > FMU_MAX_ACTUATORS || _mixing_output.useDynamicMixing()) {
ret = -EINVAL;
break;
}
@ -736,7 +833,7 @@ int PWMOut::pwm_ioctl(file *filp, int cmd, unsigned long arg)
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
/* discard if too many values are sent */
if (pwm->channel_count > FMU_MAX_ACTUATORS) {
if (pwm->channel_count > FMU_MAX_ACTUATORS || _mixing_output.useDynamicMixing()) {
ret = -EINVAL;
break;
}
@ -781,7 +878,7 @@ int PWMOut::pwm_ioctl(file *filp, int cmd, unsigned long arg)
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
/* discard if too many values are sent */
if (pwm->channel_count > FMU_MAX_ACTUATORS) {
if (pwm->channel_count > FMU_MAX_ACTUATORS || _mixing_output.useDynamicMixing()) {
ret = -EINVAL;
break;
}
@ -1223,6 +1320,27 @@ int PWMOut::print_status()
perf_print_counter(_interval_perf);
_mixing_output.printStatus();
if (_mixing_output.useDynamicMixing() && _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]);
uint32_t channels = _pwm_mask & up_pwm_servo_get_rate_group(timer);
if (channels > 0) {
PX4_INFO_RAW(" channels: ");
for (uint32_t channel = 0; channel < _num_outputs; ++channel) {
if ((1 << channel) & channels) {
PX4_INFO_RAW("%" PRIu32 " ", channel);
}
}
}
PX4_INFO_RAW("\n");
}
}
}
return 0;
}

View File

@ -47,6 +47,7 @@
#include <lib/mixer_module/mixer_module.hpp>
#include <lib/parameters/param.h>
#include <lib/perf/perf_counter.h>
#include <px4_arch/io_timer.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/log.h>
@ -137,6 +138,8 @@ private:
unsigned _pwm_alt_rate{50};
uint32_t _pwm_alt_rate_channels{0};
int _timer_rates[MAX_IO_TIMERS] {};
int _current_update_rate{0};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};

View File

@ -0,0 +1,33 @@
module_name: PWM Output
actuator_output:
output_groups:
- generator: pwm
param_prefix: PWM_FMU
channel_labels: ['PWM Main', 'PWM Capture']
standard_params:
disarmed: { min: 800, max: 2200, default: 900 }
min: { min: 800, max: 1400, default: 1000 }
max: { min: 1600, max: 2200, default: 2000 }
failsafe: { min: 800, max: 2200 }
extra_function_groups: [ pwm_fmu ]
pwm_timer_param:
description:
short: Output Protocol Configuration for ${label}
long: |
Select which Output Protocol to use for outputs ${label}.
Custom PWM rates can be used by directly setting any value >0.
type: enum
default: 400
values:
-5: DShot150
-4: DShot300
-3: DShot600
-2: DShot1200
-1: OneShot
50: PWM50
100: PWM100
200: PWM200
400: PWM400
reboot_required: true