From 1d0a28df2ab96b574d6c1c7bbca4555fdc3954ee Mon Sep 17 00:00:00 2001 From: Pernilla Date: Wed, 4 Dec 2024 14:39:10 +0100 Subject: [PATCH] adding offset and scaling as parameter --- Tools/module_config/generate_params.py | 23 +++++++++++++++---- src/lib/mixer_module/mixer_module.cpp | 17 ++++++++++++-- src/lib/mixer_module/mixer_module.hpp | 6 +++++ .../gz_bridge/GZMixingInterfaceServo.cpp | 5 ++-- src/modules/simulation/gz_bridge/module.yaml | 6 +++++ 5 files changed, 48 insertions(+), 9 deletions(-) diff --git a/Tools/module_config/generate_params.py b/Tools/module_config/generate_params.py index 8defa0615c71..2b1aa0a308fc 100755 --- a/Tools/module_config/generate_params.py +++ b/Tools/module_config/generate_params.py @@ -264,6 +264,7 @@ def add_local_param(param_name, param_def): - 'Max' for ConstantMax - 'Max' for Parachute - ('Max'+'Min')/2 for Servos +- output = (output_value - offset) / scaling - 'Disarmed' for the rest '''.format(channel_label), }, @@ -284,13 +285,25 @@ def add_local_param(param_name, param_def): ''' minimum_description = \ '''Minimum output value (when not disarmed). -Servo output represents angles [-180,180] with an offset of 180 deg, and a scaling of 10. -Ex. ailerons of -45 deg would be 1350. (Must be coherent with joint limits in the airframe/model.sdf) + +Ex. ailerons centered around zero deg with range of +-45 deg, should have min of 1350. +(Must be coherent with joint limits in the airframe/model.sdf) ''' maximum_description = \ '''Maxmimum output value (when not disarmed). -Servo output represents angles [-180,180] deg with an offset of 180 deg, and a scaling of 10. -Ex. ailerons of 45 deg would be 2250. (Must be coherent with joint limits in the airframe/model.sdf) + +Ex. ailerons centered around zero deg with range of +-45 deg, should have max of 2250. +(Must be coherent with joint limits in the airframe/model.sdf) +''' + offset_description = \ +''' Offset to output value. + +Ex. ailerons centered around zero deg with range of +-45 deg, should have offset of 1800. +''' + scaling_description = \ +''' Scales the output value by division for numerical precision due to integers (and to convert from deg to rad). + +Ex. ailerons centered around zero deg with range of +-45 deg, should have a scaling of 573 (573 = 10 / degToRad). ''' failsafe_description = \ '''This is the output value that is set when in failsafe mode. @@ -302,6 +315,8 @@ def add_local_param(param_name, param_def): ( 'min', 'Minimum', 'MIN', minimum_description ), ( 'max', 'Maximum', 'MAX', maximum_description ), ( 'failsafe', 'Failsafe', 'FAIL', failsafe_description ), + ( 'offset', 'Offset', 'OFFS', offset_description ), + ( 'scaling', 'Scaling', 'SCAL', scaling_description ), ] for key, label, param_suffix, description in standard_params_array: if key in standard_params: diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index 45f56ab54025..4032f3a2d471 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -123,6 +123,10 @@ void MixingOutput::initParamHandles(const uint8_t instance_start) _param_handles[i].max = param_find(param_name); snprintf(param_name, sizeof(param_name), "%s_%s%d", _param_prefix, "FAIL", i + instance_start); _param_handles[i].failsafe = param_find(param_name); + snprintf(param_name, sizeof(param_name), "%s_%s%d", _param_prefix, "OFFS", i + instance_start); + _param_handles[i].offset = param_find(param_name); + snprintf(param_name, sizeof(param_name), "%s_%s%d", _param_prefix, "SCAL", i + instance_start); + _param_handles[i].scaling = param_find(param_name); } snprintf(param_name, sizeof(param_name), "%s_%s", _param_prefix, "REV"); @@ -141,9 +145,10 @@ void MixingOutput::printStatus() const PX4_INFO_RAW("Channel Configuration:\n"); for (unsigned i = 0; i < _max_num_outputs; i++) { - PX4_INFO_RAW("Channel %i: func: %3i, value: %i, failsafe: %d, disarmed: %d, min: %d, max: %d\n", i, + PX4_INFO_RAW("Channel %i: func: %3i, value: %i, failsafe: %d, disarmed: %d, min: %d, max: %d, offset: %d, scaling: %d\n", + i, (int)_function_assignment[i], _current_output_value[i], - actualFailsafeValue(i), _disarmed_value[i], _min_value[i], _max_value[i]); + actualFailsafeValue(i), _disarmed_value[i], _min_value[i], _max_value[i], _offset_value[i], _scaling_value[i]); } } @@ -185,6 +190,14 @@ void MixingOutput::updateParams() if (_param_handles[i].failsafe != PARAM_INVALID && param_get(_param_handles[i].failsafe, &val) == 0) { _failsafe_value[i] = val; } + + if (_param_handles[i].offset != PARAM_INVALID && param_get(_param_handles[i].offset, &val) == 0) { + _offset_value[i] = val; + } + + if (_param_handles[i].scaling != PARAM_INVALID && param_get(_param_handles[i].scaling, &val) == 0) { + _scaling_value[i] = val; + } } _reverse_output_mask = 0; diff --git a/src/lib/mixer_module/mixer_module.hpp b/src/lib/mixer_module/mixer_module.hpp index ff400ca6d59f..8d2b1d57595b 100644 --- a/src/lib/mixer_module/mixer_module.hpp +++ b/src/lib/mixer_module/mixer_module.hpp @@ -174,6 +174,8 @@ class MixingOutput : public ModuleParams uint16_t &disarmedValue(int index) { return _disarmed_value[index]; } uint16_t &minValue(int index) { return _min_value[index]; } uint16_t &maxValue(int index) { return _max_value[index]; } + uint16_t &offsetValue(int index) { return _offset_value[index]; } + uint16_t &scalingValue(int index) { return _scaling_value[index]; } param_t functionParamHandle(int index) const { return _param_handles[index].function; } param_t disarmedParamHandle(int index) const { return _param_handles[index].disarmed; } @@ -231,6 +233,8 @@ class MixingOutput : public ModuleParams param_t min{PARAM_INVALID}; param_t max{PARAM_INVALID}; param_t failsafe{PARAM_INVALID}; + param_t offset{PARAM_INVALID}; + param_t scaling{PARAM_INVALID}; }; void lock() { do {} while (px4_sem_wait(&_lock) != 0); } @@ -242,6 +246,8 @@ class MixingOutput : public ModuleParams uint16_t _disarmed_value[MAX_ACTUATORS] {}; uint16_t _min_value[MAX_ACTUATORS] {}; uint16_t _max_value[MAX_ACTUATORS] {}; + uint16_t _offset_value[MAX_ACTUATORS] {}; + uint16_t _scaling_value[MAX_ACTUATORS] {}; uint16_t _current_output_value[MAX_ACTUATORS] {}; ///< current output values (reordered) uint16_t _reverse_output_mask{0}; ///< reverses the interval [min, max] -> [max, min], NOT motor direction diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp index 12dc17bdbca5..a8ddbb00bfac 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp @@ -64,9 +64,8 @@ bool GZMixingInterfaceServo::updateOutputs(bool stop_motors, uint16_t outputs[MA for (auto &servo_pub : _servos_pub) { if (_mixing_output.isFunctionSet(i)) { gz::msgs::Double servo_output; - // Servo output represents angles [-180,180] deg - // with an offset of 180 deg, and a scaling of 10. - double output = math::radians((double)outputs[i] / 10. - 180.); + double output = (double)(outputs[i] - _mixing_output.offsetValue(i)) / + (double)_mixing_output.scalingValue(i); // std::cout << "outputs[" << i << "]: " << outputs[i] << std::endl; // std::cout << " output: " << output << std::endl; diff --git a/src/modules/simulation/gz_bridge/module.yaml b/src/modules/simulation/gz_bridge/module.yaml index bd810369929a..91ad001865e4 100644 --- a/src/modules/simulation/gz_bridge/module.yaml +++ b/src/modules/simulation/gz_bridge/module.yaml @@ -15,6 +15,8 @@ actuator_output: min: { min: 0, max: 1000, default: 0 } max: { min: 0, max: 1000, default: 1000 } failsafe: { min: 0, max: 1000 } + offset: { min: 0, max: 3600, default: 0 } + scaling: { min: 1, max: 3600, default: 1 } num_channels: 8 - param_prefix: SIM_GZ_SV group_label: 'Servos' @@ -24,6 +26,8 @@ actuator_output: min: { min: 0, max: 3600, default: 1350 } max: { min: 0, max: 3600, default: 2250 } failsafe: { min: 0, max: 3600 } + offset: { min: 0, max: 3600, default: 1800 } + scaling: { min: 1, max: 3600, default: 573 } num_channels: 8 - param_prefix: SIM_GZ_WH group_label: 'Wheels' @@ -33,4 +37,6 @@ actuator_output: min: { min: 0, max: 200, default: 0 } max: { min: 0, max: 200, default: 200 } failsafe: { min: 0, max: 200 } + offset: { min: 0, max: 3600, default: 0 } + scaling: { min: 1, max: 3600, default: 1 } num_channels: 4