forked from Archive/PX4-Autopilot
pwm_out, dshot: add dynamic mixing support
This commit is contained in:
Beat Küng
Daniel Agar
parent
3ff6014a3c
commit
c1e5e666f0
|
|
@ -96,12 +96,18 @@ then
|
|||
|
||||
if [ $OUTPUT_MODE = $OUTPUT_CMD -o $OUTPUT_MODE = io ]
|
||||
then
|
||||
if param compare SYS_CTRL_ALLOC 1
|
||||
then
|
||||
pwm_out start
|
||||
dshot start
|
||||
else
|
||||
if ! $OUTPUT_CMD start
|
||||
then
|
||||
echo "$OUTPUT_CMD start failed"
|
||||
tune_control play error
|
||||
fi
|
||||
fi
|
||||
fi
|
||||
fi
|
||||
|
||||
if [ $MIXER != none -a $MIXER != skip ]
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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,6 +116,77 @@ int DShot::task_spawn(int argc, char *argv[])
|
|||
void DShot::enable_dshot_outputs(const bool enabled)
|
||||
{
|
||||
if (enabled && !_outputs_initialized) {
|
||||
if (_mixing_output.useDynamicMixing()) {
|
||||
|
||||
unsigned int dshot_frequency = 0;
|
||||
uint32_t dshot_frequency_param = 0;
|
||||
|
||||
for (int timer = 0; timer < MAX_IO_TIMERS; ++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);
|
||||
unsigned int dshot_frequency_request = 0;
|
||||
|
||||
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;
|
||||
|
|
@ -147,6 +220,8 @@ void DShot::enable_dshot_outputs(const bool enabled)
|
|||
}
|
||||
|
||||
_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);
|
||||
|
|
|
|||
|
|
@ -37,6 +37,8 @@ px4_add_module(
|
|||
SRCS
|
||||
PWMOut.cpp
|
||||
PWMOut.hpp
|
||||
MODULE_CONFIG
|
||||
module.yaml
|
||||
DEPENDS
|
||||
arch_io_pins
|
||||
mixer
|
||||
|
|
|
|||
|
|
@ -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"))
|
||||
{
|
||||
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);
|
||||
|
||||
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,6 +300,71 @@ 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;
|
||||
|
||||
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
|
||||
}
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
} else {
|
||||
// Collect all PWM masks from all instances
|
||||
uint32_t pwm_mask_new = 0;
|
||||
// Collect the PWM alt rate channels across all instances
|
||||
|
|
@ -318,9 +400,10 @@ bool PWMOut::update_pwm_out_state(bool on)
|
|||
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);
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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};
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
Loading…
Reference in New Issue