Adding min-max angles on controlsurface to use for normalized servo o…

…utputs

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

@@ -60,19 +60,25 @@ param set-default SIM_GZ_WH_REV 0 # no need to reverse any wheels
 # controls in practical scenarios.
 
 # Cutter deck blades clutch, PCA9685 servo channel 3,  "RC FLAPS" (406) - leftmost switch, or "Servo 3" (203):
-param set-default SIM_GZ_SV_FUNC3 203
-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_FUNC1 201
+param set-default SIM_GZ_SV_MIN1 0
+param set-default SIM_GZ_SV_MAX1 1000
+param set-default SIM_GZ_SV_DIS1 500
+param set-default SIM_GZ_SV_FAIL1 500
+param set-default CA_SV_CS0_MAXA 90
+param set-default CA_SV_CS0_MINA -90
 
 # Gas engine throttle, PCA9685 servo channel 4, "RC AUX1" (407) - left knob, or "Servo 4" (204):
 #    - on minimum when disarmed or failed:
-param set-default SIM_GZ_SV_FUNC4 204
-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_FUNC2 202
+param set-default SIM_GZ_SV_MIN2 0
+param set-default SIM_GZ_SV_MAX2 1000
+param set-default SIM_GZ_SV_DIS2 500
+param set-default SIM_GZ_SV_FAIL2 500
+param set-default CA_SV_CS1_MAXA 90
+param set-default CA_SV_CS1_MINA -90
+
+param set-default CA_SV_CS_COUNT 2
 
 # Controlling PCA9685 servos 5,6,7,8 directly via "Servo 5..8" setting, by publishing actuator_servos.control[]:
 

ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann

@@ -47,5 +47,8 @@ param set-default SIM_GZ_WH_MAX1 200
 param set-default SIM_GZ_WH_DIS1 100
 
 # Steering
+param set-default CA_SV_CS0_MAXA 30
+param set-default CA_SV_CS0_MINA -30
+param set-default CA_SV_CS_COUNT 1
 param set-default SIM_GZ_SV_FUNC1 201
 param set-default SIM_GZ_SV_REV 1

src/modules/control_allocator/module.yaml

@@ -342,6 +342,34 @@ parameters:
             instance_start: 0
             default: 0
 
+        CA_SV_CS${i}_MAXA:
+            description:
+                short: Control Surface ${i} Angle at Maximum
+                long: |
+                  Defines the angle when the servo is at the maximum.
+                  Currently only supported in gz simulation.
+            type: float
+            decimal: 0
+            num_instances: *max_num_servos
+            instance_start: 0
+            min: -180.0
+            max: 180.0
+            default: 45.0
+
+        CA_SV_CS${i}_MINA:
+            description:
+                short: Control Surface ${i} Angle at Minimum
+                long: |
+                  Defines the angle when the servo is at the minimum.
+                  Currently only supported in gz simulation.
+            type: float
+            decimal: 0
+            num_instances: *max_num_servos
+            instance_start: 0
+            min: -180.0
+            max: 180.0
+            default: -45.0
+
         # Tilts
         CA_SV_TL_COUNT:
             description:

src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp

@@ -33,6 +33,124 @@
 
 #include "GZMixingInterfaceServo.hpp"
 
