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

gimbal: correctly set gimbal_device_id

Browse Source 
When we use a gimbal connected via "RC", so PWM via the Aux channels, we
need to set the gimbal_device_id to 1 as per the protocol.

This was missing for GIMBAL_DEVICE_ATTITUDE_STATUS, so I added that, and
fixed the name of that variable while at it.
This commit is contained in:
Julian Oes 2024-12-18 14:05:55 +13:00 committed by Beat Küng
parent 9e9d352eb2
commit 98fde4cbac
7 changed files with 19 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
msg/GimbalDeviceAttitudeStatus.msg
View File

@ -16,5 +16,8 @@ float32 angular_velocity_y
float32 angular_velocity_z float32 angular_velocity_z
uint32 failure_flags uint32 failure_flags
float32 delta_yaw
float32 delta_yaw_velocity
uint8 gimbal_device_id
bool received_from_mavlink bool received_from_mavlink

2
msg/GimbalDeviceInformation.msg
View File

@ -33,4 +33,4 @@ float32 pitch_max # [rad]
float32 yaw_min # [rad] float32 yaw_min # [rad]
float32 yaw_max # [rad] float32 yaw_max # [rad]
uint8 gimbal_device_compid uint8 gimbal_device_id

8
src/modules/gimbal/output_mavlink.cpp
View File

@ -162,7 +162,7 @@ void OutputMavlinkV2::update(const ControlData &control_data, bool new_setpoints
_last_update = now; _last_update = now;
} }
gimbal_device_id = _gimbal_device_found ? _gimbal_device_compid : 0; gimbal_device_id = _gimbal_device_found ? _gimbal_device_id : 0;
_publish_gimbal_device_set_attitude(); _publish_gimbal_device_set_attitude();
} }
@ -191,7 +191,7 @@ void OutputMavlinkV2::_check_for_gimbal_device_information()
if (_gimbal_device_information_sub.update(&gimbal_device_information)) { if (_gimbal_device_information_sub.update(&gimbal_device_information)) {
_gimbal_device_found = true; _gimbal_device_found = true;
_gimbal_device_compid = gimbal_device_information.gimbal_device_compid; _gimbal_device_id = gimbal_device_information.gimbal_device_id;
} }
} }
@ -210,7 +210,7 @@ void OutputMavlinkV2::print_status() const
(double)_angle_velocity[2]); (double)_angle_velocity[2]);
if (_gimbal_device_found) { if (_gimbal_device_found) {
PX4_INFO_RAW(" gimbal device compid found: %d\n", _gimbal_device_compid); PX4_INFO_RAW(" gimbal device compid found: %d\n", _gimbal_device_id);
} else { } else {
PX4_INFO_RAW(" gimbal device compid not found\n"); PX4_INFO_RAW(" gimbal device compid not found\n");
@ -222,7 +222,7 @@ void OutputMavlinkV2::_publish_gimbal_device_set_attitude()
gimbal_device_set_attitude_s set_attitude{}; gimbal_device_set_attitude_s set_attitude{};
set_attitude.timestamp = hrt_absolute_time(); set_attitude.timestamp = hrt_absolute_time();
set_attitude.target_system = (uint8_t)_parameters.mav_sysid; set_attitude.target_system = (uint8_t)_parameters.mav_sysid;
set_attitude.target_component = _gimbal_device_compid; set_attitude.target_component = _gimbal_device_id;
set_attitude.angular_velocity_x = _angle_velocity[0]; set_attitude.angular_velocity_x = _angle_velocity[0];
set_attitude.angular_velocity_y = _angle_velocity[1]; set_attitude.angular_velocity_y = _angle_velocity[1];

2
src/modules/gimbal/output_mavlink.h
View File

@ -81,7 +81,7 @@ private:
uORB::Publication<gimbal_device_set_attitude_s> _gimbal_device_set_attitude_pub{ORB_ID(gimbal_device_set_attitude)}; uORB::Publication<gimbal_device_set_attitude_s> _gimbal_device_set_attitude_pub{ORB_ID(gimbal_device_set_attitude)};
uORB::Subscription _gimbal_device_information_sub{ORB_ID(gimbal_device_information)}; uORB::Subscription _gimbal_device_information_sub{ORB_ID(gimbal_device_information)};
uint8_t _gimbal_device_compid{0}; uint8_t _gimbal_device_id{0};
hrt_abstime _last_gimbal_device_checked{0}; hrt_abstime _last_gimbal_device_checked{0};
bool _gimbal_device_found {false}; bool _gimbal_device_found {false};
}; };

4
src/modules/gimbal/output_rc.cpp
View File

@ -115,6 +115,10 @@ void OutputRC::_stream_device_attitude_status()
q.copyTo(attitude_status.q); q.copyTo(attitude_status.q);
attitude_status.failure_flags = 0; attitude_status.failure_flags = 0;
// If the output is RC, then we signal this by referring to compid 1.
attitude_status.gimbal_device_id = 1;
_attitude_status_pub.publish(attitude_status); _attitude_status_pub.publish(attitude_status);
} }

2
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -3063,7 +3063,7 @@ MavlinkReceiver::handle_message_gimbal_device_information(mavlink_message_t *msg
gimbal_information.yaw_max = gimbal_device_info_msg.yaw_max; gimbal_information.yaw_max = gimbal_device_info_msg.yaw_max;
gimbal_information.yaw_min = gimbal_device_info_msg.yaw_min; gimbal_information.yaw_min = gimbal_device_info_msg.yaw_min;
gimbal_information.gimbal_device_compid = msg->compid; gimbal_information.gimbal_device_id = msg->compid;
_gimbal_device_information_pub.publish(gimbal_information); _gimbal_device_information_pub.publish(gimbal_information);
} }

5
src/modules/mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp
View File

@ -94,6 +94,11 @@ private:
msg.failure_flags = gimbal_device_attitude_status.failure_flags; msg.failure_flags = gimbal_device_attitude_status.failure_flags;
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);
return true; return true;