From 1d367611addf8ef09601f67caa1cd38bc35cfc95 Mon Sep 17 00:00:00 2001 From: haitomatic Date: Wed, 8 Nov 2023 11:49:18 +0000 Subject: [PATCH] update --- src/gazebo_mavlink_interface.cpp | 1 + src/mavlink_interface.cpp | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/src/gazebo_mavlink_interface.cpp b/src/gazebo_mavlink_interface.cpp index 50d98eb..31227b7 100644 --- a/src/gazebo_mavlink_interface.cpp +++ b/src/gazebo_mavlink_interface.cpp @@ -298,6 +298,7 @@ void GazeboMavlinkInterface::PoseCallback(const gz::msgs::Pose_V &_msg){ void GazeboMavlinkInterface::ImuCallback(const gz::msgs::IMU &_msg) { const std::lock_guard lock(last_imu_message_mutex_); last_imu_message_ = _msg; + std::cout << "ImuCallback with data " << last_imu_message.orientation().z() << std::endl; } void GazeboMavlinkInterface::BarometerCallback(const gz::msgs::FluidPressure &_msg) { diff --git a/src/mavlink_interface.cpp b/src/mavlink_interface.cpp index c9c213d..d8c92b6 100644 --- a/src/mavlink_interface.cpp +++ b/src/mavlink_interface.cpp @@ -483,7 +483,7 @@ void MavlinkInterface::handle_actuator_controls(mavlink_message_t *msg) for (int i = 0; i < input_reference_.size(); i++) { input_reference_[i] = controls.controls[i]; } - + std::cout << "input_reference_ : " << input_reference_[0] << std::endl; received_actuator_ = true; received_first_actuator_ = true; }