Adding tiltrotor airframe for gazebo simulation

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 SIM_GZ_SV_MAXA1 90
+param set-default SIM_GZ_SV_MINA1 -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 SIM_GZ_SV_MAXA2 90
+param set-default SIM_GZ_SV_MINA2 -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 SIM_GZ_SV_MAXA1 30
+param set-default SIM_GZ_SV_MINA1 -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

ROMFS/px4fmu_common/init.d-posix/airframes/4020_gz_tiltrotor

@@ -0,0 +1,112 @@
+#!/bin/sh
+#
+# @name VTOL Tiltrotor
+#
+# @type VTOL Tiltrotor
+#
+
+. ${R}etc/init.d/rc.vtol_defaults
+
+PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
+PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
+PX4_SIM_MODEL=${PX4_SIM_MODEL:=tiltrotor}
+
+param set-default SIM_GZ_EN 1 # Gazebo bridge
+
+param set-default SENS_EN_GPSSIM 1
+param set-default SENS_EN_BAROSIM 0
+param set-default SENS_EN_MAGSIM 1
+param set-default SENS_EN_ARSPDSIM 0
+
+param set-default MAV_TYPE 21
+
+param set-default CA_AIRFRAME 3
+
+param set-default CA_ROTOR_COUNT 4
+param set-default CA_ROTOR0_PX 0.1515
+param set-default CA_ROTOR0_PY 0.245
+param set-default CA_ROTOR0_KM 0.05
+param set-default CA_ROTOR1_PX -0.1515
+param set-default CA_ROTOR1_PY -0.1875
+param set-default CA_ROTOR1_KM 0.05
+param set-default CA_ROTOR2_PX 0.1515
+param set-default CA_ROTOR2_PY -0.245
+param set-default CA_ROTOR2_KM -0.05
+param set-default CA_ROTOR3_PX -0.1515
+param set-default CA_ROTOR3_PY 0.1875
+param set-default CA_ROTOR3_KM -0.05
+
+param set-default CA_ROTOR0_TILT 1
+param set-default CA_ROTOR2_TILT 2
+param set-default CA_SV_TL0_MAXA 90
+param set-default CA_SV_TL0_MINA 0
+param set-default CA_SV_TL0_TD 0
+param set-default CA_SV_TL0_CT 1
+param set-default CA_SV_TL1_MAXA 90
+param set-default CA_SV_TL1_MINA 0
+param set-default CA_SV_TL1_TD 0
+param set-default CA_SV_TL1_CT 1
+
+param set-default CA_SV_CS0_TRQ_R -0.5
+param set-default CA_SV_CS0_TYPE 1
+param set-default CA_SV_CS1_TRQ_R 0.5
+param set-default CA_SV_CS1_TYPE 2
+param set-default CA_SV_CS2_TRQ_P 1
+param set-default CA_SV_CS2_TYPE 3
+param set-default CA_SV_CS_COUNT 3
+param set-default CA_SV_TL_COUNT 2
+
+param set-default SIM_GZ_EC_FUNC1 101
+param set-default SIM_GZ_EC_FUNC2 102
+param set-default SIM_GZ_EC_FUNC3 103
+param set-default SIM_GZ_EC_FUNC4 104
+
+param set-default SIM_GZ_EC_MIN1 10
+param set-default SIM_GZ_EC_MIN2 10
+param set-default SIM_GZ_EC_MIN3 10
+param set-default SIM_GZ_EC_MIN4 10
+
+param set-default SIM_GZ_EC_MAX1 1500
+param set-default SIM_GZ_EC_MAX2 1500
+param set-default SIM_GZ_EC_MAX3 1500
+param set-default SIM_GZ_EC_MAX4 1500
+
+param set-default SIM_GZ_SV_FUNC1 201
+param set-default SIM_GZ_SV_FUNC2 202
+param set-default SIM_GZ_SV_FUNC3 203
+param set-default SIM_GZ_SV_FUNC4 204
+param set-default SIM_GZ_SV_FUNC5 205
+
+param set-default SIM_GZ_SV_MAXA4 90
+param set-default SIM_GZ_SV_MINA4 0
+param set-default SIM_GZ_SV_MAXA5 90
+param set-default SIM_GZ_SV_MINA5 0
+
+param set-default NPFG_PERIOD 12
+param set-default FW_PR_FF 0.2
+param set-default FW_PR_P 0.9
+param set-default FW_PSP_OFF 2
+param set-default FW_P_LIM_MIN -15
+param set-default FW_RR_FF 0.1
+param set-default FW_RR_P 0.3
+param set-default FW_THR_TRIM 0.38
+param set-default FW_THR_MAX 0.6
+param set-default FW_THR_MIN 0.05
+param set-default FW_T_CLMB_MAX 8
+param set-default FW_T_SINK_MAX 2.7
+param set-default FW_T_SINK_MIN 2.2
+
+param set-default MC_AIRMODE 1
+param set-default MC_YAWRATE_P 0.4
+param set-default MC_YAWRATE_I 0.1
+
+param set-default MPC_XY_VEL_P_ACC 3
+param set-default MPC_XY_VEL_I_ACC 4
+param set-default MPC_XY_VEL_D_ACC 0.1
+
+param set-default MIS_TAKEOFF_ALT 10
+
+param set-default VT_FWD_THRUST_EN 4
+param set-default VT_FWD_THRUST_SC 0.6
+param set-default VT_TILT_TRANS 0.6
+param set-default VT_TYPE 1

ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt

