Send angles in actual range instead of hardcoded angles

Author: perrrre

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']))

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

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 {

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);

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: -45}
+        anglemax: { min: -180, max: 180, default: 45}
       num_channels: 8
     - param_prefix: SIM_GZ_WH
       group_label: 'Wheels'