mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
GZ: add gimbal simulation (#23382)
This commit is contained in:
Stefano Colli
GitHub
parent
4a73195007
commit
0561f6c9fc
@ -0,0 +1,23 @@
|
||||
#!/bin/sh
|
||||
#
|
||||
# @name Gazebo x500 gimbal
|
||||
#
|
||||
# @type Quadrotor
|
||||
#
|
||||
|
||||
PX4_SIM_MODEL=${PX4_SIM_MODEL:=x500_gimbal}
|
||||
|
||||
. ${R}etc/init.d-posix/airframes/4001_gz_x500
|
||||
|
||||
# Gimbal settings
|
||||
param set-default MNT_MODE_IN 4
|
||||
param set-default MNT_MODE_OUT 2
|
||||
param set-default MNT_RC_IN_MODE 1
|
||||
|
||||
param set-default MNT_MAN_ROLL 1
|
||||
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_RANGE_YAW 720
|
||||
@ -91,6 +91,7 @@ px4_add_romfs_files(
|
||||
4016_gz_x500_lidar_down
|
||||
4017_gz_x500_lidar_front
|
||||
4018_gz_quadtailsitter
|
||||
4019_gz_x500_gimbal
|
||||
|
||||
6011_gazebo-classic_typhoon_h480
|
||||
6011_gazebo-classic_typhoon_h480.post
|
||||
|
||||
@ -1 +1 @@
|
||||
Subproject commit 019f63e2d50763862b8f8d40cce77371b3260c52
|
||||
Subproject commit f48a17a4b30601ebb11e2f9e3e678fec8a73ebdc
|
||||
@ -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();
|
||||
}
|
||||
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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) {
|
||||
|
||||
@ -3063,7 +3063,12 @@ MavlinkReceiver::handle_message_gimbal_device_information(mavlink_message_t *msg
|
||||
gimbal_information.yaw_max = gimbal_device_info_msg.yaw_max;
|
||||
gimbal_information.yaw_min = gimbal_device_info_msg.yaw_min;
|
||||
|
||||
gimbal_information.gimbal_device_id = msg->compid;
|
||||
if (gimbal_device_info_msg.gimbal_device_id == 0) {
|
||||
gimbal_information.gimbal_device_id = msg->compid;
|
||||
|
||||
} else {
|
||||
gimbal_information.gimbal_device_id = gimbal_device_info_msg.gimbal_device_id;
|
||||
}
|
||||
|
||||
_gimbal_device_information_pub.publish(gimbal_information);
|
||||
}
|
||||
@ -3090,6 +3095,7 @@ MavlinkReceiver::handle_message_gimbal_device_attitude_status(mavlink_message_t
|
||||
gimbal_attitude_status.failure_flags = gimbal_device_attitude_status_msg.failure_flags;
|
||||
|
||||
gimbal_attitude_status.received_from_mavlink = true;
|
||||
gimbal_attitude_status.gimbal_device_id = gimbal_device_attitude_status_msg.gimbal_device_id;
|
||||
|
||||
_gimbal_device_attitude_status_pub.publish(gimbal_attitude_status);
|
||||
}
|
||||
|
||||
@ -74,32 +74,48 @@ private:
|
||||
return false;
|
||||
}
|
||||
|
||||
mavlink_gimbal_device_attitude_status_t msg{};
|
||||
if (gimbal_device_attitude_status.gimbal_device_id >= 1 && gimbal_device_attitude_status.gimbal_device_id <= 6) {
|
||||
// A non-MAVLink gimbal is signalled and addressed using 1 to 6 as the gimbal_device_id
|
||||
mavlink_gimbal_device_attitude_status_t msg{};
|
||||
|
||||
msg.target_system = gimbal_device_attitude_status.target_system;
|
||||
msg.target_component = gimbal_device_attitude_status.target_component;
|
||||
msg.target_system = gimbal_device_attitude_status.target_system;
|
||||
msg.target_component = gimbal_device_attitude_status.target_component;
|
||||
|
||||
msg.time_boot_ms = gimbal_device_attitude_status.timestamp / 1000;
|
||||
msg.time_boot_ms = gimbal_device_attitude_status.timestamp / 1000;
|
||||
|
||||
msg.flags = gimbal_device_attitude_status.device_flags;
|
||||
msg.flags = gimbal_device_attitude_status.device_flags;
|
||||
|
||||
msg.q[0] = gimbal_device_attitude_status.q[0];
|
||||
msg.q[1] = gimbal_device_attitude_status.q[1];
|
||||
msg.q[2] = gimbal_device_attitude_status.q[2];
|
||||
msg.q[3] = gimbal_device_attitude_status.q[3];
|
||||
msg.q[0] = gimbal_device_attitude_status.q[0];
|
||||
msg.q[1] = gimbal_device_attitude_status.q[1];
|
||||
msg.q[2] = gimbal_device_attitude_status.q[2];
|
||||
msg.q[3] = gimbal_device_attitude_status.q[3];
|
||||
|
||||
msg.angular_velocity_x = gimbal_device_attitude_status.angular_velocity_x;
|
||||
msg.angular_velocity_y = gimbal_device_attitude_status.angular_velocity_y;
|
||||
msg.angular_velocity_z = gimbal_device_attitude_status.angular_velocity_z;
|
||||
msg.angular_velocity_x = gimbal_device_attitude_status.angular_velocity_x;
|
||||
msg.angular_velocity_y = gimbal_device_attitude_status.angular_velocity_y;
|
||||
msg.angular_velocity_z = gimbal_device_attitude_status.angular_velocity_z;
|
||||
|
||||
msg.failure_flags = gimbal_device_attitude_status.failure_flags;
|
||||
msg.failure_flags = gimbal_device_attitude_status.failure_flags;
|
||||
msg.gimbal_device_id = gimbal_device_attitude_status.gimbal_device_id;
|
||||
|
||||
msg.delta_yaw = gimbal_device_attitude_status.delta_yaw;
|
||||
msg.delta_yaw_velocity = gimbal_device_attitude_status.delta_yaw_velocity;
|
||||
msg.delta_yaw = gimbal_device_attitude_status.delta_yaw;
|
||||
msg.delta_yaw_velocity = gimbal_device_attitude_status.delta_yaw_velocity;
|
||||
|
||||
msg.gimbal_device_id = gimbal_device_attitude_status.gimbal_device_id;
|
||||
mavlink_msg_gimbal_device_attitude_status_send_struct(_mavlink->get_channel(), &msg);
|
||||
|
||||
mavlink_msg_gimbal_device_attitude_status_send_struct(_mavlink->get_channel(), &msg);
|
||||
} else {
|
||||
// We have a Mavlink gimbal. We simulate its mavlink instance by spoofing the component ID
|
||||
mavlink_message_t message;
|
||||
mavlink_msg_gimbal_device_attitude_status_pack_chan(_mavlink->get_system_id(), MAV_COMP_ID_GIMBAL,
|
||||
_mavlink->get_channel(), &message,
|
||||
gimbal_device_attitude_status.target_system, gimbal_device_attitude_status.target_component,
|
||||
gimbal_device_attitude_status.timestamp / 1000,
|
||||
gimbal_device_attitude_status.device_flags, gimbal_device_attitude_status.q,
|
||||
gimbal_device_attitude_status.angular_velocity_x,
|
||||
gimbal_device_attitude_status.angular_velocity_y, gimbal_device_attitude_status.angular_velocity_z,
|
||||
gimbal_device_attitude_status.failure_flags,
|
||||
0, 0, 0);
|
||||
_mavlink->forward_message(&message, _mavlink);
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
@ -83,6 +83,7 @@ private:
|
||||
msg.pitch_max = gimbal_device_information.pitch_max;
|
||||
msg.yaw_min = gimbal_device_information.yaw_min;
|
||||
msg.yaw_max = gimbal_device_information.yaw_max;
|
||||
msg.gimbal_device_id = gimbal_device_information.gimbal_device_id;
|
||||
|
||||
mavlink_msg_gimbal_device_information_send_struct(_mavlink->get_channel(), &msg);
|
||||
|
||||
|
||||
@ -61,6 +61,8 @@ if(gz-transport_FOUND)
|
||||
GZMixingInterfaceServo.hpp
|
||||
GZMixingInterfaceWheel.cpp
|
||||
GZMixingInterfaceWheel.hpp
|
||||
GZGimbal.cpp
|
||||
GZGimbal.hpp
|
||||
DEPENDS
|
||||
mixer_module
|
||||
px4_work_queue
|
||||
|
||||
@ -268,6 +268,11 @@ int GZBridge::init()
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
if (!_gimbal.init(_world_name, _model_name)) {
|
||||
PX4_ERR("failed to init gimbal");
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
ScheduleNow();
|
||||
return OK;
|
||||
}
|
||||
@ -1005,6 +1010,7 @@ void GZBridge::Run()
|
||||
_mixing_interface_esc.stop();
|
||||
_mixing_interface_servo.stop();
|
||||
_mixing_interface_wheel.stop();
|
||||
_gimbal.stop();
|
||||
|
||||
exit_and_cleanup();
|
||||
return;
|
||||
@ -1021,6 +1027,7 @@ void GZBridge::Run()
|
||||
_mixing_interface_esc.updateParams();
|
||||
_mixing_interface_servo.updateParams();
|
||||
_mixing_interface_wheel.updateParams();
|
||||
_gimbal.updateParams();
|
||||
}
|
||||
|
||||
ScheduleDelayed(10_ms);
|
||||
|
||||
@ -36,6 +36,7 @@
|
||||
#include "GZMixingInterfaceESC.hpp"
|
||||
#include "GZMixingInterfaceServo.hpp"
|
||||
#include "GZMixingInterfaceWheel.hpp"
|
||||
#include "GZGimbal.hpp"
|
||||
|
||||
#include <px4_platform_common/atomic.h>
|
||||
#include <px4_platform_common/defines.h>
|
||||
@ -184,6 +185,7 @@ private:
|
||||
GZMixingInterfaceESC _mixing_interface_esc{_node, _node_mutex};
|
||||
GZMixingInterfaceServo _mixing_interface_servo{_node, _node_mutex};
|
||||
GZMixingInterfaceWheel _mixing_interface_wheel{_node, _node_mutex};
|
||||
GZGimbal _gimbal{_node, _node_mutex};
|
||||
|
||||
px4::atomic<uint64_t> _world_time_us{0};
|
||||
|
||||
|
||||
253
src/modules/simulation/gz_bridge/GZGimbal.cpp
Normal file
253
src/modules/simulation/gz_bridge/GZGimbal.cpp
Normal file
@ -0,0 +1,253 @@
|
||||
// #define DEBUG_BUILD
|
||||
#include "GZGimbal.hpp"
|
||||
|
||||
bool GZGimbal::init(const std::string &world_name, const std::string &model_name)
|
||||
{
|
||||
// Gazebo communication
|
||||
const std::string gimbal_roll_topic = "/model/" + model_name + "/command/gimbal_roll";
|
||||
_gimbal_roll_cmd_publisher = _node.Advertise<gz::msgs::Double>(gimbal_roll_topic);
|
||||
|
||||
if (!_gimbal_roll_cmd_publisher.Valid()) {
|
||||
PX4_ERR("failed to advertise %s", gimbal_roll_topic.c_str());
|
||||
return false;
|
||||
}
|
||||
|
||||
const std::string gimbal_pitch_topic = "/model/" + model_name + "/command/gimbal_pitch";
|
||||
_gimbal_pitch_cmd_publisher = _node.Advertise<gz::msgs::Double>(gimbal_pitch_topic);
|
||||
|
||||
if (!_gimbal_pitch_cmd_publisher.Valid()) {
|
||||
PX4_ERR("failed to advertise %s", gimbal_pitch_topic.c_str());
|
||||
return false;
|
||||
}
|
||||
|
||||
const std::string gimbal_yaw_topic = "/model/" + model_name + "/command/gimbal_yaw";
|
||||
_gimbal_yaw_cmd_publisher = _node.Advertise<gz::msgs::Double>(gimbal_yaw_topic);
|
||||
|
||||
if (!_gimbal_yaw_cmd_publisher.Valid()) {
|
||||
PX4_ERR("failed to advertise %s", gimbal_yaw_topic.c_str());
|
||||
return false;
|
||||
}
|
||||
|
||||
const std::string gimbal_imu_topic = "/world/" + world_name + "/model/" + model_name +
|
||||
"/link/camera_link/sensor/camera_imu/imu";
|
||||
|
||||
if (!_node.Subscribe(gimbal_imu_topic, &GZGimbal::gimbalIMUCallback, this)) {
|
||||
PX4_ERR("failed to subscribe to %s", gimbal_imu_topic.c_str());
|
||||
return false;
|
||||
}
|
||||
|
||||
// Mount parameters
|
||||
_mnt_range_roll_handle = param_find("MNT_RANGE_ROLL");
|
||||
_mnt_range_pitch_handle = param_find("MNT_RANGE_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_range_yaw_handle == PARAM_INVALID ||
|
||||
_mnt_mode_out_handle == PARAM_INVALID) {
|
||||
return false;
|
||||
}
|
||||
|
||||
updateParameters();
|
||||
|
||||
ScheduleOnInterval(200_ms); // @5Hz
|
||||
|
||||
// Schedule on vehicle command messages
|
||||
if (!_vehicle_command_sub.registerCallback()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void GZGimbal::Run()
|
||||
{
|
||||
pthread_mutex_lock(&_node_mutex);
|
||||
|
||||
const hrt_abstime now = hrt_absolute_time();
|
||||
const float dt = (now - _last_time_update) / 1e6f;
|
||||
_last_time_update = now;
|
||||
|
||||
updateParameters();
|
||||
|
||||
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,
|
||||
dt);
|
||||
publishJointCommand(_gimbal_yaw_cmd_publisher, _yaw_stp, _yaw_rate_stp, _last_yaw_stp, _yaw_min, _yaw_max, dt);
|
||||
}
|
||||
|
||||
if (_mnt_mode_out == 2) {
|
||||
// We have a Mavlink gimbal capable of sending messages
|
||||
publishDeviceInfo();
|
||||
publishDeviceAttitude();
|
||||
}
|
||||
|
||||
pthread_mutex_unlock(&_node_mutex);
|
||||
}
|
||||
|
||||
void GZGimbal::stop()
|
||||
{
|
||||
ScheduleClear();
|
||||
}
|
||||
|
||||
void GZGimbal::gimbalIMUCallback(const gz::msgs::IMU &IMU_data)
|
||||
{
|
||||
pthread_mutex_lock(&_node_mutex);
|
||||
|
||||
static const matrix::Quatf q_FLU_to_FRD = matrix::Quatf(0.0f, 1.0f, 0.0f, 0.0f);
|
||||
const matrix::Quatf q_gimbal_FLU = matrix::Quatf(IMU_data.orientation().w(),
|
||||
IMU_data.orientation().x(),
|
||||
IMU_data.orientation().y(),
|
||||
IMU_data.orientation().z());
|
||||
_q_gimbal = q_FLU_to_FRD * q_gimbal_FLU * q_FLU_to_FRD.inversed();
|
||||
|
||||
pthread_mutex_unlock(&_node_mutex);
|
||||
}
|
||||
|
||||
void GZGimbal::updateParameters()
|
||||
{
|
||||
param_get(_mnt_range_roll_handle, &_mnt_range_roll);
|
||||
param_get(_mnt_range_pitch_handle, &_mnt_range_pitch);
|
||||
param_get(_mnt_range_yaw_handle, &_mnt_range_yaw);
|
||||
param_get(_mnt_mode_out_handle, &_mnt_mode_out);
|
||||
}
|
||||
|
||||
bool GZGimbal::pollSetpoint()
|
||||
{
|
||||
if (_gimbal_device_set_attitude_sub.updated()) {
|
||||
gimbal_device_set_attitude_s msg;
|
||||
|
||||
if (_gimbal_device_set_attitude_sub.copy(&msg)) {
|
||||
const matrix::Eulerf gimbal_att_stp(matrix::Quatf(msg.q));
|
||||
_roll_stp = gimbal_att_stp.phi();
|
||||
_pitch_stp = gimbal_att_stp.theta();
|
||||
_yaw_stp = gimbal_att_stp.psi();
|
||||
_roll_rate_stp = msg.angular_velocity_x;
|
||||
_pitch_rate_stp = msg.angular_velocity_y;
|
||||
_yaw_rate_stp = msg.angular_velocity_z;
|
||||
_gimbal_device_flags = msg.flags;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
} else if (_gimbal_controls_sub.updated()) {
|
||||
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
|
||||
_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);
|
||||
_yaw_stp = math::constrain(math::radians(msg.control[msg.INDEX_YAW] * _mnt_range_yaw / 2), _yaw_min, _yaw_max);
|
||||
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
void GZGimbal::publishDeviceInfo()
|
||||
{
|
||||
if (_vehicle_command_sub.updated()) {
|
||||
vehicle_command_s cmd;
|
||||
_vehicle_command_sub.copy(&cmd);
|
||||
|
||||
if (cmd.command == vehicle_command_s::VEHICLE_CMD_REQUEST_MESSAGE &&
|
||||
(uint16_t)cmd.param1 == vehicle_command_s::VEHICLE_CMD_GIMBAL_DEVICE_INFORMATION) {
|
||||
// Acknowledge the command
|
||||
vehicle_command_ack_s command_ack{};
|
||||
|
||||
command_ack.command = cmd.command;
|
||||
command_ack.result = (uint8_t)vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
|
||||
command_ack.target_system = cmd.source_system;
|
||||
command_ack.target_component = cmd.source_component;
|
||||
command_ack.timestamp = hrt_absolute_time();
|
||||
|
||||
_vehicle_command_ack_pub.publish(command_ack);
|
||||
|
||||
// Send the requested message
|
||||
gimbal_device_information_s device_info{};
|
||||
|
||||
memcpy(device_info.vendor_name, _vendor_name, sizeof(_vendor_name));
|
||||
memcpy(device_info.model_name, _model_name, sizeof(_model_name));
|
||||
memcpy(device_info.custom_name, _custom_name, sizeof(_custom_name));
|
||||
device_info.firmware_version = _firmware_version;
|
||||
device_info.hardware_version = _hardware_version;
|
||||
device_info.uid = _uid;
|
||||
device_info.cap_flags = _cap_flags;
|
||||
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.yaw_min = _yaw_min;
|
||||
device_info.yaw_max = _yaw_max;
|
||||
device_info.gimbal_device_id = _gimbal_device_id;
|
||||
device_info.timestamp = hrt_absolute_time();
|
||||
|
||||
_gimbal_device_information_pub.publish(device_info);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void GZGimbal::publishDeviceAttitude()
|
||||
{
|
||||
// TODO handle flags
|
||||
|
||||
gimbal_device_attitude_status_s gimbal_att{};
|
||||
|
||||
gimbal_att.target_system = 0; // Broadcast
|
||||
gimbal_att.target_component = 0; // Broadcast
|
||||
gimbal_att.device_flags = 0;
|
||||
_q_gimbal.copyTo(gimbal_att.q);
|
||||
gimbal_att.angular_velocity_x = _gimbal_rate[0];
|
||||
gimbal_att.angular_velocity_y = _gimbal_rate[1];
|
||||
gimbal_att.angular_velocity_z = _gimbal_rate[2];
|
||||
gimbal_att.failure_flags = 0;
|
||||
gimbal_att.timestamp = hrt_absolute_time();
|
||||
|
||||
_gimbal_device_attitude_status_pub.publish(gimbal_att);
|
||||
}
|
||||
|
||||
void GZGimbal::publishJointCommand(gz::transport::Node::Publisher &publisher, const float att_stp, const float rate_stp,
|
||||
float &last_stp, const float min_stp, const float max_stp, const float dt)
|
||||
{
|
||||
gz::msgs::Double msg;
|
||||
|
||||
float new_stp = computeJointSetpoint(att_stp, rate_stp, last_stp, dt);
|
||||
new_stp = math::constrain(new_stp, min_stp, max_stp);
|
||||
last_stp = new_stp;
|
||||
msg.set_data(new_stp);
|
||||
|
||||
publisher.Publish(msg);
|
||||
}
|
||||
|
||||
float GZGimbal::computeJointSetpoint(const float att_stp, const float rate_stp, const float last_stp, const float dt)
|
||||
{
|
||||
|
||||
if (PX4_ISFINITE(rate_stp)) {
|
||||
const float rate_diff = dt * rate_stp;
|
||||
const float stp_from_rate = last_stp + rate_diff;
|
||||
|
||||
if (PX4_ISFINITE(att_stp)) {
|
||||
// We use the attitude rate setpoint but we constrain it by the desired angle
|
||||
return rate_diff > 0 ? math::min(att_stp, stp_from_rate) : math::max(att_stp, stp_from_rate);
|
||||
|
||||
} else {
|
||||
// The rate setpoint is valid while the angle one is not
|
||||
return stp_from_rate;
|
||||
}
|
||||
|
||||
} else if (PX4_ISFINITE(att_stp)) {
|
||||
// Only the angle setpoint is valid
|
||||
return att_stp;
|
||||
|
||||
} else {
|
||||
// Neither setpoint is valid so we steer the gimbal to the default position (roll = pitch = yaw = 0)
|
||||
return 0.0f;
|
||||
}
|
||||
}
|
||||
151
src/modules/simulation/gz_bridge/GZGimbal.hpp
Normal file
151
src/modules/simulation/gz_bridge/GZGimbal.hpp
Normal file
@ -0,0 +1,151 @@
|
||||
#pragma once
|
||||
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
||||
|
||||
#include <uORB/topics/gimbal_device_set_attitude.h>
|
||||
#include <uORB/topics/gimbal_device_attitude_status.h>
|
||||
#include <uORB/topics/gimbal_device_information.h>
|
||||
#include <uORB/topics/vehicle_command.h>
|
||||
#include <uORB/topics/vehicle_command_ack.h>
|
||||
#include <uORB/topics/gimbal_controls.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionCallback.hpp>
|
||||
#include <parameters/param.h>
|
||||
|
||||
#include <gz/msgs.hh>
|
||||
#include <gz/transport.hh>
|
||||
|
||||
#include <lib/matrix/matrix/Quaternion.hpp>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <math.h>
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
class GZGimbal : public px4::ScheduledWorkItem, public ModuleParams
|
||||
{
|
||||
public:
|
||||
GZGimbal(gz::transport::Node &node, pthread_mutex_t &node_mutex) :
|
||||
px4::ScheduledWorkItem(MODULE_NAME "-gimbal", px4::wq_configurations::rate_ctrl),
|
||||
ModuleParams(nullptr),
|
||||
_node(node),
|
||||
_node_mutex(node_mutex)
|
||||
{}
|
||||
|
||||
private:
|
||||
friend class GZBridge;
|
||||
|
||||
gz::transport::Node &_node;
|
||||
pthread_mutex_t &_node_mutex;
|
||||
|
||||
uORB::Subscription _gimbal_device_set_attitude_sub{ORB_ID(gimbal_device_set_attitude)};
|
||||
uORB::Subscription _gimbal_controls_sub{ORB_ID(gimbal_controls)};
|
||||
uORB::SubscriptionCallbackWorkItem _vehicle_command_sub{this, ORB_ID(vehicle_command)};
|
||||
|
||||
uORB::Publication<gimbal_device_attitude_status_s> _gimbal_device_attitude_status_pub{ORB_ID(gimbal_device_attitude_status)};
|
||||
uORB::Publication<gimbal_device_information_s> _gimbal_device_information_pub{ORB_ID(gimbal_device_information)};
|
||||
uORB::Publication<vehicle_command_ack_s> _vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};
|
||||
|
||||
gz::transport::Node::Publisher _gimbal_roll_cmd_publisher;
|
||||
gz::transport::Node::Publisher _gimbal_pitch_cmd_publisher;
|
||||
gz::transport::Node::Publisher _gimbal_yaw_cmd_publisher;
|
||||
|
||||
float _roll_stp = NAN;
|
||||
float _pitch_stp = NAN;
|
||||
float _yaw_stp = NAN;
|
||||
|
||||
float _last_roll_stp = 0.0f;
|
||||
float _last_pitch_stp = 0.0f;
|
||||
float _last_yaw_stp = 0.0f;
|
||||
|
||||
float _roll_rate_stp = NAN;
|
||||
float _pitch_rate_stp = NAN;
|
||||
float _yaw_rate_stp = NAN;
|
||||
|
||||
hrt_abstime _last_time_update;
|
||||
|
||||
// Mount parameters
|
||||
param_t _mnt_range_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_range_roll = 0.0f;
|
||||
float _mnt_range_yaw = 0.0f;
|
||||
int32_t _mnt_mode_out = 0;
|
||||
|
||||
matrix::Quatf _q_gimbal = matrix::Quatf(1.0f, 0.0f, 0.0f, 0.0f);
|
||||
float _gimbal_rate[3] = {NAN};
|
||||
|
||||
// Device information attributes
|
||||
const char _vendor_name[32] = "PX4";
|
||||
const char _model_name[32] = "Gazebo Gimbal";
|
||||
const char _custom_name[32] = "";
|
||||
const uint8_t _firmware_dev_version = 0;
|
||||
const uint8_t _firmware_patch_version = 0;
|
||||
const uint8_t _firmware_minor_version = 0;
|
||||
const uint8_t _firmware_major_version = 1;
|
||||
const uint32_t _firmware_version = (_firmware_dev_version & 0xff) << 24 | (_firmware_patch_version & 0xff) << 16 |
|
||||
(_firmware_minor_version & 0xff) << 8 | (_firmware_major_version & 0xff);
|
||||
const uint8_t _hardware_dev_version = 0;
|
||||
const uint8_t _hardware_patch_version = 0;
|
||||
const uint8_t _hardware_minor_version = 0;
|
||||
const uint8_t _hardware_major_version = 1;
|
||||
const uint32_t _hardware_version = (_hardware_dev_version & 0xff) << 24 | (_hardware_patch_version & 0xff) << 16 |
|
||||
(_hardware_minor_version & 0xff) << 8 | (_hardware_major_version & 0xff);
|
||||
const uint64_t _uid = 0x9a77a55b3c10971f ;
|
||||
const uint16_t _cap_flags = gimbal_device_information_s::GIMBAL_DEVICE_CAP_FLAGS_HAS_NEUTRAL |
|
||||
gimbal_device_information_s::GIMBAL_DEVICE_CAP_FLAGS_HAS_ROLL_AXIS |
|
||||
gimbal_device_information_s::GIMBAL_DEVICE_CAP_FLAGS_HAS_ROLL_FOLLOW |
|
||||
gimbal_device_information_s::GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_AXIS |
|
||||
gimbal_device_information_s::GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_FOLLOW |
|
||||
gimbal_device_information_s::GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_AXIS |
|
||||
gimbal_device_information_s::GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_FOLLOW |
|
||||
gimbal_device_information_s::GIMBAL_DEVICE_CAP_FLAGS_SUPPORTS_INFINITE_YAW;
|
||||
const uint16_t _custom_cap_flags = 0;
|
||||
|
||||
// This module act as the gimbal driver. In case of a Mavlink compatible gimbal, the driver is aware of
|
||||
// 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
|
||||
|
||||
const uint8_t _gimbal_device_id = 154; // TODO the implementation differs from the protocol
|
||||
uint16_t _gimbal_device_flags = 0; // GIMBAL_DEVICE_FLAGS
|
||||
|
||||
bool init(const std::string &world_name, const std::string &model_name);
|
||||
void Run() override;
|
||||
void stop();
|
||||
void gimbalIMUCallback(const gz::msgs::IMU &IMU_data);
|
||||
void updateParameters();
|
||||
/// @brief Poll for new gimbal setpoints either from mavlink gimbal v2 protocol (gimbal_device_set_attitude topic) or from RC inputs (gimbal_controls topic).
|
||||
/// @return true if a new setpoint has been requested; false otherwise.
|
||||
bool pollSetpoint();
|
||||
/// @brief Respond to the gimbal manager when it requests GIMBAL_DEVICE_INFORMATION messages.
|
||||
void publishDeviceInfo();
|
||||
/// @brief Broadcast gimbal device attitude status message.
|
||||
void publishDeviceAttitude();
|
||||
/// @brief Compute joint position setpoint taking into account both desired position and velocity. Then publish the command using the specified gazebo node.
|
||||
/// @param publisher Gazebo node that will publish the setpoint
|
||||
/// @param att_stp desired joint attitude [rad]
|
||||
/// @param rate_stp desired joint attitude rate [rad/s]
|
||||
/// @param last_stp last joint attitude setpoint [rad]
|
||||
/// @param min_stp minimum joint attitude [rad]
|
||||
/// @param max_stp maximum joint attitude [rad]
|
||||
/// @param dt time interval since the last computation [s]
|
||||
static void publishJointCommand(gz::transport::Node::Publisher &publisher, const float att_stp, const float rate_stp,
|
||||
float &last_stp,
|
||||
const float min_stp, const float max_stp, const float dt);
|
||||
/// @brief Compute joint position setpoint taking into account both desired position and velocity.
|
||||
/// @param att_stp desired joint attitude [rad]
|
||||
/// @param rate_stp desired joint attitude rate [rad/s]
|
||||
/// @param last_stp last joint attitude setpoint [rad]
|
||||
/// @param dt time interval since the last computation [s]
|
||||
/// @return new joint setpoint
|
||||
static float computeJointSetpoint(const float att_stp, const float rate_stp, const float last_stp, const float dt);
|
||||
};
|
||||
Loading…
x
Reference in New Issue
Block a user