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

vmount/mavlink: update gimbal information message

Browse Source 
We should be using gimbal_manager_information and not
gimbal_device_information. Plus, this updates the fields and flags
according to the MAVLink changes.
This commit is contained in:
Julian Oes
2021-02-17 13:54:34 -05:00
committed by Daniel Agar
parent cb3ad39406
commit b0d7d19bab
6 changed files with 74 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_messages.cpp
+12 -15
View File
@@ -70,8 +70,8 @@
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/geofence_result.h>
#include <uORB/topics/gimbal_device_attitude_status.h>
#include <uORB/topics/gimbal_device_information.h>
#include <uORB/topics/gimbal_device_set_attitude.h>
#include <uORB/topics/gimbal_manager_information.h>
#include <uORB/topics/gimbal_manager_status.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/position_setpoint_triplet.h>
@@ -2078,12 +2078,12 @@ public:
unsigned get_size() override
{
return _gimbal_device_information_sub.advertised() ? (MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_LEN +
return _gimbal_manager_information_sub.advertised() ? (MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_LEN +
MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
}
private:
uORB::Subscription _gimbal_device_information_sub{ORB_ID(gimbal_device_information)};
uORB::Subscription _gimbal_manager_information_sub{ORB_ID(gimbal_manager_information)};
/* do not allow top copying this class */
MavlinkStreamGimbalManagerInformation(MavlinkStreamGimbalManagerInformation &) = delete;
@@ -2095,22 +2095,21 @@ protected:
bool send(const hrt_abstime t) override
{
gimbal_device_information_s gimbal_device_information;
gimbal_manager_information_s gimbal_manager_information;
if (_gimbal_device_information_sub.advertised() && _gimbal_device_information_sub.copy(&gimbal_device_information)) {
if (_gimbal_manager_information_sub.advertised() && _gimbal_manager_information_sub.copy(&gimbal_device_information)) {
// send out gimbal_manager_info with info from gimbal_device_information
mavlink_gimbal_manager_information_t msg{};
msg.time_boot_ms = gimbal_device_information.timestamp / 1000;
msg.gimbal_device_id = 0;
msg.cap_flags = gimbal_device_information.capability_flags;
msg.cap_flags = gimbal_device_information.cap_flags;
msg.tilt_max = gimbal_device_information.tilt_max;
msg.tilt_min = gimbal_device_information.tilt_min;
msg.tilt_rate_max = gimbal_device_information.tilt_rate_max;
msg.pan_max = gimbal_device_information.pan_max;
msg.pan_min = gimbal_device_information.pan_min;
msg.pan_rate_max = gimbal_device_information.pan_rate_max;
msg.roll_min = gimbal_device_information.roll_min;
msg.roll_max = gimbal_device_information.roll_max;
msg.pitch_min = gimbal_device_information.pitch_min;
msg.pitch_max = gimbal_device_information.pitch_max;
msg.yaw_min = gimbal_device_information.yaw_min;
msg.yaw_max = gimbal_device_information.yaw_max;
mavlink_msg_gimbal_manager_information_send_struct(_mavlink->get_channel(), &msg);
@@ -2172,7 +2171,6 @@ protected:
gimbal_manager_status_s gimbal_manager_status;
if (_gimbal_manager_status_sub.advertised() && _gimbal_manager_status_sub.copy(&gimbal_manager_status)) {
// send out gimbal_manager_info with info from gimbal_device_information
mavlink_gimbal_manager_status_t msg{};
msg.time_boot_ms = gimbal_manager_status.timestamp / 1000;
msg.gimbal_device_id = gimbal_manager_status.gimbal_device_id;
@@ -2240,7 +2238,6 @@ protected:
gimbal_device_set_attitude_s gimbal_device_set_attitude;
if (_gimbal_device_set_attitude_sub.advertised() && _gimbal_device_set_attitude_sub.copy(&gimbal_device_set_attitude)) {
// send out gimbal_manager_info with info from gimbal_device_information
mavlink_gimbal_device_set_attitude_t msg{};
msg.flags = gimbal_device_set_attitude.flags;
msg.target_system = gimbal_device_set_attitude.target_system;
src/modules/mavlink/mavlink_receiver.cpp
+8 -2
View File
@@ -3012,13 +3012,20 @@ MavlinkReceiver::handle_message_gimbal_device_information(mavlink_message_t *msg
"vendor_name length doesn't match");
static_assert(sizeof(gimbal_information.model_name) == sizeof(gimbal_device_info_msg.model_name),
"model_name length doesn't match");
static_assert(sizeof(gimbal_information.custom_name) == sizeof(gimbal_device_info_msg.custom_name),
"custom_name length doesn't match");
memcpy(gimbal_information.vendor_name, gimbal_device_info_msg.vendor_name, sizeof(gimbal_information.vendor_name));
memcpy(gimbal_information.model_name, gimbal_device_info_msg.model_name, sizeof(gimbal_information.model_name));
memcpy(gimbal_information.custom_name, gimbal_device_info_msg.custom_name, sizeof(gimbal_information.custom_name));
gimbal_device_info_msg.vendor_name[sizeof(gimbal_device_info_msg.vendor_name) - 1] = '\0';
gimbal_device_info_msg.model_name[sizeof(gimbal_device_info_msg.model_name) - 1] = '\0';
gimbal_device_info_msg.custom_name[sizeof(gimbal_device_info_msg.custom_name) - 1] = '\0';
gimbal_information.firmware_version = gimbal_device_info_msg.firmware_version;
gimbal_information.capability_flags = gimbal_device_info_msg.cap_flags;
gimbal_information.hardware_version = gimbal_device_info_msg.hardware_version;
gimbal_information.cap_flags = gimbal_device_info_msg.cap_flags;
gimbal_information.custom_cap_flags = gimbal_device_info_msg.custom_cap_flags;
gimbal_information.uid = gimbal_device_info_msg.uid;
gimbal_information.pitch_max = gimbal_device_info_msg.pitch_max;
gimbal_information.pitch_min = gimbal_device_info_msg.pitch_min;
@@ -3029,7 +3036,6 @@ MavlinkReceiver::handle_message_gimbal_device_information(mavlink_message_t *msg
gimbal_information.gimbal_device_compid = msg->compid;
_gimbal_device_information_pub.publish(gimbal_information);
}
/**