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

vmount/navigator/mavlink: gimbal v2 changes

Browse Source 
This is a collection of commits all having to do with changes in the
Mavlink gimbal v2 protocol as described in:
https://mavlink.io/en/services/gimbal_v2.html
This commit is contained in:
Julian Oes
2021-02-17 13:54:34 -05:00
committed by Daniel Agar
parent 422bac4bfd
commit e6b1775bb6
31 changed files with 790 additions and 285 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_messages.cpp
+106
View File
@@ -1482,6 +1482,111 @@ protected:
}
};
class MavlinkStreamAutopilotStateForGimbalDevice : public MavlinkStream
{
public:
const char *get_name() const override
{
return MavlinkStreamAutopilotStateForGimbalDevice::get_name_static();
}
static constexpr const char *get_name_static()
{
return "AUTOPILOT_STATE_FOR_GIMBAL_DEVICE";
}
static constexpr uint16_t get_id_static()
{
return MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE;
}
uint16_t get_id() override
{
return get_id_static();
}
static MavlinkStream *new_instance(Mavlink *mavlink)
{
return new MavlinkStreamAutopilotStateForGimbalDevice(mavlink);
}
unsigned get_size() override
{
return _att_sub.advertised() ? MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
}
private:
uORB::Subscription _att_sub{ORB_ID(vehicle_attitude)};
uORB::Subscription _lpos_sub{ORB_ID(vehicle_local_position)};
uORB::Subscription _att_sp_sub{ORB_ID(vehicle_attitude_setpoint)};
uORB::Subscription _est_sub{ORB_ID(estimator_status)};
uORB::Subscription _landed_sub{ORB_ID(vehicle_land_detected)};
/* do not allow top copying this class */
MavlinkStreamAutopilotStateForGimbalDevice(MavlinkStreamAutopilotStateForGimbalDevice &) = delete;
MavlinkStreamAutopilotStateForGimbalDevice &operator = (const MavlinkStreamAutopilotStateForGimbalDevice &) = delete;
protected:
explicit MavlinkStreamAutopilotStateForGimbalDevice(Mavlink *mavlink) : MavlinkStream(mavlink)
{}
bool send(const hrt_abstime t) override
{
if (_att_sub.advertised()) {
mavlink_autopilot_state_for_gimbal_device_t msg{};
{
vehicle_attitude_s att{};
_att_sub.copy(&att);
msg.time_boot_us = att.timestamp;
msg.q[0] = att.q[0];
msg.q[1] = att.q[1];
msg.q[2] = att.q[2];
msg.q[3] = att.q[3];
msg.q_estimated_delay_us = 0; // I don't know.
}
{
vehicle_local_position_s lpos{};
_lpos_sub.copy(&lpos);
msg.vx = lpos.vx;
msg.vy = lpos.vy;
msg.vz = lpos.vz;
msg.v_estimated_delay_us = 0; // I don't know.
}
{
vehicle_attitude_setpoint_s att_sp{};
_att_sp_sub.copy(&att_sp);
msg.feed_forward_angular_velocity_z = att_sp.yaw_sp_move_rate;
}
{
estimator_status_s est{};
_est_sub.copy(&est);
msg.estimator_status = est.solution_status_flags;
}
{
vehicle_land_detected_s land_detected{};
_landed_sub.copy(&land_detected);
// Ignore take-off and landing states for now.
msg.landed_state = land_detected.landed ? MAV_LANDED_STATE_ON_GROUND : MAV_LANDED_STATE_IN_AIR;
}
mavlink_msg_autopilot_state_for_gimbal_device_send_struct(_mavlink->get_channel(), &msg);
return true;
}
return false;
}
};
class MavlinkStreamSystemTime : public MavlinkStream
{
public:
@@ -3342,6 +3447,7 @@ static const StreamListItem streams_list[] = {
create_stream_list_item<MavlinkStreamGimbalDeviceAttitudeStatus>(),
create_stream_list_item<MavlinkStreamGimbalManagerInformation>(),
create_stream_list_item<MavlinkStreamGimbalManagerStatus>(),
create_stream_list_item<MavlinkStreamAutopilotStateForGimbalDevice>(),
create_stream_list_item<MavlinkStreamGimbalDeviceSetAttitude>(),
create_stream_list_item<MavlinkStreamHomePosition>(),
create_stream_list_item<MavlinkStreamServoOutputRaw<0> >(),
src/modules/mavlink/mavlink_mission.cpp
+6
View File
@@ -1475,6 +1475,9 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
case MAV_CMD_DO_DIGICAM_CONTROL:
case MAV_CMD_DO_MOUNT_CONFIGURE:
case MAV_CMD_DO_MOUNT_CONTROL:
case NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
case NAV_CMD_SET_CAMERA_FOCUS:
case NAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE:
case MAV_CMD_IMAGE_START_CAPTURE:
case MAV_CMD_IMAGE_STOP_CAPTURE:
case MAV_CMD_VIDEO_START_CAPTURE:
@@ -1574,12 +1577,15 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *
case NAV_CMD_DO_CONTROL_VIDEO:
case NAV_CMD_DO_MOUNT_CONFIGURE:
case NAV_CMD_DO_MOUNT_CONTROL:
case NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
case NAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE:
case NAV_CMD_DO_SET_ROI:
case NAV_CMD_DO_SET_CAM_TRIGG_DIST:
case NAV_CMD_OBLIQUE_SURVEY:
case NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL:
case NAV_CMD_SET_CAMERA_MODE:
case NAV_CMD_SET_CAMERA_ZOOM:
case NAV_CMD_SET_CAMERA_FOCUS:
case NAV_CMD_DO_VTOL_TRANSITION:
break;
src/modules/mavlink/mavlink_receiver.cpp
+34 -8
View File
@@ -283,6 +283,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
handle_message_gimbal_manager_set_attitude(msg);
break;
case MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL:
handle_message_gimbal_manager_set_manual_control(msg);
break;
case MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION:
handle_message_gimbal_device_information(msg);
break;
@@ -2946,16 +2950,39 @@ void MavlinkReceiver::CheckHeartbeats(const hrt_abstime &t, bool force)
}
}
void
MavlinkReceiver::handle_message_gimbal_manager_set_manual_control(mavlink_message_t *msg)
{
mavlink_gimbal_manager_set_manual_control_t set_manual_control_msg;
mavlink_msg_gimbal_manager_set_manual_control_decode(msg, &set_manual_control_msg);
gimbal_manager_set_manual_control_s set_manual_control{};
set_manual_control.timestamp = hrt_absolute_time();
set_manual_control.origin_sysid = msg->sysid;
set_manual_control.origin_compid = msg->compid;
set_manual_control.target_system = set_manual_control_msg.target_system;
set_manual_control.target_component = set_manual_control_msg.target_component;
set_manual_control.flags = set_manual_control_msg.flags;
set_manual_control.gimbal_device_id = set_manual_control_msg.gimbal_device_id;
set_manual_control.pitch = set_manual_control_msg.pitch;
set_manual_control.yaw = set_manual_control_msg.yaw;
set_manual_control.pitch_rate = set_manual_control_msg.pitch_rate;
set_manual_control.yaw_rate = set_manual_control_msg.yaw_rate;
_gimbal_manager_set_manual_control_pub.publish(set_manual_control);
}
void
MavlinkReceiver::handle_message_gimbal_manager_set_attitude(mavlink_message_t *msg)
{
mavlink_gimbal_manager_set_attitude_t set_attitude_msg;
mavlink_msg_gimbal_manager_set_attitude_decode(msg, &set_attitude_msg);
gimbal_manager_set_attitude_s gimbal_attitude{};
gimbal_attitude.timestamp = hrt_absolute_time();
gimbal_attitude.origin_sysid = msg->sysid;
gimbal_attitude.origin_compid = msg->compid;
gimbal_attitude.target_system = set_attitude_msg.target_system;
gimbal_attitude.target_component = set_attitude_msg.target_component;
gimbal_attitude.flags = set_attitude_msg.flags;
@@ -2969,7 +2996,6 @@ MavlinkReceiver::handle_message_gimbal_manager_set_attitude(mavlink_message_t *m
gimbal_attitude.angular_velocity_z = set_attitude_msg.angular_velocity_z;
_gimbal_manager_set_attitude_pub.publish(gimbal_attitude);
}
void
@@ -2994,13 +3020,13 @@ MavlinkReceiver::handle_message_gimbal_device_information(mavlink_message_t *msg
gimbal_information.firmware_version = gimbal_device_info_msg.firmware_version;
gimbal_information.capability_flags = gimbal_device_info_msg.cap_flags;
gimbal_information.tilt_max = gimbal_device_info_msg.tilt_max;
gimbal_information.tilt_min = gimbal_device_info_msg.tilt_min;
gimbal_information.tilt_rate_max = gimbal_device_info_msg.tilt_rate_max;
gimbal_information.pitch_max = gimbal_device_info_msg.pitch_max;
gimbal_information.pitch_min = gimbal_device_info_msg.pitch_min;
gimbal_information.pan_max = gimbal_device_info_msg.pan_max;
gimbal_information.pan_min = gimbal_device_info_msg.pan_min;
gimbal_information.pan_rate_max = gimbal_device_info_msg.pan_rate_max;
gimbal_information.yaw_max = gimbal_device_info_msg.yaw_max;
gimbal_information.yaw_min = gimbal_device_info_msg.yaw_min;
gimbal_information.gimbal_device_compid = msg->compid;
_gimbal_device_information_pub.publish(gimbal_information);
src/modules/mavlink/mavlink_receiver.h
+5
View File
@@ -68,6 +68,7 @@
#include <uORB/topics/follow_target.h>
#include <uORB/topics/generator_status.h>
#include <uORB/topics/gimbal_manager_set_attitude.h>
#include <uORB/topics/gimbal_manager_set_manual_control.h>
#include <uORB/topics/gimbal_device_information.h>
#include <uORB/topics/gps_inject_data.h>
#include <uORB/topics/home_position.h>
@@ -205,6 +206,7 @@ private:
void handle_message_vision_position_estimate(mavlink_message_t *msg);
void handle_message_onboard_computer_status(mavlink_message_t *msg);
void handle_message_gimbal_manager_set_attitude(mavlink_message_t *msg);
void handle_message_gimbal_manager_set_manual_control(mavlink_message_t *msg);
void handle_message_gimbal_device_information(mavlink_message_t *msg);
#if !defined(CONSTRAINED_FLASH)
@@ -357,6 +359,9 @@ private:
uORB::Publication<collision_report_s> _collision_report_pub{ORB_ID(collision_report)};
uORB::Publication<differential_pressure_s> _differential_pressure_pub{ORB_ID(differential_pressure)};
uORB::Publication<follow_target_s> _follow_target_pub{ORB_ID(follow_target)};
uORB::Publication<gimbal_manager_set_attitude_s> _gimbal_manager_set_attitude_pub{ORB_ID(gimbal_manager_set_attitude)};
uORB::Publication<gimbal_manager_set_manual_control_s> _gimbal_manager_set_manual_control_pub{ORB_ID(gimbal_manager_set_manual_control)};
uORB::Publication<gimbal_device_information_s> _gimbal_device_information_pub{ORB_ID(gimbal_device_information)};
uORB::Publication<irlock_report_s> _irlock_report_pub{ORB_ID(irlock_report)};
uORB::Publication<gimbal_manager_set_attitude_s> _gimbal_manager_set_attitude_pub{ORB_ID(gimbal_manager_set_attitude)};
uORB::Publication<gimbal_device_information_s> _gimbal_device_information_pub{ORB_ID(gimbal_device_information)};