From 6db6c89bd3525a80fab8e79281d77dc5fb347582 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Wed, 3 May 2023 14:23:42 +0200 Subject: [PATCH] add increasing/decreasing yaw pitch roll instead of setting --- src/px4_connection/src/px4_controller.cpp | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 4ceb09b0..6d6b005c 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -83,12 +83,19 @@ private: { if (armed) { - last_setpoint[0] = degrees_to_radians(request->yaw); - last_setpoint[1] = degrees_to_radians(request->pitch); - last_setpoint[2] = degrees_to_radians(request->roll); - + if (request->yaw == 0 && request->pitch = 0 && request->roll = 0) + { + last_setpoint[0] = degrees_to_radians(request->yaw); + last_setpoint[1] = degrees_to_radians(request->pitch); + 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; + 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;