From f53385ae0a11d0d30bf86972a65a67d667866d01 Mon Sep 17 00:00:00 2001
From: Sem van der Hoeven <semmer99@gmail.com>
Date: Tue, 25 Apr 2023 14:08:42 +0200
Subject: [PATCH] add sending offboardcontrolmode message

---
 src/px4_msgs                             |  2 +-
 src/px4_ros_com                          |  2 +-
 src/send_setpoints/src/send_setpoint.cpp | 28 +++++++++++++++++++++---
 3 files changed, 27 insertions(+), 5 deletions(-)

diff --git a/src/px4_msgs b/src/px4_msgs
index 4db0a3f1..b64ef047 160000
--- a/src/px4_msgs
+++ b/src/px4_msgs
@@ -1 +1 @@
-Subproject commit 4db0a3f14ea81b9de7511d738f8ad9bd8ae5b3ad
+Subproject commit b64ef0475c1d44605688f4770899fe453d532be4
diff --git a/src/px4_ros_com b/src/px4_ros_com
index 0bcf68bc..1562ff30 160000
--- a/src/px4_ros_com
+++ b/src/px4_ros_com
@@ -1 +1 @@
-Subproject commit 0bcf68bcb635199adcd134e8932932054e863c0d
+Subproject commit 1562ff30d56b7ba26e4d2436724490f900cc2375
diff --git a/src/send_setpoints/src/send_setpoint.cpp b/src/send_setpoints/src/send_setpoint.cpp
index c3f7e382..6f96478d 100644
--- a/src/send_setpoints/src/send_setpoint.cpp
+++ b/src/send_setpoints/src/send_setpoint.cpp
@@ -7,25 +7,47 @@ https://discuss.px4.io/t/cannot-arm-drone-with-companion-computer-arming-denied-
 
 */
 
+#include <chrono>
+
 #include "rclcpp/rclcpp.hpp"
 
 #include <px4_msgs/msg/offboard_control_mode.hpp>
 #include <px4_msgs/msg/vehicle_attitude_setpoint.hpp>
 #include <px4_msgs/msg/timesync.hpp>
 
+using namespace std::chrono_literals;
+
 class SetpointSender : public rclcpp::Node
 {
 public:
     SetpointSender() : Node("setpoint_sender")
     {
-        offboard_control_mode_publisher_ = this->create_publisher<OffboardControlMode>("/fmu/in/offboard_control_mode", 10);
+        // create a publisher on the offboard control mode topic
+        offboard_control_mode_publisher_ = this->create_publisher<px4_msgs::msg::OffboardControlMode>("/fmu/in/offboard_control_mode", 10);
+        // create timer to send heartbeat messages (offboard control) every 100ms
+        timer_ = this->create_wall_timer(100ms, std::bind(&SetpointSender::send_heartbeat, this));
     }
 
 private:
 
+/**
+ * @brief Publish offboard control mode messages as a heartbeat.
+ *        Only the attitude is enabled, because that is how the drone will be controlled.
+ * 
+ */
     void send_heartbeat()
     {
-        
+        auto msg = px4_msgs::msg::OffboardControlMode();
+        msg.position = false;
+        msg.velocity = false;
+        msg.acceleration = false;
+        msg.attitude = true;
+        msg.body_rate = false;
+        msg.actuator = false;
+
+        msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
+        offboard_control_mode_publisher_->publish(msg);
+        RCLCPP_INFO(this->get_logger(), "sent offboard control mode message!");
     }
     rclcpp::Publisher<px4_msgs::msg::OffboardControlMode>::SharedPtr offboard_control_mode_publisher_;
     rclcpp::TimerBase::SharedPtr timer_;
@@ -34,7 +56,7 @@ private:
 int main(int argc, char *argv[])
 {
     rclcpp::init(argc, argv);
-    //  rclcpp::spin(std::make_shared<LidarReader>());
+    rclcpp::spin(std::make_shared<SetpointSender>());
     rclcpp::shutdown();
     return 0;
 }