From b971c7989e337fdb3aef507f6c22b5841cefcbcf Mon Sep 17 00:00:00 2001
From: Sem van der Hoeven <semmer99@gmail.com>
Date: Tue, 2 May 2023 13:55:06 +0200
Subject: [PATCH] change to acceleration setpoints

---
 src/px4_connection/src/heartbeat.cpp      |  4 +-
 src/px4_connection/src/px4_controller.cpp | 49 ++++++++++++-----------
 2 files changed, 27 insertions(+), 26 deletions(-)

diff --git a/src/px4_connection/src/heartbeat.cpp b/src/px4_connection/src/heartbeat.cpp
index 52f6f87d..6b7b9198 100644
--- a/src/px4_connection/src/heartbeat.cpp
+++ b/src/px4_connection/src/heartbeat.cpp
@@ -42,8 +42,8 @@ private:
         auto msg = px4_msgs::msg::OffboardControlMode();
         msg.position = false;
         msg.velocity = false;
-        msg.acceleration = false;
-        msg.attitude = true;
+        msg.acceleration = true;
+        msg.attitude = false;
         msg.body_rate = false;
         msg.actuator = false;
 
diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp
index cccfd5ff..1c6af829 100644
--- a/src/px4_connection/src/px4_controller.cpp
+++ b/src/px4_connection/src/px4_controller.cpp
@@ -39,7 +39,7 @@ public:
         trajectory_setpoint_publisher = this->create_publisher<px4_msgs::msg::TrajectorySetpoint>("/fmu/in/trajectory_setpoint", 10);
         // offboard_control_mode_publisher_ = this->create_publisher<px4_msgs::msg::OffboardControlMode>("/fmu/in/offboard_control_mode", 10);
 
-        set_attitude_service_ = this->create_service<drone_services::srv::SetAttitude>("set_attitude", std::bind(&PX4Controller::set_setpoint, this, std::placeholders::_1,std::placeholders::_2,std::placeholders::_3));
+        set_attitude_service_ = this->create_service<drone_services::srv::SetVelocity>("set_attitude", std::bind(&PX4Controller::set_setpoint, this, std::placeholders::_1,std::placeholders::_2,std::placeholders::_3));
 
         // create timer to send vehicle attitude setpoints every second
         timer_ = this->create_wall_timer(100ms, std::bind(&PX4Controller::send_setpoint, this));
@@ -52,7 +52,7 @@ private:
     rclcpp::Publisher<px4_msgs::msg::VehicleAttitudeSetpoint>::SharedPtr vehicle_setpoint_publisher_;
     rclcpp::Publisher<px4_msgs::msg::TrajectorySetpoint>::SharedPtr trajectory_setpoint_publisher;
     rclcpp::Publisher<px4_msgs::msg::VehicleCommand>::SharedPtr vehicle_command_publisher_;
-    rclcpp::Service<drone_services::srv::SetAttitude>::SharedPtr set_attitude_service_;
+    rclcpp::Service<drone_services::srv::SetVelocity>::SharedPtr set_attitude_service_;
 
     // rclcpp::Publisher<px4_msgs::msg::OffboardControlMode>::SharedPtr offboard_control_mode_publisher_;
 
@@ -67,20 +67,21 @@ private:
     float cur_yaw = 0;
 
     float last_setpoint[3] = {0,0,0};
+    float last_angle = 0;
     float last_thrust = 0;
 
     void set_setpoint(
         const std::shared_ptr<rmw_request_id_t> request_header,
-        const std::shared_ptr<drone_services::srv::SetAttitude::Request> request,
-        const std::shared_ptr<drone_services::srv::SetAttitude::Response> response)
+        const std::shared_ptr<drone_services::srv::SetVelocity::Request> request,
+        const std::shared_ptr<drone_services::srv::SetVelocity::Response> response)
     {
-        last_setpoint[0] = degrees_to_radians(request->yaw);
-        last_setpoint[1] = degrees_to_radians(request->pitch);
-        last_setpoint[2] = degrees_to_radians(request->roll);
+        last_setpoint[0] = request->x_speed;
+        last_setpoint[1] = request->y_speed;
+        last_setpoint[2] = request->z_speed;
 
-        last_thrust = request->thrust;
+        last_angle = request->angle;
 
-        RCLCPP_INFO(this->get_logger(), "New setpoint: yaw:%f pitch:%f roll:%f thrust:%f",last_setpoint[0],last_setpoint[1],last_setpoint[2], last_thrust);
+        RCLCPP_INFO(this->get_logger(), "New setpoint: x:%f y:%f z:%f angle:%f",last_setpoint[0],last_setpoint[1],last_setpoint[2], last_angle);
 
         response->status = 0;
     }
@@ -91,9 +92,9 @@ private:
         auto msg = px4_msgs::msg::TrajectorySetpoint();
         if (this->get_clock()->now().seconds() - start_time_ < 10)
         {
-            msg.velocity[0] = 0;
-            msg.velocity[1] = 0;
-            msg.velocity[2] = D_SPEED(10);
+            msg.acceleration[0] = 0;
+            msg.acceleration[1] = 0;
+            msg.acceleration[2] = D_SPEED(10);
             msg.yawspeed = 0;
             msg.yaw = -3.14;
         }
@@ -105,9 +106,9 @@ private:
                 has_swithed = true;
             }
 
-            msg.velocity[0] = last_setpoint[0];
-            msg.velocity[1] = last_setpoint[1];
-            msg.velocity[2] = D_SPEED(last_setpoint[2]);
+            msg.acceleration[0] = last_setpoint[0];
+            msg.acceleration[1] = last_setpoint[1];
+            msg.acceleration[2] = D_SPEED(last_setpoint[2]);
             msg.yawspeed = 0.5;
         }
 
@@ -136,15 +137,15 @@ private:
             calculate_quaternion(q, last_setpoint[0], last_setpoint[1], last_setpoint[2]);
 
         }
-        else
-        {
-            if (flying)
-            {
-                publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0, 0);
-                flying = false;
-                RCLCPP_INFO(this->get_logger(), "Disarm command sent after 30 seconds");
-            }
-        }
+        // else
+        // {
+        //     if (flying)
+        //     {
+        //         publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0, 0);
+        //         flying = false;
+        //         RCLCPP_INFO(this->get_logger(), "Disarm command sent after 30 seconds");
+        //     }
+        // }
 
 
         // set quaternion