add try sending trajectory setpoints

src/px4_connection/src/heartbeat.cpp

@@ -41,9 +41,9 @@ private:
         // set message to enable attitude
         auto msg = px4_msgs::msg::OffboardControlMode();
         msg.position = false;
-        msg.velocity = false;
+        msg.velocity = true;
         msg.acceleration = false;
-        msg.attitude = true;
+        msg.attitude = false;
         msg.body_rate = false;
         msg.actuator = false;
 

src/px4_connection/src/px4_controller.cpp

@@ -52,34 +52,31 @@ private:
     bool has_swithed = false;
     int setpoint_count = 0;
     float thrust = 0.5;
+    bool ready_to_fly = setpoint_count == 20;
 
-    /**
+    void send_trajectory_setpoint()
-     * @brief Only the attitude is enabled, because that is how the drone will be controlled.
-     *
-     */
-    void send_setpoint()
     {
+        auto msg = px4_msgs::msg::TrajectorySetpoint();
 
-        setpoint_count++;
+        msg.velocity[0] = 2;
+        msg.velocity[1] = 0;
+        msg.velocity[2] = 0.5;
 
-        if (setpoint_count % 20 == 0 && thrust <= 1) {
+        msg.yaw = -3.14;
+        msg.yawspeed = 0;
+        msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
+
+        trajectory_setpoint_publisher->publush(msg);
+    }
+
+    void send_attitude_setpoint()
+    {
+        if (setpoint_count % 20 == 0 && thrust <= 1)
+        {
             thrust += 0.1;
             RCLCPP_INFO(this->get_logger(), "increasing thrust to %f", thrust);
         }
 
-        if (setpoint_count == 20)
-        {
-            // switch to offboard mode and arm
-
-            // set to offboard mode
-            this->publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_DO_SET_MODE, 1, 6);
-            RCLCPP_INFO(this->get_logger(), "Set to offboard mode");
-            // arm the drone
-            this->publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0, 0);
-
-            RCLCPP_INFO(this->get_logger(), "Arm command sent");
-            flying = true;
-        }
         // set message to enable attitude
         auto msg = px4_msgs::msg::VehicleAttitudeSetpoint();
         // result quaternion
@@ -99,7 +96,7 @@ private:
         {
             if (!has_swithed)
             {
-                RCLCPP_INFO(this->get_logger(),"changing to 0.5 thrust");
+                RCLCPP_INFO(this->get_logger(), "changing to 0.5 thrust");
                 has_swithed = true;
             }
 
@@ -132,6 +129,33 @@ private:
         vehicle_setpoint_publisher_->publish(msg);
     }
 
+    /**
+     * @brief Send setpoints to PX4. First, 20 setpoints will be sent before offboard mode will be engaged and the drone will be armed.
+     *
+     */
+    void send_setpoint()
+    {
+        // increase amount of setpoints sent
+        setpoint_count++;
+
+        // after 20 setpoints, send arm command to make drone actually fly
+        if (ready_to_fly)
+        {
+            // switch to offboard mode and arm
+
+            // set to offboard mode
+            this->publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_DO_SET_MODE, 1, 6);
+            RCLCPP_INFO(this->get_logger(), "Set to offboard mode");
+            // arm the drone
+            this->publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0, 0);
+
+            RCLCPP_INFO(this->get_logger(), "Arm command sent");
+            flying = true;
+        }
+
+        send_trajectory_setpoint();
+    }
+
     /**
      * @brief Publish vehicle commands
      * @param command   Command code (matches VehicleCommand and MAVLink MAV_CMD codes)