Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-09 22:08:56 +08:00
Code Issues Actions 3 Packages Projects Releases Wiki Activity

gimbal: update gz gimbal

Browse Source
This commit is contained in:
Pernilla
2026-01-07 09:06:23 +01:00
committed by Silvan Fuhrer
parent 0da6efa52d
commit 8c5c4a0504
3 changed files with 22 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/init.d-posix/airframes/4019_gz_x500_gimbal
+2 -1
View File
@@ -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
src/modules/simulation/gz_bridge/GZGimbal.cpp
+16 -9
View File
@@ -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;
src/modules/simulation/gz_bridge/GZGimbal.hpp
+4 -4
View File
@@ -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