From 43f383897966743241c5b815839c710a63de0bdb Mon Sep 17 00:00:00 2001
From: Sem van der Hoeven <semmer99@gmail.com>
Date: Tue, 25 Apr 2023 16:39:15 +0200
Subject: [PATCH] add vehicle attitude setpoint publisher

---
 src/px4_connection/src/px4_controller.cpp | 38 +++++++----------------
 1 file changed, 11 insertions(+), 27 deletions(-)

diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp
index 172c5359..a6a00fa7 100644
--- a/src/px4_connection/src/px4_controller.cpp
+++ b/src/px4_connection/src/px4_controller.cpp
@@ -11,21 +11,20 @@ https://discuss.px4.io/t/cannot-arm-drone-with-companion-computer-arming-denied-
 
 #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 HeartBeat : public rclcpp::Node
+class PX4Controller : public rclcpp::Node
 {
 public:
-    HeartBeat() : Node("setpoint_sender")
+    PX4Controller() : Node("px4_controller")
     {
-        // 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(&HeartBeat::send_heartbeat, this));
-        start_time = this->get_clock()->now().seconds();
+        // create a publisher on the vehicle attitude setpoint topic
+        offboard_control_mode_publisher_ = this->create_publisher<px4_msgs::msg::VehicleAttitudeSetpoint>("/fmu/in/vehicle_attitude_setpoint", 10);
+        // create timer to send vehicle attitude setpoints every second
+        timer_ = this->create_wall_timer(1000ms, std::bind(&PX4Controller::send_setpoint, this));
     }
 
 private:
@@ -35,40 +34,25 @@ private:
  *        Only the attitude is enabled, because that is how the drone will be controlled.
  * 
  */
-    void send_heartbeat()
+    void send_setpoint()
     {
         // set message to enable attitude
-        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;
+        auto msg = px4_msgs::msg::VehicleAttitudeSetpoint();
 
         // get timestamp and publish message
         msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
-        offboard_control_mode_publisher_->publish(msg);
-        RCLCPP_INFO(this->get_logger(), "sent offboard control mode message!");
-
-        // check if 5 seconds have elapsed
-        if (this->get_clock()->now().seconds() - start_time > 5)
-        {
-            RCLCPP_INFO(this->get_logger(), "5 seconds elapsed!");
-        }
 
 
         
     }
-    rclcpp::Publisher<px4_msgs::msg::OffboardControlMode>::SharedPtr offboard_control_mode_publisher_;
+    rclcpp::Publisher<px4_msgs::msg::VehicleAttitudeSetpoint>::SharedPtr offboard_control_mode_publisher_;
     rclcpp::TimerBase::SharedPtr timer_;
-    double start_time;
 };
 
 int main(int argc, char *argv[])
 {
     rclcpp::init(argc, argv);
-    rclcpp::spin(std::make_shared<HeartBeat>());
+    rclcpp::spin(std::make_shared<PX4Controller>());
     rclcpp::shutdown();
     return 0;
 }