+float
+GZMixingInterfaceServo::get_tl_angle_max(const size_t index)
+{
+	switch (index) {
+	case 0: return _ca_sv_tl_max_a_0.get();
+
+	case 1: return _ca_sv_tl_max_a_1.get();
+
+	case 2: return _ca_sv_tl_max_a_2.get();
+
+	case 3: return _ca_sv_tl_max_a_3.get();
+
+	default: return 0.f;
+	}
+}
+
+float
+GZMixingInterfaceServo::get_tl_angle_min(const size_t index)
+{
+	switch (index) {
+	case 0: return _ca_sv_tl_min_a_0.get();
+
+	case 1: return _ca_sv_tl_min_a_1.get();
+
+	case 2: return _ca_sv_tl_min_a_2.get();
+
+	case 3: return _ca_sv_tl_min_a_3.get();
+
+	default: return 0.f;
+	}
+}
+
+float
+GZMixingInterfaceServo::get_cs_angle_max(const size_t index)
+{
+	switch (index) {
+	case 0: return _ca_sv_cs_max_a_0.get();
+
+	case 1: return _ca_sv_cs_max_a_1.get();
+
+	case 2: return _ca_sv_cs_max_a_2.get();
+
+	case 3: return _ca_sv_cs_max_a_3.get();
+
+	case 4: return _ca_sv_cs_max_a_4.get();
+
+	case 5: return _ca_sv_cs_max_a_5.get();
+
+	case 6: return _ca_sv_cs_max_a_6.get();
+
+	case 7: return _ca_sv_cs_max_a_7.get();
+
+	default: return 0.f;
+	}
+}
+
+float
+GZMixingInterfaceServo::get_cs_angle_min(const size_t index)
+{
+	switch (index) {
+	case 0: return _ca_sv_cs_min_a_0.get();
+
+	case 1: return _ca_sv_cs_min_a_1.get();
+
+	case 2: return _ca_sv_cs_min_a_2.get();
+
+	case 3: return _ca_sv_cs_min_a_3.get();
+
+	case 4: return _ca_sv_cs_min_a_4.get();
+
+	case 5: return _ca_sv_cs_min_a_5.get();
+
+	case 6: return _ca_sv_cs_min_a_6.get();
+
+	case 7: return _ca_sv_cs_min_a_7.get();
+
+	default: return 0.f;
+	}
+}
+
+int GZMixingInterfaceServo::get_active_output_count()
+{
+	return _control_surface_count.get() + _tilt_count.get();
+}
+
+bool GZMixingInterfaceServo::is_tiltrotor(const int index)
+{
+	if (_tilt_count.get() < 1) {
+		return false;
+	}
+
+	return (index > _control_surface_count.get() - 1) && (index < get_active_output_count());
+}
+
+double GZMixingInterfaceServo::get_angle_max_wrapper(const int index)
+{
+	if (index >= get_active_output_count()) {
+		return 0.;
+	}
+
+	double angle = is_tiltrotor(index) ? (double)get_tl_angle_max(index - _control_surface_count.get()) : (
+			       double)get_cs_angle_max(index);
+
+	return math::radians(angle);
+}
+
+double GZMixingInterfaceServo::get_angle_min_wrapper(const int index)
+{
+	if (index >= get_active_output_count()) {
+		return 0.;
+	}
+
+	double angle = is_tiltrotor(index) ? (double)get_tl_angle_min(index - _control_surface_count.get()) : (
+			       double)get_cs_angle_min(index);
+
+	return math::radians(angle);
+}
+
 bool GZMixingInterfaceServo::init(const std::string &model_name)
 {
 	// /model/rascal_110_0/servo_2
@@ -46,6 +164,11 @@ bool GZMixingInterfaceServo::init(const std::string &model_name)
 			PX4_ERR("failed to advertise %s", servo_topic.c_str());
 			return false;
 		}
+
+		double min_val = get_angle_min_wrapper(i);
+		double max_val = get_angle_max_wrapper(i);
+		_angle_min_rad.push_back(min_val);
+		_angular_range_rad.push_back(max_val - min_val);
 	}
 
 	ScheduleNow();
@@ -64,8 +187,9 @@ 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 output = _angle_min_rad[i] + _angular_range_rad[i] * (outputs[i] - _mixing_output.minValue(i)) / output_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/GZMixingInterfaceServo.hpp

@@ -36,6 +36,7 @@
 #include <lib/mixer_module/mixer_module.hpp>
 
 #include <gz/transport.hh>
+#include <px4_platform_common/module_params.h>
 
 // GZBridge mixing class for Servos.
 // It is separate from GZBridge to have separate WorkItems and therefore allowing independent scheduling
