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

mixer_module: add param generation for disarmed, min, max, failsafe

This commit is contained in:
Beat Küng 2021-09-14 14:52:34 +02:00 committed by Daniel Agar
parent 6424faccb1
commit 6f01b69f49
3 changed files with 200 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

146
Tools/module_config/generate_params.py
View File

@ -35,6 +35,9 @@ verbose = args.verbose
params_output_file = args.params_file
ethernet_supported = args.ethernet
root_dir = os.path.join(os.path.dirname(os.path.realpath(__file__)),"../..")
output_functions_file = os.path.join(root_dir,"src/lib/mixer_module/output_functions.yaml")
def parse_yaml_parameters_config(yaml_config, ethernet_supported):
""" parse the parameters section from the yaml config file """
if 'parameters' not in yaml_config:
@ -107,19 +110,148 @@ PARAM_DEFINE_{param_type}({name}, {default_value});
).replace('${i}', str(i+instance_start))
return ret
all_params = ""
for yaml_file in args.config_files:
with open(yaml_file, 'r') as stream:
def get_actuator_output_params(yaml_config, output_functions):
""" parse the actuator_output section from the yaml config file
:return: dict of param definitions
"""
if not 'actuator_output' in yaml_config:
return {}
output_groups = yaml_config['actuator_output']['output_groups']
all_params = {}
for group in output_groups:
num_channels = group['num_channels']
param_prefix = group['param_prefix']
channel_label = group['channel_label']
standard_params = group.get('standard_params', {})
if len(param_prefix) > 9: # 16 - len('_FAIL') - 2 (2 digits for index)
raise Exception("param prefix {:} too long (max length=10)".format(param_prefix))
# collect the functions
function_groups = ['common']
function_groups.extend(group.get('extra_function_groups', []))
output_function_values = {}
for function_group in function_groups:
group = output_functions[function_group]
for function_name in group:
if isinstance(group[function_name], int):
output_function_values[group[function_name]] = function_name
else:
start = group[function_name]['start']
count = group[function_name]['count']
for i in range(count):
output_function_values[start+i] = function_name+str(i+1)
# function param
param = {
'description': {
'short': channel_label+' ${i} Output Function',
'long':
'''Select what should be output on {:} ${{i}}.
The default failsafe value is set according to the selected function:
- 'Min' for ConstantMin
- 'Max' for ConstantMax
- 'Max' for Parachute
- ('Max'+'Min')/2 for Servos
- 'Disarmed' for the rest
'''.format(channel_label),
},
'type': 'enum',
'instance_start': 1,
'num_instances': num_channels,
'default': 0,
'values': output_function_values
}
all_params[param_prefix+'_FUNC${i}'] = param
# handle standard_params
disarmed_description = \
'''This is the output value that is set when not armed.
Note that non-motor outputs might already be active in prearm state if COM_PREARM_MODE is set.
'''
minimum_description = \
'''Minimum output value (when not disarmed).
The output range can be reversed by setting Min > Max.
'''
maximum_description = \
'''Maxmimum output value (when not disarmed).
The output range can be reversed by setting Min > Max.
'''
failsafe_description = \
'''This is the output value that is set when in failsafe mode.
When set to -1 (default), the value depends on the function (see {:}).
'''.format(param_prefix+'_FUNC${i}')
standard_params_array = [
( 'disarmed', 'Disarmed', 'DIS', disarmed_description ),
( 'min', 'Minimum', 'MIN', minimum_description ),
( 'max', 'Maximum', 'MAX', maximum_description ),
( 'failsafe', 'Failsafe', 'FAIL', failsafe_description ),
]
for key, label, param_suffix, description in standard_params_array:
if key in standard_params:
# values must be in range of an uint16_t
if standard_params[key]['min'] < 0:
raise Exception('minimum value for {:} expected >= 0 (got {:})'.format(key, standard_params[key]['min']))
if standard_params[key]['max'] >= 1<<16:
raise Exception('maximum value for {:} expected <= {:} (got {:})'.format(key, 1<<16, standard_params[key]['max']))
if key == 'failsafe':
standard_params[key]['default'] = -1
standard_params[key]['min'] = -1
param = {
'description': {
'short': channel_label+' ${i} '+label+' Value',
'long': description
},
'type': 'int32',
'instance_start': 1,
'num_instances': num_channels,
'min': standard_params[key]['min'],
'max': standard_params[key]['max'],
'default': standard_params[key]['default'],
}
all_params[param_prefix+'_'+param_suffix+'${i}'] = param
if verbose: print('adding actuator params: {:}'.format(all_params))
return all_params
def load_yaml_file(file_name):
with open(file_name, 'r') as stream:
try:
yaml_config = yaml.load(stream, Loader=yaml.Loader)
all_params += parse_yaml_parameters_config(yaml_config, ethernet_supported)
return yaml.safe_load(stream)
except yaml.YAMLError as exc:
print(exc)
raise
output_functions_yaml = load_yaml_file(output_functions_file)
output_functions = output_functions_yaml['functions']
all_params = ""
for yaml_file in args.config_files:
yaml_config = load_yaml_file(yaml_file)
# convert 'output_groups' section into additional params
try:
actuator_output_params = get_actuator_output_params(yaml_config, output_functions)
except Exception as e:
print('Exception while parsing {:}:'.format(yaml_file))
raise e
# now add them to the yaml config
if not 'parameters' in yaml_config:
yaml_config['parameters'] = []
group_name = 'Actuator Outputs'
yaml_config['parameters'].append({'group': group_name, 'definitions': actuator_output_params})
all_params += parse_yaml_parameters_config(yaml_config, ethernet_supported)
if verbose: print("Generating {:}".format(params_output_file))
with open(params_output_file, 'w') as fid:
fid.write(all_params)

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

