diff --git a/Tools/module_config/generate_params.py b/Tools/module_config/generate_params.py index 1740a69fc0..e342164636 100755 --- a/Tools/module_config/generate_params.py +++ b/Tools/module_config/generate_params.py @@ -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) diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index b0cdea7d21..6ed5d25734 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -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]; + } } } diff --git a/src/lib/mixer_module/mixer_module.hpp b/src/lib/mixer_module/mixer_module.hpp index 4019eb77e4..9dfa8dee45 100644 --- a/src/lib/mixer_module/mixer_module.hpp +++ b/src/lib/mixer_module/mixer_module.hpp @@ -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