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

add increasing/decreasing yaw pitch roll instead of setting

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-05-03 14:23:42 +02:00
parent 779a5c9987
commit 6db6c89bd3
1 changed files with 11 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -82,13 +82,20 @@ private:
const std::shared_ptr<drone_services::srv::SetAttitude::Response> response) const std::shared_ptr<drone_services::srv::SetAttitude::Response> response)
{ {
if (armed) if (armed)
{
if (request->yaw == 0 && request->pitch = 0 && request->roll = 0)
{ {
last_setpoint[0] = degrees_to_radians(request->yaw); last_setpoint[0] = degrees_to_radians(request->yaw);
last_setpoint[1] = degrees_to_radians(request->pitch); last_setpoint[1] = degrees_to_radians(request->pitch);
last_setpoint[2] = degrees_to_radians(request->roll); last_setpoint[2] = degrees_to_radians(request->roll);
} else {
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;