diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower b/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower index d4c80a29092f..039d2cfc9399 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower +++ b/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_MAXA0 90 +param set-default SIM_GZ_SV_MINA0 -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_MAXA1 90 +param set-default SIM_GZ_SV_MINA1 -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[]: diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann b/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann index 9e54c4015909..a5b0acec8ede 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann +++ b/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_MAXA0 30 +param set-default SIM_GZ_SV_MINA0 -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 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4020_gz_tiltrotor b/ROMFS/px4fmu_common/init.d-posix/airframes/4020_gz_tiltrotor new file mode 100644 index 000000000000..06ec1add6b19 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4020_gz_tiltrotor @@ -0,0 +1,107 @@ +#!/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 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 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt index ef208ab31460..00b668d227e3 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt @@ -91,6 +91,7 @@ px4_add_romfs_files( 4016_gz_x500_lidar_down 4017_gz_x500_lidar_front 4018_gz_quadtailsitter + 4020_gz_tiltrotor 6011_gazebo-classic_typhoon_h480 6011_gazebo-classic_typhoon_h480.post diff --git a/Tools/simulation/gz b/Tools/simulation/gz index 019f63e2d507..3691e12a8955 160000 --- a/Tools/simulation/gz +++ b/Tools/simulation/gz @@ -1 +1 @@ -Subproject commit 019f63e2d50763862b8f8d40cce77371b3260c52 +Subproject commit 3691e12a8955d7bf5c452ede672af96f51439152 diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp index 9c7969b2dc1e..23a461cb1800 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp @@ -33,6 +33,156 @@ #include "GZMixingInterfaceServo.hpp" +float +GZMixingInterfaceServo::get_tilt_angle_max(const size_t index) +{ + switch (index) { + case 0: return _tilt_max_angle_0.get(); + + case 1: return _tilt_max_angle_1.get(); + + case 2: return _tilt_max_angle_2.get(); + + case 3: return _tilt_max_angle_3.get(); + + default: return NAN; + } +} + +float +GZMixingInterfaceServo::get_tilt_angle_min(const size_t index) +{ + switch (index) { + case 0: return _tilt_min_angle_0.get(); + + case 1: return _tilt_min_angle_1.get(); + + case 2: return _tilt_min_angle_2.get(); + + case 3: return _tilt_min_angle_3.get(); + + default: return NAN; + } +} + +float +GZMixingInterfaceServo::get_servo_angle_max(const size_t index) +{ + switch (index) { + case 0: return _servo_max_angle_0.get(); + + case 1: return _servo_max_angle_1.get(); + + case 2: return _servo_max_angle_2.get(); + + case 3: return _servo_max_angle_3.get(); + + case 4: return _servo_max_angle_4.get(); + + case 5: return _servo_max_angle_5.get(); + + case 6: return _servo_max_angle_6.get(); + + case 7: return _servo_max_angle_7.get(); + + default: return NAN; + } +} + +float +GZMixingInterfaceServo::get_servo_angle_min(const size_t index) +{ + switch (index) { + case 0: return _servo_min_angle_0.get(); + + case 1: return _servo_min_angle_1.get(); + + case 2: return _servo_min_angle_2.get(); + + case 3: return _servo_min_angle_3.get(); + + case 4: return _servo_min_angle_4.get(); + + case 5: return _servo_min_angle_5.get(); + + case 6: return _servo_min_angle_6.get(); + + case 7: return _servo_min_angle_7.get(); + + default: return NAN; + } +} + +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 NAN; + } + + double angle; + + if (is_tiltrotor(index)) { + angle = (double)get_tilt_angle_max(index - _control_surface_count.get()); + + if (!PX4_ISFINITE(angle)) { + PX4_ERR("max_angle: index out of range, i= %i, expected i < 4", index - _control_surface_count.get()); + return NAN; + } + + } else { + angle = (double)get_servo_angle_max(index); + + if (!PX4_ISFINITE(angle)) { + PX4_ERR("max_angle: index out of range, i= %i, expected i < 8", index); + return NAN; + } + } + + return math::radians(angle); +} + +double GZMixingInterfaceServo::get_angle_min_wrapper(const int index) +{ + if (index >= get_active_output_count()) { + return NAN; + } + + double angle; + + if (is_tiltrotor(index)) { + angle = (double)get_tilt_angle_min(index - _control_surface_count.get()); + + if (!PX4_ISFINITE(angle)) { + PX4_ERR("min_angle: index out of range, i= %i, expected i < 4", index - _control_surface_count.get()); + return NAN; + } + + } else { + angle = (double)get_servo_angle_min(index); + + if (!PX4_ISFINITE(angle)) { + PX4_ERR("min_angle: index out of range, i= %i, 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 +196,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 +219,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); diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp index 63345b3aed89..5bd06b4cf1d9 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp @@ -36,6 +36,7 @@ #include #include +#include // GZBridge mixing class for Servos. // It is separate from GZBridge to have separate WorkItems and therefore allowing independent scheduling @@ -66,6 +67,37 @@ class GZMixingInterfaceServo : public OutputModuleInterface friend class GZBridge; void Run() override; + int get_active_output_count(); + float get_tilt_angle_max(const size_t index); + float get_tilt_angle_min(const size_t index); + float get_servo_angle_max(const size_t index); + float get_servo_angle_min(const size_t index); + + /** + * @brief Checks if the servo index is configured to tiltrotor. + * Tiltrotors should be ordered after the control surfaces. + * @param index: int + * @return True if the index is as expected, otherwise False. + */ + bool is_tiltrotor(const int index); + + /** + * @brief Get maximum configured angle of servo. + * Takes the maximum value from CA_SV_TL{n}_MAXA or SIM_GZ_SV_MAXA{n} param + * dependent on input index. + * @param index: servo index + * @return angle_max [rad] + */ + double get_angle_max_wrapper(const int index); + + /** + * @brief Get minimum configured angle of servo. + * Takes the minimum value from CA_SV_TL{n}_MINA or SIM_GZ_SV_MINA{n} param + * dependent on input index. + * @param index: servo index + * @return angle_min [rad] + */ + double get_angle_min_wrapper(const int index); gz::transport::Node &_node; pthread_mutex_t &_node_mutex; @@ -73,4 +105,35 @@ class GZMixingInterfaceServo : public OutputModuleInterface MixingOutput _mixing_output{"SIM_GZ_SV", MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}; std::vector _servos_pub; + std::vector _angle_min_rad; + std::vector _angular_range_rad; + + DEFINE_PARAMETERS( + (ParamFloat) _tilt_max_angle_0, + (ParamFloat) _tilt_min_angle_0, + (ParamFloat) _tilt_max_angle_1, + (ParamFloat) _tilt_min_angle_1, + (ParamFloat) _tilt_max_angle_2, + (ParamFloat) _tilt_min_angle_2, + (ParamFloat) _tilt_max_angle_3, + (ParamFloat) _tilt_min_angle_3, + (ParamFloat) _servo_max_angle_0, + (ParamFloat) _servo_min_angle_0, + (ParamFloat) _servo_max_angle_1, + (ParamFloat) _servo_min_angle_1, + (ParamFloat) _servo_max_angle_2, + (ParamFloat) _servo_min_angle_2, + (ParamFloat) _servo_max_angle_3, + (ParamFloat) _servo_min_angle_3, + (ParamFloat) _servo_max_angle_4, + (ParamFloat) _servo_min_angle_4, + (ParamFloat) _servo_max_angle_5, + (ParamFloat) _servo_min_angle_5, + (ParamFloat) _servo_max_angle_6, + (ParamFloat) _servo_min_angle_6, + (ParamFloat) _servo_max_angle_7, + (ParamFloat) _servo_min_angle_7, + (ParamInt) _control_surface_count, + (ParamInt) _tilt_count + ) }; diff --git a/src/modules/simulation/gz_bridge/module.yaml b/src/modules/simulation/gz_bridge/module.yaml index 1b38e13d9883..226c51ee5a57 100644 --- a/src/modules/simulation/gz_bridge/module.yaml +++ b/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: Control Surface ${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. + type: float + decimal: 0 + unit: 'deg' + num_instances: *max_num_servos + instance_start: 0 + min: -180.0 + max: 180.0 + default: 45.0 + SIM_GZ_SV_MINA${i}: + 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 and must be coherent with .sdf file. + type: float + decimal: 0 + unit: 'deg' + num_instances: *max_num_servos + instance_start: 0 + min: -180.0 + max: 180.0 + default: -45.0