@@ -92,6 +92,7 @@ px4_add_romfs_files(
 	4017_gz_x500_lidar_front
 	4018_gz_quadtailsitter
 	4019_gz_x500_gimbal
+	4020_gz_tiltrotor
 
 	6011_gazebo-classic_typhoon_h480
 	6011_gazebo-classic_typhoon_h480.post

src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp

@@ -33,6 +33,74 @@
 
 #include "GZMixingInterfaceServo.hpp"
 
+
+float
+GZMixingInterfaceServo::get_servo_angle_max(const size_t index)
+{
+	float angle;
+
+	switch (index) {
+	case 0: {angle = _servo_max_angle_1.get(); break;}
+
+	case 1: {angle = _servo_max_angle_2.get(); break;}
+
+	case 2: {angle = _servo_max_angle_3.get(); break;}
+
+	case 3: {angle = _servo_max_angle_4.get(); break;}
+
+	case 4: {angle = _servo_max_angle_5.get(); break;}
+
+	case 5: {angle = _servo_max_angle_6.get(); break;}
+
+	case 6: {angle = _servo_max_angle_7.get(); break;}
+
+	case 7: {angle = _servo_max_angle_8.get(); break;}
+
+	default: {angle = NAN; break;}
+	}
+
+	if (!PX4_ISFINITE(angle)) {
+		PX4_ERR("max_angle: index out of range, i= %ld, expected i < 8", index);
+		return NAN;
+	}
+
+	return math::radians(angle);
+}
+
+float
+GZMixingInterfaceServo::get_servo_angle_min(const size_t index)
+{
+	float angle;
+
+	switch (index) {
+	case 0: {angle = _servo_min_angle_1.get(); break;}
+
+	case 1: {angle = _servo_min_angle_2.get(); break;}
+
+	case 2: {angle = _servo_min_angle_3.get(); break;}
+
+	case 3: {angle = _servo_min_angle_4.get(); break;}
+
+	case 4: {angle = _servo_min_angle_5.get(); break;}
+
+	case 5: {angle = _servo_min_angle_6.get(); break;}
+
+	case 6: {angle = _servo_min_angle_7.get(); break;}
+
+	case 7: {angle = _servo_min_angle_8.get(); break;}
+
+	default: {angle = NAN; break;}
+
+	}
+
+	if (!PX4_ISFINITE(angle)) {
+		PX4_ERR("min_angle: index out of range, i= %ld, expected i < 8", index);
+		return NAN;
+	}
+
+	return math::radians(angle);
+}
+
 bool GZMixingInterfaceServo::init(const std::string &model_name)
 {
 	// /model/rascal_110_0/servo_2
@@ -46,6 +114,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_servo_angle_min(i);
+		double max_val = get_servo_angle_max(i);
+		_angle_min_rad.push_back(min_val);
+		_angular_range_rad.push_back(max_val - min_val);
 	}
 
 	ScheduleNow();
@@ -64,8 +137,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
@@ -67,10 +68,45 @@ class GZMixingInterfaceServo : public OutputModuleInterface
 
 	void Run() override;
 
+	/**
+	 * @brief Get maximum configured angle of servo.
+	 * @param index: servo index
+	 * @return angle_max [rad]
+	 */
+	float get_servo_angle_max(const size_t index);
+
+	/**
+	 * @brief Get minimum configured angle of servo.
+	 * @param index: servo index
+	 * @return angle_min [rad]
+	 */
+	float get_servo_angle_min(const size_t 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::SIM_GZ_SV_MAXA1>) _servo_max_angle_1,
+		(ParamFloat<px4::params::SIM_GZ_SV_MINA1>) _servo_min_angle_1,
+		(ParamFloat<px4::params::SIM_GZ_SV_MAXA2>) _servo_max_angle_2,
+		(ParamFloat<px4::params::SIM_GZ_SV_MINA2>) _servo_min_angle_2,
+		(ParamFloat<px4::params::SIM_GZ_SV_MAXA3>) _servo_max_angle_3,
+		(ParamFloat<px4::params::SIM_GZ_SV_MINA3>) _servo_min_angle_3,
+		(ParamFloat<px4::params::SIM_GZ_SV_MAXA4>) _servo_max_angle_4,
+		(ParamFloat<px4::params::SIM_GZ_SV_MINA4>) _servo_min_angle_4,
+		(ParamFloat<px4::params::SIM_GZ_SV_MAXA5>) _servo_max_angle_5,
+		(ParamFloat<px4::params::SIM_GZ_SV_MINA5>) _servo_min_angle_5,
+		(ParamFloat<px4::params::SIM_GZ_SV_MAXA6>) _servo_max_angle_6,
+		(ParamFloat<px4::params::SIM_GZ_SV_MINA6>) _servo_min_angle_6,
+		(ParamFloat<px4::params::SIM_GZ_SV_MAXA7>) _servo_max_angle_7,
+		(ParamFloat<px4::params::SIM_GZ_SV_MINA7>) _servo_min_angle_7,
+		(ParamFloat<px4::params::SIM_GZ_SV_MAXA8>) _servo_max_angle_8,
+		(ParamFloat<px4::params::SIM_GZ_SV_MINA8>) _servo_min_angle_8
+	)
 };

