diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4019_gz_x500_gimbal b/ROMFS/px4fmu_common/init.d-posix/airframes/4019_gz_x500_gimbal index 80719ae3d3..9dcd642c2c 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4019_gz_x500_gimbal +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4019_gz_x500_gimbal @@ -19,5 +19,6 @@ param set-default MNT_MAN_PITCH 2 param set-default MNT_MAN_YAW 3 param set-default MNT_RANGE_ROLL 180 -param set-default MNT_RANGE_PITCH 180 +param set-default MNT_MAX_PITCH 45 +param set-default MNT_MIN_PITCH -135 param set-default MNT_RANGE_YAW 720 diff --git a/src/modules/simulation/gz_bridge/GZGimbal.cpp b/src/modules/simulation/gz_bridge/GZGimbal.cpp index 36134bb386..f5bc4881b7 100644 --- a/src/modules/simulation/gz_bridge/GZGimbal.cpp +++ b/src/modules/simulation/gz_bridge/GZGimbal.cpp @@ -38,12 +38,14 @@ bool GZGimbal::init(const std::string &world_name, const std::string &model_name // Mount parameters _mnt_range_roll_handle = param_find("MNT_RANGE_ROLL"); - _mnt_range_pitch_handle = param_find("MNT_RANGE_PITCH"); + _mnt_max_pitch_handle = param_find("MNT_MAX_PITCH"); + _mnt_min_pitch_handle = param_find("MNT_MIN_PITCH"); _mnt_range_yaw_handle = param_find("MNT_RANGE_YAW"); _mnt_mode_out_handle = param_find("MNT_MODE_OUT"); if (_mnt_range_roll_handle == PARAM_INVALID || - _mnt_range_pitch_handle == PARAM_INVALID || + _mnt_max_pitch_handle == PARAM_INVALID || + _mnt_min_pitch_handle == PARAM_INVALID || _mnt_range_yaw_handle == PARAM_INVALID || _mnt_mode_out_handle == PARAM_INVALID) { return false; @@ -76,7 +78,8 @@ void GZGimbal::Run() if (pollSetpoint()) { //TODO handle device flags publishJointCommand(_gimbal_roll_cmd_publisher, _roll_stp, _roll_rate_stp, _last_roll_stp, _roll_min, _roll_max, dt); - publishJointCommand(_gimbal_pitch_cmd_publisher, _pitch_stp, _pitch_rate_stp, _last_pitch_stp, _pitch_min, _pitch_max, + publishJointCommand(_gimbal_pitch_cmd_publisher, _pitch_stp, _pitch_rate_stp, _last_pitch_stp, _mnt_min_pitch, + _mnt_max_pitch, dt); publishJointCommand(_gimbal_yaw_cmd_publisher, _yaw_stp, _yaw_rate_stp, _last_yaw_stp, _yaw_min, _yaw_max, dt); } @@ -123,8 +126,10 @@ void GZGimbal::gimbalIMUCallback(const gz::msgs::IMU &IMU_data) void GZGimbal::updateParameters() { + param_get(_mnt_range_roll_handle, &_mnt_range_roll); - param_get(_mnt_range_pitch_handle, &_mnt_range_pitch); + param_get(_mnt_max_pitch_handle, &_mnt_max_pitch); + param_get(_mnt_min_pitch_handle, &_mnt_min_pitch); param_get(_mnt_range_yaw_handle, &_mnt_range_yaw); param_get(_mnt_mode_out_handle, &_mnt_mode_out); } @@ -151,10 +156,12 @@ bool GZGimbal::pollSetpoint() gimbal_controls_s msg; if (_gimbal_controls_sub.copy(&msg)) { - // map control inputs from [-1;1] to [min_angle; max_angle] using the range parameters + // map control inputs from [-1;1] to [min_angle; max_angle] _roll_stp = math::constrain(math::radians(msg.control[msg.INDEX_ROLL] * _mnt_range_roll / 2), _roll_min, _roll_max); - _pitch_stp = math::constrain(math::radians(msg.control[msg.INDEX_PITCH] * _mnt_range_pitch / 2), _pitch_min, - _pitch_max); + _pitch_stp = math::radians(math::constrain(_mnt_min_pitch + 0.5f * (msg.control[msg.INDEX_PITCH] + 1.f) * + (_mnt_max_pitch - _mnt_min_pitch), + _mnt_min_pitch, + _mnt_max_pitch)); _yaw_stp = math::constrain(math::radians(msg.control[msg.INDEX_YAW] * _mnt_range_yaw / 2), _yaw_min, _yaw_max); return true; @@ -196,8 +203,8 @@ void GZGimbal::publishDeviceInfo() device_info.custom_cap_flags = _custom_cap_flags; device_info.roll_min = _roll_min; device_info.roll_max = _roll_max; - device_info.pitch_min = _pitch_min; - device_info.pitch_max = _pitch_max; + device_info.pitch_min = _mnt_min_pitch; + device_info.pitch_max = _mnt_max_pitch; device_info.yaw_min = _yaw_min; device_info.yaw_max = _yaw_max; device_info.gimbal_device_id = _gimbal_device_id; diff --git a/src/modules/simulation/gz_bridge/GZGimbal.hpp b/src/modules/simulation/gz_bridge/GZGimbal.hpp index 897c43150f..a2a7965094 100644 --- a/src/modules/simulation/gz_bridge/GZGimbal.hpp +++ b/src/modules/simulation/gz_bridge/GZGimbal.hpp @@ -66,11 +66,13 @@ private: hrt_abstime _last_time_update; // Mount parameters - param_t _mnt_range_pitch_handle = PARAM_INVALID; + param_t _mnt_max_pitch_handle = PARAM_INVALID; + param_t _mnt_min_pitch_handle = PARAM_INVALID; param_t _mnt_range_roll_handle = PARAM_INVALID; param_t _mnt_range_yaw_handle = PARAM_INVALID; param_t _mnt_mode_out_handle = PARAM_INVALID; - float _mnt_range_pitch = 0.0f; + float _mnt_max_pitch = 0.0f; + float _mnt_min_pitch = 0.0f; float _mnt_range_roll = 0.0f; float _mnt_range_yaw = 0.0f; int32_t _mnt_mode_out = 0; @@ -109,8 +111,6 @@ private: // its mechanical limits. So the values below have to match the characteristics of the simulated gimbal const float _roll_min = -0.785398f; const float _roll_max = 0.785398f; - const float _pitch_min = -2.35619f; - const float _pitch_max = 0.785398f; const float _yaw_min = NAN; // infinite yaw const float _yaw_max = NAN; // infinite yaw