change heartbeat hz to 100

src/drone_controls/src/PositionChanger.cpp

@@ -119,7 +119,7 @@ public:
         auto status = future.wait_for(1s);
         if (status == std::future_status::ready)
         {
-            RCLCPP_INFO(this->get_logger(), "Set failsafe enabled: %d, message: ", future.get()->enabled, future.get()->message);
+            RCLCPP_INFO(this->get_logger(), "Set failsafe enabled: %d, message: %s", future.get()->enabled, future.get()->message);
         }
         else
         {
@@ -143,6 +143,11 @@ public:
         auto trajectory_response = this->trajectory_client->async_send_request(this->trajectory_request, std::bind(&PositionChanger::trajectory_message_callback, this, std::placeholders::_1));
     }
 
+    /**
+     * @brief Enables the failsafe with the specified message
+     * 
+     * @param message the message indicating the cause of the failsafe
+     */
     void enable_failsafe(std::string message)
     {
         this->failsafe_enabled = true;

src/failsafe/failsafe/failsafe.py

@@ -9,7 +9,6 @@ class FailSafe(Node):
         self.failsafe_enabled = False
         self.failsafe_msg = ""
         self.get_logger().info("Failsafe node started")
-        # create service on /drone/failsafe topic
         self.service = self.create_service(
             EnableFailsafe, "/drone/enable_failsafe", self.failsafe_callback)
         self.failsafe_publisher = self.create_publisher(FailsafeMsg, "/drone/failsafe", 10)
@@ -38,6 +37,5 @@ def main(args=None):
     failsafe_node.destroy_node()
     rclpy.shutdown()
 
-
 if __name__ == '__main__':
     main()

src/px4_connection/src/heartbeat.cpp

@@ -29,7 +29,7 @@ public:
         // 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));
+        timer_ = this->create_wall_timer(10ms, std::bind(&HeartBeat::send_heartbeat, this));
         start_time = this->get_clock()->now().seconds();
         RCLCPP_INFO(this->get_logger(), "done initializing at %d seconds. Sending heartbeat...", start_time);
     }

src/px4_connection/src/px4_controller.cpp

@@ -108,8 +108,7 @@ private:
     float rho_0 = 0;                                // initial rho of polar coordinate
     float theta_0 = 0;                              // initial theta of polar coordinate
     float p0_z = -0.0;                        // initial height
-    float des_x = p0_x, des_y = p0_y, des_z = p0_z; // desired position
+    float des_x = 0.0, des_y = 0.0, des_z = p0_z; // desired position
-    float dot_des_x = 0.0, dot_des_y = 0.0;         // desired velocity
     float gamma = M_PI_4;                           // desired heading direction
 
     float local_x = 0;  // local position x
@@ -258,7 +257,11 @@ private:
         const std::shared_ptr<drone_services::srv::ArmDrone::Response> response)
     {
         RCLCPP_INFO(this->get_logger(), "Got arm request...");
-
+        if (this->failsafe_enabled)
+        {
+            response->success = false;
+            return;
+        }
         if (!armed)
         {
             this->publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_DO_SET_MODE, 1, 6);
@@ -343,7 +346,6 @@ private:
         RCLCPP_INFO(this->get_logger(), "Sending position setpoint: %f %f %f", des_x, des_y, des_z);
         RCLCPP_INFO(this->get_logger(), "local position: %f %f", local_x, local_y);
         msg.position = {local_x, local_y, des_z};
-        msg.velocity = {dot_des_x, dot_des_y, 0.0};
         msg.yaw = gamma; //-3.14; // [-PI:PI]
 
         msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
@@ -391,13 +393,6 @@ private:
         des_y = rho * sin(theta);
         des_z = position[2]; // the z position can be set to the received height
 
-        // velocity computation
-        float dot_rho = K * omega;
-        dot_des_x = dot_rho * cos(theta) - rho * sin(theta) * omega;
-        dot_des_y = dot_rho * sin(theta) + rho * cos(theta) * omega;
-        // desired heading direction
-        gamma = atan2(dot_des_y, dot_des_x);
-
         if (!user_in_control)
         {
             // RCLCPP_INFO(this->get_logger(), "Sending idle attitude setpoint");
@@ -491,6 +486,11 @@ private:
         RCLCPP_INFO(this->get_logger(), "Control mode set to %d", current_control_mode);
     }
 
+    /**
+     * @brief Callback function for receiving failsafe messages
+     * 
+     * @param msg the message indicating that the failsafe was enabled
+     */
     void on_failsafe_received(const drone_services::msg::FailsafeMsg::SharedPtr msg)
     {
         if (msg->enabled)