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

change setting thrust and angles

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-05-03 15:34:57 +02:00
parent db1322e2de
commit adac3eb022
2 changed files with 19 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -83,18 +83,22 @@ private:
{
if (armed)
{
if (request->yaw == 0 && request->pitch = 0 && request->roll = 0)
if (request->yaw == 0 && request->pitch = 0 && request->roll = 0 && request->thrust = 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);
last_thrust = request->thrust;
} 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;
if (last_thrust > 1)
last_thrust = 1;
else if (last_thrust < 0)
last_thrust = 0;
}
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);