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

only send trajectory setpoint once

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-05-15 11:01:25 +02:00
parent 61ee751642
commit d50758cb73
1 changed files with 17 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -130,7 +130,6 @@ private:
RCLCPP_INFO(this->get_logger(), "got values: yaw:%f pitch:%f roll:%f thrust:%f", request->yaw, request->pitch, request->roll, request->thrust); RCLCPP_INFO(this->get_logger(), "got values: yaw:%f pitch:%f roll:%f thrust:%f", request->yaw, request->pitch, request->roll, 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);
new_setpoint = true;
response->success = true; response->success = true;
} }
@@ -175,6 +174,7 @@ private:
} }
last_angle = request->yaw; last_angle = request->yaw;
new_setpoint = true;
RCLCPP_INFO(this->get_logger(), "Yaw: %f", last_angle); RCLCPP_INFO(this->get_logger(), "Yaw: %f", last_angle);
response->success = true; response->success = true;
} }
@@ -335,15 +335,23 @@ private:
// RCLCPP_INFO(this->get_logger(), "Sending attitude setpoint"); // RCLCPP_INFO(this->get_logger(), "Sending attitude setpoint");
send_attitude_setpoint(); send_attitude_setpoint();
} }
else if (current_control_mode == CONTROL_MODE_VELOCITY) else
{ {
// RCLCPP_INFO(this->get_logger(), "Sending velocity setpoint"); if (!new_setpoint)
send_velocity_setpoint(); {
} return;
else if (current_control_mode == CONTROL_MODE_POSITION) }
{ if (current_control_mode == CONTROL_MODE_VELOCITY)
// RCLCPP_INFO(this->get_logger(), "Sending position setpoint"); {
send_position_setpoint(); // RCLCPP_INFO(this->get_logger(), "Sending velocity setpoint");
send_velocity_setpoint();
}
else if (current_control_mode == CONTROL_MODE_POSITION)
{
// RCLCPP_INFO(this->get_logger(), "Sending position setpoint");
send_position_setpoint();
}
new_setpoint = false;
} }
} }
} }