Skip to content

Commit b9adbc4

Browse files
perrrreperrrre
committed
gimbal: update gz gimbal
1 parent aa20d75 commit b9adbc4

File tree

3 files changed

+49
-40
lines changed

3 files changed

+49
-40
lines changed

‎ROMFS/px4fmu_common/init.d-posix/airframes/4019_gz_x500_gimbal‎

Lines changed: 6 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,9 @@ param set-default MNT_MAN_ROLL 1
1818
param set-default MNT_MAN_PITCH 2
1919
param set-default MNT_MAN_YAW 3
2020

21-
param set-default MNT_RANGE_ROLL 180
22-
param set-default MNT_RANGE_PITCH 180
23-
param set-default MNT_RANGE_YAW 720
21+
param set-default MNT_MAX_ROLL 45
22+
param set-default MNT_MIN_ROLL -45
23+
param set-default MNT_MAX_PITCH 45
24+
param set-default MNT_MIN_PITCH -135
25+
param set-default MNT_MAX_YAW 180
26+
param set-default MNT_MIN_YAW -180

‎src/modules/simulation/gz_bridge/GZGimbal.cpp‎

Lines changed: 31 additions & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -37,14 +37,20 @@ bool GZGimbal::init(const std::string &world_name, const std::string &model_name
3737
}
3838

3939
// Mount parameters
40-
_mnt_range_roll_handle = param_find("MNT_RANGE_ROLL");
41-
_mnt_range_pitch_handle = param_find("MNT_RANGE_PITCH");
42-
_mnt_range_yaw_handle = param_find("MNT_RANGE_YAW");
40+
_mnt_max_roll_handle = param_find("MNT_MAX_ROLL");
41+
_mnt_min_roll_handle = param_find("MNT_MIN_ROLL");
42+
_mnt_max_pitch_handle = param_find("MNT_MAX_PITCH");
43+
_mnt_min_pitch_handle = param_find("MNT_MIN_PITCH");
44+
_mnt_max_yaw_handle = param_find("MNT_MAX_YAW");
45+
_mnt_min_yaw_handle = param_find("MNT_MIN_YAW");
4346
_mnt_mode_out_handle = param_find("MNT_MODE_OUT");
4447

