diff --git a/src/lib/mixer_module/CMakeLists.txt b/src/lib/mixer_module/CMakeLists.txt index bc65a51536..50495261a1 100644 --- a/src/lib/mixer_module/CMakeLists.txt +++ b/src/lib/mixer_module/CMakeLists.txt @@ -31,9 +31,23 @@ # ############################################################################ +set(functions_header ${CMAKE_CURRENT_BINARY_DIR}/output_functions.hpp) +add_custom_command(OUTPUT ${functions_header} + COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/generate_function_header.py + ${CMAKE_CURRENT_SOURCE_DIR}/output_functions.yaml + ${functions_header} + DEPENDS + output_functions.yaml + generate_function_header.py + COMMENT "Generating output_functions.hpp" +) +add_custom_target(output_functions_header DEPENDS ${functions_header}) + px4_add_library(mixer_module mixer_module.cpp functions.cpp ) + +add_dependencies(mixer_module output_functions_header) target_link_libraries(mixer_module PRIVATE output_limit) target_compile_options(mixer_module PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) diff --git a/src/lib/mixer_module/functions.hpp b/src/lib/mixer_module/functions.hpp index 7708fdf555..4d6712ea23 100644 --- a/src/lib/mixer_module/functions.hpp +++ b/src/lib/mixer_module/functions.hpp @@ -35,7 +35,7 @@ #include -#include "output_functions.hpp" +#include #include #include @@ -72,7 +72,7 @@ public: }; /** - * Functions: ConstantMin + * Functions: Constant_Min */ class FunctionConstantMin : public FunctionProviderBase { @@ -86,7 +86,7 @@ public: }; /** - * Functions: ConstantMax + * Functions: Constant_Max */ class FunctionConstantMax : public FunctionProviderBase { @@ -126,7 +126,7 @@ private: }; /** - * Functions: ActuatorSet1 ... ActuatorSet6 + * Functions: Offboard_Actuator_Set1 ... Offboard_Actuator_Set6 */ class FunctionActuatorSet : public FunctionProviderBase { @@ -135,7 +135,7 @@ public: static FunctionProviderBase *allocate(const Context &context) { return new FunctionActuatorSet(); } void update() override; - float value(OutputFunction func) override { return _data[(int)func - (int)OutputFunction::ActuatorSet1]; } + float value(OutputFunction func) override { return _data[(int)func - (int)OutputFunction::Offboard_Actuator_Set1]; } private: static constexpr int max_num_actuators = 6; @@ -143,4 +143,3 @@ private: uORB::Subscription _topic{ORB_ID(vehicle_command)}; float _data[max_num_actuators]; }; - diff --git a/src/lib/mixer_module/generate_function_header.py b/src/lib/mixer_module/generate_function_header.py new file mode 100755 index 0000000000..a08a4d7524 --- /dev/null +++ b/src/lib/mixer_module/generate_function_header.py @@ -0,0 +1,75 @@ +#! /usr/bin/env python3 +""" Script to generate header file from YAML file """ + +import argparse +import sys +import os + +try: + import yaml +except ImportError as e: + print("Failed to import yaml: " + str(e)) + print("") + print("You may need to install it using:") + print(" pip3 install --user pyyaml") + print("") + sys.exit(1) + + +parser = argparse.ArgumentParser(description='Generate function header from yaml') + +parser.add_argument('yaml_file', type=str, help='YAML config file') +parser.add_argument('output_file', type=str, help='.hpp header file') + +args = parser.parse_args() +yaml_file = args.yaml_file +output_file = args.output_file + +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 + +document = load_yaml_file(yaml_file) +functions = document['functions'] +all_indexes=set() +function_defs = '' + +def add_function(index, name, check_duplicate=True): + global all_indexes, function_defs + if check_duplicate and index in all_indexes: + raise Exception('Duplicate function with index {0} ({1})'.format(index, name)) + all_indexes.add(index) + + function_defs += '\t{0} = {1},\n'.format(name, index) + +for group_key in functions: + group = functions[group_key] + for function_name in group: + if isinstance(group[function_name], int): + add_function(group[function_name], function_name) + else: + start = group[function_name]['start'] + count = group[function_name]['count'] + for i in range(count): + add_function(start+i, function_name+str(i+1)) + add_function(start+count-1, function_name+'Max', False) + function_defs += '\n' + + +with open(output_file, 'w') as outfile: + outfile.write(''' +#pragma once + +// This file is auto-generated by {0} from {1} + +#include + +enum class OutputFunction : int32_t {{ +{2} +}}; +'''.format(os.path.basename(__file__), yaml_file, function_defs)) + diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index cac36a5546..573fc4b96b 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -55,10 +55,10 @@ struct FunctionProvider { static const FunctionProvider all_function_providers[] = { // Providers higher up take precedence for subscription callback in case there are multiple - {OutputFunction::ConstantMin, &FunctionConstantMin::allocate}, - {OutputFunction::ConstantMax, &FunctionConstantMax::allocate}, + {OutputFunction::Constant_Min, &FunctionConstantMin::allocate}, + {OutputFunction::Constant_Max, &FunctionConstantMax::allocate}, {OutputFunction::Motor1, OutputFunction::MotorMax, &FunctionMotors::allocate}, - {OutputFunction::ActuatorSet1, OutputFunction::ActuatorSet6, &FunctionActuatorSet::allocate}, + {OutputFunction::Offboard_Actuator_Set1, OutputFunction::Offboard_Actuator_Set6, &FunctionActuatorSet::allocate}, }; MixingOutput::MixingOutput(uint8_t max_num_outputs, OutputModuleInterface &interface, diff --git a/src/lib/mixer_module/output_functions.hpp b/src/lib/mixer_module/output_functions.hpp deleted file mode 100644 index 480f9f911c..0000000000 --- a/src/lib/mixer_module/output_functions.hpp +++ /dev/null @@ -1,65 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2021 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#pragma once - -// this will be auto-generated... - -#include - -enum class OutputFunction : int32_t { - Disabled = 0, - - ConstantMin = 1, - ConstantMax = 2, - - Motor1 = 101, - Motor2 = 102, - Motor3 = 103, - Motor4 = 104, - Motor5 = 105, - Motor6 = 106, - Motor7 = 107, - Motor8 = 108, - - MotorMax = Motor8, - - - ActuatorSet1 = 300, - ActuatorSet2 = 301, - ActuatorSet3 = 302, - ActuatorSet4 = 303, - ActuatorSet5 = 304, - ActuatorSet6 = 305, - -}; diff --git a/src/lib/mixer_module/output_functions.yaml b/src/lib/mixer_module/output_functions.yaml new file mode 100644 index 0000000000..33f3561f4c --- /dev/null +++ b/src/lib/mixer_module/output_functions.yaml @@ -0,0 +1,25 @@ +functions: + # Note: do not change the index of existing functions, as it will break + # existing setups using it + common: + Disabled: 0 + + Constant_Min: 1 + Constant_Max: 2 + + Motor: + start: 101 + count: 8 + Servo: + start: 201 + count: 8 + Offboard_Actuator_Set: + start: 301 + count: 6 + + # Add your own here: + #MyCustomFunction: 10000 + + pwm_fmu: # These only exist on FMU PWM pins + Camera_Trigger: 2000 + Camera_Capture: 2032