dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

px4io: add dynamic mixing support

This commit is contained in:
Beat Küng 2021-10-05 13:36:28 +02:00 committed by Daniel Agar
parent 837a1066e0
commit f1686b1abf
9 changed files with 206 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Tools/module_config/generate_params.py
View File

@ -186,6 +186,8 @@ def get_actuator_output_params(yaml_config, output_functions,
standard_params = group.get('standard_params', [])
extra_function_groups = group.get('extra_function_groups', [])
pwm_timer_param = group.get('pwm_timer_param', None)
if 'timer_config_file' in group:
timer_config_file = os.path.join(root_dir, group['timer_config_file'])
if timer_config_file is None:
raise Exception('trying to generate pwm outputs, but --timer-config not set')
timer_groups = get_timer_groups(timer_config_file, verbose)

2
src/drivers/px4io/CMakeLists.txt
View File

@ -39,6 +39,8 @@ px4_add_module(
px4io.cpp
px4io_serial.cpp
px4io_uploader.cpp
MODULE_CONFIG
module.yaml
DEPENDS
arch_px4io_serial
circuit_breaker

29
src/drivers/px4io/module.yaml Normal file
View File

@ -0,0 +1,29 @@
module_name: IO PWM Output
actuator_output:
output_groups:
- generator: pwm
param_prefix: PWM_MAIN
channel_labels: ['PWM Main', 'PWM Capture']
timer_config_file: "boards/px4/io-v2/src/timer_config.cpp"
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 }
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:
-1: OneShot
50: PWM50
100: PWM100
200: PWM200
400: PWM400
reboot_required: true

116
src/drivers/px4io/px4io.cpp
View File

@ -174,6 +174,7 @@ private:
void updateDisarmed();
void updateFailsafe();
void updateTimerRateGroups();
static int checkcrc(int argc, char *argv[]);
static int bind(int argc, char *argv[]);
@ -218,6 +219,7 @@ private:
hrt_abstime _last_status_publish{0};
bool _param_update_force{true}; ///< force a parameter update
bool _timer_rates_configured{false};
/* advertised topics */
uORB::PublicationMulti<input_rc_s> _to_input_rc{ORB_ID(input_rc)};
@ -364,8 +366,12 @@ PX4IO::PX4IO(device::Device *interface) :
OutputModuleInterface(MODULE_NAME, px4::serial_port_to_wq(PX4IO_SERIAL_DEVICE)),
_interface(interface)
{
_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);
}
_mixing_output.setLowrateSchedulingInterval(20_ms);
}
PX4IO::~PX4IO()
@ -555,7 +561,7 @@ void PX4IO::updateFailsafe()
pwm_output_values pwm{};
for (unsigned i = 0; i < _max_actuators; i++) {
pwm.values[i] = _mixing_output.failsafeValue(i);
pwm.values[i] = _mixing_output.actualFailsafeValue(i);
}
io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, pwm.values, _max_actuators);
@ -575,7 +581,9 @@ void PX4IO::Run()
perf_count(_interval_perf);
// schedule minimal update rate if there are no actuator controls
ScheduleDelayed(20_ms);
if (!_mixing_output.useDynamicMixing()) {
ScheduleDelayed(20_ms);
}
/* if we have new control data from the ORB, handle it */
if (_param_sys_hitl.get() <= 0) {
@ -705,10 +713,60 @@ void PX4IO::Run()
perf_end(_cycle_perf);
}
void PX4IO::updateTimerRateGroups()
{
if (_timer_rates_configured) { // avoid setting timer rates on each param update
return;
}
_timer_rates_configured = true;
int timer = 0;
uint16_t timer_rates[PX4IO_P_SETUP_PWM_RATE_GROUP3 - PX4IO_P_SETUP_PWM_RATE_GROUP0 + 1] {};
for (uint8_t offset = PX4IO_P_SETUP_PWM_RATE_GROUP0; offset <= PX4IO_P_SETUP_PWM_RATE_GROUP3; ++offset) {
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);
if (handle == PARAM_INVALID) {
break;
}
param_get(handle, &tim_config);
if (tim_config > 0) {
timer_rates[timer] = tim_config;
} else if (tim_config == -1) { // OneShot
timer_rates[timer] = 0;
}
++timer;
}
int ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATE_GROUP0, timer_rates, timer);
if (ret != 0) {
PX4_ERR("io_reg_set failed (%i)", ret);
}
}
void PX4IO::update_params()
{
if (!_mixing_output.armed().armed && _mixing_output.useDynamicMixing()) {
// sync params to IO
updateTimerRateGroups();
updateFailsafe();
updateDisarmed();
return;
}
// skip update when armed or PWM disabled
if (_mixing_output.armed().armed || _class_instance == -1) {
if (_mixing_output.armed().armed || _class_instance == -1 || _mixing_output.useDynamicMixing()) {
return;
}
@ -1140,18 +1198,29 @@ int PX4IO::io_get_status()
status.pwm_failsafe[i] = io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i);
}
// PWM rates, 0 = low rate, 1 = high rate
const uint16_t pwm_rate = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES);
if (_mixing_output.useDynamicMixing()) {
const int pwm_low_rate = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE);
const int pwm_high_rate = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE);
int i = 0;
for (unsigned i = 0; i < _max_actuators; i++) {
if (pwm_rate & (1 << i)) {
status.pwm_rate_hz[i] = pwm_high_rate;
for (uint8_t offset = PX4IO_P_SETUP_PWM_RATE_GROUP0; offset <= PX4IO_P_SETUP_PWM_RATE_GROUP3; ++offset) {
// This is a bit different than below, setting the groups, not the channels
status.pwm_rate_hz[i++] = io_reg_get(PX4IO_PAGE_SETUP, offset);
}
} else {
status.pwm_rate_hz[i] = pwm_low_rate;
} else {
// PWM rates, 0 = low rate, 1 = high rate
const uint16_t pwm_rate = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES);
const int pwm_low_rate = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE);
const int pwm_high_rate = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE);
for (unsigned i = 0; i < _max_actuators; i++) {
if (pwm_rate & (1 << i)) {
status.pwm_rate_hz[i] = pwm_high_rate;
} else {
status.pwm_rate_hz[i] = pwm_low_rate;
}
}
}
@ -1532,6 +1601,12 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
case PWM_SERVO_SET_UPDATE_RATE:
PX4_DEBUG("PWM_SERVO_SET_UPDATE_RATE");
if (_mixing_output.useDynamicMixing()) {
ret = -EINVAL;
break;
}
/* set the requested alternate rate */
ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE, arg);
break;
@ -1545,6 +1620,11 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
case PWM_SERVO_SET_SELECT_UPDATE_RATE: {
PX4_DEBUG("PWM_SERVO_SET_SELECT_UPDATE_RATE");
if (_mixing_output.useDynamicMixing()) {
ret = -EINVAL;
break;
}
/* blindly clear the PWM update alarm - might be set for some other reason */
io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, PX4IO_P_STATUS_ALARMS_PWM_ERROR);
@ -1579,7 +1659,7 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
}
for (unsigned i = 0; i < pwm->channel_count; i++) {
if (pwm->values[i] != 0) {
if (pwm->values[i] != 0 && !_mixing_output.useDynamicMixing()) {
_mixing_output.failsafeValue(i) = math::constrain(pwm->values[i], (uint16_t)PWM_LOWEST_MIN, (uint16_t)PWM_HIGHEST_MAX);
}
}
@ -1613,7 +1693,7 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
}
for (unsigned i = 0; i < pwm->channel_count; i++) {
if (pwm->values[i] != 0) {
if (pwm->values[i] != 0 && !_mixing_output.useDynamicMixing()) {
_mixing_output.disarmedValue(i) = math::constrain(pwm->values[i], (uint16_t)PWM_LOWEST_MIN, (uint16_t)PWM_HIGHEST_MAX);
}
}
@ -1645,7 +1725,7 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
}
for (unsigned i = 0; i < pwm->channel_count; i++) {
if (pwm->values[i] != 0) {
if (pwm->values[i] != 0 && !_mixing_output.useDynamicMixing()) {
_mixing_output.minValue(i) = math::constrain(pwm->values[i], (uint16_t)PWM_LOWEST_MIN, (uint16_t)PWM_HIGHEST_MIN);
}
}
@ -1675,7 +1755,7 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
}
for (unsigned i = 0; i < pwm->channel_count; i++) {
if (pwm->values[i] != 0) {
if (pwm->values[i] != 0 && !_mixing_output.useDynamicMixing()) {
_mixing_output.maxValue(i) = math::constrain(pwm->values[i], (uint16_t)PWM_LOWEST_MAX, (uint16_t)PWM_HIGHEST_MAX);
}
}

42
src/lib/mixer_module/mixer_module.cpp
View File

@ -461,7 +461,7 @@ bool MixingOutput::updateSubscriptionsDynamicMixer(bool allow_wq_switch, bool li
}
} else if (all_disabled) {
_interface.ScheduleOnInterval(300_ms);
_interface.ScheduleOnInterval(_lowrate_schedule_interval);
PX4_DEBUG("Scheduling at low rate");
} else {
@ -817,19 +817,7 @@ MixingOutput::limitAndUpdateOutputs(float outputs[MAX_ACTUATORS], bool has_updat
/* overwrite outputs in case of force_failsafe with _failsafe_value values */
if (_armed.force_failsafe) {
for (size_t i = 0; i < _max_num_outputs; i++) {
if (_failsafe_value[i] == UINT16_MAX) { // if set to default, use the one provided by the function
float default_failsafe = NAN;
if (_functions[i]) {
default_failsafe = _functions[i]->defaultFailsafeValue(_function_assignment[i]);
}
_current_output_value[i] = output_limit_calc_single(_reverse_output_mask & (1 << i),
_disarmed_value[i], _min_value[i], _max_value[i], default_failsafe);
} else {
_current_output_value[i] = _failsafe_value[i];
}
_current_output_value[i] = actualFailsafeValue(i);
}
}
@ -908,6 +896,32 @@ MixingOutput::updateLatencyPerfCounter(const actuator_outputs_s &actuator_output
}
}
uint16_t
MixingOutput::actualFailsafeValue(int index)
{
if (!_use_dynamic_mixing) {
return failsafeValue(index);
}
uint16_t value = 0;
if (_failsafe_value[index] == UINT16_MAX) { // if set to default, use the one provided by the function
float default_failsafe = NAN;
if (_functions[index]) {
default_failsafe = _functions[index]->defaultFailsafeValue(_function_assignment[index]);
}
value = output_limit_calc_single(_reverse_output_mask & (1 << index),
_disarmed_value[index], _min_value[index], _max_value[index], default_failsafe);
} else {
value = _failsafe_value[index];
}
return value;
}
void
MixingOutput::reorderOutputs(uint16_t values[MAX_ACTUATORS])
{

10
src/lib/mixer_module/mixer_module.hpp
View File

@ -54,6 +54,8 @@
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/test_motor.h>
using namespace time_literals;
/**
* @class OutputModuleInterface
* Base class for an output module.
@ -187,6 +189,11 @@ public:
uint16_t &minValue(int index) { return _min_value[index]; }
uint16_t &maxValue(int index) { return _max_value[index]; }
/**
* Returns the actual failsafe value taking into account the assigned function
*/
uint16_t actualFailsafeValue(int index);
/**
* Get the motor index that maps from PX4 convention to the configured one
* @param index motor index in [0, num_motors-1]
@ -203,6 +210,8 @@ public:
const char *paramPrefix() const { return _param_prefix; }
void setLowrateSchedulingInterval(hrt_abstime interval) { _lowrate_schedule_interval = interval; }
protected:
void updateParams() override;
@ -330,6 +339,7 @@ private:
false; ///< whether or not the output module supports reversible motors (range [-1, 0] for motors)
const char *const _param_prefix;
ParamHandles _param_handles[MAX_ACTUATORS];
hrt_abstime _lowrate_schedule_interval{300_ms};
uORB::SubscriptionCallbackWorkItem *_subscription_callback{nullptr}; ///< current scheduling callback

4
src/modules/px4iofirmware/protocol.h
View File

@ -193,6 +193,10 @@ enum { /* DSM bind states */
#define PX4IO_P_SETUP_SBUS_RATE 16 /**< frame rate of SBUS1 output in Hz */
#define PX4IO_P_SETUP_THERMAL 17 /**< thermal management */
#define PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION 18 /**< flight termination; false if the circuit breaker (CBRK_FLIGHTTERM) is set */
#define PX4IO_P_SETUP_PWM_RATE_GROUP0 19 /* Configure timer group 0 update rate in Hz */
#define PX4IO_P_SETUP_PWM_RATE_GROUP1 20 /* Configure timer group 1 update rate in Hz */
#define PX4IO_P_SETUP_PWM_RATE_GROUP2 21 /* Configure timer group 2 update rate in Hz */
#define PX4IO_P_SETUP_PWM_RATE_GROUP3 22 /* Configure timer group 3 update rate in Hz */
#define PX4IO_THERMAL_IGNORE UINT16_MAX
#define PX4IO_THERMAL_OFF 0

30
src/modules/px4iofirmware/registers.c
View File

@ -145,7 +145,8 @@ volatile uint16_t r_page_setup[] = {
[PX4IO_P_SETUP_REBOOT_BL] = 0,
[PX4IO_P_SETUP_CRC ...(PX4IO_P_SETUP_CRC + 1)] = 0,
[PX4IO_P_SETUP_THERMAL] = PX4IO_THERMAL_IGNORE,
[PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION] = 0
[PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION] = 0,
[PX4IO_P_SETUP_PWM_RATE_GROUP0 ... PX4IO_P_SETUP_PWM_RATE_GROUP3] = 0
};
#define PX4IO_P_SETUP_FEATURES_VALID (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT | PX4IO_P_SETUP_FEATURES_ADC_RSSI)
@ -465,6 +466,33 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
r_page_setup[offset] = value;
break;
case PX4IO_P_SETUP_PWM_RATE_GROUP0:
case PX4IO_P_SETUP_PWM_RATE_GROUP1:
case PX4IO_P_SETUP_PWM_RATE_GROUP2:
case PX4IO_P_SETUP_PWM_RATE_GROUP3:
/* For PWM constrain to [25,400]Hz
* For Oneshot there is no rate, 0 is therefore used to select Oneshot mode
*/
if (value != 0) {
if (value < 25) {
value = 25;
}
if (value > 400) {
value = 400;
}
}
if (up_pwm_servo_set_rate_group_update(offset - PX4IO_P_SETUP_PWM_RATE_GROUP0, value) == OK) {
r_page_setup[offset] = value;
} else {
r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR;
}
break;
default:
return -1;
}

4
validation/module_schema.yaml
View File

@ -333,6 +333,10 @@ actuator_output:
maxlength: 2
schema:
type: string
timer_config_file:
# Only used for 'pwm' generator, specifies
# board-specific timer_config.cpp file
type: string
pwm_timer_param:
# Only used for 'pwm' generator, per-timer config param
type: dict