diff --git a/Tools/module_config/generate_params.py b/Tools/module_config/generate_params.py index 92b25936568e..2307f51f662a 100755 --- a/Tools/module_config/generate_params.py +++ b/Tools/module_config/generate_params.py @@ -298,12 +298,14 @@ def add_local_param(param_name, param_def): ( 'min', 'Minimum', 'MIN', minimum_description ), ( 'max', 'Maximum', 'MAX', maximum_description ), ( 'failsafe', 'Failsafe', 'FAIL', failsafe_description ), + ( 'anglemin', 'Anglemin', 'MINA', failsafe_description ), + ( 'anglemax', 'Anglemax', 'MAXA', 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: + if standard_params[key]['min'] < 0 and key != "anglemin" and key != "anglemax": 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'])) diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index 45f56ab54025..d59e89b88cb0 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, "MINA", i + instance_start); + _param_handles[i].angle_min = param_find(param_name); + snprintf(param_name, sizeof(param_name), "%s_%s%d", _param_prefix, "MAXA", i + instance_start); + _param_handles[i].angle_max = param_find(param_name); } snprintf(param_name, sizeof(param_name), "%s_%s", _param_prefix, "REV"); @@ -182,6 +186,20 @@ void MixingOutput::updateParams() _max_value[i] = tmp; } + if (_param_handles[i].angle_max != PARAM_INVALID && param_get(_param_handles[i].angle_max, &val) == 0) { + _angle_max[i] = val; + } + + if (_param_handles[i].angle_min != PARAM_INVALID && param_get(_param_handles[i].angle_min, &val) == 0) { + _angle_min[i] = val; + } + + if (_angle_min[i] > _angle_max[i]) { + int tmp = _angle_min[i]; + _angle_min[i] = _angle_max[i]; + _angle_max[i] = tmp; + } + if (_param_handles[i].failsafe != PARAM_INVALID && param_get(_param_handles[i].failsafe, &val) == 0) { _failsafe_value[i] = val; } diff --git a/src/lib/mixer_module/mixer_module.hpp b/src/lib/mixer_module/mixer_module.hpp index ff400ca6d59f..0b849ed74cdf 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]; } + int &minAngle(int index) { return _angle_min[index]; } + int &maxAngle(int index) { return _angle_max[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 angle_min{PARAM_INVALID}; + param_t angle_max{PARAM_INVALID}; }; void lock() { do {} while (px4_sem_wait(&_lock) != 0); } @@ -243,6 +247,8 @@ class MixingOutput : public ModuleParams uint16_t _min_value[MAX_ACTUATORS] {}; uint16_t _max_value[MAX_ACTUATORS] {}; uint16_t _current_output_value[MAX_ACTUATORS] {}; ///< current output values (reordered) + int _angle_min[MAX_ACTUATORS] {}; + int _angle_max[MAX_ACTUATORS] {}; uint16_t _reverse_output_mask{0}; ///< reverses the interval [min, max] -> [max, min], NOT motor direction enum class OutputLimitState { diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp index 9c7969b2dc1e..864dff69b11a 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp @@ -64,8 +64,11 @@ 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; - ///TODO: Normalize output data - double output = (outputs[i] - 500) / 500.0; + + double output_range = _mixing_output.maxValue(i) - _mixing_output.minValue(i); + double angular_range = _mixing_output.maxAngle(i) - _mixing_output.minAngle(i); + double output = math::radians((double)_mixing_output.minAngle(i) + (double)outputs[i] / output_range * angular_range); + // std::cout << "outputs[" << i << "]: " << outputs[i] << std::endl; // std::cout << " output: " << output << std::endl; servo_output.set_data(output); diff --git a/src/modules/simulation/gz_bridge/module.yaml b/src/modules/simulation/gz_bridge/module.yaml index 1b38e13d9883..f92f0ad8d6c5 100644 --- a/src/modules/simulation/gz_bridge/module.yaml +++ b/src/modules/simulation/gz_bridge/module.yaml @@ -24,6 +24,8 @@ actuator_output: min: { min: 0, max: 1000, default: 0 } max: { min: 0, max: 1000, default: 1000 } failsafe: { min: 0, max: 1000 } + anglemin: { min: -180, max: 180, default: -30} + anglemax: { min: -180, max: 180, default: 30} num_channels: 8 - param_prefix: SIM_GZ_WH group_label: 'Wheels'