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

add extra checks for when armed or not armed

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-05-03 12:05:03 +02:00
parent d084827f67
commit ade2b38a58
1 changed files with 31 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -62,6 +62,7 @@ private:
double start_time_; double start_time_;
bool has_sent_status = false; bool has_sent_status = false;
bool flying = false; bool flying = false;
bool armed = false;
bool has_swithed = false; bool has_swithed = false;
int setpoint_count = 0; int setpoint_count = 0;
float thrust = 0.0; float thrust = 0.0;
@@ -71,6 +72,7 @@ private:
float last_setpoint[3] = {0, 0, 0}; float last_setpoint[3] = {0, 0, 0};
float last_angle = 0; float last_angle = 0;
float last_thrust = 0; float last_thrust = 0;
// result quaternion // result quaternion
std::array<float, 4> q = {0, 0, 0, 0}; std::array<float, 4> q = {0, 0, 0, 0};
@@ -79,30 +81,44 @@ private:
const std::shared_ptr<drone_services::srv::SetAttitude::Request> request, const std::shared_ptr<drone_services::srv::SetAttitude::Request> request,
const std::shared_ptr<drone_services::srv::SetAttitude::Response> response) const std::shared_ptr<drone_services::srv::SetAttitude::Response> response)
{ {
last_setpoint[0] = degrees_to_radians(request->yaw); if (armed)
last_setpoint[1] = degrees_to_radians(request->pitch); {
last_setpoint[2] = degrees_to_radians(request->roll); last_setpoint[0] = degrees_to_radians(request->yaw);
last_setpoint[1] = degrees_to_radians(request->pitch);
last_setpoint[2] = degrees_to_radians(request->roll);
last_thrust = request->thrust; last_thrust = request->thrust;
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); 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; response->status = 0;
} else
{
last_thrust = 0;
last_setpoint = {0, 0, 0};
response->status = 1;
}
} }
/** /**
* @brief disarms the drone when a call to the disarm service is made * @brief disarms the drone when a call to the disarm service is made
* *
* @param request_header the header for the service request * @param request_header the header for the service request
* @param request the request (empty) * @param request the request (empty)
* @param response the response (empty) * @param response the response (empty)
*/ */
void handle_disarm_request( void handle_disarm_request(
const std::shared_ptr<rmw_request_id_t> request_header, const std::shared_ptr<rmw_request_id_t> request_header,
const std::shared_ptr<std_srvs::srv::Empty::Request> request, const std::shared_ptr<std_srvs::srv::Empty::Request> request,
const std::shared_ptr<std_srvs::srv::Empty::Response> response) const std::shared_ptr<std_srvs::srv::Empty::Response> response)
{ {
publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0, 0); 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() void send_trajectory_setpoint()
@@ -203,6 +219,7 @@ private:
RCLCPP_INFO(this->get_logger(), "Arm command sent"); RCLCPP_INFO(this->get_logger(), "Arm command sent");
flying = true; flying = true;
armed = true;
} }
send_attitude_setpoint(); send_attitude_setpoint();