allow for velocity control for servos

Author: perrrre

ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower

@@ -66,6 +66,7 @@ param set-default SIM_GZ_SV_MIN3 0
 param set-default SIM_GZ_SV_MAX3 1000
 param set-default SIM_GZ_SV_DIS3 500
 param set-default SIM_GZ_SV_FAIL3 500
+param set-default SIM_GZ_SV_PCTRL3 0
 
 # Gas engine throttle, PCA9685 servo channel 4, "RC AUX1" (407) - left knob, or "Servo 4" (204):
 #    - on minimum when disarmed or failed:
@@ -74,6 +75,7 @@ param set-default SIM_GZ_SV_MIN4 0
 param set-default SIM_GZ_SV_MAX4 1000
 param set-default SIM_GZ_SV_DIS4 500
 param set-default SIM_GZ_SV_FAIL4 500
+param set-default SIM_GZ_SV_PCTRL4 0
 
 # Controlling PCA9685 servos 5,6,7,8 directly via "Servo 5..8" setting, by publishing actuator_servos.control[]:
 

Tools/module_config/generate_params.py

@@ -293,13 +293,24 @@ def add_local_param(param_name, param_def):
 
 When set to -1 (default), the value depends on the function (see {:}).
 '''.format(param_prefix+'_FUNC${i}')
+
+        min_angle_description = \
+'''Minimum angle at minimum output value (when not disarmed).
+'''
+        max_angle_description = \
+'''Maxmimum angle at maximum output value (when not disarmed).
+'''
+        position_controller_description = \
+'''1 if servo is used for position control, otherwise 0.
+'''
         standard_params_array = [
             ( 'disarmed', 'Disarmed', 'DIS', disarmed_description ),
             ( '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 ),
+            ( 'anglemin', 'Anglemin', 'MINA', min_angle_description ),
+            ( 'anglemax', 'Anglemax', 'MAXA', max_angle_description ),
+            ( 'positionctrl', 'Positionctrl', 'PCTRL', position_controller_description ),
             ]
         for key, label, param_suffix, description in standard_params_array:
             if key in standard_params:

src/lib/mixer_module/mixer_module.cpp

@@ -127,6 +127,8 @@ void MixingOutput::initParamHandles(const uint8_t 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%d", _param_prefix, "PCTRL", i + instance_start);
+		_param_handles[i].position_ctrl = param_find(param_name);
 	}
 
 	snprintf(param_name, sizeof(param_name), "%s_%s", _param_prefix, "REV");
@@ -200,6 +202,10 @@ void MixingOutput::updateParams()
 			_angle_max[i] = tmp;
 		}
 
+		if (_param_handles[i].position_ctrl != PARAM_INVALID && param_get(_param_handles[i].position_ctrl, &val) == 0) {
+			_position_ctrl[i] = val;
+		}
+
 		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

@@ -176,6 +176,7 @@ class MixingOutput : public ModuleParams
 	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]; }
+	uint16_t &usePositionCtrl(int index) { return _position_ctrl[index]; }
 
 	param_t functionParamHandle(int index) const { return _param_handles[index].function; }
 	param_t disarmedParamHandle(int index) const { return _param_handles[index].disarmed; }
@@ -235,6 +236,7 @@ class MixingOutput : public ModuleParams
 		param_t failsafe{PARAM_INVALID};
 		param_t angle_min{PARAM_INVALID};
 		param_t angle_max{PARAM_INVALID};
+		param_t position_ctrl{PARAM_INVALID};
 	};
 
 	void lock() { do {} while (px4_sem_wait(&_lock) != 0); }
@@ -249,6 +251,7 @@ class MixingOutput : public ModuleParams
 	uint16_t _current_output_value[MAX_ACTUATORS] {}; ///< current output values (reordered)
 	int _angle_min[MAX_ACTUATORS] {};
 	int _angle_max[MAX_ACTUATORS] {};
+	uint16_t _position_ctrl[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

@@ -66,8 +66,17 @@ bool GZMixingInterfaceServo::updateOutputs(bool stop_motors, uint16_t outputs[MA
 			gz::msgs::Double servo_output;
 
 			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);
+			double output;
+
+			if (_mixing_output.usePositionCtrl(i)) {
+				double angular_range = _mixing_output.maxAngle(i) - _mixing_output.minAngle(i);
+				output = math::radians((double)_mixing_output.minAngle(i) + (double)outputs[i] / output_range * angular_range);
+
+			} else {
+				// This is currently only used by the landmower
+				double half_range = output_range / 2.;
+				output = ((double)outputs[i] - half_range) / half_range;
+			}
 
 			// std::cout << "outputs[" << i << "]: " << outputs[i] << std::endl;
 			// std::cout << "  output: " << output << std::endl;

src/modules/simulation/gz_bridge/module.yaml

@@ -26,6 +26,7 @@ actuator_output:
         failsafe: { min: 0, max: 1000 }
         anglemin: { min: -180, max: 180, default: -30}
         anglemax: { min: -180, max: 180, default: 30}
+        positionctrl: { min: 0, max: 1, default: 1}
       num_channels: 8
     - param_prefix: SIM_GZ_WH
       group_label: 'Wheels'