@ -102,6 +102,16 @@ _control_latency_perf(perf_alloc(PC_ELAPSED, "control latency"))
_motor_test.test_motor_sub.subscribe();
_use_dynamic_mixing = _param_sys_ctrl_alloc.get();
if (_use_dynamic_mixing) {
initParamHandles();
for (unsigned i = 0; i < MAX_ACTUATORS; i++) {
_failsafe_value[i] = UINT16_MAX;
}
updateParams();
}
}
MixingOutput::~MixingOutput()
@ -113,8 +123,27 @@ MixingOutput::~MixingOutput()
cleanupFunctions();
}
void MixingOutput::initParamHandles()
{
char param_name[17];
for (unsigned i = 0; i < _max_num_outputs; ++i) {
snprintf(param_name, sizeof(param_name), "%s_%s%d", _param_prefix, "FUNC", i + 1);
_param_handles[i].function = param_find(param_name);
snprintf(param_name, sizeof(param_name), "%s_%s%d", _param_prefix, "DIS", i + 1);
_param_handles[i].disarmed = param_find(param_name);
snprintf(param_name, sizeof(param_name), "%s_%s%d", _param_prefix, "MIN", i + 1);
_param_handles[i].min = param_find(param_name);
snprintf(param_name, sizeof(param_name), "%s_%s%d", _param_prefix, "MAX", i + 1);
_param_handles[i].max = param_find(param_name);
snprintf(param_name, sizeof(param_name), "%s_%s%d", _param_prefix, "FAIL", i + 1);
_param_handles[i].failsafe = param_find(param_name);
}
}
void MixingOutput::printStatus() const
{
PX4_INFO("Param prefix: %s", _param_prefix);
perf_print_counter(_control_latency_perf);
if (_wq_switched) {
@ -143,7 +172,6 @@ void MixingOutput::printStatus() const
_current_output_value[i],
_failsafe_value[reordered_i], _disarmed_value[reordered_i], _min_value[reordered_i], _max_value[reordered_i]);
}
}
}
@ -332,8 +360,13 @@ bool MixingOutput::updateSubscriptionsDynamicMixer(bool allow_wq_switch, bool li
// potentially switch work queue if we run motor outputs
for (unsigned i = 0; i < _max_num_outputs; i++) {
if (_function_assignment[i] >= OutputFunction::Motor1 && _function_assignment[i] <= OutputFunction::MotorMax) {
switch_requested = true;
// read function directly from param, as _function_assignment[i] is updated later
int32_t function;
if (_param_handles[i].function != PARAM_INVALID && param_get(_param_handles[i].function, &function) == 0) {
if (function >= (int32_t)OutputFunction::Motor1 && function <= (int32_t)OutputFunction::MotorMax) {
switch_requested = true;
}
}
}
@ -349,6 +382,7 @@ bool MixingOutput::updateSubscriptionsDynamicMixer(bool allow_wq_switch, bool li
}
// Now update the functions
PX4_DEBUG("updating functions");
cleanupFunctions();
@ -359,6 +393,15 @@ bool MixingOutput::updateSubscriptionsDynamicMixer(bool allow_wq_switch, bool li
bool all_disabled = true;
for (int i = 0; i < _max_num_outputs; ++i) {
int32_t val;
if (_param_handles[i].function != PARAM_INVALID && param_get(_param_handles[i].function, &val) == 0) {
_function_assignment[i] = (OutputFunction)val;
} else {
_function_assignment[i] = OutputFunction::Disabled;
}
for (int p = 0; p < (int)(sizeof(all_function_providers) / sizeof(all_function_providers[0])); ++p) {
if (_function_assignment[i] >= all_function_providers[p].min_func &&
_function_assignment[i] <= all_function_providers[p].max_func) {
@ -773,7 +816,19 @@ 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++) {
_current_output_value[i] = _failsafe_value[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];
}
}
}

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

@ -326,6 +326,8 @@ private:
bool _has_backup_schedule{false};
bool _reversible_motors =
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];
uORB::SubscriptionCallbackWorkItem *_subscription_callback{nullptr}; ///< current scheduling callback