add extra checks for when armed or not armed

src/px4_connection/src/px4_controller.cpp

@@ -62,6 +62,7 @@ private:
     double start_time_;
     bool has_sent_status = false;
     bool flying = false;
+    bool armed = false;
     bool has_swithed = false;
     int setpoint_count = 0;
     float thrust = 0.0;
@@ -71,6 +72,7 @@ private:
     float last_setpoint[3] = {0, 0, 0};
     float last_angle = 0;
     float last_thrust = 0;
+
     // result quaternion
     std::array<float, 4> q = {0, 0, 0, 0};
 
@@ -78,6 +80,8 @@ private:
         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)
+    {
+        if (armed)
         {
             last_setpoint[0] = degrees_to_radians(request->yaw);
             last_setpoint[1] = degrees_to_radians(request->pitch);
@@ -88,6 +92,12 @@ private:
             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);
 
             response->status = 0;
+        } else 
+        {
+            last_thrust = 0;
+            last_setpoint = {0, 0, 0};
+            response->status = 1;
+        }
     }
 
     /**
@@ -102,8 +112,14 @@ private:
         const std::shared_ptr<std_srvs::srv::Empty::Request> request,
         const std::shared_ptr<std_srvs::srv::Empty::Response> response)
     {
+        RCLCPP_INFO(this->get_logger(), "Got disarm request...");
+
+        if (armed)
+        {
+            armed = false;
             publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0, 0);
         }
+    }
 
     void send_trajectory_setpoint()
     {
@@ -203,6 +219,7 @@ private:
 
             RCLCPP_INFO(this->get_logger(), "Arm command sent");
             flying = true;
+            armed = true;
         }
 
         send_attitude_setpoint();