From 5b4c6c149c2b7b28551fb8485872869d8bca817b Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 12 May 2023 16:19:27 +0200 Subject: [PATCH] change back control mode --- src/px4_connection/src/heartbeat.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/px4_connection/src/heartbeat.cpp b/src/px4_connection/src/heartbeat.cpp index eb662cf3..330a85ed 100644 --- a/src/px4_connection/src/heartbeat.cpp +++ b/src/px4_connection/src/heartbeat.cpp @@ -54,22 +54,23 @@ private: */ void send_heartbeat() { - // 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);