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 039d2cfc9399..0fec76b5c4f6 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower @@ -65,8 +65,8 @@ 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 +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: @@ -75,8 +75,8 @@ 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 SIM_GZ_SV_MAXA2 90 +param set-default SIM_GZ_SV_MINA2 -90 param set-default CA_SV_CS_COUNT 2 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 a5b0acec8ede..e058b87dc42b 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,8 +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 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 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4020_gz_tiltrotor b/ROMFS/px4fmu_common/init.d-posix/airframes/4020_gz_tiltrotor index 06ec1add6b19..c798650b5473 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4020_gz_tiltrotor +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4020_gz_tiltrotor @@ -77,6 +77,11 @@ 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 diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp index 23a461cb1800..2fc656074c9b 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp @@ -33,151 +33,69 @@ #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 angle; -float -GZMixingInterfaceServo::get_servo_angle_min(const size_t index) -{ switch (index) { - case 0: return _servo_min_angle_0.get(); + case 0: {angle = _servo_max_angle_1.get(); break;} - case 1: return _servo_min_angle_1.get(); + case 1: {angle = _servo_max_angle_2.get(); break;} - case 2: return _servo_min_angle_2.get(); + case 2: {angle = _servo_max_angle_3.get(); break;} - case 3: return _servo_min_angle_3.get(); + case 3: {angle = _servo_max_angle_4.get(); break;} - case 4: return _servo_min_angle_4.get(); + case 4: {angle = _servo_max_angle_5.get(); break;} - case 5: return _servo_min_angle_5.get(); + case 5: {angle = _servo_max_angle_6.get(); break;} - case 6: return _servo_min_angle_6.get(); + case 6: {angle = _servo_max_angle_7.get(); break;} - case 7: return _servo_min_angle_7.get(); + case 7: {angle = _servo_max_angle_8.get(); break;} - default: return NAN; + default: {angle = NAN; break;} } -} - -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; + if (!PX4_ISFINITE(angle)) { + PX4_ERR("max_angle: index out of range, i= %ld, expected i < 8", index); + return NAN; } - return (index > _control_surface_count.get() - 1) && (index < get_active_output_count()); + return math::radians(angle); } -double GZMixingInterfaceServo::get_angle_max_wrapper(const int index) +float +GZMixingInterfaceServo::get_servo_angle_min(const size_t index) { - if (index >= get_active_output_count()) { - return NAN; - } + float angle; - double angle; + switch (index) { + case 0: {angle = _servo_min_angle_1.get(); break;} - if (is_tiltrotor(index)) { - angle = (double)get_tilt_angle_max(index - _control_surface_count.get()); + case 1: {angle = _servo_min_angle_2.get(); break;} - if (!PX4_ISFINITE(angle)) { - PX4_ERR("max_angle: index out of range, i= %i, expected i < 4", index - _control_surface_count.get()); - return NAN; - } + case 2: {angle = _servo_min_angle_3.get(); break;} - } else { - angle = (double)get_servo_angle_max(index); + case 3: {angle = _servo_min_angle_4.get(); break;} - if (!PX4_ISFINITE(angle)) { - PX4_ERR("max_angle: index out of range, i= %i, expected i < 8", index); - return NAN; - } - } - - return math::radians(angle); -} + case 4: {angle = _servo_min_angle_5.get(); break;} -double GZMixingInterfaceServo::get_angle_min_wrapper(const int index) -{ - if (index >= get_active_output_count()) { - return NAN; - } + case 5: {angle = _servo_min_angle_6.get(); break;} - double angle; + case 6: {angle = _servo_min_angle_7.get(); break;} - if (is_tiltrotor(index)) { - angle = (double)get_tilt_angle_min(index - _control_surface_count.get()); + case 7: {angle = _servo_min_angle_8.get(); break;} - if (!PX4_ISFINITE(angle)) { - PX4_ERR("min_angle: index out of range, i= %i, expected i < 4", index - _control_surface_count.get()); - return NAN; - } + default: {angle = NAN; break;} - } 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; - } + if (!PX4_ISFINITE(angle)) { + PX4_ERR("min_angle: index out of range, i= %ld, expected i < 8", index); + return NAN; } return math::radians(angle); @@ -197,8 +115,8 @@ bool GZMixingInterfaceServo::init(const std::string &model_name) return false; } - double min_val = get_angle_min_wrapper(i); - double max_val = get_angle_max_wrapper(i); + 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); } diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp index 5bd06b4cf1d9..180f6cbca204 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp @@ -67,37 +67,20 @@ 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); + float get_servo_angle_max(const size_t 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); + float get_servo_angle_min(const size_t index); gz::transport::Node &_node; pthread_mutex_t &_node_mutex; @@ -109,16 +92,6 @@ class GZMixingInterfaceServo : public OutputModuleInterface 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, @@ -133,7 +106,7 @@ class GZMixingInterfaceServo : public OutputModuleInterface (ParamFloat) _servo_min_angle_6, (ParamFloat) _servo_max_angle_7, (ParamFloat) _servo_min_angle_7, - (ParamInt) _control_surface_count, - (ParamInt) _tilt_count + (ParamFloat) _servo_max_angle_8, + (ParamFloat) _servo_min_angle_8 ) }; diff --git a/src/modules/simulation/gz_bridge/module.yaml b/src/modules/simulation/gz_bridge/module.yaml index 226c51ee5a57..e41aedb1e324 100644 --- a/src/modules/simulation/gz_bridge/module.yaml +++ b/src/modules/simulation/gz_bridge/module.yaml @@ -43,29 +43,29 @@ parameters: definitions: SIM_GZ_SV_MAXA${i}: description: - short: Control Surface ${i} Angle at Maximum + 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. + 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: 0 + instance_start: 1 min: -180.0 max: 180.0 default: 45.0 SIM_GZ_SV_MINA${i}: description: - short: Control Surface ${i} Angle at Minimum + 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. + 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: 0 + instance_start: 1 min: -180.0 max: 180.0 default: -45.0