From 46d32066a294bed56741cb1cf06d3196a5ecfeb4 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 12 May 2023 15:46:22 +0200 Subject: [PATCH] bruh --- src/px4_connection/src/heartbeat.cpp | 9 ++++++++- src/px4_connection/src/px4_controller.cpp | 3 +++ 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/src/px4_connection/src/heartbeat.cpp b/src/px4_connection/src/heartbeat.cpp index 4e5795c5..eb662cf3 100644 --- a/src/px4_connection/src/heartbeat.cpp +++ b/src/px4_connection/src/heartbeat.cpp @@ -56,13 +56,20 @@ private: { // set message to enable attitude based on control mode variable auto msg = px4_msgs::msg::OffboardControlMode(); + /* msg.position = (this->control_mode >> CONTROL_POSITION_POS) & 1 ? true : false; msg.velocity = (this->control_mode >> CONTROL_VELOCITY_POS) & 1 ? true : false; msg.acceleration = (this->control_mode >> CONTROL_ACCELERATION_POS) & 1 ? true : false; msg.attitude = (this->control_mode >> CONTROL_ATTITUDE_POS) & 1 ? true : false; msg.body_rate = (this->control_mode >> CONTROL_BODY_RATE_POS) & 1 ? true : false; msg.actuator = (this->control_mode >> CONTROL_ACTUATOR_POS) & 1 ? true : false; - + */ + msg.position = false; + msg.velocity = false; + msg.acceleration = false; + msg.attitude = true; + msg.body_rate = false; + msg.actuator = false; // get timestamp and publish message msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; offboard_control_mode_publisher_->publish(msg); diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 39d25c57..98bcc1a0 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -269,6 +269,8 @@ private: armed = true; } + send_attitude_setpoint(); + /* RCLCPP_INFO(this->get_logger(), "current values: %f %f %f %f", last_setpoint[0], last_setpoint[1], last_setpoint[2], last_thrust); if (!flying) { @@ -293,6 +295,7 @@ private: send_position_setpoint(); } } + */ } void on_attitude_receive(const px4_msgs::msg::VehicleAttitude::SharedPtr msg)