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

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:
-        try:
-            yaml_config = yaml.load(stream, Loader=yaml.Loader)
+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']))
 
-            all_params += parse_yaml_parameters_config(yaml_config, ethernet_supported)
+                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:
+            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)

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];
+			}
 		}
 	}
 

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