diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp
index 250ba29a..64ed0358 100644
--- a/src/px4_connection/src/px4_controller.cpp
+++ b/src/px4_connection/src/px4_controller.cpp
@@ -13,6 +13,8 @@ https://discuss.px4.io/t/cannot-arm-drone-with-companion-computer-arming-denied-
 
 #include <px4_msgs/msg/vehicle_attitude_setpoint.hpp>
 #include <px4_msgs/msg/timesync.hpp>
+#include <px4_msgs/msg/vehicle_command.hpp>
+#include <px4_msgs/msg/vehicle_control_mode.hpp>
 
 using namespace std::chrono_literals;
 
@@ -23,17 +25,29 @@ public:
     {
         // create a publisher on the vehicle attitude setpoint topic
         vehicle_setpoint_publisher_ = this->create_publisher<px4_msgs::msg::VehicleAttitudeSetpoint>("/fmu/in/vehicle_attitude_setpoint", 10);
+        vehicle_command_publisher_ = this->create_publisher<px4::msgs::msg::VehicleCommand>("/fmu/in/vehicle_command", 10);
+
+        // 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
+        publish_vehicle_command(VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0);
+
+        RCLCPP_INFO(this->get_logger(), "Arm command sent");
+
         // create timer to send vehicle attitude setpoints every second
         timer_ = this->create_wall_timer(1000ms, std::bind(&PX4Controller::send_setpoint, this));
     }
 
 private:
+    rclcpp::Publisher<px4_msgs::msg::VehicleAttitudeSetpoint>::SharedPtr vehicle_setpoint_publisher_;
+    rclcpp::Publisher<px4_msgs::msg::VehicleCommand>::SharedPtr vehicle_command_publisher_;
+    rclcpp::TimerBase::SharedPtr timer_;
 
-/**
- * @brief Publish offboard control mode messages as a heartbeat.
- *        Only the attitude is enabled, because that is how the drone will be controlled.
- * 
- */
+    /**
+     * @brief Only the attitude is enabled, because that is how the drone will be controlled.
+     *
+     */
     void send_setpoint()
     {
         // set message to enable attitude
@@ -41,7 +55,7 @@ private:
 
         // get timestamp and publish message
 
-        //https://github.com/PX4/px4_msgs/blob/main/msg/VehicleAttitudeSetpoint.msg
+        // https://github.com/PX4/px4_msgs/blob/main/msg/VehicleAttitudeSetpoint.msg
         msg.roll_body = 1;
         msg.pitch_body = 1;
         msg.yaw_body = 1;
@@ -64,8 +78,27 @@ private:
 
         RCLCPP_INFO(this->get_logger(), "published message");
     }
-    rclcpp::Publisher<px4_msgs::msg::VehicleAttitudeSetpoint>::SharedPtr vehicle_setpoint_publisher_;
-    rclcpp::TimerBase::SharedPtr timer_;
+
+    /**
+     * @brief Publish vehicle commands
+     * @param command   Command code (matches VehicleCommand and MAVLink MAV_CMD codes)
+     * @param param1    Command parameter 1
+     * @param param2    Command parameter 2
+     */
+    void publish_vehicle_command(uint16_t command, float param1, float param2)
+    {
+        px4_msgs::msg::VehicleCommand msg{};
+        msg.param1 = param1;
+        msg.param2 = param2;
+        msg.command = command;
+        msg.target_system = 1;
+        msg.target_component = 1;
+        msg.source_system = 1;
+        msg.source_component = 1;
+        msg.from_external = true;
+        msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
+        vehicle_command_publisher_->publish(msg);
+    }
 };
 
 int main(int argc, char *argv[])