src/modules/simulation/gz_bridge/module.yaml

@@ -1,3 +1,6 @@
+__max_num_servos: &max_num_servos 8
+__max_num_tilts: &max_num_tilts 4
+
 module_name: SIM_GZ
 actuator_output:
   show_subgroups_if: 'SIM_GZ_EN>=1'
@@ -15,7 +18,7 @@ actuator_output:
         min: { min: 0, max: 1000, default: 0 }
         max: { min: 0, max: 1000, default: 1000 }
         failsafe: { min: 0, max: 1000 }
-      num_channels: 8
+      num_channels: *max_num_servos
     - param_prefix: SIM_GZ_SV
       group_label: 'Servos'
       channel_label: 'Servo'
@@ -24,7 +27,7 @@ actuator_output:
         min: { min: 0, max: 1000, default: 0 }
         max: { min: 0, max: 1000, default: 1000 }
         failsafe: { min: 0, max: 1000 }
-      num_channels: 8
+      num_channels: *max_num_servos
     - param_prefix: SIM_GZ_WH
       group_label: 'Wheels'
       channel_label: 'Wheels'
@@ -34,3 +37,35 @@ actuator_output:
         max: { min: 0, max: 200, default: 200 }
         failsafe: { min: 0, max: 200 }
       num_channels: 4
+
+parameters:
+    - group: Geometry
+      definitions:
+        SIM_GZ_SV_MAXA${i}:
+            description:
+                short: Servo ${i} Angle at Maximum
+                long: |
+                  Defines the angle when the servo is at the maximum.
+                  Currently only supported in gz simulation and must be coherent with .sdf file and CA_SV_TL{n}_MAXA.
+            type: float
+            decimal: 0
+            unit: 'deg'
+            num_instances: *max_num_servos
+            instance_start: 1
+            min: -180.0
+            max: 180.0
+            default: 45.0
+        SIM_GZ_SV_MINA${i}:
+            description:
+                short: Servo ${i} Angle at Minimum
+                long: |
+                  Defines the angle when the servo is at the minimum.
+                  Currently only supported in gz simulation and must be coherent with .sdf file and CA_SV_TL{n}_MINA.
+            type: float
+            decimal: 0
+            unit: 'deg'
+            num_instances: *max_num_servos
+            instance_start: 1
+            min: -180.0
+            max: 180.0
+            default: -45.0