fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-27 07:03:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

GZ: add gimbal simulation (#23382)

Browse Source
This commit is contained in:
Stefano Colli
2024-12-18 12:15:03 +01:00
committed by GitHub
parent 4a73195007
commit 0561f6c9fc
17 changed files with 558 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/gimbal/gimbal.cpp
+9 -9
View File
@@ -113,7 +113,7 @@ static int gimbal_thread_main(int argc, char *argv[])
thread_data.input_objs[thread_data.input_objs_len++] = thread_data.test_input;
switch (params.mnt_mode_in) {
case 0:
case MNT_MODE_IN_AUTO:
// Automatic
// MAVLINK_V2 as well as RC input are supported together.
// Whichever signal is updated last, gets control, for RC there is a deadzone
@@ -123,19 +123,19 @@ static int gimbal_thread_main(int argc, char *argv[])
thread_data.input_objs[thread_data.input_objs_len++] = new InputRC(params);
break;
case 1: // RC only
case MNT_MODE_IN_RC: // RC only
thread_data.input_objs[thread_data.input_objs_len++] = new InputRC(params);
break;
case 2: // MAVLINK_ROI commands only (to be deprecated)
case MNT_MODE_IN_MAVLINK_ROI: // MAVLINK_ROI commands only (to be deprecated)
thread_data.input_objs[thread_data.input_objs_len++] = new InputMavlinkROI(params);
break;
case 3: // MAVLINK_DO_MOUNT commands only (to be deprecated)
case MNT_MODE_IN_MAVLINK_DO_MOUNT: // MAVLINK_DO_MOUNT commands only (to be deprecated)
thread_data.input_objs[thread_data.input_objs_len++] = new InputMavlinkCmdMount(params);
break;
case 4: //MAVLINK_V2
case MNT_MODE_IN_MAVLINK_V2: //MAVLINK_V2
thread_data.input_objs[thread_data.input_objs_len++] = new InputMavlinkGimbalV2(params);
break;
@@ -165,21 +165,21 @@ static int gimbal_thread_main(int argc, char *argv[])
}
switch (params.mnt_mode_out) {
case 0: //AUX
case MNT_MODE_OUT_AUX: //AUX
thread_data.output_obj = new OutputRC(params);
if (!thread_data.output_obj) { alloc_failed = true; }
break;
case 1: //MAVLink gimbal v1 protocol
case MNT_MODE_OUT_MAVLINK_V1: //MAVLink gimbal v1 protocol
thread_data.output_obj = new OutputMavlinkV1(params);
if (!thread_data.output_obj) { alloc_failed = true; }
break;
case 2: //MAVLink gimbal v2 protocol
case MNT_MODE_OUT_MAVLINK_V2: //MAVLink gimbal v2 protocol
thread_data.output_obj = new OutputMavlinkV2(params);
if (!thread_data.output_obj) { alloc_failed = true; }
@@ -276,7 +276,7 @@ static int gimbal_thread_main(int argc, char *argv[])
// Only publish the mount orientation if the mode is not mavlink v1 or v2
// If the gimbal speaks mavlink it publishes its own orientation.
if (params.mnt_mode_out != 1 && params.mnt_mode_out != 2) { // 1 = MAVLink v1, 2 = MAVLink v2
if (params.mnt_mode_out != MNT_MODE_OUT_MAVLINK_V1 && params.mnt_mode_out != MNT_MODE_OUT_MAVLINK_V2) {
thread_data.output_obj->publish();
}
src/modules/gimbal/gimbal_params.h
+15
View File
@@ -40,6 +40,21 @@
namespace gimbal
{
enum MntModeIn {
MNT_MODE_IN_DISABLED = -1,
MNT_MODE_IN_AUTO, // RC and MAVLink gimbal protocol v2
MNT_MODE_IN_RC,
MNT_MODE_IN_MAVLINK_ROI, // MAVLink gimbal protocol v1 (to be deprecated)
MNT_MODE_IN_MAVLINK_DO_MOUNT, // MAVLink gimbal protocol v1 (to be deprecated)
MNT_MODE_IN_MAVLINK_V2 // MAVLink gimbal protocol v2
};
enum MntModeOut {
MNT_MODE_OUT_AUX = 0,
MNT_MODE_OUT_MAVLINK_V1, // MAVLink gimbal protocol v1 (to be deprecated)
MNT_MODE_OUT_MAVLINK_V2 // MAVLink gimbal protocol v2
};
struct Parameters {
int32_t mnt_mode_in;
int32_t mnt_mode_out;
src/modules/gimbal/input_mavlink.cpp
+38 -17
View File
@@ -473,28 +473,49 @@ void InputMavlinkGimbalV2::_stream_gimbal_manager_status(const ControlData &cont
void InputMavlinkGimbalV2::_stream_gimbal_manager_information(const ControlData &control_data)
{
// TODO: Take gimbal_device_information into account.
gimbal_device_information_s gimbal_device_info;
gimbal_manager_information_s gimbal_manager_info;
gimbal_manager_info.timestamp = hrt_absolute_time();
if (_gimbal_device_information_sub.update(&gimbal_device_info) && _parameters.mnt_mode_out == MNT_MODE_OUT_MAVLINK_V2) {
gimbal_manager_information_s gimbal_manager_info;
gimbal_manager_info.timestamp = hrt_absolute_time();
gimbal_manager_info.cap_flags =
gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_NEUTRAL |
gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_ROLL_LOCK |
gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_PITCH_AXIS |
gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_PITCH_LOCK |
gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_AXIS |
gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_LOCK |
gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_CAN_POINT_LOCATION_GLOBAL;
gimbal_manager_info.cap_flags = gimbal_device_info.cap_flags;
gimbal_manager_info.pitch_max = M_PI_F / 2;
gimbal_manager_info.pitch_min = -M_PI_F / 2;
gimbal_manager_info.yaw_max = M_PI_F;
gimbal_manager_info.yaw_min = -M_PI_F;
gimbal_manager_info.roll_max = gimbal_device_info.roll_max;
gimbal_manager_info.roll_min = gimbal_device_info.roll_min;
gimbal_manager_info.pitch_max = gimbal_device_info.pitch_max;
gimbal_manager_info.pitch_min = gimbal_device_info.pitch_min;
gimbal_manager_info.yaw_max = gimbal_device_info.yaw_max;
gimbal_manager_info.yaw_min = gimbal_device_info.yaw_min;
gimbal_manager_info.gimbal_device_id = control_data.device_compid;
gimbal_manager_info.gimbal_device_id = control_data.device_compid;
_gimbal_manager_info_pub.publish(gimbal_manager_info);
} else if (_parameters.mnt_mode_out == MNT_MODE_OUT_AUX) {
// since we have a non-Mavlink gimbal device, the gimbal manager itself has to act as the gimbal device
gimbal_manager_information_s gimbal_manager_info;
gimbal_manager_info.timestamp = hrt_absolute_time();
gimbal_manager_info.cap_flags =
gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_NEUTRAL |
gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_ROLL_LOCK |
gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_PITCH_AXIS |
gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_PITCH_LOCK |
gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_AXIS |
gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_LOCK |
gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_CAN_POINT_LOCATION_GLOBAL;
gimbal_manager_info.pitch_max = _parameters.mnt_range_pitch;
gimbal_manager_info.pitch_min = -_parameters.mnt_range_pitch;
gimbal_manager_info.yaw_max = _parameters.mnt_range_yaw;
gimbal_manager_info.yaw_min = -_parameters.mnt_range_yaw;
gimbal_manager_info.gimbal_device_id = control_data.device_compid;
_gimbal_manager_info_pub.publish(gimbal_manager_info);
}
_gimbal_manager_info_pub.publish(gimbal_manager_info);
}
InputMavlinkGimbalV2::UpdateResult
src/modules/gimbal/input_mavlink.h
+2
View File
@@ -41,6 +41,7 @@
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/gimbal_device_attitude_status.h>
#include <uORB/topics/gimbal_device_information.h>
#include <uORB/topics/gimbal_manager_information.h>
#include <uORB/topics/gimbal_manager_status.h>
#include <uORB/topics/gimbal_manager_set_attitude.h>
@@ -125,6 +126,7 @@ private:
int _vehicle_command_sub = -1;
uORB::Subscription _gimbal_device_attitude_status_sub{ORB_ID(gimbal_device_attitude_status)};
uORB::Subscription _gimbal_device_information_sub{ORB_ID(gimbal_device_information)};
uORB::Publication<gimbal_manager_information_s> _gimbal_manager_info_pub{ORB_ID(gimbal_manager_information)};
uORB::Publication<gimbal_manager_status_s> _gimbal_manager_status_pub{ORB_ID(gimbal_manager_status)};
uint8_t _cur_roi_mode = vehicle_roi_s::ROI_NONE;
src/modules/gimbal/input_rc.cpp
+4 -2
View File
@@ -130,9 +130,11 @@ InputRC::UpdateResult InputRC::_read_control_data_from_subscription(ControlData
if (_parameters.mnt_rc_in_mode == 0) {
// We scale manual input from roll -180..180, pitch -90..90, yaw, -180..180 degrees.
matrix::Eulerf euler(new_aux_values[0] * math::radians(180.f),
// We use 179.99 instead of 180 so to avoid that the conversion between quaternions and Euler representation
// (when new_aux_value = 1) gives the equivalent angle (e.g., -180 instead of 180).
matrix::Eulerf euler(new_aux_values[0] * math::radians(179.99f),
new_aux_values[1] * math::radians(90.f),
new_aux_values[2] * math::radians(180.f));
new_aux_values[2] * math::radians(179.99f));
matrix::Quatf q(euler);
q.copyTo(control_data.type_data.angle.q);
src/modules/gimbal/output.cpp
+9 -2
View File
@@ -258,12 +258,19 @@ void OutputBase::_calculate_angle_output(const hrt_abstime &t)
_angle_outputs[i] -= euler_vehicle(i);
}
if (PX4_ISFINITE(_angle_outputs[i])) {
// bring angles into proper range [-pi, pi]
if (PX4_ISFINITE(_angle_outputs[i]) && _parameters.mnt_rc_in_mode == 0) {
// if we are in angle input mode, we bring angles into proper range [-pi, pi]
_angle_outputs[i] = matrix::wrap_pi(_angle_outputs[i]);
}
}
// constrain angle outputs to [-range/2, range/2]
_angle_outputs[0] = math::constrain(_angle_outputs[0], math::radians(-_parameters.mnt_range_roll / 2),
math::radians(_parameters.mnt_range_roll / 2));
_angle_outputs[1] = math::constrain(_angle_outputs[1], math::radians(-_parameters.mnt_range_pitch / 2),
math::radians(_parameters.mnt_range_pitch / 2));
_angle_outputs[2] = math::constrain(_angle_outputs[2], math::radians(-_parameters.mnt_range_yaw / 2),
math::radians(_parameters.mnt_range_yaw / 2));
// constrain pitch to [MNT_LND_P_MIN, MNT_LND_P_MAX] if landed
if (_landed) {