dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mavlink: handle for GIMBAL_DEVICE_ATTITUDE_STATUS 

That way we can log it later.
This commit is contained in:
Julian Oes 2021-09-20 15:45:56 +02:00 committed by Daniel Agar
parent 5b07398b3e
commit 7e7a99b542
2 changed files with 31 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -304,6 +304,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
handle_message_request_event(msg);
break;
case MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS:
handle_message_gimbal_device_attitude_status(msg);
break;
default:
break;
}
@ -3045,6 +3049,30 @@ MavlinkReceiver::handle_message_gimbal_device_information(mavlink_message_t *msg
_gimbal_device_information_pub.publish(gimbal_information);
}
void
MavlinkReceiver::handle_message_gimbal_device_attitude_status(mavlink_message_t *msg)
{
mavlink_gimbal_device_attitude_status_t gimbal_device_attitude_status_msg;
mavlink_msg_gimbal_device_attitude_status_decode(msg, &gimbal_device_attitude_status_msg);
gimbal_device_attitude_status_s gimbal_attitude_status{};
gimbal_attitude_status.timestamp = static_cast<uint64_t>(gimbal_device_attitude_status_msg.time_boot_ms) * 1000;
gimbal_attitude_status.target_system = gimbal_device_attitude_status_msg.target_system;
gimbal_attitude_status.target_component = gimbal_device_attitude_status_msg.target_component;
gimbal_attitude_status.device_flags = gimbal_device_attitude_status_msg.flags;
for (unsigned i = 0; i < 4; ++i) {
gimbal_attitude_status.q[i] = gimbal_device_attitude_status_msg.q[i];
}
gimbal_attitude_status.angular_velocity_x = gimbal_device_attitude_status_msg.angular_velocity_x;
gimbal_attitude_status.angular_velocity_y = gimbal_device_attitude_status_msg.angular_velocity_y;
gimbal_attitude_status.angular_velocity_z = gimbal_device_attitude_status_msg.angular_velocity_z;
gimbal_attitude_status.failure_flags = gimbal_device_attitude_status_msg.failure_flags;
_gimbal_device_attitude_status_pub.publish(gimbal_attitude_status);
}
void
MavlinkReceiver::run()
{

3
src/modules/mavlink/mavlink_receiver.h
View File

@ -75,6 +75,7 @@
#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/gimbal_device_attitude_status.h>
#include <uORB/topics/gps_inject_data.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/input_rc.h>
@ -199,6 +200,7 @@ private:
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);
void handle_message_gimbal_device_attitude_status(mavlink_message_t *msg);
#if !defined(CONSTRAINED_FLASH)
void handle_message_debug(mavlink_message_t *msg);
@ -296,6 +298,7 @@ private:
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<gimbal_device_attitude_status_s> _gimbal_device_attitude_status_pub{ORB_ID(gimbal_device_attitude_status)};
uORB::Publication<irlock_report_s> _irlock_report_pub{ORB_ID(irlock_report)};
uORB::Publication<landing_target_pose_s> _landing_target_pose_pub{ORB_ID(landing_target_pose)};
uORB::Publication<log_message_s> _log_message_pub{ORB_ID(log_message)};