sem/5g_drone_ROS2
1
0
Fork 0
Code Issues 1 Pull Requests 1 Actions Packages Projects Releases Wiki Activity

add disarm

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-04-28 18:03:14 +02:00
parent 9464dbf5ac
commit 7545a8a2a8
1 changed files with 10 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
src/px4_connection/src/px4_controller.cpp
View File

@@ -37,6 +37,7 @@ public:
this->publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0, 0); this->publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0, 0);
RCLCPP_INFO(this->get_logger(), "Arm command sent"); RCLCPP_INFO(this->get_logger(), "Arm command sent");
armed = true;
// create timer to send vehicle attitude setpoints every second // create timer to send vehicle attitude setpoints every second
timer_ = this->create_wall_timer(1000ms, std::bind(&PX4Controller::send_setpoint, this)); timer_ = this->create_wall_timer(1000ms, std::bind(&PX4Controller::send_setpoint, this));
@@ -52,6 +53,7 @@ private:
rclcpp::TimerBase::SharedPtr timer_; rclcpp::TimerBase::SharedPtr timer_;
double start_time_; double start_time_;
bool has_sent_status = false; bool has_sent_status = false;
bool armed = false;
/** /**
* @brief 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.
@@ -72,6 +74,14 @@ private:
msg.thrust_body[2] = 1; // down, 100% thrust up msg.thrust_body[2] = 1; // down, 100% thrust up
calculate_quaternion(q, 0, degrees_to_radians(10), 0); calculate_quaternion(q, 0, degrees_to_radians(10), 0);
} else if (this->get_clock()->now().seconds() - start_time > 20)
{
if (armed)
{
publish_vehicle_command(VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0);
armed = false;
RCLCPP_INFO(this->get_logger(), "Disarm command sent after 20 seconds");
}
} }
else else
{ {