@@ -66,11 +67,50 @@ class GZMixingInterfaceServo : public OutputModuleInterface
 	friend class GZBridge;
 
 	void Run() override;
+	int get_active_output_count();
+	float get_tl_angle_max(const size_t index);
+	float get_tl_angle_min(const size_t index);
+	float get_cs_angle_max(const size_t index);
+	float get_cs_angle_min(const size_t index);
+	bool is_tiltrotor(const int index);
+	double get_angle_min_wrapper(const int index);
+	double get_angle_max_wrapper(const int index);
 
 	gz::transport::Node &_node;
 	pthread_mutex_t &_node_mutex;
 
 	MixingOutput _mixing_output{"SIM_GZ_SV", MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
 
 	std::vector<gz::transport::Node::Publisher> _servos_pub;
+	std::vector<double> _angle_min_rad;
+	std::vector<double> _angular_range_rad;
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::CA_SV_TL0_MAXA>) _ca_sv_tl_max_a_0,
+		(ParamFloat<px4::params::CA_SV_TL0_MINA>) _ca_sv_tl_min_a_0,
+		(ParamFloat<px4::params::CA_SV_TL1_MAXA>) _ca_sv_tl_max_a_1,
+		(ParamFloat<px4::params::CA_SV_TL1_MINA>) _ca_sv_tl_min_a_1,
+		(ParamFloat<px4::params::CA_SV_TL2_MAXA>) _ca_sv_tl_max_a_2,
+		(ParamFloat<px4::params::CA_SV_TL2_MINA>) _ca_sv_tl_min_a_2,
+		(ParamFloat<px4::params::CA_SV_TL3_MAXA>) _ca_sv_tl_max_a_3,
+		(ParamFloat<px4::params::CA_SV_TL3_MINA>) _ca_sv_tl_min_a_3,
+		(ParamFloat<px4::params::CA_SV_CS0_MAXA>) _ca_sv_cs_max_a_0,
+		(ParamFloat<px4::params::CA_SV_CS0_MINA>) _ca_sv_cs_min_a_0,
+		(ParamFloat<px4::params::CA_SV_CS1_MAXA>) _ca_sv_cs_max_a_1,
+		(ParamFloat<px4::params::CA_SV_CS1_MINA>) _ca_sv_cs_min_a_1,
+		(ParamFloat<px4::params::CA_SV_CS2_MAXA>) _ca_sv_cs_max_a_2,
+		(ParamFloat<px4::params::CA_SV_CS2_MINA>) _ca_sv_cs_min_a_2,
+		(ParamFloat<px4::params::CA_SV_CS3_MAXA>) _ca_sv_cs_max_a_3,
+		(ParamFloat<px4::params::CA_SV_CS3_MINA>) _ca_sv_cs_min_a_3,
+		(ParamFloat<px4::params::CA_SV_CS4_MAXA>) _ca_sv_cs_max_a_4,
+		(ParamFloat<px4::params::CA_SV_CS4_MINA>) _ca_sv_cs_min_a_4,
+		(ParamFloat<px4::params::CA_SV_CS5_MAXA>) _ca_sv_cs_max_a_5,
+		(ParamFloat<px4::params::CA_SV_CS5_MINA>) _ca_sv_cs_min_a_5,
+		(ParamFloat<px4::params::CA_SV_CS6_MAXA>) _ca_sv_cs_max_a_6,
+		(ParamFloat<px4::params::CA_SV_CS6_MINA>) _ca_sv_cs_min_a_6,
+		(ParamFloat<px4::params::CA_SV_CS7_MAXA>) _ca_sv_cs_max_a_7,
+		(ParamFloat<px4::params::CA_SV_CS7_MINA>) _ca_sv_cs_min_a_7,
+		(ParamInt<px4::params::CA_SV_CS_COUNT>) _control_surface_count,
+		(ParamInt<px4::params::CA_SV_TL_COUNT>) _tilt_count
+	)
 };