45-
if (_mnt_range_roll_handle == PARAM_INVALID ||
46-
_mnt_range_pitch_handle == PARAM_INVALID ||
47-
_mnt_range_yaw_handle == PARAM_INVALID ||
48+
if (_mnt_max_roll_handle == PARAM_INVALID ||
49+
_mnt_min_roll_handle == PARAM_INVALID ||
50+
_mnt_max_pitch_handle == PARAM_INVALID ||
51+
_mnt_min_pitch_handle == PARAM_INVALID ||
52+
_mnt_max_yaw_handle == PARAM_INVALID ||
53+
_mnt_min_yaw_handle == PARAM_INVALID ||
4854
_mnt_mode_out_handle == PARAM_INVALID) {
4955
return false;
5056
}
@@ -75,10 +81,10 @@ void GZGimbal::Run()
7581

7682
if (pollSetpoint()) {
7783
//TODO handle device flags
78-
publishJointCommand(_gimbal_roll_cmd_publisher, _roll_stp, _roll_rate_stp, _last_roll_stp, _roll_min, _roll_max, dt);
79-
publishJointCommand(_gimbal_pitch_cmd_publisher, _pitch_stp, _pitch_rate_stp, _last_pitch_stp, _pitch_min, _pitch_max,
84+
publishJointCommand(_gimbal_roll_cmd_publisher, _roll_stp, _roll_rate_stp, _last_roll_stp, _mnt_min_roll, _mnt_max_roll, dt);
85+
publishJointCommand(_gimbal_pitch_cmd_publisher, _pitch_stp, _pitch_rate_stp, _last_pitch_stp, _mnt_min_pitch, _mnt_max_pitch,
8086
dt);
81-
publishJointCommand(_gimbal_yaw_cmd_publisher, _yaw_stp, _yaw_rate_stp, _last_yaw_stp, _yaw_min, _yaw_max, dt);
87+
publishJointCommand(_gimbal_yaw_cmd_publisher, _yaw_stp, _yaw_rate_stp, _last_yaw_stp, _mnt_min_yaw, _mnt_max_yaw, dt);
8288
}
8389

8490
if (_mnt_mode_out == 2) {
@@ -123,9 +129,12 @@ void GZGimbal::gimbalIMUCallback(const gz::msgs::IMU &IMU_data)
123129

124130
void GZGimbal::updateParameters()
125131
{
126-
param_get(_mnt_range_roll_handle, &_mnt_range_roll);
127-
param_get(_mnt_range_pitch_handle, &_mnt_range_pitch);
128-
param_get(_mnt_range_yaw_handle, &_mnt_range_yaw);
132+
param_get(_mnt_max_roll_handle, &_mnt_max_roll);
133+
param_get(_mnt_min_roll_handle, &_mnt_min_roll);
134+
param_get(_mnt_max_pitch_handle, &_mnt_max_pitch);
135+
param_get(_mnt_min_pitch_handle, &_mnt_min_pitch);
136+
param_get(_mnt_max_yaw_handle, &_mnt_max_yaw);
137+
param_get(_mnt_min_yaw_handle, &_mnt_min_yaw);
129138
param_get(_mnt_mode_out_handle, &_mnt_mode_out);
130139
}
131140

@@ -152,10 +161,10 @@ bool GZGimbal::pollSetpoint()
152161

153162
if (_gimbal_controls_sub.copy(&msg)) {
154163
// map control inputs from [-1;1] to [min_angle; max_angle] using the range parameters
155-
_roll_stp = math::constrain(math::radians(msg.control[msg.INDEX_ROLL] * _mnt_range_roll / 2), _roll_min, _roll_max);
156-
_pitch_stp = math::constrain(math::radians(msg.control[msg.INDEX_PITCH] * _mnt_range_pitch / 2), _pitch_min,
157-
_pitch_max);
158-
_yaw_stp = math::constrain(math::radians(msg.control[msg.INDEX_YAW] * _mnt_range_yaw / 2), _yaw_min, _yaw_max);
164+
_roll_stp = math::constrain(math::radians(msg.control[msg.INDEX_ROLL] * (_mnt_max_roll - _mnt_min_roll) / 2), _mnt_min_roll, _mnt_max_roll);
165+
_pitch_stp = math::constrain(math::radians(msg.control[msg.INDEX_PITCH] * (_mnt_max_pitch - _mnt_min_pitch) / 2), _mnt_min_pitch,
166+
_mnt_max_pitch);
167+
_yaw_stp = math::constrain(math::radians(msg.control[msg.INDEX_YAW] * (_mnt_max_yaw - _mnt_min_yaw) / 2), _mnt_min_yaw, _mnt_max_yaw);
159168

160169
return true;
161170
}
@@ -194,12 +203,12 @@ void GZGimbal::publishDeviceInfo()
194203
device_info.uid = _uid;
195204
device_info.cap_flags = _cap_flags;
196205
device_info.custom_cap_flags = _custom_cap_flags;
197-
device_info.roll_min = _roll_min;
198-
device_info.roll_max = _roll_max;
199-
device_info.pitch_min = _pitch_min;
200-
device_info.pitch_max = _pitch_max;
201-
device_info.yaw_min = _yaw_min;
202-
device_info.yaw_max = _yaw_max;
206+
device_info.roll_min = _mnt_min_roll;
207+
device_info.roll_max = _mnt_max_roll;
208+
device_info.pitch_min = _mnt_min_pitch;
209+
device_info.pitch_max = _mnt_max_pitch;
210+
device_info.yaw_min = _mnt_max_yaw;
211+
device_info.yaw_max = _mnt_min_yaw;
203212
device_info.gimbal_device_id = _gimbal_device_id;
204213
device_info.timestamp = hrt_absolute_time();
205214

‎src/modules/simulation/gz_bridge/GZGimbal.hpp‎

Lines changed: 12 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -66,13 +66,19 @@ class GZGimbal : public px4::ScheduledWorkItem, public ModuleParams
6666
hrt_abstime _last_time_update;
6767

6868
// Mount parameters
69-
param_t _mnt_range_pitch_handle = PARAM_INVALID;
70-
param_t _mnt_range_roll_handle = PARAM_INVALID;
71-
param_t _mnt_range_yaw_handle = PARAM_INVALID;
69+
param_t _mnt_max_pitch_handle = PARAM_INVALID;
70+
param_t _mnt_min_pitch_handle = PARAM_INVALID;
71+
param_t _mnt_max_roll_handle = PARAM_INVALID;
72+
param_t _mnt_min_roll_handle = PARAM_INVALID;
73+
param_t _mnt_max_yaw_handle = PARAM_INVALID;
74+
param_t _mnt_min_yaw_handle = PARAM_INVALID;
7275
param_t _mnt_mode_out_handle = PARAM_INVALID;
73-
float _mnt_range_pitch = 0.0f;
74-
float _mnt_range_roll = 0.0f;
75-
float _mnt_range_yaw = 0.0f;
76+
float _mnt_max_pitch = 0.0f;
77+
float _mnt_min_pitch = 0.0f;
78+
float _mnt_max_roll = 0.0f;
79+
float _mnt_min_roll = 0.0f;
80+
float _mnt_max_yaw = 0.0f;
81+
float _mnt_min_yaw = 0.0f;
7682
int32_t _mnt_mode_out = 0;
7783

7884
matrix::Quatf _q_gimbal = matrix::Quatf(1.0f, 0.0f, 0.0f, 0.0f);
@@ -105,15 +111,6 @@ class GZGimbal : public px4::ScheduledWorkItem, public ModuleParams
105111
gimbal_device_information_s::GIMBAL_DEVICE_CAP_FLAGS_SUPPORTS_INFINITE_YAW;
106112
const uint16_t _custom_cap_flags = 0;
107113

108-
// This module act as the gimbal driver. In case of a Mavlink compatible gimbal, the driver is aware of
109-
// its mechanical limits. So the values below have to match the characteristics of the simulated gimbal
110-
const float _roll_min = -0.785398f;
111-
const float _roll_max = 0.785398f;
112-
const float _pitch_min = -2.35619f;
113-
const float _pitch_max = 0.785398f;
114-
const float _yaw_min = NAN; // infinite yaw
115-
const float _yaw_max = NAN; // infinite yaw
116-
117114
const uint8_t _gimbal_device_id = 1; // Gimbal is implemented by the same component: options are 1..6
118115
uint16_t _gimbal_device_flags = 0; // GIMBAL_DEVICE_FLAGS
119116

0 commit comments

Comments
 (0)